四元数姿态解算及多传感器融合详细解析

代码路径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.   
9.     // Use IMU algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation)  
10.     if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) {  
11.         MahonyAHRSupdateIMU(gx, gy, gz, ax, ay, az);  /*如果测量的磁传感器值为0，则只需要用加速度值对陀螺数据进行补偿*/
12.         return;  
13.     }  
14.   
15.     // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)  
16.     if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {  
17.   
18.         /* 对重力加速度进行归一化*/
19.         recipNorm = invSqrt(ax * ax + ay * ay + az * az);  
20.         ax *= recipNorm;  
21.         ay *= recipNorm;  
22.         az *= recipNorm;       
23.          /*归一化结束*/
24.  
25.         /* 对磁传感器进行归一化*/
26.         recipNorm = invSqrt(mx * mx + my * my + mz * mz);  
27.         mx *= recipNorm;  
28.         my *= recipNorm;  
29.         mz *= recipNorm;     
30.          /*归一化结束*/
31.  
32.       /*提前进行四元数的相关运算，为后面的矩阵旋转做准备，更便于与推导矩阵风格统一
33.         q0q0 = q0 * q0;  
34.         q0q1 = q0 * q1;  
35.         q0q2 = q0 * q2;  
36.         q0q3 = q0 * q3;  
37.         q1q1 = q1 * q1;  
38.         q1q2 = q1 * q2;  
39.         q1q3 = q1 * q3;  
40.         q2q2 = q2 * q2;  
41.         q2q3 = q2 * q3;  
42.         q3q3 = q3 * q3;     
43.   

        /*根据机体坐标系下实际测量得到磁传感器值左乘四元数旋转矩阵得到地理坐标系下的磁传感器值*/  

现在我们假设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（这部分解说间黄色底色部分）

44.         hx = 2.0f * (mx * (0.5f - q2q2 - q3q3) + my * (q1q2 - q0q3) + mz * (q1q3 + q0q2));  
45.         hy = 2.0f * (mx * (q1q2 + q0q3) + my * (0.5f - q1q1 - q3q3) + mz * (q2q3 - q0q1));  
46.         bx = sqrt(hx * hx + hy * hy);  
47.         bz = 2.0f * (mx * (q1q3 - q0q2) + my * (q2q3 + q0q1) + mz * (0.5f - q1q1 - q2q2));  

对于重力加速度的补偿相比于磁传感器要简单的多，我们认为理想情况下的飞机状态能够达到和当地地理坐标系XOY水平的状态，那么此时的重力加速度值应该为0,0，1（归一化后），那么根据此地理坐标系下的重力加速度值，右乘CbR即可得到此时机体坐标系下的重力加速度估计值（见红色底色部分）

48.         // Estimated direction of gravity and magnetic field  
49.         halfvx = q1q3 - q0q2;  
50.         halfvy = q0q1 + q2q3;  
51.         halfvz = q0q0 - 0.5f + q3q3;  
52.         halfwx = bx * (0.5f - q2q2 - q3q3) + bz * (q1q3 - q0q2);  
53.         halfwy = bx * (q1q2 - q0q3) + bz * (q0q1 + q2q3);  
54.         halfwz = bx * (q0q2 + q1q3) + bz * (0.5f - q1q1 - q2q2);    
55.       
56.         /*然后根据上述得到估计值和实测值做叉乘，即可得到陀螺仪的漂移误差，从而使用PI对陀螺仪的结果进行补偿（见绿色部分）*/ 
57.         halfex = (ay * halfvz - az * halfvy) + (my * halfwz - mz * halfwy);  
58.         halfey = (az * halfvx - ax * halfvz) + (mz * halfwx - mx * halfwz);  
59.         halfez = (ax * halfvy - ay * halfvx) + (mx * halfwy - my * halfwx);  
60.   
61.         // Compute and apply integral feedback if enabled  
62.         if(twoKi > 0.0f) {  
63.             integralFBx += twoKi * halfex * (1.0f / sampleFreq);    // integral error scaled by Ki  
64.             integralFBy += twoKi * halfey * (1.0f / sampleFreq);  
65.             integralFBz += twoKi * halfez * (1.0f / sampleFreq);  
66.             gx += integralFBx;  // apply integral feedback  
67.             gy += integralFBy;  
68.             gz += integralFBz;  
69.         }  
70.         else {  
71.             integralFBx = 0.0f; // prevent integral windup  
72.             integralFBy = 0.0f;  
73.             integralFBz = 0.0f;  
74.         }  
75.   
76.         // Apply proportional feedback  
77.         gx += twoKp * halfex;  
78.         gy += twoKp * halfey;  
79.         gz += twoKp * halfez;  
80.     }  
81.       /*至此我们认为得到了准确的gx，gy，gz，然后根据得到的值跟新四元数，更新的依据是四元数的一阶龙格库塔法*/

 

82.     // Integrate rate of change of quaternion  
83.     gx *= (0.5f * (1.0f / sampleFreq));     // pre-multiply common factors  
84.     gy *= (0.5f * (1.0f / sampleFreq));  
85.     gz *= (0.5f * (1.0f / sampleFreq));  
86.     qa = q0;  
87.     qb = q1;  
88.     qc = q2;  
89.     q0 += (-qb * gx - qc * gy - q3 * gz);  
90.     q1 += (qa * gx + qc * gz - q3 * gy);  
91.     q2 += (qa * gy - qb * gz + q3 * gx);  
92.     q3 += (qa * gz + qb * gy - qc * gx);   

/*四元数更新完毕*/

93.     // Normalise quaternion  
94.     recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);  
95.     q0 *= recipNorm;  
96.     q1 *= recipNorm;  
97.     q2 *= recipNorm;  
98.     q3 *= recipNorm; 

     /*将更新后的四元数进行归一化，并根据与余弦矩阵的关系可以得到yaw,pitch，roll角度，进而进行下，一次的数据融合 

99. } 

根据余弦矩阵和四元数旋转矩阵的关系可以得到角度关系

（roll,pitch，yaw）