运动学与雅可比矩阵举例

最新推荐文章于 2023-02-23 23:19:23 发布

飘零过客

最新推荐文章于 2023-02-23 23:19:23 发布

阅读量1.4w

点赞数 9

分类专栏：机器人学

本文链接：https://blog.csdn.net/xuehuafeiwu123/article/details/52221896

版权

机器人学专栏收录该内容

22 篇文章 95 订阅

订阅专栏

以三连杆平面机械臂为例，来说明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}$
根据以上方程，可以得到：

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

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

p 0 = p 1 = ⎡ ⎣ ⎢ 000 ⎤ ⎦ ⎥

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

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

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

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

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

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

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

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

$J_{P_i}=\begin{bmatrix}Z_{i}*(P_e-P_{i})\\Z_{i}\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 ⎤ ⎦ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥

三、比较说明。

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

四、参考文献

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

飘零过客

关注

9
点赞
踩
56

收藏

觉得还不错? 一键收藏
19
评论
运动学与雅可比矩阵举例

以三连杆平面机械臂为例，来说明D-H矩阵和雅可比矩阵的求法，并且比较后置法和前置法的异同和运动学末端位姿的一致性和雅可比矩阵的一致性。一、后置法。（1）、模型、坐标系及D-H参数表。（2）、齐次矩阵。如图建立个连杆的坐标系，D-H参数如下表所示：连杆 a alpha d theta 1 a1 0 0 theta...
复制链接

扫一扫