Математическое обеспечение промышленных роботов

МИНИСТЕРСТВО ОБРАЗОВАНИЯ РЕСПУБЛИКИ БЕЛАРУСЬ Белорусский национальный технический университет

БН

ТУ

Кафедра «Робототехнические системы»

ри й

МАТЕМАТИЧЕСКОЕ ОБЕСПЕЧЕНИЕ ПРОМЫШЛЕННЫХ РОБОТОВ

Ре по з

ит о

Учебно-методическое пособие

Минск БНТУ 2015

МИНИСТЕРСТВО ОБРАЗОВАНИЯ РЕСПУБЛИКИ БЕЛАРУСЬ Белорусский национальный технический университет

БН

ТУ

Кафедра «Робототехнические системы»

ри й

МАТЕМАТИЧЕСКОЕ ОБЕСПЕЧЕНИЕ ПРОМЫШЛЕННЫХ РОБОТОВ

Ре

по з

ит о

Учебно-методическое пособие к лекциям для студентов специальности 1-53 01 06 «Промышленные роботы и робототехнические комплексы»

Минск БН Т У 2015

1

УДК 007.52:51(076.5) ББК 32.816я7 М34

ри й

БН

Рецензенты: С. Н. Павлович, В. С. Юденков

ТУ

Авторы: А. Р. Околов, А. В. Дрозд, Ю. Н. Матрунчик, О. B. Лицкевич

ит о

Математическое обеспечение промышленных роботов : учебноМ34 методическое пособие к лекциям для студентов специальности 1-53 01 06 «Промышленные роботы и робототехнические комплексы» / А. Р. Околов [и др.]. – Минск : БНТУ, 2015. – 55 с. ISBN 978-985-550-561-8.

Ре

по з

Данное учебно-методическое пособие направленно на более качественное и эффективное изложение лекционного материала курса «Математическое обеспечение промышленных роботов», так как позволяет освободить лектора и обучаемых от необходимости механического переписывания объемных и сложных формул и матриц и дает возможность сосредоточить основное внимание студентов на понимании и усвоении основных вопросов, связанных с математическим описанием динамики и кинематики промышленного робота, планированием и моделированием траектории его движения. Учебно-методическое пособие может быть полезно студентам, инженерам и преподавателям, занимающихся проектирование и эксплуатацией промышленных роботов. 

ISBN 978-985-550-561-8

2

УДК 007.52:51(076.5) ББК 32.816я7

© Белорусский национальный технический университет, 2015

ЛЕКЦИЯ 1 Кинематика манипулятора Основные задачи кинематики манипулятора

ри й

БН

ТУ

 

Рис. 1.1. Схема взаимосвязи прямой и обратной задач кинематики

Ре

по з

ит о

Прямая задача кинематики Матрицы поворота (вращения)

Рис. 1.2. Абсолютная и связанная системы координат

3

puvw = (pu, pv, pw)T

и

pxyz = (px, py, pz)T,

(1.1)

где Т – операция транспонирования. (1.2)

puvw = puiu + pvjv + pwkw,

(1.3)

ТУ

pxyz = Rpuvw.

Или в матричной форме

ix jv jy jv

ix kw   pu     jy kw    pv  kz kw   pw 

ри й

 px   ix iu     py    jy iu     pz   kz iu

БН

px = ixp = ixiupu + ixjvpv + ixkwpw, py = jyp = iyiupu + jyjvpv + jykwpw, pz = kzp = kziupu + kzjvpv + kzkwpw.

kz jv

(1.4)

(1.5)

ит о

С учетом этого выражения матрица R в равенстве (1.2) примет вид

по з

 ix iu  R   jy iu k i  zu

ix jv jy jv

kz jv

ix kw   jy kw  . kz kw 

(1.6)

Ре

Аналогично координаты puvw можно получить из координат pxyz:

или

4

puvw = Qpxyz,

 pu   iu ix     pv    j ix  pw   k i  w x

iu iy j iy kw jy

iu kz   px     j kz    py  .  kw kz   pz 

(1.7)

(1.8)

Поскольку операция скалярного произведения коммутативна, то из соотношений (1.6), (1.7) и (1.8) следует (1.9)

QR = RTR = R–1R = I3,

(1.10)

БН

где I3 – единичная матрица размерностью 3  3.

ТУ

Q = R–1 = RT;

Причем ix  iu, и

ix jv jy jv

ix kw  1 0 0    jy kw    0 cos   sin   , kz kw   0 sin  cos  

ит о

