基于MEMS AHRS/GNSS的全組合導航

◆毛紅瑛 陳至坤

（華北理工大學 電氣工程學院 河北 063210）

多傳感器信息融合技術在20世紀80年代被廣泛應用在無人機導航系統、控制系統、機器人的運動控制，游戲機、虛擬現實以及各種可穿戴設備中[1]。全球導航衛星系統（GNSS）技術設計之初被用來提供位置、航跡等信息，該技術可以實現全天24 小時實時監測、幾乎覆蓋全球所有地域[2]。隨著國內外學者對慣性器件的研究越來越深入，發現陀螺儀短時精度高、動態性能較好，受震動的影響較小，但是它長時間使用會出現累計誤差；而加速度計電子磁羅盤在性能上可以很好與陀螺儀互補，雖然其輸出可以得到姿態角，也不存在誤差累積，但對環境要求較高，難以實現飛行器姿態的準確度量。綜合上述對三種傳感器優缺點的分析，筆者提出了更為完善的姿態解算方法，即將陀螺儀、加速度計、電子磁羅盤三者結合組成航姿參考系統（AHRS），利用傳感器間的互補融合[3]，達到提高解算精度的目的。

關于組合導航技術，國外發展的較早，未來的發展主要集中在進一步提升GNSS 的抗干擾能力、以及與多種輔助傳感器（如里程計、攝像機和光流計等）的結合這兩方面[4]。本文采用擴展卡爾曼濾波算法利用加速度計、陀螺儀和電子磁羅盤求解姿態角，然后再采用松組合方式結合GNSS 來求解導航信息，提供了一種既適用于短時間也適用于長時間內的全姿態參數。GNSS 可以防止AHRS 在導航時產生的較大偏差，AHRS 可以緩解GNSS 信號中斷時所造成的嚴重后果，因此將兩者互補使結果最優。將優缺點總結如表1所示：

1 姿態解算系統

1.1 姿態初校準

當物體處于水平狀態時，令俯仰角和橫滾角等于零，此時將地球磁場在空間坐標軸上的三個分量依次記為故通過補償地磁偏角，得到偏航角φ：

1.2 姿態融合算法

根據四元數微分方程[5]，將其離散模型可由一階龍格-庫塔法得到，如下所示：

對于加速度計來說，在n系中加速度計的輸出為經過轉換之后得到機體坐標系下的值

其中，

因此，將加速度計和子磁羅盤的測量值記作量測方程的量測量，建立量測方程：

2 組合導航系統

組合導航系統的狀態向量包括位置、速度、平臺失準角誤差分量以及加速度計零偏、陀螺漂移誤差分量，列寫系統狀態方程的15 維方陣如下：

組合導航系統的量測方程分為位置誤差和速度誤差，其中位置誤差量為慣性導航計算得到的經度、緯度、高度信息與GNSS 接收機給出的相應信息的差值，速度誤差同理，因此量測方程列寫如公式11。

在卡爾曼濾波算法中，采集的數據都是離散的，因此在解決問題時需要利用泰勒公式進行線性化處理，然后再根據卡爾曼濾波算法的五大核心公式[6]來完成對輸出的最優估計。

3 實驗結果與討論

3.1 平臺搭建

關于硬件的選取，需綜合考慮成本、精度等要求，來選取滿足條件的最佳傳感器件。硬件平臺的搭建是以STM32F051 的微處理器作為控制和計算單元，實驗過程的數據采集是在搭載有陀螺儀和加速度計的ICM42605 芯片上獲取載體速度、加速度信息，AK8963 型號的電子磁羅盤芯片獲得載體的磁場強度信息，WT-NEO6M 型號的GPS芯片獲取速度和位置信息，并把該傳感器集成模塊與計算機利用USB-TTL 的方式連接起來。實驗開始前給整個模塊供電，讓系統正常工作5min 進行預熱過程。

3.2 初始對準結果分析和姿態解算

預熱結束后，將傳感器的輸出數據進行采集，然后利用第1.1 節的姿態解算公式進行初始校準。實驗過程是模塊以任意姿態靜止放置在桌面上，并保持這種狀態大概1min，讓周圍遠離其他磁場環境的干擾，然后對三軸加速度和磁場強度數據進行采集。

