基于ARM的SINS/GNSS緊組合導(dǎo)航系統(tǒng)設(shè)計

王樹森， 張 鵬， 李孟委,2

(1. 中北大學(xué) 儀器與電子學(xué)院， 山西 太原 030051； 2. 中北大學(xué) 電子測試技術(shù)重點實驗室， 山西 太原 030051)

0 引 言

目前應(yīng)用最為廣泛的導(dǎo)航系統(tǒng)是捷聯(lián)慣性導(dǎo)航系統(tǒng)(Strapdown Inertial Navigation System, SINS)與全球衛(wèi)星導(dǎo)航系統(tǒng)(Global Navigation Satellite System, GNSS). SINS具有可靠性高、 隱蔽性好、 體積小等優(yōu)勢[1-2]， 但是其定位誤差會隨時間而積累； GNSS具有全球性、 高精度、 全天候、 實時導(dǎo)航等優(yōu)點， 但衛(wèi)星信號容易被遮擋， 動態(tài)性能和可靠性較差， 不能輸出載體的姿態(tài)信息[3]. SINS/GNSS組合系統(tǒng)可以利用SINS與GNSS的優(yōu)勢來彌補各自在導(dǎo)航性能上存在的缺陷， 不僅能夠輸出更加全面的參數(shù)信息， 而且可以提高系統(tǒng)精度與魯棒性[4].

SINS/GNSS組合系統(tǒng)采用的松組合方式原理與結(jié)構(gòu)相對簡單， 算法與工程上也相對容易實現(xiàn)[5]. 然而， 當(dāng)GNSS信號受到干擾失鎖， 可見衛(wèi)星數(shù)量少于4顆時， 沒有GNSS系統(tǒng)穩(wěn)定的輸出對SINS系統(tǒng)輸出進行修正， 松組合導(dǎo)航只能依靠純捷聯(lián)慣性導(dǎo)航解算， 導(dǎo)航精度難以滿足實際應(yīng)用需要. SINS/GNSS緊組合導(dǎo)航采用偽距和偽距率組合方式， GNSS衛(wèi)星負責(zé)輸出所得原始偽距、 偽距率信息和星歷數(shù)據(jù)， 不用進行GNSS導(dǎo)航解算， 系統(tǒng)的魯棒性好， 精度更高， 且當(dāng)GNSS信號受到干擾失鎖， 可見衛(wèi)星數(shù)少于4顆時， 仍能夠使用僅有的GNSS衛(wèi)星數(shù)據(jù)輔助SINS進行數(shù)據(jù)融合完成導(dǎo)航定位. 超緊組合的導(dǎo)航精度雖然更高， 但GNSS衛(wèi)星接收機需要矢量跟蹤與反饋環(huán)路， 工程不容易實現(xiàn)[6]. 由于系統(tǒng)的狀態(tài)與量測方程均為非線性的， 其線性化過程中的線性化誤差會導(dǎo)致濾波精度降低， 而無跡卡爾曼濾波(Unscented Kalman Filter， UKF)算法能夠避免非線性系統(tǒng)線性化的過程， 具有魯棒性好、 精度高等優(yōu)點[7]. 因此， 本文選用UKF算法對系統(tǒng)數(shù)據(jù)進行融合處理， 并設(shè)計了基于ARM的SINS/GNSS緊組合導(dǎo)航系統(tǒng).

1 組合導(dǎo)航系統(tǒng)總體設(shè)計

基于ARM的緊組合系統(tǒng)， 采用三星Exynos4412微處理器作為原始數(shù)據(jù)采集、 捷聯(lián)解算和數(shù)據(jù)融合的核心， 慣性測量單元(Inertial Measurement Unit， IMU)采用MPU9250， 陀螺噪聲為0.06 °/s， 加速度計噪聲為4 mg. GNSS接收機采用MH16-T1， 可以輸出1 PPS秒脈沖及偽距、 偽距率與星歷等原始觀測信息， 時間精度(1 PPS)達到±20 ns， 原始數(shù)據(jù)的偽距觀測精度可達到0.3 m， 系統(tǒng)的總體設(shè)計方案如圖 1 所示.

