韓勇強(qiáng)，王新健，孫壽才，陳家斌

（1.北京理工大學(xué)，北京 100081；2.北京航天發(fā)射技術(shù)研究所，北京 100076）

基于純慣性解算的導(dǎo)航方法是地面移動平臺的一種重要工作模式，其不受地面條件、電磁干擾等外部因素的影響，具有高度的自主性與可靠性，但導(dǎo)航誤差隨時間累積的問題較為顯著。零速修正是提高陸用平臺慣性導(dǎo)航系統(tǒng)定位定向精度的一種廉價且有效的方法[1]。傳統(tǒng)的零速修正方法包括二次曲線擬合以及卡爾曼濾波方法等[2-5]，這些方法需要每間隔一段時間進(jìn)行停車修正，且行進(jìn)過程中的定位精度發(fā)散趨勢較為明顯。在卡爾曼濾波方法基礎(chǔ)上，文獻(xiàn)[6-8]從不同角度提出了一種基于運(yùn)動學(xué)約束的車載捷聯(lián)慣性導(dǎo)航誤差阻尼算法。該算法利用車體運(yùn)動信息構(gòu)建觀測方程，實(shí)現(xiàn)了車體運(yùn)動狀態(tài)的不完整約束。但由于實(shí)際車體運(yùn)行時，運(yùn)動約束條件難以嚴(yán)格保證，可能導(dǎo)致卡爾曼濾波發(fā)散，且由于非完整約束導(dǎo)致縱向速度無法準(zhǔn)確估計，在某些特定形式狀態(tài)（如長時間同向行駛）時誤差阻尼效果不佳。因此本文在動態(tài)零速修正算法（DZUPT，Dynamic Zero velocity update）的基礎(chǔ)上提出了一種結(jié)合徑向誤差曲線擬合的自適應(yīng)動態(tài)零速修正算法（CA-DZUPT，Curve-fitting and Aaptive Dynamic Zero velocity update），提高車載捷聯(lián)慣導(dǎo)長時間行駛定位精度。經(jīng)車載實(shí)驗(yàn)證明，在每間隔15分鐘停車一次的條件下，定位精度可以達(dá)到10 m（CEP）。

1 DZUPT 算法

1.1 動態(tài)約束條件

理想情況下，當(dāng)車體處于運(yùn)動狀態(tài)時，只要不發(fā)生跳躍和側(cè)滑，車體坐標(biāo)系內(nèi)其天向與側(cè)向的速度為0。

其中m為車體坐標(biāo)系，x，y分別指向車體右側(cè)及正前方，z軸垂直于x，y軸并形成右手坐標(biāo)系。

1.2 算法模型

選取姿態(tài)誤差、速度誤差、位置誤差、陀螺漂移、加速度計零偏及慣導(dǎo)安裝誤差角誤差（俯仰與航向誤差角）為17 維觀測量設(shè)計卡爾曼濾波器。

假設(shè)陀螺漂移及加速度計零偏均為常值，則有以下系統(tǒng)誤差方程[9]:

式中，

式(4)中Mxx為捷聯(lián)慣導(dǎo)系統(tǒng)狀態(tài)方程中的對應(yīng)項(xiàng)，其含義為

其中，RM和RN分別為子午圈及卯酉圈曲率半徑，ωie為地速，L為當(dāng)?shù)鼐暥龋瑅E、vN為東西向速度。

慣導(dǎo)系統(tǒng)輸出的導(dǎo)航系（取東北天坐標(biāo)系）速度為Vn，姿態(tài)轉(zhuǎn)換矩陣為，由于慣導(dǎo)安裝誤差角的存在，車體坐標(biāo)系的速度分量可按式(6)計算:

其中，n表示東北天地理坐標(biāo)系，b表示捷聯(lián)慣導(dǎo)體坐標(biāo)系，m表示車體坐標(biāo)系。

可利用車體坐標(biāo)系x軸速度和z軸速度為零的約束條件構(gòu)成量測值:

考慮到慣導(dǎo)安裝誤差角的剩余誤差為小量，可以得到

結(jié)合式(8)，對式(6)進(jìn)行全微分可得

動態(tài)條件下濾波器的量測方程為

其中V為等效量測噪聲，H為量測矩陣，其定義為

式中M1(1,:)表示矩陣M1的第一行，其它類似形式含義相似。

