【机器人学|运动学与动力学】#2 更多姿态描述

在上一篇中，我们使用旋转矩阵的9个分量来描述物体的姿态，但是这9个分量并不是相互独立的。对于一个旋转矩阵$R$，我们可以写出6个约束方程。如果我们用参考坐标系中某坐标系的三个单位轴来表示旋转矩阵$R$

$$R=(\hat{X}\hat{Y}\hat{Z})$$

这三个矢量均为单位矢量，且两两正交，故有如下6个约束

$$\begin{array}{rl}& |\hat{X}|=|\hat{Y}|=|\hat{Z}|=1\\ & \hat{X}\cdot \hat{Y}=\hat{X}\cdot \hat{Z}=\hat{Y}\cdot \hat{Z}=0\end{array}$$

这意味着我们最少只需给出3个参数就能确定一个物体的姿态。

X-Y-Z固定角

围绕方向固定不动的参考坐标系主轴旋转的拆解方式称为固定角（Fixed Angles）。从X-Y-Z固定角的视角来看，任意姿态的{B}\{B\}{B}都可以由以下过程表示：将{B}\{B\}{B}与{A}\{A\}{A}重合，先绕${\hat{X}}_A$旋转$\gamma$角，再绕${\hat{Y}}_A$旋转$\beta$角，最后绕${\hat{Z}}_A$旋转$\alpha$角得到最终的{B}\{B\}{B}。X-Y-Z表示绕主轴旋转的顺序。

因而可以推导{B}\{B\}{B}到{A}\{A\}{A}的等价旋转矩阵（式中$c\theta =\cos \theta$，$s\theta =\sin \theta$）

$$\begin{array}{rl}{}_B^A{R}_{XYZ}(\gamma ,\beta ,\alpha )& =R_Z(\alpha )R_Y(\beta )R_X(\gamma )\\ & =\left[\begin{array}{ccc}c\alpha & -s\alpha & 0\\ s\alpha & c\alpha & 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{ccc}c\beta & 0& s\beta \\ 0& 1& 0\\ -s\beta & 0& c\beta \end{array}\right]\left[\begin{array}{ccc}1& 0& 0\\ 0& c\gamma & -s\gamma \\ 0& s\gamma & c\gamma \end{array}\right]\\ & =\left[\begin{array}{ccc}c\alpha c\beta & c\alpha s\beta s\gamma -s\alpha c\gamma & c\alpha s\beta c\gamma +s\alpha s\gamma \\ s\alpha c\beta & s\alpha s\beta s\gamma +c\alpha c\gamma & s\alpha s\beta c\gamma -c\alpha s\gamma \\ -s\beta & c\beta s\gamma & c\beta c\gamma \end{array}\right]\end{array}$$

矩阵书写顺序与实际旋转操作顺序相反，从旋转一个矢量的角度来看，是因为旋转矩阵乘在矢量的左边，所以右侧的矩阵会先与矢量相乘。

相应地，我们也可以通过任意旋转矩阵求得X-Y-Z固定角。将旋转矩阵的九个元素与上式联立有

$$\begin{array}{rl}{}_B^A{R}_{XYZ}(\gamma ,\beta ,\alpha )& =\left[\begin{array}{ccc}c\alpha c\beta & c\alpha s\beta s\gamma -s\alpha c\gamma & c\alpha s\beta c\gamma +s\alpha s\gamma \\ s\alpha c\beta & s\alpha s\beta s\gamma +c\alpha c\gamma & s\alpha s\beta c\gamma -c\alpha s\gamma \\ -s\beta & c\beta s\gamma & c\beta c\gamma \end{array}\right]\\ & =\left[\begin{array}{ccc}{r}_{11}& r_{12}& r_{13}\\ r_{21}& r_{22}& r_{23}\\ r_{31}& r_{32}& r_{33}\end{array}\right]\end{array}$$

可解得

$$\begin{array}{r}\beta =\mathrm{Atan}2\left(-r_{31},\sqrt{r_{11}^2+r_{21}^2}\right)\\ \alpha =\mathrm{Atan}2\left(\frac{r_{21}}{c\beta },\frac{r_{11}}{c\beta }\right)\\ \gamma =\mathrm{Atan}2\left(\frac{r_{32}}{c\beta },\frac{r_{33}}{c\beta }\right)\end{array}$$

式中$Atan2(y,x)={\tan }^{-1}(y/x)$为反正切函数的双参变量表示法，其可根据$x$和$y$的符号判别求得的角所在的象限。

当$\beta =\pm 90^{\circ }$时，$c\beta =0$，此时仅能求出$\alpha$和$\gamma$的和或差。一般取$\alpha =0^{\circ }$，则

