第六讲 机器人运动学逆解

解：（1）运动学方程 d、建立方程
c124 s124 0 400c1 300c12
M0h
M 01
M 12
M 23
M 34(h)
s
124
0
0
c124
0 0
0
4 0 0s 1
3 0 0s 12
1 0
600 d3 1
式中：c124 cos(1 2 4 ), s124 sin(1 2 4 ) c12 cos(1 2 ),s12 sin(1 2 )
解：（3）逆解数学表达式
已知运动学方程，用通式表示为：
已知
关系
nx ox ax px c124 s124 0 l1c1 l2c12
n y
oy
ay
py
s
124
c124
0
l1 s 1
l2
s12
nz 0
oz 0
az 0
pz 1
0
0
0 0
1 0
d1
d3 1
d4
分析：上述矩阵方程有4个未知量，由于第一行第一列元 素与第二行第二列元素相等，第一行第二列元素与第二 行第一列元素大小相等、符号相反；因此，仅4个元素相 互独立，与变量数相同。
2、运动学方程的逆解
解得存在性： 解是否存在与机器人的工作空间密切
相关，工作空间又取决于机器人的结构、 杆件参数，或手部（工具）的位姿。
一般情况下，如果手部坐标系的位置 和姿态都位于工作空间内，则至少存在一 个解；相反，若手部坐标系的位置和姿态 都位于工作空间外，则无解。
山东大学机械工程学院机电工程研究所2010/09/02
解：（1）运动学方
程
cM、2相3 邻T杆r件an位s(姿0,矩0,阵d3 )
1 0 0 0
0 1 0
0
0 0
0 0
1 0
d3 1
400
300
z1
z2
x1
x2
z3 x3
z4h
200
800
x4h
z0 x0
山东大学机械工程学院机电工程研究所2010/09/02
3.3 机器人运动学方程
解：（1）运动学方
0
00
1
80 0
z0 x0
z1
30 0
x1
20 0
z2 x2
z3 x3
z4h x4h
山东大学机械工程学院机电工程研究所2010/09/02
3.3 机器人运动学方程
解：（1）运动学方
程
Mc1、2 相Ro邻t(杆z,件2 ) 位 Tr姿an矩s(3阵00,0,0)
c2 s2 0 0 1 0 0 300
解：（3）逆解数学表达式
l1s1 l2s12 py (d )
d1 d3 d4 pz (e)
为了求θ1，由上面（c）、（d）两式展开可
得：
l1c1 l（2 c1c2 s1s2） px
l1s1 l（2 s1c2 c1s2） py
化简，得：
这时2 已经求出。
(l1 l2c2 )c1－(l2s2 )s1 px (l2s2 )c1 (l1 l2c2 )s1 py
3.3 机器人运动学方程
解：
1）运动学方程
400
a、建立坐标系（前置模式）
机座坐标系｛0｝
1
杆件坐标系｛i｝
手部坐标系｛h｝
800 0
300
z1 x1
2
200
z2 x2
z3 3 x3
z4h x4h
z0 x0
山东大学机械工程学院机电工程研究所2010/09/02
3.3 机器人运动学方程
解：（1）运动学方 程 b、i 确d定i 参θ数i li αi qi
2、运动学方程的逆解
ax c1(c23c4c5 s23c5 ) s1s4s5 ay s1(c23c4c5 s23c5 ) c1s4s5 az s23c4c5 c23c5
px c1[d6 (c23c4s5 s23c5 ) d4s23 l2c2 ] s1(d6s4s5 d2 ) py s1[d6 (c23c4s5 s23c5 ) d4s23 l2c2 ] c1(d6s4s5 d2 ) pz d6 (c23c5 s23c4c5 ) d4c23 l2s2
山东大学机械工程学院机电工程研究所2010/09/02
其中：
cij cosi cos j sini sin j cos(i j ) sij cosi sin j sini cos j sin(i j )
山东大学机械工程学院机电工程研究所2010/09/02
2、运动学方程的逆解
可见，我们有12个方程及6个未知数。 上述12个方程关系如何？ 我们先看看转动部分，它是3X3子矩阵 ，共有9个元素；我们知道，转动矩阵的每 列都是单位矢量，并且每列之间都两两正交 ；因此，9个元素中仅三个是独立的，或则 说，12个方程中仅有6个是独立，对应6个 未知数。 因此，一般情况下，单从数学的角度看 ，方程组应该是有解的。
研究其可解性。
其中：
nx c1[c23 (c4c5c6 s4s6 ) s23s5c6 ] s1(s4c5c6 c4s6 ) ny s1[c23 (c4c5c6 s4s6 ) s23s5c6 ] c1(s4c5c6 c4s6 ) nz s23 (c4c5c6 s4s6 ) c23s5c6
前置模式：
{i-1}→坐标系{i} 。 仅涉及i杆件的参数，
1、杆长:沿xi轴从zi-1到zi的距离。 2、扭角：绕xi从zi-1转到zi的角度。 3、平移量：沿zi-1轴从xi-1轴量至xi轴的距离。 4、转角：绕zi-1轴从xi-1轴到xi的转角。
山东大学机械工程学院机电工程研究所2010/09/02
山东大学机械工程学院机电工程研究所2010/09/02
3.3 机器人运动学方程
2)数值解(Numerical solution): 特点：递推求解。 求解方法分类：
代数法、几何法以及数值法，前两种 用于求闭式解，后一种用于数值解。
下面我们结合几个实例，介绍机器人 闭式解析解的求解方法。
山东大学机械工程学院机电工程研究所2010/09/02
1
2
4
tan 1
ny nx
山东大学机械工程学院机电工程研究所2010/09/02
3.3 机器人运动学方程
由上面（c）、（d）两式：l1 cos1 l2 co（ s 1 2） px l1 sin1 l2 sin（1 2） py
两边平方可得 ：
l12 cos2 1 2l1l2 cos1 cos(1 2 ) l22 cos（2 1 2） px2 l12 sin2 1 2l1l2 sin1 sin(1 2 ) l22 sin（2 1 2） py2
山东大学机械工程学院机电工程研究所2010/09/02
2、运动学方程的逆解
上述方程组是由一些非线性的、超越 、难解的方程组成。为了降低求解难度， 机器人的杆件参数应仅可能地取为0，如 常见的PUMA机器人那样。对于任何非线 性方程组，必须关心其解的存在性、多解 性和求解方法。
山东大学机械工程学院机电工程研究所2010/09/02
2、运动学方程的逆解
多解性问题： 解得数量不仅与机器人的关节数有关，
还与它的杆件参数、关节活动范围等相关。 一般说，连杆的非零参数越多，解的数量 就越多，即到达某个位置的路经就越多。 多个解的存在使我们面临选择。
如何选择？如：路径最短、最近原则。 多解的应用： 躲避障碍物等。
山东大学机械工程学院机电工程研究所2010/09/02
程
M3c4、(h) 相 T邻r杆an件s(0位,0姿,矩20阵0) Rot(z,4 )
c4 s4 0 0
s
4
c4
0
0
0 0 1 200
0
0
0
1
800
400
300
z1
z2
x1
x2
z3 x3
z4h 200
x4h
z0 x0
山东大学机械工程学院机电工程研究所2010/09/02
3.3 机器人运动学方程
将两式相加得：
cos2
px2
p
2 y
l12
l22
2l1l2
c 21c 2 c1s1s 2 s 21c2 c1s1s 2
则 ： 2
co
s1
p
2 x
p
y
l12
2l1l2
l22
山东大学机械工程学院机电工程研究所2010/09/02
c124 nx oy (a)
3.3 机器人运动学方程 s124 ny ox (b) l1c1 l2c12 px (c)
机器人运动学方程的逆解，也称机器 人的逆运动学问题，或间接位置求解。
逆运动学问题：对某个机器人，当给 出机器人手部在基座标系中所处的位置和
姿态时（即M0h中各元素给定），求出其对 应的关节变量值qi。
山东大学机械工程学院机电工程研究所2010/09/02
2、运动学方程的逆解
逆运动学问题的可解性： 下面以六自由度机器人PUMA为例，
山东大学机械工程学院机电工程研究所2010/09/02
3.3 机器人运动学方程
解：（2）已知qi=[30°，-60°，－120，90°]T，则：
1
3
2
2
0 350 3
M0h
3 2
1 2
0
50
0 0 1 480
0 0 0 1
山东大学机械工程学院机电工程研究所2010/09/02
3.3 机器人运动学方程
山东大学机械工程学院机电工程研究所2010/09/02
3.3 机器人运动学方程