王 迪，王 冰，黃浩乾，王廣才，劉錫祥，姚逸卿

（1.河海大學(xué) 能源與電氣學(xué)院，南京 211100；2.湖南大學(xué) 機(jī)械與運(yùn)載工程學(xué)院，長沙 410006；3.東南大學(xué) 儀器科學(xué)與工程學(xué)院，南京 210096）

海洋資源豐富，而水下機(jī)器人又是開發(fā)利用海洋資源的有效載體，同時(shí)，水下機(jī)器人高效作業(yè)離不開高精度的導(dǎo)航定位信息。因此，水下高精度定位是水下機(jī)器人開發(fā)的關(guān)鍵技術(shù)之一。目前常用的水下導(dǎo)航方式有捷聯(lián)慣性導(dǎo)航系統(tǒng)（Strap-down Inertial Navigation System,SINS）、多普勒計(jì)程儀（Doppler Velocity Log,DVL）、水聲定位系統(tǒng)、地球物理導(dǎo)航系統(tǒng)等[1,2]。其中，捷聯(lián)慣性導(dǎo)航系統(tǒng)因其自主性高、導(dǎo)航信息全面，在水下導(dǎo)航系統(tǒng)中承擔(dān)主要角色。由于SINS 基于積分原理，存在誤差隨時(shí)間累積的問題，因此，SINS 一般與其他定位方式結(jié)合使用。多普勒計(jì)程儀利用多普勒聲學(xué)原理，能夠?qū)崟r(shí)測量載體相對(duì)于海底的速度信息，且誤差不隨時(shí)間累積。水聲定位系統(tǒng)包括：長基線（Long Baseline,LBL）、短基線（Short Baseline,SBL）以及超短基線（Ultra-short Baseline,USBL）。以上水聲定位系統(tǒng)具有精度高、誤差不隨時(shí)間累積的特點(diǎn)[3,4]。但是，水聲定位系統(tǒng)均需要事先在水下鋪設(shè)基陣，具有維護(hù)復(fù)雜，且在未知海域無法使用的問題。地球物理導(dǎo)航方法包括：地形匹配、地磁匹配等，此類方法需要事先建立龐大的地形、地磁數(shù)據(jù)庫，需要定期更新數(shù)據(jù)庫且操作復(fù)雜[5]。因此，以SINS 為主、DVL 為輔的自主式導(dǎo)航系統(tǒng)受到了水下機(jī)器人的青睞，同時(shí)也成為水下機(jī)器人組合導(dǎo)航系統(tǒng)的標(biāo)配并得到廣泛應(yīng)用。

由于水下環(huán)境復(fù)雜，聲學(xué)信號(hào)傳播存在不確定性和干擾性，因此基于SINS/DVL 的魯棒濾波算法研究迫在眉睫。文獻(xiàn)[6]為提高復(fù)雜環(huán)境下SINS/DVL 導(dǎo)航精度，從組合模型角度出發(fā)，提出了基于波束信息的緊組合容錯(cuò)模型，同時(shí)設(shè)計(jì)了DVL 波束信息缺失處理機(jī)制，提高了復(fù)雜環(huán)境下的系統(tǒng)魯棒性；文獻(xiàn)[7][8]從信息融合算法角度出發(fā)，將Huber 魯棒濾波算法引入到SINS/DVL 系統(tǒng)中，提高了對(duì)外界的抗干擾能力，并通過實(shí)驗(yàn)驗(yàn)證了該算法的優(yōu)越性；文獻(xiàn)[9]針對(duì)SINS/DVL 組合導(dǎo)航過程中野值干擾問題，提出了一種改進(jìn)的雙因子抗差自適應(yīng)濾波算法，通過引入卡方檢測輔助雙因子自適應(yīng)濾波算法，提高濾波的魯棒性。上述方法從系統(tǒng)抗干擾角度入手解決了導(dǎo)航的魯棒性問題，但對(duì)精度的提升存在局限性。文獻(xiàn)[10][11]針對(duì)水下噪聲復(fù)雜多變的問題，將變分貝葉斯自適應(yīng)濾波算法引入到SINS/DVL 組合系統(tǒng)中，進(jìn)一步提高了系統(tǒng)對(duì)外界復(fù)雜噪聲的自適應(yīng)性。然而，采用變分貝葉斯自適應(yīng)算法，雖然在一定程度上提高了復(fù)雜環(huán)境下的導(dǎo)航精度，但該算法涉及較多可調(diào)節(jié)參數(shù)，在自適應(yīng)方面存在一定的局限性；文獻(xiàn)[12][13]將機(jī)器學(xué)習(xí)引入到SINS/DVL 組合導(dǎo)航中，解決復(fù)雜環(huán)境下導(dǎo)航信息失效的問題，然而，該方法在訓(xùn)練樣本和實(shí)際使用中都存在一定局限性。

