呂志剛

（1.中國(guó)船舶集團(tuán)有限公司第七一〇研究所，湖北 宜昌 443003；2.清江創(chuàng)新中心，湖北 武漢 430076）

0 引言

組合導(dǎo)航系統(tǒng)可為UUV長(zhǎng)航時(shí)水下航行提供精確的姿態(tài)、航向、速度和位置信息，精確的導(dǎo)航能力是 UUV任務(wù)完成和安全回收的一項(xiàng)關(guān)鍵技術(shù)。但由于受總體尺寸、重量、功耗等條件限制，水下環(huán)境的特殊性及隱蔽性等諸多因素的影響，實(shí)現(xiàn)UUV水下高精度導(dǎo)航仍然很困難，水下組合導(dǎo)航仍然是UUV所面臨的主要技術(shù)挑戰(zhàn)之一。

目前，單一的導(dǎo)航方式由于自身的不足，如單一的慣性導(dǎo)航系統(tǒng)和 GPS導(dǎo)航，已無法滿足水下高精度的導(dǎo)航要求，而目前基于INS/DVL/GPS的水下組合導(dǎo)航方式為UUV通常采用的一種導(dǎo)航方式，可解決導(dǎo)航誤差隨時(shí)間積累的難題[1]，滿足UUV長(zhǎng)航時(shí)水下航行的要求。

1 水下組合導(dǎo)航系統(tǒng)的結(jié)構(gòu)和組成

水下組合導(dǎo)航系統(tǒng)一般由INS、DVL、GPS和深度計(jì)構(gòu)成。深度計(jì)為可選擇的配置，下潛深度在300 m以內(nèi)的UUV可以不配置深度計(jì)。配置了深度計(jì)的UUV導(dǎo)航精度可以進(jìn)一步提升，特別是天向速度和高程信息精度可以大大提升。下潛深度遠(yuǎn)大于300 m的UUV，一般要求配置深度計(jì)，以提高導(dǎo)航的精度[2-3]。水下組合導(dǎo)航原理簡(jiǎn)圖見圖1。

圖1 水下組合導(dǎo)航原理圖Fig.1 Schematic diagram of underwater integrated navigation

2 組合導(dǎo)航系統(tǒng)的關(guān)鍵技術(shù)

制約 INS與 DVL組合導(dǎo)航的因素有很多，DVL的測(cè)速精度和INS的姿態(tài)精度是影響組合導(dǎo)航精度的2個(gè)重要因素。通過DVL標(biāo)定可降低INS與DVL之間的安裝偏差對(duì)DVL測(cè)速精度的影響，從而提高系統(tǒng)的導(dǎo)航精度。對(duì)準(zhǔn)精度和對(duì)準(zhǔn)速度是慣性導(dǎo)航系統(tǒng)進(jìn)行初始對(duì)準(zhǔn)時(shí)的 2項(xiàng)重要技術(shù)指標(biāo)，初始對(duì)準(zhǔn)精度影響慣性導(dǎo)航系統(tǒng)的性能，為了提高慣性導(dǎo)航系統(tǒng)的性能，需要通過高精度的初始對(duì)準(zhǔn)技術(shù)來保證系統(tǒng)的初始信息精度，尤其是初始姿態(tài)精度。

2.1 DVL標(biāo)定

DVL測(cè)量得到的是DVL在UUV坐標(biāo)系下的速度，它通過INS的姿態(tài)轉(zhuǎn)換到導(dǎo)航坐標(biāo)系下。在生產(chǎn)制造和安裝過程中，很難保證INS的UUV坐標(biāo)系與DVL的UUV坐標(biāo)系完全重合。因此，若不進(jìn)行INS與DVL的安裝偏差進(jìn)行標(biāo)定，將產(chǎn)生一定的測(cè)速誤差，從而影響導(dǎo)航的定位精度。由于INS的精度在很大程度上由器件決定，因此，提高INS與 DVL組合導(dǎo)航精度的主要途徑在于提高DVL的測(cè)速精度。通過對(duì)DVL標(biāo)度因數(shù)和INS與DVL之間安裝誤差的標(biāo)定可提高INS、DVL組合導(dǎo)航精度，為了提高INS/DVL組合導(dǎo)航系統(tǒng)精度，國(guó)內(nèi)外學(xué)者廣泛關(guān)注了INS與DVL安裝偏差的標(biāo)定問題。最早的標(biāo)定方法只考慮了 1個(gè)自由度，1989年，JOYCE提出了一種基于最小二乘的估計(jì)船載DVL航向安裝誤差的方法，該方法以GPS的速度作為參考來估計(jì)安裝偏差[4]。對(duì)于DVL標(biāo)定，國(guó)內(nèi)通常采用 GPS輔助下的標(biāo)定方式，國(guó)內(nèi)學(xué)者對(duì)DVL安裝偏差的在線估計(jì)也作了一定的研究，但是，由于水下航行器的機(jī)動(dòng)性較小，也難以取得比較好的估計(jì)效果。

