1. 引言

上一篇文章主要介绍了四元数与旋转矩阵之间的转换，这篇文章介绍旋转矩阵与轴角/旋转向量之间的关系。

2. 轴角/旋转向量

轴角和旋转向量本质上是一个东西，轴角用四个元素表达旋转，其中的三个元素用来描述旋转轴，另外一个元素描述旋转的角度,如下所示：

$r=[x,y,z,\theta]$

其中单位向量 $\vec{n}=[x,y,z]$ 对应的是旋转轴， $\theta$ 对应的是旋转角度。旋转向量与轴角相同，只是旋转向量用三个元素来描述旋转，它把 $\theta$ 角乘到了旋转轴上，如下：

$r_v=[x*\theta,y*\theta,z*\theta]$

如果你还记得我们上一篇文章介绍的四元数，会发现姿态的轴角表示法与四元数十分神似。是的，他们确实很像，都是描述了绕某一个轴旋转一个角度。你可能也听说过这样一个定理，任何姿态都可以通过绕某一个轴旋转特定的角度得到。也就是说只要两个座标系原点是重合的，那么他们之间的姿态关系一定可以表达为绕某个轴旋转一个角度。

3. 罗德里格斯公式

讲到轴角转旋转矩阵我觉得有必要介绍一下罗德里格斯公式。现在假设有一个惯性座标系{A}，一个运动座标系{B}原点始终与{A}重合，座标系{A}与{B}之间某一瞬间的旋转矩阵为。假设有一个质点与座标系{B}固连在一起，质点在座标系{B}下的座标为，在这一瞬时，这个点在座标系{A}下的座标为，根据旋转矩阵的定义我们很容易确定和之间的关系如下式：

$p_{A}(t) =R(t) p_{B}$

大学物理中我们知道 $p_{A}(t)$ 其实描述的是质点在座标系{A}下的位移。现在我希望求质点相对于座标系{A}的速度要怎么求解呢？显然位移的时间导数是速度，因此我们对上式求导得到以下关系。

$\dot{p}_{A}(t)=\dot{R}(t)p_{B}+R(t)\dot{p}_{B}$

所谓矩阵的导数就是对各个元素都求导。以上求导法则与函数乘法求导法则一致，即：

$\varphi (t)=f(t)g(t)$

$\dot{\varphi} (t)=\dot{f}(t)g(t)+f(t)\dot{g}(t)$

回到正题，前面已经提到质点与座标系{B}是固连的，也就是说 $p_{B}$ 是个常量，常量的导数是0，因此得到以下等式：

$\dot{p}_{A}(t)=\dot{R}(t)p_{B}$ （1）

大学物理中我们知道在一个惯性系中的质点运动满足 $v=\dot{p}=\omega \times p$ ，我们知道叉乘运算实际上可以转化为矩阵运算：

$\omega \times p=\begin{bmatrix} 0& -\omega_{z} & \omega_{y} \\ \omega_{z}& 0& -\omega_{x} \\ -\omega_{y}& \omega_{x}& 0 \end{bmatrix}p=\hat{\omega}p$

上式中用 $\hat{\omega}$ 代表由 $\omega$ 得到的反对称矩阵。套用到前面的公式中得到：

$\dot{p}_{A}(t)=\omega\times p_{A}(t)=\hat{\omega}p_{A}(t)=\hat{\omega}R(t)p_{B}$ （2）

根据式（1）和式（2）我们很容易得到以下关系：

$\dot{R}=\hat{\omega}R$

这个等式就很有意思了，不知道你是否还记得大学时学的关于e指数求导的知识， $f(x)=e^{ax}$ ，那么 $\dot{f}(x)=a\cdot e^{ax}$ ，按照这个方式去看上面的式子，你会发现他们在形式上完全相同，那么这个旋转矩阵的导数对应的原函数是不是也是一种e指数呢？答案是肯定的，以上微分方程的解如下：

$R=e^{\hat{\omega}t}$