針對(duì)以上研究及存在的問題，本文以復(fù)雜環(huán)境下SINS/DVL 組合導(dǎo)航系統(tǒng)為研究對(duì)象，分別從模型和算法兩個(gè)角度解決外界野值和復(fù)雜噪聲干擾問題。一方面，從組合導(dǎo)航模型角度出發(fā)，針對(duì)組合濾波中方差估計(jì)不一致的問題，引入狀態(tài)變換原理，建立了SINS/DVL 導(dǎo)航模型；另一方面，為提高算法的魯棒性，在Huber 魯棒濾波算法的基礎(chǔ)上，分別改進(jìn)噪聲方差陣以及引入自適應(yīng)粒子群優(yōu)化算法，實(shí)現(xiàn)對(duì)外界復(fù)雜測量信息的魯棒處理，進(jìn)而提高SINS/DVL 導(dǎo)航精度。

1 水下SINS/DVL 自主導(dǎo)航原理

由于SINS 和DVL 都是靠自身獲取載體信息，不與外界發(fā)生信息交互，因此，基于SINS/DVL 的組合導(dǎo)航系統(tǒng)具有較高的自主性，其中包括慣性測量單元（Inertial Measurement Unit,IMU）和DVL。IMU 包括三軸陀螺儀和三軸加速度計(jì)，分別用來測量載體的角運(yùn)動(dòng)和線運(yùn)動(dòng)；DVL 基于多普勒原理，能夠測量載體相對(duì)于海底的三維速度信息。

圖1 給出了基于SINS/DVL 的組合導(dǎo)航框圖。在IMU 測量信息的基礎(chǔ)上，通過捷聯(lián)解算能夠獲取載體相對(duì)導(dǎo)航坐標(biāo)系的姿態(tài)、速度和位置信息。將經(jīng)過坐標(biāo)系轉(zhuǎn)換的DVL 速度與SINS 速度的差值作為卡爾曼濾波器的量測，進(jìn)行更新處理，即可完成整個(gè)導(dǎo)航過程。

2 SINS/DVL 組合導(dǎo)航方法

本文針對(duì)復(fù)雜水下環(huán)境對(duì)SINS/DVL 導(dǎo)航系統(tǒng)的影響，提出一種自適應(yīng)粒子群優(yōu)化的SINS/DVL 導(dǎo)航方法，具體如圖2 所示。首先，針對(duì)導(dǎo)航模型魯棒性問題，使用不易受干擾的地球重力信息代替易受干擾的加速度測量信息，構(gòu)建基于狀態(tài)變換原理的導(dǎo)航模型，推導(dǎo)出狀態(tài)方程和量測方程，提高模型的魯棒性。其次，在Huber 魯棒濾波算法的基礎(chǔ)上，考慮系統(tǒng)噪聲和量測噪聲的強(qiáng)非線性問題，引入粒子群優(yōu)化算法，實(shí)現(xiàn)系統(tǒng)噪聲方差陣和量測噪聲方差陣的自適應(yīng)估計(jì)。設(shè)計(jì)適應(yīng)度函數(shù)，提高算法對(duì)干擾噪聲的處理能力。最后，在粒子群優(yōu)化算法的基礎(chǔ)上，針對(duì)權(quán)重對(duì)粒子群搜索速度和收斂性的影響，提出一種改進(jìn)的慣性權(quán)重計(jì)算方法，提高粒子群算法的自適應(yīng)性。

圖2 自適應(yīng)粒子群優(yōu)化的SINS/DVL 導(dǎo)航方法Fig.2 Adaptive particle swarm optimization method for SINS/DVL navigation system

2.1 基于狀態(tài)變換原理的導(dǎo)航模型

一般情況下，當(dāng)?shù)貙?dǎo)航系下的速度微分方程為：

基于狀態(tài)變換原理建立SINS/DVL 組合導(dǎo)航狀態(tài)方程[14]：

其中，F(xiàn)為組合導(dǎo)航狀態(tài)轉(zhuǎn)移矩陣；W為組合導(dǎo)航系統(tǒng)噪聲；X為15 維的組合導(dǎo)航狀態(tài)向量：

組合導(dǎo)航狀態(tài)轉(zhuǎn)移矩陣由四部分組成：

