快捷導(dǎo)航

C++利用eigen庫(kù)實(shí)現(xiàn)求歐拉角

更新時(shí)間：2023年11月01日 10:02:13 作者：Cc

這篇文章主要為大家詳細(xì)介紹了C++如何利用eigen庫(kù)自帶的matrix.eulerAngles()函數(shù)實(shí)現(xiàn)求歐拉角，文中的示例代碼講解詳細(xì)，有需要的小伙伴可以參考下

不同順序歐拉角轉(zhuǎn)旋轉(zhuǎn)矩陣對(duì)照公式

eigen庫(kù)求歐拉角公式

分別試驗(yàn)eigen庫(kù)自帶的matrix.eulerAngles()函數(shù)，與根據(jù)上述公式推導(dǎo)的兩種方法求歐拉角
eigen庫(kù)求得歐拉角的范圍一定是x−>roll方向在[0,π]之間，y−>pitch方向在[−π,π]之間，z−>yaw方向在[−π,π]之間，優(yōu)先會(huì)保證x−>roll方向處于[0,π]之間，才回去算別的角度。

以下代碼驗(yàn)證：

#include <iostream>
#include <cmath>
#include <Eigen/Dense>
Quaterniond q(0.704,-0.708,0.05,0.002)//定義一個(gè)四元數(shù);
Matrix3d m = q.toRotationMatrix();//四元數(shù)轉(zhuǎn)旋轉(zhuǎn)矩陣;
cout << q << endl;
cout << m << endl;
Quaterniond q1(m);//旋轉(zhuǎn)矩陣轉(zhuǎn)四元數(shù)，驗(yàn)證下四元數(shù)是否和原來(lái)輸入的四元數(shù)一致
cout << q1 << endl;
cout << "XYZ------------" << endl;
Vector3d v=m.eulerAngles(0,1,2);//旋轉(zhuǎn)矩陣轉(zhuǎn)歐拉角，以XYZ為旋轉(zhuǎn)方向
cout << v << endl;
cout << "ZYX------------" << endl;
Vector3d v1=m.eulerAngles(2,1,0);//旋轉(zhuǎn)矩陣轉(zhuǎn)歐拉角，以ZYX為旋轉(zhuǎn)方向
cout << v1 << endl;
Vector3d v2;

輸出的結(jié)果為

-0.708i + 0.05j + 0.002k + 0.704//四元數(shù)
0.994992 -0.073616 0.067568
-0.067984 -0.002536 0.997064
-0.073232 -0.996664 -0.007528//旋轉(zhuǎn)矩陣
-0.70756i + 0.049969j + 0.00199876k + 0.704437//四元數(shù)
XYZ------------//先轉(zhuǎn)x
1.56325//x roll
3.07397//y pitch
-3.06774//z yaw
ZYX------------//先轉(zhuǎn)z
3.07337// z yaw
3.06825//y pitch
1.56324//x roll

可以看到以上兩種旋轉(zhuǎn)順序情況下用eigen庫(kù)自帶的matrix.eulerAngles()函數(shù)會(huì)使得z yaw，y pitch方向上計(jì)算的歐拉角數(shù)值增大，不利于機(jī)器人做角度控制，原本機(jī)器人可能朝反方向轉(zhuǎn)很小的一個(gè)角度，可能現(xiàn)在需要轉(zhuǎn)很大一圈。

用公式推導(dǎo)編寫(xiě)的代碼：

//接著上面代碼段補(bǔ)充運(yùn)行即可
double sy=sqrt(m(0,0) * m(0,0) +  m(0,1) * m(0,1));
        if (sy>0.00001)//判斷萬(wàn)向鎖奇異
        {
            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)//判斷萬(wàn)向鎖奇異
        {
            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;

輸出結(jié)果：

XYZ------------
-1.57835//x
0.0676197//y
0.073852//z
ZYX------------
-1.57835 //注意這里還是x，與`eigen`庫(kù)自帶的`matrix.eulerAngles()`出來(lái)的順序不一樣
-0.0732686
0.0682201//注意這里還是z

這里角度明顯小很多，兩種不同的方法的結(jié)果可以通過(guò)加減π得到，但是XYZ、ZYX兩種不同順序下的結(jié)果還不一樣

因此建議直接用公式推導(dǎo)出來(lái)的方法。

到此這篇關(guān)于C++利用eigen庫(kù)實(shí)現(xiàn)求歐拉角的文章就介紹到這了,更多相關(guān)C++求歐拉角內(nèi)容請(qǐng)搜索腳本之家以前的文章或繼續(xù)瀏覽下面的相關(guān)文章希望大家以后多多支持腳本之家！

您可能感興趣的文章: