基于狀態(tài)變換的DVL輔助SINS初始對(duì)準(zhǔn)方法

王連釗，徐 博，李盛新，李漢領(lǐng)

（哈爾濱工程大學(xué) 智能科學(xué)與工程學(xué)院，哈爾濱 150001）

初始對(duì)準(zhǔn)即賦予慣性導(dǎo)航系統(tǒng)初始的姿態(tài)、速度、位置信息的過(guò)程，其對(duì)慣性導(dǎo)航系統(tǒng)的導(dǎo)航精度有重要的影響，初始對(duì)準(zhǔn)技術(shù)通常有快速性、精確性的性能要求，隨著慣性器件及信息融合算法的發(fā)展，這兩項(xiàng)指標(biāo)不斷提升，但快速性與精確性的相互約束也越來(lái)越明顯，通常要根據(jù)不同工況進(jìn)行取舍，在惡劣條件下甚至不能滿足任一項(xiàng)[1,2]。SINS初始對(duì)準(zhǔn)主要包括基于經(jīng)典控制理論的羅經(jīng)回路方法、矢量定姿方法及基于最優(yōu)估計(jì)理論的濾波方法。其中羅經(jīng)回路方法在靜態(tài)環(huán)境下能通過(guò)調(diào)節(jié)阻尼系數(shù)方式根據(jù)對(duì)準(zhǔn)精度的需求調(diào)整對(duì)準(zhǔn)時(shí)間，能夠很好地實(shí)現(xiàn)SINS快速初始對(duì)準(zhǔn)，然而并不適用于較大的機(jī)動(dòng)環(huán)境。矢量定姿方法是目前常用的短時(shí)間對(duì)準(zhǔn)方法，因?yàn)殡y以實(shí)現(xiàn)器件誤差的建模，所以該方法無(wú)法突破初始對(duì)準(zhǔn)的極限精度。現(xiàn)有的初始對(duì)準(zhǔn)中更多采用“粗對(duì)準(zhǔn)”+“精對(duì)準(zhǔn)”兩階段對(duì)準(zhǔn)的方案[3-5]，其中粗對(duì)準(zhǔn)通常采用解析計(jì)算方法，所需時(shí)間短但對(duì)準(zhǔn)精度較低，主要用來(lái)為精對(duì)準(zhǔn)提供較為準(zhǔn)確的姿態(tài)初值，令SINS誤差模型近似線性[6]；精對(duì)準(zhǔn)則多采用卡爾曼濾波算法，通過(guò)輔助信息完成誤差狀態(tài)的估計(jì)。

對(duì)于潛航器水下啟動(dòng)，通常僅利用DVL提供輔助信息，由于缺少衛(wèi)星導(dǎo)航的輔助信息，僅能在較為粗略的初始位置及速度信息基礎(chǔ)上結(jié)合DVL測(cè)量速度信息完成初始對(duì)準(zhǔn)。在輔助信息不夠準(zhǔn)確時(shí)，機(jī)動(dòng)環(huán)境下容易造成矢量定姿方法的粗對(duì)準(zhǔn)精度不夠理想，將直接影響系統(tǒng)誤差模型的精度，基于卡爾曼濾波的精對(duì)準(zhǔn)精度也將下降，導(dǎo)致SINS難以完成快速、高精度動(dòng)基座對(duì)準(zhǔn)。現(xiàn)有對(duì)粗對(duì)準(zhǔn)方法研究主要集中在對(duì)慣性系對(duì)準(zhǔn)的優(yōu)化方案，其缺陷是在動(dòng)態(tài)環(huán)境下對(duì)外部輔助信息不加判別地利用，將輔助信息誤差引入姿態(tài)誤差中，造成對(duì)準(zhǔn)精度較低[7]。針對(duì)上述問(wèn)題，文獻(xiàn)[8]提出了一種DVL異常值檢查和隔離方法，有效地解決了輔助信息不準(zhǔn)確對(duì)初始對(duì)準(zhǔn)的影響。文獻(xiàn)[9]在優(yōu)化慣性系基礎(chǔ)上改進(jìn)，提出了基于非線性濾波的精對(duì)準(zhǔn)方法，在對(duì)準(zhǔn)過(guò)程中可實(shí)時(shí)更新速度、位置信息。但以上兩種方法中未對(duì)慣性器件誤差建模，降低了對(duì)準(zhǔn)極限精度。文獻(xiàn)[10]針對(duì)大初始誤差角的初始對(duì)準(zhǔn)非線性問(wèn)題提出了一種基于平方根容積信息濾波方法，然而容積濾波器計(jì)算量大，不適用于實(shí)際工程。文獻(xiàn)[11-15]指出在導(dǎo)航坐標(biāo)系與計(jì)算導(dǎo)航坐標(biāo)系存在較大差異時(shí)，傳統(tǒng)慣性系誤差中速度誤差的定義將不能完整描述出當(dāng)前導(dǎo)航誤差狀態(tài)，若以速度匹配方式進(jìn)行組合導(dǎo)航，將導(dǎo)致對(duì)準(zhǔn)效率較低，并總結(jié)概括為模型不精確所導(dǎo)致的協(xié)方差不一致問(wèn)題，這點(diǎn)在大初始失準(zhǔn)角條件下表現(xiàn)更為突出，同時(shí)文獻(xiàn)中給出了基于導(dǎo)航坐標(biāo)系的狀態(tài)變換的初始對(duì)準(zhǔn)方法。

