電賽四軸小結——姿態解算篇（mahony算法）

前言：前段時間一直在備賽沒時間做總結，其實無人機控制算法中主要是姿態解算系統和姿態控制系統，這裏先總結下姿態解算系統的相關要點，研究一下關於姿態解算的過程和實現，本篇博文主要是以mahony的算法爲基礎理解姿態解算的過程。

傳感器：

陀螺儀計（gyroscope，也稱作 gyro）：

靈敏度：陀螺儀計測量的是角速度，具有高動態特性，關鍵參數就是gyro sensitivity（其單位是millivolts per degree persecond，把轉速轉換到電壓值），測量範圍越小氣靈敏度越好。也就是說測量的是角度的導數，即角速度，要將角速度對時間積分才能得到角度。陀螺儀的每個通道檢測一個軸的旋轉，這樣就可以通過與初始方向的偏差計算出旋轉方向和角度。

零點偏移：偏移就是在陀螺沒有轉動的時候卻又輸出，這個輸出量的大小和供電電壓以及溫度有關，該偏移可以在陀螺儀上電時通過一小段時間的測量來修正。

漂移：它是由於在時間的積累下偏移和噪聲相互影響的結果，例如有一個偏置（offset）0.1dps加在上面，於是測量出來是0.1dps，積分一秒之後，得到的角度是0.1度，1分鐘之後是6度，還能忍受，一小時之後是360度，轉了一圈，也就是說，陀螺儀在短時間內有很大的參考價值，但是長時間觀測會造成很大的誤差。

白噪聲：電信號的測量中，一定會帶有白噪聲，陀螺儀數據的測量也不例外。所以獲得的陀螺儀數據中也會帶有白噪聲，而且這種白噪聲會隨着積分而累加。

積分誤差：對陀螺儀角速度的積分是離散的，長時間的積分會出現漂移的情況。所以要考慮積分誤差的問題。

溢出：就是轉速超過了其測量的最大轉速範圍。關於這個問題的解決辦法，傳聞在APM的代碼中有解決方法，還沒看到暫且不談。

這是由於陀螺儀計融合出來的姿態存在這麼多的誤差，所以我們必須要使用其它傳感器輔助校正。

加速度計（accelerometer）：

加速度計的低頻特性好，其它瞬間加速度可以忽略，可以比較準確測量低速的靜態加速度。當把加速度計拿在手上隨意轉動時，看的是重力加速度在三個軸上的分量值。加速度計在自由落體時，其輸出爲0。爲什麼會這樣呢？這裏涉及到加速度計的設計原理：加速度計測量的加速度是通過比力來測量(比力方程，《慣性導航》秦永元的書中有介紹)，而不是直接測量得到加速度。加速度計僅僅測量的是重力加速度在機載座標下的三個軸上的分量，而重力加速度與剛纔所說的R座標系（EarthFrame）是固連的，通過這種關係,可以得到加速度計所在平面與地面的角度關係。

注意：加速度計若是繞着重力加速度的軸轉動，則測量值不會改變，也就是說加速度計無法感知這種水平旋轉，在飛機上來說就是加速度計是沒法測量航向的。
這裏有一篇翻譯了一篇詳細講解了陀螺儀和加計的文章：https://blog.csdn.net/lovewubo/article/details/9084291

電子羅盤（地磁計）：

測量磁場，在沒有其他磁場的情況下，僅僅測量的是地球的磁場，而地磁也是和R座標系固連的，通過這種關係，可以得到和磁場方向垂直的平面（平面A）和地平面的關係。和加速度計一樣，若是沿着磁場方向的軸旋轉，測量值不會改變，無法感知這種旋轉。加速度計在靜止時測量的是重力加速度，是有大小和方向的；同理，地磁計同樣測量的是地球磁場的大小和方向，只不過這個方向與x軸（或者y軸）呈一個角度，與z軸呈一個角度。實際上對水平方向的兩個分量來說，他們的矢量和總是指向磁北的，羅盤中的航向角（Azimuth）就是當前方向和磁北的夾角。所以當羅盤保持水平時，只需要用磁力計水平方向兩軸（通常爲X軸和Y軸）的檢測數據就可以計算出航向角。當羅盤水平旋轉的時候，航向角在0- 360之間變化。

關於電子羅盤工作原理的參考文章：https://blog.csdn.net/wxlinwzl/article/details/6903548

姿態解算：

