王東升，何光宇，姜希會(huì)

（1. 天津航海儀器研究所，天津 300131；2. 中國(guó)船舶集團(tuán)有限公司，北京 100097）

行進(jìn)間對(duì)準(zhǔn)作為一種能顯著提高車輛機(jī)動(dòng)性的技術(shù)，而受到國(guó)內(nèi)外學(xué)者的廣泛關(guān)注。文獻(xiàn)[1]通過車輛行駛前和行駛途中停車狀態(tài)下的重力矢量，確定初始時(shí)刻載體坐標(biāo)系和中間時(shí)刻過渡坐標(biāo)系的姿態(tài)關(guān)系間接計(jì)算出姿態(tài)矩陣，完成初始對(duì)準(zhǔn)。文獻(xiàn)[2][3]在組合導(dǎo)航系統(tǒng)中使用自適應(yīng)Kalman 濾波實(shí)現(xiàn)行進(jìn)間精對(duì)準(zhǔn)。這些方法均需要在車輛行駛前進(jìn)行一段時(shí)間的靜態(tài)粗對(duì)準(zhǔn)，限制了車輛的機(jī)動(dòng)性。為解決此問題，文獻(xiàn)[4-6]將慣性系對(duì)準(zhǔn)的思想應(yīng)用在行進(jìn)間對(duì)準(zhǔn)中，但是在對(duì)準(zhǔn)過程中沒有GPS 輔助的情況下難以進(jìn)行精確的導(dǎo)航定位，因而在對(duì)準(zhǔn)結(jié)束時(shí)要通過地標(biāo)點(diǎn)進(jìn)行位置修正。文獻(xiàn)[7]提出了基于一種逆向數(shù)據(jù)處理的行進(jìn)間對(duì)準(zhǔn)方案，通過逆向?qū)Ш剿惴▽?dòng)基座初始對(duì)準(zhǔn)與位置導(dǎo)航有機(jī)結(jié)合起來，先進(jìn)行捷聯(lián)羅經(jīng)動(dòng)基座姿態(tài)初始對(duì)準(zhǔn)和傳感器采樣數(shù)據(jù)存儲(chǔ)，再利用逆向航位推算算法和正向航位推算算法獲得位置信息，解決了以往行進(jìn)間對(duì)準(zhǔn)結(jié)束后需要利用地標(biāo)點(diǎn)修正位置的問題，為捷聯(lián)慣導(dǎo)的初始對(duì)準(zhǔn)提供了一個(gè)新的方法。近年來，文獻(xiàn)[8]將逆向?qū)Ш剿惴☉?yīng)用在捷聯(lián)慣導(dǎo)的靜基座對(duì)準(zhǔn)中，可以有效縮短對(duì)準(zhǔn)時(shí)間并提高對(duì)準(zhǔn)精度。本文受文獻(xiàn)[7]的啟發(fā)，并引入逆向Kalman 濾波技術(shù)，提出了一種行進(jìn)間對(duì)準(zhǔn)方案，通過正逆向Kalman 濾波算法對(duì)采集的歷史數(shù)據(jù)進(jìn)行多次解算，取消了靜態(tài)粗對(duì)準(zhǔn)過程，節(jié)約了對(duì)準(zhǔn)時(shí)間，同時(shí)也獲得了較高的姿態(tài)和位置對(duì)準(zhǔn)精度。

1 坐標(biāo)系定義

導(dǎo)航坐標(biāo)系（n系）：原點(diǎn)on為載體中心，o n xn軸指向地理東向，o n yn軸指向地理北向，on zn軸指向天向。

載體坐標(biāo)系（b系）：原點(diǎn)ob為載體中心，o b xb軸沿載體橫軸向右，ob yb軸沿載體縱軸向前，ob zb軸沿載體立軸向上。

地心慣性系（i系）：原點(diǎn)oi為地球質(zhì)心，o i xi軸在赤道平面內(nèi)指向春分點(diǎn)，o i yi軸在赤道平面垂直于o i xi軸，oi zi軸為地球自轉(zhuǎn)軸。

初始時(shí)刻地心慣性系（i0系）：與對(duì)準(zhǔn)初始時(shí)刻地心慣性系（i系）重合。

初始時(shí)刻載體慣性系（ib0系）：與對(duì)準(zhǔn)初始時(shí)刻載體坐標(biāo)系（b系）重合。

2 行進(jìn)間對(duì)準(zhǔn)方案設(shè)計(jì)