本文針對(duì)上述系統(tǒng)誤差模型不精確所導(dǎo)致的協(xié)方差不一致問(wèn)題給出一種新的解決方案，提出了基于狀態(tài)變換的SINS初始對(duì)準(zhǔn)方法。本文推導(dǎo)了基于載體系速度誤差方程的SINS系統(tǒng)誤差模型；構(gòu)建了基于狀態(tài)變換的SINS初始對(duì)準(zhǔn)濾波模型；最后通過(guò)數(shù)值仿真及湖面實(shí)驗(yàn)驗(yàn)證了所提對(duì)準(zhǔn)方法的優(yōu)越性。

1 SINS速度誤差問(wèn)題描述

傳統(tǒng)的捷聯(lián)慣導(dǎo)系統(tǒng)速度誤差方程如下：

其中速度誤差的定義為導(dǎo)航計(jì)算機(jī)解算速度與真實(shí)的導(dǎo)航坐標(biāo)系速度的差值：

式(2)中速度誤差定義僅反映了向量間大小的差異，卻忽略了兩者坐標(biāo)系之間的區(qū)別，僅在失準(zhǔn)角較小時(shí)能準(zhǔn)確地描述SINS誤差狀態(tài)。若在粗對(duì)準(zhǔn)未能達(dá)到理想的對(duì)準(zhǔn)精度，后續(xù)的精對(duì)準(zhǔn)將受到較大影響，需要較長(zhǎng)的時(shí)間完成高精度的對(duì)準(zhǔn)，不符合初始對(duì)準(zhǔn)的快速性要求。文獻(xiàn)[11-15]中描述為模型定義不準(zhǔn)確導(dǎo)致的協(xié)方差不一致問(wèn)題，并針對(duì)速度誤差定義不嚴(yán)格問(wèn)題對(duì)導(dǎo)航坐標(biāo)系下速度誤差進(jìn)行了嚴(yán)謹(jǐn)?shù)亩x，重新推導(dǎo)了新的速度誤差定義下的導(dǎo)航系統(tǒng)速度誤差方程，新的速度誤差定義如下：

可以看出其中與傳統(tǒng)誤差模型的主要區(qū)別在于速度誤差中包含-×φn項(xiàng)，其物理含義為解算速度的角度偏差，主要由失準(zhǔn)角引起。

2 基于載體系速度誤差的SINS誤差方程