首先給模塊上電，對傳感器的輸出數據采集分析，實驗將采集頻率設定在100Hz，時間為1min，初始化過程按照偏航角所在四個象限進行劃分，得到誤差對比如表2所示。

表2 誤差對比/（°）

由表2 可以看出，當載體處于靜止狀態，通過加速度計和電子磁羅盤計算得到的姿態角與參考角的對比可知數據較為穩定，平均誤差較小，均未超過0.7°。通過分析可知，此時的誤差基本上是由慣性器件本身引起的，如果想要減小靜態情況下的誤差，需要選用精度更高的傳感器。因此上述實驗證明初始化過程良好，滿足無人機對初始姿態的測量條件。

3.3 姿態解算實驗結果

初對準結束后，接下來就要驗證基于加速度計、陀螺儀和電子磁羅盤的擴展卡爾曼濾波算法在實際中測量姿態角的效果，手持模塊緩慢的進行較大幅度的轉動，通過人為繞著XYZ 軸任意轉動去控制模塊運動的姿態，并且保持模塊所在的位置不發生變化，整個過程持續3min 左右，實驗的采樣頻率設置為100Hz，得到姿態解算結果如下圖1所示：

圖1 姿態解算結果

由該實驗得到的結算結果可知，此時γ平均誤差為1.6063°，θ平均誤差為1.5060°，φ平均誤差為1.5106°，由于手持緩慢轉動，故此時的加速度計測量噪聲較小，通過擴展卡爾曼濾波算法得到的曲線足夠平滑，與參考角度基本吻合，因此該算法性能較好，能夠給出正確的姿態角度信息。

3.4 組合導航實驗結果

手持模塊先沿著Y 軸方向作直線運動，再繞Z 軸旋轉大致90°沿著X 軸方向做直線運動，因此整個過程中除偏航角外的姿態變化并不是很大。但是由于手持運動，會產生較為明顯的抖動。實驗的采樣頻率設置為100Hz，經度118.59°、緯度39.19°，加速度計零偏誤差為0.0001g，陀螺儀的常值漂移為，GPS 的位置誤差為10米，速度誤差為0.2m/s，運動時間為4min 左右，其中GPS 信號無遮擋為前3min 左右，然后數據讀入寫好的程序中。得到AHRS 姿態解算結果如下圖2所示：

圖2 姿態解算結果

由圖2 可知，藍色的實線表示通過擴展卡爾曼濾波算法的融合后得到的姿態角，紅色實線代表參考角度，實驗表明，經過擴展卡爾曼濾波算法以后的姿態角與參考角度接近，由于實驗過程為手持沿著直角做直線運動，因此擾動較大，得到的曲線不夠平滑，特別是在120s~130s 的拐彎處時，橫滾角產生的最大誤差為7°。整個實驗橫滾角的平均誤差為1.1883°，俯仰角的平均誤差為1.4075°，偏航角的平均誤差為4.0337°，所設計的航姿參考系統誤差結果符合預期，因此該算法可以得到較好的解算結果，使得最終的誤差結果較小，滿足低成本姿態解算需求。

從實驗軌跡圖3 可知，藍色曲線代表組合導航的解算結果，紅色曲線表示參考軌跡，通過經緯度誤差對比可知，在220s 以前，GPS未被遮擋，此時組合導航效果良好，最大誤差是5 米，組合導航能夠較好跟蹤GPS，而在220s 以后，GPS 被遮擋，此時只能通過慣性導航來定位，由于誤差隨著時間的不斷累積，最終誤差達到了20 米，由實驗可知，當GPS 被遮擋失去信號，在10s 以內的累積誤差最大為6 米，該結果在可接受的范圍內。

圖3 運動軌跡圖

4 結論

由于慣性導航在長時間工作中容易產生累積誤差，于是將其與電子磁羅盤、GPS 組合，通過融合算法得到最優估計，首先在靜止條件下對系統進行初校準，然后通過陀螺儀得到相關狀態向量，通過加速度計和磁力計得到相關量測向量，進行擴展卡爾曼濾波算法迭代求最優四元數，進而得到最終的姿態角，最后使用GPS 的松組合算法來輔助修正航姿參考系統，實驗從低動態環境和高動態環境兩方面展開，結果表明該算法能夠較好反應數據的真實情況，并且在GPS 失鎖的短時間內能夠起到一定的導航定位作用，達到了預期效果。