圖 1 系統(tǒng)總體設(shè)計方案Fig.1 Overall system design

硬件系統(tǒng)選用Exynos4412作為核心， Exynos4412微處理器利用SPI接口實時采集IMU模塊產(chǎn)生的三軸加速度、 角速度與磁力計信息， 利用UART接口將GNSS接收機產(chǎn)生的星歷數(shù)據(jù)與偽距、 偽距率信息以及1 PPS秒脈沖傳輸?shù)紼xynos4412微處理器. 對接收到的原始數(shù)據(jù)進行預(yù)處理和導(dǎo)航初始化， 根據(jù)初始的IMU信息進行初始對準(zhǔn)， 得到初始速度、 位置和姿態(tài)信息， 然后再進行捷聯(lián)解算. 當(dāng)微處理器收到PPS秒脈沖和GNSS數(shù)據(jù)時， 進行SINS偽距、 偽距率計算并將計算結(jié)果轉(zhuǎn)換至地心地固坐標(biāo)系(Earth-Centered, Earth-Fixed, ECEF)， 用UKF算法對緊組合數(shù)據(jù)進行融合處理， 修正SINS解算的誤差， 并在上位機上顯示組合導(dǎo)航信息.

由于GNSS接收機與IMU模塊是兩個獨立的模塊， 其各自的數(shù)據(jù)更新頻率不同， 為保證數(shù)據(jù)融合時的SINS和GNSS數(shù)據(jù)來自同一時刻， 每當(dāng)GNSS接收機MH16-T1輸出的1 PPS秒脈沖送入ARM微處理器時會引發(fā)中斷， 定時器發(fā)生重置. 在無秒脈沖輸入時， 定時器每隔10 ms向MPU-9250獲取IMU數(shù)據(jù)， 在收到秒脈沖后定時器會進行重置， 從而完成GNSS和SINS數(shù)據(jù)的時間同步.

2 SINS/GNSS緊組合非線性模型

2.1 緊組合導(dǎo)航算法流程

圖 2 為SINS/GNSS緊組合算法流程圖. SINS/GNSS緊組合算法由捷聯(lián)慣性導(dǎo)航SINS算法與UKF算法兩種構(gòu)成[8]. 其中， SINS采用自對準(zhǔn)算法進行粗對準(zhǔn)和精對準(zhǔn)， 采用旋轉(zhuǎn)矢量的三子樣算法進行捷聯(lián)姿態(tài)更新. SINS/GNSS緊組合采用偽距和偽距率組合方式， 其中GNSS模塊只負責(zé)輸出所測偽距和偽距率信息以及對應(yīng)星歷數(shù)據(jù). SINS模塊由IMU輸出原始數(shù)據(jù)， 經(jīng)過捷聯(lián)解算后， 再根據(jù)星歷數(shù)據(jù)進行SINS偽距和偽距率計算. 最后， 使用UKF算法對SINS模塊與GNSS接收機模塊的數(shù)據(jù)進行融合， 在誤差補償后輸出組合導(dǎo)航信息[9-10].

圖 2 GNSS/SINS緊組合算法流程圖Fig.2 GNSS/SINS tight combination algorithm flow chart

2.2 緊組合系統(tǒng)狀態(tài)方程

系統(tǒng)狀態(tài)誤差矢量為

X(t)=[φE,φN,φU,δνE,δνN,δνU,δL,δλ,δh,

εbx,εby,εbz,εrx,εry,εrz,x,y,z,δtu,δtru]T,

(1)

