歐拉角和旋轉(zhuǎn)矩陣之間的轉(zhuǎn)換

在使用Eigen時(shí)，經(jīng)常會(huì)遇到旋轉(zhuǎn)矩陣，旋轉(zhuǎn)向量，四元數(shù)，歐拉角之間的兩兩相互轉(zhuǎn)換。這里最常見(jiàn)、最容易出錯(cuò)的是歐拉角和旋轉(zhuǎn)矩陣之間的相互轉(zhuǎn)換。下面就歐拉角和旋轉(zhuǎn)矩陣之間的轉(zhuǎn)換進(jìn)行詳細(xì)分析。

圖1 旋轉(zhuǎn)順序Z-Y-X，正方向內(nèi)旋

1. 歐拉角

（1）歐拉角的叫法：

• 歐拉角的叫法不固定，跟坐標(biāo)軸的定義強(qiáng)相關(guān)。
• 在圖1中，假設(shè)$X$是車頭，$Y$是車左方，$Z$是車上方，那么繞X軸旋轉(zhuǎn)得到的是 $roll$，繞 $Y$ 軸旋轉(zhuǎn)得到的是$pitch$，繞$Z$軸得到的是$yaw$。
• 在圖1中，假設(shè)$Y$是車頭，$X$是車右方，$Z$是車上方，那么繞X軸旋轉(zhuǎn)得到的是 $pitch$，繞 $Y$ 軸旋轉(zhuǎn)得到的是 $roll$，繞$Z$軸得到的是$yaw$。

（2）歐拉角正負(fù)：

• 如果是右手系，旋轉(zhuǎn)軸正方向面對(duì)觀察者時(shí)，逆時(shí)針?lè)较虻男D(zhuǎn)是正、順時(shí)針?lè)较虻男D(zhuǎn)是負(fù)。
• 亦可這樣描述：使用右手的大拇指指向旋轉(zhuǎn)軸正方向，其他4個(gè)手指在握拳過(guò)程中的指向便是正方向。
• 如圖1中的三次旋轉(zhuǎn)都是正向旋轉(zhuǎn)。

（3）歐拉角的范圍：

• 這個(gè)要具體問(wèn)題具體對(duì)待。
• 假如是車體坐標(biāo)系（$x$-前，$y$-左，$z$-上），那么 $roll$ 和 $pitch$ 應(yīng)該定義在（-90°，+90°），$yaw$ 應(yīng)該定義在（-180°，+180°）。
• 假如是飛機(jī)坐標(biāo)系，那么 $roll$、$pitch$ 和 $yaw$ 都應(yīng)該定義在（-180°，+180°）。
• Eigen中的默認(rèn)范圍 $roll$、$pitch$ 和 $yaw$都是（-180°，+180°）。

（4）明確旋轉(zhuǎn)順序和旋轉(zhuǎn)軸：

• 對(duì)于x,y,z三個(gè)軸的不同旋轉(zhuǎn)順序一共有($x?y?z,y?z?x,z?x?y,x?z?y,z?y?x,y?x?z$)六種組合，在旋轉(zhuǎn)相同的角度的情況下不同的旋轉(zhuǎn)順序得到的姿態(tài)是不一樣的。
• 比如，先繞$x$軸旋轉(zhuǎn)$\alpha$，再繞$y$軸旋轉(zhuǎn)$\beta$；先繞$y$軸旋轉(zhuǎn)$\beta$，再繞$x$軸旋轉(zhuǎn)$\alpha$。這兩種順序得到的姿態(tài)是不一樣的。

（5）內(nèi)旋和外旋：

• 每次旋轉(zhuǎn)是繞固定軸（一個(gè)固定參考系，比如世界坐標(biāo)系）旋轉(zhuǎn)，稱為外旋。
• 每次旋轉(zhuǎn)是繞自身旋轉(zhuǎn)之后的軸旋轉(zhuǎn)，稱為內(nèi)旋。
• 下圖說(shuō)明了內(nèi)旋和外旋的區(qū)別。

2. 旋轉(zhuǎn)矩陣