Rx,

 ix iu    jy iu k i  zu

ри й

pxyz = Rx,puvw,

по з

 cos  0 sin     0 1 0  ,   sin  0 cos 

Rz ,

 cos   sin  0    sin  cos  0  .  0 0 1 

(1.12)

(1.13)

Ре

Ry ,

kz jv

(1.11)

Матрицы Rx,, Ry, и Rz, называют матрицами элементарных поворотов.

5

ТУ БН ри й ит о по з

Рис. 1.3. Вращающаяся система координат

Матрицы сложных поворотов

Ре

0   C  0 S   C   S  0   1 0      R = Ry,Rz,Rx, =  0 1 0    S  C  0    0 C   S   =   S  0 C   0 0 1  0 S  C    C C  S S   C S C  C S S   S C   , =  S  C C  C S     S C  C C C   C C  C C   S S S  

6

(1.14)

где С = cos; S = sin; C = cos; S = sin; C = cos; S = sin.

ТУ

0  C   S  0   C  0 S   1 0  1 0  = R = Rx,Rz,Ry, =  0 C   S     S  C  0    0  0 S  C    0 0 1    S  0 C  

(1.15)

БН

C C  S  C S     = C S C   S S  C C  C S S   S C  .  S S C   C S  S C  S S S   C C 

Решение:

ри й

Пример. Требуется найти матрицу поворота, являющегося результатом последовательного выполнения поворотов сначала на угол , вокруг оси OY, затем на угол  вокруг оси OW на угол  вокруг оси OU.

ит о

R = Ry,I3Rw,Ru, = Ry,I3Rw,Ru, =

по з

0   C  0 S   C   S  0   1 0      =  0 1 0    S  C  0   0 C   S   =   S  0 C   0 0 1  0 S  C  

Ре

 C C  S S   C S C  C S S   S C   . =  S  C C  C S     S C  S S C   C S  C C   S S S  

Матрица результирующего поворота такая же, как (1.14), но последовательность поворотов отличается в последовательности, результатом которой является выражение (1.14).

7

ЛЕКЦИЯ 2

ри й

БН

ТУ

Матрица поворота вокруг произвольной оси

Рис. 2.1. Вращение вокруг произвольной оси

ит о

Rr, = Rx,-Ry,Rz,Ry,-Rx, =

Ре

по з

0 0   C 0 S   C  S  0  1    0 C  S     0 1 0    S  C 0    0  S  C     S  0 C   0 0 1 

sin =

8

0   C 0  S    1 0    0   0 C   S   .  0 1  S  0 C   0 S  C  

ry

ry2

 rz2

; cos =

rz ry2

 rz2

; sin = rx; cos = ry2  rz2 .

Подстановка этих равенств в предыдущее выражение дает

(2.1)

ТУ

Rr ,

 rx2V   c rx ryV   rz s rx rzV   ry s   2  ry V   c ry rzV   rx s  ,  rx ryV   rz s    rx rzV   ry s ry rzV   rx s rz2V   c   

где V = vers = 1 – cos.

БН

Представление матриц поворота через углы Эйлера

Таблица 2.1

Три системы углов Эйлера

Ре

по з

ит о

ри й

1 2 3 Последова- На  вокруг оси OZ На  вокруг оси OZ На  вокруг оси OX тельность На  вокруг оси OU На  вокруг оси OV На  вокруг оси OY поворотов На  вокруг оси OW На  вокруг оси OW На  вокруг оси OZ

Рис. 2.2. Первая система углов Эйлера

9

Результирующая матрица поворота имеет следующий вид:

R,, = Rz,Ru,Rw, =

БН

ТУ

0  C   S  0  C   S  0   1 0    =  S  C  0    0 C   S     S  C  0  =  0 0 1   0 S  C    0 0 1 

Ре

по з

ит о

ри й

C C   S C S  C S   S C C  S S   =  S C   C C S   S S   C C C  C S  .  S S  sC  C  

Рис. 2.3. Вторая система углов Эйлера

10

(2.2)

Результирующая матрица поворота имеет следующий вид:

R,, = Rz,Rv,Rw, =

ТУ

C   S  0   C  0 S   C   S  0  =  S  C  0    0 1 0    S  C  0  = 0 1    S  0 C    0 0 1   0

(2.3)

ит о

ри й

БН

C C C   C S  C C S   S C  C S   =  S C с  C S   S C S   C C  S S   .  S S  C    S C 

по з

Рис. 2.4. Крен, тангаж, рысканье (третья система углов Эйлера)

