weixin_40725706

这个屌丝很懒，什么也没留下！

热门标签

UR3机器人运动学分析之正运动学分析_ur3e的坐标系分布图

作者：weixin_40725706 | 2024-07-29 20:11:18

踩

ur3e的坐标系分布图

前往我的博客阅读体验更佳：本文链接

UR3工业机器人是一个6自由度的连杆机器人，其结构图如下图所示：

UR3机械臂尺寸图 下面对其进行正运动学和逆运动学分析。

1 正运动学分析

1.1 建立机器人连杆坐标系

UR3机器人属于连杆机器人，对于一个连杆机器人来说，可以用四个参数：连杆长度 $a_i$ ，连杆扭角 $\alpha_i$ ，连杆距离 $d_i$ ，关节转角 $\theta_i$ 来描述。这四个参数即为连杆机器人的DH参数，在不同类型的连杆坐标系下，DH参数的含义有所不同。

连杆坐标系的建立有标准型DH法（SDH）建模和改进型DH法（MDH）建模两种方法，下面分别使用两种方法建立UR3机器人的连杆坐标系。

1.1.1 标准型DH法（SDH）建模

标准型DH法建模规则如下：

找出各关节轴，并画出这些轴线的延长线；
找 $Z$ 轴：与轴线重合， $Z_{i-1}$ 轴为关节 $i$ 的运动轴；
找 $X$ 轴：关节轴线相离时，在公垂线分析上定义 $X$ 轴，如果 $a_i$ 表示 $Z_{i-1}$ 与 $Z$ 之间的公垂线，则 $X_i$ 的方向沿 $a_i$ ；关节轴线平行时，选取前一关节的公垂线共线的一条公垂线作为 $X$ 轴；关节轴线相交时， $X$ 的方向与两周的叉积方向相同。

根据此建模规则，有SDH建模示意图如图1.1所示：

图1.1 SDH建模示意图 使用SDH法建立UR3机器人的连杆坐标系如图1.2所示：

图1.2 UR3连杆坐标系（SDH）

在该坐标系下，各DH参数的含义为：

$a_i$ ：关节轴线 $i - 1$ 和关节轴线 $i$ 的公垂线长度；
$\alpha_i$ ：关节轴线 $i - 1$ 和关节轴线 $i$ 的夹角，指向为从轴线 $i - 1$ 到轴线 $i$ ；
$d_i$ ：关节 $i$ 上的两条公垂线 $a_i$ 与 $a_{i-1}$ 之间的距离，沿关节轴 $i$ 测量；
$\theta_i$ ：连杆 $i$ 相对于连杆 $i - 1$ 绕轴线 $i$ 的旋转角度，绕关节轴线 $i - 1$ 测量。

根据UR3机器人参数得到其DH参数如表1.1所示：

表1.1 SDH坐标系下的DH参数

连杆序号 $i$	连杆长度 $a_i/mm$	连杆扭角 $\alpha_i/rad$	连杆距离 $d_i/mm$	关节转角 $\theta_i/rad$
1	$0$	$\pi/2$	$0$	$\theta_1$
2	$243.65$	$0$	$119.85$	$\theta_2$
3	$213$	$0$	$- 92.85$	$\theta_3$
4	$0$	$\pi/2$	$83.4$	$\theta_4$
5	$0$	$\pi/2$	$83.4$	$\theta_5$
6	$0$	$0$	$82.4$	$\theta_6$

1.1.2 改进型DH法（MDH）建模

改进型DH法建模规则如下：

找原点：找出关节轴 $i$ 和 $i + 1$ 之间的公垂线或关节轴 $i$ 和 $i + 1$ 的交点，以关节轴 $i$ 和 $i + 1$ 的交点或公垂线与关节轴 $i$ 的交点作为连杆坐标系 $i$ 的原点；
找 $Z$ 轴： $Z_i$ 轴沿关节轴 $i$ 的指向；
找 $X$ 轴： $X_i$ 轴沿公垂线的指向，若关节轴 $i$ 和 $i + 1$ 相交，则规定 $X_i$ 垂直于关节轴 $i$ 和 $i + 1$ 所在平面；
确定 $Y$ 轴：按照右手定则确定 $Y_i$ 轴。