式(3)反映了更為嚴(yán)謹(jǐn)?shù)膶?dǎo)航坐標(biāo)系下速度誤差，然而在DVL輔助SINS進(jìn)行導(dǎo)航及對(duì)準(zhǔn)時(shí)，直接獲取的是載體坐標(biāo)系速度信息作為參考，現(xiàn)有方案是利用SINS姿態(tài)信息將DVL速度投影至導(dǎo)航坐標(biāo)系，或?qū)INS速度信息投影至DVL坐標(biāo)系，再進(jìn)行信息融合，這兩種方案也引入了部分失準(zhǔn)角誤差及速度誤差，導(dǎo)致了匹配信息不一致問(wèn)題，誤差狀態(tài)協(xié)方差由于模型的不準(zhǔn)確的原因不能準(zhǔn)確地反映真實(shí)的誤差狀態(tài)。為解決以上問(wèn)題，本文將推導(dǎo)新的基于載體坐標(biāo)系速度信息的導(dǎo)航誤差方程及誤差方程，采用慣導(dǎo)解算出載體坐標(biāo)系速度信息，結(jié)合DVL測(cè)速信息完成匹配。

將vn=vb代入比力方程，整理比力方程為如下形式：

根據(jù)式(4)，SINS計(jì)算載體坐標(biāo)系速度微分方程為：

定義載體系速度誤差為δvb=-vb，考慮計(jì)算坐標(biāo)系中存在失準(zhǔn)角φ、速度誤差δ vb、位置誤差δ p及重力投影誤差δ gn，代入式(5)等號(hào)右側(cè)，各項(xiàng)可寫(xiě)為：

將式(4)(5)做差，考慮各誤差項(xiàng)為小量并省略二階以上誤差項(xiàng)的乘積，可得載體坐標(biāo)系的速度誤差微分方程為：

式(8)即是SINS載體系速度誤差微分方程，為嚴(yán)謹(jǐn)?shù)赝瓿蒘INS/DVL組合導(dǎo)航模型，還需要建立載體系速度誤差與失準(zhǔn)角、定位誤差及器件誤差之間的關(guān)系，本文基于傳統(tǒng)的導(dǎo)航坐標(biāo)系誤差方程，建立載體坐標(biāo)系速度誤差與導(dǎo)航坐標(biāo)系速度誤差的關(guān)聯(lián)，再利用傳統(tǒng)的導(dǎo)航系誤差方程實(shí)現(xiàn)較為快速的誤差方程建立。

從式(9)可以看出，在傳統(tǒng)誤差方程中將導(dǎo)航系速度誤差替換為δvb的同時(shí)，再加入有關(guān)失準(zhǔn)角的誤差項(xiàng)×φ就可以得到基于載體坐標(biāo)系速度誤差的誤差方程，將式(9)代入傳統(tǒng)姿態(tài)誤差方程中可得基于載體速度誤差的SINS姿態(tài)誤差方程為：

其中，

將式(9)代入傳統(tǒng)位置誤差方程中，可得基于載體速度誤差的SINS位置誤差方程為：

其中

短時(shí)間內(nèi)考慮加速度和陀螺儀誤差無(wú)變化，即為常值偏置加白噪聲模型，?b、εb模型為：

式(8)(10)(11)(12)構(gòu)成了基于載體速度誤差的SINS誤差方程，其與導(dǎo)航系速度誤差方程的區(qū)別是較為明顯的。需要指出的是采用式(9)可以快速建立基于載體系速度誤差匹配的初始對(duì)準(zhǔn)量測(cè)模型，并實(shí)現(xiàn)同樣高精度的模型建立，但其僅從量測(cè)模型方面進(jìn)行了改變，并未改變系統(tǒng)模型，這種方案下對(duì)準(zhǔn)受量測(cè)誤差影響較大，仍然不能解決誤差協(xié)方差與狀態(tài)不一致性的問(wèn)題，所以由速度微分方程出發(fā)建立基于載體系速度誤差方程的SINS誤差方程是有意義的。同時(shí)式(8)也為實(shí)現(xiàn)更深層次的SINS/DVL緊組合提供了可能。

3 基于狀態(tài)變換初始對(duì)準(zhǔn)的卡爾曼濾波模型

根據(jù)已推導(dǎo)出的基于載體坐標(biāo)系速度的誤差方程可建立SINS/DVL組合導(dǎo)航模型，在SINS初始化誤差較大時(shí)，可由卡爾曼濾波方法實(shí)現(xiàn)誤差狀態(tài)的估計(jì)，并完成初始對(duì)準(zhǔn)任務(wù)。通常選取SINS誤差狀態(tài)為濾波中的狀態(tài)變量，在SINS/DVL組合系統(tǒng)中通常存在安裝誤差角，其對(duì)SINS初始對(duì)準(zhǔn)的影響與載體航行速度相關(guān)，不能忽略。

