12. 机器人正运动学 |
您所在的位置:网站首页 › 向量的夹角的取值范围是多少啊怎么算 › 12. 机器人正运动学 |
目录
1. 引言 2. 轴角/旋转向量 3. 罗德里格斯公式 4. 轴角转旋转矩阵 5. 旋转矩阵转轴角 6. 轴角与旋转矩阵转换的C++实现 7. 总结 1. 引言上一篇文章主要介绍了四元数与旋转矩阵之间的转换,这篇文章介绍旋转矩阵与轴角/旋转向量之间的关系。 2. 轴角/旋转向量轴角和旋转向量本质上是一个东西,轴角用四个元素表达旋转,其中的三个元素用来描述旋转轴,另外一个元素描述旋转的角度,如下所示: 其中单位向量 如果你还记得我们上一篇文章介绍的四元数,会发现姿态的轴角表示法与四元数十分神似。是的,他们确实很像,都是描述了绕某一个轴旋转一个角度。你可能也听说过这样一个定理,任何姿态都可以通过绕某一个轴旋转特定的角度得到。也就是说只要两个坐标系原点是重合的,那么他们之间的姿态关系一定可以表达为绕某个轴旋转一个角度。 3. 罗德里格斯公式讲到轴角转旋转矩阵我觉得有必要介绍一下罗德里格斯公式。现在假设有一个惯性坐标系{A},一个运动坐标系{B}原点始终与{A}重合,坐标系{A}与{B}之间某一瞬间的旋转矩阵为 大学物理中我们知道 所谓矩阵的导数就是对各个元素都求导。以上求导法则与函数乘法求导法则一致,即: 回到正题,前面已经提到质点 大学物理中我们知道在一个惯性系中的质点运动满足 上式中用 根据式(1)和式(2)我们很容易得到以下关系: 这个等式就很有意思了,不知道你是否还记得大学时学的关于e指数求导的知识, 为了让答案更明显,我们可以再进一步,把角速度归一化,我们定义一个与角速度 以上就是旋转矩阵的e指数表达,从他的指数项我们可以看出这个旋转矩阵描述了绕 下面我们研究一下 有了这两个关系我们了解到不管是多少次方我们总能用 所以我们看到所有这些项分成了两类,一类是含有 你可以再去查一下正弦函数 这个等式就是著名的罗德里格斯公式(Rodriguez formula),它描述的是绕任意轴 前面介绍了罗德里格斯公式,这里轴角转旋转矩阵就很容易了,我们直接把轴和角度代入罗德里格斯公式就可以得到旋转矩阵。在这里,旋转轴为 旋转矩阵转轴角思路上和上一节介绍的旋转矩阵转四元数类似。我们还是先蹚个雷,上一节我们提到四元数以及它的相反四元数描述的是同一个旋转。这个命题对于轴角也成立,绕 旋转矩阵中比较特殊的是对角线元素( 所以 旋转轴对应的元素就比较容易了,比如求 接下来开始踩坑,我们知道反余弦正常是有两个解的, 继续观察求解公式发现其中存在分母项 好像没什么特别的,事实上,当 当 这时我们找对角线元素最大值来求解对应的元素(这是为了减小舍入误差带来的影响),假设 当 轴角转旋转矩阵十分简单,直接把旋转矩阵各个元素的公式代入即可,旋转矩阵转轴角由于需要分类讨论,逻辑稍微复杂一些,如下: 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是正的还是负的,如果是正的,那么 如果v是负的,那么 旋转矩阵与轴角的转换相关C++源代码我已经上传到github: https://github.com/hitgavin/rosws/tree/master/src/frames,感兴趣可以参考一下。 7. 总结这篇文章主要介绍了轴角与旋转矩阵之间的转换。姿态描述到这里就告一段落了,事实上还有一些其他的描述方式,比如旋量等,这部分高级一点,后面有机会我们再介绍。实际上姿态描述和他们之间的转换有很多开源库都已经做了,比如Eigen,ROS等(感兴趣的可以查阅一下),我们这里只是为了夯实基础做了一些原理上的分析与实现,希望能够帮助大家更好地理解姿态这个概念。 由于个人能力有限,所述内容难免存在疏漏,欢迎指出,欢迎讨论。 |
今日新闻 |
点击排行 |
|
推荐新闻 |
图片新闻 |
|
专题文章 |
CopyRight 2018-2019 实验室设备网 版权所有 win10的实时保护怎么永久关闭 |