为了让答案更明显，我们可以再进一步，把角速度归一化，我们定义一个与角速度 $\omega$ 方向一致的单位矢量，设角速度的大小为 $\dot{q}$ ，那么你可以得到下面的关系 $\omega=a\dot{q}$ ，同理 $\hat{\omega}=\hat{a}\dot{q}$ ，令 $\dot{q}t=\theta$ ，将这些关系代入以上方程，你会得到一个更清晰的表达：

$R=e^{\hat{a}\theta}$

以上就是旋转矩阵的e指数表达，从他的指数项我们可以看出这个旋转矩阵描述了绕轴旋转 $\theta$ 得到的旋转矩阵。接下来的问题是怎么计算呢？看起来无从下手的样子，这个时候需要再借助一点点级数展开的知识，e指数是有标准级数展开式的，如果你忘记了可以去网上查一下，我们无需关心这个展开是怎么来的，用就可以了，e指数展开式应用于以上表达式可以得到：

$R(\theta)=E+\hat{a}\theta+\frac{(\hat{a}\theta)^{2}}{2!}+\frac{(\hat{a}\theta)^{3}}{3!}+...$

下面我们研究一下 $\hat{a}$ 这个反对称矩阵，分别计算一下这个反对称矩阵的二次方和三次方，注意由于是单位向量，因此元素平方和为1，根据这个原则我们可以计算得到以下关系：

$\hat{a}^{2}=\begin{bmatrix} x^2-1 & xy & xz\\ xy& y^2-1& yz\\ xz& yz& z^2-1 \end{bmatrix}$

$\hat{a}^{3}=\begin{bmatrix} 0 & z & -y\\ -z&0& x\\y& -x& 0 \end{bmatrix}=-\hat{a}$

有了这两个关系我们了解到不管是多少次方我们总能用 $\hat{a}$ 和 $\hat{a}^{2}$ 来表达。用这个关系来化简前面的 $R(\theta)$ ，我们多写几项找找规律：

$R(\theta)=E+\hat{a}\theta+\frac{\hat{a}^2\theta^{2}}{2!}-\frac{\hat{a}\theta^{3}}{3!}-\frac{\hat{a}^2\theta^{4}}{4!}+\frac{\hat{a}\theta^{5}}{5!}+...$

所以我们看到所有这些项分成了两类，一类是含有 $\hat{a}$ 的项，一类是含有 $\hat{a}^{2}$ 的项，整理一下：

$R(\theta)=E+\hat{a}(\theta-\frac{\theta^{3}}{3!}+\frac{\theta^{5}}{5!}-...)+\hat{a}^{2}(\frac{\theta^{2}}{2!}-\frac{\theta^{4}}{4!}+...)$

你可以再去查一下正弦函数 $sin(\theta)$ 和余弦函数 $cos(\theta)$ 的级数展开，会发现他们分别可以与括号中的表达式对应！因此我们得到最终的旋转矩阵表达式：

$R(\theta)=e^{\hat{a}\theta}=E+\hat{a}sin\theta+\hat{a}^2(1-cos\theta)$

这个等式就是著名的罗德里格斯公式（Rodriguez formula），它描述的是绕任意轴旋转 $\theta$ 对应的旋转矩阵！

4. 轴角转旋转矩阵

前面介绍了罗德里格斯公式，这里轴角转旋转矩阵就很容易了，我们直接把轴和角度代入罗德里格斯公式就可以得到旋转矩阵。在这里，旋转轴为，旋转角度为 $\theta$ 。代入罗德里格斯公式（ $s_\theta$ 是 $sin\theta$ 的简写， $c_\theta$ 是 $cos\theta$ 的简写）：

$R(\theta)=e^{\hat{a}\theta}=E+\hat{a}sin\theta+\hat{a}^2(1-cos\theta)=\begin{bmatrix} x^2(1-c_\theta)+c_\theta & xy(1-c_\theta)-zs_\theta & xz(1-c_\theta)+ys_\theta\\ xy(1-c_\theta)+zs_\theta & y^2(1-c_\theta)+c_\theta & yz(1-c_\theta)-xs_\theta\\ xz(1-c_\theta)-ys_\theta & yz(1-c_\theta)+xs_\theta & z^2(1-c_\theta)+c_\theta \end{bmatrix}$