式中：φE,φN,φu為姿態(tài)誤差角；δνE,δνN,δνU分別為東北天向速度誤差；δL,δλ,δh分別為經(jīng)度、 緯度和高度誤差；εbx,εby,εbz為陀螺儀隨機誤差；εrx,εry,εrz為陀螺儀的一階馬爾科夫過程；x,y,z為加速度計零偏；δtu為GNSS時鐘誤差引起的距離誤差；δtru為GNSS時鐘頻率誤差引起的速度誤差.

SINS/GNSS緊組合系統(tǒng)誤差狀態(tài)方程由SINS誤差狀態(tài)方程與GNSS誤差狀態(tài)方程構(gòu)成

(2)

即

(3)

式中：

(4)

(5)

FN為9階慣導(dǎo)誤差方陣，

(6)

Tgi,Tai(i=x,y,z)分別為陀螺儀與加速度計所對應(yīng)的一階馬爾可夫相關(guān)時間常數(shù)，

(7)

2.3 緊組合系統(tǒng)量測方程

選取SINS計算的偽距和偽距率與GNSS提供的偽距和偽距率分別作差作為緊組合系統(tǒng)的觀測量.

(8)

式中：ρI為SINS計算所得載體和第j顆衛(wèi)星間偽距；ρG為GNSS所得載體和第j顆衛(wèi)星間偽距，

ρG=ρ+δtu+vρ,

(9)

式中：vρ為偽距的量測噪聲.

理想偽距ρ與ρI可由下式計算得到

(10)

(11)

2.4 無跡卡爾曼濾波器

為了避免非線性系統(tǒng)的線性化誤差， 獲取更加精確的系統(tǒng)狀態(tài)量均值與協(xié)方差估計[11]， 本文選用UKF算法對緊組合系統(tǒng)的數(shù)據(jù)進行融合處理. 通過采集的一組近似高斯分布的采樣點對系統(tǒng)狀態(tài)均值與誤差協(xié)方差進行更新. 每次進行更新時， 采樣點會隨非線性狀態(tài)方程進行傳遞并會隨量測方程進行變換， 從而避免了非線性系統(tǒng)的線性化誤差， 保證了狀態(tài)估計精度， 具有較好的魯棒性[12-14].

(13)

i=1,2,…,n,

(14)

i=n+1,n+2,…,L,

(15)

(16)

(17)

(18)

量測更新為

(19)

(20)

(21)

(22)

(23)

式中：Rv為系統(tǒng)噪聲方差；Rn為觀測噪聲方差.

3 系統(tǒng)試驗與分析

為了驗證緊組合系統(tǒng)的總體性能， 對樣機進行了車載試驗. 圖 3 為緊組合系統(tǒng)實物圖.

圖 3 緊組合系統(tǒng)實物圖 Fig.3 Product photo of tight combinatio system

車載試驗時， 將系統(tǒng)樣機固定在一塊金屬板上置于汽車內(nèi)， 將GNSS天線固定在樣機上方的汽車頂部， 并忽略GNSS天線和樣機之間的桿臂誤差. 跑車試驗于中北大學(xué)南門外新蘭路進行， 跑車行駛路線為環(huán)形， 初始對準(zhǔn)后， 行駛時間約為 450 s. 系統(tǒng)IMU模塊的數(shù)據(jù)更新頻率為100 Hz， GNSS的數(shù)據(jù)更新頻率為 1 Hz， 實驗過程中衛(wèi)星信號良好， 可見星有7顆北斗與8顆GPS衛(wèi)星， 最后對試驗結(jié)果進行處理與分析， 得到緊組合與松組合導(dǎo)航系統(tǒng)的速度誤差和位置誤差對比.

ΔvE, ΔvN, ΔvU分別為車輛在東北天向速度誤差， ΔPE, ΔPN, ΔPU分別為車輛在東北天向位置誤差. 圖 4 和圖 5 分別表示GNSS衛(wèi)星信號受到干擾失鎖下緊組合和松組合算法的速度和位置誤差的對比圖.

