考慮里程計(jì)截?cái)嗾`差的SINS/OD組合導(dǎo)航算法

周召發(fā)，趙芝謙，張志利，曾 進(jìn)

（1. 火箭軍工程大學(xué) 導(dǎo)彈工程學(xué)院，西安 710025；2. 航空工業(yè)自控所 慣性技術(shù)航空科技重點(diǎn)實(shí)驗(yàn)室，西安 710065）

捷聯(lián)慣導(dǎo)系統(tǒng)（SINS）能夠?qū)崟r(shí)提供載體的姿態(tài)、速度和位置，具有全天候、自主、隱蔽等優(yōu)點(diǎn)，已成為陸地車輛上必不可少的設(shè)備。但其系統(tǒng)誤差隨時(shí)間不斷積累，不能長(zhǎng)時(shí)間進(jìn)行導(dǎo)航，車輛的機(jī)動(dòng)能力將受到極大的制約[1,2]。目前車載組合導(dǎo)航常用SINS/GNSS組合導(dǎo)航的方式來實(shí)現(xiàn)車輛的高精度定位，由于全球衛(wèi)星定位系統(tǒng)（GPS）信號(hào)容易被干擾，有可能暴露自身位置的風(fēng)險(xiǎn)，不能滿足軍事車輛對(duì)于自主性的要求[3]。里程計(jì)在車輛中隨處可見，作為外部測(cè)量工具使用非常方便，將里程計(jì)測(cè)量的速度信息引入慣導(dǎo)系統(tǒng)，既能夠保證車輛的自主性，又能夠保證車輛的導(dǎo)航精度[4,5]。

在SINS/OD組合導(dǎo)航系統(tǒng)中，近年來的研究熱點(diǎn)主要集中在慣導(dǎo)安裝誤差角和里程計(jì)誤差補(bǔ)償方面。文獻(xiàn)[6]采用零速修正的方法進(jìn)行里程計(jì)的標(biāo)定，但需要間隔一段時(shí)間停一次車，降低了車輛的機(jī)動(dòng)性；文獻(xiàn)[7]通過向心加速度作為觀測(cè)量進(jìn)行濾波，但車輛實(shí)際運(yùn)動(dòng)環(huán)境復(fù)雜、噪聲多，導(dǎo)致估計(jì)結(jié)果不準(zhǔn)確，從而定位精度并不高；文獻(xiàn)[8]利用車輛正常行駛過程中橫向和法向速度為零的約束條件，考慮了慣導(dǎo)安裝誤差角，但沒有考慮里程計(jì)刻度系數(shù)誤差；文獻(xiàn)[9]研究了INS/OD集成系統(tǒng)中里程計(jì)的測(cè)量模型，并采用多模型自適應(yīng)估計(jì)方法進(jìn)一步提高了定位性能；文獻(xiàn)[10]利用非線性參數(shù)運(yùn)動(dòng)模型與地面真實(shí)訓(xùn)練數(shù)據(jù)之間的殘差訓(xùn)練里程計(jì)誤差的高斯過程模型參數(shù)，通過有效計(jì)算圖像幀和有效分布關(guān)鍵幀來增強(qiáng)視覺SLAM；文獻(xiàn)[11]提出了一種基于高斯過程回歸（GPR）的里程計(jì)故障檢測(cè)方法，采用粒子群優(yōu)化方法尋找高斯模型的最優(yōu)超參數(shù)，提高了導(dǎo)航精度。

為實(shí)現(xiàn)車載慣導(dǎo)的高精度自主定位，里程計(jì)的精確建模越來越重要，本文除慣導(dǎo)安裝誤差角和里程計(jì)刻度系數(shù)誤差以外，研究了里程計(jì)截?cái)嗾`差對(duì)定位精度的影響，提出了基于里程計(jì)截?cái)嗾`差補(bǔ)償?shù)腟INS/OD組合導(dǎo)航算法。通過分析里程計(jì)脈沖輸出的誤差特性，完善了傳統(tǒng)的速度匹配組合導(dǎo)航算法，但考慮到以速度形式容易擴(kuò)大誤差，而里程計(jì)的輸出與車輛運(yùn)動(dòng)特性有關(guān)，采取捷聯(lián)慣導(dǎo)輸出轉(zhuǎn)化為脈沖增量輸出與里程計(jì)輸出做差作為量測(cè)的補(bǔ)償模式，直接作用于里程計(jì)原始輸出，效果是很理想的。前兩種方法都是將里程計(jì)截?cái)嗾`差作為隨機(jī)常值，利用卡爾曼濾波進(jìn)行補(bǔ)償，但局限于里程計(jì)正常工作，同時(shí)補(bǔ)償可能不精確，為了改善性能，充分利用歷史數(shù)據(jù)，提出基于高斯回歸的里程計(jì)截?cái)嗾`差預(yù)測(cè)模型，該模型能夠有效預(yù)測(cè)車輪里程計(jì)的測(cè)量殘差，當(dāng)里程計(jì)發(fā)生故障時(shí)也能很好補(bǔ)償，大大提高了組合導(dǎo)航的精確度，三種算法由淺而深，逐層遞進(jìn)，都證明了補(bǔ)償里程計(jì)截?cái)嗾`差的重要性。

