顧明星 劉 衛(wèi) 胡 媛 謝宗軒 趙建森 王勝正

1.上海海事大學(xué)商船學(xué)院，上海 201306 2.上海海洋大學(xué)工程學(xué)院，上海 201306

0 引言

無人駕駛技術(shù)的發(fā)展對航行器導(dǎo)航系統(tǒng)的準(zhǔn)確性和可靠性提出了更高要求[1-2]。由全球?qū)Ш叫l(wèi)星系統(tǒng)(GNSS)和捷聯(lián)式慣性導(dǎo)航系統(tǒng)(SINS)構(gòu)成的GNSS/SINS組合導(dǎo)航系統(tǒng)[3]會因衛(wèi)星信號的受干擾或丟失而失效。陀螺羅經(jīng)和計(jì)程儀輔助的組合導(dǎo)航系統(tǒng)是一種新的導(dǎo)航系統(tǒng)，該系統(tǒng)是在GNSS/SINS松組合的基礎(chǔ)上引入航行器的航向和航速信息，能抑制SINS單獨(dú)工作時誤差迅速擴(kuò)大的影響。

GNSS/SINS組合導(dǎo)航系統(tǒng)能夠優(yōu)勢互補(bǔ)，SINS能輔助GNSS捕獲和跟蹤衛(wèi)星信號，增強(qiáng)GNSS的抗干擾性；GNSS能抑制SINS的誤差積累，提高導(dǎo)航精度[3]。GNSS和SINS的組合方式分為松組合、緊組合和超緊組合[4]。其中，GNSS/SINS松組合是GNSS和SINS相互獨(dú)立地進(jìn)行定位、定速算法，然后兩者的定位結(jié)果通過一個前反饋式卡爾曼濾波器(Kalman Filter，KF)整合到一起，最終SINS輸出經(jīng)濾波器修正的導(dǎo)航結(jié)果[5]。

目前主要有2種方法提高GNSS失效情況下SINS單獨(dú)導(dǎo)航時的導(dǎo)航精度:1)預(yù)先對慣性器件的誤差進(jìn)行建模，然后在導(dǎo)航解算過程中對慣性誤差進(jìn)行補(bǔ)償[6]；2)利用與載體相關(guān)的運(yùn)動信息，輔助SINS修正其誤差[4,7-9]。文獻(xiàn)[4]直接利用飛行器的速度變化特性約束飛機(jī)的SINS/GNSS組合導(dǎo)航誤差，對于其他航行器的導(dǎo)航應(yīng)用具有局限性。文獻(xiàn)[7]提出一種融入慣性器件隨機(jī)噪聲修正和載體運(yùn)動特征約束的車載式MEMS-SINS/GNSS松組合導(dǎo)航系統(tǒng)，該系統(tǒng)利用加速度計(jì)提供的速度信息輔助修正SINS誤差，使其在GNSS失效情況下的導(dǎo)航精度優(yōu)于傳統(tǒng)導(dǎo)航系統(tǒng)。文獻(xiàn)[8-9]提出利用計(jì)程儀輔助GNSS/SINS組合導(dǎo)航系統(tǒng)，該系統(tǒng)在獲取初始位置信息后能不依賴GNSS信號而完成一段時間的高精度導(dǎo)航。

