运动学与雅可比矩阵举例
以三连杆平面机械臂为例，来说明D-H矩阵和雅可比矩阵的求法，并且比较后置法和前置法的异同和运动学末端位姿的一致性和雅可比矩阵的一致性。一、后置法。（1）、模型、坐标系及D-H参数表。（2）、齐次矩阵。如图建立个连杆的坐标系，D-H参数如下表所示：连杆aalphadtheta1a100theta...
·
以三连杆平面机械臂为例，来说明D-H矩阵和雅可比矩阵的求法，并且比较后置法和前置法的异同和运动学末端位姿的一致性和雅可比矩阵的一致性。
一、 后置法。
（1）、模型、坐标系及D-H参数表。 
（2）、齐次矩阵。
如图建立个连杆的坐标系，D-H参数如下表所示：
| 连杆 | a | alpha | d | theta |
|---|---|---|---|---|
| 1 | a1 | 0 | 0 | theta1 |
| 2 | a2 | 0 | 0 | theta2 |
| 3 | a3 | 0 | 0 | theta3 |
对应的齐次矩阵是：
Ti−1i(θi)=⎡⎣⎢⎢⎢cisi00−sici000010aiciaisi01⎤⎦⎥⎥⎥Tii−1(θi)=[ci−si0aicisici0aisi00100001]
<script type="math/tex; mode=display" id="MathJax-Element-81">T_i^{i-1}(\theta_i)= \begin{bmatrix}c_i & -s_i & 0 & a_ic_i\\s_i & c_i & 0 & a_is_i\\0 & 0 & 1 & 0 \\0 & 0 & 0 & 1 \end{bmatrix}</script>
（3）、运动学方程。
To3(q)=⎡⎣⎢⎢⎢c123s12300−s123c123000010a1c1+a1c12+a1c123a1s1+a1s12+a1s12301⎤⎦⎥⎥⎥T3o(q)=[c123−s1230a1c1+a1c12+a1c123s123c1230a1s1+a1s12+a1s12300100001]
<script type="math/tex; mode=display" id="MathJax-Element-82">T_3^{o}(q)=\begin{bmatrix}c_{123} & -s_{123} & 0 & a_1c_1+a_1c_{12}+a_1c_{123}\\s_{123} & c_{123} & 0 & a_1s_1+a_1s_{12}+a_1s_{123}\\0 & 0 & 1 & 0 \\0 & 0 & 0 & 1 \end{bmatrix}</script>
（4）、雅可比矩阵。
To1(q)=⎡⎣⎢⎢⎢c1s100−s1c1000010a1c1a1s101⎤⎦⎥⎥⎥T1o(q)=[c1−s10a1c1s1c10a1s100100001]
<script type="math/tex; mode=display" id="MathJax-Element-83">T_1^{o}(q)= \begin{bmatrix}c_{1} & -s_{1} & 0 & a_1c_1\\s_{1} & c_{1} & 0 & a_1s_1\\0 & 0 & 1 & 0 \\0 & 0 & 0 & 1 \end{bmatrix}</script>
T02(q)=⎡⎣⎢⎢⎢c12s1200−s12c12000010a1c1+a1c12a1s1+a1s1201⎤⎦⎥⎥⎥T20(q)=[c12−s120a1c1+a1c12s12c120a1s1+a1s1200100001]
<script type="math/tex; mode=display" id="MathJax-Element-84">T_2^{0}(q)= \begin{bmatrix}c_{12} & -s_{12} & 0 & a_1c_1+a_1c_{12}\\s_{12} & c_{12} & 0 & a_1s_1+a_1s_{12}\\0 & 0 & 1 & 0 \\0 & 0 & 0 & 1\end{bmatrix}</script>
根据以上方程，可以得到：
z0=z1=z2=z3=⎡⎣⎢001⎤⎦⎥z0=z1=z2=z3=[001]
<script type="math/tex; mode=display" id="MathJax-Element-85">z_0=z_1=z_2=z_3=\begin{bmatrix} 0\\ 0\\ 1 \end{bmatrix}</script>
p0=⎡⎣⎢000⎤⎦⎥p0=[000]
<script type="math/tex; mode=display" id="MathJax-Element-86">p_0=\begin{bmatrix} 0\\ 0\\ 0 \end{bmatrix}</script>
p1=⎡⎣⎢a1c1a1s10⎤⎦⎥p1=[a1c1a1s10]
<script type="math/tex; mode=display" id="MathJax-Element-87">p_1=\begin{bmatrix} a_1c_1\\ a_1s_1\\0 \end{bmatrix}</script>
p2=⎡⎣⎢a1c1+a1c12a1s1+a1s120⎤⎦⎥p2=[a1c1+a1c12a1s1+a1s120]
<script type="math/tex; mode=display" id="MathJax-Element-88">p_2=\begin{bmatrix} a_1c_1+a_1c_{12}\\ a_1s_1+a_1s_{12}\\ 0 \end{bmatrix}</script>
p3=⎡⎣⎢a1c1+a1c12+a1c123a1s1+a1s12+a1s1230⎤⎦⎥p3=[a1c1+a1c12+a1c123a1s1+a1s12+a1s1230]
<script type="math/tex; mode=display" id="MathJax-Element-89">p_3=\begin{bmatrix} a_1c_1+a_1c_{12}+a_1c_{123}\\a_1s_1+a_1s_{12}+a_1s_{123}\\ 0 \end{bmatrix}</script>
根据旋转关节的雅可比计算公式，
JPi=[Zi−1∗(Pe−Pi−1)Zi−1]JPi=[Zi−1∗(Pe−Pi−1)Zi−1]
<script type="math/tex; mode=display" id="MathJax-Element-90">J_{P_i}=\begin{bmatrix}Z_{i-1}*(P_e-P_{i-1})\\Z_{i-1}\end{bmatrix}</script>
可以得到：
J=⎡⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢−s1(a2c2+a3c23)c1(a2c2+a3c23)0001−c1(a2s2+a3s23)−s1(a2s2+a3s23)a2c2+a3c23s1−c10−a3c1s23−a3s1s23a3c23s1−c10⎤⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥J=[−s1(a2c2+a3c23)−c1(a2s2+a3s23)−a3c1s23c1(a2c2+a3c23)−s1(a2s2+a3s23)−a3s1s230a2c2+a3c23a3c230s1s10−c1−c1100]
<script type="math/tex; mode=display" id="MathJax-Element-91">J=\begin{bmatrix}-s_1(a_2c_2+a_3c_{23}) & -c_1(a_2s_2+a_3s_{23} )& -a_3c_1s_{23}\\c_1(a_2c_2+a_3c_{23}) & -s_1(a_2s_2+a_3s_{23}) & -a_3s_1s_{23}\\0 & a_2c_2+a_3c_{23} & a_3c_{23} \\0 & s_1 & s_1 \\ 0 & -c_1 & -c_1 \\1 & 0 & 0\end{bmatrix}</script>
二、 前置法。
（1）、模型、坐标系及D-H参数表。 
（2）、齐次矩阵。
如图建立个连杆的坐标系，D-H参数如下表所示：
| 连杆 | a | alpha | d | theta |
|---|---|---|---|---|
| 1 | 0 | 0 | 0 | theta1 |
| 2 | a1 | 0 | 0 | theta2 |
| 3 | a2 | 0 | 0 | theta3 |
| 4 | a3 | 0 | 0 |
对应的齐次矩阵是：
To1(θ1)=⎡⎣⎢⎢⎢c1s100−s1c10000100001⎤⎦⎥⎥⎥T1o(θ1)=[c1−s100s1c10000100001]
<script type="math/tex; mode=display" id="MathJax-Element-134">T_1^{o}(\theta_1)= \begin{bmatrix}c_{1} & -s_{1} & 0 & 0\\s_{1} & c_{1} & 0 & 0\\0 & 0 & 1 & 0 \\0 & 0 & 0 & 1 \end{bmatrix}</script>
Ti−1i(θi)=⎡⎣⎢⎢⎢cisi00−sici000010ai−1ciai−1si01⎤⎦⎥⎥⎥i=2,3Tii−1(θi)=[ci−si0ai−1cisici0ai−1si00100001]i=2,3
<script type="math/tex; mode=display" id="MathJax-Element-135">T_i^{i-1}(\theta_i)=\begin{bmatrix}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 \end{bmatrix}i=2,3</script>
T34(θi)=⎡⎣⎢⎢⎢100001000010a3001⎤⎦⎥⎥⎥T43(θi)=[100a3010000100001]
<script type="math/tex; mode=display" id="MathJax-Element-136">T_4^{3}(\theta_i)=\begin{bmatrix}1 & 0 & 0 & a_3\\0 & 1 & 0 & 0\\0 & 0 & 1 & 0 \\0 & 0 & 0 & 1\end{bmatrix}</script>
（3）、运动学方程。
To4(q)=⎡⎣⎢⎢⎢c123s12300−s123c123000010a1c1+a1c12+a1c123a1s1+a1s12+a1s12301⎤⎦⎥⎥⎥T4o(q)=[c123−s1230a1c1+a1c12+a1c123s123c1230a1s1+a1s12+a1s12300100001]
<script type="math/tex; mode=display" id="MathJax-Element-137">T_4^{o}(q)=\begin{bmatrix}c_{123} & -s_{123} & 0 & a_1c_1+a_1c_{12}+a_1c_{123}\\s_{123} & c_{123} & 0 & a_1s_1+a_1s_{12}+a_1s_{123}\\0 & 0 & 1 & 0 \\0 & 0 & 0 & 1 \end{bmatrix}</script>
（4）、雅可比矩阵。
To1(θ1)=⎡⎣⎢⎢⎢c1s100−s1c10000100001⎤⎦⎥⎥⎥T1o(θ1)=[c1−s100s1c10000100001]
<script type="math/tex; mode=display" id="MathJax-Element-138">T_1^{o}(\theta_1)=\begin{bmatrix}c_{1} & -s_{1} & 0 & 0\\s_{1} & c_{1} & 0 & 0\\0 & 0 & 1 & 0 \\0 & 0 & 0 & 1\end{bmatrix}</script>
To2(q)=⎡⎣⎢⎢⎢c1s100−s1c1000010a1c1a1s101⎤⎦⎥⎥⎥T2o(q)=[c1−s10a1c1s1c10a1s100100001]
<script type="math/tex; mode=display" id="MathJax-Element-139">T_2^{o}(q)= \begin{bmatrix}c_{1} & -s_{1} & 0 & a_1c_1\\s_{1} & c_{1} & 0 & a_1s_1\\0 & 0 & 1 & 0 \\0 & 0 & 0 & 1 \end{bmatrix}</script>
T03(q)=⎡⎣⎢⎢⎢c12s1200−s12c12000010a1c1+a1c12a1s1+a1s1201⎤⎦⎥⎥⎥T30(q)=[c12−s120a1c1+a1c12s12c120a1s1+a1s1200100001]
<script type="math/tex; mode=display" id="MathJax-Element-140">T_3^{0}(q)= \begin{bmatrix}c_{12} & -s_{12} & 0 & a_1c_1+a_1c_{12}\\s_{12} & c_{12} & 0 & a_1s_1+a_1s_{12}\\0 & 0 & 1 & 0 \\0 & 0 & 0 & 1\end{bmatrix}</script>
根据以上方程，可以得到：
z0=z1=z2=z3=z4=⎡⎣⎢001⎤⎦⎥z0=z1=z2=z3=z4=[001]
<script type="math/tex; mode=display" id="MathJax-Element-141">z_0=z_1=z_2=z_3=z_4=\begin{bmatrix} 0\\ 0\\ 1 \end{bmatrix}</script>
p0=p1=⎡⎣⎢000⎤⎦⎥p0=p1=[000]
<script type="math/tex; mode=display" id="MathJax-Element-142">p_0=p_1=\begin{bmatrix} 0\\ 0\\ 0 \end{bmatrix}</script>
p2=⎡⎣⎢a1c1a1s10⎤⎦⎥p2=[a1c1a1s10]
<script type="math/tex; mode=display" id="MathJax-Element-143">p_2=\begin{bmatrix} a_1c_1\\ a_1s_1\\0 \end{bmatrix}</script>
p3=⎡⎣⎢a1c1+a1c12a1s1+a1s120⎤⎦⎥p3=[a1c1+a1c12a1s1+a1s120]
<script type="math/tex; mode=display" id="MathJax-Element-144">p_3=\begin{bmatrix} a_1c_1+a_1c_{12}\\ a_1s_1+a_1s_{12}\\ 0 \end{bmatrix}</script>
p4=⎡⎣⎢a1c1+a1c12+a1c123a1s1+a1s12+a1s1230⎤⎦⎥p4=[a1c1+a1c12+a1c123a1s1+a1s12+a1s1230]
<script type="math/tex; mode=display" id="MathJax-Element-145">p_4=\begin{bmatrix} a_1c_1+a_1c_{12}+a_1c_{123}\\a_1s_1+a_1s_{12}+a_1s_{123}\\ 0 \end{bmatrix}</script>
根据旋转关节的雅可比计算公式，
JPi=[Zi∗(Pe−Pi)Zi]JPi=[Zi∗(Pe−Pi)Zi]
<script type="math/tex; mode=display" id="MathJax-Element-146">J_{P_i}=\begin{bmatrix}Z_{i}*(P_e-P_{i})\\Z_{i}\end{bmatrix}</script>
可以得到：
J=⎡⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢−s1(a2c2+a3c23)c1(a2c2+a3c23)0001−c1(a2s2+a3s23)−s1(a2s2+a3s23)a2c2+a3c23s1−c10−a3c1s23−a3s1s23a3c23s1−c10⎤⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥J=[−s1(a2c2+a3c23)−c1(a2s2+a3s23)−a3c1s23c1(a2c2+a3c23)−s1(a2s2+a3s23)−a3s1s230a2c2+a3c23a3c230s1s10−c1−c1100]
<script type="math/tex; mode=display" id="MathJax-Element-147">J=\begin{bmatrix}-s_1(a_2c_2+a_3c_{23}) & -c_1(a_2s_2+a_3s_{23} )& -a_3c_1s_{23}\\c_1(a_2c_2+a_3c_{23}) & -s_1(a_2s_2+a_3s_{23}) & -a_3s_1s_{23}\\0 & a_2c_2+a_3c_{23} & a_3c_{23} \\0 & s_1 & s_1 \\ 0 & -c_1 & -c_1 \\1 & 0 & 0\end{bmatrix}</script>
三、 比较说明。
通过比较可以发现，后置法和前置法计算的运动学方程和雅可比矩阵是相同的。
四、 参考文献
1) John J Craig, 机器人学导论（第三版），机械工业出版社，2006.6.
2) Saeed B.Niku 等，机器人学导论——分析、系统及应用，电子工业出版社，2004.1.
3) Bruno Siciliano 等，机器人学 建模、规划与控制，西安交通大学出版社，2013.11.
欢迎加入 MCP 技术社区！与志同道合者携手前行，一同解锁 MCP 技术的无限可能！
更多推荐
9
0- 0
已为社区贡献3条内容
所有评论(0)