1 SINS/OD組合導(dǎo)航系統(tǒng)

1.1 系統(tǒng)模型描述

如圖1所示，陸地車輛配備有一個(gè)導(dǎo)航級(jí)的IMU，里程計(jì)安裝在車輛非轉(zhuǎn)向軸上。為了方便開展研究，需要對(duì)車載慣導(dǎo)/里程計(jì)組合導(dǎo)航系統(tǒng)坐標(biāo)系進(jìn)行定義。選取東北天為導(dǎo)航參考坐標(biāo)系，記為n系；車體坐標(biāo)系記為m系，其中車體中心 Om位于車輛前軸的中點(diǎn)，xm軸指向車體橫向的右方，ym軸指向車體縱向的正前方，zm軸指向天向，里程計(jì)用累積的脈沖來測(cè)量車輛前進(jìn)的運(yùn)動(dòng)，即從車輛運(yùn)動(dòng)開始時(shí)產(chǎn)生脈沖數(shù)，為了簡(jiǎn)單起見，可認(rèn)為里程計(jì)坐標(biāo)系與車體系一致；慣導(dǎo)坐標(biāo)系記為b系，與車體坐標(biāo)系指向一樣，滿足右手直角坐標(biāo)系，但由于安裝精度的影響，m系與b系之間存在安裝誤差角，分別記為俯仰誤差角αθ、橫滾誤差角 αγ、航向誤差角αψ。

1.2 里程計(jì)刻度系數(shù)誤差分析

里程計(jì)輸出是脈沖信號(hào)，在采樣周期T內(nèi)，里程計(jì)輸出p個(gè)脈沖，令里程計(jì)刻度系數(shù)為K，則車輛行駛速度νD與脈沖滿足以下關(guān)系：

假設(shè)車輛行駛過程中，車輪沒有打滑和跳動(dòng)，車輛天向和橫向速度均為零，里程計(jì)能夠準(zhǔn)確測(cè)量車輛前進(jìn)速度，里程計(jì)速度在車體坐標(biāo)投影為：

然而車輛在實(shí)際運(yùn)動(dòng)中，里程計(jì)的刻度系數(shù)會(huì)受到車輛載荷、輪胎磨損、氣壓和溫度等因素的影響發(fā)生變化導(dǎo)致測(cè)量誤差[12]，設(shè)里程計(jì)的刻度系數(shù)誤差為δk，則里程計(jì)真實(shí)速度為：

1.3 車載慣導(dǎo)安裝誤差角分析

由上述可知慣導(dǎo)存在安裝偏差角，則慣導(dǎo)坐標(biāo)系到車體坐標(biāo)系的安裝誤差矩陣可以表示為：

式中c表示cos，s表示sin，由于安裝誤差角均為小角度，我們可認(rèn)為cosαi=1、sinαi= 0，其中i=θ、γ、ψ，則有：

式中 α =[αθ0 αψ]T，可以看出橫滾誤差角αγ不影響里程計(jì)的速度測(cè)量值。實(shí)際建立的導(dǎo)航坐標(biāo)系n'與理想導(dǎo)航坐標(biāo)系n之間存在失準(zhǔn)角φ= [φEφNφU]T，慣導(dǎo)坐標(biāo)系到導(dǎo)航坐標(biāo)系的轉(zhuǎn)換矩陣為：

