Окончательно на основании (2.10) имеем матрицу преобразования координат от собственной системы схвата манипулятора к неподвижной системе, связанной со стойкой робота:
M4→0 = M1→0 × M2→1 × M3→2 × M4→3 .
Развернутый вид последнего матричного уравнения довольно громоздок и в целях сокращения объема выкладок приводиться не будет (внимательный читатель может получить его самостоятельно). А конечное выражение преобразования коор- динат точки схвата манипулятора к системе стойки робота имеет вид:
A0 = M4→0 × A4 .
В общем случае координаты, содержащиеся в векторе А0 являются функциями от всех обобщенных координат кинематической структуры, а от размеров звеньев.
2.3.4. Преобразования систем координат для кинематической структуры манипулятора ПР.
Координаты точки А в собственной системе координат звена 4 из-за наличия эксцентриситета в общем виде принимают значения: A9 (R9 cosα9 , R9 sinα9 , L9 ) (рис.
2.6, б). Здесь величина L9 характеризует длину звена 9, а величина R9 – эксцентри- ситет точки А относительно оси вращения. Для вычисления координат этой же точ- ки, но в системе 0 необходимо найти законы перехода между соседними системами координат, начиная с системы 9 и заканчивая системой 0.
Поскольку переходов, а следовательно, и матриц преобразования довольно значительное количество, то имеет смысл для сокращения математических выкла- док на основании выражения (2.14) и данных табл. 2.1 провести функциональное описание расширенных матриц перехода (табл. 2.2).
Таблица 2.2 Функциональное описание расширенных матриц преобразования координат.
Действие |
Функциональное описание |
|
|
||||||
Поступательное перемещение |
|
|
|
æ1 |
0 |
0 |
b |
ö |
|
|
|
|
ç |
|
|
x |
÷ |
|
|
вдоль осей х, у и z на величины |
Ms(bx ,by ,bz ) = ç0 |
1 |
0 |
by ÷ |
|
||||
bx, by и bz соответственно |
|
|
|
ç0 |
0 |
1 |
b ÷ |
|
|
|
|
|
ç |
|
|
z |
÷ |
|
|
|
|
|
|
|
|
|
|||
|
|
|
|
0 |
0 |
1 |
|
||
|
|
|
|
è0 |
ø |
|
|||
|
|
æ1 |
0 |
|
0 |
|
0ö |
||
|
Mϕx (α ) = |
ç |
0 cosα -sinα |
0 |
÷ |
||||
Поворот вокруг оси х на угол α |
ç |
÷ |
|||||||
ç |
0 |
sinα |
|
cosα |
|
0 |
÷ |
||
|
|
ç |
0 |
0 |
|
0 |
|
1 |
÷ |
|
|
è |
|
|
ø |
||||
|
41 |
|
|
|
|
|
|
|
|
|
|
æ cosϕ |
0 |
sinϕ |
0ö |
|||
|
Mϕy (α ) = |
ç |
0 |
1 |
|
0 |
0 |
÷ |
Поворот вокруг оси у на угол α |
ç |
|
÷ |
|||||
ç |
-sinϕ |
0 |
cosϕ |
0 |
÷ |
|||
|
|
ç |
0 |
0 |
|
0 |
1 |
÷ |
|
|
è |
|
ø |
||||
|
|
æcosϕ |
-sinϕ |
0 |
0ö |
|||
|
|
ç |
|
cosϕ |
0 |
0 |
÷ |
|
Поворот вокруг оси z на угол α |
Mϕz (α ) = |
ç sinϕ |
÷ |
|||||
ç |
0 |
|
0 |
1 |
0 |
÷ |
||
|
|
ç |
0 |
|
0 |
0 |
1 |
÷ |
|
|
è |
|
ø |
||||
Переход от 9й системы коорди- |
а) |
б) |
нат к 8й (рис. 2.19, а). Очевидно, что |
|
|
оси z9 и z8 взаимоперпендикулярны и |
|
|
в то же время оси х9 и z8 сонаправле- |
|
|
ны при любых значениях обобщенной |
|
|
координаты α8 во вращательной ки- |
Рис. 2.19. Переход от системы 9 к системе 8. |
|
нематической паре. Однако та же самая координата влияет на взаимную ориента- цию осей у9 и у8. В данном случае имеет место сложный поворот осей, который мы делим на два простых: во-первых учитываем поворот вокруг оси z8 с тем, чтобы добиться сонаправленности осей у, а затем учетом поворота вокруг оси у добиваем- ся сонаправленности всех одноименных координатных осей.
Кроме вышеуказанного, необходимо учесть линейное смещение точек начал систем координат. Для этого достаточно определить координаты точки В, являю- щейся началом системы 9, в системе 8. Зависимости для их определения несложны
и для выбранных направлений координатных осей в общем случае справедливо следующее выражение: B (L8 ×cos(α8 ), L8 ×sin (α8 ),0) . Таким образом, для учета линейных смещений будет использоваться следующая матрица:
|
æ1 |
0 |
0 |
L8 ×cos(α8 )ö |
||
|
ç |
|
|
|
|
÷ |
M = Ms (L8 ×cos(α8 ), L8 ×sin(α8 ),0) |
или M = ç |
0 |
1 |
0 |
L8 ×sin (α8 )÷ . |
|
|
ç |
0 |
0 |
1 |
0 |
÷ |
|
ç |
|
|
|
|
÷ |
|
ç |
0 |
0 |
0 |
1 |
÷ |
|
è |
ø |
||||
Угловое смещение, обусловленное наличием обобщенной координаты α8, учи- тываем матрицей поворота вокруг оси z. Величина углового смещения в данном
· |
|
|
r |
r |
-π (рис. 2.19, б), следовательно, матрица преобразования |
случае равна (y8 |
, y9 ) = α8 |
|
примет вид:
42
|
|
æcos(α8 -π ) |
-sin (α8 -π ) |
0 |
0 |
ö |
|
|
|
ç |
|
cos(α8 -π ) |
0 |
0 |
÷ |
M ¢ = Mϕz (α8 -π ) |
или M ¢ = |
ç sin (α8 -π ) |
÷ |
||||
ç |
0 |
0 |
1 |
0 |
÷ . |
||
|
|
ç |
|
|
|
|
÷ |
|
|
ç |
0 |
0 |
0 |
1 |
÷ |
|
|
è |
ø |
||||
После сонаправления осей у обеих систем координат добиться сонаправленно- сти остальных пар координатных векторов, не потеряв достигнутого результата, можно только поворотом вокруг оси у. После учета первого углового смещения как бы получается ситуация, изображенная на рис. 2.6. Учитываем угловое смещение с помощью матрицы поворота вокруг оси. Модуль углового смещения равен 90°, а его направление, определяемое знаком, – по часовой стрелке, если смотреть с поло- жительного направления оси у. Следовательно, для правосторонней системы коор- динат это отрицательное направление. Значит, в матрицу поворота необходимо вме- сто общего значения угла поворота подставить значение –90°.
|
|
|
æcos(-π / 2) |
0 |
-sin (-π / 2) |
0 |
ö |
|
æ |
- |
π ö |
ç |
0 |
1 |
0 |
0 |
÷ |
M ¢¢ = Mϕy ç |
÷ |
или M ¢¢ = ç |
|
|
cos(-π / 2) |
|
÷ . |
|
è |
|
2 ø |
ç |
|
0 |
0 |
÷ |
|
|
ç sin (-π / 2) |
÷ |
||||||
|
|
|
ç |
0 |
0 |
0 |
1 |
÷ |
|
|
|
è |
ø |
||||
Таким образом, переход от системы координат 9 к системе 8, учитывающий все вышеприведенные действия, описывается некоторой матрицей5, которая получается в результате перемножения матриц, учитывающих каждое из оговоренных элемен- тарных действий:
M9→8 = M ×M ¢×M ¢¢ = Ms(L8 ×cos(α8 ), L8 ×sin (α8 ),0)× Mϕz (α8 -π )×Mϕy (-π / 2) .
Перейдем теперь к преобразованию системы координат 8 к системе 7 (рис. 2.20). Обе эти системы ориентированы друг относительно друга постоянным обра- зом, поскольку вращения в кинематической паре 7 не происходит. Также остаются постоянными и величины линейных смещений точек начал систем координат, по-
скольку при назначении собственных систем координат для данной кинематической структуры согласно п. 2.2.4 учет линейного перемещения s7
в паре 7 происходит в системе координат 6.
Величины и направления линейных смещений исчер-
пывающим образом описываются координатами точки С в системе 7: C (-R7 ,0, -L7 ) . Это означает, что матрица преоб-
разования будет иметь вид: M = Ms (-R7 ,0,-L7 ) .
Угловое смещение координатных векторов относитель- |
Рис. 2.20. Переход от |
системы 8 к системе 7. |
5 Здесь и далее матрицы преобразований в целях сокращения видимого объема по- яснений в виде таблицы элементов представляться не будут.
43
но одноименных им хотя и постоянное, но имеется. В частности из трех пар коор- динатных векторов сонаправленными являются только вектора у7 и у8. Для осталь- ных пар с учетом направления углового смещения справедливо следующее равенст-
r |
r |
r |
r |
· |
· |
||
во (x7 |
, x8 ) = (z7 |
, z8 ) = -π / 2 . Для учета этой ситуации необходимо воспользоваться |
|
матрицей поворота вокруг оси у на угол –π/2, т.е. M ¢ = Mϕy (-π / 2) .
Окончательный вид матрицы перехода от системы 8 к системе 7:
M8→7 = M ×M ¢ = Ms (-R7 ,0, -L7 )× Mϕy (-π / 2) .
Далее предстоит найти матрицу преобразования коор- динат точек из системы 7 в систему 6 (рис. 2.21). При этом необходимо отметить, что независимо от значений обоб- щенных координат α6 и s7 координатные вектора z6 и y7 ос- таются сонаправленными, а, например, пара векторов z в
силу конструктивных особенностей манипулятора всегда взаимоперпендикулярна.
На величину линейных смещений оказывает влияние не
только координата α6, но и s7, поскольку точка D, являющаяся началом системы 7 в 6й системе имеет следующие координаты: D(s7 ×cos(α6 ), s7 ×sin (α6 ),0) . Это означа- ет, что для учета линейных смещений между системами необходимо использовать матрицу вида: M = Ms (s7 ×cos(α6 ), s7 ×sin (α6 ),0) .
Для учета углового смещения, создаваемого обобщенной координатой α6 во вращательной кинематической паре, необходимо использовать матрицу поворота вокруг оси z. Исходя из взаимного расположения координатных векторов, заметим,
что |
величина углового смещения равна по |
величине и по направлению углу |
|||
r |
r |
r |
r |
|
|
· · |
-π / 2 . Следовательно, |
матрица перехода будет иметь вид: |
|||
(y6 |
, y7 ) = (x6 |
, x7 ) = α6 |
|||
M ¢ = Mϕz (α6 -π / 2) . После этого действия мы как бы повернем систему 6 на угол
α6 – p/2 вокруг оси z, сонаправив тем самым оси х. Однако остальные одноименные координатные вектора будут смещены вокруг оси х на угол 90°, что математически
выльется в использование матрицы M ¢¢ = Mϕx (π / 2) . |
|
|
|
|
|
|
Матрица полного перехода между этими системами имеет вид |
|
|
|
|||
æ |
α6 |
- |
π ö |
æπ |
ö |
|
M7→6 = M ×M ¢×M ¢¢ = Ms (s7 ×cos(α6 ), s7 ×sin (α6 ),0)× Mϕz ç |
÷ |
× Mϕx ç |
2 |
÷ . |
||
è |
|
|
2 ø |
è |
ø |
|
При переходе от системы координат 6 к 5й системе (рис. 2.22) для рассматри-
ваемой кинематической структуры расположение точек начал относительно друг друга постоянно и описывается следующими координатами E (-L5 ,0,0) . Кинема-
тическая пара 5 не вращательного действия, что в совокупности с фактом изначаль-
44
ной попарной сонаправленности координатных векторов, исключает необходимость учета углового смещения систем координат. Следовательно, матрица перехода бу- дет иметь вид:
M6→5 = Ms (-L5 ,0,0) .
Рис. 2.22. Переход от |
Рис. 2.23. Переход от |
Рис. 2.24. Переход от |
|
системы 6 к системе 5 |
системы 5 |
к системе 4. |
системы 4 к системе 3. |
В свою очередь система 5 имеет как |
линейное так и угловое смещения относи- |
||
тельно системы 4 (рис. 2.23). Оба смещения переменны и находятся в зависимости от обобщенных координат α4 и s5. На величину линейного смещения вдоль осей х4 и у4 влияет обобщенная координата α4 и постоянный параметр L4 (длина звена 4), а вдоль оси z4 – s5. Окончательно имеем координаты точки начала системы 5 в систе-
ме 4: F (L4 ×cos(α4 ), L4 ×sin (α4 ), s5 ) ; и матрицу учета линейных смещений:
M = Ms (L4 ×cos(α4 ), L4 ×sin(α4 ), s5 ) .
Характер углового смещения для данного перехода аналогичен ситуации, сло- жившейся при конвертации координат системы 7 в координаты системы 6, т.е.
r |
r |
r |
r |
· · |
|||
(y4 |
, y5 ) = (x4 , x5 ) = α4 -π , а оси z обеих систем сонаправлены. Таким образом, угло- |
||
вое |
|
смещение систем описывается матрицей поворота вокруг оси z: |
|
M ¢ = Mϕz (α4 -π ) . |
|||
|
Учитывая все приведенные данные, касающиеся перехода от 5й системы к 4й, |
||
получаем следующую матрицу перехода |
|||
|
|
M5→4 |
= M ×M ¢ = Ms (L4 ×cos(α4 ), L4 ×sin (α4 ), s5 )× Mϕz (α4 -π ) . |
|
Система 4, жестко связанная со звеном 3, имеет линейные смещения вдоль осей |
||
y3 и z3, обусловленные конфигурацией и размерами звена 3 (рис. 2.24). Последнее находит свое отражение в координатах точки G (0, -R3 , L3 ) . Что касается угловых
смещений, то из всех координатных векторов обеих систем сонаправлены только оси у, поэтому требуется учесть угловое смещение относительно указанной оси на величину 90°. Окончательно получаем вид матрицы перехода между системами
M4→3 = Ms(0, -R3 , L3 )× Mϕy (π / 2) .
45
Описывая переход от системы 3 к системе 2 (рис. 2.25) |
|
можно заметить, что ротация в кинематической паре 2 звена |
|
2 при сохранении сонаправленности осей z обеих систем, |
|
приводит к появлению углового смещения в остальных па- |
|
рах координатных векторов, что необходимо учесть с по- |
|
мощью матрицы поворота вокруг оси z. Задача составления |
|
матрицы перехода в этом случае аналогична решенной при |
|
конвертации координат системы 5 в координаты системы 6. |
|
Однако в данном случае значение углового смещения во- |
|
круг оси z равно α2 – π/2. |
|
Рассматривая линейные смещения, необходимо отме- |
|
тить, что конфигурация звена 2 манипулятора (а именно |
|
размер R2) и обобщенные координаты s3 и α2 безусловно |
Рис. 2.25. Переход от |
оказывают влияние на значение этих смещений. Последнее |
системы 3 к системе 2. |
наглядно можно продемонстрировать на координатах точки |
|
H в общем виде в системе 2: H (R2 ×cos(α2 ), R2 ×sin (α2 ), s3 ) . Итоговая матрица пе-
рехода между системами координат имеет вид:
M3→2 = Ms(R2 ×cos(α2 ), R2 ×sin (α2 ), s3 )×Mϕz (α2 -π / 2) .
При конвертации координат системы 2 в систему 1 |
|
(рис. 2.26) необходимо, как и во всех предыдущих случаях, |
|
учесть как линейные, так и угловые смещения системы, |
|
которую приводят, относительно системы, к которой приво- |
|
дят. Система 2, жестко связана со звеном 1 (как впрочем, в |
|
данном случае и система 1), которое имеет возможность |
|
поступательного перемещения в кинематической паре. Не- |
|
обходимо отметить, что при этом относительном движении |
|
линейные смещения точек начал систем координат остаются |
Рис. 2.26. Переход от |
постоянными – на величину L1 (длину звена 1) вдоль оси х, |
системы 2 к системе 1. |
что находит свое отражение в координатах точки J: J (-L1 ,0,0) . Следовательно,
матрица учета этого смещения в функциональной форме запишется следующим образом: M = Ms (-L1 ,0,0) .
Из трех пар одноименных осей постоянно сонаправлены только у, остальные же имеют фиксированное угловое смещение на угол –90°. Последнее находит сове отражение в использовании матрицы перехода вида: M ¢ = Mϕy (-π / 2) .
Итоговое преобразование указанных систем координат осуществляется с по- мощью матрицы:
M2→1 = M ×M ¢ = Ms (-L1 ,0,0) ×Mϕy (-π / 2) .
46
И, наконец, последнюю конвертацию необходимо осу- |
|
ществить между системами 1 и 0 (рис. 2.27). Здесь, как и в |
|
предыдущем случае, взаимная ориентация осей систем ко- |
|
ординат имеет фиксированное угловое смещение, однако |
|
теперь линейное смещение переменно и зависит от обоб- |
|
щенной координаты s1. |
|
Величины линейных перемещений выявляются уже |
|
традиционным способом – нахождением координат точки |
Рис. 2.27. Переход от |
начала последующей системы относительно предыдущей. |
системы 1 к системе 0. |
При данном направлении осей системы 0 координаты точки начала системы 1 име- ют вид: K (s1 ,0,0) . Это приводит к необходимости использования матрицу линей-
ного сдвига: M = Ms(s1 ,0,0) .
Угловое смещение происходит вокруг оси у на угол 90°, что находит свое от- ражение в применении следующей матрицы: M ¢ = Mϕy (π / 2) .
Таким образом, полный переход от координат системы 1 к координатам систе- мы 0 производится с помощью матрицы:
M1→0 = M × M ¢ = Ms(s1 ,0,0)× Mϕy (π / 2) .
Получив матрицы перехода между соседними системами, на основании (2.10) имеем матрицу преобразования координат в собственной системе схвата манипуля- тора к координатам в неподвижной системе, связанной со стойкой робота:
M9→0 = M1→0 × M2→1 ×M3→2 ×M4→3 ×M5→4 × M6→5 × M7→6 ×M8→7 ×M9→8 .
Конечное выражение преобразования координат точки схвата манипулятора к системе стойки робота имеет вид:
A0 = M9→0 × A9 .
Компьютерная реализация данного алгоритма и получение вектора, содержа- щего координаты точки схвата робота относительно его стойки в численном виде,
являются решением поставленной прямой задачи о положениях пространственного манипулятора.
Частные вопросы, касающиеся компьютерной реализации алгоритма в среде MathCAD, приведены далее.
47