1. 引言

2. 轴角/旋转向量

3. 罗德里格斯公式

4. 轴角转旋转矩阵

5. 旋转矩阵转轴角

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

7. 总结

1. 引言

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

2. 轴角/旋转向量

轴角和旋转向量本质上是一个东西，轴角用四个元素表达旋转，其中的三个元素用来描述旋转轴，另外一个元素描述旋转的角度,如下所示：
其中单位向量
对应的是旋转轴，
$\theta$ 对应的是旋转角度。旋转向量与轴角相同，只是旋转向量用三个元素来描述旋转，它把
$\theta$ 角乘到了旋转轴上，如下：
如果你还记得我们上一篇文章介绍的四元数，会发现姿态的轴角表示法与四元数十分神似。是的，他们确实很像，都是描述了绕某一个轴旋转一个角度。你可能也听说过这样一个定理，任何姿态都可以通过绕某一个轴旋转特定的角度得到。也就是说只要两个坐标系原点是重合的，那么他们之间的姿态关系一定可以表达为绕某个轴旋转一个角度。

3. 罗德里格斯公式

讲到轴角转旋转矩阵我觉得有必要介绍一下罗德里格斯公式。现在假设有一个惯性坐标系{A}，一个运动坐标系{B}原点始终与{A}重合，坐标系{A}与{B}之间某一瞬间的旋转矩阵为。假设有一个质点与坐标系{B}固连在一起，质点在坐标系{B}下的坐标为，在这一瞬时，这个点在坐标系{A}下的坐标为，根据旋转矩阵的定义我们很容易确定和之间的关系如下式：大学物理中我们知道其实描述的是质点在坐标系{A}下的位移。现在我希望求质点相对于坐标系{A}的速度要怎么求解呢？显然位移的时间导数是速度，因此我们对上式求导得到以下关系。所谓矩阵的导数就是对各个元素都求导。以上求导法则与函数乘法求导法则一致，即：回到正题，前面已经提到质点与坐标系{B}是固连的，也就是说是个常量，常量的导数是0，因此得到以下等式：大学物理中我们知道在一个惯性系中的质点运动满足，我们知道叉乘运算实际上可以转化为矩阵运算：上式中用代表由得到的反对称矩阵。套用到前面的公式中得到：根据式（1）和式（2）我们很容易得到以下关系：这个等式就很有意思了，不知道你是否还记得大学时学的关于e指数求导的知识，，那么，按照这个方式去看上面的式子，你会发现他们在形式上完全相同，那么这个旋转矩阵的导数对应的原函数是不是也是一种e指数呢？答案是肯定的，以上微分方程的解如下：为了让答案更明显，我们可以再进一步，把角速度归一化，我们定义一个与角速度方向一致的单位矢量，设角速度的大小为，那么你可以得到下面的关系，同理，令，将这些关系代入以上方程，你会得到一个更清晰的表达：以上就是旋转矩阵的e指数表达，从他的指数项我们可以看出这个旋转矩阵描述了绕轴旋转得到的旋转矩阵。接下来的问题是怎么计算呢？看起来无从下手的样子，这个时候需要再借助一点点级数展开的知识，e指数是有标准级数展开式的，如果你忘记了可以去网上查一下，我们无需关心这个展开是怎么来的，用就可以了，e指数展开式应用于以上表达式可以得到：
下面我们研究一下这个反对称矩阵，分别计算一下这个反对称矩阵的二次方和三次方，注意由于是单位向量，因此元素平方和为1，根据这个原则我们可以计算得到以下关系：有了这两个关系我们了解到不管是多少次方我们总能用和来表达。用这个关系来化简前面的，我们多写几项找找规律：所以我们看到所有这些项分成了两类，一类是含有
$\hat{a}$ 的项，一类是含有的项，整理一下：你可以再去查一下正弦函数
$sin(\theta)$ 和余弦函数
$cos(\theta)$ 的级数展开，会发现他们分别可以与括号中的表达式对应！因此我们得到最终的旋转矩阵表达式：这个等式就是著名的罗德里格斯公式（Rodriguez formula），它描述的是绕任意轴
$a$ 旋转
$\theta$ 对应的旋转矩阵！