在慣性/里程計(jì)組合導(dǎo)航系統(tǒng)中使用Kalman 濾波實(shí)現(xiàn)行進(jìn)間精對(duì)準(zhǔn)時(shí)，往往需要一段時(shí)間的靜態(tài)粗對(duì)準(zhǔn)來滿足實(shí)施Kalman 濾波的線性化條件。同時(shí)，起始階段姿態(tài)誤差大會(huì)引起較大的位置誤差，導(dǎo)致車輛需要行駛到地標(biāo)點(diǎn)進(jìn)行位置修正。

針對(duì)上述問題，本文提出了一種基于逆向Kalman濾波的行進(jìn)間對(duì)準(zhǔn)方案，在整個(gè)初始對(duì)準(zhǔn)過程中進(jìn)行數(shù)據(jù)采集和存儲(chǔ)，并將這些歷史數(shù)據(jù)同時(shí)用于粗對(duì)準(zhǔn)和精對(duì)準(zhǔn)。通過慣性系對(duì)準(zhǔn)方法，使用里程計(jì)作為外界速度測(cè)量裝置，完成行進(jìn)間粗對(duì)準(zhǔn)過程。對(duì)于已采集的數(shù)據(jù)信息，利用導(dǎo)航計(jì)算機(jī)的高速運(yùn)算能力通過逆向?qū)Ш剿惴▽⑾到y(tǒng)狀態(tài)在短時(shí)間內(nèi)由粗對(duì)準(zhǔn)結(jié)束時(shí)刻t1 逆向解算至對(duì)準(zhǔn)起始時(shí)刻t0（起始時(shí)刻車輛為靜止?fàn)顟B(tài)），同時(shí)使用逆向Kalman 濾波回溯估算出的姿態(tài)信息結(jié)合起點(diǎn)處已知的速度位置信息對(duì)慣導(dǎo)進(jìn)行校正，完成逆向精對(duì)準(zhǔn)過程并得到具有一定精度的姿態(tài)信息。緊接著進(jìn)行快速正向組合導(dǎo)航使系統(tǒng)狀態(tài)追趕上正常的實(shí)時(shí)計(jì)算直至對(duì)準(zhǔn)結(jié)束時(shí)刻t2，完成全部精對(duì)準(zhǔn)過程并獲得較高精度的姿態(tài)和位置信息。整個(gè)行進(jìn)間對(duì)準(zhǔn)方案如圖1 所示。

圖1 行進(jìn)間對(duì)準(zhǔn)方案Fig.1 The scheme of in-motion alignment

2.1 粗對(duì)準(zhǔn)

式中，表示導(dǎo)航系n到初始時(shí)刻慣性系i0的變換矩陣，通過對(duì)準(zhǔn)時(shí)的實(shí)時(shí)位置變化和地球自轉(zhuǎn)角速度ωie確定，由于粗對(duì)準(zhǔn)過程中難以獲得準(zhǔn)確的位置信息，在車輛行駛距離不長(zhǎng)的情況下忽略位置變化的影響并使用初始時(shí)刻的位置代替計(jì)算。表示初始時(shí)刻載體慣性系ib0到載體坐標(biāo)系b的變換矩陣，由陀螺輸出的角速率得到。示初始時(shí)刻慣性系i0到初始時(shí)刻載體慣性系ib0的變換矩陣，容易通過加速度計(jì)輸出fb、里程計(jì)速度和重力矢量gn之間的轉(zhuǎn)換關(guān)系求得。

2.2 逆向?qū)Ш胶秃轿煌扑闼惴?/h3>

在實(shí)際計(jì)算時(shí)，捷聯(lián)慣導(dǎo)的姿態(tài)、速度和位置更新方程一般可表示為

為得到逆向解算時(shí)的慣導(dǎo)姿態(tài)、速度和位置更新方程[7]，對(duì)式(2)-(6)進(jìn)行移項(xiàng)變換，如下：

對(duì)比式(2)-(6)和式(7)-(11)可知，通過對(duì)地球自轉(zhuǎn)角速度導(dǎo)航系相對(duì)地球的旋轉(zhuǎn)角速度、重力加速度gn、陀螺輸出的角速率加速度計(jì)輸出的比力fb和位置更新方程中的位置變化量都進(jìn)行取反處理，實(shí)現(xiàn)了逆向?qū)Ш健?/p>

對(duì)正向航位推算算法進(jìn)行簡(jiǎn)單的變換即可得到逆向航位推算算法，如下所示：

