机器人建模与控制课件：机器人逆运动学

机器人建模与控制

机器人逆运动学⚫逆运动学已知工具坐标系相对于工作台坐标系的期望位置和姿态，计算一系列满足

期望要求的关节角⚫为计算关节角以获得相对于工作台坐标系{S}的工具坐标系{T}，可分二步求解：➢进行坐标变换求出相对

于基坐标系{B}的腕部

坐标系{W}➢应用逆运动学求关节角机械臂逆运动学逆运动学问题：以基座坐标系为参考系，已知末端工具联体坐标系的位姿，

求各关节变量的值 11|T

=T(91

)T(92

)T(93

)T(94

)T(95

)T(96

)=11r3r265544332211060这些方程为非线性超越方程，要考虑其解的存在性、多重解性以及求解方法4.1

逆运动学问题

上述等式总共给出6个方程，其中含有6个未知量逆运动学

笛卡尔空间→关节空间对于6R操作臂，该问题是已知

T

，求91

,92

,

,96

满足60px

]|py

|pz

1

」|12223201323330||L0rrrrr「rr4.2.1

工作空间和解的存在性➢工作空间是机器人末端工具联体坐标系原点所能到达的范围➢灵巧工作空间:

机器人末端工具能够以任何姿态到达的区域➢可达工作空间:机器人末端工具以至少一种姿态到达的区域➢灵巧工作空间是可达工作空间的子集➢若目标位置在灵巧工作空间内，则逆运动学问题的解存在➢若目标位置不在可达工作空间内，则逆运动学问题的解不存在➢当操作臂少于6自由度时，它在三维空间内不能达到全部位姿4.2

逆运动学的可解性4.2.1

工作空间和解的存在性例:考虑一个两连杆操作臂.如果

L1

=

L2

，

则可达工作空间是半径为2L1

的圆，

而灵巧工作空间仅是单独的一点，即原点如果

L1

≠

L2

，则不存在灵巧工作空间，可达工作空间为一外径为L1

+

L2

，内径为

|L1

-

L2

|4.2

逆运动学的可解性的圆环4.2.2

逆运动学的多解可能性➢一个具有3个旋转关节的平面操作臂，由于可以任何姿态到达工作空间内的

许多位置，因此在平面中有较大的灵巧工作空间(给定适当的连杆长度和大

的关节运动范围)4.2

逆运动学的可解性4.2.2

逆运动学的多解可能性➢若同一位姿有多个解，系统最终只能选择一个解，比较合理的一种选择是

取“最短行程”解➢如图操作臂处于点A，若希望它移动到点B，最短行程解就是使得每一个

运动关节的移动量最小➢

在无障碍的情况下，可选图中上部虚线所示的位形➢在有障碍的情况下，最短行程发生干涉，这时选择较长行程4.2

逆运动学的可解性4.2.2

逆运动学的多解可能性➢典型的机器人有3个大连杆，附带3个小连杆，姿态连杆靠近末端执行器➢计算最短行程需要加权，使得选择侧重于移动小连杆而不是移动大连杆4.2

逆运动学的可解性例:

PUMA

560到达一个确定目标有8个不同的解。

图中给出了其中的4个解。它

们对于末端手部运动来说具有相同的

位姿。对于图中所示的每一个解存在

另外一种解。动范围的函数。4.2.2

逆运动学的多解可能性➢解的个数取决于操作臂的关节数量，解的个数也是连杆参数和关节运➢由于关节运动的限制，这8个解中的某些解是不能实现的。4.2

逆运动学的可解性➢数值解法:数值解法具有迭代性质，所以比封闭解法的求解速度慢得多。在多重解的情况下，数值解法不能求出全部的解➢封闭解法:封闭解法是指基于解析形式的算法。将运动学的封闭解法分为两类：

代数解法和几何解法4.2.3

逆运动学的封闭解和数值解4.2

逆运动学的可解性关于运动学逆解的几个结论➢所有包含转动关节和移动关节的串联型6自由度操作臂都是可解的，