$$\gamma =\{\begin{array}{rl}Atan2(r_{12},r_{22}),& \beta =90^{\circ }\\ -Atan2(r_{12},r_{22}),& \beta =-90^{\circ }\end{array}$$

Z-Y-X欧拉角

围绕运动坐标系自身主轴旋转的拆解方式称为欧拉角（Euler Angles）。从Z-Y-X欧拉角的视角来看，任意姿态的{B}\{B\}{B}都可以由以下过程表示：将{B}\{B\}{B}与{A}\{A\}{A}重合，先绕${\hat{Z}}_B$旋转$\alpha$角，再绕${\hat{Y}}_B$旋转$\beta$角，最后绕${\hat{X}}_B$旋转$\alpha$角得到最终的{B}\{B\}{B}。

通过中间坐标系{B′}\{B'\}{B′}和{B′′}\{B''\}{B′′}，我们可以写出由{A}\{A\}{A}到{B}\{B\}{B}（{B′′′}\{B'''\}{B′′′}）的旋转矩阵为

$${}_B^{A}R={}_{B^{\prime }}^{A}R{}_{B^{\prime \prime }}^{B^{\prime }}R{}_B^{B^{\prime \prime }}R$$
则由Z-Y-X欧拉角参数化的旋转矩阵为（下标中的撇号表明这是用欧拉角而非固定角描述的旋转）

$$\begin{array}{rl}{}_B^A{R}_{Z^{\prime }{Y}^{\prime }{X}^{\prime }}(\alpha ,\beta ,\gamma )& ={}_{B^{\prime }}^{A}R{}_{B^{\prime \prime }}^{B^{\prime }}R{}_B^{B^{\prime \prime }}R\\ & =R_Z(\alpha )R_Y(\beta )R_X(\gamma )\\ & =\left[\begin{array}{ccc}c\alpha & -s\alpha & 0\\ s\alpha & c\alpha & 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{ccc}c\beta & 0& s\beta \\ 0& 1& 0\\ -s\beta & 0& c\beta \end{array}\right]\left[\begin{array}{ccc}1& 0& 0\\ 0& c\gamma & -s\gamma \\ 0& s\gamma & c\gamma \end{array}\right]\\ & =\left[\begin{array}{ccc}c\alpha c\beta & c\alpha s\beta s\gamma -s\alpha c\gamma & c\alpha s\beta c\gamma +s\alpha s\gamma \\ s\alpha c\beta & s\alpha s\beta s\gamma +c\alpha c\gamma & s\alpha s\beta c\gamma -c\alpha s\gamma \\ -s\beta & c\beta s\gamma & c\beta c\gamma \end{array}\right]\end{array}$$

该结果与X-Y-Z固定角中的一致。由此可见，三次绕固定轴旋转的最终姿态和以相反顺序绕运动坐标轴旋转的最终姿态相同。

固定角和欧拉角均需要按一定顺序进行绕主轴的三个旋转，将XYZ三个主轴进行排列组合（只需要满足相邻两次旋转围绕的主轴不同即可，例如Z-Y-Z），我们可以得到24种旋转矩阵表示法，其中12种为固定角，12种为欧拉角，它们均被称为转角排列设定法。

等效角度-轴线表示法

在上一篇的变换算子一节中，我们给出过旋转算子$R_K(\theta )$表示绕$\hat{K}$轴旋转$\theta$角度。如果轴的方向是一般方向而非主轴方向，任何姿态都可通过选择适当的轴和角度来得到，这种表示法即为等效角度-轴线表示法。

令${}^A\hat{K}=(k_x{k}_y{k}_z{)}^T$，则等效旋转矩阵为

$$R_K(\theta )=\left[\begin{array}{ccc}{k}_x^{2}v\theta +c\theta & k_x{k}_{y}v\theta -k_{z}c\theta & k_x{k}_{z}v\theta +k_{y}s\theta \\ k_x{k}_{y}v\theta +k_{z}s\theta & k_x{k}_{y}v\theta +c\theta & k_y{k}_{z}v\theta -k_{x}s\theta \\ k_x{k}_{z}v\theta -k_{y}s\theta & k_y{k}_{z}v\theta +k_{x}s\theta & k_z^{2}c\theta \end{array}\right]$$

式中$c\theta =\cos \theta$，$s\theta =\sin \theta$，$v\theta =1-\cos \theta$。

其推导思路为，假设有一辅助坐标系{U}\{U\}{U}，其$\hat{Z}$轴与旋转轴$\hat{K}$一致。

1. 将{B}\{B\}{B}与{A}\{A\}{A}重合；
2. 通过旋转矩阵${}_A^{U}R$将{B}\{B\}{B}相对于{A}\{A\}{A}的表示转换为{B}\{B\}{B}相对于{U}\{U\}{U}的表示；
3. 通过旋转算子$R_Z(\theta )$将{B}\{B\}{B}绕{U}\{U\}{U}的$\hat{Z}$轴旋转$\theta$；
4. 再通过旋转矩阵${}_U^{A}R$将{B}\{B\}{B}相对于{U}\{U\}{U}的表示恢复为{B}\{B\}{B}相对于{A}\{A\}{A}的表示。

由此等效旋转矩阵可表示为

$$R_K(\theta )={}_U^{A}R{R}_Z(\theta ){}_A^{U}R$$

已知${}^A{\hat{Z}}_U{=}^A\hat{K}=(k_x{k}_y{k}_z{)}^T$，则

$${}_U^{A}R=\left[\begin{array}{ccc}A& D& k_x\\ B& E& k_y\\ C& F& k_z\end{array}\right]$$

其中六个未知数根据旋转矩阵的性质满足

$$\begin{array}{c}{A}^2+B^2+C^2=1\\ D^2+E^2+F^2=1\\ (ABC)\cdot (DEF{)}^T=0\\ (ABC)\times (DEF)=(k_x{k}_y{k}_z{)}^T\end{array}$$

利用上述性质，再由${}_A^{U}R={}_U^A{R}^T$，可化简等效旋转矩阵为最终结果。

旋转矩阵、固定角、欧拉角和等效角度-轴线表示法均为姿态表示的经典方法，它们在后续更复杂的实际应用中将逐渐面临一些根本性的挑战。现代机器人领域则发展出了四元数表示法以及更高层次的旋量理论，在姿态表示中实现了更加优雅而统一的数学框架，博主将在运动学与动力学的基础篇章结束后对其进行介绍。