導(dǎo)航及無人機(jī)姿態(tài)檢測(cè)融合算法很難？用sensor fusion來搞定！

1.什么是姿態(tài)融合算法

簡(jiǎn)單來說，姿態(tài)融合算法就是融合多種運(yùn)動(dòng)傳感器數(shù)據(jù)（一般需要3軸加速度， 3軸陀螺儀或者3軸地磁感應(yīng)傳感器），通過數(shù)字濾波算法容錯(cuò)補(bǔ)償，實(shí)現(xiàn)當(dāng)前姿態(tài)檢測(cè)。

2.數(shù)字濾波算法的選擇

根據(jù)運(yùn)動(dòng)傳感器噪聲模型，一般以下濾波算法可供融合算法選擇：

a)互補(bǔ)濾波算法

b)擴(kuò)展卡爾曼濾波算法

c)無跡卡爾曼濾波算法

d)粒子濾波算法

e)Mahony互補(bǔ)濾波算法

3.ST姿態(tài)檢測(cè)融合算法

ST能提供完整的，高性能姿態(tài)檢測(cè)融合算法--MotionFX/iNemoEngine。其能最大限度規(guī)避因加速度和地磁感應(yīng)傳感器數(shù)據(jù)異常，或者陀螺儀零飄帶來的誤差。多種數(shù)據(jù)輸出選擇，包括四元素，歐拉角，線性加速度以及重力矢量。

還有9軸和6軸兩種選擇：9軸的話需要3軸加速度，3軸陀螺儀和3軸地磁感應(yīng)，能夠提供當(dāng)前三維空間的絕對(duì)方向信息，可以用作導(dǎo)航及無人機(jī)姿態(tài)檢測(cè)等應(yīng)用；6軸需要3軸加速度和3軸陀螺儀數(shù)據(jù)，可用于游戲，AR/VR等應(yīng)用。

應(yīng)用案例——GD32F103+MPU9150四旋翼飛行器第一步：姿態(tài)融合算法

前言：

相比直升機(jī)來說，四旋翼乃至多旋翼飛行器的機(jī)械結(jié)構(gòu)簡(jiǎn)單，操控靈活，飛行穩(wěn)定，體積也能做的更小，當(dāng)然也能更大，它將直升機(jī)復(fù)雜的機(jī)械結(jié)構(gòu)設(shè)計(jì)難度轉(zhuǎn)化到了電子電路和算法上面，因此四旋翼飛行器的設(shè)計(jì)更容易上手，更民眾化。

四旋翼飛行器的軟件核心包括兩大部分：姿態(tài)融合算法和控制算法；硬件核心便是MCU和傳感器。

先介紹下姿態(tài)融合算法，姿態(tài)融合說白了就是將3軸加速度、3軸角速度和3軸磁場(chǎng)強(qiáng)度融合成四元數(shù)，再將四元數(shù)轉(zhuǎn)化為歐拉角，最后將歐拉角最為控制量輸送到所有電機(jī)以達(dá)控制飛行器姿態(tài)的目的。歐拉角包括偏航角Yaw、俯仰角Pitch和滾動(dòng)角Roll。我用的算法是Madgwick寫的AHRSUpdate和IMUUpdate，簡(jiǎn)單有效，其中AHRSUpdate是融合了陀螺儀、加速度計(jì)和磁力計(jì)，而IMUUpdate只融合了陀螺儀和加速度計(jì)，就優(yōu)缺點(diǎn)來說，IMUupdate算法只融合了加速度計(jì)和陀螺儀的數(shù)據(jù)，還需要使用互補(bǔ)濾波算法來融合磁力計(jì)以修正偏航角Yaw，不然飛行器會(huì)找不到北，但是這種互補(bǔ)濾波有個(gè)小問題，就是假如定義偏航角的范圍是0-360度，那么當(dāng)機(jī)頭大概從北偏西1度轉(zhuǎn)到北偏東364度時(shí)，機(jī)頭會(huì)經(jīng)過0度(360度)這個(gè)點(diǎn)，那么這時(shí)，yaw不會(huì)直接1-0-364這么變化，而是會(huì)被逆向積分從1-20-180-270-364這么轉(zhuǎn)一圈，這是個(gè)不好的現(xiàn)象，實(shí)驗(yàn)了半天也沒有解決，而AHRSupdate很好的解決了這個(gè)問題，不過由于AHRSupdate把磁力計(jì)的數(shù)據(jù)融合進(jìn)了所有歐拉角，因此當(dāng)傳感器受到外圍強(qiáng)磁場(chǎng)干擾時(shí)，就會(huì)造成全方位失控，導(dǎo)致墜機(jī)，而使用IMUupdate算法，頂多飛行器會(huì)轉(zhuǎn)圈而已。