在多傳感器的數(shù)據(jù)融合方面[10-12]，根據(jù)系統(tǒng)特性，濾波方法包括加權(quán)最小二乘法(Weighted Least Square Principle，WLSP)、擴(kuò)展卡爾曼濾波(Extended Kalman Filter，EKF)、無跡卡爾曼濾波(Unscented Kalman Filter，UKF)、中心差分卡爾曼濾波(Central Difference Kalman Filter，CDKF)等。文獻(xiàn)[10]在GPS/INS/DVL的數(shù)據(jù)融合過程中，提出一種基于加權(quán)最小二乘法的自適應(yīng)聯(lián)邦強(qiáng)跟蹤濾波算法以保證整個系統(tǒng)的可靠性。文獻(xiàn)[11]提出一種基于模糊自適應(yīng)UKF的艦船SINS/GPS/DVL組合系統(tǒng)聯(lián)合濾波算法，SINS分別與GPS和多普勒計(jì)程儀構(gòu)成2個子濾波器，再將子濾波得到的數(shù)據(jù)輸入主濾波器，但系統(tǒng)的非線性誤差仍然無法避免。文獻(xiàn)[12]在CDKF的基礎(chǔ)上，提出了基于高斯過程的CDKF參數(shù)預(yù)測能力，減小系統(tǒng)預(yù)測誤差。

針對GNSS/SINS組合導(dǎo)航系統(tǒng)在GNSS失效情況下，SINS單獨(dú)工作時導(dǎo)航誤差會迅速擴(kuò)大的特點(diǎn)，本文提出了利用陀螺羅經(jīng)(Gyrocompass，GC)和計(jì)程儀(Vialog，VL)輔助GNSS/SINS的組合導(dǎo)航方法，可以實(shí)現(xiàn)通過增加載體的運(yùn)動信息并構(gòu)造線性系統(tǒng)來克服導(dǎo)航誤差迅速擴(kuò)大的缺點(diǎn)。該方法采用GNSS/SINS松組合方式，將GC和VL分別提供的航向和航速信息導(dǎo)入由SINS誤差模型構(gòu)造的KF中。仿真實(shí)驗(yàn)表明，在GNSS失效情況下，有GC/VL輔助的GNSS/SINS組合導(dǎo)航系統(tǒng)導(dǎo)航誤差明顯小于無輔助的導(dǎo)航系統(tǒng)。

1 GNSS/SINS組合導(dǎo)航系統(tǒng)設(shè)計(jì)方案

1.1 捷聯(lián)式慣性導(dǎo)航系統(tǒng)更新算法

SINS的定位原理是利用慣性器件測得的角速度和加速度，推算出當(dāng)前載體的位置、速度和姿態(tài)[13]，更新方程為

(1)

(2)

(3)

1.2 捷聯(lián)式慣性導(dǎo)航系統(tǒng)誤差分析

1.2.1 慣性傳感器測量誤差

慣性導(dǎo)航系統(tǒng)中的慣性傳感器包括3個陀螺儀和3個加速度計(jì)[13]。

陀螺組件的測量誤差模型為

(4)

加速度計(jì)組件的測量誤差模型為

(5)

1.2.2 姿態(tài)誤差方程

捷聯(lián)慣導(dǎo)姿態(tài)誤差微分方程為

(6)

(7)

(8)

(8.1)

(8.2)

(8.3)

則最終姿態(tài)誤差方程(6)變?yōu)?/p>

(9)

1.2.3 速度誤差方程

導(dǎo)航計(jì)算機(jī)解算出的速度和理想速度存在偏差，該偏差稱為速度誤差。捷聯(lián)慣導(dǎo)速度誤差微分方程為

(10)

M3為重力誤差方程δgn=M3δp中的系數(shù)，則最終速度誤差方程(10)變?yōu)?/p>

(11)

1.2.4 位置誤差方程

位置誤差方程為

(12)

則最終位置誤差方程為

(13)

(13.1)

(13.2)

1.3 卡爾曼濾波算法

卡爾曼濾波算法包括狀態(tài)方程和量測方程[14]。

根據(jù)SINS的線性誤差模型，構(gòu)造狀態(tài)方程如下

(14)

(15)

GNSS正常運(yùn)行時能為載體提供位置信息和速度信息，并實(shí)時校正SINS的誤差。GNSS/SINS系統(tǒng)的量測方程為

(16)

(17)

卡爾曼濾波算法具體流程如下：

狀態(tài)一步預(yù)測誤差

(18)

狀態(tài)一步預(yù)測均方誤差陣

