matlab和Eigen库中的一些旋转矩阵（方向余弦矩阵）、四元数和欧拉角之间的转换和绘图的注意事项

最近用matlab和Eigen库中的一些旋转矩阵（方向余弦矩阵）、四元数和欧拉角之间的转换和绘图，弄得我有些头疼，把遇到的问题记录一下，以防以后又脑阔疼....有不同的理解可以再评论区批评指正～

主要问题有两个

1、matlab工具箱自带的的姿态转换函数定义有歧义（quat2eul（）和quat2angle( )定义不一样），会导致转换出来的欧拉角结果出错；

2、eigen库中使用欧拉角转换eulerAngles（）时，得到的结果每轴相差180度(看起来不太对，其实真正结果时对的)

定义

首先要明确一下欧拉角、旋转矩阵、四元数的在表示旋转时的定义，参考：

https://zh.wikipedia.org/wiki/%E6%AC%A7%E6%8B%89%E8%A7%92

https://zh.wikipedia.org/wiki/%E6%97%8B%E8%BD%AC%E7%9F%A9%E9%98%B5

https://zh.wikipedia.org/wiki/%E5%9B%9B%E5%85%83%E6%95%B8

很可惜地，對於夾角的順序和標記，夾角的兩個軸的指定，並沒有任何常規。科學家對此從未達成共識。每當用到歐拉角時，我們必須明確的表示出夾角的順序，指定其參考軸。

这就是欧拉角很烦人的原因...很多情况下，我感觉在说欧拉角的时候，并没有提到其的旋转轴的顺序，故在画图的时候一般使用欧拉角来表示比较直观，就非常非常的头疼（也可能是我太菜了orz）...

所以，对于姿态表示欧拉角是不唯一的，而旋转矩阵和四元数是唯一的！

欧拉角的表示方式有24种，即2×（3*2*2）种，2代表内旋和外旋，即是固定坐标轴转还是按转动后的坐标轴转的；3代表第一次旋转的3个轴向，接着2代表除了上次的那个轴外的另外两个轴向，还有一个2也是代表除了上次的那个轴外的另外两个轴向。

内旋(intrinsic rotations) = 旋转轴(rotated axis)

外旋(extrinsic rotations) = 固定轴(static/fixed axis)

https://zhuanlan.zhihu.com/p/85108850

所以我们在使用欧拉角转旋转矩阵和四元数的时候，要首先分清时内旋还是外旋，然后在分清其旋转轴向...

比如该图代表的就是按转动后的坐标轴（蓝色的）计算欧拉角的，即为内旋

在一般的导航定位，比如无人车、无人机等的欧拉角表示，如果没有说明，则很可能是内旋的ZYX顺序。

Matlab函数转换

一般情况下，我们做完位姿解算，会使用matlab画出误差曲线来判断结果是否准确，这个时候一般用欧拉角来绘图比较直观，然而在绘图的时候，有时候会感觉画出来的欧拉角很奇怪，比如欧拉角上下相反...

在MATLAB工具包使用了两种不同的方式来表示四元数，在空航天工具箱中使用的四元数惯例本质上是其机器人学工具箱中使用的四元数惯例的共轭。有关这些区别的更多详细信息，请参见以下链接：

https://ww2.mathworks.cn/matlabcentral/answers/352465-what-is-the-aerospace-blockset-quaternion-convention?s_tid=answers_rc1-2_p2_MLT

https://ww2.mathworks.cn/matlabcentral/answers/465053-rotation-order-of-quatrotate

Robotics Toolbox的是quat2eul( )

Aerospace Toolbox的是quat2angle( )

https://ww2.mathworks.cn/matlabcentral/answers/523677-quat2eul-quat-and-dcm2angle-r-difference-for-zyx-sequence

并且在matlab中，四元数的定义是a+bi+cj+dk，即w、x、y、z.

https://ww2.mathworks.cn/help/robotics/ref/quaternion.html

一般我们定义的四元数是按右手法则的，即拇指朝向旋转轴（xyz的方向），四指指向旋转方向（w代表旋转大小），这和Robotics Toolbox的是quat2eul( ) 的函数是吻合的，而Aerospace Toolbox的是quat2angle( )则是左手法则，故差一个共轭！

这些在使用相应函数时都要注意。

另外，这个工具箱有时候不好安装，要激活matlab的权限，对有的破解版来说比较麻烦，所以我觉得还是直接自己写一个转换代码比较方便T_T

这里代码的定义是一般无人机使用的欧拉角的定义，即内旋的ZYX

