繼之前研究了一些飛行姿態理論方面的問題后，又找到了之前很流行的一段外國大神寫的代碼，來分析分析。

先貼上代碼：
#include "AHRS.h"
//----------------------------------------------------------------------------------------------------
// Definitions
#define Kp 2.0f
#define Ki 0.005f
#define halfT 0.5f
//----------------------------------------------------------------------------------------------------
// Variable definitions
float q0 = 1, q1 = 0, q2 = 0, q3 = 0;
float exInt = 0, eyInt = 0, ezInt = 0;
//----------------------------------------------------------------------------------------------------
// Function
void AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) {
float norm;
float hx, hy, hz, bx, bz;
float vx, vy, vz, wx, wy, wz;
float ex, ey, ez;

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 = sqrt(ax*ax + ay*ay + az*az);
ax = ax / norm;
ay = ay / norm;
az = az / norm;
norm = sqrt(mx*mx + my*my + mz*mz);
mx = mx / norm;
my = my / norm;
mz = mz / norm;

hx = 2*mx*(0.5 - q2q2 - q3q3) + 2*my*(q1q2 - q0q3) + 2*mz*(q1q3 + q0q2);
hy = 2*mx*(q1q2 + q0q3) + 2*my*(0.5 - q1q1 - q3q3) + 2*mz*(q2q3 - q0q1);
hz = 2*mx*(q1q3 - q0q2) + 2*my*(q2q3 + q0q1) + 2*mz*(0.5 - q1q1 - q2q2);
bx = sqrt((hx*hx) + (hy*hy));
bz = hz;

vx = 2*(q1q3 - q0q2);
vy = 2*(q0q1 + q2q3);
vz = q0q0 - q1q1 - q2q2 + q3q3;

wx = 2*bx*(0.5 - q2q2 - q3q3) + 2*bz*(q1q3 - q0q2);
wy = 2*bx*(q1q2 - q0q3) + 2*bz*(q0q1 + q2q3);
wz = 2*bx*(q0q2 + q1q3) + 2*bz*(0.5 - q1q1 - q2q2);

ex = (ay*vz - az*vy) + (my*wz - mz*wy);
ey = (az*vx - ax*vz) + (mz*wx - mx*wz);
ez = (ax*vy - ay*vx) + (mx*wy - my*wx);

exInt = exInt + ex*Ki;
eyInt = eyInt + ey*Ki;
ezInt = ezInt + ez*Ki;

gx = gx + Kp*ex + exInt;
gy = gy + Kp*ey + eyInt;
gz = gz + Kp*ez + ezInt;

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;

norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
q0 = q0 / norm;
q1 = q1 / norm;
q2 = q2 / norm;
q3 = q3 / norm;
}

代碼不長，但是數學理論基礎卻是十分扎實的，必須要要有之前（一）的鋪墊。
一段段分析代碼：

#define Kp 2.0f
#define Ki 0.005f
#define halfT 0.5f
float q0 = 1, q1 = 0, q2 = 0, q3 = 0;
float exInt = 0, eyInt = 0, ezInt = 0;
前三行是一些常數的定義，包括Kp參數，Ki參數，采樣時間。
后兩行是一些變量的申明，q0，q1，q2，q3分別代表四元數的4個參數a，b，c，d，v=q0+q1i+q2j+q3k
exInt，eyInt，ezInt代表誤差的積分項。這個后面會有說明。

void AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz)
float norm;
float hx, hy, hz, bx, bz;
float vx, vy, vz, wx, wy, wz;
float ex, ey, ez;
來說說這些變量代表的意義，再注意各個參數是相對于哪個參考系的。這是算法理解的重點。
先說形參的
gx gy gz 表示陀螺儀讀出的數據，分別為繞飛行器參考系 x軸，y軸，z軸正方向旋轉的角速度。
ax ay az 表示加速度計讀出的數據，分別為重力加速度在 飛行器參考系 x軸，y軸，z軸的分量。
mx my mz 表示磁場傳感器讀出的數據，分別為地磁場在 飛行器參考系 x軸，y軸，z軸的分量。

norm是做歸一化的中間變量。
hx, hy, hz 表示將飛行器參考系上的地磁矢量轉換到地理坐標系（參考坐標系）后的矢量。
bx，bz 與上面一樣，（注意）區別是 bx在算法里為sqrt（hx*hx+hy*hy）這實際上是建立了一個誤差函數，bx越接近hx，則整個估計姿態與電子羅盤測得的姿態越重合。
vx, vy, vz為將 標準單位重力 轉換到飛行器參考系后 各個坐標軸上的分量。（重力加速度）
wx, wy, wz為將 bx與bz又重新 轉換到飛行器參考系后 各個坐標軸上的分量。（地磁場）這個地方是比較難理解的，為什么轉換過來又轉換回去，后面會說。
ex, ey, ez 為向量a和向量m，與向量v和向量w的外積的在飛行器參考系的 向量。該向量描述了a和m與v和w的偏差程度。

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 = sqrt(ax*ax + ay*ay + az*az);
ax = ax / norm;
ay = ay / norm;
az = az / norm;
norm = sqrt(mx*mx + my*my + mz*mz);
mx = mx / norm;
my = my / norm;
mz = mz / norm;
對加速度傳感器和磁場傳感器得到的數值進行歸一化，方便計算。