在傳感器中陀螺儀纔是主角，加速度計和磁傳感器僅僅是起輔助校正作用的。其中加速度計無法對航向角進行修正，修正航向角需要磁力計。在常用座標系中，無人機一般採用以下兩種座標系：參考座標系是n系（地理座標系），載體座標系是b系（機載座標系）。在姿態解算過程中，姿態表示的方法有很多種，比如歐拉角、四元數、DCM（方向餘弦），各有的各的優勢，比較常用的就是四元數。

這裏以四元數解算代碼實現過程結合數學原理分析來論述一下：

對於四元數法的姿態解算，最終需求的就是四元數的值；方向餘弦矩陣（用於表示n系和b系的相對關係）中的元素本來應該是三角函數，這裏由於使用四元數法，所以矩陣中的元素就成了四元數。所以姿態解算的任務就是求解由四元數構成的方向餘弦矩陣nCb（nCb表示從b繫到n的轉換矩陣，同理，bCn表示從n繫到b的轉換矩陣，它們的關係是轉置）。

顯然，對於一個確定的向量n，用不同的座標系表示時，理論上它們所表示的大小和方向一定是相同的。但是實際上我們的轉換矩陣在這兩個座標系下並不相等，而是存在誤差，那麼當一個向量經過這麼一個有誤差存在的旋轉矩陣變換後，在另一個座標系中肯定和理論值是有偏差的，我們通過這個偏差來修正這個旋轉矩陣。這個旋轉矩陣的元素是四元數，這就是說修正的就是四元數，於是姿態就這樣被修正了，這纔是姿態解算本質原理。

姿態解算求的是四元數，是通過修正旋轉矩陣中的四元數來達到姿態解算的目的，而不要以爲通過加速度計和地磁計來修正姿態，加速度計和地磁計只是測量工具和載體，通過這兩個器件表徵旋轉矩陣的誤差存在，然後通過算法修正誤差，修正四元數以修正姿態。

算法主要思路就是利用加速度計和磁力計修正陀螺儀的誤差，使用了PI反饋控制器實現反饋修正。大致思路如下：

1、觀測加速度計數據，因爲已知地理座標系的標準重力加速度 [0 0 1]，然後變換到機體座標系，理論與實際叉乘求出誤差
2、觀測電子羅盤數據，由於標準重力加速度在所有地方都是一樣的，可以由其直接從地理座標系變換到機體座標系，但是在地理座標系中，地磁大小卻不是相同的，需要根據實際情況測出來，因此磁力計數據的來源則必然是時刻測量更新。所以首先進行機體座標系變換到地理座標系，相當於求出理論地磁，之後處理和加速度計差不多，就是變換到機體座標系中，最後利用理論和實際測量叉乘求出誤差
3、將誤差累加，利用PI補償至陀螺儀即可求解得四元數。

下面貼出具體代碼實現過程：

//陀螺儀、加速度計、磁力計數據融合出姿態四元數
    void MahonyAHRSupdate(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; //v*當前姿態計算得來的重力在機載座標下三軸上的分量
            float ex, ey, ez;
 
            // auxiliary variables to reduce number of repeated operations
            //先計算好相關乘積項爲了後面做矩陣運算做準備
            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;
           
            // normalise the measurements
            //加速度計和地磁計向量標準化
            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;
           
            // compute reference direction of magnetic field
            //這裏計算得到的是理論地磁參考座標系下的三個軸的分量
            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計算的是當前航向角和磁北的夾角，也就是北天東座標下的航向角
            //當羅盤水平旋轉的時候，航向角在0-360之間變化
            bx = sqrt((hx*hx) + (hy*hy));
            bz = hz; 
           
           // estimated direction of gravity and magnetic field (v and w) 
           //用四元數表示的方向餘弦矩陣從參考座標n系轉化到載體座標b系。
           //加速度計重力向量轉換到b系
            vx = 2*(q1q3 - q0q2);
            vy = 2*(q0q1 + q2q3);
            vz = q0q0 - q1q1 - q2q2 + q3q3;
           //地磁計n系下磁向量轉換到b系下啊，反向使用DCM得到
            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);  
           
            // error is sum of cross product between reference direction of fields and direction measured by sensors 
            //體現在加速計補償和磁力計補償，因爲僅僅依靠加速計補償沒法修正Z軸的變差，所以還需要通過磁力計來修正Z軸。
            //下面是叉積運算的過程
            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);
           
            // integral error scaled integral gain 
            exInt = exInt + ex*Ki* (1.0f / sampleFreq);
            eyInt = eyInt + ey*Ki* (1.0f / sampleFreq);
            ezInt = ezInt + ez*Ki* (1.0f / sampleFreq);
            // adjusted gyroscope measurements
            //將誤差PI後補償到陀螺儀，即補償零點漂移。通過調節Kp、Ki兩個參數，可以控制加速度計修                正陀螺儀積分姿態的速度。（公式16和公式29）
            gx = gx + Kp*ex + exInt;
            gy = gy + Kp*ey + eyInt;
            gz = gz + Kp*ez + ezInt;
           
            // integrate quaternion rate and normalize
            //一階龍格庫塔法更新四元數
            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;  
           
            // normalise quaternion
            norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
            q0 = q0 / norm;
            q1 = q1 / norm;
            q2 = q2 / norm;
            q3 = q3 / norm;
}