1.4 捷聯(lián)慣導(dǎo)誤差方程

根據(jù)文獻(xiàn)[13]，直接列寫出捷聯(lián)慣導(dǎo)誤差方程：

其中：φ為失準(zhǔn)角；nν為車輛在導(dǎo)航系的運(yùn)動(dòng)速度；為導(dǎo)航系相對(duì)于慣性系的角速度在導(dǎo)航坐標(biāo)系的投影；為地球自轉(zhuǎn)角速度在導(dǎo)航系的投影；為導(dǎo)航坐標(biāo)系相對(duì)于地球坐標(biāo)系的旋轉(zhuǎn)速度在導(dǎo)航系的投影；fn為加速度計(jì)比力輸出在導(dǎo)航系的投影；εb為陀螺誤差在慣導(dǎo)坐標(biāo)系的投影；?b為加速度誤差在慣導(dǎo)坐標(biāo)系的投影；L、λ、h分別為經(jīng)度、緯度、高程； RM為子午圈主曲率半徑；RN卯酉圈主曲率半徑。

2 濾波算法

2.1 傳統(tǒng)速度匹配組合導(dǎo)航算法

為了方便后續(xù)描述，本小節(jié)所述傳統(tǒng)速度匹配組合導(dǎo)航算法稱為Tv-kf（T：傳統(tǒng)；v：速度觀測(cè)；kf：卡爾曼濾波）?？紤]里程計(jì)刻度系數(shù)誤差和慣導(dǎo)安裝誤差角，車輛真實(shí)速度在導(dǎo)航系投影：

代入式(3)(5)(6)，忽略二階小量有：

則里程計(jì)速度誤差方程為：

式中ei表示第i個(gè)元素為1的單位列向量。將捷聯(lián)慣導(dǎo)系統(tǒng)的姿態(tài)誤差 φn，速度誤差 δ νn，位置誤差δPn，陀螺零漂 εb，加速度計(jì)零偏 ?b，里程計(jì)刻度系數(shù)誤差 δ k和慣導(dǎo)安裝誤差角αθ、αψ作為系統(tǒng)狀態(tài)變量，共18維：

系統(tǒng)狀態(tài)方程為：

式中，1F為狀態(tài)轉(zhuǎn)移矩陣，令F15×15是由式(7)推出的基本捷聯(lián)慣導(dǎo)狀態(tài)轉(zhuǎn)移矩陣，具體公式在此不再贅述，則W1為系統(tǒng)噪聲。選取捷聯(lián)慣導(dǎo)與里程計(jì)輸出的速度之差作為觀測(cè)量，則有：

系統(tǒng)量測(cè)方程為 Z1= H1X1+ V1，V1為測(cè)量噪聲，其中量測(cè)矩陣為：

2.2 基于速度觀測(cè)的截?cái)嗾`差補(bǔ)償算法

本小節(jié)所述基于速度觀測(cè)的截?cái)嗾`差補(bǔ)償算法稱為Iv-kf（I：改進(jìn)；v：速度觀測(cè)；kf：卡爾曼濾波）。當(dāng)考慮里程計(jì)脈沖截?cái)嗾`差時(shí)，設(shè)采樣周期T內(nèi)脈沖測(cè)量誤差為δpod，車輛真實(shí)行駛速度為：

同理可得里程計(jì)速度誤差方程：

將里程計(jì)截?cái)嗾`差δpod作為系統(tǒng)狀態(tài)，為隨機(jī)常值，利用卡爾曼濾波進(jìn)行實(shí)時(shí)補(bǔ)償，則系統(tǒng)狀態(tài)變量為：

同樣選取捷聯(lián)慣導(dǎo)與里程計(jì)輸出的速度之差作為觀測(cè)量，則有：

由于系統(tǒng)狀態(tài)變量增加了里程計(jì)截?cái)嗾`差δpod，則系統(tǒng)狀態(tài)轉(zhuǎn)移矩陣為因此系統(tǒng)量測(cè)矩陣變?yōu)椋?/p>

2.3 基于脈沖觀測(cè)的截?cái)嗾`差補(bǔ)償算法

