最近用matlab和Eigen库中的一些旋转矩阵（方向余弦矩阵）、四元数和欧拉角之间的转换和绘图，弄得我有些头疼，把遇到的问题记录一下，以防以后又脑阔疼....有不同的理解可以再评论区批评指正～
- 定义
首先要明确一下欧拉角、旋转矩阵、四元数的在表示旋转时的定义，参考：
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)
所以我们在使用欧拉角转旋转矩阵和四元数的时候，要首先分清时内旋还是外旋，然后在分清其旋转轴向...
比如该图代表的就是按转动后的座标轴（蓝色的）计算欧拉角的，即为内旋
在一般的导航定位，比如无人车、无人机等的欧拉角表示，如果没有说明，则很可能是内旋的ZYX顺序。
- Matlab函数转换
一般情况下，我们做完位姿解算，会使用matlab画出误差曲线来判断结果是否准确，这个时候一般用欧拉角来绘图比较直观，然而在绘图的时候，有时候会感觉画出来的欧拉角很奇怪，比如欧拉角上下相反...
在MATLAB工具包使用了两种不同的方式来表示四元数，在空航天工具箱中使用的四元数惯例本质上是其机器人学工具箱中使用的四元数惯例的共轭。 有关这些区别的更多详细信息，请参见以下链接：
https://ww2.mathworks.cn/matlabcentral/answers/465053-rotation-order-of-quatrotate
Robotics Toolbox的是quat2eul( )
Aerospace Toolbox的是quat2angle( )
并且在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,所以转换的输出也是YPR
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());
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,所以转换的输出也是RPY
c = 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();