最近用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();