Результирующая матрица поворота имеет следующий вид:

R,, = Rz,Ry,Rx, =

Ре

0  C   S  0   C  0 S    1 0      =  S  C 0   0 1 0   0 C   S   = 0 1    S  0 C   0 S  C    0

C C  C S S   S C  C S C   S S   =  S C  S S S   C C  S S C   C S   .   S   C S  C C 

(2.4) 11

ЛЕКЦИЯ 3 Геометрический смысл матриц поворота Свойства матриц поворота Однородные координаты и матрицы преобразований

ТУ

ри й

  cos    0    sin     0

Ty ,

ит о

Tx ,

  0 0  1 0   0 cos   sin  0   , 0 sin  cos  0    0 0 0 1   

БН

R Т =  33  f13

    p31   Поворот Сдвиг  . (3.1)  = 1  1   Преобразование Масштабирование      перспективы

 0 0  0 0 .   1 0  0 1 

(3.2)

Ре

по з

Tz ,

  cos   sin     sin  cos   0  0  0  0

 0 sin  0   1 0 0 , 0 cos  0    0 0 1

Эти матрицы размерностью 4  4 называются однородными матрицами элементарных поворотов. pˆ x, y , z  T pˆuvw

12

(3.3)

 sx ax px   sy ay py  . sz az pz    0 0 1 

БН

ЛЕКЦИЯ 4

(3.4)

ТУ

  nx   ny T  n  z  0

Ре

по з

ит о

ри й

Звенья, сочленения и их параметры

Рис. 4.1. Звенья и сочленения манипулятора Пума

13

ТУ БН ри й

Ре

по з

ит о

Рис. 4.2. Элементарные сочленения

Рис. 4.3. Система координат и ее параметры

14

Представление Денавита – Хартенберга

Алгоритм формирования систем координат звеньев Параметры систем координат звеньев манипулятора Пума

1 2 3 4 5 6

90 0 90 0 0 0

–90 0 90 –90 90 0

ai

di

Пределы измерения –160–+160 –225–45 –45–225 –110–170 –100–100 –266–266

ТУ

i

0 0 431,8 мм 149,09 мм –20,32 мм 0 0 433,07 мм 0 0 0 56,25 мм

БН

i

Ре

по з

ит о

ри й

Сочленение i

Рис. 4.4. Формирование систем координат звеньев для манипулятора Пума

15

ДХ-матрица преобразования для смежных систем координат с номерами i и i – 1

1 0  0  0

0 1 0 0

0 ai  1 0   0 0   0 cos i 1 0   0 sin i  0 1  0

0  sin i cos i

БН

0 0  0  1

 cos i sin Qi cos i cos Qi sin i

sin i sin Qi  sin i cos Qi cos i 0

0

ит о

 cos Qi  sin Q i =   0   0

0 0 1 0

0 0 0 0  × 1 di   0 1

ри й

 sin Qi cos Qi 0 0

cos Qi  sin Q i ×  0   0

0 1 0 0

ТУ

1 0 i–1 Ai = Tz,dTz,  Tx,aTx,α =  0  0

0

ai cos Qi  ai sin Qi  . di   1 

0 0  = 0  1

(4.1)

Преобразуя (3.1), найдем, что матрица, обратная к i–1Аi, имеет вид

Ре

по з

 cos i sin i   1  cos i sin i cos i cos i i i  1  Ai   Ai 1       sin i sin i  sin i cos i   0 0 

pi 1 

где pi 1   xi 1 , yi 1 , zi 1 ,1

T

16

и

   sin i  di sin i   ; (4.2) cos i  di cos i    0 1   ai

0

i 1

Ai pi ,

(4.3)

pi   xi , yi , zi , 1 . T

 S3 a3C3   0  C3 a3 S3  , 1 0 0   0 0 1  0

по з

Ре

 C5  S 4 A5   5 0  0 

БН

  C2  S2 0 a2C2     S2 C2 0 a2 S2  1 A2   ; 0 0 1 d2    0  0 0 1  

ит о

 C3  S 2 A3   3 0  0 

 0  0 , 0  1 

ри й

 C1 0  S1   S 0 C1 0 A1   1 0  1 0  0 0 0 

ТУ

  cos i  cos i sin i sin i sin i ai cos i    sin i cos i cos i  sin i cos i ai sin i  i 1 Ai   ;  0  sin i cos i di    0  0 0 1  

 S5 0   0  C5 0  , 1 0 0  0 0 1  0

 C4 0  S4   S 0 C4 3 A4   4 0  1 0  0 0 0 

 C6  S6  C6 S 5 A6   6 0 0  0 0 

 0  0  ; d4   1 

 0 0  0 0 ; 1 d6   0 1 

