高小鵬

（西安精準測控有限責任公司，陜西 西安 710075）

0 引 言

捷聯(lián)慣性導(dǎo)航系統(tǒng)（SINS）具有自主導(dǎo)航能力，不需要任何外界電磁信號就可以獨立給出載體的姿態(tài)、速度和位置信息，抗外界干擾能力強。但是SINS定位誤差隨時間的延續(xù)不斷增大，即誤差積累、漂移大[1]。

全球?qū)Ш叫l(wèi)星系統(tǒng)（GNSS）具有全球、全天候、高精度、實時定位等優(yōu)點，它的誤差不隨時間積累，但是GPS存在信號容易被遮擋的缺陷，特別是當車輛行駛到高大建筑物下或者隧道里面等地方，車輛GPS接收機就接收不到衛(wèi)星信號，從而使定位誤差瞬間增大甚至不能獲得定位信息。

航位推算（Dead Reckoning, DR）是一種自主定位技術(shù)，在提供給它確定的初始位置之后，通過陀螺儀和里程儀推算出車輛的瞬時位移增量，從而推算出該車輛當前的具體位置。但是該方法的定位誤差會隨著時間的積累而積累，時間過長將失去定位效果[2]。

因此，本文提出了一種基于慣導(dǎo)/衛(wèi)星信號/里程儀組合的車載導(dǎo)航系統(tǒng)[3]，可以充分發(fā)揮各自系統(tǒng)的優(yōu)點，克服缺點，實現(xiàn)在高動態(tài)和短時強電子干擾的環(huán)境下實時、高精度的導(dǎo)航定位。

1 系統(tǒng)構(gòu)成

1.1 系統(tǒng)算法結(jié)構(gòu)設(shè)計

系統(tǒng)算法框架如圖1所示。首先由IMU經(jīng)過純慣性解算得到車輛的位置+速度和姿態(tài)信息，里程儀通過慣導(dǎo)的姿態(tài)矩陣經(jīng)過航位推算得到位置。在衛(wèi)星信號有效時，將接收機與SINS各自輸出的位置+速度作差構(gòu)成位置+速度觀測量，同時將里程計推算的位置與GPS的輸出位置作差形成位置量測；在衛(wèi)星信號無效時，將里程計推算的位置與SINS的輸出位置作差形成位置量測。經(jīng)過卡爾曼濾波后得到SINS的誤差估計和里程計的誤差估計，進而分別對SINS和里程計進行反饋校正。

圖1 SINS/GPS/OD組合導(dǎo)航原理框架

1.2 系統(tǒng)硬件結(jié)構(gòu)設(shè)計

本系統(tǒng)采取模塊化設(shè)計，采用積木式搭建，生產(chǎn)維修便捷。首先把主要選取的元器件做一個簡要介紹。主處理器選用意法半導(dǎo)體的STM32F767IGK6，該處理器自帶DSP和FPU的高性能ARM Cortex-M7 MCU，具有1 MB FLASH、216 MHz CPU、ART加速器、一級緩存、SDRAM、TFT、JPEG編解碼器和DFSDM，非常適合在空間體積受限，接口要求多，計算精度、速度、功耗都有一定要求的場合。IMU選用挪威SENSONOR公司的STIM300高性能MEMS慣性測量單元，其陀螺的零偏穩(wěn)定性優(yōu)于0.3（ °）/h，加速度表的零偏穩(wěn)定性優(yōu)于50 μg，性能非常優(yōu)越。采用RS 422接口，易于集成到新設(shè)計中。衛(wèi)星接收機選用加拿大諾瓦泰公司的OEM718D，OEM718D采用555跟蹤通道設(shè)計，支持所有GNSS系統(tǒng)，包括GPS、GLONASS、BEIDOU、Galileo。小尺寸外形設(shè)計、全新硬件平臺、高抗干擾算法、支持雙天線輸入等，為用戶提供厘米級差分定位和高精度航向測量，低功耗、重量輕、接口靈活等特點尤其適合各領(lǐng)域集成應(yīng)用。里程計信號直接讀取汽車總線的原始信息。其硬件結(jié)構(gòu)如圖2所示。

圖2 SINS/GNSS/OD組合模塊硬件結(jié)構(gòu)

1.3 系統(tǒng)軟件結(jié)構(gòu)設(shè)計

產(chǎn)品上電后微處理器自動進行端口配置，等待STIM300信息接入，對接收機及千尋4G模塊進行自檢，當產(chǎn)品各個模塊自檢完成后，連接4G網(wǎng)絡(luò)，等待接收機差分定位完成之后跳轉(zhuǎn)至正常工作模式。在正常工作模式下，以100 Hz的解算頻率讀取IMU輸出的增量信息和衛(wèi)星高精度差分定位定向信息。車載ODB接口輸出的車載總線信息經(jīng)過多信號融合算法，得出當前車輛的姿態(tài)、位置和速度信息，并通過DMA接口模塊進行幀格式的打包及對外接口的DMA控制器數(shù)據(jù)發(fā)送。系統(tǒng)軟件流程如圖3所示。

圖3 系統(tǒng)軟件流程