Pk/k-1=(I+Fδt)Pk-1(I+Fδt)T+GQGTδt2

(19)

式(19)中：Q=D(Wb)。

濾波增益

Kk=Pk/k-1HT(HPk/k-1HT+R)-1

(20)

式(20)中：R=D(V)。

狀態(tài)估計(jì)

(21)

狀態(tài)估計(jì)均方誤差陣

Pk=(I-KkH)Pk/k-1

(22)

2 GC/VL輔助系統(tǒng)

在GNSS失效情況下，SINS/GC/VL作為備用導(dǎo)航系統(tǒng)開始運(yùn)行，并且GC/VL能夠輔助修正SINS的慣性器件長時間累積的誤差。

2.1 陀螺羅經(jīng)輔助模型

陀螺羅經(jīng)是根據(jù)陀螺儀的定軸性和進(jìn)動性，并結(jié)合地球的自轉(zhuǎn)與重力特性，為載體提供航向信息的儀器[15]。

(23)

(24)

式(23)中：Z1是量測向量，H1是量測矩陣，X是狀態(tài)向量，V1是量測噪聲向量。

2.2 計(jì)程儀輔助模型

計(jì)程儀是一種具有測速和計(jì)算航程功能的導(dǎo)航儀器[16]。計(jì)程儀可精確測量載體相對于地面或海底的絕對速度水平分量，其測量誤差不隨時間增加而累積，可用來實(shí)時補(bǔ)償SINS的誤差[17]。

(25)

(26)

式(25)中，Z2是量測向量，H2是量測矩陣，X是狀態(tài)向量，V2是量測噪聲向量。

2.3 GNSS/SINS/GC/VL組合導(dǎo)航系統(tǒng)設(shè)計(jì)

有陀螺羅經(jīng)和計(jì)程儀輔助的GNSS/SINS松組合導(dǎo)航系統(tǒng)的結(jié)構(gòu)框圖如圖1所示。

圖1 GNSS/SINS/GC/VL松組合導(dǎo)航系統(tǒng)框圖

在GNSS正常情況下，GNSS/SINS/GC組合導(dǎo)航系統(tǒng)正常運(yùn)行。SINS輸出航向、速度、位置信息，GNSS輸出速度、位置信息，GC輸出航向信息，這些數(shù)據(jù)分別作差并經(jīng)KF1濾波得到慣導(dǎo)誤差，反饋修正SINS，系統(tǒng)最終輸出修正后的導(dǎo)航結(jié)果。

結(jié)合式(16)和(23)，GNSS/SINS/GC組合導(dǎo)航系統(tǒng)的量測方程為

(27)

(28)

式(27)中：Z是量測向量，H是量測矩陣，X是狀態(tài)向量，V是量測噪聲向量。

在SINS失效情況下，啟用備用SINS/GC/VL組合導(dǎo)航系統(tǒng)。SINS輸出航向、速度信息，GC輸出航向信息，VL輸出航速信息，這些數(shù)據(jù)分別作差并經(jīng)KF2濾波得到航向誤差和速度誤差，并反饋修正SINS，系統(tǒng)最終輸出修正后的導(dǎo)航結(jié)果。

結(jié)合式(23)和(25)，SINS/GC/VL組合導(dǎo)航系統(tǒng)的量測方程為

(29)

(30)

3 仿真實(shí)驗(yàn)

3.1 軌跡模擬與慣性器件數(shù)據(jù)的生成

利用軌跡發(fā)生器設(shè)計(jì)航行器航行軌跡，通過捷聯(lián)慣導(dǎo)反演算，反推出航行器經(jīng)該航跡產(chǎn)生的陀螺儀和加速度計(jì)數(shù)據(jù)，如圖2～3所示。

圖2 角速度值

圖3 加速度值

3.2 仿真校驗(yàn)