17

ТУ

  C1C23  S1 C1S23 a2C1C2  a3C1C23  d2 S1    T1  0 A1 1A2 2 A3   S1C23 C1 S1S23 a2 S1C2  a3 S1C23  d2C1  ;     S23  0 C23  a2 S2  a3 S23  

где Ci  cos i ; Si  sin i ;





Cij  cos i   j ;





Ре

по з

ит о

Sij  i   j .

18

ри й

БН

  C4C5 D6  S4 S6  C4C5 S6  S4C6 C4 S5 d6C4 S5     S4C5C6  C4 S6  S4C5 S6  C4C6 S4 S5 d6 S4 S5  3 4 5 T2  A4 A5 A6   ,   S5C6 S5 S6 C5 d6C5  d4     0 0 0 1  

ЛЕКЦИЯ 5

ри й

БН

ТУ

Уравнения кинематики манипулятора

0

ит о

Рис. 5.1. Система координат схвата i

Ti = 0Ai 1Ai … i–1Ai = 

x Aj =  i 0

j 1

j 1

yi

zi

0

0

pi   0Ri = 1   0

pi   1 

0

по з

для i = 1, 2, …, n,

y6 0

Ре

x T=  6 0

z6 0

 nx n  0 p6  = n s a p =  y    1  0 0 0 1   nz  0

p6   0R = 6 1   0 

абс

Tинстр  B 0T6 H .

При этом H ≡ 6 Aинстр , B ≡

абс

Sx Sy

ax ay

Sz 0

az 0

px  py  , pz   1 

(5.1)

A0 . 19

Матрица T манипулятора Пума имеет вид Sx Sy

ax ay

Sz 0

az 0

px  py  , pz   1 

(5.2)

ТУ

 nx n y 0 1 2 3 4 5 T = A1 A2 A3 A4 A5 A6 =  n  z  0

где nx  С1 С23 (С4С5С6  S4 S6 )  S23 S5С6   S1 ( S4C5C6  C4 S6 ) ; nx   S23 С4С5С6  S4 S6   C23 S5 S6 ) ;

БН

ny  S1 С23 (С4С5С6  S4 S6 )  S23 S5С6   C1 ( S4C5C6  C4 S6 ) ;

Sx  С1  С23 (С4С5 S6  S4C6 )  S23 S5 S6   S1 ( S4C5 S6  C4C6 ) ;

ри й

S y  S1  С23 (С4С5С6  S4C6 )  S23 S5 S6   C1 ( S4C5 S6  C4C6 ) ; Sz  S23 С4С5 S6  S4C6   C23 S5 S6 ) ; ax  C1 (C23C4 S5  S23C5 )  S1S4 S5 ;

ит о

ay  S1 (C23C4 S5  S23C5 )  C1S4 S5 ; az   S23C4C5  C23C5 ;

px  C1[d6 (C23C4 S5  S23C5 )  S23d4  a3C23  a2C2 ]  S1 (d6 S4 S5  d2 ) ;

по з

py  S1[d6 (C23C4 S5  S23C5 )  S24 d4  a3C23  a2C2 ]  C1 (d6 S4 S5  d2 ) ;

pz  d6 (C23C5  S23C4 S5 )  C23d4  a3 S23  a2 S2 .

Ре

Например, при 1  90, 2  0, 3  90, 4  0, 5  0, 6  0, имеем

20

 0 1 0 0 T=   1 0  0 0

0 149,09  1 921,12  . 0 20,32   0 1 

Обратная задача кинематики

sx sy

ax ay

sz 0

az 0

px  py  0 1 2 3 4 5 = A1 A2 A3 A4 A5 A6. pz   1 

(5.7)

ТУ

 nx n y T6 =  n  z  0

ax   ay  = Rz , Ru , R,  az 

ри й

Sx Sy Sz

ит о

 nx   ny n  z

БН

Методы обратных преобразований

по з

C C   S C S  C S   S C C  S S     S C   C C S   S S   C C C  C S  , (5.8)  S S  S C  C  

Ре

где Сij  cos(Qi  Q j ) и Sij  sin(Qi  Q j ) ,

nx  С C   S C S ;

(5.9а)

ny  S C   C C S ;

(5.9б)

nx  S S ;

(5.9в) 21

Sx  С S   S C C ;