在車體實(shí)際行駛過程中，由于慣導(dǎo)不可能與車體運(yùn)動中心完全重合，除了慣導(dǎo)與車體之間存在安裝誤差角，還存在外桿臂效應(yīng)。平穩(wěn)行駛外桿臂影響較小，可忽略不計，轉(zhuǎn)彎幅度較大時，可以考慮不進(jìn)行量測更新或?qū)⑼鈼U臂擴(kuò)充為狀態(tài)變量，此處不再贅述。

2 CA-DZUPT 算法原理

由動態(tài)零速修正的約束條件可知，卡爾曼濾波只對車體的側(cè)向以及天向速度誤差進(jìn)行觀測，當(dāng)載體處于長直路線行駛時，徑向速度誤差會產(chǎn)生顯著積累而無法消除，導(dǎo)致定位精度變差。由此想到，可以在DZUPT 基礎(chǔ)上利用自適應(yīng)噪聲以及曲線擬合的方法將定位誤差擬合出來，在車體停止時將其補(bǔ)償回輸出值。

2.1 零速檢測

準(zhǔn)確地獲取零速區(qū)間是進(jìn)行零速修正的關(guān)鍵。本文采用角速度檢測（ARED）方法，通過獲取陀螺儀的數(shù)據(jù)對車體的零速進(jìn)行判定[10]。若車體處于零速區(qū)間，需滿足:

誤判率和可探測率通常是評估零速探測器性能優(yōu)劣的兩項(xiàng)指標(biāo)，其含義做如下定義:

可探測率P(S)及誤判率P(F)兩項(xiàng)指標(biāo)可以用來評判零速區(qū)間探測器的性能。可探測率P(S)及誤判率P(F)可按照式(13)計算得出:

其中，len（*）代表所用實(shí)驗(yàn)數(shù)據(jù)集中數(shù)據(jù)的長度，D為實(shí)際的零速區(qū)間數(shù)據(jù),D1為檢測到的正確的零速區(qū)間數(shù)據(jù)，D2為被誤判為零速區(qū)間數(shù)據(jù)的非零速數(shù)據(jù)，D′為由零速檢測器檢測到的零速區(qū)間數(shù)據(jù)。

經(jīng)過不同窗口長度實(shí)驗(yàn)測試，該方法可行且檢測準(zhǔn)確。本文系統(tǒng)某次實(shí)驗(yàn)的零速檢測結(jié)果如表1所示。

表1 不同窗口長度的影響Tab.1 Effects of different lengths of window

2.2 Sage-Husa 自適應(yīng)卡爾曼濾波

動態(tài)零速修正過程采用Sage-Husa 自適應(yīng)卡爾曼濾波方法，它是一種次優(yōu)無偏后驗(yàn)噪聲估計算法，能夠利用量測過程中采集到的數(shù)據(jù)，對系統(tǒng)的系統(tǒng)噪聲和量測噪聲進(jìn)行實(shí)時的估計[11]。而動態(tài)零速修正過程中，約束條件不完整，慣導(dǎo)系統(tǒng)模型存在誤差，量測噪聲難以選擇，不合適的參數(shù)易導(dǎo)致濾波發(fā)散。

系統(tǒng)噪聲主要由系統(tǒng)本身結(jié)構(gòu)決定，受環(huán)境影響較??；而量測噪聲受外界環(huán)境影響較大。為解決卡爾曼濾波零速修正算法在行駛、停車等不同狀態(tài)條件下的濾波器參數(shù)問題，本文采用Sage-Husa 自適應(yīng)卡爾曼濾波方法對量測噪聲進(jìn)行實(shí)時自適應(yīng)估計。

首先確定自適應(yīng)估計的加權(quán)系數(shù)

其中b為遺忘因子，本文中設(shè)為0.9。由式（14）可知，隨著時間推移，dt逐漸接近1-b。

量測噪聲的實(shí)時估計為

其中?t為殘差，Pt+1∣t為一步預(yù)測均方誤差矩陣。Rt在更新過程中始終保持正定。

2.3 徑向速度誤差曲線擬合