結合代碼我們來討論下具體到底是怎麼實現加計和羅盤修正姿態的：

1、加速度計修正pitch和roll

在n系中的重力向量 $g_{n}=[0,0,1]^{T}$ 經過bCn（用四元數表示的轉換矩陣）轉換之後到b系中的值爲 $[vx,vy,vz]^{T}$ ，現在和加速度計在b系下的輸出爲 $[ax,ay,az]^{T}$ 均表示在b系中的豎直向下的向量。由此做向量積（叉積），得到誤差，利用這個誤差來修正bCn矩陣，於是四元數就在這樣一個過程中被修正了。但是，由於加速度計無法感知z軸上的旋轉運動，所以還需要用地磁計來進一步補償。

2、地磁計修正yaw

上面我們說了地磁計測量的是地球磁場的大小和方向，只不過這個方向是指向磁北。現記x軸對準北邊，所以by=0，即bx = sqrt((hx*hx) + (hy*hy))。如果可以直接測得bx和bz的準確值，那麼就可以採用和加速度計一樣的修正方法來修正，甚至可以擺脫掉加速度計的補償，直接用地磁計和陀螺儀進行姿態解算，但是沒人會去測量當地的地磁場相對於東北天座標的夾角，也就是bx和bz，這裏可以對比重力加速度，就像vx vy vz似的，因爲在每一處的歸一化以後的重力加速度都是0 0 1然後旋轉到機體座標系，然而地球每一處的地磁大小都不一樣的，不能像重力加速度那樣直接旋轉得到了，只能用磁力計測量到的數據去強制擬合，這裏面的干擾因素太多了，得到的地磁值相對加速度計來說正確率要低，所以基本沒人只會拿磁力計和陀螺儀做數據融合。前面已經講了，姿態解算就是求解旋轉矩陣，這個矩陣的作用就是將b系和n正確的轉化直到重合，現在我們假設nCb旋轉矩陣是經過加速度計校正後的矩陣，當某個確定的向量（b系中）經過這個矩陣旋轉之後（到n系），這兩個座標系在XOY平面上重合，只剩下在z軸旋轉上會存在一個偏航角的誤差。下圖表示的是經過nCb旋轉之後的b系和n系的相對關係。可以明顯發現加速度計可以把b系通過四元數法從任意角度拉到與n系水平的位置上，此時只剩下一個偏航角誤差。

我們先把b系中的觀測值旋轉到n系中然後算得n系中的 $[bx,0,bz]^{T}$ 再轉換到b系中和原輸出做比較即可得到誤差，然後根據誤差修正旋轉即可。利用地磁計修正偏航的具體過程如下：

設地磁計在b系中的觀測爲 $[mx,my,mz]^{^{T}}$ ，經過nCb矩陣旋轉之後得到n系中的 $[hx,hy,hz]^{T}$ ，令在n系下XOY平面的投影爲bx = sqrt((hx*hx) + (hy*hy))，bz=hz，這樣就得到了n系中的 $[bx,0,bz]^{T}$ ，然後經過bCn矩陣旋轉回b系裏面即可得到 $[wx,wy,wz]^{T}$ ,然後再拿這個值和b系的原始輸出 $[mx,my,mz]^{^{T}}$ 做叉乘得到誤差，然後拿這個誤差和上面加計的誤差疊加，再做PI補償到陀螺儀數據即可修正bCn矩陣，從而得到了沒有偏差的的實時姿態。