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

257
0
2020年12月20日 10时38分

目录

1. 引言

2. 轴角/旋转向量

3. 罗德里格斯公式

4. 轴角转旋转矩阵

5. 旋转矩阵转轴角

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

7. 总结


1. 引言

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

2. 轴角/旋转向量

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

 

1

 

其中单位向量1.1对应的是旋转轴,\theta对应的是旋转角度。旋转向量与轴角相同,只是旋转向量用三个元素来描述旋转,它把\theta角乘到了旋转轴上,如下:

 

2

 

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

3. 罗德里格斯公式

讲到轴角转旋转矩阵我觉得有必要介绍一下罗德里格斯公式。现在假设有一个惯性坐标系{A},一个运动坐标系{B}原点始终与{A}重合,坐标系{A}与{B}之间某一瞬间的旋转矩阵为12. 机器人正运动学—姿态描述之轴角(旋转向量)插图(4)。假设有一个质点12. 机器人正运动学—姿态描述之轴角(旋转向量)插图(5)与坐标系{B}固连在一起,质点在坐标系{B}下的坐标为12. 机器人正运动学—姿态描述之轴角(旋转向量)插图(6),在这一瞬时,这个点在坐标系{A}下的坐标为12. 机器人正运动学—姿态描述之轴角(旋转向量)插图(7),根据旋转矩阵的定义我们很容易确定12. 机器人正运动学—姿态描述之轴角(旋转向量)插图(6)12. 机器人正运动学—姿态描述之轴角(旋转向量)插图(7)之间的关系如下式:

12. 机器人正运动学—姿态描述之轴角(旋转向量)插图(8)

大学物理中我们知道12. 机器人正运动学—姿态描述之轴角(旋转向量)插图(7)其实描述的是质点12. 机器人正运动学—姿态描述之轴角(旋转向量)插图(5)在坐标系{A}下的位移。现在我希望求质点12. 机器人正运动学—姿态描述之轴角(旋转向量)插图(5)相对于坐标系{A}的速度要怎么求解呢?显然位移的时间导数是速度,因此我们对上式求导得到以下关系。

 

12. 机器人正运动学—姿态描述之轴角(旋转向量)插图(9)

 

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

 

12. 机器人正运动学—姿态描述之轴角(旋转向量)插图(10)

 

回到正题,前面已经提到质点12. 机器人正运动学—姿态描述之轴角(旋转向量)插图(5)与坐标系{B}是固连的,也就是说12. 机器人正运动学—姿态描述之轴角(旋转向量)插图(6)是个常量,常量的导数是0,因此得到以下等式:

 

12. 机器人正运动学—姿态描述之轴角(旋转向量)插图(11)

 

大学物理中我们知道在一个惯性系中的质点运动满足12. 机器人正运动学—姿态描述之轴角(旋转向量)插图(12),我们知道叉乘运算实际上可以转化为矩阵运算:

 

12. 机器人正运动学—姿态描述之轴角(旋转向量)插图(13)

 

上式中用12. 机器人正运动学—姿态描述之轴角(旋转向量)插图(14)代表由12. 机器人正运动学—姿态描述之轴角(旋转向量)插图(15)得到的反对称矩阵。套用到前面的公式中得到:

 

12. 机器人正运动学—姿态描述之轴角(旋转向量)插图(16)

 

根据式(1)和式(2)我们很容易得到以下关系:

 

12. 机器人正运动学—姿态描述之轴角(旋转向量)插图(17)

 

这个等式就很有意思了,不知道你是否还记得大学时学的关于e指数求导的知识,12. 机器人正运动学—姿态描述之轴角(旋转向量)插图(18),那么12. 机器人正运动学—姿态描述之轴角(旋转向量)插图(19),按照这个方式去看上面的式子,你会发现他们在形式上完全相同,那么这个旋转矩阵的导数对应的原函数是不是也是一种e指数呢?答案是肯定的,以上微分方程的解如下:

 

12. 机器人正运动学—姿态描述之轴角(旋转向量)插图(20)

 

