低成本SINS/GPS組合導(dǎo)航系統(tǒng)方案

中南大學(xué)地球科學(xué)與信息物理學(xué)院，湖南長(zhǎng)沙 410083

關(guān)鍵字：慣性導(dǎo)航；組合導(dǎo)航；卡爾曼濾波

一、引言

導(dǎo)航定位已成為了交通工具不可缺少的一部分。導(dǎo)航分為自主式和非自主式，自主式導(dǎo)航包括慣性導(dǎo)航、多普勒導(dǎo)航和天文導(dǎo)航等。非自主式導(dǎo)航包括衛(wèi)星導(dǎo)航、無(wú)線電導(dǎo)航等。以GPS為代表的衛(wèi)星導(dǎo)航能夠提供全球性、全天候的導(dǎo)航定位服務(wù)，但是衛(wèi)星導(dǎo)航的信號(hào)容易受到遮擋導(dǎo)致定位失敗，且無(wú)法獲取載體的姿態(tài)信息。

捷聯(lián)慣性導(dǎo)航系統(tǒng)（Strapdown Inertial Navigation System,SINS)是慣性導(dǎo)航的一種實(shí)現(xiàn)方式，SINS不受外界干擾，可以輸出載體的位置、速度、姿態(tài)。但是SINS獨(dú)立導(dǎo)航時(shí)誤差會(huì)迅速積累。SINSGPS組合導(dǎo)航是公認(rèn)的有發(fā)展前景的一種導(dǎo)航模式。

針對(duì)低成本SINSGPS組合導(dǎo)航，國(guó)內(nèi)學(xué)者進(jìn)行了大量研究。周姜濱設(shè)計(jì)了適合于工程應(yīng)用的組合導(dǎo)航系統(tǒng)的結(jié)構(gòu)與方案，并給出了組合導(dǎo)航系統(tǒng)位置速度交替組合模式的數(shù)學(xué)模型[1]；曹娟娟提出一種利用磁強(qiáng)計(jì)輔助MEMS-SINS組合導(dǎo)航系統(tǒng)信息融合模型[2]；林雪原提出了利用AHRS代替SINS航向角信息的方法，試驗(yàn)表明利用該方法可有效地抑制組合系統(tǒng)航向角的發(fā)散趨勢(shì)[3]；馮紹軍設(shè)計(jì)了組合系統(tǒng)硬件和相應(yīng)軟件，并驗(yàn)證了組合系統(tǒng)的性能及可行性[4]。其他學(xué)者也進(jìn)行了相應(yīng)研究[5-6]。

以上研究重點(diǎn)介紹了組合導(dǎo)航系統(tǒng)的算法部分，對(duì)組合系統(tǒng)其他部分的介紹較少，沒(méi)有形成一套完整的組合方案。本文設(shè)計(jì)了一套完整的組合導(dǎo)航系統(tǒng)方案，介紹了低成本SINSGPS的硬件組成、GPS差分?jǐn)?shù)據(jù)傳輸、組合系統(tǒng)數(shù)據(jù)接收及數(shù)據(jù)同步的方法，給出了SINS導(dǎo)航數(shù)據(jù)更新的方案及簡(jiǎn)化數(shù)學(xué)模型，論述了數(shù)據(jù)融合卡爾曼濾波模型并進(jìn)行了車載實(shí)驗(yàn)。

二、硬件組成與TCP/IP鏈路

系統(tǒng)組成包括：2塊GPS接收板，1個(gè)SINS模塊，2臺(tái)筆記本電腦，其中1臺(tái)筆記本需要連接互聯(lián)網(wǎng)。

系統(tǒng)工作流程如圖1，可分為四步：

（1）GPS基準(zhǔn)站通過(guò)TCP/IP鏈路將定位數(shù)據(jù)上傳到服務(wù)器；

（2）服務(wù)器通過(guò)TCP/IP鏈路將GPS基準(zhǔn)站的定位數(shù)據(jù)發(fā)送到GPS流動(dòng)站，流 動(dòng)站進(jìn)行差分處理；

（3）GPS流動(dòng)站通過(guò)串口2將差分后的定位數(shù)據(jù)傳輸?shù)教幚碇行?，SINS通過(guò)串口1將陀螺儀和加速度計(jì)數(shù)據(jù)傳輸?shù)教幚碇行模?/p>

（4）處理中心融合SINS、GPS數(shù)據(jù)。

1、硬件組成

