歐拉角和旋轉矩陣之間的轉換

在使用Eigen時，經常會遇到旋轉矩陣，旋轉向量，四元數，歐拉角之間的兩兩相互轉換。這里最常見、最容易出錯的是歐拉角和旋轉矩陣之間的相互轉換。下面就歐拉角和旋轉矩陣之間的轉換進行詳細分析。

圖1 旋轉順序Z-Y-X，正方向內旋

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. 旋轉矩陣

假設繞$X、Y、Z$三個軸旋轉的角度分別為 $\alpha 、\beta 、\gamma$，則三次旋轉的旋轉矩陣計算方法如下：

按照內旋方式，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,rollcout << "roll_1 pitch_1 yaw_1 = " << eulerAngle1[2] << " " << eulerAngle1[1] << " " << eulerAngle1[0] << endl << endl;// 使用自定義函數將旋轉矩陣轉換為歐拉角Eigen::Vector3d eulerAngle2 = rotationMatrixToEulerAngles(rotation_matrix1); // roll,pitch,yawcout << "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}; }

程序輸出如下：

4. 總結

歐拉角和旋轉矩陣之間的轉換要注意的細節很多，在使用時我們一定要明確歐拉角的旋轉順序、內旋還是外旋、正向旋轉還是反向旋轉以及歐拉角范圍。另外，文章中如果有錯誤的地方，還請大佬不要噴，在評論區指出來，我核實后會修改的，謝謝。

總結

以上是生活随笔為你收集整理的【自动驾驶】欧拉角和旋转矩阵之间的转换的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。