通過以上推導(dǎo)不難發(fā)現(xiàn)，正向與逆向?qū)Ш竭^程中相同時(shí)刻的姿態(tài)、速度、位置都可基本保持一致，便可對(duì)同一段導(dǎo)航數(shù)據(jù)行進(jìn)重復(fù)解算，并利用起點(diǎn)處的已知信息進(jìn)行校正，來提高對(duì)準(zhǔn)精度。

2.3 逆向?qū)Ш较到y(tǒng)誤差模型與逆向Kalman 濾波模型

根據(jù)捷聯(lián)慣導(dǎo)誤差方程對(duì)逆向?qū)Ш较到y(tǒng)的誤差方程進(jìn)行推導(dǎo)[9]，過程如下：

實(shí)際計(jì)算的姿態(tài)矩陣與理想姿態(tài)矩陣存在小角度的姿態(tài)誤差角φ，那么可表示為：

計(jì)算姿態(tài)矩陣一般通過如下微分方程求解：

由上文可知，實(shí)現(xiàn)逆向?qū)Ш竭^程需要對(duì)陀螺輸出角速度和導(dǎo)航系的旋轉(zhuǎn)角速度進(jìn)行取反，即

將式(18)(19)和式(16)的微分代入式(17)中，得

對(duì)式(21)進(jìn)行化簡(jiǎn)便得逆向?qū)Ш降膽T導(dǎo)姿態(tài)誤差方程，如下所示：

根據(jù)逆向?qū)Ш降乃俣雀路匠淌?8)，逆向?qū)Ш降膽T導(dǎo)速度微分方程可以表示為

實(shí)際的計(jì)算速度通過式(24)得到

將式(16)代入式(25)中，展開得

對(duì)式(26)進(jìn)行化簡(jiǎn)，略去二階小量誤差和重力誤差δ gn，得到逆向?qū)Ш降乃俣日`差方程如下：

由式(9)-(11)可知，逆向?qū)Ш降奈恢酶挛⒎址匠炭蓪憺?/p>

通過位置更新微分方程式(28)-(30)可得逆向?qū)Ш降奈恢谜`差方程如下：