(5.9г)

S y   S S   C C C ;

(5.9д) (5.9е)

ax  S S ;

(5.9ж)

ТУ

Sz  S C ;

ay  C S ;

БН

az  C .

(5.9з)

ри й

  arccos(az ),

(5.11)

  ay    arccos  .  S 

(5.12)

ит о

по з

Ре 22

(5.10)

s    arccos  z  ,  S 

0    90, 90    180,    ATAN 2( y, x)  180    90, 90    0,

   C S  0    S  C 0    0 0 1  

(5.9и)

если если если если

    0   nx sx ax  1 0      ny sy ay   0 C   S       n s a  0 S  C     z z z  

x  0, y  0; x  0, y  0; x  0, y  0; x  0, y  0.

(5.13)

  C   S  0     S  C 0    0 0 1  

или

БН

  0  S C    C S  C C   S  ;    S S  S C  C    

(5.14)

ТУ

  Cnx  S ny Csx  S sy Cax  S ay     S nx  Cny  S sx  Csy  S ax  Cay       nz sz az  

(5.15)

 a    arctg  x   ATAN 2( ax ,  ay ). .   ay 

(5.16)

C   C nx  S ny ;

(5.17а)

S   C sx  S sy ;

(5.17б)

ит о

ри й

C ax  S ay  0;

по з

 C sx  S sy   S    arctg   arctg     C   C nx  S ny 

(5.18)

Ре

 ATAN 2(C sx  S sy , C nx  S ny ). S   S ax  C ay ;

C   az ;

(5.19)

 S ax  C ay   S    arctg    arctg    ATAN 2  S ax  C az  . (5.20) az  C   

23

ЛЕКЦИЯ 6 Геометрический подход к решению обратной задачи кинематики

ax ay

Sz 0

az 0

px  py  . pz   1 

(6.1)

Ре

по з

ит о

ри й

БН

Sx Sy

ТУ

T6  T  B1 абсTинстр H 1

0

 nx n y =  n  z  0

Рис. 6.1. Определение различных конфигураций манипулятора

24

1, для ПРАВОЙ руки; РУКА =  1, для ЛЕВОЙ руки;

(6.2)

1, для ВЕРХНЕЙ руки; ЛОКОТЬ =  1, для НИЖНЕЙ руки;

(6.3)

1, если КИСТЬ ВНИЗ; ЗАПЯСТЬЕ =  1, если КИСТЬ ВВЕРХ;

(6.4)

ТУ

1  сменить ориентацию запястья; ПЕРЕКЛЮЧАТЕЛЬ =  (6.5) 1  не менять ориентацию запястья.

БН

Решение обратной задачи кинематики для первых трех сочленений

(6.6)

 px  С1 ( a2C2  a3C23  d4 S23 )  d2 S1       py  =  S1 (a2C2  a3C23  d4 S23 )  d2C1  . p    d4C23  a3 S23  a2 S2  z 

(6.7)

ри й

P  P6  d6 a  ( px , py , pz )T ,

ит о

Решение для первого сочленения

1L    ,

px2  p2y  dz2 ,

по з

r

sin  

Ре

1R      ;

sin  

py

R

px2  p2y ;

(6.8) (6.9)

,

cos  

px ; R

(6.10)

d2 , R

cos  

r , R

(6.11)

R

где индексы L и R означают ЛЕВУЮ и ПРАВУЮ конфигурацию манипулятора.

25

ТУ БН ри й ит о по з

Рис. 6.2. Решение для первого сочленения

Ре

sin 1L  sin(  )  sin  cos   cos  sin  

cos 1L  cos(  )  cos  cos   sin  sin   sin 1R  sin(    ) 

26

py r  px d2 R2

px r  py d2

 py r  px d2 R2

R2 ;

; (6.12)

; (6.13) (6.14)

sin 1R  sin(    ) 

 РУКА py px2  p2y  d22  px d2 px2  p2y  РУКА px px2  p2y  d22  py d2 px2  p2y

(6.15)

.

;

(6.16)

.

(6.17)

БН

cos 1 

R2

ТУ

sin 1 

 px r  py d2

 sin 1  1  arctg    cos 1 

ри й

(6.18)   РУКА p p2  p2  d 2  p d  y x y x 2 2   arctg    1  .   РУКА p p2  p2  d 2  p d  x x y y 2 2  

Ре

по з

ит о

Решение для второго сочленения

Рис. 6.3. Решение для второго сочленения

27