基于脈沖觀測(cè)的截?cái)嗾`差補(bǔ)償算法稱為P-kf（P：脈沖觀測(cè)；kf：卡爾曼濾波）。圖2表示了里程計(jì)的測(cè)量模型，黑色點(diǎn)為脈沖發(fā)生節(jié)點(diǎn)，相鄰兩個(gè)時(shí)間差值為采樣周期，則在任何一個(gè)采樣時(shí)間段 (tk, tk+1)內(nèi)，里程計(jì)真實(shí)脈沖值pK+1與測(cè)量輸出值有如下關(guān)系：

圖2 里程計(jì)測(cè)量模型Fig.2 Odometer measurement model

式中 δpk+1=Δpk+1-Δpk，為里程計(jì)測(cè)量的截?cái)嗾`差，取值范圍為(-1,1)。

里程計(jì)脈沖輸出與車輛運(yùn)動(dòng)狀態(tài)有關(guān)，單位時(shí)間內(nèi)車輛運(yùn)動(dòng)越快里程計(jì)脈沖輸出個(gè)數(shù)越多，由于慣導(dǎo)設(shè)備精度高，能夠準(zhǔn)確反映車輛運(yùn)動(dòng)狀態(tài)，則在相同采樣時(shí)間段 (tk, tk+1)內(nèi)，計(jì)算出車輛行駛時(shí)慣導(dǎo)所產(chǎn)生脈沖數(shù)為：

式中sk+1為車輛在采樣時(shí)間內(nèi)行駛的距離，分別為tk和tk+1時(shí)刻車輛在m系下的速度，為中點(diǎn)速度，為車輛行駛的加速度。

由于慣導(dǎo)測(cè)量存在誤差，導(dǎo)致采樣時(shí)間內(nèi)車輛行駛距離與真實(shí)值存在偏差Δsk1+，則慣導(dǎo)輸出轉(zhuǎn)化為脈沖輸出與里程計(jì)脈沖真實(shí)值pk+1有如下的關(guān)系：

聯(lián)立式(21)(23)，可得以下關(guān)系：

由于非完整性約束，只有沿著車體前進(jìn)方向有脈沖增量，采樣時(shí)間 T = tk+1- tk很短，可認(rèn)為為常量，因此在采樣時(shí)間 (tk, tk+1)內(nèi)慣導(dǎo)輸出脈沖增量為：

式中px、pz稱為虛觀測(cè)量，為車輛橫向和天向的觀測(cè)量；為慣導(dǎo)在導(dǎo)航系下采樣時(shí)間段內(nèi)的中點(diǎn)速度。由于里程計(jì)刻度系數(shù)誤差和慣導(dǎo)安裝誤差角的存在，代入式(5)(6)，其真實(shí)脈沖為：

忽略二階小量，由此可得慣導(dǎo)脈沖增量誤差為：

選取慣導(dǎo)脈沖輸出與里程計(jì)輸出之差作為觀測(cè)量：

同樣將里程計(jì)截?cái)嗾`差作為隨機(jī)常值，引入到系統(tǒng)狀態(tài)變量中，則有：

而系統(tǒng)狀態(tài)轉(zhuǎn)移矩陣 F3=F2，因此可得系統(tǒng)量測(cè)矩陣為：

2.4 基于高斯回歸模型的截?cái)嗾`差預(yù)測(cè)