function [rpy] = q2euler(q)
% [qx qy qz qw] --> [roll, pitch, yaw] qx = q(1);qy = q(2);qz = q(3);qw = q(4);roll = atan2(2*(qw*qx+qy*qz), 1-2*(qx*qx+qy*qy));pitch = asin(2*(qw*qy-qz*qx));yaw = atan2(2*(qw*qz+qx*qy), 1-2*(qy*qy+qz*qz));rpy = [roll, pitch, yaw];
end

function [rpy] = c2euler(c)
%UNTITLED2 此处显示有关此函数的摘要
%   此处显示详细说明rpy(1) = atan2(c(3,2),c(3,3));rpy(2) = atan2(-c(3,1),sqrt(c(3,2)^2+c(3,3)^2));rpy(3) = atan2(c(2,1),c(1,1));
end

具体请参考

https://www.cnblogs.com/tiandsp/p/10733607.html

Eigen库转换

在Eigen库中有互相转换的函数，见如下代码示例

旋转矩阵转欧拉角时，若该旋转矩阵是之前由欧拉角转换来的，想得到之前的欧拉角的值，则旋转轴的顺序必须一致，如从ZYX顺序来的，则使用eulerAngles（）是得用ZYX顺序，即（2,1,0），得到的为ypr的值。

注意，Eigen库中eulerAngles（）是默认内旋的方式

* \returns the Euler-angles of the rotation matrix \c *this using the convention defined by the triplet (\a a0,\a a1,\a a2)** Each of the three parameters \a a0,\a a1,\a a2 represents the respective rotation axis as an integer in {0,1,2}.* For instance, in:* \code Vector3f ea = mat.eulerAngles(2, 0, 2); \endcode* "2" represents the z axis and "0" the x axis, etc. The returned angles are such that* we have the following equality:* \code* mat == AngleAxisf(ea[0], Vector3f::UnitZ())*      * AngleAxisf(ea[1], Vector3f::UnitX())*      * AngleAxisf(ea[2], Vector3f::UnitZ()); \endcode* This corresponds to the right-multiply conventions (with right hand side frames).* * The returned angles are in the ranges [0:pi]x[-pi:pi]x[-pi:pi].int main(int argc, char **argv)
{
Eigen::Vector3d rpy_raw, ypr;rpy_raw << 30, 60, 150;rpy_raw = rpy_raw * M_PI / 180;Eigen::Matrix3d c;//输入部分为YPR,所以转换的输出也是YPRc = Eigen::AngleAxisd(rpy_raw[2], Eigen::Vector3d::UnitZ()) * Eigen::AngleAxisd(rpy_raw[1], Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(rpy_raw[0], Eigen::Vector3d::UnitX());ypr = c.eulerAngles(2, 1, 0);cout << ypr.transpose() * 180 / M_PI << endl;return 0;
}

这个是按ZYX转的

下面这个是按照XYZ方式旋转的

//主函数
int main(int argc, char **argv)
{Eigen::Vector3d rpy_raw, rpy_1;rpy_raw << 30, 60, 150;rpy_raw = rpy_raw * M_PI / 180;Eigen::Matrix3d c;//输入部分为RPY,所以转换的输出也是RPYc = Eigen::AngleAxisd(rpy_raw[0], Eigen::Vector3d::UnitX()) * Eigen::AngleAxisd(rpy_raw[1], Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(rpy_raw[2], Eigen::Vector3d::UnitZ());// c = Eigen::AngleAxisd(rpy_raw[2], Eigen::Vector3d::UnitZ()) * Eigen::AngleAxisd(rpy_raw[1], Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(rpy_raw[0], Eigen::Vector3d::UnitX());rpy_1 = c.eulerAngles(0, 1, 2);cout << rpy_1.transpose() * 180 / M_PI << endl;return 0;
}

若时四元数的话，只需要先把四元数转成旋转矩阵，再转成欧拉角即可

    Eigen::Quaterniond q;Eigen::Matrix3d c;c = q.toRotationMatrix();

回到最初的问题，为什么使用eulerAngles（）有时候得到的欧拉角会相差180度

代码如下