綜合上文推導(dǎo)的逆向?qū)Ш降恼`差方程，考慮到里程計(jì)的安裝誤差角和標(biāo)度因數(shù)會(huì)受到車輛的振動(dòng)、輪胎的形變等因素的影響。相應(yīng)的慣性/里程計(jì)逆向組合導(dǎo)航系統(tǒng)的系統(tǒng)誤差方程如下：

式中，φ、δ v和δ p分別表示東北天三個(gè)方向上慣導(dǎo)的姿態(tài)誤差、速度誤差和位置誤差，ε和?分別為陀螺漂移和加速度計(jì)零位誤差，δ pD表示航位推算的位置誤差，δ K為里程計(jì)的標(biāo)度因數(shù)誤差，α ψ為里程計(jì)和慣導(dǎo)的安裝方位誤差。

根據(jù)式(34)可推出慣性/里程計(jì)組合導(dǎo)航系統(tǒng)的逆向Kalman 濾波模型為

其中，X為20 維的狀態(tài)變量,Z為觀測(cè)向量，F(xiàn)和H分別表示系統(tǒng)矩陣和觀測(cè)矩陣，G為噪聲分配矩陣，W和V分別表示系統(tǒng)噪聲和觀測(cè)噪聲。它們定義如下：

狀態(tài)變量

逆向Kalman 濾波中的系統(tǒng)矩陣分為兩部分，分別由捷聯(lián)慣導(dǎo)的誤差方程和航位推算的誤差方程得出，如式(37)-(39)所示。

噪聲分配矩陣

系統(tǒng)噪聲

觀測(cè)向量

觀測(cè)矩陣

根據(jù)以上公式，通過逆向Kalman 濾波方法將慣導(dǎo)和里程計(jì)進(jìn)行組合，對(duì)系統(tǒng)狀態(tài)進(jìn)行逆向解算便可得到較為準(zhǔn)確的姿態(tài)信息。

3 車載行進(jìn)間對(duì)準(zhǔn)實(shí)驗(yàn)

為驗(yàn)證該行進(jìn)間對(duì)準(zhǔn)方案的有效性，本文開展了車載行進(jìn)間對(duì)準(zhǔn)實(shí)驗(yàn)。實(shí)驗(yàn)設(shè)備主要由激光捷聯(lián)慣導(dǎo)、里程計(jì)、GPS 和獵豹實(shí)驗(yàn)車組成。其中激光陀螺的精度為0.002 °/h ，加速度計(jì)的精度為50 ug，GPS 定位誤差小于0.1 m，慣性元件的采樣頻率和慣導(dǎo)解算頻率均為200 Hz。實(shí)驗(yàn)過程如下：

（1）車輛靜止時(shí)啟動(dòng)GPS，記錄起點(diǎn)處的位置信息。

（2）啟動(dòng)慣導(dǎo)，開始行進(jìn)間粗對(duì)準(zhǔn)的同時(shí)進(jìn)行數(shù)據(jù)存儲(chǔ)，車輛行駛速度約為30 km/h 至50 km/h。

（3）車輛行駛到500 s 時(shí)，開始逆向精對(duì)準(zhǔn)，導(dǎo)航計(jì)算機(jī)進(jìn)行高速運(yùn)算，將車輛的狀態(tài)回溯至初始時(shí)刻，并對(duì)慣導(dǎo)進(jìn)行校正，整個(gè)過程耗時(shí)約40 s，期間車輛繼續(xù)行駛。

（4）逆向精對(duì)準(zhǔn)結(jié)束后，再進(jìn)行約60 s 的正向組合導(dǎo)航直至行進(jìn)間對(duì)準(zhǔn)結(jié)束，整個(gè)過程中導(dǎo)航計(jì)算機(jī)仍保持高速運(yùn)算，確保在對(duì)準(zhǔn)結(jié)束前追趕上系統(tǒng)正常的實(shí)時(shí)計(jì)算。

（5）車輛行駛到600 s 對(duì)準(zhǔn)完成時(shí)停車，得到此時(shí)的航向ψ′和位置p′，再多次靜基座對(duì)準(zhǔn)得到均值ψ，將ψ作為參考航向基準(zhǔn)，并記錄終點(diǎn)處的位置信息p作為參考定位基準(zhǔn)。

行車軌跡如圖2 所示，全程約7 km，道路為柏油馬路。

圖2 行車軌跡Fig.2 Test trajectory

圖3、圖4 和圖5 為整個(gè)行進(jìn)間對(duì)準(zhǔn)過程中三個(gè)姿態(tài)失準(zhǔn)角的Kalman 濾波估計(jì)曲線。

圖3 北向失準(zhǔn)角估計(jì)曲線Fig.3 Estimation of north misalignment

圖4 東向失準(zhǔn)角估計(jì)曲線Fig.4 Estimation of east misalignment

圖5 天向失準(zhǔn)角估計(jì)曲線Fig.5 Estimation of azimuth misalignment

從圖3-5 可以看出，利用逆向Kalman 濾波回溯至初始時(shí)刻，用估計(jì)結(jié)果對(duì)慣導(dǎo)進(jìn)行校正后，修正了大部分的姿態(tài)失準(zhǔn)角，之后的正向組合對(duì)準(zhǔn)過程中，濾波曲線收斂，可以保證對(duì)準(zhǔn)精度。一共進(jìn)行了9 組行進(jìn)間對(duì)準(zhǔn)實(shí)驗(yàn)，實(shí)驗(yàn)結(jié)果如表1 所示。

表1 行進(jìn)間對(duì)準(zhǔn)實(shí)驗(yàn)結(jié)果Tab.1 Results of in-motion alignment

由表1 可知，應(yīng)用逆向Kalman 濾波方案，系統(tǒng)行進(jìn)間對(duì)準(zhǔn)的航向誤差為0.042°(σ1 )，位置誤差為5.99 m(RMS)。

4 結(jié) 論

本文將逆向數(shù)據(jù)處理的思維方式引入到行進(jìn)間對(duì)準(zhǔn)中，通過對(duì)歷史采樣數(shù)據(jù)的分時(shí)復(fù)用，在粗對(duì)準(zhǔn)結(jié)果的基礎(chǔ)上進(jìn)行逆向精對(duì)準(zhǔn)和正向組合導(dǎo)航，完成了兩次精對(duì)準(zhǔn)過程。不僅取消了傳統(tǒng)的靜態(tài)粗對(duì)準(zhǔn)過程，而且提高了定向和定位精度，保證了在粗對(duì)準(zhǔn)較差的情況下仍能獲得較高精度的姿態(tài)和位置信息。最后，通過車載對(duì)準(zhǔn)實(shí)驗(yàn)驗(yàn)證了所提出的行進(jìn)間對(duì)準(zhǔn)方案的正確性和有效性。該算法的提出為后續(xù)的工程應(yīng)用奠定了基礎(chǔ)。