5. 旋转矩阵转轴角

旋转矩阵转轴角思路上和上一节介绍的旋转矩阵转四元数类似。我们还是先蹚个雷，上一节我们提到四元数以及它的相反四元数描述的是同一个旋转。这个命题对于轴角也成立，绕 $\vec{r}$ 轴旋转 $\theta$ 角与绕 $-\vec{r}$ 轴旋转 $-\theta$ 角描述得也是同一个旋转。这个问题其实很好解释，我们可以从几何的角度去思考， $\vec{r}$ 和 $-\vec{r}$ 在三维空间中是共线的，只是方向相反，正好旋转角度也相反，你可以想象他们其实是在沿着相同的方向旋转，如下图所示：

旋转矩阵中比较特殊的是对角线元素（ $r_{ij}$ 代表旋转矩阵的第i行第j列的元素）。把对角线元素相加如下：

$r_{11}+r_{22}+r_{33}=x^2+y^2+z^2-(x^2+y^2+z^2)c_\theta+3*c_\theta=1+2c_\theta$

所以 $\theta$ 的求解就简单了：

$\theta=arccos(\frac{r_{11}+r_{22}+r_{33}-1}{2})$

旋转轴对应的元素就比较容易了，比如求分量，观察旋转矩阵的 $r_{32}$ 和 $r_{23}$ ，将两者作差然后除以 $2sin\theta$ 即可，另外两个元素类似。结果如下：

$r=\frac{1}{2sin\theta}\begin{bmatrix} r_{32}-r_{23}\\ r_{13}-r_{31}\\ r_{21}-r_{12} \end{bmatrix}$

接下来开始踩坑，我们知道反余弦正常是有两个解的， $\pm \theta$ ，这里我们只取了一个解，为什么呢？这就是我们前面提到的，一旦取 $-\theta$ ，那么旋转轴的每一个元素都含有一个 $\frac{1}{2sin\theta}$ 项，因此这些元素也全都加了一个负号，这样得到的轴角正好是公式中给出的轴角的相反轴角，它们描述的是同一个旋转，所以我们取一组就可以了。

继续观察求解公式发现其中存在分母项 $2sin\theta$ ，这个值是有可能为0的，当 $\theta=0,\pi$ 时， $2sin\theta$ 等于0（角度的取值范围是 $[0,\pi]$ ），这个轴角求解式出现表达式奇异的情况。当这种情况出现时我们需要讨论一下，将 $sin\theta=0$ 代入到旋转矩阵中如下：

$R(\theta)=\begin{bmatrix} x^2(1-c_\theta)+c_\theta & xy(1-c_\theta) & xz(1-c_\theta)\\ xy(1-c_\theta)& y^2(1-c_\theta)+c_\theta & yz(1-c_\theta)\\ xz(1-c_\theta) & yz(1-c_\theta) & z^2(1-c_\theta)+c_\theta \end{bmatrix}$

好像没什么特别的，事实上，当 $\theta=0,\pi$ 时， $cos\theta$ 可能是1或者-1，即 $\frac{r_{11}+r_{22}+r_{33}-1}{2}$ 取1或者-1，我们利用这一点来判断实际的旋转角度到底是还是 $\pi$ 。

当 $\theta=\pi$ 时，旋转矩阵如下：

$R(\theta)=\begin{bmatrix} 2x^2-1 & 2xy & 2xz\\ 2xy & 2y^2-1 & 2yz\\ 2xz & 2yz & 2z^2-1 \end{bmatrix}$

这时我们找对角线元素最大值来求解对应的元素（这是为了减小舍入误差带来的影响），假设 $r_{11}$ 最大，则我们先求 $x=\sqrt{(r_{11}+1)/2}$ ，对应的可以求出 $y=r_{12}/2x$ ， $z=r_{13}/2x$ 。这里有一点需要注意，当 $\theta=\pi$ 时，旋转轴是 $\vec{r}$ 和 $-\vec{r}$ 将产生相同的结果，因此开根号我们取正值对应的一组解作为旋转轴。

