运动学与雅可比矩阵举例

以三连杆平面机械臂为例，来说明D-H矩阵和雅可比矩阵的求法，并且比较后置法和前置法的异同和运动学末端位姿的一致性和雅可比矩阵的一致性。

一、 后置法。

（1）、模型、坐标系及D-H参数表。 （2）、齐次矩阵。

如图建立个连杆的坐标系，D-H参数如下表所示：

连杆aalphadtheta1a100theta12a200theta23a300theta3

对应的齐次矩阵是：

Ti−1i(θi)=⎡⎣⎢⎢⎢cisi00−sici000010aiciaisi01⎤⎦⎥⎥⎥ T i i − 1 ( θ i ) = [ c i − s i 0 a i c i s i c i 0 a i s i 0 0 1 0 0 0 0 1 ] （3）、运动学方程。 To3(q)=⎡⎣⎢⎢⎢c123s12300−s123c123000010a1c1+a1c12+a1c123a1s1+a1s12+a1s12301⎤⎦⎥⎥⎥ T 3 o ( q ) = [ c 123 − s 123 0 a 1 c 1 + a 1 c 12 + a 1 c 123 s 123 c 123 0 a 1 s 1 + a 1 s 12 + a 1 s 123 0 0 1 0 0 0 0 1 ] （4）、雅可比矩阵。 To1(q)=⎡⎣⎢⎢⎢c1s100−s1c1000010a1c1a1s101⎤⎦⎥⎥⎥ T 1 o ( q ) = [ c 1 − s 1 0 a 1 c 1 s 1 c 1 0 a 1 s 1 0 0 1 0 0 0 0 1 ] T02(q)=⎡⎣⎢⎢⎢c12s1200−s12c12000010a1c1+a1c12a1s1+a1s1201⎤⎦⎥⎥⎥ T 2 0 ( q ) = [ c 12 − s 12 0 a 1 c 1 + a 1 c 12 s 12 c 12 0 a 1 s 1 + a 1 s 12 0 0 1 0 0 0 0 1 ] 根据以上方程，可以得到： z0=z1=z2=z3=⎡⎣⎢001⎤⎦⎥ z 0 = z 1 = z 2 = z 3 = [ 0 0 1 ] p0=⎡⎣⎢000⎤⎦⎥ p 0 = [ 0 0 0 ] p1=⎡⎣⎢a1c1a1s10⎤⎦⎥ p 1 = [ a 1 c 1 a 1 s 1 0 ] p2=⎡⎣⎢a1c1+a1c12a1s1+a1s120⎤⎦⎥ p 2 = [ a 1 c 1 + a 1 c 12 a 1 s 1 + a 1 s 12 0 ] p3=⎡⎣⎢a1c1+a1c12+a1c123a1s1+a1s12+a1s1230⎤⎦⎥ p 3 = [ a 1 c 1 + a 1 c 12 + a 1 c 123 a 1 s 1 + a 1 s 12 + a 1 s 123 0 ] 根据旋转关节的雅可比计算公式， JPi=[Zi−1∗(Pe−Pi−1)Zi−1] J P i = [ Z i − 1 ∗ ( P e − P i − 1 ) Z i − 1 ] 可以得到： J=⎡⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢−s1(a2c2+a3c23)c1(a2c2+a3c23)0001−c1(a2s2+a3s23)−s1(a2s2+a3s23)a2c2+a3c23s1−c10−a3c1s23−a3s1s23a3c23s1−c10⎤⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥ J = [ − s 1 ( a 2 c 2 + a 3 c 23 ) − c 1 ( a 2 s 2 + a 3 s 23 ) − a 3 c 1 s 23 c 1 ( a 2 c 2 + a 3 c 23 ) − s 1 ( a 2 s 2 + a 3 s 23 ) − a 3 s 1 s 23 0 a 2 c 2 + a 3 c 23 a 3 c 23 0 s 1 s 1 0 − c 1 − c 1 1 0 0 ]

二、 前置法。

（1）、模型、坐标系及D-H参数表。 （2）、齐次矩阵。

如图建立个连杆的坐标系，D-H参数如下表所示：

连杆aalphadtheta1000theta12a100theta23a200theta34a300

对应的齐次矩阵是：

