四軸軟件姿態解算
轉載地址:http://www.crazepony.com/wiki/software-algorithm.html#四元數求解
作者：路洋/nieyong
常見姿態解算算法
首先亮出現在常見的軟件姿態解算算法，這樣在閱讀相關姿態解算的文章時，不會越整越糊塗。
- 非線性互補濾波算法
- 卡爾曼濾波算法
- Mahony互補濾波算法（Crazepony開源四軸飛行器使用這種）
這些姿態解算算法都是使用一些巧妙的方式用加速度計的數據（或者加上電子羅盤），去修正由陀螺儀數據快速解算得到的存在誤差的飛行器姿態（即四元數）。最終得到准確的飛行器姿態。要讀懂本文，必須先閱讀Crazepony開源四軸百科中的《姿態解算簡介》和《陀螺儀加速度計MPU6050》。
算法主要參考的文章有：
Mahony互補濾波算法
下面將會以Crazepony開源四軸飛行器的代碼為實例講解Mahony互補濾波算法。看看如何通過軟件姿態解算，把IMU輸出的數據融合為飛行器精准的姿態。
代碼位於IMUSO3.c文件中，函數如下。
//函數名：NonlinearSO3AHRSupdate()
//描述：姿態解算融合，是Crazepony和核心算法
//使用的是Mahony互補濾波算法，沒有使用Kalman濾波算法
//改算法是直接參考pixhawk飛控的算法，可以在Github上看到出處
//https://github.com/hsteinhaus/PX4Firmware/blob/master/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp
static void NonlinearSO3AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, float twoKp, float twoKi, float dt)
{
……
}
- 輸入參數
gx，gy，gz分別對應三個軸的角速度，單位是弧度/秒。 - 輸入參數
ax，ay，az分別對應三個軸的加速度數據，由於加速度的噪聲較大，在從MPU6050讀取的時候該數據是采用LPF2pApply_x低通濾波函數的。 - 輸出參數
mx，my，mz分別對應三軸的電子羅盤數據，Crazepony四軸暫時沒有使用到。 - 輸入參數
twoKp，twoKi是控制加速度計修正陀螺儀積分姿態的速度，是定義的一個常量。 - 輸入參數
dt是從上一次解算到本次解算的時間差，也就是角速度積分用的時間項。
輸入參數twoKp，twoKi的宏定義如下：
#define so3_comp_params_Kp 1.0f
#define so3_comp_params_Ki 0.05f
下面我們逐行分析Mohony互補濾波姿態解算的代碼。
if(!((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f))) {
{
……
}
if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f)))
{
……
}
當電子羅盤數據有效的時候，需要融合電子羅盤的數據。Crazepony沒有使用磁力計，所以這段代碼略過。下面進入第二段的加速度數據融合。
recipNorm = invSqrt(ax * ax + ay * ay + az * az);
ax *= recipNorm;
ay *= recipNorm;
az *= recipNorm;
對加速度數據進行歸一化，得到單位加速度。
halfvx = q1q3 - q0q2;
halfvy = q0q1 + q2q3;
halfvz = q0q0 - 0.5f + q3q3;
把飛行器上次計算得到的姿態（四元數）換算成“方向余弦矩陣”中的第三列的三個元素。根據余弦矩陣和歐拉角的定義，地理坐標系的重力向量，轉到機體坐標系，正好是這三個元素。所以這里的halfvx、halfvy、halfvz，其實就是用上一次飛行器機體姿態（四元數）換算出來的在當前的機體坐標系上的重力單位向量。注意，Crazepony代碼中取了其中的一半1/2，下面的誤差同理。
halfex += ay * halfvz - az * halfvy;
halfey += az * halfvx - ax * halfvz;
halfez += ax * halfvy - ay * halfvx;
在機體坐標系上，加速度計測出來的重力向量是ax、ay、az。由上次姿態解算的姿態（可以簡單認為是陀螺積分）來推算出的重力向量是halfvx、halfvy、halfvz。它們之間的誤差向量，就是上次姿態解算（可以認為是陀螺儀積分）后的姿態和加速度計測出來的姿態之間的誤差。
向量間的誤差，可以用向量積(也叫外積、叉乘)來表示，ex、ey、ez就是兩個重力向量的叉積。這個叉積向量仍舊是位於機體坐標系上的，而陀螺積分誤差也是在機體坐標系，而且叉積的大小與陀螺積分誤差成正比，正好拿來糾正陀螺。由於陀螺是對機體直接積分，所以對陀螺的糾正量會直接體現在對機體坐標系的糾正。
所以上面一段代碼含義就是獲得叉乘誤差。
gyro_bias[0] += twoKi * halfex * dt; // integral error scaled by Ki
gyro_bias[1] += twoKi * halfey * dt;
gyro_bias[2] += twoKi * halfez * dt;
// apply integral feedback
gx += gyro_bias[0];
gy += gyro_bias[1];
gz += gyro_bias[2];
上面一段代碼，叉乘誤差進行積分。
// Apply proportional feedback
gx += twoKp * halfex;
gy += twoKp * halfey;
gz += twoKp * halfez;
上面一段代碼，用叉乘誤差來做PI修正陀螺零偏，通過調節twoKp，twoKi兩個參數，可以控制加速度計修正陀螺儀積分姿態的速度。
到此為止，使用加速度計數據對陀螺儀數據的修正已經完成，這就是姿態解算中的深度融合。
下面就是四元數微分方程，使用修正后的陀螺儀數據對時間積分，得到飛行器的當前姿態（四元數表示）。然后進行四元數的單位化處理。
dq0 = 0.5f*(-q1 * gx - q2 * gy - q3 * gz);
dq1 = 0.5f*(q0 * gx + q2 * gz - q3 * gy);
dq2 = 0.5f*(q0 * gy - q1 * gz + q3 * gx);
dq3 = 0.5f*(q0 * gz + q1 * gy - q2 * gx);
q0 += dt*dq0;
q1 += dt*dq1;
q2 += dt*dq2;
q3 += dt*dq3;
// Normalise quaternion
recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
q0 *= recipNorm;
q1 *= recipNorm;
q2 *= recipNorm;
q3 *= recipNorm;
歐拉微分方程
這部分由路洋提供
使用MPU6050硬件DMP解算姿態是非常簡單的，下面介紹由三軸陀螺儀和加速度計的值來使用軟件算法解算姿態的方法。
我們先來看看如何用歐拉角描述一次平面旋轉(坐標變換)：
設坐標系繞旋轉α角后得到坐標系,在空間中有一個矢量在坐標系中的投影為,在內的投影為由於旋轉繞進行，所以Z坐標未變，即有。
轉換成矩陣形式表示為：
整理一下：
所以從旋轉到可以寫成
上面僅僅是繞一根軸的旋轉，如果三維空間中的歐拉角旋轉要轉三次:
上面得到了一個表示旋轉的方向余弦矩陣。
不過要想用歐拉角解算姿態，其實我們套用歐拉角微分方程就行了：
上式中左側,,是本次更新后的歐拉角,對應row,pit,yaw。右側，是上個周期測算出來的角度，，，三個角速度由直接安裝在四軸飛行器的三軸陀螺儀在這個周期轉動的角度，單位為弧度，計算間隔時T陀螺角速度，比如0.02秒0.01弧度/秒=0.0002弧度。間因此求解這個微分方程就能解算出當前的歐拉角。
前面介紹了什么是歐拉角，而且歐拉角微分方程解算姿態關系簡單明了，概念直觀容易理解，那么我們為什么不用歐拉角來表示旋轉而要引入四元數呢？
一方面是因為歐拉角微分方程中包含了大量的三角運算，這給實時解算帶來了一定的困難。而且當俯仰角為90度時方程式會出現神奇的“GimbalLock”。所以歐拉角方法只適用於水平姿態變化不大的情況，而不適用於全姿態飛行器的姿態確定。
四元數求解
四元數法只求解四個未知量的線性微分方程組，計算量小，易於操作，是比較實用的工程方法。
我們知道在平面(x,y)中的旋轉可以用復數來表示，同樣的三維中的旋轉可以用單位四元數來描述。我們來定義一個四元數：
我們可以把它寫成,其中,。那么是矢量，表示三維空間中的旋轉軸。w是標量，表示旋轉角度。那么就是繞軸旋轉w度，所以一個四元數可以表示一個完整的旋轉。只有單位四元數才可以表示旋轉，至於為什么，因為這就是四元數表示旋轉的約束條件。
而剛才用歐拉角描述的方向余弦矩陣用四元數描述則為：
所以在軟件解算中，我們要首先把加速度計采集到的值(三維向量)轉化為單位向量,即向量除以模，傳入參數是陀螺儀x,y,z值和加速度計x,y,z值：
void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az) {
float norm;
float vx, vy, vz;
float ex, ey, ez;
norm = sqrt(ax*ax + ay*ay + az*az);
ax = ax / norm;
ay = ay / norm;
az = az / norm;
下面把四元數換算成方向余弦中的第三行的三個元素。剛好vx,vy,vz 其實就是上一次的歐拉角（四元數）的機體坐標參考系換算出來的重力的單位向量。
// estimated direction of gravity
vx = 2*(q1*q3 - q0*q2);
vy = 2*(q0*q1 + q2*q3);
vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;axyz是機體坐標參照系上，加速度計測出來的重力向量，也就是實際測出來的重力向量。
axyz是測量得到的重力向量，vxyz是陀螺積分后的姿態來推算出的重力向量，它們都是機體坐標參照系上的重力向量。
那它們之間的誤差向量，就是陀螺積分后的姿態和加計測出來的姿態之間的誤差。
向量間的誤差，可以用向量叉積（也叫向量外積、叉乘）來表示，exyz就是兩個重力向量的叉積。
這個叉積向量仍舊是位於機體坐標系上的，而陀螺積分誤差也是在機體坐標系，而且叉積的大小與陀螺積分誤差成正比，正好拿來糾正陀螺。（你可以自己拿東西想象一下）由於陀螺是對機體直接積分，所以對陀螺的糾正量會直接體現在對機體坐標系的糾正。
// integral error scaled integral gain
exInt = exInt + ex*Ki;
eyInt = eyInt + ey*Ki;
ezInt = ezInt + ez*Ki;用叉積誤差來做PI修正陀螺零偏
// integral error scaled integral gain
exInt = exInt + ex*Ki;
eyInt = eyInt + ey*Ki;
ezInt = ezInt + ez*Ki;
// adjusted gyroscope measurements
gx = gx + Kp*ex + exInt;
gy = gy + Kp*ey + eyInt;
gz = gz + Kp*ez + ezInt;
得到的原始角速度數據需要乘一個系數(halfT)轉換成實際的角速度（弧度每秒），而加速度不需要，因為加速度歸一化后與大小無關了,這里使用了一階龍哥庫塔求解四元數微分方程：
// integrate quaternion rate and normalise
q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;
q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;
q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;
q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT; 最后根據四元數方向余弦陣和歐拉角的轉換關系，把四元數轉換成歐拉角：
所以有：
Q_ANGLE.Yaw = atan2(2 * q1 * q2 + 2 * q0 * q3, -2 * q2*q2 - 2 * q3* q3 + 1)* 57.3; // yaw
Q_ANGLE.Y = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitch
Q_ANGLE.X = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll
更多閱讀：
基於四元數的簡單互補濾波姿態解算：https://blog.csdn.net/wubaobao1993/article/details/53130362
四元數+互補濾波:https://www.cnblogs.com/jins-note/p/9512586.html