在實(shí)驗(yàn)過程中我們發(fā)現(xiàn)，基于自適應(yīng)卡爾曼濾波的動態(tài)零速修正算法在長時間沿直線行駛過程中，其誤差修正效果相對于有航向變化的線路有明顯的下降，而在停車時的靜態(tài)零速修正作用也不顯著。通過理論分析，其主要原因在于動態(tài)零速約束算法的不完整性，缺乏載體行駛方向速度誤差的有效阻尼，從而導(dǎo)致徑向?qū)Ш秸`差的累積。而在停車時，雖然能夠準(zhǔn)確觀測到徑向速度誤差，但由于濾波器模型并未考慮徑向殘差，導(dǎo)致難以實(shí)現(xiàn)累積位置誤差的準(zhǔn)確估計。

本文提出了一種基于徑向速度誤差曲線擬合的零速修正方法，利用停車修正環(huán)節(jié)，可以實(shí)現(xiàn)對動態(tài)零速修正過程中由徑向速度殘差所導(dǎo)致定位誤差的有效補(bǔ)償。

曲線擬合法是指將速度誤差變化過程擬合成一條曲線，再將此曲線進(jìn)行積分得到動態(tài)零速修正后的位置誤差校正值。由于采取純慣性工作模式，車體行駛過程中沒有精確的外部速度參考，無法獲取速度誤差，因此利用零速檢測器檢測到停車時，用此時的速度輸出值作為速度誤差進(jìn)行計算。假設(shè)停車時刻為T，此時的徑向速度誤差殘差為ΔV，位置誤差ΔP可表示為

由于動態(tài)零速修正過程對慣導(dǎo)側(cè)向以及天向速度已經(jīng)進(jìn)行了補(bǔ)償，導(dǎo)航系內(nèi)的速度已經(jīng)不再滿足舒拉震蕩特點(diǎn)，此時必須針對慣導(dǎo)坐標(biāo)系縱向速度誤差導(dǎo)致的位置誤差進(jìn)行建模擬合。圖1為某次實(shí)驗(yàn)的長直路段一個停車周期（15 min）內(nèi)慣導(dǎo)系縱向速度誤差的變化情況。

圖1 一個停車周期內(nèi)慣導(dǎo)徑向速度變化Fig.1 Radial velocity change of SINS during one stop cycle

由圖1可以看出，縱向速度誤差走勢可以用高次曲線進(jìn)行近似擬合。由于停車間隔較長，所以將模型進(jìn)行了簡化。以n次曲線為例，假設(shè)0 時刻經(jīng)過零速補(bǔ)償?shù)乃俣日`差為0，那么τ時刻的速度誤差δ vb(τ)可表示為

其中冪次常數(shù)n=1,2 …N，則參數(shù)，其中ΔV為停車時刻觀測的徑向速度誤差，T為本次停車與上次零速修正的時間間隔，則式(16)可以轉(zhuǎn)化為

其中Δτ為慣導(dǎo)解算更新周期。由于停車時刻ΔV與T為已知量，只需在車體處于運(yùn)動狀態(tài)時持續(xù)計算，待到停車時刻得到ΔV，進(jìn)行修正計算出ΔP即可。

在實(shí)際測試時發(fā)現(xiàn)，由于停車間隔內(nèi)徑向速度誤差的累積特性比較復(fù)雜，二次曲線并不能很好地擬合出徑向速度誤差的變化，其估計出的位置誤差絕對值偏大，需要采取其它曲線模型進(jìn)行擬合。仿照上述推算過程，使用3 次、4 次等高次曲線進(jìn)行擬合，最終發(fā)現(xiàn)當(dāng)n=4 時位置誤差補(bǔ)償效果最佳，并使用多組實(shí)驗(yàn)對其效果進(jìn)行了驗(yàn)證。

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

3.1 實(shí)驗(yàn)設(shè)備組成

實(shí)驗(yàn)所用設(shè)備如圖2所示。

實(shí)驗(yàn)主要設(shè)備由高精度激光陀螺捷聯(lián)慣導(dǎo)系統(tǒng)、與高精度實(shí)時差分GPS 組成。慣導(dǎo)采用高精度激光陀螺雙軸轉(zhuǎn)位捷聯(lián)慣性導(dǎo)航系統(tǒng)，系統(tǒng)參數(shù)如表2所示。

圖2 實(shí)驗(yàn)設(shè)備Fig.2 Devices for experiments

表2 陀螺儀和加速度計基本參數(shù)Tab.2 Basic parameters of gyroscope and accelerometer

GPS 實(shí)時記錄車體經(jīng)緯度坐標(biāo)，作為標(biāo)準(zhǔn)參考位置，其定位誤差為0.01 m。與CA-DZUPT 定位結(jié)果進(jìn)行對比，計算慣導(dǎo)定位精度。

3.2 實(shí)驗(yàn)路線

實(shí)驗(yàn)路線全程35 km，共行駛約90 分鐘，實(shí)驗(yàn)路線圖如圖3所示。

