繼之前研究了一些飛行姿態理論方面的問題后，又找到了之前很流行的一段外國大神寫的代碼，來分析分析。 第二篇文章的最后，講到了文章中的算法在實際使用中有重大缺陷。

大家都知道，分析算法理論的時候很多情況下我們沒有考慮太多外界干擾的情況，原因是很多情況下，傳感器的精度以及受到的干擾并不會特別大，而顯著的影響到算法。但是在IMU系統中，有點不同。由于地磁場十分微弱，而我們生活中有大量使用電子設備，使得磁場非常的混亂，以至于地磁傳感器非常容易受到干擾。

由于以上算法把地磁傳感器一同加入到姿態的測定中，并基本給予了地磁傳感器與加速度傳感器同樣的加權，導致地磁傳感器一旦被干擾，會對姿態產生地球重力突然被干擾一樣的結果。。。對姿態的測量是毀滅性的。

綜上，考慮到磁場的不穩定性，必須對地磁傳感器進行降權處理，使得他對姿態的影響變小。

于是設計了以下的算法。

將磁場傳感器的數據在姿態角度中剔除，更新姿態的俯仰角（PITCH）以及橫滾角（ROLL）的時候只使用加速度傳感器以及陀螺儀(角速度傳感器)。

只在計算偏航角的（YAW）的時候使用磁場傳感器。也就是只使用磁場傳感器作為一個電子指南針，定位整個姿態在水平面旋轉的角度。這樣設計，讓磁場傳感器只影響姿態中的一個數值，減少了磁場的權重，即使磁場收到干擾，也不會導致姿態驟變，使得四軸墜機。

在對YAW進行計算的時候使用了如下函數。

eulerAngleRaw.yaw = 0.9 * (eulerAngleRaw.yaw - gzF*2*halfT) + 0.1 * angleMagYaw;

此處的0.9和0.1是可以變動的但他們相加應該為1，此處為一個最簡單的1階低通濾波器，增加0.1則是增大截止頻率。

算法的流程圖是這樣的：

程序流程圖

新的姿態更新算法是這樣的

voidAHRSupdate(float gxf, float gyf, float gzf, float axf, float ayf, float azf, float mxf, float myf, float mzf)

{

double norm;

float vx, vy, vz;

float ex, ey, ez;

float halfT; //采樣周期的一半

// 輔助變量，以減少重復操作數

float q0q0 = q0*q0;

float q0q1 = q0*q1;

float q0q2 = q0*q2;

float q0q3 = q0*q3;

float q1q1 = q1*q1;

float q1q2 = q1*q2;

float q1q3 = q1*q3;

float q2q2 = q2*q2;

float q2q3 = q2*q3;

float q3q3 = q3*q3;

// 測量歸一化

norm = invSqrt(axf*axf + ayf*ayf + azf*azf);

axf = axf * norm; //向量a 為傳感器重力 飛行器分量

ayf = ayf * norm;

azf = azf * norm;

//norm = invSqrt(mxf*mxf + myf*myf + mzf*mzf);

// 向量m 為傳感器磁場 飛行器分量

//mxf = mxf * norm;

//myf = myf * norm;

//mzf = mzf * norm;

// 計算參考磁通方向

//hx = 2*mxf*(0.5 - q2q2 - q3q3) + 2*myf*(q1q2 - q0q3) + 2*mzf*(q1q3 + q0q2); //向量h 為磁場通過旋轉以后 參考系分量

//hy = 2*mxf*(q1q2 + q0q3) + 2*myf*(0.5 - q1q1 - q3q3) + 2*mzf*(q2q3 - q0q1);

//hz = 2*mxf*(q1q3 - q0q2) + 2*myf*(q2q3 + q0q1) + 2*mzf*(0.5 - q1q1 - q2q2);

//bx = 1.0f/invSqrt((hx*hx) + (hy*hy));

//原則上應該只有X向的分量 ex的磁場傳感器部分誤差就是由此式產生。

//bz = hz;

//估計方向的重力和磁通（V和W）

//反向使用 四元數 及把a用-a替換。

vx = 2*(q1q3 - q0q2);

//v為把重力反向旋轉到飛行器參考系時重力的向量

vy = 2*(q0q1 + q2q3);

vz = q0q0 - q1q1 - q2q2 + q3q3;

//wx = 2*bx*(0.5 - q2q2 - q3q3) + 2*bz*(q1q3 - q0q2);

// w為把磁場反向旋轉到飛行器參考系時磁場的向量

//wy = 2*bx*(q1q2 - q0q3) + 2*bz*(q0q1 + q2q3);

//wz = 2*bx*(q0q2 + q1q3) + 2*bz*(0.5 - q1q1 - q2q2);

// 錯誤是跨產品的總和之間的參考方向的領域和方向測量傳感器 //建立誤差函數 向量的外積

//ex = (ayf*vz - azf*vy) + (myf*wz - mzf*wy);

//ey = (azf*vx - axf*vz) + (mzf*wx - mxf*wz);

//ez = (axf*vy - ayf*vx) + (mxf*wy - myf*wx);

ex = (ayf*vz - azf*vy);

ey = (azf*vx - axf*vz);

ez = (axf*vy - ayf*vx);

halfT=getCurrentTime(timer4);

if (halfT>0.05)halfT=0;

//printf("%f\n\r",halfT);

if(ex != 0.0f && ey != 0.0f && ez != 0.0f)

{

// 積分誤差比例積分增益

exInt = exInt + ex*Ki*(halfT);

eyInt = eyInt + ey*Ki*(halfT);

ezInt = ezInt + ez*Ki*(halfT);

// 調整后的陀螺儀測量

gxf = gxf + Kp*ex + exInt;

gyf = gyf + Kp*ey + eyInt;

gzf = gzf + Kp*ez + ezInt;

}

// halfT=getCurrentTime();

// printf("%f \n\r", halfT);

// 整合四元數率和歸一化

//龍格-庫格法

q0 = q0 + (-q1*gxf - q2*gyf - q3*gzf)*halfT;

q1 = q1 + (q0*gxf + q2*gzf - q3*gyf)*halfT;

q2 = q2 + (q0*gyf - q1*gzf + q3*gxf)*halfT;

q3 = q3 + (q0*gzf + q1*gyf - q2*gxf)*halfT;

// 歸一化四元數

norm = invSqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);

q0 = q0 * norm;

q1 = q1 * norm;

q2 = q2 * norm;

q3 = q3 * norm;

}

與第二篇相比，算法中注釋掉了所有與磁場有關的部分。

姿態更新后，用如下語句與磁場傳感器計算出來的YAW(偏航角)進行混合

eulerAngleRaw.yaw = 0.9 * (eulerAngleRaw.yaw - gzF*2*halfT) + 0.1 * angleMagYaw;

經過檢驗，該算法姿態穩定，準確，變化迅速，YAW的值也能長期保證穩定，能為四軸飛行器提供很好的姿態數據。

發布于 2015-12-10

總結

以上是生活随笔為你收集整理的飞行姿态解算（三）的全部內容，希望文章能夠幫你解決所遇到的問題。

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