第六讲 机器人运动学逆解

s
2
c2
0
0 0 1
0
0
0
0 1 00 0 1 0
4
0
0
0 1 0 0 0
1
0 0
c2 s2 0 300c2
s
2
c2
0
3
0
0s
2
0
01
0
0
00
1
8 0 0
z0 x0
3 z1 0
0x1
z2 x2
z3 x3
2
z4h
0
x4h
0
山东大学机械工程学院机电工程研究所2010/09/02
3.3 机器人运动学方程
2、运动学方程的逆解
多解性问题： 解得数量不仅与机器人的关节数有关，
还与它的杆件参数、关节活动范围等相关。 一般说，连杆的非零参数越多，解的数量 就越多，即到达某个位置的路经就越多。 多个解的存在使我们面临选择。
如何选择？如：路径最短、最近原则。 多解的应用： 躲避障碍物等。
山东大学机械工程学院机电工程研究所2010/09/02
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章 机器人运动学
3.1 机器人的位姿描述 3.2 齐次变换及运算 3.3 机器人运动学方程 3.4 机器人微分运动
山东大学机械工程学院机电工程研究所2010/09/02
3.3 机器人运动学方程
3.3.2小节 运 动 学 方 程 的 逆解
山东大学机械工程学院机电工程研究所2010/09/02
3.3 机器人运动学方程
山东大学机械工程学院机电工程研究所2010/09/02
山东大学机械工程学院机电工程研究所2010/09/02
2、运动学方程的逆解
上述方程组是由一些非线性的、超越 、难解的方程组成。为了降低求解难度， 机器人的杆件参数应仅可能地取为0，如 常见的PUMA机器人那样。对于任何非线 性方程组，必须关心其解的存在性、多解 性和求解方法。
山东大学机械工程学院机电工程研究所2010/09/02
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
解：（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 )
程
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 机器人运动学方程
解：（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）运动学方
研究其可解性。
其中：
nx c1[c23 (c4c5c6 s4s6 ) s23s5c6 ] s1(s4c5c6 c4s6 ) ny s1[c23 (c4c5c6 s4s6 ) s23s5c6 ] c1(s4c5c6 c4s6 ) nz s23 (c4c5c6 s4s6 ) c23s5c6
山东大学机械工程学院机电工程研究所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 机器人运动学方程
程
Mc01、 T相ra邻ns杆(0,件0,8位00)姿 R矩ot阵(z,1 ) Trans(400,0,0)
c1 s1 0 0 1 0 0 400
s1
c1
0
0
0
1
0
0
0 0 1 8000 0 1 0
0
0
0
1
0
0
0
1
40 0
c1 s1 0 400c1
s1
c1
0
4
0
0s
1
0 0 1 800
2、运动学方程的逆解
解得存在性： 解是否存在与机器人的工作空间密切
相关，工作空间又取决于机器人的结构、 杆件参数，或手部（工具）的位姿。
一般情况下，如果手部坐标系的位置 和姿态都位于工作空间内，则至少存在一 个解；相反，若手部坐标系的位置和姿态 都位于工作空间外，则无解。
山东大学机械工程学院机电工程研究所2010/09/02
1 800 θ1 400 0 θ1 2 0 θ2 300 0 θ2 3 d3 0 0 0 d3 4 -200 θ4 0 0 θ4
400
300
z1
z2
1
x1
x2
2
z3
3 x3
z4h
200
800
x4h
z0 x0
0
山东大学机械工程学院机电工程研究所2010/09/02
3.3 机器人运动学方程
解：（1）运动学方
机器人运动学方程的逆解，也称机器 人的逆运动学问题，或间接位置求解。
逆运动学问题：对某个机器人，当给 出机器人手部在基座标系中所处的位置和
姿态时（即M0h中各元素给定），求出其对 应的关节变量值qi。
山东大学机械工程学院机电工程研究所2010/09/02
2、运动学方程的逆解
逆运动学问题的可解性： 下面以六自由度机器人PUMA为例，
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
将两式相加得：
cos2
px2
p
2 y
l12
l22
2l1l2
c 21c 2 c1s1s 2 s 21c2 c1s1s 2
则 ： 2
co
s1
p
2 x
p
2 y
l12
2l1l2
l22
山东大学机械工程学院机电工程研究所2010/09/02
c124 nx oy (a)
3.3 机器人运动学方程 s124 ny ox (b) l1c1 l2c12 px (c)
3.3 机器人运动学方程
运动学逆解的求解方法 不像线性方程，不存在求解非线性方程
组的通用算法。 非线性方程组的算法应能求出它的所
有解；因此，某些数值递推方法不适用。 逆解的形式：
1)闭式解(Close-form solution):用解 析函数式表示解。 特点：求解速度快。
存在闭式解是机器人设计的目标，仅仅 在一些特殊情况下，机器人存在解析的闭式 解，如：相邻的多个关节轴交与一点，杆件 扭角等于0或90度等。
山东大学机械工程学院机电工程研究所2010/09/02
3.3 机器人运动学方程
解：（3）逆解数学表达式 联立方程：
c124 nx oy (a) s124 ny ox (b) l1c1 l2c12 px (c) l1s1 l2s12 py (d )
d1 d3 d4 pz (e)
3.3 机器人运动学方程
例1：已知四轴平面关节SCARA机
器
人如图所示，试计算：
（1）机器人的运动学方程；
（2）当关节变量取
qi=[30°，-60°，－120，90°]T 时，机器人手部的位置和姿态；
（3）机器人运动学逆解的数学
表达式。
800
400
300
200
山东大学机械工程学院机电工程研究所2010/09/02
ox c1[c23 (c4c5s6 s4c6 ) s23s5s6 ] s1(s4c5s6 c4c6 ) oy s1[c23 (c4c5s6 s4c6 ) s23s5s6 ] c1(s4c5s6 c4c6 ) oz s23 (c4c5c6 s4c6 ) c23s5s6
山东大学机械工程学院机电工程研究所2010/09/02
前置模式：
{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
其中：
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个 未知数。 因此，一般情况下，单从数学的角度看 ，方程组应该是有解的。
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
解：（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个元素相 互独立，与变量数相同。
山东大学机械工程学院机电工程研究所2010/09/02
3.3 机器人运动学方程
2)数值解(Numerical solution): 特点：递推求解。 求解方法分类：
代数法、几何法以及数值法，前两种 用于求闭式解，后一种用于数值解。
下面我们结合几个实例，介绍机器人 闭式解析解的求解方法。
山东大学机械工程学院机电工程研究所2010/09/02
解：（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
其中，nx、ny、px、py和pz是已知的手的位姿，θ1、
θ2、θ4及d3是待求的未知量。
山东大学机械工程学院机电工程研究所2010/09/02
Fra Baidu bibliotek
3.3 机器人运动学方程
解：（3）逆解数学表达式 由上面（a）、（b）两式可得 ：
c124 cos(1 2 4 ) nx s124 sin(1 2 4 ) ny