为了让答案更明显,我们可以再进一步,把角速度归一化,我们定义一个与角速度12. 机器人正运动学—姿态描述之轴角(旋转向量)插图(15)方向一致的单位矢量12. 机器人正运动学—姿态描述之轴角(旋转向量)插图(21),设角速度的大小为12. 机器人正运动学—姿态描述之轴角(旋转向量)插图(22),那么你可以得到下面的关系,同理12. 机器人正运动学—姿态描述之轴角(旋转向量)插图(23),令12. 机器人正运动学—姿态描述之轴角(旋转向量)插图(24),将这些关系代入以上方程,你会得到一个更清晰的表达:

 

12. 机器人正运动学—姿态描述之轴角(旋转向量)插图(25)

 

 

以上就是旋转矩阵的e指数表达,从他的指数项我们可以看出这个旋转矩阵描述了绕12. 机器人正运动学—姿态描述之轴角(旋转向量)插图(21)轴旋转12. 机器人正运动学—姿态描述之轴角(旋转向量)插图(26)得到的旋转矩阵。接下来的问题是怎么计算呢?看起来无从下手的样子,这个时候需要再借助一点点级数展开的知识,e指数是有标准级数展开式的,如果你忘记了可以去网上查一下,我们无需关心这个展开是怎么来的,用就可以了,e指数展开式应用于以上表达式可以得到:

 

6

 

下面我们研究一下12. 机器人正运动学—姿态描述之轴角(旋转向量)插图(28)这个反对称矩阵,分别计算一下这个反对称矩阵的二次方和三次方,注意由于12. 机器人正运动学—姿态描述之轴角(旋转向量)插图(21)是单位向量,因此元素平方和为1,根据这个原则我们可以计算得到以下关系:

 

12. 机器人正运动学—姿态描述之轴角(旋转向量)插图(29)

 

有了这两个关系我们了解到不管是多少次方我们总能用12. 机器人正运动学—姿态描述之轴角(旋转向量)插图(28)12. 机器人正运动学—姿态描述之轴角(旋转向量)插图(30)来表达。用这个关系来化简前面的12. 机器人正运动学—姿态描述之轴角(旋转向量)插图(31),我们多写几项找找规律:

 

12. 机器人正运动学—姿态描述之轴角(旋转向量)插图(32)

 

所以我们看到所有这些项分成了两类,一类是含有\hat{a}的项,一类是含有12. 机器人正运动学—姿态描述之轴角(旋转向量)插图(30)的项,整理一下:

 

12. 机器人正运动学—姿态描述之轴角(旋转向量)插图(34)

 

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

 

12. 机器人正运动学—姿态描述之轴角(旋转向量)插图(37)

 

 

这个等式就是著名的罗德里格斯公式(Rodriguez formula),它描述的是绕任意轴a旋转\theta对应的旋转矩阵!

4. 轴角转旋转矩阵

前面介绍了罗德里格斯公式,这里轴角转旋转矩阵就很容易了,我们直接把轴和角度代入罗德里格斯公式就可以得到旋转矩阵。在这里,旋转轴为a=[x,y,z],旋转角度为\theta。代入罗德里格斯公式(s_\thetasin\theta的简写,c_\thetacos\theta的简写):

 

8

 

5. 旋转矩阵转轴角

 

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

 

9

 

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

 

 

12. 机器人正运动学—姿态描述之轴角(旋转向量)插图(51)

 

所以\theta的求解就简单了

 

12. 机器人正运动学—姿态描述之轴角(旋转向量)插图(52)

 

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

 

12. 机器人正运动学—姿态描述之轴角(旋转向量)插图(57)

 

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

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

 

12. 机器人正运动学—姿态描述之轴角(旋转向量)插图(63)

 

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

\theta=\pi时,旋转矩阵如下:

 

12. 机器人正运动学—姿态描述之轴角(旋转向量)插图(68)

 

这时我们找对角线元素最大值来求解对应的元素(这是为了减小舍入误差带来的影响),假设r_{11}最大,则我们先求x=\sqrt{(r_{11}+1)/2},对应的可以求出y=r_{12}/2xz=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等(感兴趣的可以查阅一下),我们这里只是为了夯实基础做了一些原理上的分析与实现,希望能够帮助大家更好地理解姿态这个概念。

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

 

 

发表评论

后才能评论