傳感器我用的是invensense公司的MPU9150，MPU9150芯片集成了加速度計(jì)、陀螺儀和磁力計(jì)，并且內(nèi)置硬件DMP用于姿態(tài)融合，不過不好用；MCU則用是Gigadevice公司的GD32F103系列，由于我也是剛接觸四旋翼飛行器，第一個(gè)目的當(dāng)然是能夠平穩(wěn)的飛起來，暫不考慮加入其他外圍設(shè)備。后續(xù)可能會(huì)考慮使用GD32F107或者GD32F2xx系列，可擴(kuò)展攝像頭小玩一把航拍，當(dāng)然更好的是GD32F4xx系列（期盼中），自帶浮點(diǎn)運(yùn)算單元，由于我軟件太菜，算法中出現(xiàn)大量的浮點(diǎn)運(yùn)算導(dǎo)致姿態(tài)更新頻率和控制頻率達(dá)不到很高。

做四軸飛行器也是為了好玩，目前我只完成了第一步：姿態(tài)融合。接下來才是更重要的，選擇合適的機(jī)架、電調(diào)、電機(jī)、螺旋槳，寫PID控制代碼，系統(tǒng)整合以后還要調(diào)試各種參數(shù)，抗干擾，抗震動(dòng)，最后還要加各種應(yīng)用器件。在此鼓勵(lì)一下自己，堅(jiān)持就是勝利，慢慢磨洋工。

我現(xiàn)在軟件實(shí)現(xiàn)的功能：算法用AHRSupdate、陀螺儀零偏校準(zhǔn)、加速度計(jì)平滑濾波、磁力計(jì)平面校準(zhǔn)，以后看情況可能會(huì)慢慢更新加速度計(jì)精確校準(zhǔn)、磁力計(jì)橢球擬合校準(zhǔn)、陀螺儀溫度補(bǔ)償?shù)取?/p>

第一部分：硬件

1.傳感器：MPU9150（INVENSENSE公司的，單芯片內(nèi)集成了加速度計(jì)、陀螺儀和磁力計(jì)，并且內(nèi)置DMP用于姿態(tài)融合，不過只融合了加速度計(jì)和陀螺儀，沒有融合磁力計(jì)進(jìn)去，具有自校準(zhǔn)功能，價(jià)格比MPU6050貴很多，但是省PCB面積，省事，軸向重合度高。實(shí)際上就是把MPU6050和磁力計(jì)AK8975放在同一個(gè)芯片里，程序還是使用MPU6050的驅(qū)動(dòng)，缺點(diǎn)是會(huì)偶爾丟失數(shù)據(jù)，自帶的姿態(tài)融合算法的更新頻率不高）；

2.MCU：GD32F103CB（Gigadevice公司的，ARMcortex-M3內(nèi)核，32位MCU，主頻最高108Mhz，48Pin，與ST同型號(hào)的32位MCU 直接兼容，性價(jià)比更高，外接8M晶振，也可使用內(nèi)部8M晶振，晶振遠(yuǎn)離傳感器，避免干擾磁力計(jì)，不過當(dāng)時(shí)考慮不周，這個(gè)芯片的timer太少了，以后會(huì)先采用GD32F103VCT6或者GD32F107VCT6）；

3.電源芯片：TLV70233DBVR（TI的LDO，輸入2-6V，輸出3.3V，只需要外接2個(gè)X7R無極性陶瓷電容，價(jià)格太高）