綜上所述，DVL測(cè)速誤差標(biāo)定的難點(diǎn)在于水下航行器難以得到額外的速度、位置觀測(cè)，且水下航行器機(jī)動(dòng)性較小，為實(shí)現(xiàn)目標(biāo)定位帶來極大的難度。

2.2 INS初始對(duì)準(zhǔn)

INS初始對(duì)準(zhǔn)的目的是為了確定初始時(shí)刻的姿態(tài)矩陣。初始對(duì)準(zhǔn)的精度對(duì)慣性導(dǎo)航系統(tǒng)的導(dǎo)航精度有著重要的影響，初始對(duì)準(zhǔn)的時(shí)間在很大程度上決定了 UUV的快速反應(yīng)能力，因此，對(duì)準(zhǔn)精度和對(duì)準(zhǔn)的快速性是初始對(duì)準(zhǔn)最重要的2個(gè)指標(biāo)。按不同的劃分標(biāo)準(zhǔn)可以把INS的初始對(duì)準(zhǔn)分為靜基座對(duì)準(zhǔn)和動(dòng)基座對(duì)準(zhǔn)。對(duì)于靜基座對(duì)準(zhǔn)技術(shù)，研究成果已經(jīng)比較成熟，而動(dòng)基座一直都是慣性導(dǎo)航學(xué)術(shù)界關(guān)注的熱點(diǎn)。動(dòng)基座對(duì)準(zhǔn)包含2種情況：1）UUV受到機(jī)械振動(dòng)、外界風(fēng)力、浪涌等影響而引起的基座擾動(dòng)；2）UUV在水面航行中進(jìn)行初始對(duì)準(zhǔn)。

一般來說，初始對(duì)準(zhǔn)包括粗對(duì)準(zhǔn)和精對(duì)準(zhǔn)2大部分，粗對(duì)準(zhǔn)過程是一個(gè)提供UUV坐標(biāo)系和導(dǎo)航坐標(biāo)系之間姿態(tài)矩陣初始值的過程。目前初始對(duì)準(zhǔn)最常用的方法是慣性系粗對(duì)準(zhǔn)法，該算法有一定的抗基座搖晃能力，適合于系泊狀態(tài)下的航行器或艦船的粗對(duì)準(zhǔn)。精對(duì)準(zhǔn)則是利用外部觀測(cè)量的信息，進(jìn)一步得到UUV坐標(biāo)系和導(dǎo)航坐標(biāo)系之間姿態(tài)矩陣的精確值。典型的精對(duì)準(zhǔn)方法有羅經(jīng)法、DVL輔助下的UUV行進(jìn)間羅經(jīng)法、基于最優(yōu)估計(jì)（常用卡爾曼濾波估計(jì)）的對(duì)準(zhǔn)方法等。

3 導(dǎo)航系統(tǒng)算法

導(dǎo)航系統(tǒng)算法主要包括初始對(duì)準(zhǔn)、DVL標(biāo)定、純慣性導(dǎo)航算法、組合導(dǎo)航算法和校準(zhǔn)算法。

3.1 初始對(duì)準(zhǔn)算法

初始對(duì)準(zhǔn)算法有很多種，除了少數(shù)先進(jìn)的對(duì)準(zhǔn)算法，大部分初始對(duì)準(zhǔn)都包含粗對(duì)準(zhǔn)和精對(duì)準(zhǔn)2個(gè)過程，最合適AUV的是慣性系粗對(duì)準(zhǔn)和卡爾曼精對(duì)準(zhǔn)算法。

3.1.1 慣性系粗對(duì)準(zhǔn)[5-7]

慣性系粗對(duì)準(zhǔn)過程采用晃動(dòng)基座對(duì)準(zhǔn)方式解算INS的初始姿態(tài)，姿態(tài)矩陣方程為

e系和n系的坐標(biāo)轉(zhuǎn)換僅與大地維度信息有關(guān)，從e系到n系的變換矩陣為公式（2），其中L0為大地維度。

公式（3）為從i系和e系的轉(zhuǎn)換矩陣，即e系相對(duì)于i系的轉(zhuǎn)動(dòng)，其中ieω為地球自轉(zhuǎn)角速度。

3.1.2 卡爾曼精對(duì)準(zhǔn)

選用常用的卡爾曼模型，狀態(tài)變量選擇為如下的15維列向量：