考慮初始對(duì)準(zhǔn)過(guò)程有短時(shí)高精度的需求，而SINS和DVL導(dǎo)航系統(tǒng)之間安裝的桿臂和DVL量測(cè)刻度系數(shù)誤差已經(jīng)校正，且短時(shí)間內(nèi)DVL刻度系數(shù)誤差穩(wěn)定，安裝偏差角較小，則DVL測(cè)速模型為：

其中wd為DVL速度測(cè)量噪聲，安裝偏差角ψ將一定程度上影響失準(zhǔn)角的估計(jì)，因此需將安裝偏差角擴(kuò)充至濾波系統(tǒng)狀態(tài)中，認(rèn)為安裝偏差角短時(shí)間內(nèi)不變化，其常值模型如下：

選取基于載體系速度誤差的SINS誤差與SINS/DVL組合導(dǎo)航系統(tǒng)安裝偏差角作為初始對(duì)準(zhǔn)系統(tǒng)待估計(jì)狀態(tài)。卡爾曼濾波器狀態(tài)量X為：

根據(jù)式(8)(10)-(12)，連續(xù)時(shí)間系統(tǒng)模型為：

其中，

其中wg、wa分別為陀螺儀和加速度計(jì)測(cè)量噪聲。

以T為離散時(shí)間對(duì)線性微分方程(14)進(jìn)行離散化，可得離散化的一步轉(zhuǎn)移矩陣Φk/k-1及噪聲分配陣Γk-1為：

可以看出所建立模型與基于導(dǎo)航坐標(biāo)系誤差方程的傳統(tǒng)模型在噪聲分配陣部分也存在區(qū)別，在速度誤差傳遞方程中不僅有加速度計(jì)偏置項(xiàng)，還包含與速度信息耦合的陀螺儀偏置項(xiàng)，這種現(xiàn)象也出現(xiàn)在系統(tǒng)噪聲傳遞中，由此可以看出傳統(tǒng)模型中是存在不準(zhǔn)確描述的，在濾波中也將導(dǎo)致?tīng)顟B(tài)協(xié)方差與估計(jì)狀態(tài)一致性差的問(wèn)題。

4 仿真分析與實(shí)驗(yàn)驗(yàn)證

為驗(yàn)證所提出對(duì)準(zhǔn)方法的有效性，本文進(jìn)行了數(shù)值仿真驗(yàn)證及湖面實(shí)驗(yàn)驗(yàn)證。數(shù)值仿真部分采用蒙特卡洛仿真說(shuō)明所提算法的適用性及優(yōu)勢(shì)。有限的湖面實(shí)驗(yàn)航次用以驗(yàn)證所提出對(duì)準(zhǔn)算法在實(shí)際應(yīng)用的可行性。為充分反映所提算法的優(yōu)越性，本文將以下常用對(duì)準(zhǔn)模型與所提模型進(jìn)行對(duì)比，考慮空間桿臂差異容易被測(cè)量并補(bǔ)償，所以所有驗(yàn)證工作均在桿臂修正基礎(chǔ)上展開(kāi)。為方便說(shuō)明將所提出的基于載體系速度誤差方程的對(duì)準(zhǔn)模型稱為模型1，其余對(duì)準(zhǔn)模型分別為：

模型3：以傳統(tǒng)導(dǎo)航坐標(biāo)系誤差方程為基礎(chǔ)，狀態(tài)量為X=[φT(δvn)T(δp)T(εb)T(?b)T]T，系統(tǒng)轉(zhuǎn)移矩陣F及觀測(cè)量同模型2，因?yàn)闋顟B(tài)中沒(méi)有考慮安裝偏差角的存在，所以量測(cè)矩陣H為：

4.1 蒙特卡洛仿真驗(yàn)證