4.串口：MAX3232（方便調(diào)試）

5.USB供電，輸出電壓5V

6.目前機(jī)架、電調(diào)、電池、電機(jī)和螺旋槳已買好，來張圖，比較大眾化：

上圖：

圖1：PCB的3D效果圖，測(cè)試版，先追求調(diào)通得出姿態(tài)角，后續(xù)改版會(huì)做大的調(diào)整：

第二部分：軟件

1.使用keil，uvision4.1.0，工具鏈：RealViewMDK-ARM Version4.12；

2.驅(qū)動(dòng)：官方的MPU6050驅(qū)動(dòng)inv_mpu.c和inv_mpu_dmp_motion_driver.c；

先看幾個(gè)圖，然后再說坐標(biāo)軸的設(shè)定和算法部分。

上圖：

圖1：系統(tǒng)初始化，順序從上到下依次是：初始化MPU、設(shè)置需要使用哪些傳感器、設(shè)置陀螺儀測(cè)量范圍（我設(shè)的是正負(fù)500度/s）、設(shè)置加速度計(jì)測(cè)量范圍（我設(shè)的是正負(fù)4g）、配置fifo、設(shè)置采樣率、裝載DMP、設(shè)置陀螺儀軸向(比較重要)、使能DMP的一些玩意兒、設(shè)置DMP的FIFO、自校準(zhǔn)陀螺儀和加速度計(jì)、開啟DMP、開始姿態(tài)融合，見下圖：

圖2：由四元數(shù)求出的最終姿態(tài)角，其中Yaw為航向角，表示機(jī)頭偏離正北方多少度，范

圍-180到+180；Pitch為俯仰角，表示機(jī)頭正方向與水平線的夾角，范圍-90到+90；Roll為翻滾角，表示機(jī)翼與水平線的夾角，范圍：-180到+180。下圖為機(jī)身水平，且機(jī)頭正北偏西37度左右的數(shù)據(jù)：

圖3：下圖為機(jī)翼水平，機(jī)頭指向正北，且機(jī)頭向下25度的數(shù)據(jù)

圖4：下圖為機(jī)頭指向正北，保持水平，且機(jī)翼的右翼向下傾斜23度的數(shù)據(jù)

圖5：看下歐拉角的奇異點(diǎn)，在奇異點(diǎn)處一個(gè)轉(zhuǎn)動(dòng)狀態(tài)對(duì)應(yīng)無窮多組自由度值，當(dāng)物體轉(zhuǎn)到這些奇異點(diǎn)附近，便沒法求解。圖中當(dāng)Pitch為+90度時(shí)，機(jī)體的姿態(tài)便沒法控制，Roll的軸向發(fā)生了變化。如下圖：

第三部分：如何確定自己的軸向

首先，軸向的定義跟初始化四元數(shù)和最后結(jié)算的歐拉角有關(guān)，跟四元數(shù)更新算法無關(guān)，換句話說，不管你的軸向如何定義，姿態(tài)融合算法隨便用，但是初始化四元數(shù)的公式和最后結(jié)算歐拉角的公式要做適當(dāng)?shù)母淖?，這個(gè)后面算法中有說。加速度計(jì)也好，陀螺儀也好，磁力計(jì)也好，他們的軸向都要滿足右手定理，如下圖：

再附上一段注釋用于解釋如何定義合理的軸向，以及如何正確旋轉(zhuǎn)傳感器的軸向，解釋這么多其實(shí)就是說定義好的軸向要滿足右手定理，如下圖：

下圖，旋轉(zhuǎn)前是[x y z]，旋轉(zhuǎn)后就是[-y x z]：

下圖是如何確定旋轉(zhuǎn)角度的正方向，用右手握住坐標(biāo)軸，拇指指向軸向的正方向，四個(gè)指頭彎曲的方向就是旋轉(zhuǎn)角度的正方向，在初始化四元數(shù)時(shí)，計(jì)算出的歐拉角的正方向也要滿足這個(gè)條件：

我的程序使用的軸向如下圖所示，未作任何改變：

第四部分：算法