其中，Mav、Maa、Map只是為了表述方便而定義的中間變量，不代表任何意義；ωie表示地球旋轉(zhuǎn)角速率；L和h分別表示載體所在位置的地理緯度和高度；表示SINS 輸出的速度向量；RM和RN表示載體所在位置的子午圈半徑和卯酉圈半徑。

從式(5)可以看出，新的速度誤差微分方程對(duì)應(yīng)的狀態(tài)轉(zhuǎn)移矩陣不再含有比力向量，而是由重力向量替代，避免了傳統(tǒng)SINS/DVL 系統(tǒng)矩陣計(jì)算受動(dòng)態(tài)環(huán)境下的比力fn影響，進(jìn)而導(dǎo)致方差不一致性。

SINS/DVL 組合導(dǎo)航系統(tǒng)的量測方程[15,16]為：

其中，H表示量測轉(zhuǎn)移矩陣；V表示量測噪聲矩陣；Z表示量測矩陣：

SINS/DVL 組合導(dǎo)航系統(tǒng)通過DVL 速度信息修正并反饋給SINS，從而為載體提供高精度的位置信息。

2.2 濾波狀態(tài)更新及噪聲矩陣構(gòu)造

在上述模型的基礎(chǔ)上，進(jìn)行濾波狀態(tài)更新：

其中，Pk-1表示狀態(tài)向量的均方誤差陣；Qk-1表示k-1時(shí)刻的系統(tǒng)噪聲方差陣。

基于Huber M估計(jì)原理，構(gòu)造量測噪聲方差陣Sk：

式(18)給出了量測噪聲方差陣Sk的表達(dá)形式。為了能夠更好地滿足濾波算法對(duì)魯棒性的需求，在上述兩個(gè)噪聲方差陣的基礎(chǔ)上，引入自適應(yīng)粒子濾波算法，對(duì)兩個(gè)噪聲方差陣進(jìn)行修正，進(jìn)而提高濾波的魯棒性。此過程分為兩步，粒子濾波適應(yīng)度函數(shù)構(gòu)造和自適應(yīng)粒子群優(yōu)化算法。

2.3 自適應(yīng)粒子群優(yōu)化處理

首先，構(gòu)造適應(yīng)度函數(shù)。

將系統(tǒng)噪聲方差陣Qk和量測噪聲方差陣Sk作為粒子，而SINS/DVL 組合導(dǎo)航系統(tǒng)性能主要取決于位置精度，所以將濾波器輸出的位置誤差均方差作為適應(yīng)度函數(shù)。

其中，f（Qi,Si）是Qi,Si為參數(shù)時(shí)SINS/DVL 組合導(dǎo)航系統(tǒng)位置誤差的均方差；ω?（Qi,Si）為估計(jì)的位置；M為估計(jì)長度；Lk為參考的實(shí)際位置值。

其次，優(yōu)化自適應(yīng)粒子群。

①初始化粒子群參數(shù)，在參數(shù)區(qū)間內(nèi)初始化粒子群的位置和速度，隨機(jī)產(chǎn)生若干粒子并初始化最優(yōu)粒子；

② 根據(jù)上述適應(yīng)度函數(shù)，對(duì)每一個(gè)粒子的適應(yīng)度進(jìn)行評(píng)價(jià)，比較其與最優(yōu)粒子的適應(yīng)度值，若適應(yīng)度更好則取代最優(yōu)粒子；

③粒子x＝（Qk,Si）迭代更新自己的速度和位置，每個(gè)粒子根據(jù)自身和群體經(jīng)驗(yàn)調(diào)整軌跡向最優(yōu)點(diǎn)靠攏，按式(20)(21)進(jìn)行更新[17,18]：

為了更合理地改變慣性權(quán)重得到更優(yōu)結(jié)果，對(duì)慣性權(quán)重進(jìn)行改進(jìn)：

其中，ustart和uend分別為慣性權(quán)重的初值和末值，取值范圍為0.4≤uend≤ustart≤0 .9；IM為迭代次數(shù)的最大值；IG為當(dāng)前迭代次數(shù)。

④ 重復(fù)②和③，更新最優(yōu)粒子，直至滿足迭代次數(shù)或精度要求，產(chǎn)生最優(yōu)粒子。

2.4 濾波量測更新過程

在濾波狀態(tài)更新和噪聲矩陣修正的基礎(chǔ)上，進(jìn)行濾波量測更新：

其中，?k-1表示k-1時(shí)刻的狀態(tài)估計(jì)權(quán)重；Ak是定義的中間變量，不代表任何意義，其具體形式為[19]：

其中，sgn(?)表示符號(hào)函數(shù)；γ表示因子，γ＝0.8。

