C++求欧拉角（eigen库中暴露的一些问题）

库中暴露的一些问题）"/>

C++求欧拉角（eigen库中暴露的一些问题）

不同顺序欧拉角转旋转矩阵对照公式

eigen库求欧拉角公式

分别试验eigen库自带的matrix.eulerAngles()函数，与根据上述公式推导的两种方法求欧拉角
eigen库求得欧拉角的范围一定是 x − > r o l l x->roll x−>roll方向在 [ 0 , π ] [0,π] [0,π]之间， y − > p i t c h y->pitch y−>pitch方向在 [ − π , π ] [-π,π] [−π,π]之间， z − > y a w z->yaw z−>yaw方向在 [ − π , π ] [-π,π] [−π,π]之间，优先会保证 x − > r o l l x->roll x−>roll方向处于 [ 0 , π ] [0,π] [0,π]之间，才回去算别的角度。

以下代码验证：

#include <iostream>
#include <cmath>
#include <Eigen/Dense>Quaterniond q(0.704,-0.708,0.05,0.002)//定义一个四元数;Matrix3d m = q.toRotationMatrix();//四元数转旋转矩阵;cout << q << endl;cout << m << endl;Quaterniond q1(m);//旋转矩阵转四元数，验证下四元数是否和原来输入的四元数一致cout << q1 << endl;cout << "XYZ------------" << endl;Vector3d v=m.eulerAngles(0,1,2);//旋转矩阵转欧拉角，以XYZ为旋转方向cout << v << endl;cout << "ZYX------------" << endl;Vector3d v1=m.eulerAngles(2,1,0);//旋转矩阵转欧拉角，以ZYX为旋转方向cout << v1 << endl;Vector3d v2;

输出的结果为

-0.708i + 0.05j + 0.002k + 0.704//四元数0.994992 -0.073616  0.067568
-0.067984 -0.002536  0.997064
-0.073232 -0.996664 -0.007528//旋转矩阵
-0.70756i + 0.049969j + 0.00199876k + 0.704437//四元数
XYZ------------//先转x1.56325//x roll3.07397//y pitch
-3.06774//z yaw
ZYX------------//先转z
3.07337// z yaw
3.06825//y pitch
1.56324//x roll

可以看到以上两种旋转顺序情况下用eigen库自带的matrix.eulerAngles()函数会使得z yaw，y pitch方向上计算的欧拉角数值增大，不利于机器人做角度控制，原本机器人可能朝反方向转很小的一个角度，可能现在需要转很大一圈。

用公式推导编写的代码：

//接着上面代码段补充运行即可
double sy=sqrt(m(0,0) * m(0,0) +  m(0,1) * m(0,1));if (sy>0.00001)//判断万向锁奇异{v2(0) = std::atan2(-m(1, 2), m(2, 2));v2(1) = std::atan2(m(0, 2), sy);v2(2) = std::atan2(-m(0, 1), m(0, 0));}else//奇异{v2(0) = 0;v2(1) = std::atan2(m(0, 2), sy);v2(2) = std::atan2(-m(1, 0), m(2, 0));}
cout << "XYZ------------" << endl;
cout << v2 << endl;if (sy>0.00001)//判断万向锁奇异{v2(0) = std::atan2(m(2, 1), m(2, 2));v2(1) = std::atan2(m(2, 0), sy);v2(2) = std::atan2(-m(1, 0), m(0, 0));}else//奇异{v2(0) = std::atan2(-m(1, 2), m(1, 1));;v2(1) = std::atan2(m(2, 0), sy);v2(2) = 0;}
cout << "ZYX------------" << endl;
cout << v2 << endl;

输出结果：

XYZ-------------1.57835//x
0.0676197//y0.073852//z
ZYX-------------1.57835  //注意这里还是x，与`eigen`库自带的`matrix.eulerAngles()`出来的顺序不一样
-0.07326860.0682201//注意这里还是z

这里角度明显小很多，两种不同的方法的结果可以通过加减π得到，但是XYZ、ZYX两种不同顺序下的结果还不一样

因此建议直接用公式推导出来的方法。

参考: