• 欢迎访问开心洋葱网站,在线教程,推荐使用最新版火狐浏览器和Chrome浏览器访问本网站,欢迎加入开心洋葱 QQ群
  • 为方便开心洋葱网用户,开心洋葱官网已经开启复制功能!
  • 欢迎访问开心洋葱网站,手机也能访问哦~欢迎加入开心洋葱多维思维学习平台 QQ群
  • 如果您觉得本站非常有看点,那么赶紧使用Ctrl+D 收藏开心洋葱吧~~~~~~~~~~~~~!
  • 由于近期流量激增,小站的ECS没能经的起亲们的访问,本站依然没有盈利,如果各位看如果觉着文字不错,还请看官给小站打个赏~~~~~~~~~~~~~!

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

人工智能 hitgavin 2364次浏览 0个评论

目录

1. 引言

2. 轴角/旋转向量

3. 罗德里格斯公式

4. 轴角转旋转矩阵

5. 旋转矩阵转轴角

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

7. 总结


1. 引言

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

2. 轴角/旋转向量

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

3. 罗德里格斯公式

讲到轴角转旋转矩阵我觉得有必要介绍一下罗德里格斯公式。现在假设有一个惯性坐标系{A},一个运动坐标系{B}原点始终与{A}重合,坐标系{A}与{B}之间某一瞬间的旋转矩阵为12. 机器人正运动学---姿态描述之轴角(旋转向量)。假设有一个质点12. 机器人正运动学---姿态描述之轴角(旋转向量)与坐标系{B}固连在一起,质点在坐标系{B}下的坐标为12. 机器人正运动学---姿态描述之轴角(旋转向量),在这一瞬时,这个点在坐标系{A}下的坐标为12. 机器人正运动学---姿态描述之轴角(旋转向量),根据旋转矩阵的定义我们很容易确定12. 机器人正运动学---姿态描述之轴角(旋转向量)12. 机器人正运动学---姿态描述之轴角(旋转向量)之间的关系如下式: 12. 机器人正运动学---姿态描述之轴角(旋转向量) 大学物理中我们知道12. 机器人正运动学---姿态描述之轴角(旋转向量)其实描述的是质点12. 机器人正运动学---姿态描述之轴角(旋转向量)在坐标系{A}下的位移。现在我希望求质点12. 机器人正运动学---姿态描述之轴角(旋转向量)相对于坐标系{A}的速度要怎么求解呢?显然位移的时间导数是速度,因此我们对上式求导得到以下关系。   12. 机器人正运动学---姿态描述之轴角(旋转向量)   所谓矩阵的导数就是对各个元素都求导。以上求导法则与函数乘法求导法则一致,即:   12. 机器人正运动学---姿态描述之轴角(旋转向量)   回到正题,前面已经提到质点12. 机器人正运动学---姿态描述之轴角(旋转向量)与坐标系{B}是固连的,也就是说12. 机器人正运动学---姿态描述之轴角(旋转向量)是个常量,常量的导数是0,因此得到以下等式:   12. 机器人正运动学---姿态描述之轴角(旋转向量)   大学物理中我们知道在一个惯性系中的质点运动满足12. 机器人正运动学---姿态描述之轴角(旋转向量),我们知道叉乘运算实际上可以转化为矩阵运算:   12. 机器人正运动学---姿态描述之轴角(旋转向量)   上式中用12. 机器人正运动学---姿态描述之轴角(旋转向量)代表由12. 机器人正运动学---姿态描述之轴角(旋转向量)得到的反对称矩阵。套用到前面的公式中得到:   12. 机器人正运动学---姿态描述之轴角(旋转向量)   根据式(1)和式(2)我们很容易得到以下关系:   12. 机器人正运动学---姿态描述之轴角(旋转向量)   这个等式就很有意思了,不知道你是否还记得大学时学的关于e指数求导的知识,12. 机器人正运动学---姿态描述之轴角(旋转向量),那么12. 机器人正运动学---姿态描述之轴角(旋转向量),按照这个方式去看上面的式子,你会发现他们在形式上完全相同,那么这个旋转矩阵的导数对应的原函数是不是也是一种e指数呢?答案是肯定的,以上微分方程的解如下:   12. 机器人正运动学---姿态描述之轴角(旋转向量)   为了让答案更明显,我们可以再进一步,把角速度归一化,我们定义一个与角速度12. 机器人正运动学---姿态描述之轴角(旋转向量)方向一致的单位矢量12. 机器人正运动学---姿态描述之轴角(旋转向量),设角速度的大小为12. 机器人正运动学---姿态描述之轴角(旋转向量),那么你可以得到下面的关系,同理12. 机器人正运动学---姿态描述之轴角(旋转向量),令12. 机器人正运动学---姿态描述之轴角(旋转向量),将这些关系代入以上方程,你会得到一个更清晰的表达:   12. 机器人正运动学---姿态描述之轴角(旋转向量)     以上就是旋转矩阵的e指数表达,从他的指数项我们可以看出这个旋转矩阵描述了绕12. 机器人正运动学---姿态描述之轴角(旋转向量)轴旋转12. 机器人正运动学---姿态描述之轴角(旋转向量)得到的旋转矩阵。接下来的问题是怎么计算呢?看起来无从下手的样子,这个时候需要再借助一点点级数展开的知识,e指数是有标准级数展开式的,如果你忘记了可以去网上查一下,我们无需关心这个展开是怎么来的,用就可以了,e指数展开式应用于以上表达式可以得到:  
12. 机器人正运动学---姿态描述之轴角(旋转向量)   下面我们研究一下12. 机器人正运动学---姿态描述之轴角(旋转向量)这个反对称矩阵,分别计算一下这个反对称矩阵的二次方和三次方,注意由于12. 机器人正运动学---姿态描述之轴角(旋转向量)是单位向量,因此元素平方和为1,根据这个原则我们可以计算得到以下关系:   12. 机器人正运动学---姿态描述之轴角(旋转向量)   有了这两个关系我们了解到不管是多少次方我们总能用12. 机器人正运动学---姿态描述之轴角(旋转向量)12. 机器人正运动学---姿态描述之轴角(旋转向量)来表达。用这个关系来化简前面的12. 机器人正运动学---姿态描述之轴角(旋转向量),我们多写几项找找规律:   12. 机器人正运动学---姿态描述之轴角(旋转向量)   所以我们看到所有这些项分成了两类,一类是含有
\hat{a}的项,一类是含有12. 机器人正运动学---姿态描述之轴角(旋转向量)的项,整理一下:   12. 机器人正运动学---姿态描述之轴角(旋转向量)   你可以再去查一下正弦函数
sin(\theta)和余弦函数
cos(\theta)的级数展开,会发现他们分别可以与括号中的表达式对应!因此我们得到最终的旋转矩阵表达式:   12. 机器人正运动学---姿态描述之轴角(旋转向量)     这个等式就是著名的罗德里格斯公式(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列的元素)。把对角线元素相加如下:     12. 机器人正运动学---姿态描述之轴角(旋转向量)   所以
\theta的求解就简单了   12. 机器人正运动学---姿态描述之轴角(旋转向量)   旋转轴对应的元素就比较容易了,比如求
x分量,观察旋转矩阵的
r_{32}
r_{23},将两者作差然后除以
2sin\theta即可,另外两个元素类似。结果如下:   12. 机器人正运动学---姿态描述之轴角(旋转向量)   接下来开始踩坑,我们知道反余弦正常是有两个解的,
\pm \theta,这里我们只取了一个解,为什么呢?这就是我们前面提到的,一旦取
-\theta,那么旋转轴的每一个元素都含有一个
\frac{1}{2sin\theta}项,因此这些元素也全都加了一个负号,这样得到的轴角正好是公式中给出的轴角的相反轴角,它们描述的是同一个旋转,所以我们取一组就可以了。 继续观察求解公式发现其中存在分母项
2sin\theta,这个值是有可能为0的,当
\theta=0,\pi时,
2sin\theta等于0(角度的取值范围是
[0,\pi]),这个轴角求解式出现表达式奇异的情况。当这种情况出现时我们需要讨论一下,将
sin\theta=0代入到旋转矩阵中如下:   12. 机器人正运动学---姿态描述之轴角(旋转向量)   好像没什么特别的,事实上,当
\theta=0,\pi时,
cos\theta可能是1或者-1,即
\frac{r_{11}+r_{22}+r_{33}-1}{2} 取1或者-1,我们利用这一点来判断实际的旋转角度到底是
0还是
\pi。 当
\theta=\pi时,旋转矩阵如下:   12. 机器人正运动学---姿态描述之轴角(旋转向量)   这时我们找对角线元素最大值来求解对应的元素(这是为了减小舍入误差带来的影响),假设
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. 机器人正运动学—姿态描述之轴角(旋转向量)
喜欢 (0)

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

加载中……