根据此建模规则，有MDH建模示意图如图1.3所示：

图1.3 MDH建模示意图

使用MDH法建立UR3机器人的连杆坐标系如图1.4所示：

图1.4 UR3连杆坐标系（MDH）

其中各参数的含义如下：

$a_{i-1}$ ：沿 $X_{i-1}$ 轴，从 $Z_{i-1}$ 移动到 $Z_i$ 的距离；
$\alpha_{i-1}$ ：沿 $X_{i-1}$ 轴，从 $Z_{i-1}$ 旋转到 $Z_i$ 的角度；
$d_i$ ：沿 $Z_i$ 轴，从 $X_{i-1}$ 移动到$ X_i$的距离；
$\theta_i$ ：沿 $Z_i$ 轴，从 $X_{i-1}$ 旋转到$ X_i$的角度；

根据UR3机器人参数得到其DH参数如表1.2所示：

表1.2 MDH坐标系下的DH参数

连杆序号 $i$	连杆长度 $a_{i-1}/mm$	连杆扭角 $\alpha_{i-1}/rad$	连杆距离 $d_i/mm$	关节转角 $\theta_i/rad$
1	$0$	$0$	$151.9$	$\theta_1$
2	$0$	$\pi/2$	$119.85$	$\theta_2$
3	$- 243.65$	$0$	$0$	$\theta_3$
4	$- 213$	$0$	$- 9.45$	$\theta_4$
5	$0$	$\pi/2$	$83.4$	$\theta_5$
6	$0$	$-\pi/2$	$82.4$	$\theta_6$

1.2 UR3机器人正运动学

根据所建立的坐标系，分别在SDH坐标系以及MDH坐标系下进行正运动学的建模。

1.2.1 SDH坐标系下的正运动学建模

根据SDH坐标系下的连杆关系，有：
$KaTeX parse error: No such environment: equation at position 8: \begin{̲e̲q̲u̲a̲t̲i̲o̲n̲}̲ T_{i}^{i-1}=Ro…$
将式(1)右边的四个变换展开可得：
$KaTeX parse error: No such environment: align at position 8: \begin{̲a̲l̲i̲g̲n̲}̲ T_i^{i-1}=\lef…$
式中 $i = 1, 2, 3, . . ., 6$ 。

为方便表示，定义如下简写：

$c_i=cos\theta_i$
$s_i=sin\theta_i$
$c_{ij}=cos(\theta_i+\theta_j)$
$s_{ij}=sin(\theta_i+\theta_j)$
$c_{ijk}=cos(\theta_i+\theta_j+\theta_k)$
$s_{ijk}=sin(\theta_i+\theta_j+\theta_k)$

根据表1.1中的DH参数以及式(2)可得各连杆得变换矩阵 $T_i^{i-1}$ ，将各连杆的变换矩阵相乘可得机械臂的变矩阵 $T^0_6$ 。