但这种解一般是数值解➢对于6自由度操作臂来说，只有在特殊情况下才有解析解。这种存在

解析解(封闭解)的操作臂具有如下特性：存在几个正交关节轴或者

有多个

i

为0或

土90。➢具有6个旋转关节的操作臂存在封闭解的充分条件是相邻的三个关节

轴线相交于一点4.2

逆运动学的可解性4.3.1

代数解法用代数解法对一个简单的平面三连杆操作臂进行求解D-H

连杆参数表关节iαi−1ai−1diθi1000120102302030

l1c1

+l2c12

]|0

l1s1

+

l2s12

|

1

0

||0

1

」4.3

代数解法和几何解法

123|

T

=

L01231231231230s30该操作臂的运动方程为:−sc001

2

3123123「cX八X八X八X八0

2

3

1

θθθlllll20314.3.1

代数解法对于平面操作臂，目标点的位姿可由三个量

x,y,0确定:4.3

代数解法和几何解法

123|0

T

=

|s1233

|0|L0l1c1

+l2

c12

]l1s1

+l2s120该操作臂的逆运动学问题可描述为「c0|0T

|s03

=

|0|

|

|

|

|

0s0L0−s0c00−s0c00−sc00x

]

|

y

|

0

|x

]

|

y

|

0

|00100010|

1

」||

1

」|01001||L0|

」123123「c「c000|||=x2

+y2=l

+l

+2l1l2

(c1c12

+s1s12

)=l

+l

+2l1l2

(c

c2

−c1s1s2

+s

c2

+s1c1s2

)=l

+l

+2l1l2

c2x2

+y2

−l

−l22122212121222122212若

−1c2

1，上式有解；否则无解若上式有解，则s2

=,92

=Atan2(s2

,c2

)得到2个可行的

9x

=l1c1

+l2

c12y

=l1s1

+l2s12c

=

cs

=s4.3

代数解法和几何解法得到四个非线性方程:c2

=2l1l24.3.1

代数解法012301232于是

+91

=Atan2(,)=Atan2(y,x)91

=Atan2(y,x)−Atan2(k2

,k1

)r

r最后，可以解得

91

+92

+93

=Atan2(s0,c0)=093

=0−91

−92为了求解这种形式的方程，进行变量代换:

令r

=,=Atan2(k2

,

k1

)即

k1

=rcos,k2

=rsin为便于计算引入新的变量:x

=l1c1

+l2

c12

x

=k1c1

−

k2

s1

y

=l1s1

+l2s12

y

=k1s1

+k2

c1=coscos91

−sin

sin91=cossin91

+sincos91

1

rysin(+91

)=r代数法是求解

逆运动学的基

本方法4.3

代数解法和几何解法k1

=l1

+l2

c2k2

=l2s24.3.1

代数解法cos(+9)=xryr其中y

xx4.3.2

几何解法对于上例中的3自由度操作臂，

由于操作臂是平面的，

因此可利用平面几

何关系直接求解由余弦定理知x2

+y2

=l

+l

−2l1l2

cos(180+92

)cos(180+92

)=−cos(92

)x2

+y2

−l

−l为使该三角形成立，必须有l1

−l2

l1

+l2当上述条件成立时，可以解得一个

92

=[−180o,0o]另一个解

92,

=

−9222122212θ2l2

214.3

代数解法和几何解法c2

=2l1l2X八