式中：δVE、δVN為速度誤差；δL、δλ為緯度、經(jīng)度誤差；Eφ，Nφ，Uφ為姿態(tài)誤差；xε，yε，zε為陀螺漂移；?x，?y，?z為加速度計(jì)零偏。狀態(tài)變量對(duì)應(yīng)的高斯白噪聲選為

卡爾曼模型中的狀態(tài)轉(zhuǎn)移矩陣F如下：

卡爾曼模型中的X的噪聲W前面的矩陣

模型中量測(cè)量Z對(duì)應(yīng)的噪聲記為V，它是高斯白噪聲。

模型中量測(cè)方程

以上就是卡爾曼精對(duì)準(zhǔn)的全部計(jì)算公式，但精對(duì)準(zhǔn)結(jié)束時(shí)刻，應(yīng)該用卡爾曼濾波估計(jì)的誤差修正純慣性的計(jì)算結(jié)果，而不是直接用衛(wèi)星定位裝置或DVL的量測(cè)信息進(jìn)行修正。因?yàn)榍罢卟缓肼?，精度較高，后者包含了一定的噪聲，修正的效果不好。

3.2 純慣性解算算法

純慣性解算算法是利用慣性測(cè)量單元（IMU）的測(cè)量數(shù)據(jù)不斷地推算UUV當(dāng)前的姿態(tài)、速度和位置信息，包括姿態(tài)更新算法和速度位置更新算法2部分。

純慣性解算算法的流程圖見圖2。

圖 2 純慣性算法流程圖Fig.2 Flow chart of pure inertial algorithm

3.3 組合導(dǎo)航算法

組合導(dǎo)航算法是利用 DVL的速度信息進(jìn)行組合導(dǎo)航，主要有線性卡爾曼濾波、非線性卡爾曼濾波與粒子濾波等，最常用的就是線性卡爾曼濾波法。

組合導(dǎo)航過程采用 Kalman濾波計(jì)算濾波模型。估算出誤差以后，用純慣性結(jié)果減去誤差得到組合導(dǎo)航的結(jié)果。

在每個(gè)導(dǎo)航周期進(jìn)行狀態(tài)轉(zhuǎn)移矩陣F的離散化計(jì)算，轉(zhuǎn)移矩陣Φk,k-1計(jì)算如下[8-10]：

式中，T為導(dǎo)航周期。

當(dāng)接收到有效的接收機(jī)信息時(shí)，進(jìn)行 Kalman濾波計(jì)算，計(jì)算過程分為5步：

式中：為當(dāng)前濾波周期的實(shí)時(shí)狀態(tài)估計(jì)值；k/k-1為上一濾波周期對(duì)當(dāng)前濾波周期的狀態(tài)預(yù)測(cè)值；Kk為當(dāng)前濾波周期的濾波增益陣；Pk/k-1為上一濾波周期對(duì)當(dāng)前濾波周期的預(yù)測(cè)誤差估計(jì)的協(xié)方差陣；Pk為當(dāng)前濾波周期的實(shí)時(shí)誤差估計(jì)協(xié)方差陣；Qk為系統(tǒng)噪聲方差陣；Rk為量測(cè)噪聲方差陣。

量測(cè)方程為

卡爾曼濾波算法流程如圖3所示。

圖 3 卡爾曼濾波算法流程圖Fig.3 Flow chart of Kalman filtering algorithm

3.4 DVL標(biāo)定算法

由于DVL與INS之間有安裝誤差，導(dǎo)致DVL測(cè)量的速度與INS需要的速度值不一致，需要標(biāo)定出安裝誤差進(jìn)行補(bǔ)償才能夠提高組合導(dǎo)航的精度。最常用的標(biāo)定算法是兩點(diǎn)法，利用起點(diǎn)和終點(diǎn)處的GPS信息進(jìn)行航向角安裝誤差與標(biāo)度因子這2個(gè)變量的標(biāo)定。

3.4.1 基于卡爾曼濾波的標(biāo)定方法

DVL輸出的測(cè)量速度VDVL與INS組合導(dǎo)航所需要的速度vb之間的關(guān)系為[11]

式中：VDVL是DVL輸出的自身坐標(biāo)系下的原始速度數(shù)據(jù)；Kg是標(biāo)度因數(shù)。

姿態(tài)矩陣為

式中，α、β、γ分別為俯仰、橫滾、航向安裝誤差角。

標(biāo)定算法數(shù)據(jù)傳遞關(guān)系圖和標(biāo)定算法流程圖分別如圖4和圖5所示。圖5中加入了反饋，這樣即使設(shè)定的初始值偏離真實(shí)值很遠(yuǎn)，幾步迭代以后也可以保證算法收斂。

圖 4 標(biāo)定算法數(shù)據(jù)傳遞關(guān)系圖Fig.4 Data transmission relation diagram of calibration algorithm

