復雜水下環(huán)境中自適應粒子群優(yōu)化的SINS/DVL 組合導航方法

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

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

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

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

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

1 水下SINS/DVL 自主導航原理

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

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

2 SINS/DVL 組合導航方法

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

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

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

一般情況下，當?shù)貙Ш较迪碌乃俣任⒎址匠虨椋?/p>

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

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

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

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

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

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

其中，H表示量測轉移矩陣；V表示量測噪聲矩陣；Z表示量測矩陣：

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

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

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

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

基于Huber M估計原理，構造量測噪聲方差陣Sk：

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

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

首先，構造適應度函數(shù)。

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

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

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

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

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

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

為了更合理地改變慣性權重得到更優(yōu)結果，對慣性權重進行改進：

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

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

2.4 濾波量測更新過程

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

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

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

均方誤差陣更新為：

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

3 實驗分析與結果

為了驗證本文方法的有效性，分別進行仿真實驗和湖試實驗。

3.1 仿真實驗

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

圖3 仿真運動軌跡Fig.3 Simulation motion trajectories

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

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

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

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

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

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

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

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

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

3.2 湖試實驗

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

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

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

表3 實驗相關設備參數(shù)Tab.3 Related equipment parameters

圖7 為實驗過程中載體的運動軌跡，整個運動過程持續(xù)了1050 s，包括加速、減速、轉彎等各種運動。

圖7 湖試運動軌跡Fig.7 Motion trajectories for lake test

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

圖8 三種方法的湖試運動軌跡Fig.8 Lake test motion trajectories for three methods

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

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

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

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

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

圖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 結論

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