l2(b+v,若92

共0进一步，可得

91

=〈lb

−v,若92

>0注意到三个连杆角度之和即为连杆3的姿态91

+92

+93

=0由此可求得934.3.2

几何解法应用反正切公式

b=Atan2(y,x)应用余弦定理

cosv

=几何法在求操作臂的逆运动学解时，需将臂

的空间结构分解为多个平面几何结构，然后

通过平面几何方法求出相应的关节变量。这种分解在连杆扭转角为0。或土90。时最方便。2l1

可以解得一个

v

=[0。,180。]4.3

代数解法和几何解法x2

+y2

+l

−l2212

3

21X八X八X八X八3

2

1

0

lll20314.3.3

通过化简为多项式的代数解法逆运动学需解超越方程，有些情况下可将超越方程化为一元n次方程n不大于4时，一元n次方程有封闭形式的解半角公式

u

=

tan

9

,

cos9

=

,

sin9

=

21+u

1+u例:

求解超越方程

acos9+bsin9c

9解：利用

cos9

=

,

sin9

=

1+u

1+u得到

a(1−u)+2bu2

=c(1+u)2取u的幂函数形式(a+c)u2

−2bu

+(c−a)=0u

=

b

,

9

=

2Atan(

b

)a

+c

a

+c如果

a

+c

=0,

那么

9=

18004.3

代数解法和几何解法得到PIEPER研究了3个相邻的轴相交于一点的6自由度操作臂(包括3个相邻的轴平行的情况)。PIEPER的方法主要针对6个关节均为旋转关节的操作臂，且后面3个轴正交4.4

三轴相交的PIEPER解法尽管一般具有6自由度的操作臂没有封闭解，但是在某些特殊情况下封闭解是存在的该成果广泛应用于产品化的机器人中−s9i

c9icai−1

c9isai−1

0i−

T

=

L

0i10−sai−1

cai−1

0cai−1di

1

」−

sai−1di

|「

c9iai−1

]||「

c93其中

P4

=T

P4

=L

02222222222223321ORG321ORGf2

=f2

(93

)=a3ca2s3

−d4sa3ca2c3

−d4sa2ca3

−d3sa2f3

=f3

(93

)=a3sa2s3

−d4sa3sa2c3

+d4ca2ca3

+d3ca2a2

]「a3

]「f1

(93

)]−

sa2

d3

|

|

−d4

sa3

|

=

|f2

(93

)

|ca2

d3

|

|

d4

ca3

|

|f3

(93

)|「x]

=P4

=

T

T

T

P4

0000000000003301ORG3221101ORGzy当最后3根轴相交时，连杆坐标系{4}

、{5}

、{6}的原点均位于这个交点上，这点的基坐标为4.4

三轴相交的PIEPER解法f1

=f1

(93

)=a3c3

+d4sa3s3

+a2−s93c93ca2

c93sa20|||||1

」L

1

」

L

1

」0−sa2

ca20|||||||

L1」令

r

=x2

+y2

+z2

=(c1g1

−s1g2

)2

+(s1g1

+c1g2

)2

+g=g

+g

+g

=f12

+f22

+f32

+a

+d

+2d2

f3

+2a1

(c2

f1

−s2

f2

)221232221232g1

=g1

(92

,93

)=c2

f1

−s2

f2

+a1g2

=g2

(92

,93

)=s2

c

1f1

+c2

c

1f2

−s

1f3

−d2s

1]「f1

(93

)]「g1

(92

,93

)]||||||

|f2

(93

)

|

=

|g2

(92

,93

)

||

|f3

(93

)|

|g3

(92

,93

)

|g3

=g3

(92

,93

)=s2

s

1f1

+c2s

1f2

+c

1f3

+d2

c

100]「g1

]「c1g1

−s1g2

]0

0

|

|g2|

=

|s1g1+

c1g2|10||g3

||g3

|「c91

=T

P4

=s110911ORG104.4

三轴相交的PIEPER解法「1P

]「2P|

4ORG

|

=T

|

4ORGL

1

」

L

121|||||」L

1

」L

1

」「c92||L

0「x]

=

P4001ORGzya1−s

1d2c

1d2

1−s92c92

c

1

c92s

10

=

||||

|0

1」L1」L1

」0−s

1

c

10利用

T,T

,得到2110−s91c9100||||

|||

L1」|

L

0求解93简化表达，得到

r

=(k1c2

+k2s2

)2a1

+k3

,z

=(k1s2

−k2c2

)s

1

+k4其中k1

=f1

,k2

=−f2

,k3

=f12

+f22

+f32

+a

+d

+2d2

f3

,k4

=f3c