圖 4 衛(wèi)星失鎖下速度誤差對比圖Fig.4 Comparison of speed errors under satellite lock less

圖 5 衛(wèi)星失鎖下位置誤差對比圖Fig.5 Comparison of position errors under satellite lock loss

對緊組合和松組合算法導(dǎo)航結(jié)果進行對比， 在0～100 s內(nèi)， 可見衛(wèi)星數(shù)大于4顆， 如圖 4 和圖 5 所示， 緊組合的速度誤差和位置誤差比松組合略小， 精度略高. 在100～300 s內(nèi)， 選用3顆衛(wèi)星進行數(shù)據(jù)融合， 此時可見衛(wèi)星數(shù)小于4顆， SINS/GNSS松組合不能正常運行， 只能通過純SINS解算進行定位. 松組合在GNSS信號受到干擾失鎖200 s后， 東向、 北向和天向的速度誤差分別達到了7.01， 6.71和4.90 m/s； 由于只能通過SINS解算進行定位， 在GNSS信號受到干擾失鎖200 s后， 東向、 北向和天向的位置誤差分別達到了196.4， 311.9和144.3 m. 由于捷聯(lián)慣導(dǎo)導(dǎo)航誤差會隨時間進行累積， 因而發(fā)散速度特別快， 松組合系統(tǒng)的穩(wěn)定性特別差. 而緊組合在可見衛(wèi)星數(shù)目少于4顆時依然能夠利用3顆衛(wèi)星的數(shù)據(jù)進行數(shù)據(jù)融合， 并且速度誤差和位置誤差仍可以保持在一定的誤差范圍內(nèi).

圖 6 和圖 7 分別表示緊組合與松組合系統(tǒng)分別解算得到的東北天向速度與位置誤差對比圖.

圖 6 緊組合東北天向的速度誤差圖Fig.6 Velocity error diagram of tigh combination in east, north and sky directions

圖 7 緊組合東北天向的位置誤差圖Fig.7 Position error diagram of tigh combination in east,north and sky directions

在0～100 s內(nèi)， 可見衛(wèi)星數(shù)大于4顆， 在100～300 s內(nèi)， 選用3顆衛(wèi)星信號進行數(shù)據(jù)融合. 如圖 6 所示， 緊組合導(dǎo)航算法的速度誤差小于1 m/s， 緊組合導(dǎo)航算法的東向、 北向和天向的速度誤差的極值分別達到0.75， 0.69和0.52 m/s； 如圖 7 所示， 緊組合算法的位置誤差在5 m內(nèi)， 緊組合算法的東向、 北向和天向的位置誤差的極值分別達到3.11， 4.69和2.87 m.

試驗結(jié)果表明， 所設(shè)計的基于ARM的SINS/GNSS緊組合導(dǎo)航系統(tǒng)， 與松組合算法相比具有更高的精度和穩(wěn)定性， 在GNSS信號受到干擾失鎖， 可見衛(wèi)星數(shù)目少于4顆時仍能輔助SINS進行數(shù)據(jù)融合完成導(dǎo)航定位， 在一段時間里具有比較高的精度.

4 結(jié) 論

針對GNSS信號受到干擾失鎖， 導(dǎo)致可見衛(wèi)星數(shù)目少于4顆時SINS/GNSS松組合系統(tǒng)無法正常運行問題， 設(shè)計了基于ARM的SINS/GNSS緊組合導(dǎo)航系統(tǒng)， 建立了緊組合系統(tǒng)非線性模型， 并搭建了硬件系統(tǒng)， 采用UKF算法對緊組合數(shù)據(jù)進行融合. 測試結(jié)果表明， 設(shè)計的緊組合系統(tǒng)有著比松組合系統(tǒng)更高的導(dǎo)航精度與穩(wěn)定性， 在GNSS信號受到干擾失鎖時， 緊組合系統(tǒng)在短期內(nèi)仍可保持較高的導(dǎo)航精度.