四元數姿态解算及多傳感器融合詳細解析

代碼路徑ardupolit/modules/PX4Firmware/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp

最近結合慣性導航這本書，詳細看了四元數姿态解算的代碼，然後對這部分代碼進行了詳細的分析，分享給大家，如果分析有誤請大家留言不吝賜教！！

1. void AHRS_update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) {  
2.     float recipNorm;  
3.     float q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;    
4.     float hx, hy, bx, bz; 
5.     float halfvx, halfvy, halfvz, halfwx, halfwy, halfwz;
6.     float halfex, halfey, halfez;  
7.     float qa, qb, qc;  
8.     // Use IMU algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation)  
9.     if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) {  
10.         MahonyAHRSupdateIMU(gx, gy, gz, ax, ay, az);  
11.         return;  
12.     }  
13.     // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)  
14.     if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {  
15.         recipNorm = invSqrt(ax * ax + ay * ay + az * az);  
16.         ax *= recipNorm;  
17.         ay *= recipNorm;  
18.         az *= recipNorm;       
19.         recipNorm = invSqrt(mx * mx + my * my + mz * mz);  
20.         mx *= recipNorm;  
21.         my *= recipNorm;  
22.         mz *= recipNorm;     
23. 現在我們假設CbR旋轉矩陣是經過加速度計校正後的矩陣，當某個确定的向量（機體系中）經過這個矩陣旋轉之後（到地理坐标系），這兩個坐标系在XOY平面上重合，隻是在z軸旋轉上會存在一個偏航角的誤差。下圖表示的是經過CbR旋轉之後的機體坐标系b和地理坐标系n的相對關系。可以明顯發現加速度計可以把機體坐标系b通過四元數法從任意角度拉到與地理坐标系n水準的位置上，這時，隻剩下一個偏航角誤差。這也是為什麼加速度計誤差修正偏航的原因。

    

    

    hx，hy，hz是地理坐标系下的磁傳感器值，可以有機體坐标系下的mx，my，mz左乘CbR得到，假設理想情況下的機體能夠和當地的地理坐标系處于同一XOY平面，且機頭指北，那麼此時的磁傳感器值應該為bx，0，bz，此時我們很友善的可以得到bx²=hx²+hy²，bz=hz，當時理想必定是理想，飛機的姿态不可能達到這種狀态，是以我們再根據bx，0，bz（地理坐标系）右乘CbR得到估計後的磁傳感器值halfwx，halfwy，halfwz（這部分解說間黃色底色部分）
    1.         hx = 2.0f * (mx * (0.5f - q2q2 - q3q3) + my * (q1q2 - q0q3) + mz * (q1q3 + q0q2));  
    2.         hy = 2.0f * (mx * (q1q2 + q0q3) + my * (0.5f - q1q1 - q3q3) + mz * (q2q3 - q0q1));  
    3.         bx = sqrt(hx * hx + hy * hy);  
    4.         bz = 2.0f * (mx * (q1q3 - q0q2) + my * (q2q3 + q0q1) + mz * (0.5f - q1q1 - q2q2));  
    對于重力加速度的補償相比于磁傳感器要簡單的多，我們認為理想情況下的飛機狀态能夠達到和當地地理坐标系XOY水準的狀态，那麼此時的重力加速度值應該為0,0，1（歸一化後），那麼根據此地理坐标系下的重力加速度值，右乘CbR即可得到此時機體坐标系下的重力加速度估計值（見紅色底色部分）

    

    1.         // Estimated direction of gravity and magnetic field  
    2.         halfvx = q1q3 - q0q2;  
    3.         halfvy = q0q1 + q2q3;  
    4.         halfvz = q0q0 - 0.5f + q3q3;  
    5.         halfwx = bx * (0.5f - q2q2 - q3q3) + bz * (q1q3 - q0q2);  
    6.         halfwy = bx * (q1q2 - q0q3) + bz * (q0q1 + q2q3);  
    7.         halfwz = bx * (q0q2 + q1q3) + bz * (0.5f - q1q1 - q2q2);    
    8.         halfex = (ay * halfvz - az * halfvy) + (my * halfwz - mz * halfwy);  
    9.         halfey = (az * halfvx - ax * halfvz) + (mz * halfwx - mx * halfwz);  
    10.         halfez = (ax * halfvy - ay * halfvx) + (mx * halfwy - my * halfwx);  
    11.         // Compute and apply integral feedback if enabled  
    12.         if(twoKi > 0.0f) {  
    13.             integralFBx += twoKi * halfex * (1.0f / sampleFreq);    // integral error scaled by Ki  
    14.             integralFBy += twoKi * halfey * (1.0f / sampleFreq);  
    15.             integralFBz += twoKi * halfez * (1.0f / sampleFreq);  
    16.             gx += integralFBx;  // apply integral feedback  
    17.             gy += integralFBy;  
    18.             gz += integralFBz;  
    19.         }  
    20.         else {  
    21.             integralFBx = 0.0f; // prevent integral windup  
    22.             integralFBy = 0.0f;  
    23.             integralFBz = 0.0f;  
    24.         }  
    25.         // Apply proportional feedback  
    26.         gx += twoKp * halfex;  
    27.         gy += twoKp * halfey;  
    28.         gz += twoKp * halfez;  
    29.     }  

    

    1.     // Integrate rate of change of quaternion  
    2.     gx *= (0.5f * (1.0f / sampleFreq));     // pre-multiply common factors  
    3.     gy *= (0.5f * (1.0f / sampleFreq));  
    4.     gz *= (0.5f * (1.0f / sampleFreq));  
    5.     qa = q0;  
    6.     qb = q1;  
    7.     qc = q2;  
    8.     q0 += (-qb * gx - qc * gy - q3 * gz);  
    9.     q1 += (qa * gx + qc * gz - q3 * gy);  
    10.     q2 += (qa * gy - qb * gz + q3 * gx);  
    11.     q3 += (qa * gz + qb * gy - qc * gx);   
    1.     // Normalise quaternion  
    2.     recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);  
    3.     q0 *= recipNorm;  
    4.     q1 *= recipNorm;  
    5.     q2 *= recipNorm;  
    6.     q3 *= recipNorm; 
         /*将更新後的四元數進行歸一化，并根據與餘弦矩陣的關系可以得到yaw,pitch，roll角度，進而進行下，一次的資料融合 
    1. } 
    根據餘弦矩陣和四元數旋轉矩陣的關系可以得到角度關系

    

    

    

    （roll,pitch，yaw）