第一步是校準(zhǔn)，加速度計(jì)和陀螺儀我用的是MPU9150內(nèi)部自校準(zhǔn)，磁力計(jì)的校準(zhǔn)采用如下方法：見附件-磁力計(jì)校準(zhǔn)

第二步是初始化四元數(shù)，常見的軸向定義是繞x軸旋轉(zhuǎn)是Roll，繞y軸旋轉(zhuǎn)是Pitch，繞z軸旋轉(zhuǎn)是Yaw，我的程序中也有這樣的定義，不過被我注釋掉了，這里舉個(gè)另外一種軸向定義來對(duì)初始化四元數(shù)進(jìn)行說明，方便比較，也是我目前正在用的軸定義。

下面我們來定義繞x軸旋轉(zhuǎn)是Pitch，繞y軸旋轉(zhuǎn)是Roll，繞z軸旋轉(zhuǎn)是Yaw，軸向的正方向如上圖一樣，不變。

先對(duì)加速度計(jì)和磁力計(jì)的數(shù)據(jù)進(jìn)行處理，得到init_xx來供我們使用如下圖：

然后通過公式計(jì)算出初始化的Roll、Pitch、Yaw，注意加負(fù)號(hào)保證旋轉(zhuǎn)角度的正方向，如下圖：

其中Yaw的正方向未必對(duì)，可以自己去驗(yàn)證下，具體參考公式見附件-ST電子羅盤計(jì)算Yaw

然后由上面的歐拉角求出初始化四元數(shù)，這時(shí)要注意旋轉(zhuǎn)順序的不同，公式也不同，大部分旋轉(zhuǎn)順序是Z-Y-X，我的程序里也用的這個(gè)順序，在這里我們按Z-X-Y的順序來旋轉(zhuǎn)，并得出求四元數(shù)的公式以做比較，其旋轉(zhuǎn)矩陣:

q=qyaw*qpitch*qroll=

(cos(0.5*Yaw)+ksin(0.5*Yaw)) *(cos(0.5*Pitch)+isin(0.5* Pitch)) * (cos(0.5*Roll)+jsin(0.5* Roll))

得出初始化四元數(shù)計(jì)算公式如下圖所示：

其中i，j，k之間相乘的順序不能隨意變，在前的先計(jì)算，在后的后計(jì)算相乘的公式如下圖：

至此初始化四元數(shù)完成。

第三步就是使用AHRSUpdate算法了，用完以后再根據(jù)公式計(jì)算出歐拉角，此公式跟旋轉(zhuǎn)順序和旋轉(zhuǎn)使用的軸向有關(guān)，我們的旋轉(zhuǎn)順序是Z-X-Y，且繞Z是Yaw，繞X是Pitch，繞Y是Roll，推到過程如下圖：

首先得出3個(gè)方向余旋矩陣：

下圖繞Z軸Yaw：

下圖繞X軸pitch：

下圖繞Y軸Roll：

然后按照我們的Z-X-Y順序求得C=Croll * Cpitch * Cyaw，如下圖：

將上圖的方向余旋矩陣C與下圖的四元數(shù)姿態(tài)矩陣做對(duì)比，即可求出歐拉角，注意上圖的方向余旋矩陣C是隨著我們對(duì)坐標(biāo)軸的定義變化而變化的，而下圖的四元數(shù)姿態(tài)矩陣是固定的：

最后一步就是求出歐拉角，公式如下圖：

以下是靜止時(shí)，剛上電時(shí)的數(shù)據(jù)：

以下是上電半個(gè)小時(shí)以后的數(shù)據(jù)：

可以看出正常的靜止?fàn)顟B(tài)下，數(shù)據(jù)的波動(dòng)范圍是不超過1度的，也不會(huì)有漂移。具體效果還需要上機(jī)架飛起來以后再做調(diào)整。

最后總結(jié)一下：

我的代碼中，0°

注意跑108Mhz時(shí)，需要修改延遲函數(shù)和串口函數(shù)，適當(dāng)增加一下延遲函數(shù)的數(shù)值避免I2C通信失敗，修改串口函數(shù)是為了避免108Mhz下串口亂碼問題