MPU6050是本文使用SINS的核心元器件，集成了3軸陀螺儀、3軸加速儀，可測(cè)量載體的線加速度和角速度。模塊內(nèi)部整合了16位的ADC傳感器允許陀螺儀和加速度計(jì)的實(shí)時(shí)采樣，提高了測(cè)量精度以及實(shí)現(xiàn)了數(shù)據(jù)的實(shí)時(shí)性。

SIM908是一款集成了GSM/GPRS技術(shù)的單頻C/A碼GPS模塊，每秒輸出1次NMEA-0183格式的GPS定位信息，由于其集成了GSM/GPRS功能，所以配合使用手機(jī)SIM卡就能快速搭建TCP/IP鏈路。

2、TCP/IP鏈路

SIM908定位精度低，無(wú)法輸出偽距?？梢允褂脙蓧KSIM908開(kāi)發(fā)板進(jìn)行位置差分，從而提高GPS的定位精度。利用SIM908的GPRS功能搭建TCP/IP鏈路，利用該鏈路將基準(zhǔn)站的定位信息傳輸給流動(dòng)站(即組合系統(tǒng)的GPS)。

基準(zhǔn)站與流動(dòng)站數(shù)據(jù)傳輸?shù)姆椒椋?/p>

（1）使用一臺(tái)處于公網(wǎng)中的電腦作為遠(yuǎn)程服務(wù)器；

（2）基準(zhǔn)站和流動(dòng)站分別與服務(wù)器建立TCP/IP連接；

（3）利用服務(wù)器將基準(zhǔn)站的數(shù)據(jù)轉(zhuǎn)發(fā)給流動(dòng)站。

利用Visual Basic(VB)語(yǔ)言及TCP/IP通訊控件Winsock可以快速實(shí)現(xiàn)TCP/IP通訊[7]。當(dāng)遠(yuǎn)程服務(wù)器處于內(nèi)網(wǎng)中時(shí)，可以使用內(nèi)網(wǎng)通軟件映射一個(gè)外網(wǎng)IP。

三、數(shù)據(jù)接收與同步

1、數(shù)據(jù)接收

SIM908開(kāi)發(fā)板采用RS-232串口通信接口標(biāo)準(zhǔn)，采用異步串行方式進(jìn)行數(shù)據(jù)傳送。SINS數(shù)據(jù)通訊時(shí)可以通過(guò)硬件轉(zhuǎn)換為同樣的接口標(biāo)準(zhǔn)。

通過(guò)VB語(yǔ)言及串口通訊控件MSComm可以快速、方便地實(shí)現(xiàn)串口通訊[7]。本文使用兩個(gè)串口分別接收SINS和GPS數(shù)據(jù)。

2、數(shù)據(jù)同步

GPS能提供授時(shí)服務(wù)，而SINS不具備時(shí)間系統(tǒng)，因此，SINS和GPS存在數(shù)據(jù)不同步的問(wèn)題。在組合導(dǎo)航系統(tǒng)進(jìn)行數(shù)據(jù)融合時(shí)，數(shù)據(jù)不同步會(huì)影響組合系統(tǒng)的定位精度[8]。數(shù)據(jù)同步可以通過(guò)三種方式實(shí)現(xiàn)：軟件、硬件、軟硬件結(jié)合。

本文使用軟件實(shí)現(xiàn)數(shù)據(jù)同步。思路是將SINS和GPS置于同一個(gè)時(shí)間系統(tǒng)下，用該系統(tǒng)的時(shí)間對(duì)SINS和GPS數(shù)據(jù)進(jìn)行標(biāo)記，從而達(dá)到數(shù)據(jù)同步的目的。可以利用計(jì)算機(jī)時(shí)間作為統(tǒng)一的時(shí)間系統(tǒng)，在提取數(shù)據(jù)的同時(shí)記錄計(jì)算機(jī)時(shí)間。

使用windows系統(tǒng)的API函數(shù)GetLocalTime能方便地獲取計(jì)算機(jī)時(shí)間，時(shí)間精度為毫秒級(jí)[9]。此外，數(shù)據(jù)同步的時(shí)間誤差還包含：兩個(gè)串口的數(shù)據(jù)傳輸時(shí)間之差；程序提取、解析數(shù)據(jù)的時(shí)間。但對(duì)于低精度的組合導(dǎo)航系統(tǒng)，數(shù)據(jù)同步的要求較低[8]。

四、慣導(dǎo)數(shù)據(jù)更新

1、航姿的獲取