基于高斯回歸模型的截?cái)嗾`差預(yù)測(cè)算法稱為Gp-kf（G：高斯回歸；p：脈沖觀測(cè)；kf：卡爾曼濾波）。在2.3節(jié)中，詳細(xì)介紹了里程計(jì)脈沖輸出和慣導(dǎo)脈沖的誤差模型，將里程計(jì)截?cái)嗾`差作為隨機(jī)常值利用卡爾曼濾波進(jìn)行補(bǔ)償，可能補(bǔ)償不夠精確，同時(shí)里程計(jì)發(fā)生故障會(huì)產(chǎn)生較大誤差。高斯過程（GPs）是一個(gè)強(qiáng)大的非參數(shù)工具，用于從樣本數(shù)據(jù)學(xué)習(xí)回歸函數(shù)，可以很好地處理糟糕和嘈雜的數(shù)據(jù)，因此在解決現(xiàn)實(shí)場(chǎng)景時(shí)非常實(shí)用。由式(24)可知，慣導(dǎo)脈沖輸出減去里程計(jì)輸出等于里程計(jì)誤差加上慣導(dǎo)脈沖誤差，在標(biāo)準(zhǔn)的卡爾曼濾波中，模型的量測(cè)誤差主要是噪聲引起的，因此慣導(dǎo)脈沖的測(cè)量誤差 δpins亦可認(rèn)作為零均值高斯白噪聲。因此，有了以下的帶噪聲的高斯模型：

式中x表示輸入向量，在此系統(tǒng)中用里程計(jì)輸出的脈沖pod來描述；f(x)是函數(shù)在x處的真值，即里程計(jì)誤差；y是被噪音污染的觀測(cè)，即慣導(dǎo)脈沖輸出減去里程計(jì)脈沖輸出；噪聲ε是服從 N(0,σn2)的高斯分布，即慣導(dǎo)脈沖誤差 δpins。

下面將詳細(xì)介紹如何利用高斯回歸模型對(duì)里程計(jì)誤差進(jìn)行預(yù)測(cè)。已知輸入向量集合 x = [x1x2…xn]和帶噪聲的輸出集合 y = ［ y1y2… yn］，對(duì)于新的輸入x*，為了得到最小噪聲的預(yù)測(cè)輸出f(x*)，可以得到已知觀測(cè)x的先驗(yàn)分布和預(yù)測(cè)點(diǎn)x*與已知觀測(cè)x的聯(lián)合分布分別為：

式中In為n維單位向量，k(x,x')為協(xié)方差矩陣，用于度量 xi與xj之間的相關(guān)性；k(x*,x')是預(yù)測(cè)點(diǎn)x*和已知觀測(cè)x之間的協(xié)方差矩陣； k(x*,x*)是預(yù)測(cè)點(diǎn)本身的協(xié)方差。即為以下公式：

高斯回歸是一種非參數(shù)模型，沒有訓(xùn)練模型參數(shù)的過程，一旦協(xié)方差函數(shù)、輸入數(shù)據(jù)給定，則模型就被唯一確定下來。所以，回歸的重點(diǎn)在于確定協(xié)方差函數(shù)。協(xié)方差函數(shù)有多種，通過對(duì)模型的分析，選擇SE核函數(shù)，其表達(dá)式為：

式(36)的超參數(shù)為未知參數(shù)，當(dāng)l選擇越大，函數(shù)更加平滑，同時(shí)訓(xùn)練數(shù)據(jù)點(diǎn)之間的預(yù)測(cè)方差更小，反之l越小則函數(shù)更加“曲折”，訓(xùn)練數(shù)據(jù)點(diǎn)之間的預(yù)測(cè)方差更大；σ則直接控制方差大小，σ越大方差越大，反之亦然[14]。如何選擇最優(yōu)超參數(shù)，即在最優(yōu)超參數(shù)下y出現(xiàn)的概率為最大化，可通過用極大似然法求解，多元高斯分布的負(fù)對(duì)數(shù)邊際似然函數(shù)為：

先選擇合適的超參數(shù)初值，然后通過式(37)可以求解出最佳超參數(shù)，就能得到協(xié)方差函數(shù)k(x,x)的具體表達(dá)式，從而求解高斯過程回歸的關(guān)鍵預(yù)測(cè)方程：

其中：

由上述分析得到了基于高斯回歸模型的里程計(jì)截?cái)嗾`差預(yù)測(cè)值，在下一步進(jìn)行濾波時(shí)，有如下觀測(cè)方程：

由于里程計(jì)截?cái)嗾`差已經(jīng)在觀測(cè)量中進(jìn)行了補(bǔ)償，則系統(tǒng)的狀態(tài)變量為：

系統(tǒng)狀態(tài)轉(zhuǎn)移矩陣 F4=F1，因此量測(cè)矩陣為：

