1 前言

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

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

$R_{x}(\theta)=\begin{bmatrix} 1 & 0 & 0 \\ 0 & \cos\theta & -\sin\theta \\ 0 & \sin\theta & \cos\theta \\ \end{bmatrix} \in \mathbb{R}^{3\times 3}$ $R_{y}(\theta)=\begin{bmatrix} \cos\theta & 0 & \sin\theta \\ 0 & 1 & 0 \\ -\sin\theta & 0 & \cos\theta \\ \end{bmatrix} \in \mathbb{R}^{3\times 3}$ $R_{z}(\theta)=\begin{bmatrix} \cos\theta & -\sin\theta & 0 \\ \sin\theta & \cos\theta & 0\\ 0 & 0 & 1 \\ \end{bmatrix} \in \mathbb{R}^{3\times 3}$ $\mathcal{R}_x(\theta)=\begin{bmatrix} R_x(\theta) & 1 \\ 0_{1\times 3} & 1 \\ \end{bmatrix} \in \mathbb{R}^{4\times 4}$

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

$\mathcal{T}_x(d)=\begin{bmatrix} I_{3\times 3} & t_x \\ 0_{1\times 3} & 1 \\ \end{bmatrix} \in \mathbb{R}^{4\times 4}$

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

demo

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

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

SDH

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

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

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

$\begin{aligned} ^{j-1}\xi_j & = \mathcal{T}_z(d_j)\mathcal{R}_z(\theta_j)\mathcal{T}_x(a_j)\mathcal{R}_x(\alpha_j)\\ & = \begin{bmatrix} \cos\theta_j & -\cos\alpha_j\sin\theta_j & \sin\alpha_j\sin\theta_j & a_j\cos\theta_j \\ \sin\theta_j & \cos\alpha_j\cos\theta_j & -\sin\alpha_j\cos\theta_j & a_j\sin\theta_j \\ 0 & \sin\alpha_j & \cos\alpha_j & d_j \\ 0 & 0 & 0 & 1\\ \end{bmatrix} \end{aligned}\tag{1}$

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

$\begin{aligned} ^0\xi_6 &= ^0\xi_1\cdot^1\xi_2\cdot^2\xi_3\cdot^3\xi_4\cdot^4\xi_5\cdot^5\xi_6\\ &=\begin{bmatrix} ^0\mathbf{R}_6 & ^0\mathbf{p}_6\\ 0 & 1\\ \end{bmatrix} \end{aligned}\tag{2}$

对于机器人而言，更直观的是用末端位姿来表示式(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$ 表示，有：

$^B P_1 = Rx_B(r)\cdot^BP \tag{3}$

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

$^B P_2 = Ry_B( p)\cdot^BP_1 \tag{4}$

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

$^B P_3 = Rz_B( y)\cdot^BP_2 \tag{5}$

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

$\begin{aligned} ^BP_3 & = Rz_B( y)\cdot^BP_2 \\ &= Rz_B( y)\cdot Ry_B( p)\cdot^BP_1 \\ &= Rz_B( y)\cdot Ry_B( p)\cdot Rx_B(r)\cdot^BP \\ &= \begin{bmatrix} \cos y & -\sin y & 0 \\ \sin y & \cos y & 0\\ 0 & 0 & 1 \\ \end{bmatrix} \begin{bmatrix} \cos p & 0 & \sin p \\ 0 & 1 & 0\\ -\sin p & 0 & \cos p \\ \end{bmatrix} \begin{bmatrix} 1 & 0 & 0 \\ 0 &\cos r & -\sin r \\ 0 &\sin r & \cos r \\ \end{bmatrix}\cdot ^BP \\ &= \begin{bmatrix} \cos y \cos p & -\sin y\cos r+\cos y\sin p\sin r & \sin y\sin r+\cos y\sin p\cos r\\ \sin y\cos p & \cos y\cos r+\sin y\sin p\sin r & -\cos y\sin r + \sin y\sin p\cos r\\ -\sin p & \cos p\sin r & \cos p\cos r\\ \end{bmatrix}\cdot ^BP\\ & = \begin{bmatrix} r_{11} & r_{12} & r_{13}\\ r_{21} & r_{22} & r_{23}\\ r_{31} & r_{32} & r_{33}\\ \end{bmatrix}\cdot {^{B}}P \end{aligned}\tag{6}$ 注意：上述每一步的旋转都是相对于基座标系（固定坐标系）旋转的。因此，最终的旋转矩阵是左乘。

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

当 $| p| \neq 90°$ 时， $\left\{\begin{aligned} p &= atan2(-r_{31},\sqrt{r_{11}^2 + r_{21}^2})\\ y &= atan2(\frac{r_{21}}{\cos p},\frac{r_{11}}{\cos p})\\ r &= atan2(\frac{r_{32}}{\cos p},\frac{r_{33}}{\cos p})\\ \end{aligned}\right. \tag{7}$
当 $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$，因此，解为 $\left\{\begin{aligned} r &= 0°\\ y &= - atan2(r_{12},r_{13})\\ \end{aligned}\right. \tag{8}$
同理，当 $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$，因此，解为 $\left\{\begin{aligned} r &= 0°\\ y &= atan2(-r_{12},-r_{13})\\ \end{aligned}\right. \tag{9}$