考慮到SINS存在實(shí)際誤差，設(shè)置SINS的標(biāo)定誤差參數(shù)，并進(jìn)行仿真實(shí)驗(yàn)。SINS中，陀螺零偏為5(°)/h，加速度計(jì)零偏為200μg，角度隨機(jī)游走系數(shù)為0.05(°)/h1/2，速度隨機(jī)游走系數(shù)為5μg/Hz1/2。

為了探究有GC/VL輔助的GNSS/SINS在GNSS異常情況下的導(dǎo)航性能，分別進(jìn)行三組實(shí)驗(yàn)并對比實(shí)驗(yàn)結(jié)果。

實(shí)驗(yàn)1：SINS單獨(dú)導(dǎo)航，并記錄位置、速度和姿態(tài)等導(dǎo)航結(jié)果；

實(shí)驗(yàn)2：SINS/GC/VL組合導(dǎo)航，并記錄導(dǎo)航結(jié)果；

實(shí)驗(yàn)3：GNSS/SINS/GC/VL組合導(dǎo)航，并記錄導(dǎo)航結(jié)果。

3種導(dǎo)航系統(tǒng)分別經(jīng)過設(shè)計(jì)的軌跡，以實(shí)驗(yàn)3的導(dǎo)航結(jié)果為參考，實(shí)驗(yàn)1、2相對實(shí)驗(yàn)3的誤差如圖5～7所示。圖4為3種導(dǎo)航系統(tǒng)的位置結(jié)果，圖5為位置相對誤差，圖6為速度相對誤差，圖7為姿態(tài)相對誤差。

圖4 位置對比側(cè)視圖

圖5 導(dǎo)航位置相對誤差

圖6 導(dǎo)航速度相對誤差

圖7 導(dǎo)航姿態(tài)相對誤差

在圖4中，以GNSS/SINS/GC/VL的位置結(jié)果作為參考(“○”標(biāo)記)，SINS的位置結(jié)果(“+”標(biāo)記)迅速偏移，SINS/GC/VL組合導(dǎo)航系統(tǒng)的位置結(jié)果(“△”標(biāo)記)優(yōu)于單獨(dú)的SINS。

在圖5和圖6中，由于VL所提供的航速信息為水平方向上航行器的運(yùn)動狀態(tài)，SINS/GC/VL組合導(dǎo)航系統(tǒng)的速度和位置的水平誤差明顯小于SINS單獨(dú)導(dǎo)航的，但在垂直方向上，兩種導(dǎo)航系統(tǒng)的結(jié)果差異不大。

在圖7中，由于GC所提供的航向信息被引入到導(dǎo)航系統(tǒng)，SINS/GC/VL組合導(dǎo)航系統(tǒng)的航向角誤差明顯小于SINS單獨(dú)導(dǎo)航的，但俯仰角與橫滾角的誤差未得到顯著改善。

4 結(jié)論

為解決GNSS/SINS松組合導(dǎo)航系統(tǒng)在GNSS失效時，導(dǎo)航結(jié)果失真的現(xiàn)象，本文提出一種有陀螺羅經(jīng)和計(jì)程儀輔助的GNSS/SINS組合導(dǎo)航系統(tǒng)，根據(jù)引入的航向信息和航速信息構(gòu)造兩組濾波器，實(shí)現(xiàn)GNSS/SINS組合導(dǎo)航系統(tǒng)與陀螺羅經(jīng)、計(jì)程儀的有效結(jié)合。根據(jù)SINS的誤差模型，對GNSS/SINS/GC/VL組合導(dǎo)航系統(tǒng)進(jìn)行仿真，結(jié)果表明，GNSS/SINS/GC/VL在GNSS暫時失效時，SINS/GC/VL的導(dǎo)航誤差比單獨(dú)SINS的導(dǎo)航誤差小。此導(dǎo)航方法不僅減輕組合導(dǎo)航系統(tǒng)對GNSS的實(shí)時依賴性，還提高了航行器運(yùn)動信息的利用率，是一種簡單有效的導(dǎo)航信息融合方法。