1

+d2c

11)若

a1

=0，则

r

=k3

=f12

+f22

+f32

+a

+d

+2d2

f3注意到

f1

=f1

(93

)=a3c3

+d4

s

3s3

+a2f2

=f2

(93

)=a3c

2

s3

−d4

s

3c

2

c3

−d4s

2

c

3

−d3s

2f3

=f3

(93

)=a3s

2

s3

−d4

s

3s

2

c3

+d4

c

2

c

3

+d3c

2将

u

=

tan

3

,

c3

=

2

,

s3

=

2

代入，可将r

=k3化为u的二次方程21+u

1+u22122212r

=f12

+f22

+f32

+a

+d

+2d2

f3

+2a1

(c2

f1

−s2

f2

)22124.4

三轴相交的PIEPER解法z

=s2

s

1f1

+c2s

1f2

+c

1f3

+d2

c

19

1−u2

2u

r和z是已知的利用二次方程可以得到

932)若

s

1

=0，则z

=

k4，同样采用化简为多项式的办法，

由二次方程得

933)否则，消去s2

和c2

，得到(r

−

k3)

(z

−

k4)

2

2采用化简为多项式的办法，可得到一个四次方程，由此解得9322222222求解92根据

r

=(k1c2

+k2s2

)2a1

+k3

,z

=(k1s2

−k2c2

)s

1

+k4

，可解得92求解91根据

|

|

=

|

1

1

1

2

|

，x和y是已知的，可解得91r

=(k1c2

+k2s2

)2a1

+k3

,z

=(k1s2

−k2c2

)s

1

+k4k1

=f1

,k2

=−f2

,k3

=f12

+f22

+f32

+a

+d

+2d2

f3

,k4

=f3c

1

+d2c

122124.4

三轴相交的PIEPER解法「x]「c

g

−s

g

]Ly」Ls1g1

+

c1g2

」2

+2

=k1

+k24a1

s

1求解

94

,95

,96求出

91

,92

,93

后，若94

=0

可计算出坐标系{4}相对于基坐标的姿态

R|94=0

=R

R|94=0

=R

21R

R

R|94=0再由已知的

R，坐标系{6}的期望姿态与坐标系{4}的姿态差别仅

在于最后三个关节的作用R|94=0

=R-1

|94=0

R对于任何一个4

、5

、6轴相互正交的6R操作臂，

最后三个关节角

是一种欧拉角，即

R

|94=0

可由这种欧拉角表示这时，94

,95

,96可用欧拉角解法求得6460406460433210433040−s9i

c9icai−1

c9isai

−1

0i−

T

=

L

0i10−sai−1

cai

−1

0−

sai−1di

|cai

−1di

||1

」「c9iai−1

]||4.4

三轴相交的PIEPER解法⚫PUMA

560机器人PIEPER解法适用于该机器人，这里展示另一种代数解法「r11

r12

r13

px

]|

|问题描述：已知

T

=

，求

91

,92

,93

,94

,95

,96|L0001」|使得

T

=T(91

)T(92

)T(93

)T(94

)T(95

)

T(96

)解：将含有91

的部分移到方程的左边[T(91

)]−1

T

=T(92

)T(93

)T(94

)T(95

)T(96

)=T616554433221601065544332211060604.5

机器人逆运动学实例

4.5

机器人逆运动学实例

r

r

1r

p1

1T

=

|

r21r22r23py

|「c1

−s1

0

0]

|

1r31

1r32

1r33

1pz

|因

T

=

是正交阵，有

|

|

r11

=

c23

[c4

c5

c6

−

s4

s6

]

−

s23s5

s6L0001」1110661111111111111111111112x11313112111111求解

91：

由元素

(2,4)

相等，得到

−s1px

+

c1py

=

d3令三角变换

px

=pcos0,py

=psin0其中p=

,0=

Atan2(py

,

px

)得−s1c0

+

c1s0

=

d3

/

p=

sin(0−91

)

因此cos(0−91

)

=

土

1

−

则

0−91

