基于高精度MEMS慣性器件的多傳感器融合定位系統的設計與研究

高小鵬

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

0 引 言

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

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

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

因此，本文提出了一種基于慣導/衛星信號/里程儀組合的車載導航系統[3]，可以充分發揮各自系統的優點，克服缺點，實現在高動態和短時強電子干擾的環境下實時、高精度的導航定位。

1 系統構成

1.1 系統算法結構設計

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

圖1 SINS/GPS/OD組合導航原理框架

1.2 系統硬件結構設計

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

圖2 SINS/GNSS/OD組合模塊硬件結構

1.3 系統軟件結構設計

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

圖3 系統軟件流程

2 組合算法

2.1 初始對準

慣性導航角速度關系及比力方程如下：

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

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

式中：L，g和ωie分別表示當地緯度、重力加速度大小和地球自轉角速率大小；且記地球自轉角速度的北向分量ωN=ωiecosL和天向分量ωU=ωiesinL。

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

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

2.2 航位推算算法

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

2.3 組合狀態空間模型

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

2.4 自適應卡爾曼濾波

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

3 實驗和仿真結果

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

圖4 實驗設備安裝圖

為驗證本文算法的效果，進行相應的跑車試驗。車輛在城市內辦公園區繞行1周，根據實際采集數據產生一條軌跡。

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

圖5 跑車實驗路線

圖6 GPS和組合后軌跡對比

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

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

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

圖8 GPS和組合后軌跡對比

圖9 北向位置誤差曲線

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

4 結 語

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