4. 轴角转旋转矩阵

前面介绍了罗德里格斯公式，这里轴角转旋转矩阵就很容易了，我们直接把轴和角度代入罗德里格斯公式就可以得到旋转矩阵。在这里，旋转轴为
$a=[x,y,z]$ ，旋转角度为
$\theta$ 。代入罗德里格斯公式（
$s_\theta$ 是
$sin\theta$ 的简写，
$c_\theta$ 是
$cos\theta$ 的简写）：
12. 机器人正运动学---姿态描述之轴角（旋转向量）

5. 旋转矩阵转轴角

旋转矩阵转轴角思路上和上一节介绍的旋转矩阵转四元数类似。我们还是先蹚个雷，上一节我们提到四元数以及它的相反四元数描述的是同一个旋转。这个命题对于轴角也成立，绕
$\vec{r}$ 轴旋转
$\theta$ 角与绕
$-\vec{r}$ 轴旋转
$-\theta$ 角描述得也是同一个旋转。这个问题其实很好解释，我们可以从几何的角度去思考，
$\vec{r}$ 和
$-\vec{r}$ 在三维空间中是共线的，只是方向相反，正好旋转角度也相反，你可以想象他们其实是在沿着相同的方向旋转，如下图所示：
12. 机器人正运动学---姿态描述之轴角（旋转向量）旋转矩阵中比较特殊的是对角线元素（
$r_{ij}$ 代表旋转矩阵的第i行第j列的元素）。把对角线元素相加如下：所以
$\theta$ 的求解就简单了旋转轴对应的元素就比较容易了，比如求
$x$ 分量，观察旋转矩阵的
$r_{32}$ 和
$r_{23}$ ，将两者作差然后除以
$2sin\theta$ 即可，另外两个元素类似。结果如下：接下来开始踩坑，我们知道反余弦正常是有两个解的，
$\pm \theta$ ，这里我们只取了一个解，为什么呢？这就是我们前面提到的，一旦取
$-\theta$ ，那么旋转轴的每一个元素都含有一个
$\frac{1}{2sin\theta}$ 项，因此这些元素也全都加了一个负号，这样得到的轴角正好是公式中给出的轴角的相反轴角，它们描述的是同一个旋转，所以我们取一组就可以了。继续观察求解公式发现其中存在分母项
$2sin\theta$ ，这个值是有可能为0的，当
$\theta=0,\pi$ 时，
$2sin\theta$ 等于0（角度的取值范围是
$[0,\pi]$ ），这个轴角求解式出现表达式奇异的情况。当这种情况出现时我们需要讨论一下，将
$sin\theta=0$ 代入到旋转矩阵中如下：好像没什么特别的，事实上，当
$\theta=0,\pi$ 时，
$cos\theta$ 可能是1或者-1，即
$\frac{r_{11}+r_{22}+r_{33}-1}{2}$ 取1或者-1，我们利用这一点来判断实际的旋转角度到底是
$0$ 还是
$\pi$ 。当
$\theta=\pi$ 时，旋转矩阵如下：这时我们找对角线元素最大值来求解对应的元素（这是为了减小舍入误差带来的影响），假设
$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$ ，旋转轴是任意的，我们默认取
$z$ 轴。如果v是负的，那么
$\theta=\pi$ ，为了避免舍入误差带来的影响，我们需要判断一下
$x,y,z$ 哪个绝对值比较大，我们先求绝对值最大的，另外两个元素计算公式的分母中会包含这个绝对值最大的元素，这样可以有效屏蔽舍入误差的影响。旋转矩阵与轴角的转换相关C++源代码我已经上传到github: https://github.com/hitgavin/rosws/tree/master/src/frames，感兴趣可以参考一下。

7. 总结

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

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

1. 引言

2. 轴角/旋转向量

3. 罗德里格斯公式

4. 轴角转旋转矩阵

5. 旋转矩阵转轴角

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

7. 总结

您必须登录才能发表评论！

1. 引言

2. 轴角/旋转向量

3. 罗德里格斯公式

4. 轴角转旋转矩阵

5. 旋转矩阵转轴角

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

7. 总结

您必须 登录 才能发表评论！

您必须登录才能发表评论！