=Atan2(

d3

,土1−),91

=Atan2(py

,px

)−Atan2(d3

,土)|||L

0001」|1px

=a2c2

+a3c23

−d4s23

1py

=d31pz

=−a3s23

−a2s2

−d4c23「c1|0T

−1

0T

=

|

−s11

6

|0|

L

0px

]|py

|

1Tp1

」|0]「r11||||

31

1」|L00

||r210

||rp

p12223201323330s1c1

0

00010rrrrrr−pz

=a3s23

+d4

c23

+a2s2上三式平方后相加，有

a3c3

−d4s3

=K式中

K

=

p

+

p

+

p

−

a

−

a

−

d

−

d2a2同样采用三角变换办法，得93

=Atan2(a3

,d4

)−Atan2(K,土

)42323222z2y2x2pz

=

1r311

」|

|L0「

c1

||

−s1|0

|L

01r131r23

1r3301r121r22

1r320||

31

1」|L00

||r210

||rpx

]

「1r11py

|

|

1r211px1py10]「r11||s1c1

0

00010]

| | | |」|z1|

|33012r22rr32r13r23r0求解

93：由元素

(1,4)

、(2,4)和

(3,4)

分别相等得到

c1px

+s1py

=a3c23

−d4s23

+

a2c2−s1px

+c1py

=d31r11

=c23

[c4c5c6

−s4s6

]−s23s5s61px

=a2c2

+a3c23

−d4s231py

=d31pz

=−a3s23

−a2s2

−d4c234.5

机器人逆运动学实例pc1c23px

+s1c23py

−s23pz

−a2c3

=a3

,−c1s23px

−

s1s23py

−

c23pz

+

a2s3

=

d4联立求解上述方程，得s23

=[(−a3

−a2

c3

)pz

+(c1px

+s1py

)(a2s3

−d4

)][p

+(c1px

+s1py

)]2

c23

=[(a2s3

−d4

)pz

+(a3

+a2c3

)(c1px

+s1py

)][p

+(c1px

+s1py

)]2于是92

=Atan2[(−a3

−a2

c3

)pz

−(c1px

+s1py

)(d4

−a2s3

),(a2

s3

−d4

)pz

−(a3

+a2

c3

)(c1px

+s1py

)]−93z2z2−c4

c5s6

−s4

c6−s5s6s4

c5s6

−c4

c60|

−s4

c5c6

−

c4s6

|L

0

T

=

s5c663−c4s5

c5

s4s5

0「c4

c5c6

−s4s6a3

]d4

|0

|

1

」||4.5

机器人逆运动学实例求解

92：建立等式「c1c23|[

T(92

)]−1

T

=|−c1s236030−a2

c3

]「r11||−

d

0r3113px

]|py

|

3Tp1

」|由元素

(1,4)和(2,4)分别相等，有s1c23−s1s23c0−s−c00|−s1a2s3

|

|r211222320|L

01323330rrrrrr23231r13c1c23

+r23s1c23

−r33s23

=−c4s5

,−r13s1

+

r23c1

=

s4s5当s5

丰0时，可得94

=Atan2(−r13s1

+r23c1

,−r13c1c23

−r23s1c23

+r33s23

)当s5

=0时，操作臂处于奇异位形，此时轴4和轴6成一条直

线。在此情况下，所有结果(所有可能的解)都是94和96的

和或差，

94可以任意选取−c4

c5s6

−s4

c6−s5s6s4

c5s6

−c4

c60|

−s4

c5c6

−

c4s6

|L

0

T

=

s5c663−c4s5

c5

s4s5

0「c4

c5c6

−s4s6a3

]d4

|0

|

1

」||4.5

机器人逆运动学实例求解

94：建立等式−a2

c3

]「r11||−

d

0r3113「c1c23|3T

0T

=

|

−c1s230

6

|−

s1px

]|py

|

3Tp1

」|由元素

(1,3)和(3,3)分别相等，有s1c23−s1s23c0−s−c00a2s3

|

|r211222320|

L