故障診斷：將慣導(dǎo)輸出轉(zhuǎn)化為脈沖，可以輕易的判斷里程計(jì)工況是否正常，假設(shè)測(cè)量速度誤差在m系下為±2m/s（很大了），則由式(22)可知慣導(dǎo)產(chǎn)生的脈沖誤差為：

式中采樣周期T和里程計(jì)刻度系數(shù)K均為實(shí)驗(yàn)真實(shí)值，可真實(shí)慣導(dǎo)引起的脈沖誤差是很小的，遠(yuǎn)遠(yuǎn)小于1個(gè)脈沖。通過里程計(jì)誤差輸出模型，得知截?cái)嗾`差 δpod∈(-1 ,1)，因此有以下判斷基準(zhǔn)：

當(dāng)發(fā)生故障時(shí)，里程計(jì)脈沖輸出不可信，由故障診斷條件可知，正常工作情況下兩者之間的脈沖差值小于2個(gè)脈沖，所以在短時(shí)間內(nèi)可取慣導(dǎo)脈沖輸出的整數(shù)部分作為里程計(jì)輸出是可行的，目的是減小因故障產(chǎn)生的較大觀測(cè)誤差，從而在一定程度上減小定位誤差。

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

3.1 仿真試驗(yàn)與分析

為了驗(yàn)證所提方法的性能，進(jìn)行了仿真試驗(yàn)。設(shè)定車輛初始位置：經(jīng)度108.909°E，緯度34.246°N，海拔高度為380m。車輛初始速度為零，模擬了車的運(yùn)動(dòng)狀態(tài)，具體包括：①靜止100s后，沿正北方向以1m/s2的加速度行駛10s，然后勻速直線行駛200s；②以2°/s的角速度左轉(zhuǎn)90°，之后行駛200s；以2°/s的角速度左轉(zhuǎn)90°，之后行駛200s；③以 -1 m/s2的加速度行駛5s；④以2°/s的角速度左轉(zhuǎn)450°，之后行駛200s；⑤以2°/s的仰角速度爬升10s，之后行駛200s；以2°/s的俯角速度下行10s，之后行駛20s；⑥以3°/s的角速度左轉(zhuǎn)90°，之后行駛200s⑦以1m/s2的加速度行駛5s，之后行駛200s；⑧以2°/s的角速度左轉(zhuǎn)90°，之后行駛300s。

設(shè)定慣性元件參數(shù)：陀螺儀零偏為0.01°/h，隨機(jī)游走為0.001°/，加速度計(jì)常值漂移為50μg，隨機(jī)游走為5μg/，輸出頻率為100 Hz。初始姿態(tài)角均為零，慣導(dǎo)安裝誤差角為俯仰安裝誤差角αθ為20'、航向安裝誤差角αψ為30'，里程計(jì)刻度系數(shù)誤差為0.02，為了仿真里程計(jì)截?cái)嗾`差，在里程計(jì)脈沖輸出加上隨機(jī)誤差。車輛運(yùn)動(dòng)軌跡如圖3所示，行駛時(shí)間約為37 min，軌跡共長(zhǎng)17.25公里。

圖3 仿真軌跡Fig.3 Simulation trajectory

慣導(dǎo)安裝誤差角和里程計(jì)刻度系數(shù)誤差在線標(biāo)定結(jié)果如圖4所示。從圖中可以看到，所提方法都能夠較準(zhǔn)確地估計(jì)慣導(dǎo)安裝偏差角，補(bǔ)償截?cái)嗾`差的速度匹配組合導(dǎo)航在俯仰安裝誤差角估計(jì)上精確度差了些；由于里程計(jì)輸出加了隨機(jī)誤差，刻度系數(shù)誤差都在0.02左右。（俯仰安裝誤差角只影響高度誤差，航向安裝誤差角影響水平定位誤差）。

圖4 仿真試驗(yàn)在線標(biāo)定結(jié)果Fig.4 Online calibration results simulation test

以仿真的SINS/GPS為定位基準(zhǔn)，四種組合導(dǎo)航算法解算的水平定位誤差結(jié)果和定位誤差隨里程變化所占百分比結(jié)果如圖5所示。從圖中可以看到，所提的三種截?cái)嗾`差補(bǔ)償算法較傳統(tǒng)速度匹配組合導(dǎo)航在定位精度上都有不同提升，驗(yàn)證了算法的有效性。