Как следует из табл. 6.1, используя индикаторы конфигурации РУКА и ЛОКОТЬ, для 2 можно записать единое для всех возможных конфигураций манипулятора выражение 2    (РУКА  ЛОКОТЬ)    K ;

sin   

pz  R

px2  p2y  d22 ;

r pz px2



p2y

 pz2  d22

(6.20)

ТУ

px2  p2y  pz2  d22 ,

;

БН

R

(6.19)

(6.22)

ри й

РУКА  px2  p2y  d22 РУКА  r cos     ; R px2  p2y  pz2  d22

(6.21)

ит о

2 2 2 2 2 2 2 a22  R2  (d42  a32 ) px  py  pz  a2  d2  (d4  a3 )  cos   ; (6.23) 2a2 R 2a2 px2  p2y  pz2  d22

по з

sin   1  cos2  .

(6.24) Таблица 6.1

Угол 2 при различных конфигурациях манипулятора

Ре

Конфигурация манипулятора

2

РУКА ЛОКОТЬ РУКА· ЛОКОТЬ

ЛЕВАЯ ВЕРХНЯЯ рука

 

–1

+1

–1

ЛЕВАЯ НИЖНЯЯ рука



–1

–1

+1

ПРАВАЯ ВЕРХНЯЯ рука



+1

+1

+1

ПРАВАЯ НИЖНЯЯ рука

 

+1

–1

–1

28

Из равенств (6.19) – (6.24) можно определить значение функций синуса и косинуса угла 2: sin 2  sin(  K )  sin  cos( K )  cos  sin( K )  (6.25)

ТУ

 sin  cos   (РУКА  ЛОКОТЬ) cos  sin ;

cos 2  cos(  K )  cos  cos   (РУКА  ЛОКОТЬ)sin  sin ; (6.26)

БН

 sin 2  2  arctg   ,    2  .  cos 2 

(6.27)

ри й

ЛЕКЦИЯ 7

Решение для третьего сочленения

ит о

Таблица 7.1

Угол 3 при различных конфигурациях манипулятора ( 2 p4 ) y

по з

Конфигурация манипулятора

3

РУКА ЛОКОТЬ РУКА· ЛОКОТЬ

0



–1

+1

–1

ЛЕВАЯ НИЖНЯЯ рука

0



–1

–1

+1

ПРАВАЯ ВЕРХНЯЯ рука

0



+1

+1

+1

ПРАВАЯ НИЖНЯЯ рука

0



+1

–1

–1

Ре

ЛЕВАЯ ВЕРХНЯЯ рука

29

ТУ БН ри й ит о по з

Ре

Рис. 7.1. Решение для третьего сочленения

30

R

px2  p2y  pz2  d22 ,

cos  

a22  (d42  a32 )  R2 2a2 d42  a32

(7.1)

;

(7.2)

sin   РУКА  ЛОКОТЬ 1  cos2  ; sin  

d4 d42  a32

,

cos  

a3 d42  a32

3    .

.

(7.3)

ТУ

(7.4)

БН

sin 3  sin(  )  sin  cos   cos  sin ;

(7.5)

(7.6)

 sin 3  3  arctg   ,    3  .  cos 3 

(7.7)

ри й

cos 3  cos(  )  cos  cos   sin  sin .

Решение обратной задачи кинематики для последних трех сочленений

 | ( z3a ) при заданном a  (ax , ay , az )T ; z3a

ит о

z4 

(7.9)

по з

a  z5 при заданном a  (ax , ay , az )T ;

(7.8)

Ре

S  y6 при заданных s  ( sx , sy , sz )T и n  (nx , ny , nz )T . (7.10) Решение для четвертого сочленения

 0 в вырожденном случае;     sy5 , если sy5  0;  ny5 , если sy5  0. 

(7.11)

31

cos 4  M ( z4 y3 ).

(7.13)

БН

sin 4   M ( z4 x3 ),

(7.12)

ТУ

0 в вырожденном случае;   s   z3a  , если s   z a   0; 3  z3 a  n   z3 a  , если s  z a  0. 3   z3a  

ри й

 1, если x  0; ign (x)=  1, если x  0. 

(7.14)

  M (C1ay  S1ax )  sin 4  4  arctg  ,   arctg   cos 4   M (C1C23a x  S1C23 ay  S23az ) 

ит о

  4  .

(7.15) Таблица 7.2

по з

Различные ориентации запястья

Ориентация запястья

 = sy5 или ny5

М-ЗАПЯСТЬЕ  sign()

0

+1

+1

КИСТЬ ВНИЗ