低成本SINS的陀螺不能感知地球自轉(zhuǎn)，導(dǎo)致組合系統(tǒng)無(wú)法獲取航向角。為了解決這一現(xiàn)象，可以利用SINS自帶的姿態(tài)和航向參考系統(tǒng)(Attitude and Heading Reference System, AHRS)。AHRS分 為 兩 部分：在SINS中植入姿態(tài)角算法，從而得到姿態(tài)角；在SINS中集成三軸磁力計(jì)獲取地球磁北，經(jīng)過(guò)磁偏角補(bǔ)償后得到航向角。

本文使用AHRS輸出的航向角和姿態(tài)角替代慣導(dǎo)解算的航姿，省去了航姿更新的步驟，也無(wú)需考慮慣導(dǎo)的初始對(duì)準(zhǔn)問(wèn)題。

2、速度與位置更新

首先定義坐標(biāo)系，i表示慣性坐標(biāo)系，e表示地心地固坐標(biāo)系，選取東-北-天為導(dǎo)航坐標(biāo)系n（忽略n系變化）。

由慣導(dǎo)基本方程，速度更新算法如下[10]：

式中，下標(biāo)m、m-1—分別表示tm、tm-1時(shí)刻；

Tm—慣導(dǎo)數(shù)據(jù)更新時(shí)間間隔，Tm=tm-tm-1；

ωen—位置速率；

ωie—地球自轉(zhuǎn)角速度；

g—當(dāng)?shù)刂亓铀俣龋?/p>

C—表示tm-1時(shí)刻的姿態(tài)矩陣

Δvm—比力產(chǎn)生的速度增量；

Δvrotm—表示速度旋轉(zhuǎn)效應(yīng)產(chǎn)生的速度增量；Δvsculm—表示劃槳效應(yīng)產(chǎn)生的速度增量。

Δvrotm和Δvsculm采用雙子樣優(yōu)化算法計(jì)算得出，到此完成速度更新。

位置更新算法如下[10]：

式中，L、λ、h—分別表示緯度、經(jīng)度和大地高；

vN、vE、vU—分別表示北向、東向和天向速度；

RM、RN—分別表示子午圈和卯酉圈半徑。

五、數(shù)據(jù)融合

使用卡爾曼濾波器融合SINS與GPS數(shù)據(jù)，組合模式為位置組合，數(shù)學(xué)模型[11]為：

由于使用AHRS輸出的航姿替代慣導(dǎo)解算的航姿，所以卡爾曼濾波器狀態(tài)方程僅包含慣導(dǎo)速度和位置誤差方程。

綜上所述，選取狀態(tài)估計(jì)量為：

式中，δvn—速度誤差；

δpn—位置誤差；

ob—加速度計(jì)零偏。

六、實(shí)驗(yàn)

為了驗(yàn)證該方案的可行性，在中南大學(xué)新校區(qū)進(jìn)行了車載實(shí)驗(yàn)。實(shí)驗(yàn)中，除了采集SINS和GPS數(shù)據(jù)，還利用商用GPS接收機(jī)進(jìn)行了RTK定位，并以RTK數(shù)據(jù)作為車載的真實(shí)軌跡。SINS和GPS的各項(xiàng)精度指標(biāo)見(jiàn)表1，其中，GPS定位中誤差(位置差分后)由原始數(shù)據(jù)計(jì)算得出；SINS加速度計(jì)零偏和AHRS航姿誤差(無(wú)干擾磁場(chǎng)時(shí))為SINS生產(chǎn)廠家提供值。

圖2中藍(lán)色和紅色曲線分別顯示了GPS定位誤差和組合系統(tǒng)的定位誤差，可以看出組合系統(tǒng)的定位精度較GPS有了較大提高。為了驗(yàn)證在GPS短時(shí)間失鎖的情況下組合系統(tǒng)的定位性能，模擬GPS信號(hào)失鎖20秒。圖3顯示了GPS失鎖狀態(tài)下組合系統(tǒng)的定位誤差，緯度方向約9m，經(jīng)度方向約20m，高程方向約7m，加速度計(jì)零偏在線估計(jì)值0.012g。

表1 SINS和GPS各項(xiàng)精度指標(biāo)

七、結(jié)語(yǔ)

低成本SINSGPS組合導(dǎo)航較GPS導(dǎo)航定位誤差明顯降低，原因是慣導(dǎo)的測(cè)量信息和濾波器的作用平滑了GPS定位誤差。AHRS易受磁場(chǎng)干擾，航向角測(cè)量精度較低，只能在較短時(shí)間內(nèi)進(jìn)行自主導(dǎo)航定位，但在GPS短時(shí)間失鎖或故障時(shí)仍然能發(fā)揮一定作用。