圖5 仿真試驗(yàn)水平定位誤差結(jié)果Fig.5 Horizontal positioning error results of simulation test

為了更客觀對(duì)比所提算法的性能，對(duì)圖5水平定位結(jié)果的均方根誤差（RMSE）以及RMSE占總行駛里程的百分比進(jìn)行了量化統(tǒng)計(jì)，如表1所示。傳統(tǒng)速度匹配組合導(dǎo)航的RMSE為5.8542 m，在速度觀測(cè)中補(bǔ)償里程計(jì)截?cái)嗾`差后提升了0.5437 m的定位精度；而基于脈沖觀測(cè)濾波補(bǔ)償?shù)腞MSE為3.5127 m，實(shí)現(xiàn)了定位精度的較大提升，基于脈沖觀測(cè)的高斯回歸模型預(yù)測(cè)顯示了它的優(yōu)越性能，提升到了1.5936 m，兩種算法的RMSE相對(duì)傳統(tǒng)算法分別減少了40%和72.78%。

表1 仿真水平定位誤差結(jié)果統(tǒng)計(jì)Tab.1 Statistics of simulation horizontal positioning error results

3.2 實(shí)驗(yàn)驗(yàn)證

本文還通過車載實(shí)驗(yàn)來驗(yàn)證算法的實(shí)際性能，該實(shí)驗(yàn)車配備有高精度激光慣導(dǎo)、輪式里程計(jì)和GPS，如圖6所示。

圖6 實(shí)驗(yàn)裝置Fig.6 Experimental device

IMU的輸出頻率為200 Hz，其陀螺儀零偏為0.005°/h量級(jí)，角度隨機(jī)游走系數(shù)為0.0005°/h水平；加速度計(jì)零偏為50μg量級(jí)；里程計(jì)輸出頻率為200 Hz，精度為0.1%，按照傳統(tǒng)平直路線起點(diǎn)終點(diǎn)方式標(biāo)定，得到里程計(jì)刻度系數(shù) K= 0.013034m/脈沖，以航位推算和GPS組合得到慣導(dǎo)的安裝偏差角αθ= 28.88'、 αψ=-2 4.14'；GPS接收機(jī)定位精度為5 m，輸出頻率為1 Hz。本次進(jìn)行了較長(zhǎng)里程的跑車實(shí)驗(yàn)，行車時(shí)間總計(jì)為80 min，總行程143.18 km，圖7為本次實(shí)驗(yàn)的行車軌跡。

圖7 行駛軌跡Fig.7 Traveling track

按照上述方法對(duì)慣導(dǎo)安裝誤差角和里程計(jì)刻度系數(shù)誤差在線標(biāo)定補(bǔ)償，得到車載實(shí)驗(yàn)的誤差參數(shù)如圖8所示。由圖8可知，P-kf濾波補(bǔ)償算法和Gp-kf估計(jì)更精確，收斂也快；補(bǔ)償里程截?cái)嗾`差的速度匹配組合導(dǎo)航前期相對(duì)傳統(tǒng)算法估計(jì)更精確一些，后期也能較準(zhǔn)確地估計(jì)安裝誤差角；里程計(jì)刻度系數(shù)誤差受諸多因素影響，變化無規(guī)律，每個(gè)算法補(bǔ)償也有差異。

圖8 車載實(shí)驗(yàn)在線標(biāo)定結(jié)果Fig.8 On line calibration results of vehicle-mounted experiment

如圖9所示，為捷聯(lián)慣導(dǎo)與里程計(jì)脈沖輸出的差值統(tǒng)計(jì)圖，橫坐標(biāo)為誤差范圍，縱坐標(biāo)為不同誤差所占脈沖個(gè)數(shù)，由圖9可以看出，兩者之間的脈沖差值的絕對(duì)值大多數(shù)要小于2個(gè)脈沖，所以整個(gè)跑車過程里程計(jì)工作狀態(tài)良好，當(dāng)差值的絕對(duì)值大于2時(shí)，可判斷里程計(jì)輸出不可信，采用發(fā)生故障時(shí)的補(bǔ)償方法。