圖 5 標(biāo)定算法流程圖Fig.5 Flow chart of calibration algorithm

標(biāo)定完成得到標(biāo)定參數(shù)以后便可以將VDVL轉(zhuǎn)換為vn進(jìn)行組合導(dǎo)航：

還可以改進(jìn)卡爾曼模型，把速度標(biāo)度因數(shù)Kg也包含進(jìn)去，一起進(jìn)行卡爾曼濾波估算?？梢宰孕胁捎妙愃频姆椒ㄟM(jìn)行推導(dǎo)。

3.4.2 基于航程起點(diǎn)終點(diǎn)位置信息的標(biāo)定方法

利用一段直線航程（一般選為6 km或7 km為1個(gè)標(biāo)準(zhǔn)航程）的起點(diǎn)與終點(diǎn) GPS信息可以進(jìn)行DVL的標(biāo)定，這種方式常稱為兩點(diǎn)標(biāo)定法。此方法只需要 UUV在航程起點(diǎn)和終點(diǎn)處位于水面即可，其余時(shí)間UUV都可以在水下航行，標(biāo)定方式更利于UUV的隱蔽。

這種標(biāo)定方法的原理和計(jì)算公式十分簡(jiǎn)單，就是計(jì)算組合導(dǎo)航推算的直線航路、GPS起點(diǎn)與終點(diǎn)構(gòu)成的實(shí)際直線航路之間的夾角和長(zhǎng)度比值。

3.5 校準(zhǔn)算法

校準(zhǔn)是指當(dāng) UUV下潛前或上浮后接收 GPS（常用GPS和北斗等）的位置信息進(jìn)行INS誤差的估算與補(bǔ)償。其本質(zhì)就是基于 GPS位置信息的組合導(dǎo)航算法，只要對(duì)組合導(dǎo)航算法稍加修改就可以實(shí)現(xiàn)校準(zhǔn)算法。

1）純慣性導(dǎo)航時(shí)的校準(zhǔn)。

衛(wèi)星定位裝置僅位置信息有效時(shí)，就用位置誤差信息作為觀測(cè)進(jìn)行校準(zhǔn)；衛(wèi)星定位裝置僅速度信息有效時(shí)，就用速度誤差作為觀測(cè)量進(jìn)行校準(zhǔn)；衛(wèi)星定位裝置的位置信息和速度信息同時(shí)有效時(shí)，就用位置誤差信息和速度誤差同時(shí)作為觀測(cè)量進(jìn)行校準(zhǔn)。

2）組合導(dǎo)航時(shí)的校準(zhǔn)。

不管 GPS的速度信息是否有效，都不用其速度信息，只用 GPS的位置信息得到的位置誤差作為觀測(cè)量進(jìn)行校準(zhǔn)。為了解決DVL組合模式切換到 GPS組合模式時(shí)卡爾曼濾波器的振蕩甚至發(fā)散的問題，也可以采用校準(zhǔn)時(shí)重啟卡爾曼濾波器或替換法等技巧。當(dāng)然，這種方法也會(huì)帶來一些弊端，重啟卡爾曼濾波器的缺點(diǎn)是卡爾曼重新收斂一般需要幾分鐘的時(shí)間；替換法的缺點(diǎn)是只對(duì)位置進(jìn)行了校準(zhǔn)，速度和姿態(tài)沒有得到校準(zhǔn)，組合導(dǎo)航精度不夠高。

校準(zhǔn)方式不限于如上2種，對(duì)于大深度UUV，位置信息還可能來自水聲定位系統(tǒng)，考慮延時(shí)補(bǔ)償后對(duì)大深度AUV進(jìn)行校準(zhǔn)。同時(shí)，還應(yīng)該建立重力加速度模型考慮不同深度下重力加速度值的變化。校準(zhǔn)算法的本質(zhì)就是卡爾曼組合導(dǎo)航，所以校準(zhǔn)算法的公式參考組合導(dǎo)航算法即可。

4 結(jié)束語

水下組合導(dǎo)航是UUV完成長(zhǎng)航時(shí)水下自主航行和任務(wù)的技術(shù)保障和基礎(chǔ)，而組合導(dǎo)航技術(shù)是提高 UUV導(dǎo)航精度的主要手段。本文章針對(duì)基于SINS/DVL/GPS的水下組合導(dǎo)航技術(shù)進(jìn)行了分析，闡述了水下組合導(dǎo)航系統(tǒng)的關(guān)鍵技術(shù)，隨后對(duì)初始對(duì)準(zhǔn)算法、純慣性解算算法、DVL標(biāo)定算法和校準(zhǔn)算法進(jìn)行了詳細(xì)論述，對(duì)水下組合導(dǎo)航具有一定的參考價(jià)值。