To1(θ1)=⎡⎣⎢⎢⎢c1s100−s1c10000100001⎤⎦⎥⎥⎥ T 1 o ( θ 1 ) = [ c 1 − s 1 0 0 s 1 c 1 0 0 0 0 1 0 0 0 0 1 ] Ti−1i(θi)=⎡⎣⎢⎢⎢cisi00−sici000010ai−1ciai−1si01⎤⎦⎥⎥⎥i=2,3 T i i − 1 ( θ i ) = [ c i − s i 0 a i − 1 c i s i c i 0 a i − 1 s i 0 0 1 0 0 0 0 1 ] i = 2 , 3 T34(θi)=⎡⎣⎢⎢⎢100001000010a3001⎤⎦⎥⎥⎥ T 4 3 ( θ i ) = [ 1 0 0 a 3 0 1 0 0 0 0 1 0 0 0 0 1 ] （3）、运动学方程。 To4(q)=⎡⎣⎢⎢⎢c123s12300−s123c123000010a1c1+a1c12+a1c123a1s1+a1s12+a1s12301⎤⎦⎥⎥⎥ T 4 o ( q ) = [ c 123 − s 123 0 a 1 c 1 + a 1 c 12 + a 1 c 123 s 123 c 123 0 a 1 s 1 + a 1 s 12 + a 1 s 123 0 0 1 0 0 0 0 1 ] （4）、雅可比矩阵。 To1(θ1)=⎡⎣⎢⎢⎢c1s100−s1c10000100001⎤⎦⎥⎥⎥ T 1 o ( θ 1 ) = [ c 1 − s 1 0 0 s 1 c 1 0 0 0 0 1 0 0 0 0 1 ] To2(q)=⎡⎣⎢⎢⎢c1s100−s1c1000010a1c1a1s101⎤⎦⎥⎥⎥ T 2 o ( q ) = [ c 1 − s 1 0 a 1 c 1 s 1 c 1 0 a 1 s 1 0 0 1 0 0 0 0 1 ] T03(q)=⎡⎣⎢⎢⎢c12s1200−s12c12000010a1c1+a1c12a1s1+a1s1201⎤⎦⎥⎥⎥ T 3 0 ( q ) = [ c 12 − s 12 0 a 1 c 1 + a 1 c 12 s 12 c 12 0 a 1 s 1 + a 1 s 12 0 0 1 0 0 0 0 1 ] 根据以上方程，可以得到： z0=z1=z2=z3=z4=⎡⎣⎢001⎤⎦⎥ z 0 = z 1 = z 2 = z 3 = z 4 = [ 0 0 1 ] p0=p1=⎡⎣⎢000⎤⎦⎥ p 0 = p 1 = [ 0 0 0 ] p2=⎡⎣⎢a1c1a1s10⎤⎦⎥ p 2 = [ a 1 c 1 a 1 s 1 0 ] p3=⎡⎣⎢a1c1+a1c12a1s1+a1s120⎤⎦⎥ p 3 = [ a 1 c 1 + a 1 c 12 a 1 s 1 + a 1 s 12 0 ] p4=⎡⎣⎢a1c1+a1c12+a1c123a1s1+a1s12+a1s1230⎤⎦⎥ p 4 = [ a 1 c 1 + a 1 c 12 + a 1 c 123 a 1 s 1 + a 1 s 12 + a 1 s 123 0 ] 根据旋转关节的雅可比计算公式， JPi=[Zi∗(Pe−Pi)Zi] J P i = [ Z i ∗ ( P e − P i ) Z i ] 可以得到： J=⎡⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢−s1(a2c2+a3c23)c1(a2c2+a3c23)0001−c1(a2s2+a3s23)−s1(a2s2+a3s23)a2c2+a3c23s1−c10−a3c1s23−a3s1s23a3c23s1−c10⎤⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥ J = [ − s 1 ( a 2 c 2 + a 3 c 23 ) − c 1 ( a 2 s 2 + a 3 s 23 ) − a 3 c 1 s 23 c 1 ( a 2 c 2 + a 3 c 23 ) − s 1 ( a 2 s 2 + a 3 s 23 ) − a 3 s 1 s 23 0 a 2 c 2 + a 3 c 23 a 3 c 23 0 s 1 s 1 0 − c 1 − c 1 1 0 0 ]

三、 比较说明。

通过比较可以发现，后置法和前置法计算的运动学方程和雅可比矩阵是相同的。

四、 参考文献

1) John J Craig, 机器人学导论（第三版），机械工业出版社，2006.6. 2) Saeed B.Niku 等，机器人学导论——分析、系统及应用，电子工业出版社，2004.1. 3) Bruno Siciliano 等，机器人学 建模、规划与控制，西安交通大学出版社，2013.11.