假設(shè)繞三個(gè)軸旋轉(zhuǎn)的角度分別為 ，則三次旋轉(zhuǎn)的旋轉(zhuǎn)矩陣計(jì)算方法如下：

按照內(nèi)旋方式，Z-Y-X旋轉(zhuǎn)順序（指先繞自身軸Z，再繞自身軸Y，最后繞自身軸X），可得旋轉(zhuǎn)矩陣（內(nèi)旋是右乘）：

按照外旋方式，X-Y-Z旋轉(zhuǎn)順序（指先繞固定軸X，再繞固定軸Y，最后繞固定軸Z），可得旋轉(zhuǎn)矩陣（外旋是左乘）：

故R1=R2，具體不在此證明，記住即可。這個(gè)結(jié)論說(shuō)明ZYX順序的內(nèi)旋等價(jià)于XYZ順序的外旋。

slam十四講中提到的常用旋轉(zhuǎn)順序是Z-Y-X，對(duì)應(yīng)rpy，指的就是內(nèi)旋（繞自身軸）Z-Y-X順序。而歐拉角轉(zhuǎn)換成旋轉(zhuǎn)矩陣（相對(duì)于世界坐標(biāo)系的旋轉(zhuǎn)矩陣）通常是按外旋方式（繞固定軸），即X-Y-Z順序，所以旋轉(zhuǎn)矩陣為：

3. 歐拉角和旋轉(zhuǎn)矩陣之間的轉(zhuǎn)換程序示例

下面程序分別使用Eigen和自定義函數(shù)測(cè)試歐拉角和旋轉(zhuǎn)矩陣之間的轉(zhuǎn)換：

#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() {// 設(shè)定車體歐拉角(角度)，繞固定軸double roll_deg = 0.5; // 繞X軸double pitch_deg = 0.8; // 繞Y軸double yaw_deg = 108.5; // 繞Z軸// 轉(zhuǎn)化為弧度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）,對(duì)應(yīng)繞x軸，繞y軸，繞z軸的旋轉(zhuǎn)角度Eigen::Vector3d euler_angle(roll_arc, pitch_arc, yaw_arc);// 使用Eigen庫(kù)將歐拉角轉(zhuǎn)換為旋轉(zhuǎn)矩陣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;// 使用自定義函數(shù)將歐拉角轉(zhuǎn)換為旋轉(zhuǎn)矩陣rotation_matrix2 = eulerAnglesToRotationMatrix(euler_angle);cout << "rotation matrix2 =\n" << rotation_matrix2 << endl << endl;// 使用Eigen將旋轉(zhuǎn)矩陣轉(zhuǎn)換為歐拉角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;// 使用自定義函數(shù)將旋轉(zhuǎn)矩陣轉(zhuǎn)換為歐拉角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; // 計(jì)算旋轉(zhuǎn)矩陣的X分量R_x <<1, 0, 0,0, cos(theta[0]), -sin(theta[0]),0, sin(theta[0]), cos(theta[0]);Eigen::Matrix3d R_y; // 計(jì)算旋轉(zhuǎn)矩陣的Y分量R_y <<cos(theta[1]), 0, sin(theta[1]),0, 1, 0,-sin(theta[1]), 0, cos(theta[1]);Eigen::Matrix3d R_z; // 計(jì)算旋轉(zhuǎn)矩陣的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. 總結(jié)

歐拉角和旋轉(zhuǎn)矩陣之間的轉(zhuǎn)換要注意的細(xì)節(jié)很多，在使用時(shí)我們一定要明確歐拉角的旋轉(zhuǎn)順序、內(nèi)旋還是外旋、正向旋轉(zhuǎn)還是反向旋轉(zhuǎn)以及歐拉角范圍。另外，文章中如果有錯(cuò)誤的地方，還請(qǐng)大佬不要噴，在評(píng)論區(qū)指出來(lái)，我核實(shí)后會(huì)修改的，謝謝。

總結(jié)

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

如果覺(jué)得生活随笔網(wǎng)站內(nèi)容還不錯(cuò)，歡迎將生活随笔推薦給好友。