圖9 慣導(dǎo)與里程脈沖輸出差值統(tǒng)計(jì)圖Fig.9 Statistical diagram of difference between inertial navigation system and odometer pulse output

以跑車的SINS/GPS組合導(dǎo)航為定位基準(zhǔn)，四種組合導(dǎo)航算法解算的水平定位誤差結(jié)果和定位誤差隨里程變化所占百分比結(jié)果如圖10所示，統(tǒng)計(jì)結(jié)果如表2所示。在140多公里的行程中，傳統(tǒng)速度匹配組合導(dǎo)航水平定位誤差為174.6372 m，在傳統(tǒng)速度匹配組合導(dǎo)航中補(bǔ)償里程計(jì)截?cái)嗾`差后就變?yōu)?59.7581 m，減少了8.52%的定位誤差，可見補(bǔ)償里程計(jì)截?cái)嗾`差是很有必要的?；诿}沖觀測(cè)的濾波補(bǔ)償和基于高斯回歸模型預(yù)測(cè)的方法凸顯了其良好性能，雖然采用的是標(biāo)準(zhǔn)的卡爾曼濾波，但水平定位誤差相對(duì)于傳統(tǒng)速度組合導(dǎo)航分別減少了82.42%和87.09%（注：基于脈沖觀測(cè)，但未補(bǔ)償里程計(jì)截?cái)嗾`差的RMSE為46.37 m，圖10中未畫出），歸結(jié)原因有兩個(gè)：其一，考慮了里程計(jì)的截?cái)嗾`差，建模更加完整，同時(shí)在航向角的估計(jì)上更加精準(zhǔn)；其二：量測(cè)的不同，傳統(tǒng)的速度觀測(cè)對(duì)里程計(jì)輸出進(jìn)行了微分處理，擴(kuò)大了誤差，以脈沖觀測(cè)的形式直接作用里程計(jì)原始輸出，利于濾波。但基于脈沖觀測(cè)的濾波補(bǔ)償在40 min-50 min期間是優(yōu)于高斯回歸模型預(yù)測(cè)的，由于高斯回歸模型預(yù)測(cè)的可靠性取決于超參數(shù)，對(duì)數(shù)據(jù)的擬合程度是有置信區(qū)間，而在此時(shí)段對(duì)里程計(jì)截?cái)嗾`差的擬合效果較差，雖然提出基于高斯回歸模型預(yù)測(cè)里程計(jì)截?cái)嗾`差的算法，驗(yàn)證了其可實(shí)施性，但并不是一勞永逸的，需要繼續(xù)對(duì)里程計(jì)截?cái)嗾`差做更深入研究。

圖10 水平定位誤差結(jié)果Fig.10 Horizontal positioning error result

表2 水平定位誤差結(jié)果統(tǒng)計(jì)Tab.2 Statistics of horizontal positioning error results

4 結(jié) 論

本文對(duì)車載慣導(dǎo)與里程計(jì)的組合導(dǎo)航進(jìn)行了研究，重點(diǎn)分析了里程計(jì)脈沖輸出的誤差特性及其補(bǔ)償算法。具體地說，在里程計(jì)刻度系數(shù)誤差和慣導(dǎo)安裝誤差角的基礎(chǔ)上考慮里程計(jì)脈沖測(cè)量誤差，制定了三種不同補(bǔ)償算法，仿真試驗(yàn)和跑車實(shí)驗(yàn)結(jié)果都表明，補(bǔ)償里程計(jì)截?cái)嗾`差后的速度匹配組合導(dǎo)航較傳統(tǒng)速度匹配組合導(dǎo)航在定位精度上能有效提升，驗(yàn)證了其補(bǔ)償里程計(jì)截?cái)嗾`差的重要性。同時(shí)，針對(duì)傳統(tǒng)算法的不足，提出效果更好的基于脈沖觀測(cè)的截?cái)嗾`差補(bǔ)償算法和基于高斯回歸的截?cái)嗾`差預(yù)測(cè)模型，兩種算法全程水平定位誤差均低于0.2%，有較高的定位精度，所提出的方法具有工程應(yīng)用價(jià)值。