{\begin{cases} T_{1}^{0} = [\begin{array}{cccc} c_{1} & 0 & s_{1} & 0 \\ s_{1} & 0 & - c_{1} & 0 \\ 0 & 1 & 0 & d_{1} \\ 0 & 0 & 0 & 1 \end{array}] \\ T_{2}^{1} = [\begin{array}{cccc} c_{2} & - s_{2} & 0 & a_{2} c_{2} \\ s_{2} & c_{2} & 0 & a_{2} s_{2} \\ 0 & 0 & 1 & d_{2} \\ 0 & 0 & 0 & 1 \end{array}] \\ T_{3}^{2} = [\begin{array}{cccc} c_{3} & - s_{3} & 0 & a_{3} c_{3} \\ s_{3} & c_{3} & 0 & a_{3} s_{3} \\ 0 & 0 & 1 & d_{3} \\ 0 & 0 & 0 & 1 \end{array}] \\ T_{4}^{3} = [\begin{array}{cccc} c_{4} & 0 & s_{4} & 0 \\ s_{4} & 0 & - c_{4} & 0 \\ 0 & 1 & 0 & d_{4} \\ 0 & 0 & 0 & 1 \end{array}] \\ T_{5}^{4} = [\begin{array}{cccc} c_{5} & 0 & s_{5} & 0 \\ s_{5} & 0 & - c_{5} & 0 \\ 0 & 1 & 0 & d_{5} \\ 0 & 0 & 0 & 1 \end{array}] \\ T_{6}^{5} = [\begin{array}{cccc} c_{6} & - s_{6} & 0 & 0 \\ s_{6} & c_{6} & 0 & 0 \\ 0 & 0 & 1 & d_{6} \\ 0 & 0 & 0 & 1 \end{array}] \end{cases}

$\begin{cases} T_1^0= \left[\begin{array}{cccc} c_1 & 0 & s_1 & 0\\ s_1 & 0 & -c_1 & 0\\ 0 & 1 & 0 & d_1\\ 0 & 0 & 0 & 1 \end{array}\right]\\ T_2^1= \left[\begin{array}{cccc} c_2 & -s_2 & 0 & a_2c_2\\ s_2 & c_2 & 0 & a_2s_2\\ 0 & 0 & 1 & d_2\\ 0 & 0 & 0 & 1 \end{array}\right]\\ T_3^2= \left[\begin{array}{cccc} c_3 & -s_3 & 0 & a_3c_3\\ s_3 & c_3 & 0 & a_3s_3\\ 0 & 0 & 1 & d_3\\ 0 & 0 & 0 & 1 \end{array}\right]\\ T_4^3= \left[\begin{array}{cccc} c_4 & 0 & s_4 & 0\\ s_4 & 0 & -c_4 & 0\\ 0 & 1 & 0 & d_4\\ 0 & 0 & 0 & 1 \end{array}\right]\\ T_5^4= \left[\begin{array}{cccc} c_5 & 0 & s_5 & 0\\ s_5 & 0 & -c_5 & 0\\ 0 & 1 & 0 & d_5\\ 0 & 0 & 0 & 1 \end{array}\right]\\ T_6^5= \left[\begin{array}{cccc} c_6 & -s_6 & 0 & 0\\ s_6 & c_6 & 0 & 0\\ 0 & 0 & 1 & d_6\\ 0 & 0 & 0 & 1 \end{array}\right]\\ \end{cases}$

⎩ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎨ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎧ T_{1}^{0} = ⎣ ⎢ ⎢ ⎡ c_{1} s_{1} 00 0010 s_{1} - c_{1} 00 00 d_{1} 1 ⎦ ⎥ ⎥ ⎤ T_{2}^{1} = ⎣ ⎢ ⎢ ⎡ c_{2} s_{2} 00 - s_{2} c_{2} 00 0010 a_{2} c_{2} a_{2} s_{2} d_{2} 1 ⎦ ⎥ ⎥ ⎤ T_{3}^{2} = ⎣ ⎢ ⎢ ⎡ c_{3} s_{3} 00 - s_{3} c_{3} 00 0010 a_{3} c_{3} a_{3} s_{3} d_{3} 1 ⎦ ⎥ ⎥ ⎤ T_{4}^{3} = ⎣ ⎢ ⎢ ⎡ c_{4} s_{4} 00 0010 s_{4} - c_{4} 00 00 d_{4} 1 ⎦ ⎥ ⎥ ⎤ T_{5}^{4} = ⎣ ⎢ ⎢ ⎡ c_{5} s_{5} 00 0010 s_{5} - c_{5} 00 00 d_{5} 1 ⎦ ⎥ ⎥ ⎤ T_{6}^{5} = ⎣ ⎢ ⎢ ⎡ c_{6} s_{6} 00 - s_{6} c_{6} 00 0010 00 d_{6} 1 ⎦ ⎥ ⎥ ⎤

$KaTeX parse error: No such environment: equation at position 8: \begin{̲e̲q̲u̲a̲t̲i̲o̲n̲}̲ T_6^0=T_1^0T_2…$

式(3)中各简式为：

{\begin{cases} n_{x} = c_{6} (s_{1} s_{5} + c_{1} c_{234} c_{5}) + c_{1} s_{234} s_{6} \\ n_{y} = s_{1} s_{234} s_{6} - c_{6} (c_{1} s_{5} - s_{1} c_{234} c_{5}) \\ n_{z} = s_{234} c_{5} c_{6} - c_{234} s_{6} \\ o_{x} = c_{1} s_{234} c_{6} - s_{6} (s_{1} s_{5} + c_{1} c_{234} c_{5}) \\ o_{y} = s_{6} (c_{1} s_{5} - s_{1} c_{234} c_{5}) + s_{1} s_{234} c_{6} \\ o_{z} = - c_{234} c_{6} - s_{234} c_{5} s_{6} \\ a_{x} = c_{1} c_{234} s_{5} - s_{1} c_{5} \\ a_{y} = s_{1} c_{234} s_{5} + c_{1} c_{5} \\ a_{z} = s_{345} s_{5} \\ p_{x} = (c_{1} c_{234} s_{5} - s_{1} c_{5}) d_{6} + c_{1} s_{234} d_{5} + s_{1} d_{4} + c_{1} c_{23} a_{3} + s_{1} d_{3} + c_{1} c_{2} a_{2} + s_{1} d_{2} \\ p_{y} = (s_{1} c_{234} s_{5} + c_{1} c_{5}) d_{6} + s_{1} s_{234} d_{5} - c_{1} d_{4} + s_{1} c_{23} a_{3} - s_{1} d_{3} + s_{1} c_{2} a_{2} - c_{1} d_{2} \\ p_{z} = s_{234} s_{5} d_{6} - c_{234} d_{5} + s_{23} a_{3} + s_{2} a_{2} \end{cases}

$\begin{cases} n_x = c_6(s_1s_5+c_1c_{234}c_5)+c_1s_{234}s_6\\ n_y = s_1s_{234}s_6-c_6(c_1s_5-s_1c_{234}c_5)\\ n_z = s_{234}c_5c_6-c_{234}s_6\\ o_x = c_1s_{234}c_6-s_6(s_1s_5+c_1c_{234}c_5)\\ o_y = s_6(c_1s_5-s_1c_{234}c_5)+s_1s_{234}c_6\\ o_z = -c_{234}c_6-s_{234}c_5s_6\\ a_x = c_1c_{234}s_5-s_1c_5\\ a_y = s_1c_{234}s_5+c_1c_5\\ a_z = s_{345}s_5\\ p_x = (c_1c_{234}s_5-s_1c_5)d_6+c_1s_{234}d_5+s_1d_4+c_1c_{23}a_3+s_1d_3+c_1c_2a_2+s_1d_2\\ p_y = (s_1c_{234}s_5+c_1c_5)d_6+s_1s_{234}d_5-c_1d_4+s_1c_{23}a_3-s_1d_3+s_1c_2a_2-c_1d_2\\ p_z = s_{234}s_5d_6-c_{234}d_5+s_{23}a_3+s_2a_2 \end{cases}$

⎩ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎨ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎧ n_{x} = c_{6} (s_{1} s_{5} + c_{1} c_{234} c_{5}) + c_{1} s_{234} s_{6} n_{y} = s_{1} s_{234} s_{6} - c_{6} (c_{1} s_{5} - s_{1} c_{234} c_{5}) n_{z} = s_{234} c_{5} c_{6} - c_{234} s_{6} o_{x} = c_{1} s_{234} c_{6} - s_{6} (s_{1} s_{5} + c_{1} c_{234} c_{5}) o_{y} = s_{6} (c_{1} s_{5} - s_{1} c_{234} c_{5}) + s_{1} s_{234} c_{6} o_{z} = - c_{234} c_{6} - s_{234} c_{5} s_{6} a_{x} = c_{1} c_{234} s_{5} - s_{1} c_{5} a_{y} = s_{1} c_{234} s_{5} + c_{1} c_{5} a_{z} = s_{345} s_{5} p_{x} = (c_{1} c_{234} s_{5} - s_{1} c_{5}) d_{6} + c_{1} s_{234} d_{5} + s_{1} d_{4} + c_{1} c_{23} a_{3} + s_{1} d_{3} + c_{1} c_{2} a_{2} + s_{1} d_{2} p_{y} = (s_{1} c_{234} s_{5} + c_{1} c_{5}) d_{6} + s_{1} s_{234} d_{5} - c_{1} d_{4} + s_{1} c_{23} a_{3} - s_{1} d_{3} + s_{1} c_{2} a_{2} - c_{1} d_{2} p_{z} = s_{234} s_{5} d_{6} - c_{234} d_{5} + s_{23} a_{3} + s_{2} a_{2}

为了验证模型的正确性，可将其中一个状态代入模型进行检验。对于图1.2中UR3机器人的形态，其对应的关节变量为

\Theta=[0\ \frac{\pi}{2}\ 0\ \frac{\pi}{2}\ \pi\ 0]^T

，将其带入式(3)中，得到如下的转换矩阵：

\begin{array}{cccc} 1 & 0 & 0 & 0 \\ 0 & 0 & - 1 & - (d_{2} + d_{3} + d_{4} + d_{6}) \\ 0 & 1 & 0 & d_{4} + a_{2} + a_{3} \\ 0 & 0 & 0 & 1 \end{array}

其中

\begin{array}{ccc} 1 & 0 & 0 \\ 0 & 0 & - 1 \\ 0 & 1 & 0 \end{array}

正好对应{0}系到{6}系的旋转矩阵，并且

\left[0\ -192.8\ 540.05 \right]^T

正好对应{0}系到{6}系平移向量，因此可以初步验证所建立的正运动学模型是正确的。

1.2.2 MDH坐标系下的正运动学建模

根据MDH坐标系下的连杆关系，有：
$KaTeX parse error: No such environment: equation at position 8: \begin{̲e̲q̲u̲a̲t̲i̲o̲n̲}̲ T_i^{i-1}=Tran…$
将式(4)右边的四个变换展开可得：
$KaTeX parse error: No such environment: align at position 8: \begin{̲a̲l̲i̲g̲n̲}̲ T_i^{i-1}=\lef…$
式中 $i = 1, 2, 3, . . ., 6$

根据表1.2中的DH参数以及式(5)可得各连杆得变换矩阵 $T_i^{i-1}$ ，将各连杆的变换矩阵相乘可得机械臂的变矩阵 $T^0_6$ 。

{\begin{cases} T_{1}^{0} = [\begin{array}{cccc} c_{1} & - s_{1} & 0 & 0 \\ s_{1} & c_{1} & 0 & 0 \\ 0 & 0 & 1 & d_{1} \\ 0 & 0 & 0 & 1 \end{array}] \\ T_{2}^{1} = [\begin{array}{cccc} c_{2} & - s_{2} & 0 & 0 \\ 0 & 0 & - 1 & - d_{2} \\ s_{2} & c_{2} & 0 & 0 \\ 0 & 0 & 0 & 1 \end{array}] \\ T_{3}^{2} = [\begin{array}{cccc} c_{3} & - s_{3} & 0 & a_{2} \\ s_{3} & c_{3} & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{array}] \\ T_{4}^{3} = [\begin{array}{cccc} c_{4} & - s_{4} & 0 & a_{3} \\ s_{4} & c_{4} & 0 & 0 \\ 0 & 0 & 1 & d_{4} \\ 0 & 0 & 0 & 1 \end{array}] \\ T_{5}^{4} = [\begin{array}{cccc} c_{5} & - s_{5} & 0 & 0 \\ 0 & 0 & - 1 & - d_{5} \\ s_{5} & c_{5} & 0 & 0 \\ 0 & 0 & 0 & 1 \end{array}] \\ T_{6}^{5} = [\begin{array}{cccc} c_{6} & - s_{6} & 0 & 0 \\ 0 & 0 & 1 & d_{6} \\ - s_{6} & - c_{6} & 0 & 0 \\ 0 & 0 & 0 & 1 \end{array}] \end{cases}

$\begin{cases} T_1^0= \left[\begin{array}{cccc} c_1 & -s_1 & 0 & 0\\ s_1 & c_1 & 0 & 0\\ 0 & 0 & 1 & d_1\\ 0 & 0 & 0 & 1 \end{array}\right]\\ T_2^1= \left[\begin{array}{cccc} c_2 & -s_2 & 0 & 0\\ 0 & 0 & -1 & -d_2\\ s_2 & c_2 & 0 & 0\\ 0 & 0 & 0 & 1 \end{array}\right]\\ T_3^2= \left[\begin{array}{cccc} c_3 & -s_3 & 0 & a_2\\ s_3 & c_3 & 0 & 0\\ 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 1 \end{array}\right]\\ T_4^3= \left[\begin{array}{cccc} c_4 & -s_4 & 0 & a_3\\ s_4 & c_4 & 0 & 0\\ 0 & 0 & 1 & d_4\\ 0 & 0 & 0 & 1 \end{array}\right]\\ T_5^4= \left[\begin{array}{cccc} c_5 & -s_5 & 0 & 0\\ 0 & 0 & -1 & -d_5\\ s_5 & c_5 & 0 & 0\\ 0 & 0 & 0 & 1 \end{array}\right]\\ T_6^5= \left[\begin{array}{cccc} c_6 & -s_6 & 0 & 0\\ 0 & 0 & 1 & d_6\\ -s_6 & -c_6 & 0 & 0\\ 0 & 0 & 0 & 1 \end{array}\right] \end{cases}$

⎩ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎨ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎧ T_{1}^{0} = ⎣ ⎢ ⎢ ⎡ c_{1} s_{1} 00 - s_{1} c_{1} 00 0010 00 d_{1} 1 ⎦ ⎥ ⎥ ⎤ T_{2}^{1} = ⎣ ⎢ ⎢ ⎡ c_{2} 0 s_{2} 0 - s_{2} 0 c_{2} 0 0 - 1 00 0 - d_{2} 01 ⎦ ⎥ ⎥ ⎤ T_{3}^{2} = ⎣ ⎢ ⎢ ⎡ c_{3} s_{3} 00 - s_{3} c_{3} 00 0010 a_{2} 001 ⎦ ⎥ ⎥ ⎤ T_{4}^{3} = ⎣ ⎢ ⎢ ⎡ c_{4} s_{4} 00 - s_{4} c_{4} 00 0010 a_{3} 0 d_{4} 1 ⎦ ⎥ ⎥ ⎤ T_{5}^{4} = ⎣ ⎢ ⎢ ⎡ c_{5} 0 s_{5} 0 - s_{5} 0 c_{5} 0 0 - 1 00 0 - d_{5} 01 ⎦ ⎥ ⎥ ⎤ T_{6}^{5} = ⎣ ⎢ ⎢ ⎡ c_{6} 0 - s_{6} 0 - s_{6} 0 - c_{6} 0 0100 0 d_{6} 01 ⎦ ⎥ ⎥ ⎤

$KaTeX parse error: No such environment: equation at position 8: \begin{̲e̲q̲u̲a̲t̲i̲o̲n̲}̲ T_6^0=T_1^0T_2…$

式(6)中各简式如下：

{\begin{cases} n_{x} = c_{6} (s_{1} s_{5} + c_{1} c_{234} c_{5}) - c_{1} s_{234} s_{6} \\ n_{y} = - c_{6} (c_{1} s_{5} - s_{1} c_{234} c_{5}) - s_{1} s_{234} s_{6} \\ n_{z} = c_{234} s_{6} + s_{234} c_{5} c_{6} \\ o_{x} = - s_{6} (s_{1} s_{5} + c_{1} c_{234} c_{5}) - c_{1} s_{234} c_{6} \\ o_{y} = s_{6} (c_{1} s_{5} - s_{1} c_{234} c_{5}) - s_{1} s_{234} c_{6} \\ o_{z} = c_{234} c_{6} - s_{234} c_{5} s_{6} \\ a_{x} = s_{1} c_{5} - c_{1} c_{234} s_{5} \\ a_{y} = - c_{1} c_{5} - s_{1} c_{234} s_{5} \\ a_{z} = - s_{234} s_{5} \\ p_{x} = (s_{1} c_{5} - c_{1} c_{234} s_{5}) d_{6} + c_{1} s_{234} d_{5} + c_{1} c_{23} a_{3} + s_{1} d_{4} + c_{1} c_{2} a_{2} + s_{1} d_{2} \\ p_{y} = - (c_{1} c_{5} + s_{1} c_{234} s_{5}) d_{6} + s_{1} s_{234} d_{5} + s_{1} c_{23} a_{3} - c_{1} d_{4} + s_{1} c_{2} a_{2} - c_{1} d_{2} \\ p_{z} = - s_{234} s_{5} d_{6} - c_{234} d_{5} + s_{23} a_{3} + s_{2} a_{2} + d_{1} \end{cases}

$\begin{cases} n_x = c_6(s_1s_5+c_1c_{234}c_5)-c_1s_{234}s_6\\ n_y = -c_6(c_1s_5-s_1c_{234}c_5)-s_1s_{234}s_6\\ n_z = c_{234}s_6+s_{234}c_5c_6\\ o_x = -s_6(s_1s_5+c_1c_{234}c_5)-c_1s_{234}c_6\\ o_y = s_6(c_1s_5-s_1c_{234}c_5)-s_1s_{234}c_6\\ o_z = c_{234}c_6-s_{234}c_5s_6\\ a_x = s_1c_5-c_1c_{234}s_5\\ a_y = -c_1c_5-s_1c_{234}s_5\\ a_z = -s_{234}s_5\\ p_x = (s_1c_5-c_1c_{234}s_5)d_6+c_1s_{234}d_5+c_1c_{23}a_3+s_1d_4+c_1c_2a_2+s_1d_2\\ p_y = -(c_1c_5+s_1c_{234}s_5)d_6+s_1s_{234}d_5+s_1c_{23}a_3-c_1d_4+s_1c_2a_2-c_1d_2\\ p_z = -s_{234}s_5d_6-c_{234}d_5+s_{23}a_3+s_2a_2+d_1 \end{cases}$

⎩ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎨ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎧ n_{x} = c_{6} (s_{1} s_{5} + c_{1} c_{234} c_{5}) - c_{1} s_{234} s_{6} n_{y} = - c_{6} (c_{1} s_{5} - s_{1} c_{234} c_{5}) - s_{1} s_{234} s_{6} n_{z} = c_{234} s_{6} + s_{234} c_{5} c_{6} o_{x} = - s_{6} (s_{1} s_{5} + c_{1} c_{234} c_{5}) - c_{1} s_{234} c_{6} o_{y} = s_{6} (c_{1} s_{5} - s_{1} c_{234} c_{5}) - s_{1} s_{234} c_{6} o_{z} = c_{234} c_{6} - s_{234} c_{5} s_{6} a_{x} = s_{1} c_{5} - c_{1} c_{234} s_{5} a_{y} = - c_{1} c_{5} - s_{1} c_{234} s_{5} a_{z} = - s_{234} s_{5} p_{x} = (s_{1} c_{5} - c_{1} c_{234} s_{5}) d_{6} + c_{1} s_{234} d_{5} + c_{1} c_{23} a_{3} + s_{1} d_{4} + c_{1} c_{2} a_{2} + s_{1} d_{2} p_{y} = - (c_{1} c_{5} + s_{1} c_{234} s_{5}) d_{6} + s_{1} s_{234} d_{5} + s_{1} c_{23} a_{3} - c_{1} d_{4} + s_{1} c_{2} a_{2} - c_{1} d_{2} p_{z} = - s_{234} s_{5} d_{6} - c_{234} d_{5} + s_{23} a_{3} + s_{2} a_{2} + d_{1}

为了验证模型的正确性，可将其中一个状态代入模型进行检验。对于图1.4中UR3机器人的形态，其对应的关节变量为

\Theta=[0\ -\frac{\pi}{2}\ 0\ -\frac{\pi}{2}\ 0\ 0]^T

，将其带入式(6)中，得到如下的转换矩阵：

\begin{array}{cccc} - 1 & 0 & 0 & 0 \\ 0 & 0 & - 1 & - (d_{6} + d_{4} + d_{2}) \\ 0 & - 1 & 0 & d_{1} + d_{5} - a_{2} - a_{3} \\ 0 & 0 & 0 & 1 \end{array}

其中

\begin{array}{ccc} - 1 & 0 & 0 \\ 0 & 0 & - 1 \\ 0 & - 1 & 0 \end{array}

正好对应{0}系到{6}系的旋转矩阵，并且

\left[0\ -192.8\ 691.95 \right]^T

正好对应{0}系到{6}系平移向量，因此可以初步验证所建立的正运动学模型是正确的。后文中将通过仿真来进一步验证模型的正确性。

由于在不同的坐标系下DH参数的含义有所不同，推导出来的正运动学模型也会有所差别。在后文的分析中，若无特别说明，将使用MDH坐标系下推导出来的正运动学模型。

声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/weixin_40725706/article/detail/900379