均方誤差陣更新為：

其中，I15表示15 行15 列的單位矩陣；Kk表示k時(shí)刻的濾波增益。

3 實(shí)驗(yàn)分析與結(jié)果

為了驗(yàn)證本文方法的有效性，分別進(jìn)行仿真實(shí)驗(yàn)和湖試實(shí)驗(yàn)。

3.1 仿真實(shí)驗(yàn)

基于MATLAB 軟件開展仿真實(shí)驗(yàn)，其中傳感器仿真參數(shù)設(shè)置為：陀螺儀零偏誤差0.02 °/h，加速度計(jì)零偏誤差100 μg，陀螺儀和加速度計(jì)數(shù)據(jù)輸出頻率為200 Hz。DVL 刻度因子誤差0.003，DVL 輸出頻率為2 Hz。圖3 給出了卡爾曼濾波方法、Huber 魯棒濾波方法以及本文自適應(yīng)粒子群優(yōu)化方法的仿真運(yùn)動(dòng)軌跡。

圖3 仿真運(yùn)動(dòng)軌跡Fig.3 Simulation motion trajectories

為了能夠模擬水下復(fù)雜環(huán)境對(duì)DVL 測量的干擾，在仿真參數(shù)的基礎(chǔ)上增加噪聲：

其中，RD表示設(shè)置的量測噪聲方差陣表示DVL 輸出的速度噪聲；w.p.0.60 表示概率，即方差為原來100 倍的量測噪聲以60%概率出現(xiàn)在原來的高斯分布中。

從圖3 可以看出，三種方法的位置誤差均越來越大，這是由于DVL 刻度因子誤差及噪聲的存在，使得SINS/DVL 導(dǎo)航系統(tǒng)誤差會(huì)緩慢累積，而自適應(yīng)粒子群優(yōu)化方法的最終位置與參考軌跡最接近。為了更加準(zhǔn)確地描述不同方法的定位誤差，圖4-5 給出了三種方法對(duì)應(yīng)的SINS/DVL 組合導(dǎo)航系統(tǒng)的東向位置誤差和北向位置誤差。

圖4 東向位置誤差仿真曲線Fig.4 Curves of east position error for simulation

從圖4 可以看出，由于受到復(fù)雜環(huán)境干擾，卡爾曼濾波方法的東向位置誤差發(fā)散最大，Huber 魯棒濾波方法雖然一定程度上能夠減少外界復(fù)雜噪聲干擾，但其誤差仍然比本文的自適應(yīng)粒子群優(yōu)化方法大。從圖5 也可看出本文方法優(yōu)于其他兩種方法。

圖5 北向位置誤差仿真曲線Fig.5 Curves of north position error for simulation

表1 給出三種方法的位置誤差絕對(duì)值的均值，表2給出三種方法的位置誤差絕對(duì)值的最大值。

表2 三種方法的位置誤差絕對(duì)值的最大值Tab.2 Max of position error for three methods

根據(jù)表1-2 可知，無論是位置誤差均值還是位置誤差最大值，本文的自適應(yīng)粒子群優(yōu)化方法均優(yōu)于卡爾曼濾波方法和Huber 魯棒濾波方法。

3.2 湖試實(shí)驗(yàn)

為驗(yàn)證本文方法的有效性，搭建了湖試平臺(tái)。實(shí)驗(yàn)設(shè)備包括IMU、DVL、衛(wèi)星導(dǎo)航系統(tǒng)、高精度光纖SINS、移動(dòng)電源、數(shù)據(jù)采集設(shè)備、導(dǎo)航計(jì)算機(jī)等，如圖6 所示。實(shí)驗(yàn)選擇在江蘇省盱眙縣象山國家地質(zhì)公園湖泊內(nèi)進(jìn)行，DVL 通過橫跨船體的方型鋼架及鋼桿與船體固連，測速儀聲頭深入水下20 cm，在保證聲頭不被船底遮擋的同時(shí)還可以規(guī)避淺灘觸底的風(fēng)險(xiǎn)。

圖6 湖試現(xiàn)場圖Fig.6 Lake test site photos

基準(zhǔn)系統(tǒng)由RTK GPS 和高精度光纖SINS 組成，測得的高精度數(shù)據(jù)經(jīng)過卡爾曼濾波算法融合后，作為參考的位姿信息。實(shí)驗(yàn)相關(guān)設(shè)備參數(shù)如表3 所示，其中IMU更新頻率為200 Hz，DVL 更新頻率為1 Hz。

表3 實(shí)驗(yàn)相關(guān)設(shè)備參數(shù)Tab.3 Related equipment parameters