int main(int argc, char **argv)
{Eigen::Vector3d rpy_raw;rpy_raw << -1, 2, 3;rpy_raw = rpy_raw * M_PI / 180;Eigen::Matrix3d c;c = Eigen::AngleAxisd(rpy_raw[2], Eigen::Vector3d::UnitZ()) * Eigen::AngleAxisd(rpy_raw[1], Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(rpy_raw[0], Eigen::Vector3d::UnitX());Eigen::Vector3d rpy_tran = c.transpose().eulerAngles(2, 1, 0) / M_PI * 180;std::cout << rpy_tran[2] << " " << rpy_tran[1] << " " << rpy_tran[0] << std::endl;return 0;

输出结果

-178.896 -178.055 176.964

正确的结果应该时-1,2,3的，我们转换得到的结果貌似每轴和正确结果都相差180度

回到eulerAngles（）定义中去，有一句话

The returned angles are in the ranges [0:pi]x[-pi:pi]x[-pi:pi].

即row的角度范围时【0-180°】，当我们row小于0时，Eigen库会把YZ两轴旋转180度，从而达到让X轴旋转角度大于0°的目的，各位可以试一试按ZYX的方式旋转，将ZY分别旋转180度，原本X为负的旋转角就变成了正的！

这就是Eigen库输出的欧拉角看起来不太对的原因了！其实我们用到欧拉角，主要还是在画图中，比较直观，计算姿态的话直接用旋转矩阵和四元数，就没有这么多麻烦事了～

matlab和Eigen库中的一些旋转矩阵（方向余弦矩阵）、四元数和欧拉角之间的转换和绘图的注意事项相关推荐

Matlab ——旋转矩阵，四元数，欧拉角之间的转换
最近要用这方面的东西,整理,记录,分享一下基于Matlab现有函数下的内容 Matlab --旋转矩阵,四元数,欧拉角之间的转换旋转矩阵 dcm R 四元数 quat q = [q0 q1 q2 ...
C++ Eigen 库中旋转向量、旋转矩阵、欧拉角、四元数的定义及互相转换
今天看师兄写的使用力反馈设备操作机械臂的代码,里边涉及到了Eigen 库中的旋转变换,表征旋转变换的有旋转向量Eigen::AngleAxisd.欧拉角Eigen::Vector3d.旋转矩阵Eige ...
Eigen库中的Identity()函数作用
今天学习Eigen库,看到示例代码中有这样一行: Matrix3d rotation_matrix = Matrix3d::Identity(); Matrix3d:Eigen库中typedef的数据 ...
旋转矩阵与欧拉角之间的转换
简介: 对于欧拉角与旋转矩阵之间的转换公式和程序实现进行了测试.也显示了这其中的转换关系的复杂性,来自于欧拉角的方向.范围.转换顺序.这在实际应用中需要特别的关注. 关键词: 欧拉角,旋转矩阵 #m ...
转换矩阵、平移矩阵、旋转矩阵关系以及python实现旋转矩阵、四元数、欧拉角之间转换
文章目录 1. 转换矩阵.平移矩阵.旋转矩阵之间的关系 2. 缩放变换.平移变换和旋转变换 2. python实现旋转矩阵.四元数.欧拉角互相转化由于在平时总是或多或少的遇到平移旋转的问题,每次都是 ...
罗德里格斯公式推导（轴角与旋转矩阵的关系）以及四元数与旋转向量、旋转矩阵、欧拉角之间的转换关系
罗德里格斯公式推导(轴角与旋转矩阵的关系) 意义:罗德里格斯公式表示旋转向量到旋转矩阵之间爱你的转换关系旋转向量:一个向量,方向与旋转轴一致,长度等于旋转角度空间中任意旋转都可以用一个旋转轴和一个 ...
matlab分离实部虚部,c – 如何在Eigen3库中有效地提取复杂矩阵的实部/虚部？
我在Eigen3库中有一些复杂,密集的矢量/矩阵,我想将实部和虚部提取到单独的数组中.在Matlab中,我可以做类似的事情 cplxFoo = [1,1i; -1i -1] re = real(cpl ...
旋转矩阵、旋转向量（轴角）、四元数、欧拉角之间相互转换的代码实现(利用Eigen实现)...
1 #include <iostream> 2 #include <cmath> 3 using namespace std; 4 5 #include <Eigen/C ...
刚体运动中的坐标变换-旋转矩阵、旋转向量、欧拉角及四元数
坐标变换及其方法 1.转化关系图 2 换算关系 3.1 旋转矩阵换算至其他 3.2 四元数换算至其他 3.3 旋转向量转换至旋转矩阵与四元数 3.3 欧拉角转换到旋转矩阵和四元数 3 坐标变换 4 坐 ...

matlab和Eigen库中的一些旋转矩阵（方向余弦矩阵）、四元数和欧拉角之间的转换和绘图的注意事项

matlab和Eigen库中的一些旋转矩阵（方向余弦矩阵）、四元数和欧拉角之间的转换和绘图的注意事项相关推荐

最新文章

热门文章