当 $\theta=0$ 时，情况比较特殊，这时旋转矩阵是单位阵，旋转轴可以任意，我们给出一个默认的旋转轴即可。

6. 轴角与旋转矩阵转换的C++实现

轴角转旋转矩阵十分简单，直接把旋转矩阵各个元素的公式代入即可，旋转矩阵转轴角由于需要分类讨论，逻辑稍微复杂一些，如下：

void Rotation::getAxialAngle(double &x, double &y, double &z,
                             double &theta) const {
  double epsilon = 1E-12;
  double v = (data[0] + data[4] + data[8] - 1.0f) / 2.0f;
  if (fabs(v) < 1 - epsilon) {
    theta = acos(v);
    x = 1 / (2 * sin(theta)) * (data[7] - data[5]);
    y = 1 / (2 * sin(theta)) * (data[2] - data[6]);
    z = 1 / (2 * sin(theta)) * (data[3] - data[1]);
  } else {
    if (v > 0.0f) {
      // \theta = 0, diagonal elements approaching 1
      theta = 0;
      x = 0;
      y = 0;
      z = 1;
    } else {
      // \theta = \pi
      // find maximum element in the diagonal elements
      theta = PI;
      if (data[0] >= data[4] && data[0] >= data[8]) {
        // calculate x first
        x = sqrt((data[0] + 1) / 2);
        y = data[1] / (2 * x);
        z = data[2] / (2 * x);
      } else if (data[4] >= data[0] && data[4] >= data[8]) {
        // calculate y first
        y = sqrt((data[4] + 1) / 2);
        x = data[3] / (2 * y);
        z = data[5] / (2 * y);
      } else {
        // calculate z first
        z = sqrt((data[8] + 1) / 2);
        x = data[6] / (2 * z);
        y = data[7] / (2 * z);
      }
    }
  }
}

代码中的分类讨论就是我们介绍旋转矩阵转轴角时的特殊情况，当v的绝对值没有趋向于1说明不存在表达式奇异的问题，因此直接计算。

当v的绝对值趋向于1我们就要判断v是正的还是负的，如果是正的，那么 $\theta=0$ ，旋转轴是任意的，我们默认取轴。

如果v是负的，那么 $\theta=\pi$ ，为了避免舍入误差带来的影响，我们需要判断一下哪个绝对值比较大，我们先求绝对值最大的，另外两个元素计算公式的分母中会包含这个绝对值最大的元素，这样可以有效屏蔽舍入误差的影响。

旋转矩阵与轴角的转换相关C++源代码我已经上传到github: https://github.com/hitgavin/rosws/tree/master/src/frames，感兴趣可以参考一下。

7. 总结

这篇文章主要介绍了轴角与旋转矩阵之间的转换。姿态描述到这里就告一段落了，事实上还有一些其他的描述方式，比如旋量等，这部分高级一点，后面有机会我们再介绍。实际上姿态描述和他们之间的转换有很多开源库都已经做了，比如Eigen，ROS等（感兴趣的可以查阅一下），我们这里只是为了夯实基础做了一些原理上的分析与实现，希望能够帮助大家更好地理解姿态这个概念。

由于个人能力有限，所述内容难免存在疏漏，欢迎指出，欢迎讨论。

12. 机器人正运动学---姿态描述之轴角（旋转向量）

1. 引言

2. 轴角/旋转向量

3. 罗德里格斯公式

4. 轴角转旋转矩阵

5. 旋转矩阵转轴角

6. 轴角与旋转矩阵转换的C++实现

7. 总结

12. 機器人正運動學---姿態描述之軸角（旋轉向量）

1. SimMechanics/Multibody入門

3. 機器人正運動學---座標系及其變換

9. 機器人正運動學---修改DH參數

1. 機器人簡介

Mac下配置sublime實現LaTeX

https://yachay.unat.edu.pe/blog/index.php?comment_area=format_blog&comment_component=blog&comment_co

linux以太網驅動總結