运动学与雅可比矩阵举例

最新推荐文章于 2025-06-19 20:23:34 发布

原创最新推荐文章于 2025-06-19 20:23:34 发布 · 1.5w 阅读

62 ·

CC 4.0 BY-SA版权

机器人学专栏收录该内容

22 篇文章

订阅专栏

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

一、后置法。

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

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

连杆	a	theta
1	a1	theta1
2	a2	theta2
3	a3	theta3

对应的齐次矩阵是：

T i - 1 i (θ i) = ⎡ ⎣ ⎢ ⎢ ⎢ c i s i 00 - s i c i 00 0010 a i c i a i s i 01 ⎤ ⎦ ⎥ ⎥ ⎥

$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}$
（3）、运动学方程。

T o 3 (q) = ⎡ ⎣ ⎢ ⎢ ⎢ c 123 s 123 00 - s 123 c 123 00 0010 a 1 c 1 + a 1 c 12 + a 1 c 123 a 1 s 1 + a 1 s 12 + a 1 s 123 01 ⎤ ⎦ ⎥ ⎥ ⎥

$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}$
（4）、雅可比矩阵。

T o 1 (q) = ⎡ ⎣ ⎢ ⎢ ⎢ c 1 s 1 00 - s 1 c 1 00 0010 a 1 c 1 a 1 s 1 01 ⎤ ⎦ ⎥ ⎥ ⎥

$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}$

T 02 (q) = ⎡ ⎣ ⎢ ⎢ ⎢ c 12 s 12 00 - s 12 c 12 00 0010 a 1 c 1 + a 1 c 12 a 1 s 1 + a 1 s 12 01 ⎤ ⎦ ⎥ ⎥ ⎥

$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}$
根据以上方程，可以得到：

z 0 = z 1 = z 2 = z 3 = ⎡ ⎣ ⎢ 001 ⎤ ⎦ ⎥

$z_0=z_1=z_2=z_3=\begin{bmatrix} 0\\ 0\\ 1 \end{bmatrix}$

p 0 = ⎡ ⎣ ⎢ 000 ⎤ ⎦ ⎥

$p_0=\begin{bmatrix} 0\\ 0\\ 0 \end{bmatrix}$

p 1 = ⎡ ⎣ ⎢ a 1 c 1 a 1 s 1 0 ⎤ ⎦ ⎥

$p_1=\begin{bmatrix} a_1c_1\\ a_1s_1\\0 \end{bmatrix}$

p 2 = ⎡ ⎣ ⎢ a 1 c 1 + a 1 c 12 a 1 s 1 + a 1 s 12 0 ⎤ ⎦ ⎥

$p_2=\begin{bmatrix} a_1c_1+a_1c_{12}\\ a_1s_1+a_1s_{12}\\ 0 \end{bmatrix}$

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 ⎤ ⎦ ⎥

$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}$
根据旋转关节的雅可比计算公式，

J P i = [Z i - 1 * (P e - P i - 1) Z i - 1]

$J_{P_i}=\begin{bmatrix}Z_{i-1}*(P_e-P_{i-1})\\Z_{i-1}\end{bmatrix}$
可以得到：

J = ⎡ ⎣ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ - s 1 (a 2 c 2 + a 3 c 23) c 1 (a 2 c 2 + a 3 c 23) 0001 - c 1 (a 2 s 2 + a 3 s 23) - s 1 (a 2 s 2 + a 3 s 23) a 2 c 2 + a 3 c 23 s 1 - c 1 0 - a 3 c 1 s 23 - a 3 s 1 s 23 a 3 c 23 s 1 - c 1 0 ⎤ ⎦ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥

$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}$

二、前置法。

（1）、模型、坐标系及D-H参数表。
这里写图片描述
（2）、齐次矩阵。

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

连杆	a	theta
1	0	theta1
2	a1	theta2
3	a2	theta3
4	a3

对应的齐次矩阵是：

T o 1 (θ 1) = ⎡ ⎣ ⎢ ⎢ ⎢ c 1 s 1 00 - s 1 c 1 00 00100001 ⎤ ⎦ ⎥ ⎥ ⎥

$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}$

T i - 1 i (θ i) = ⎡ ⎣ ⎢ ⎢ ⎢ c i s i 00 - s i c i 00 0010 a i - 1 c i a i - 1 s i 01 ⎤ ⎦ ⎥ ⎥ ⎥ i = 2, 3

$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$

T 34 (θ i) = ⎡ ⎣ ⎢ ⎢ ⎢ 100001000010 a 3 001 ⎤ ⎦ ⎥ ⎥ ⎥

$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}$
（3）、运动学方程。

T o 4 (q) = ⎡ ⎣ ⎢ ⎢ ⎢ c 123 s 123 00 - s 123 c 123 00 0010 a 1 c 1 + a 1 c 12 + a 1 c 123 a 1 s 1 + a 1 s 12 + a 1 s 123 01 ⎤ ⎦ ⎥ ⎥ ⎥

$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}$
（4）、雅可比矩阵。

T o 1 (θ 1) = ⎡ ⎣ ⎢ ⎢ ⎢ c 1 s 1 00 - s 1 c 1 00 00100001 ⎤ ⎦ ⎥ ⎥ ⎥

$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}$

T o 2 (q) = ⎡ ⎣ ⎢ ⎢ ⎢ c 1 s 1 00 - s 1 c 1 00 0010 a 1 c 1 a 1 s 1 01 ⎤ ⎦ ⎥ ⎥ ⎥

$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}$

T 03 (q) = ⎡ ⎣ ⎢ ⎢ ⎢ c 12 s 12 00 - s 12 c 12 00 0010 a 1 c 1 + a 1 c 12 a 1 s 1 + a 1 s 12 01 ⎤ ⎦ ⎥ ⎥ ⎥

$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}$
根据以上方程，可以得到：