本文主要針對(duì)于較大方位初始偏差角及SINS、DVL之間存在安裝偏差的環(huán)境下，提出一種有效的DVL輔助SINS的初始對(duì)準(zhǔn)算法，為證明所提算法的一般性，數(shù)值仿真部分將在不同條件下進(jìn)行三組蒙特卡洛仿真。仿真條件為：初始位置誤差10 m，初始速度誤差0.1 m/s，三軸陀螺常值漂移0.01 °/h，三軸加速度計(jì)常值零偏100 μg，DVL測(cè)速誤差0.1 m/s。三組仿真控制變量如表1所示（±1σ：蒙特卡洛隨機(jī)變量標(biāo)準(zhǔn)差為1 °）。

表1 三組蒙特卡洛仿真初始誤差設(shè)置Tab.1 Initial error settings of Monte Carlo simulation

每組實(shí)驗(yàn)蒙特卡洛仿真次數(shù)為220次，速度為2 m/s，仿真軌跡如圖1所示。

圖1 蒙特卡洛仿真軌跡Fig.1 Trajectory of Monte Carlo simulation

通常較為關(guān)注的方位誤差角仿真結(jié)果如圖2所示，

4.2 湖面實(shí)驗(yàn)驗(yàn)證

本文利用湖面實(shí)驗(yàn)采集數(shù)據(jù)對(duì)本文提出的基于載體坐標(biāo)系速度誤差方程的對(duì)準(zhǔn)方法實(shí)用性進(jìn)行驗(yàn)證。船上配備了光纖捷聯(lián)慣性導(dǎo)航系統(tǒng)（SINS）、法國(guó)光纖捷聯(lián)慣性導(dǎo)航系統(tǒng)（PHINS）、聲學(xué)多普勒測(cè)速儀（DVL）和GPS，其中PHINS與GPS組合系統(tǒng)（PHINS/GPS）作為導(dǎo)航基準(zhǔn)用來(lái)衡量SINS與DVL的對(duì)準(zhǔn)精度，主要傳感器參數(shù)如表2所示。實(shí)驗(yàn)分別在船舶岸邊系泊狀態(tài)、湖面直線航行狀態(tài)和湖面轉(zhuǎn)向機(jī)動(dòng)狀態(tài)進(jìn)行對(duì)準(zhǔn)，各實(shí)驗(yàn)狀態(tài)軌跡特征及DVL測(cè)量所得三軸速度如圖5所示。實(shí)驗(yàn)中未對(duì)DVL刻度系數(shù)誤差進(jìn)行校正，根據(jù)后續(xù)實(shí)驗(yàn)過(guò)程中多次標(biāo)定所得SINS與DVL之間安裝誤差角約為[-0.5869,-4.4299,-4.2106] °。

表2 湖面實(shí)驗(yàn)主要傳感器參數(shù)Tab.2 Sensor parameters of lake experiment

圖5 實(shí)驗(yàn)軌跡及DVL測(cè)量速度Fig.5 Experimental track and Measurement velocity of DVL

每組實(shí)驗(yàn)仍采用4.1中三種模型進(jìn)行比較。1200 s對(duì)準(zhǔn)時(shí)間內(nèi)失準(zhǔn)角、速度誤差及位置誤差估計(jì)情況如圖6-8所示。由圖可以看出在系泊條件下三種模型估計(jì)結(jié)果幾乎一致，速度、位置誤差較小，由于PHINS與SINS之間沒(méi)有做同步觸發(fā)處理，且兩者之間存在安裝偏差未進(jìn)行補(bǔ)償，所以失準(zhǔn)角不為零。在直航條件下，因?yàn)榇嬖谳^大的初始方位誤差角，模型2、模型3航向收斂緩慢，且水平姿態(tài)誤差及速度誤差存在較大的變動(dòng)，位置誤差也比較大，而模型1姿態(tài)誤差、速度誤差較為平緩，位置誤差也較大，其直接原因是北向速度誤差存在恒定的較大誤差，誤差源為SINS與DVL的安裝偏差角未能在直航中有效估計(jì)出來(lái)。在轉(zhuǎn)彎運(yùn)動(dòng)對(duì)準(zhǔn)實(shí)驗(yàn)中三種模型都能有較好的收斂，其中模型1效果最好，航向失準(zhǔn)角、速度誤差收斂較快，位置誤差在轉(zhuǎn)彎后有明顯的減小，其主要原因是在轉(zhuǎn)彎后SINS和DVL的安裝偏差角一定程度上被估計(jì)，位置誤差也得到了有效的估計(jì)，1200 s時(shí)位置誤差僅為15.8 m，精度遠(yuǎn)高于模型2與模型3。

