生活随笔
收集整理的這篇文章主要介紹了
四元数姿态解算及多传感器融合详细解析
小編覺得挺不錯的,現在分享給大家,幫大家做個參考.
代碼路徑ardupolit/modules/PX4Firmware/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp
最近結合慣性導航這本書，詳細看了四元數姿態解算的代碼，然后對這部分代碼進行了詳細的分析，分享給大家，如果分析有誤請大家留言不吝賜教！！
void?AHRS_update(float?gx,?float?gy,?float?gz,?float?ax,?float?ay,?float?az,?float?mx,?float?my,?float?mz)?{??????float?recipNorm;??????float?q0q0,?q0q1,?q0q2,?q0q3,?q1q1,?q1q2,?q1q3,?q2q2,?q2q3,?q3q3;????????float?hx,?hy,?bx,?bz;?/*地理坐標系下的磁強度值*/????float?halfvx,?halfvy,?halfvz,?halfwx,?halfwy,?halfwz;/*根據陀螺儀更新的四元數估計出的機體坐標系下的加速度值和磁傳感器值*/????float?halfex,?halfey,?halfez;??/*根據實際測得的加速度值和磁傳感器值和估計得出的上述值叉乘得到的估計誤差*/????float?qa,?qb,?qc;??/*四元數*/??????//?Use?IMU?algorithm?if?magnetometer?measurement?invalid?(avoids?NaN?in?magnetometer?normalisation)??????if((mx?==?0.0f)?&&?(my?==?0.0f)?&&?(mz?==?0.0f))?{??????????MahonyAHRSupdateIMU(gx,?gy,?gz,?ax,?ay,?az);??/*如果測量的磁傳感器值為0，則只需要用加速度值對陀螺數據進行補償*/????????return;??????}????????//?Compute?feedback?only?if?accelerometer?measurement?valid?(avoids?NaN?in?accelerometer?normalisation)??????if(!((ax?==?0.0f)?&&?(ay?==?0.0f)?&&?(az?==?0.0f)))?{????????????/*?對重力加速度進行歸一化*/????????recipNorm?=?invSqrt(ax?*?ax?+?ay?*?ay?+?az?*?az);??????????ax?*=?recipNorm;??????????ay?*=?recipNorm;??????????az?*=?recipNorm;??????????????? /*歸一化結束*/?????????/*?對磁傳感器進行歸一化*/????????recipNorm?=?invSqrt(mx?*?mx?+?my?*?my?+?mz?*?mz);??????????mx?*=?recipNorm;??????????my?*=?recipNorm;??????????mz?*=?recipNorm;????????? ??? /*歸一化結束*/???????/*提前進行四元數的相關運算，為后面的矩陣旋轉做準備，更便于與推導矩陣風格統一????????q0q0?=?q0?*?q0;??????????q0q1?=?q0?*?q1;??????????q0q2?=?q0?*?q2;??????????q0q3?=?q0?*?q3;??????????q1q1?=?q1?*?q1;??????????q1q2?=?q1?*?q2;??????????q1q3?=?q1?*?q3;??????????q2q2?=?q2?*?q2;??????????q2q3?=?q2?*?q3;??????????q3q3?=?q3?*?q3;???????????????/*根據機體坐標系下實際測量得到磁傳感器值左乘四元數旋轉矩陣得到地理坐標系下的磁傳感器值*/??
現在我們假設CbR旋轉矩陣是經過加速度計校正后的矩陣，當某個確定的向量（機體系中）經過這個矩陣旋轉之后（到地理坐標系），這兩個坐標系在XOY平面上重合，只是在z軸旋轉上會存在一個偏航角的誤差。下圖表示的是經過CbR旋轉之后的機體坐標系b和地理坐標系n的相對關系。可以明顯發現加速度計可以把機體坐標系b通過四元數法從任意角度拉到與地理坐標系n水平的位置上，這時，只剩下一個偏航角誤差。這也是為什么加速度計誤差修正偏航的原因。
? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ??
hx，hy，hz是地理坐標系下的磁傳感器值，可以有機體坐標系下的mx，my，mz左乘CbR得到，假設理想情況下的機體能夠和當地的地理坐標系處于同一XOY平面，且機頭指北，那么此時的磁傳感器值應該為bx，0，bz，此時我們很方便的可以得到bx2=hx2+hy2，bz=hz，當時理想必定是理想，飛機的姿態不可能達到這種狀態，所以我們再根據bx，0，bz（地理坐標系）右乘CbR得到估計后的磁傳感器值halfwx，halfwy，halfwz（這部分解說間黃色底色部分）
????????hx?=?2.0f?*?(mx?*?(0.5f?-?q2q2?-?q3q3)?+?my?*?(q1q2?-?q0q3)?+?mz?*?(q1q3?+?q0q2));??????????hy?=?2.0f?*?(mx?*?(q1q2?+?q0q3)?+?my?*?(0.5f?-?q1q1?-?q3q3)?+?mz?*?(q2q3?-?q0q1));??????????bx?=?sqrt(hx?*?hx?+?hy?*?hy);??????????bz?=?2.0f?*?(mx?*?(q1q3?-?q0q2)?+?my?*?(q2q3?+?q0q1)?+?mz?*?(0.5f?-?q1q1?-?q2q2));??對于重力加速度的補償相比于磁傳感器要簡單的多，我們認為理想情況下的飛機狀態能夠達到和當地地理坐標系XOY水平的狀態，那么此時的重力加速度值應該為0,0，1（歸一化后），那么根據此地理坐標系下的重力加速度值，右乘CbR即可得到此時機體坐標系下的重力加速度估計值（見紅色底色部分）
????????//?Estimated?direction?of?gravity?and?magnetic?field??????????halfvx?=?q1q3?-?q0q2;??????????halfvy?=?q0q1?+?q2q3;??????????halfvz?=?q0q0?-?0.5f?+?q3q3;??????????halfwx?=?bx?*?(0.5f?-?q2q2?-?q3q3)?+?bz?*?(q1q3?-?q0q2);??????????halfwy?=?bx?*?(q1q2?-?q0q3)?+?bz?*?(q0q1?+?q2q3);??????????halfwz?=?bx?*?(q0q2?+?q1q3)?+?bz?*?(0.5f?-?q1q1?-?q2q2);??????????????????/*然后根據上述得到估計值和實測值做叉乘，即可得到陀螺儀的漂移誤差，從而使用PI對陀螺儀的結果進行補償（見綠色部分）*/?????????halfex?=?(ay?*?halfvz?-?az?*?halfvy)?+?(my?*?halfwz?-?mz?*?halfwy);??????????halfey?=?(az?*?halfvx?-?ax?*?halfvz)?+?(mz?*?halfwx?-?mx?*?halfwz);??????????halfez?=?(ax?*?halfvy?-?ay?*?halfvx)?+?(mx?*?halfwy?-?my?*?halfwx);????????????//?Compute?and?apply?integral?feedback?if?enabled??????????if(twoKi?>?0.0f)?{??????????????integralFBx?+=?twoKi?*?halfex?*?(1.0f?/?sampleFreq);????//?integral?error?scaled?by?Ki??????????????integralFBy?+=?twoKi?*?halfey?*?(1.0f?/?sampleFreq);??????????????integralFBz?+=?twoKi?*?halfez?*?(1.0f?/?sampleFreq);??????????????gx?+=?integralFBx;??//?apply?integral?feedback??????????????gy?+=?integralFBy;??????????????gz?+=?integralFBz;??????????}??????????else?{??????????????integralFBx?=?0.0f;?//?prevent?integral?windup??????????????integralFBy?=?0.0f;??????????????integralFBz?=?0.0f;??????????}????????????//?Apply?proportional?feedback??????????gx?+=?twoKp?*?halfex;??????????gy?+=?twoKp?*?halfey;??????????gz?+=?twoKp?*?halfez;??????}????????/*至此我們認為得到了準確的gx，gy，gz，然后根據得到的值跟新四元數，更新的依據是四元數的一階龍格庫塔法*/?
????//?Integrate?rate?of?change?of?quaternion??????gx?*=?(0.5f?*?(1.0f?/?sampleFreq));?????//?pre-multiply?common?factors??????gy?*=?(0.5f?*?(1.0f?/?sampleFreq));??????gz?*=?(0.5f?*?(1.0f?/?sampleFreq));??????qa?=?q0;??????qb?=?q1;??????qc?=?q2;??????q0?+=?(-qb?*?gx?-?qc?*?gy?-?q3?*?gz);??????q1?+=?(qa?*?gx?+?qc?*?gz?-?q3?*?gy);??????q2?+=?(qa?*?gy?-?qb?*?gz?+?q3?*?gx);??????q3?+=?(qa?*?gz?+?qb?*?gy?-?qc?*?gx);???/*四元數更新完畢*/
????//?Normalise?quaternion??????recipNorm?=?invSqrt(q0?*?q0?+?q1?*?q1?+?q2?*?q2?+?q3?*?q3);??????q0?*=?recipNorm;??????q1?*=?recipNorm;??????q2?*=?recipNorm;??????q3?*=?recipNorm;??????/*將更新后的四元數進行歸一化，并根據與余弦矩陣的關系可以得到yaw,pitch，roll角度，進而進行下，一次的數據融合?
}?根據余弦矩陣和四元數旋轉矩陣的關系可以得到角度關系
（roll,pitch，yaw）
原文來自于https://blog.csdn.net/u014645605/article/details/75006451
總結
以上是生活随笔為你收集整理的四元数姿态解算及多传感器融合详细解析的全部內容，希望文章能夠幫你解決所遇到的問題。
如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。