圖3 實(shí)驗(yàn)路線Fig.3 The route of experiments

圖中圓形標(biāo)注點(diǎn)為停車點(diǎn)，相鄰?fù)＼圏c(diǎn)時間間隔約為15～17 分鐘，路程約為6.5 km。

3.3 實(shí)驗(yàn)結(jié)果

本文實(shí)驗(yàn)共進(jìn)行五組，實(shí)驗(yàn)前首先將激光系統(tǒng)進(jìn)行約5 min 的初始對準(zhǔn)，然后沿圖3所示路線行駛，并將各組實(shí)驗(yàn)定位誤差進(jìn)行記錄。圖4為不進(jìn)行零速修正，天向速度約束條件下慣導(dǎo)定位誤差圖。

圖5為僅使用DZUPT 方法時，第一組實(shí)驗(yàn)的定位誤差。由定位誤差曲線可以看到，當(dāng)僅采用DZUPT算法時，雖然動態(tài)定位誤差得到了較好的抑制，但由于采用了速度、位置實(shí)時反饋的補(bǔ)償策略，狀態(tài)量δvnδp,濾波器增益K保持在較小范圍，從而在停車時，雖然能夠觀測到較明顯的速度誤差，但對位置誤差的補(bǔ)償幅度很小。

圖4 無零速修正定位誤差Fig.4 Positioning error without zero velocity update

圖5 DZUPT 實(shí)驗(yàn)定位誤差Fig.5 Experiment positioning error of DZUPT

圖6～7 為使用CA-DZUPT 算法修正，第一組實(shí)驗(yàn)的定位誤差，虛線標(biāo)注區(qū)間為圖3中停車點(diǎn)。

圖6 CA-DZUPT 實(shí)驗(yàn)定位誤差Fig.6 Positioning error of CA-DZUPT experiment

圖3中表明，第二個停車點(diǎn)到第三個停車點(diǎn)（對應(yīng)2000 s～3000 s 的時間區(qū)間）之間為東西方向的一條長直路線。由圖7中的實(shí)驗(yàn)結(jié)果可知，在該時間段，東向誤差快速增長，而北向定位誤差保持在很小范圍。最后一個停車點(diǎn)處情況類似，只是北向誤差增長較快，東向誤差較平穩(wěn)。該現(xiàn)象同時也體現(xiàn)出動態(tài)零速修正算法對側(cè)向?qū)Ш秸`差的抑制效果較好，而對徑向誤差的累積較難補(bǔ)償。

圖7 CA-DZUPT 實(shí)驗(yàn)東向、北向定位誤差Fig.7 East and north positioning errors of CA-DZUPT experiment

按照上述實(shí)驗(yàn)方法，共進(jìn)行了5 組對比實(shí)驗(yàn)，結(jié)果見表3。

表3 15 分鐘停車間隔兩種算法對比實(shí)驗(yàn)定位誤差（DZ：DZUPT，CA：CA-DZUPT，單位：m）Tab.3 Positioning error of comparison experiments of two algorithms under a 15-minute stop interval（DZ：DZUPT，CA：CA-DZUPT，Unit:m）

從實(shí)驗(yàn)結(jié)果可以看出，使用DZUPT算法修正后，動態(tài)條件下的定位精度有顯著提高，但停車對導(dǎo)航誤差修正效果沒有顯著提升。而使用CA-DZUPT 算法后，定位精度大幅提高，達(dá)到15 min 零速間隔10 m（CEP）。

4 總 結(jié)

本文提出了一種基于自適應(yīng)卡爾曼濾波與徑向速度殘差結(jié)合曲線擬合的車載激光陀螺捷聯(lián)慣導(dǎo)自適應(yīng)動態(tài)復(fù)合零速修正算法。在DZUPT 算法的基礎(chǔ)上，引入Sage-Husa 自適應(yīng)卡爾曼濾波，解決了量測噪聲自適應(yīng)調(diào)整的問題，提高了動態(tài)修正時濾波算法的魯棒性；由實(shí)驗(yàn)數(shù)據(jù)出發(fā)，分析了動態(tài)零速修正過程中的徑向速度殘差問題，提出了基于高次曲線擬合的徑向速度殘差補(bǔ)償算法。經(jīng)多組車載實(shí)驗(yàn)驗(yàn)證，本文提出的CA-DZUPT 算法穩(wěn)定有效，系統(tǒng)15 min 零速修正間隔條件下定位精度達(dá)到了10 m（CEP），算法具有重要的理論意義與工程價值。