圖6 失準(zhǔn)角估計(jì)結(jié)果比較Fig.6 Comparison of misalignment angle estimation results

圖7 速度誤差估計(jì)結(jié)果比較Fig.7 Comparison of velocity error estimation results

圖8 位置誤差估計(jì)結(jié)果比較Fig.8 Comparison of position error estimation results

三種模型對(duì)準(zhǔn)過(guò)程中的器件誤差估計(jì)情況及SINS和DVL安裝誤差角估計(jì)情況如圖9-10所示，模型3未對(duì)安裝偏差角建模，因此不做討論。

圖9 陀螺漂移估計(jì)結(jié)果比較Fig.9 Comparison of gyro drift estimation results

圖10 加速度計(jì)零偏估計(jì)結(jié)果比較Fig.10 Comparison of zero bias estimation results of accelerometer

系泊和直航條件下可以看出各軸陀螺漂移和加速度計(jì)零偏都沒(méi)有明顯的收斂趨勢(shì)，其主要原因是在系泊及勻速直線運(yùn)動(dòng)條件下陀螺等效東向零漂和水平加速度計(jì)零偏不可觀測(cè)。其中由于直線航行時(shí)大初始方位誤差角造成量測(cè)誤差較大也造成了模型2、模型3陀螺漂移、加速度計(jì)零偏過(guò)估計(jì)情況。在轉(zhuǎn)彎對(duì)準(zhǔn)實(shí)驗(yàn)中，陀螺漂移和加速度計(jì)零偏有明顯收斂趨勢(shì)，其中陀螺漂移收斂與實(shí)驗(yàn)室測(cè)試環(huán)境下所得結(jié)果接近，而加速度計(jì)零偏超出100 μg，其原因?yàn)閷?shí)驗(yàn)船只較小，在實(shí)驗(yàn)過(guò)程中線運(yùn)動(dòng)較為明顯，加速度計(jì)動(dòng)態(tài)誤差較大，這也是造成動(dòng)態(tài)對(duì)準(zhǔn)姿態(tài)結(jié)果與系泊對(duì)準(zhǔn)姿態(tài)存在明顯區(qū)別的主要原因。

5 結(jié) 論

本文以大初始方位誤差角為背景，針對(duì)傳統(tǒng)DVL輔助SINS模型不精確造成的初始對(duì)準(zhǔn)精度低、時(shí)間長(zhǎng)的問(wèn)題，提出了基于載體速度誤差方程的初始對(duì)準(zhǔn)模型。由比力方程出發(fā)，建立新的初始對(duì)準(zhǔn)模型，并由蒙特卡洛仿真實(shí)驗(yàn)及湖面實(shí)驗(yàn)驗(yàn)證了所提出初始對(duì)準(zhǔn)模型的有效性。蒙特卡洛仿真實(shí)驗(yàn)在多種誤差源環(huán)境下證明所提模型在常規(guī)使用環(huán)境中精度與傳統(tǒng)模型精度相差不多，在大方位誤差角及SINS和DVL間存在明顯安裝誤差角條件下能大幅度提高對(duì)準(zhǔn)精度及定位精度。湖面實(shí)驗(yàn)結(jié)果證明所提模型在大初始方位誤差角結(jié)合多運(yùn)動(dòng)狀態(tài)條件下能實(shí)現(xiàn)高精度初始對(duì)準(zhǔn)，且在存在轉(zhuǎn)彎?rùn)C(jī)動(dòng)的環(huán)境下能獲得高精度位置估計(jì)。綜合以上，可以證明本文所提出的基于載體系速度誤差方程的初始對(duì)準(zhǔn)模型能有效提高對(duì)準(zhǔn)模型精度，在大方位誤差角條件下所提對(duì)準(zhǔn)方法相對(duì)于傳統(tǒng)對(duì)準(zhǔn)方法有明顯優(yōu)勢(shì)。