SDH,一变就通

介绍标准DH(SDH)确定坐标系间的齐次变换矩阵

1 前言

上篇已经介绍过 SDH 建立坐标系的过程,本篇来介绍下 SDH 参数表示坐标系间的齐次变换矩阵。

用 $R_x$ 表示绕 $x$ 轴的三维旋转矩阵,用 $\mathcal{R}_x$ 表示绕 $x$ 轴旋转的齐次变换矩阵。

用 $t_x = \begin{bmatrix}
d & 0 &0
\end{bmatrix}^T$表示沿 $x$ 方向平移 $d$,用$\mathcal{T}_x(d)$ 表示平移 $t$ 的齐次变换矩阵。即:

接下来用一个示例说明旋转矩阵的两种含义:

demo

上图中的两种含义会决定旋转矩阵的左乘还是右乘。当表示第一种含义的时候,用的右乘;第二种含义,旋转矩阵则应左乘。

2 用 SDH 参数表示坐标系见的齐次变换矩阵

SDH

上图清晰地展示了从 $j-1$ 系如何变换到 $j$ 系,其过程如下:

  1. 沿 $z_{j-1}$ 轴移动 $d_j$
  2. 沿 $z_{j-1}$ 轴转动 $\theta_j$
  3. 沿 $x_{j}$ 轴移动 $a_j$
  4. 沿 $x_{j}$ 轴转动 $\alpha_j$

因此,从 $j-1$ 系到 $j$ 系的变换矩阵 $^{j-1}\xi_j$ 就可以表示为

于是,对于 UR10 这样的 6 轴机器人,从基座开始到末端的变换矩阵为:

对于机器人而言,更直观的是用末端位姿来表示式(2)所示的齐次变换矩阵。机器人末端在空间中的位置即用式(2)中的 $^0\mathbf{p}_6 = \begin{bmatrix}
x&y&z
\end{bmatrix}^T$ 表示,姿态矩阵 $^0\mathbf{R}_6$ 则用三个变量来表示。
这里介绍用X-Y-Z固定角坐标系来描述的方法[1]

首先将坐标系 {$B$} 和一个已知参考坐标系 {$A$} 重合。先将 {$B$} 绕 {$A$} 系的 $X$ 轴旋转 $r$ 角度(roll),再绕 $A$ 系的 $Y$ 轴旋转 $p$ 角度(pitch),最后绕 $A$ 的 $Z$ 轴旋转 $y$ 角度(yaw)。每个旋转轴都是绕着固定参考系{$A$}的轴,规定这种姿态的表示法为X-Y-Z固定角坐标系。有的地方也把它们定义为回转角、俯仰角和偏航角。因此,上述旋转顺序是 $X-Y-Z$。假定基座标系中的一点为 $^BP$,$B$ 表示基座标系。
先绕 $X_B$ 轴转 $\theta_y = r$,旋转后的点在基座标系中的表示用 $^B P_1$ 表示,有:

再绕 $Y_B$ 轴转 $\theta_p = p$,旋转后的点在基座标系中的用 $^B P_2$ 表示,有:

最后绕 $Z_B$ 轴转 $\theta_r = y$,旋转后的点在基座标系中的用 $^B P_3$ 表示,有:

将上式三式整合,得基座标系中最终点的坐标和初始点坐标之间的关系为:

注意:上述每一步的旋转都是相对于基座标系(固定坐标系)旋转的。因此,最终的旋转矩阵是左乘。

依据式(6)进行求解 $y, p, r$:

  1. 当 $| p| \neq 90°$ 时,
  2. 当 $p = 90°$ 时,$r_{12} = -\sin y\cos r+\cos y\sin r = \sin(r-y)$, $r_{13} = \sin y\sin r + \cos y\cos r = \cos(r-y)$, 无法直接解算出,直接设定此时 $r = 0$,此时 $r_{12} = -\sin y$,$r_{13} = \cos y$,因此,解为
  3. 同理,当 $p = -90°$ 时, $r_{12} = -\sin y\cos r - \cos y\sin r = -\sin(r+y)$, $r_{13} = \sin y\sin r - \cos y\cos r = -\cos(y+r)$,指定 $r = 0$,此时 $r_{12} = -\sin y$,$r_{13} = -\cos y$,因此,解为

atan2 是 MATLAB 自带的函数。

3 验证

3.1 rpy 验证

装完PeterCoke的 rcv_tools 工具箱后,运行 tripleangle, 如下所示:

20210520

图中,Rz 表示绕世界坐标系的 Z 轴旋转,Ry 表示绕世界坐标系的 Y 轴旋转,Rx表示绕世界坐标系的 X 轴旋转。在PeterCoke的工具箱中,使用 rpy2r 可以将 rpy 转为旋转矩阵,程序实例如下:

xyz

通过运行 open rpy2r 打开函数的源码如下所示:

xyz

使用时该函数的输入参数是 roll,pitch,yaw,工具箱的默认旋转顺序是 $ZYX$,从第 96 行可以看出采用的旋转顺序是 $R_z(yaw)R_y(pitch)R_x(roll)$,与式 (6) 一致。因此,工具箱的旋转顺序命名是按照旋转矩阵从左至右命名的,与实际的旋转顺序正好相反。

3.2 机器人末端位姿验证

零位时的 UR10 和关节 3 转 $90°$ 后的位姿计算结果如下所示:

以第二组结果为例,反算出来的 $roll = 0, pitch = -\pi/2, yaw = \pi /2$,即末端姿态绕基座标系 $x$ 轴旋转了 $0°$,再绕基座标系的 $y$ 轴旋转了 $-90°$,最后绕基座标系的 $z$ 轴旋转了 $90°$,与图示末端姿态一致。

简单计算,即可得到由旋转矩阵求 rpy 的公式 (7)~(9) 与用工具箱得到的结果一致。

参考文献

  1. 机器人建模和控制[M]
  2. 【机器人运动学/姿态角】欧拉角和RPY角
  3. 机器人运动控制算法——位姿
------ 本文结束感谢您的阅读------
Donate a cup of cola?