基于MEMS AHRS/GNSS的全組合導(dǎo)航

◆毛紅瑛 陳至坤

（華北理工大學(xué) 電氣工程學(xué)院 河北 063210）

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

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

1 姿態(tài)解算系統(tǒng)

1.1 姿態(tài)初校準(zhǔn)

當(dāng)物體處于水平狀態(tài)時，令俯仰角和橫滾角等于零，此時將地球磁場在空間坐標(biāo)軸上的三個分量依次記為故通過補(bǔ)償?shù)卮牌?，得到偏航角φ?/p>

1.2 姿態(tài)融合算法

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

對于加速度計來說，在n系中加速度計的輸出為經(jīng)過轉(zhuǎn)換之后得到機(jī)體坐標(biāo)系下的值

其中，

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

2 組合導(dǎo)航系統(tǒng)

組合導(dǎo)航系統(tǒng)的狀態(tài)向量包括位置、速度、平臺失準(zhǔn)角誤差分量以及加速度計零偏、陀螺漂移誤差分量，列寫系統(tǒng)狀態(tài)方程的15 維方陣如下：

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

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

3 實(shí)驗(yàn)結(jié)果與討論

3.1 平臺搭建

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

3.2 初始對準(zhǔn)結(jié)果分析和姿態(tài)解算

預(yù)熱結(jié)束后，將傳感器的輸出數(shù)據(jù)進(jìn)行采集，然后利用第1.1 節(jié)的姿態(tài)解算公式進(jìn)行初始校準(zhǔn)。實(shí)驗(yàn)過程是模塊以任意姿態(tài)靜止放置在桌面上，并保持這種狀態(tài)大概1min，讓周圍遠(yuǎn)離其他磁場環(huán)境的干擾，然后對三軸加速度和磁場強(qiáng)度數(shù)據(jù)進(jìn)行采集。

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

表2 誤差對比/（°）

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

3.3 姿態(tài)解算實(shí)驗(yàn)結(jié)果

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

圖1 姿態(tài)解算結(jié)果

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

3.4 組合導(dǎo)航實(shí)驗(yàn)結(jié)果

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

圖2 姿態(tài)解算結(jié)果

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

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

圖3 運(yùn)動軌跡圖

4 結(jié)論

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