圖7 為實(shí)驗(yàn)過程中載體的運(yùn)動(dòng)軌跡，整個(gè)運(yùn)動(dòng)過程持續(xù)了1050 s，包括加速、減速、轉(zhuǎn)彎等各種運(yùn)動(dòng)。

圖7 湖試運(yùn)動(dòng)軌跡Fig.7 Motion trajectories for lake test

圖8 給出了卡爾曼濾波方法、Huber 魯棒濾波方法以及本文自適應(yīng)粒子群優(yōu)化方法的湖試運(yùn)動(dòng)軌跡。從圖中可以看出，三種方法的軌跡均能緊跟參考軌跡。從局部放大圖可以看出，自適應(yīng)粒子群優(yōu)化方法更接近參考軌跡，定位精度更高。

圖8 三種方法的湖試運(yùn)動(dòng)軌跡Fig.8 Lake test motion trajectories for three methods

圖9-11 給出了三種方法的東向位置誤差、北向位置誤差以及水平位置誤差。由于高度方向一般有深度計(jì)修正，因此不再考慮高度誤差。

圖9 湖試東向位置誤差Fig.9 East position error for lake test

圖10 湖試北向位置誤差Fig.10 North position error for lake test

從圖9-11 可以看出，本文方法的位置誤差較小。值得注意的是在北向位置誤差方面，本文方法的位置誤差大于其他兩種方法，這與運(yùn)動(dòng)軌跡方向和水下復(fù)雜噪聲有關(guān)。上述因素會(huì)導(dǎo)致定位誤差在較小范圍內(nèi)波動(dòng)，進(jìn)而產(chǎn)生上述的現(xiàn)象。但是，從水平位置誤差來看，整體上本文方法的定位精度優(yōu)于其他兩種方法。

基于SINS/DVL 的組合導(dǎo)航系統(tǒng)考慮到IMU 和DVL 均有測量誤差，因此，系統(tǒng)的定位誤差會(huì)隨著時(shí)間不斷累積。圖11 的水平定位誤差出現(xiàn)了較大波動(dòng)，導(dǎo)致首段和尾端出現(xiàn)了較高精度。同時(shí)，這種情況和載體的運(yùn)動(dòng)軌跡有關(guān)。當(dāng)水平位置誤差發(fā)散方向與載體運(yùn)動(dòng)方向相同時(shí)，會(huì)存在誤差減少的情況。但是，這種現(xiàn)象只是局部的，從長期導(dǎo)航過程來看，定位誤差仍然越來越大。

圖11 湖試水平位置誤差Fig.11 Horizontal position error for lake test

表4 給出三種方法的水平位置誤差均值和最大值。從表中可以看出：三種方法的水平位置誤差均值分別為3.30 m、2.65 m 和1.67 m，相比于其他兩種方法，本文方法的水平位置誤差均值分別減少了49.39%和36.98%；三種方法的水平位置誤差最大值分別為6.82 m、5.85 m和3.44 m，相比于其他兩種方法，本文方法的水平位置誤差最大值分別減少了49.56%和41.20%。

表4 三種方法的水平位置誤差均值和最大值Tab.4 Mean and maximum horizontal position error for three methods

4 結(jié)論

本文針對(duì)復(fù)雜環(huán)境下SINS/DVL 自主導(dǎo)航系統(tǒng)易受干擾的問題，提出了一種自適應(yīng)粒子群優(yōu)化的SINS/DVL 組合導(dǎo)航方法。本文從模型和算法兩個(gè)方面進(jìn)行分析：在模型方面，針對(duì)系統(tǒng)矩陣計(jì)算容易產(chǎn)生方差估計(jì)不一致問題，基于狀態(tài)變換原理，用重力矢量替代不穩(wěn)定的比力向量，推導(dǎo)了相關(guān)導(dǎo)航模型；在算法方面，在Huber 魯棒濾波算法的基礎(chǔ)上，引入并改進(jìn)了粒子群優(yōu)化算法，實(shí)現(xiàn)了系統(tǒng)噪聲方差陣和量測噪聲方差陣的自適應(yīng)估計(jì)。最后在實(shí)驗(yàn)驗(yàn)證方面，分別設(shè)計(jì)了仿真實(shí)驗(yàn)并搭建了湖試實(shí)驗(yàn)。實(shí)驗(yàn)結(jié)果表明，本文方法在復(fù)雜干擾環(huán)境下其定位性能優(yōu)于卡爾曼濾波方法和Huber 魯棒濾波方法，定位精度得到進(jìn)一步提升。