hx = 2*mx*(0.5 - q2q2 - q3q3) + 2*my*(q1q2 - q0q3) + 2*mz*(q1q3 + q0q2);
hy = 2*mx*(q1q2 + q0q3) + 2*my*(0.5 - q1q1 - q3q3) + 2*mz*(q2q3 - q0q1);
hz = 2*mx*(q1q3 - q0q2) + 2*my*(q2q3 + q0q1) + 2*mz*(0.5 - q1q1 - q2q2);

利用四元數變換坐標系。具體為什么這樣變換上一張圖，筆記一里有詳細過程。


上圖為從飛行器坐標系轉換到地理坐標系的余弦矩陣的四元數表達。

bx = sqrt((hx*hx) + (hy*hy));
bz = hz;
bx在當由磁場得到的飛行器參考系和由所有參數融合得到的飛行器參考系重合的時候理論上是等于hx的，此時hy應該等于0

vx = 2*(q1q3 - q0q2);
vy = 2*(q0q1 + q2q3);
vz = q0q0 - q1q1 - q2q2 + q3q3;

反向使用上圖余弦矩陣的四元數表達。及把a替換成-a，原理為四元數表達旋轉時，a為旋轉的角度，所以想旋轉回去只需要把a變成-a。
(vx, vy, vz) 為一標準重力在飛行器參考系上的矢量。

wx = 2*bx*(0.5 - q2q2 - q3q3) + 2*bz*(q1q3 - q0q2);
wy = 2*bx*(q1q2 - q0q3) + 2*bz*(q0q1 + q2q3);
wz = 2*bx*(q0q2 + q1q3) + 2*bz*(0.5 - q1q1 - q2q2);
同理，反向使用余弦矩陣的四元數表達。
這里可能大家已經發現，怎么wx wy wz是由傳感器得到的。

ex = (ay*vz - az*vy) + (my*wz - mz*wy);
ey = (az*vx - ax*vz) + (mz*wx - mx*wz);
ez = (ax*vy - ay*vx) + (mx*wy - my*wx);
e 為誤差向量 他是 向量a和v的外積加上m和w的外積。

exInt = exInt + ex*Ki;
eyInt = eyInt + ey*Ki;
ezInt = ezInt + ez*Ki;
對誤差進行積分；

gx = gx + Kp*ex + exInt;
gy = gy + Kp*ey + eyInt;
gz = gz + Kp*ez + ezInt;
用誤差的積分和誤差本身與Kp的乘積的和對角速度進行補償。

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;
利用龍格-庫格法求出四元數的值，此值由補償后的角速度求出。

norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
q0 = q0 / norm;
q1 = q1 / norm;
q2 = q2 / norm;
q3 = q3 / norm;
對四元數進行歸一化。
至此算法結束。剩余的事情就是把四元數轉化為歐拉角，即可求出我們可以理解的飛行器的姿態yaw，roll，pitch

另外稍微分析下這個算法的實現原理。
a與v的外積 結果為 sinθ |a||v|n n為垂直于a，v組成平面的單位向量，方向遵守右手定理。由于都是歸一化的，所以，這個結果的大小與θ有關，這個θ為a與v的夾角，因為a與v在halfT時間內的區別應該非常小，所以，此時θ=sinθ，這時候可以說這個外積的每個參數與θ成正比。
這個誤差是怎么來的？就是因為飛行器在這短時間旋轉了一定角度。
而從另一個角度，如果單純從陀螺儀，同樣可以得到飛行器旋轉的一個角度。這個時候就有個問題？我們選擇相信誰的？
我們誰也不相信，而是選擇按一定比例相信了3個參數，e（誤差），g（陀螺儀角速度）和eInt（誤差的積分），這樣我們就融合了這三個參數，最終得到我們的姿態。

PS：此方式在數學上在算法上沒有任何問題，但是由于地磁傳感器極易受到各種干擾（想想百度地圖中的指南針精確度），而此算法又將地磁傳感器所指示的方向過度的融入到了姿態當中，導致實際使用中數據會非常不穩定。我也是在此之后才發現。所以之后我使用了另一種融合算法，這個留到下一次再說。

總結

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

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