当前位置：网站首页>UR3机器人运动学分析之正运动学分析

UR3机器人运动学分析之正运动学分析

2022-07-31 05:17:00 【G_Summer_Song】

前往我的博客阅读体验更佳：本文链接
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坐标系下的连杆关系，有：
$\begin{equation} T_{i}^{i-1}=Rot(z,\theta_i)Trans(z,d_i)Trans(x,a_i)Rot(x,\alpha_i) \end{equation}$
将式(1)右边的四个变换展开可得：
$\begin{align} T_i^{i-1}=\left[ \begin{array}{cccc} c\theta_i & -s\theta_ic\alpha_i & s\theta_is\alpha_i & a_ic\theta_i\\ s\theta_i & c\theta_ic\alpha_i & -c\theta_is\alpha_i & a_is\theta_i\\ 0 & s\alpha_i & c\alpha_i & d_i\\ 0 & 0 & 0 & 1 \end{array} \right] \end{align}$
式中 $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= \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}$

$\begin{equation} T_6^0=T_1^0T_2^1T_3^2T_4^3T_5^4T_6^5= \left[ \begin{array}{cccc} n_x & o_x & a_x & p_x\\ n_y & o_y & a_y & p_y\\ n_z & o_z & a_z & p_z\\ 0 & 0 & 0 & 1\\ \end{array} \right] \end{equation}$

式(3)中各简式为：
$\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}$
为了验证模型的正确性，可将其中一个状态代入模型进行检验。对于图1.2中UR3机器人的形态，其对应的关节变量为 $\Theta=[0\ \frac{\pi}{2}\ 0\ \frac{\pi}{2}\ \pi\ 0]^T$ ，将其带入式(3)中，得到如下的转换矩阵：
$T_6^0=\left[ \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} \right]=\left[ \begin{array}{cccc} 1 & 0 & 0 & 0\\ 0 & 0 & -1 & -192.8\\ 0 & 1 & 0 & 540.05\\ 0 & 0 & 0 & 1 \end{array} \right]$
其中
$\left[ \begin{array}{ccc} 1 & 0 & 0 \\ 0 & 0 & -1 \\ 0 & 1 & 0 \\ \end{array} \right]$
正好对应{0}系到{6}系的旋转矩阵，并且 $\left[0\ -192.8\ 540.05 \right]^T$ 正好对应{0}系到{6}系平移向量，因此可以初步验证所建立的正运动学模型是正确的。

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

根据MDH坐标系下的连杆关系，有：
$\begin{equation} T_i^{i-1}=Trans(x,a_{i-1})Rot(x,\alpha_{i-1})Trans(z,d_i)Rot(z,\theta_i) \end{equation}$
将式(4)右边的四个变换展开可得：
$\begin{align} T_i^{i-1}=\left[ \begin{array}{cccc} c\theta_i & -s\theta_i & 0 & a_{i-1}\\ s\theta_ic\alpha_{i-1} & c\theta_ic\alpha_{i-1} & -s\alpha_{i-1} & -d_is\alpha_{i-1}\\ s\theta_is\alpha_{i-1} & c\theta_is\alpha_{i-1} & c\alpha_{i-1} & d_ic\alpha_{i-1}\\ 0 & 0 & 0 & 1 \end{array} \right] \end{align}$
式中 $i = 1, 2, 3, ..., 6$

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

式(6)中各简式如下：
$\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}$
为了验证模型的正确性，可将其中一个状态代入模型进行检验。对于图1.4中UR3机器人的形态，其对应的关节变量为 $\Theta=[0\ -\frac{\pi}{2}\ 0\ -\frac{\pi}{2}\ 0\ 0]^T$ ，将其带入式(6)中，得到如下的转换矩阵：
$T_6^0=\left[ \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} \right]=\left[ \begin{array}{cccc} -1 & 0 & 0 & 0\\ 0 & 0 & -1 & -192.8\\ 0 & -1 & 0 & 691.95\\ 0 & 0 & 0 & 1 \end{array} \right]$
其中
$\left[ \begin{array}{ccc} -1 & 0 & 0 \\ 0 & 0 & -1 \\ 0 & -1 & 0 \\ \end{array} \right]$
正好对应{0}系到{6}系的旋转矩阵，并且 $\left[0\ -192.8\ 691.95 \right]^T$ 正好对应{0}系到{6}系平移向量，因此可以初步验证所建立的正运动学模型是正确的。后文中将通过仿真来进一步验证模型的正确性。