2 組合算法

2.1 初始對準

慣性導(dǎo)航角速度關(guān)系及比力方程如下：

載體靜止時ωn en和非常小可以忽略，考慮陀螺和加速度計測量誤差遠小于有用信號時，式（1）、式（2）分別改寫為：

初始對準一般是在運載體相對靜止的環(huán)境下進行的，那么重力矢量和地球自轉(zhuǎn)角速度矢量在地理坐標系（初始對準參考坐標系）的分量如下：

式中：L，g和ωie分別表示當?shù)鼐暥?、重力加速度大小和地球自轉(zhuǎn)角速率大小；且記地球自轉(zhuǎn)角速度的北向分量ωN=ωiecosL和天向分量ωU=ωiesinL。

根據(jù)雙矢量對準原理，選擇(-gn)作為主參考矢量，可得姿態(tài)陣估計：

將式（5）代入式（6），得：

2.2 航位推算算法

設(shè)載車的行駛速度為vD，它指向載車的正前方，寫成矢量形式為vb=[0 vD0]T。因此，載車的速度在n系的表達形式為[4-5]：

2.3 組合狀態(tài)空間模型

將慣性導(dǎo)航失準角誤差和航位推算誤差合并在一起，同時考慮車體坐標系（m系）和IMU坐標系（b系）的俯仰角偏差αθ和方位角偏差αφ，里程儀刻度系數(shù)誤差δkD，可以得到以速度位置誤差作為觀測量的慣導(dǎo)/衛(wèi)星/里程儀的組合導(dǎo)航狀態(tài)空間模型如下：

2.4 自適應(yīng)卡爾曼濾波

普通卡爾曼濾波是在線性高斯情況下利用最小均方誤差準則獲得目標估計，適應(yīng)于過程和測量都屬于線性系統(tǒng)，且誤差符合高斯分布系統(tǒng)。但是實際上很多系統(tǒng)都存在一定的非線性，這種情況下就不能使用一般的卡爾曼濾波。擴展卡爾曼濾波模型可以實現(xiàn)非線性系統(tǒng)的近似線性化，將其轉(zhuǎn)化為線性問題，可進一步提高求解精度。

3 實驗和仿真結(jié)果

實驗載體是別克英朗小轎車，如圖4所示。系統(tǒng)包括：車頂前后放置的2根GNSS天線（與高精度定位定向板卡配套，提供高精度GNSS RTK定位定向解算結(jié)果），車頂中間放置設(shè)備，后備箱蓋上放置千尋4G天線，設(shè)備同時接入汽車OBD總線。

圖4 實驗設(shè)備安裝圖

為驗證本文算法的效果，進行相應(yīng)的跑車試驗。車輛在城市內(nèi)辦公園區(qū)繞行1周，根據(jù)實際采集數(shù)據(jù)產(chǎn)生一條軌跡。

從實驗結(jié)果圖5和圖6可以看出，GPS系統(tǒng)定位結(jié)果基本能夠反映出車輛行駛的真實軌跡，但是定位曲線不夠平滑，存在較多的野值點，甚至某些時刻沒有定位。如圖7所示，在仿真軌跡中選取一段城市建筑比較密集的地段進行局部放大，可以看出在該路段GPS定位結(jié)果中出現(xiàn)很多跳點，而采用慣導(dǎo)／衛(wèi)星／里程儀組合導(dǎo)航系統(tǒng)的定位結(jié)果明顯改善了這一現(xiàn)象，能夠很好地跟蹤行車軌跡，定位曲線更趨平滑。

圖5 跑車實驗路線

圖6 GPS和組合后軌跡對比

圖7 GPS和組合后軌跡對比局部放大圖

為了進一步產(chǎn)品性能，用一套接收機板卡作為對比，在中途跑車進行中（1 920～2 117 s），人為拔掉慣組外接天線，對比純慣性和慣導(dǎo)/里程計組合后位置誤差。

從圖8可以看出，在200 s連續(xù)丟星情況下慣組和里程儀組合定位曲線和衛(wèi)星定位結(jié)果趨勢一致，表明慣組和里程儀組合導(dǎo)航系統(tǒng)能有效抑制導(dǎo)航精度隨時間的發(fā)散。由于里程計提供只是相對定位，從圖9中看出組合后位置誤差隨著時間和距離增大趨勢逐漸增大。從圖10得出，定位精度為2.28/1 225=0.001 8，也就是0.18%，表明這種組合方式在人行天橋、高架、 隧道等場景和城市復(fù)雜環(huán)境時能提供較高精度的定位。

圖8 GPS和組合后軌跡對比

圖9 北向位置誤差曲線

圖10 總位置誤差隨行駛距離變化曲線

4 結(jié) 語

通過實驗結(jié)果驗證，在衛(wèi)星信號遮擋或者丟失情況下，慣導(dǎo)/衛(wèi)星/里程計組合導(dǎo)航系統(tǒng)能夠平滑行車軌跡，較好地抑制慣導(dǎo)誤差發(fā)散，并將其誤差限制在較低的水平，提高該組合導(dǎo)航系統(tǒng)的精度與可靠性。