在使用Eigen时,经常会遇到旋转矩阵,旋转向量,四元数,欧拉角之间的两两相互转换。这里最常见、最容易出错的是欧拉角和旋转矩阵之间的相互转换。下面就欧拉角和旋转矩阵之间的转换进行详细分析。
1. 欧拉角
(1)欧拉角的叫法:
- 欧拉角的叫法不固定,跟坐标轴的定义强相关。
- 在图1中,假设X是车头,Y是车左方,Z是车上方,那么绕X轴旋转得到的是roll,绕Y轴旋转得到的是pitch,绕Z轴得到的是yaw。
- 在图1中,假设Y是车头,X是车右方,Z是车上方,那么绕X轴旋转得到的是pitch,绕Y轴旋转得到的是roll,绕Z轴得到的是yaw。
(2)欧拉角正负:
- 如果是右手系,旋转轴正方向面对观察者时,逆时针方向的旋转是正、顺时针方向的旋转是负。
- 亦可这样描述:使用右手的大拇指指向旋转轴正方向,其他4个手指在握拳过程中的指向便是正方向。
- 如图1中的三次旋转都是正向旋转。
(3)欧拉角的范围:
- 这个要具体问题具体对待。
- 假如是车体坐标系(x-前,y-左,z-上),那么roll和pitch应该定义在(-90°,+90°),yaw应该定义在(-180°,+180°)。
- 假如是飞机坐标系,那么roll、pitch和yaw都应该定义在(-180°,+180°)。
- Eigen中的默认范围roll、pitch和yaw都是(-180°,+180°)。
(4)明确旋转顺序和旋转轴:
- 对于x,y,z三个轴的不同旋转顺序一共有(x-y-z,y-z-x,z-x-y,x-z-y,z-y-x,y-x-z)六种组合,在旋转相同的角度的情况下不同的旋转顺序得到的姿态是不一样的。
- 比如,先绕x轴旋转alpha,再绕y轴旋转beta;先绕y轴旋转beta,再绕x轴旋转alpha。这两种顺序得到的姿态是不一样的。
(5)内旋和外旋:
- 每次旋转是绕固定轴(一个固定参考系,比如世界坐标系)旋转,称为外旋。
- 每次旋转是绕自身旋转之后的轴旋转,称为内旋。
- 下图说明了内旋和外旋的区别。
2. 旋转矩阵
假设绕XYZ三个轴旋转的角度分别为
,则三次旋转的旋转矩阵计算方法如下:
按照内旋方式,Z-Y-X旋转顺序(指先绕自身轴Z,再绕自身轴Y,最后绕自身轴X),可得旋转矩阵(内旋是右乘):
按照外旋方式,X-Y-Z旋转顺序(指先绕固定轴X,再绕固定轴Y,最后绕固定轴Z),可得旋转矩阵(外旋是左乘):
故R1=R2,具体不在此证明,记住即可。这个结论说明ZYX顺序的内旋等价于XYZ顺序的外旋。
slam十四讲中提到的常用旋转顺序是Z-Y-X,对应rpy,指的就是内旋(绕自身轴)Z-Y-X顺序。而欧拉角转换成旋转矩阵(相对于世界坐标系的旋转矩阵)通常是按外旋方式(绕固定轴),即X-Y-Z顺序,所以旋转矩阵为:
3. 欧拉角和旋转矩阵之间的转换程序示例
下面程序分别使用Eigen和自定义函数测试欧拉角和旋转矩阵之间的转换:
#include <iostream>
#include <Eigen/Core>
#include <Eigen/Geometry>
using namespace std;
Eigen::Matrix3d eulerAnglesToRotationMatrix(Eigen::Vector3d &theta);
bool isRotationMatirx(Eigen::Matrix3d R);
Eigen::Vector3d rotationMatrixToEulerAngles(Eigen::Matrix3d &R);
const double ARC_TO_DEG = 57.29577951308238;
const double DEG_TO_ARC = 0.0174532925199433;
int main()
{
// 设定车体欧拉角(角度),绕固定轴
double roll_deg = 0.5; // 绕X轴
double pitch_deg = 0.8; // 绕Y轴
double yaw_deg = 108.5; // 绕Z轴
// 转化为弧度
double roll_arc = roll_deg * DEG_TO_ARC; // 绕X轴
double pitch_arc = pitch_deg * DEG_TO_ARC; // 绕Y轴
double yaw_arc = yaw_deg * DEG_TO_ARC; // 绕Z轴
cout << endl;
cout << "roll_arc = " << roll_arc << endl;
cout << "pitch_arc = " << pitch_arc << endl;
cout << "yaw_arc = " << yaw_arc << endl;
// 初始化欧拉角(rpy),对应绕x轴,绕y轴,绕z轴的旋转角度
Eigen::Vector3d euler_angle(roll_arc, pitch_arc, yaw_arc);
// 使用Eigen库将欧拉角转换为旋转矩阵
Eigen::Matrix3d rotation_matrix1, rotation_matrix2;
rotation_matrix1 = Eigen::AngleAxisd(euler_angle[2], Eigen::Vector3d::UnitZ()) *
Eigen::AngleAxisd(euler_angle[1], Eigen::Vector3d::UnitY()) *
Eigen::AngleAxisd(euler_angle[0], Eigen::Vector3d::UnitX());
cout << "nrotation matrix1 =n" << rotation_matrix1 << endl << endl;
// 使用自定义函数将欧拉角转换为旋转矩阵
rotation_matrix2 = eulerAnglesToRotationMatrix(euler_angle);
cout << "rotation matrix2 =n" << rotation_matrix2 << endl << endl;
// 使用Eigen将旋转矩阵转换为欧拉角
Eigen::Vector3d eulerAngle1 = rotation_matrix1.eulerAngles(2,1,0); // ZYX顺序,yaw,pitch,roll
cout << "roll_1 pitch_1 yaw_1 = " << eulerAngle1[2] << " " << eulerAngle1[1]
<< " " << eulerAngle1[0] << endl << endl;
// 使用自定义函数将旋转矩阵转换为欧拉角
Eigen::Vector3d eulerAngle2 = rotationMatrixToEulerAngles(rotation_matrix1); // roll,pitch,yaw
cout << "roll_2 pitch_2 yaw_2 = " << eulerAngle2[0] << " " << eulerAngle2[1]
<< " " << eulerAngle2[2] << endl << endl;
return 0;
}
Eigen::Matrix3d eulerAnglesToRotationMatrix(Eigen::Vector3d &theta)
{
Eigen::Matrix3d R_x; // 计算旋转矩阵的X分量
R_x <<
1, 0, 0,
0, cos(theta[0]), -sin(theta[0]),
0, sin(theta[0]), cos(theta[0]);
Eigen::Matrix3d R_y; // 计算旋转矩阵的Y分量
R_y <<
cos(theta[1]), 0, sin(theta[1]),
0, 1, 0,
-sin(theta[1]), 0, cos(theta[1]);
Eigen::Matrix3d R_z; // 计算旋转矩阵的Z分量
R_z <<
cos(theta[2]), -sin(theta[2]), 0,
sin(theta[2]), cos(theta[2]), 0,
0, 0, 1;
Eigen::Matrix3d R = R_z * R_y * R_x;
return R;
}
bool isRotationMatirx(Eigen::Matrix3d R)
{
double err=1e-6;
Eigen::Matrix3d shouldIdenity;
shouldIdenity=R*R.transpose();
Eigen::Matrix3d I=Eigen::Matrix3d::Identity();
return (shouldIdenity - I).norm() < err;
}
Eigen::Vector3d rotationMatrixToEulerAngles(Eigen::Matrix3d &R)
{
assert(isRotationMatirx(R));
double sy = sqrt(R(0,0) * R(0,0) + R(1,0) * R(1,0));
bool singular = sy < 1e-6;
double x, y, z;
if (!singular)
{
x = atan2( R(2,1), R(2,2));
y = atan2(-R(2,0), sy);
z = atan2( R(1,0), R(0,0));
}
else
{
x = atan2(-R(1,2), R(1,1));
y = atan2(-R(2,0), sy);
z = 0;
}
return {x, y, z};
}
程序输出如下:
roll_arc = 0.00872665
pitch_arc = 0.0139626
yaw_arc = 1.89368
rotation matrix1 =
-0.317274 -0.948326 0.00384548
0.948231 -0.317177 0.0160091
-0.0139622 0.00872568 0.999864
rotation matrix2 =
-0.317274 -0.948326 0.00384548
0.948231 -0.317177 0.0160091
-0.0139622 0.00872568 0.999864
roll_1 pitch_1 yaw_1 = 0.00872665 0.0139626 1.89368
roll_2 pitch_2 yaw_2 = 0.00872665 0.0139626 1.89368
可以看出二者的输出结果是一样的。
4. 总结
欧拉角和旋转矩阵之间的转换要注意的细节很多,在使用时我们一定要明确欧拉角的旋转顺序、内旋还是外旋、正向旋转还是反向旋转以及欧拉角范围。另外,文章中如果有错误的地方,还请大佬不要喷,在评论区指出来,我核实后会修改的,谢谢。