冉昌艷，祝 執(zhí)，王 偉，羅志會，朱倚嫻

（1.三峽大學(xué) 湖北省弱磁探測工程技術(shù)研究中心，宜昌 443002；2.三峽大學(xué) 理學(xué)院，宜昌 443002；3.湖北省水電工程智能視覺監(jiān)測重點實驗室，宜昌 443002；4.三峽大學(xué) 計算機與信息學(xué)院，宜昌 443002；5.南通大學(xué) 機械工程學(xué)院，南通 226019）

自主水下航行器（Autonomous Underwater Vehicle,AUV）具有自主性、隱蔽性、高機動性等優(yōu)點，在海洋探索及開發(fā)中發(fā)揮著舉足輕重的作用。AUV 的自主導(dǎo)航能力是衡量其自身工作性能的重要指標之一，同時也是制約其自身發(fā)展的關(guān)鍵技術(shù)之一[1]。因此，構(gòu)建具有更高精度的水下自主導(dǎo)航系統(tǒng)是進一步提升AUV 性能的關(guān)鍵。捷聯(lián)慣性導(dǎo)航系統(tǒng)（Strapdown Inertial Navigation System,SINS）不向外界輻射能量，雖然導(dǎo)航誤差隨時間累積，且無法為AUV 長時間提供高精度導(dǎo)航信息，但其隱蔽性和自主性較好，因此常作為AUV 的主導(dǎo)航系統(tǒng)。在輔助導(dǎo)航系統(tǒng)的選擇中，多普勒測速儀（Doppler Velocity Log,DVL）采用對底跟蹤或?qū)λ櫮Ｊ綔y量AUV 相對于海底或水層的高精度速度，其速度誤差不隨時間累積，可利用DVL 提供的高精度速度信息來抑制主慣導(dǎo)系統(tǒng)累積的速度誤差，DVL 能長時間為AUV 提供高精度速度信息。因此，SINS/DVL 組合導(dǎo)航系統(tǒng)被廣泛應(yīng)用于水下環(huán)境中[2,3]。

在水下組合導(dǎo)航中，基于擴展卡爾曼濾波器（Extended Kalman Filter,EKF）的SINS/DVL 組合導(dǎo)航系統(tǒng)，對于速度誤差的定義僅考慮了真實導(dǎo)航坐標系和計算的導(dǎo)航坐標系下的速度向量的大小差異，忽略了速度向量的方向差異，會造成方差的理論值與實際估計值不符，進而導(dǎo)致導(dǎo)航精度的下降甚至是濾波的發(fā)散[4]。狀態(tài)變換擴展卡爾曼濾波器（State Transformation Extended Kalman Filter,ST-EKF）對速度誤差進行了更嚴格的定義，兼顧了真實導(dǎo)航坐標系和計算的導(dǎo)航坐標系下的速度向量的大小和方向差異[5]。

SINS/DVL 組合導(dǎo)航可分為松組合與緊組合兩種形式。SINS/DVL 松組合導(dǎo)航采用速度匹配，將導(dǎo)航坐標系下的慣導(dǎo)系統(tǒng)的速度信息與經(jīng)過坐標變換后DVL 的速度信息的差值作為量測量，僅能在DVL 有三個以上的波束返回有效信息時進行導(dǎo)航任務(wù)[6]。SINS/DVL 緊組合導(dǎo)航采用波束速度或波束多普勒頻移信息進行匹配，將慣導(dǎo)系統(tǒng)的速度信息轉(zhuǎn)化為四個波束的速度或頻移信息與DVL 獲得的四個波束的速度或頻移信息的差值作為量測量，在DVL 有一個以上的波束返回有效信息時，持續(xù)提供高精度的導(dǎo)航[7-10]。

徐博等人[11]提出了一種基于波束速度信息重構(gòu)的SINS/DVL 緊組合導(dǎo)航系統(tǒng)故障處理方法。根據(jù)四波束Janus 陣列的結(jié)構(gòu)特點，通過引入慣導(dǎo)速度對缺失波束的速度信息進行重構(gòu)，能極大提高在波束速度信息缺失時的導(dǎo)航精度，但其重構(gòu)的波束速度值不能準確反映其真實值，會造成系統(tǒng)量測噪聲與量測量不匹配，影響SINS/DVL 緊組合導(dǎo)航的導(dǎo)航精度。

粒子群優(yōu)化（Particle Swarm Optimization,PSO）算法被廣泛應(yīng)用于參數(shù)優(yōu)化、信號處理等實際問題中[12,13]。徐曉蘇等人[14]在SINS/GPS 松組合導(dǎo)航中采用粒子群優(yōu)化算法，將量測預(yù)測值與量測量的絕對誤差累計作為目標函數(shù)，對系統(tǒng)噪聲協(xié)方差陣和量測噪聲協(xié)方差陣進行實時優(yōu)化，提高了SINS/GPS 松組合導(dǎo)航的精度。

本文提出了一種波束故障條件下基于ST-EKF 的SINS/DVL 緊組合導(dǎo)航優(yōu)化方法。首先，將ST-EKF應(yīng)用于水下SINS/DVL 緊組合導(dǎo)航系統(tǒng)中，提高了準靜止環(huán)境下的初始對準和組合導(dǎo)航濾波精度；其次，在SINS/DVL 緊組合導(dǎo)航系統(tǒng)中采用波束的多普勒頻移信息作為量測量；針對波束故障下重構(gòu)的波束頻移信息不能準確反映其真實值，造成量測噪聲方差矩陣的噪聲特性發(fā)生變化的問題，使用粒子群優(yōu)化算法對導(dǎo)航系統(tǒng)的噪聲方差矩陣Q和R進行實時優(yōu)化，并從四波束Janus 陣列的結(jié)構(gòu)特性入手，提出適用于水下SINS/DVL 緊組合導(dǎo)航系統(tǒng)的目標函數(shù)，有效提高了基于波束頻移信息重構(gòu)的SINS/DVL 緊組合導(dǎo)航系統(tǒng)的導(dǎo)航精度。仿真實驗結(jié)果驗證了本文方法能有效提高波束故障條件下SINS/DVL 緊組合的導(dǎo)航精度。

1 基于ST-EKF 的SINS/DVL 緊組合導(dǎo)航濾波模型

本文選取“東—北—天”坐標系作為導(dǎo)航坐標系，即n系；定義“右—前—上”坐標系為載體坐標系，即b系；DVL 所在坐標系為d系；DVL 中波束所在坐標系為f系。

在SINS/DVL 組合導(dǎo)航中常采用四波束Janus 配置，配置如圖1 所示，波束1 與波束3 相對，波束1與波束2 相鄰，波束2 與波束4 相對。

圖1 四波束Janus 配置圖Fig.1 Four-beam Janus configuration diagram

根據(jù)DVL 四波束Janus 配置的測速原理，測量速度的計算公式為：

其中，fd1、fd2、fd3、fd4分別為DVL 四個波束的多普勒頻移值；VX/DVL、VY/DVL、VZ/DVL分別為DVL測量的載體在側(cè)向、前向、天向的速度分量；c0為聲波在水中的速度，通常為1500 m/s；f0為DVL 發(fā)射聲波的頻率，根據(jù)不同的工作環(huán)境設(shè)定大小均不同；α為DVL 結(jié)構(gòu)決定的固定波束發(fā)射角度，通常為60 °。

通過式(1)可推出四個波束的多普勒頻移值的計算公式：

基于ST-EKF 的SINS/DVL 緊組合導(dǎo)航系統(tǒng)誤差狀態(tài)方程為：

其中，x為狀態(tài)向量；F為狀態(tài)轉(zhuǎn)移矩陣，可參考文獻[5]；G為系統(tǒng)噪聲轉(zhuǎn)移矩陣；w為系統(tǒng)噪聲；Q為系統(tǒng)噪聲對應(yīng)的協(xié)方差矩陣。各個量的定義為：

基于ST-EKF 的SINS/DVL 緊組合導(dǎo)航將DVL 測量獲得的四個波束的頻移信息與慣導(dǎo)系統(tǒng)速度轉(zhuǎn)化得到的頻移差值作為量測量。量測方程為：

其中，δz為量測量；H為量測矩陣；v為量測噪聲；R為量測噪聲對應(yīng)的協(xié)方差矩陣。各個量的定義為：

2 基于波束多普勒頻移信息的SINS/DVL 緊組合波束缺失補償方案

為不同的故障情況提供相應(yīng)的解決方案，以提高波束故障時SINS/DVL 緊組合導(dǎo)航系統(tǒng)的導(dǎo)航精度。文獻[10]提出了基于四波束多普勒頻移的SINS/DVL 緊組合系統(tǒng)，并與基于波束速度的SINS/DVL 緊組合系統(tǒng)進行了性能對比，實驗結(jié)果表明在波束正常返回信息和部分波束信息缺失情況下，基于波束多普勒頻移的SINS/DVL 緊組合系統(tǒng)的導(dǎo)航精度優(yōu)于基于波束速度的SINS/DVL 緊組合系統(tǒng)。為此，本文采用基于波束多普勒頻移的重構(gòu)方案。

1）單波束頻移信息缺失

當(dāng)DVL 單波束頻移信息缺失時，可以通過四波束Janus 陣列的結(jié)構(gòu)特性，計算出缺失波束的多普勒頻移信息。根據(jù)式(2)可得出缺失波束頻移信息fves為：

其中，i、j表示為與故障波束相鄰的波束，k為與故障波束相對的波束。通過式(19)可完成對應(yīng)單波束缺失的重構(gòu)。

2）相鄰兩波束頻移信息缺失

在多波束信息缺失時，需引入慣導(dǎo)速度信息才可重構(gòu)。將已知波束的多普勒頻移信息代入式(1)，結(jié)合式(2)對缺失的波束頻移信息進行計算，通過下述公式對缺失波束的頻移信息進行重構(gòu)。

當(dāng)相鄰雙波束k與l(k,l?{ [1,2],[2,3],[3,4],[4,1]})的頻移信息缺失時：

其中，k、l分別為與i、j呈對角線關(guān)系的故障波束；為慣導(dǎo)解算出的速度向量投影至b系后的天向速度分量。

3）相對兩波束頻移信息缺失

當(dāng)相對雙波束缺失時，若DVL 配置為“×”，重構(gòu)缺失波束的頻移信息公式為：

當(dāng)相對雙波束缺失時，若DVL 配置為“+”，重構(gòu)缺失波束的頻移信息公式為：

4）三波束頻移信息缺失

在兩波束頻移信息缺失和三波束頻移信息缺失時，提供的補償方法需引入慣導(dǎo)解算出的載體速度對缺失頻移信息進行重構(gòu)，但慣導(dǎo)解算出的載體速度與載體真實速度之間存在誤差，導(dǎo)致重構(gòu)的頻移信息不能準確地反映真實值，造成量測噪聲方差矩陣的噪聲特性發(fā)生變化，影響SINS/DVL 緊組合導(dǎo)航精度。因此，在重構(gòu)波束頻移信息的同時，應(yīng)適當(dāng)調(diào)節(jié)相應(yīng)的量測噪聲方差矩陣。

3 粒子群算法對噪聲方差矩陣Q 和R 的優(yōu)化

經(jīng)過上述波束信息缺失補償方法后，基于ST-EKF的SINS/DVL 緊組合導(dǎo)航系統(tǒng)的噪聲方差矩陣應(yīng)進行適當(dāng)調(diào)節(jié)，但其調(diào)節(jié)程度無法通過反復(fù)試驗或者依據(jù)前人給出的經(jīng)驗值進行設(shè)定。若設(shè)置不當(dāng)將影響ST-EKF 的濾波修正速度，使濾波過程不穩(wěn)定甚至發(fā)散。為此，本文采用粒子群優(yōu)化算法對噪聲方差矩陣Q和R進行實時優(yōu)化，以提高經(jīng)過波束缺失補償方法后的SINS/DVL 緊組合導(dǎo)航系統(tǒng)的導(dǎo)航精度。

目標函數(shù)的設(shè)定是粒子群算法的關(guān)鍵。上述基于ST-EKF 的SINS/DVL 緊組合導(dǎo)航系統(tǒng)模型中，將卡爾曼濾波器中的噪聲方差矩陣Q和R作為待優(yōu)化參數(shù)，對wa、wg、進行尋優(yōu)。

在對缺失波束的頻移信息重構(gòu)后，使用文獻[13]提出的目標函數(shù)對噪聲方差矩陣Q和R實時優(yōu)化，其目標函數(shù)為：

其中，yk為DVL 測量速度在n系的投影；M為當(dāng)前進行過的卡爾曼濾波次數(shù)；v（Qi,Ri）為ST-EKF 濾波后估計的載體速度。

以缺失3、4 波束的頻移信息為例，使用式(23)作為目標函數(shù)對噪聲方差矩陣Q和R進行實時優(yōu)化時，將已知波束的頻移信息的fd1、fd2代入式(20)，得到重構(gòu)后的fd3、fd4：

將式(24)代入式(1)，計算出當(dāng)前DVL 測量速度為：

由式(25)可以得到當(dāng)前DVL 測量速度在n系的投影：

將式(26)代入式(23)，則當(dāng)前目標函數(shù)的初始狀態(tài)為：

其中，i表示為當(dāng)前進行過的卡爾曼濾波次數(shù)；fd1(Qi,Ri)、fd2(Qi,Ri)、fd3(Qi,Ri)、fd4(Qi,Ri)表示為ST-EKF 濾波后估計的DVL 四個波束的多普勒頻移值。尋優(yōu)后目標函數(shù)值越小，重構(gòu)后的噪聲方差矩陣Q和R約接近于當(dāng)前的真實情況。仍以缺失3、4波束頻移信息為例，將式(24)代入式(28)。若計算出的DVL 測量值為真實值，進行尋優(yōu)時目標函數(shù)的初始狀態(tài)為：

對比式(27)和式(29)可以看出，在相同條件下，本文提出的目標函數(shù)初始狀態(tài)中不包含這一誤差項，能有效提高粒子群優(yōu)化算法的優(yōu)化效率和精度。

在波束故障條件下，基于ST-EKF 的SINS/DVL緊組合導(dǎo)航系統(tǒng)的優(yōu)化方法流程圖如圖2 所示。

圖2 波束故障條件下基于ST-EKF的SINS/DVL緊組合導(dǎo)航優(yōu)化方法Fig.2 Optimization method of SINS/DVL tightly coupled navigation based on ST-EKF under beam fault condition

4 實驗結(jié)果與分析

4.1 仿真實驗設(shè)置

為了驗證本文方法的可行性，設(shè)置如圖3 所示的仿真運動軌跡。

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

具體仿真條件設(shè)置如下：

1）軌跡設(shè)置

運動總時間為3600 s，起點位置：L0＝ 30.434°、λ0＝ 111.185 °、h0＝-1 0 m，初始速度：VE0＝ 0、VN0＝ 0、VU0＝ 0，初始姿態(tài)：俯仰角θ＝ 0 °、橫滾角γ＝ 0 °、航向角ψ＝135 °，初始姿態(tài)失準角：φx＝ 0.01°、φy＝-0 .01 °、φz＝ 0.1 °。速度變化曲線如圖4 所示。

圖4 仿真運動軌跡速度變化曲線Fig.4 Simulation running track velocity curve

1）慣性測量單元（Inertial Measurement Unit,IMU）設(shè)置

IMU 采樣頻率為200 Hz，陀螺零偏為0.02 °/h，陀螺的隨機游走系數(shù)為加速度計零偏為100 μg，加速度計的隨機游走系數(shù)為

2）DVL 設(shè)置

DVL 采樣頻率為1 Hz，測速誤差為0.005 m/s 的隨機噪聲。

3）粒子群優(yōu)化算法設(shè)置

設(shè)定初始種群個數(shù)M＝20，粒子的個人學(xué)習(xí)因子c1＝1.9，粒子的社會學(xué)習(xí)因子c2＝1.9，粒子的最大速度Vmax＝ 0.01，尋優(yōu)終止條件：設(shè)定為最大迭代次數(shù)20，慣性權(quán)重ω＝ 0.8。

4）DVL 工作狀況設(shè)置

考慮到DVL 受水下環(huán)境影響會出現(xiàn)波束頻移信息缺失的情況，設(shè)定在400～600 s 時DVL 的fd2、fd3、fd4缺失；1700～1900 s時DVL的fd4缺失；2500～2700 s時DVL 的fd3、fd4缺失。

4.2 仿真結(jié)果及分析

根據(jù)設(shè)定的仿真條件，對比下面五種SINS/DVL緊組合導(dǎo)航方案的導(dǎo)航精度：

導(dǎo)航方案1：當(dāng)波束的多普勒頻移信息缺失時，不對缺失信息進行重構(gòu)，僅以返回的多普勒頻移信息為基礎(chǔ)，使用噪聲方差矩陣Q和R的經(jīng)驗值進行基于EKF 的SINS/DVL 緊組合導(dǎo)航；

導(dǎo)航方案2：當(dāng)波束的多普勒頻移信息缺失時，對缺失信息進行重構(gòu)后，使用噪聲方差矩陣Q和R的經(jīng)驗值進行基于EKF 的SINS/DVL 緊組合導(dǎo)航；

導(dǎo)航方案3：當(dāng)波束的多普勒頻移信息缺失時，對缺失信息進行重構(gòu)后，使用噪聲方差矩陣Q和R的經(jīng)驗值進行基于ST-EKF 的SINS/DVL 緊組合導(dǎo)航；

導(dǎo)航方案4：當(dāng)波束的多普勒頻移信息缺失時，對缺失信息進行重構(gòu)后，使用式(23)作為目標函數(shù)進行PSO 算法，對噪聲方差矩陣Q和R進行實時優(yōu)化，再進行基于ST-EKF 的SINS/DVL 緊組合導(dǎo)航；

導(dǎo)航方案5：當(dāng)波束的多普勒頻移信息缺失時，對缺失信息進行重構(gòu)后，使用式(29)作為目標函數(shù)進行PSO 算法，對噪聲方差矩陣Q和R進行實時優(yōu)化，再進行基于ST-EKF 的SINS/DVL 緊組合導(dǎo)航。

上述五種導(dǎo)航方案的東向定位誤差Error-PE與北向定位誤差Error-PN結(jié)果如圖5-6 所示。設(shè)定的北向速度遠大于東向速度，受載體速度變化的影響，最終北向定位誤差大于最終的東向定位誤差，導(dǎo)航方案5的最終東向定位誤差為0.437 m，最終北向定位差為14.201 m。

圖5 五種導(dǎo)航方案的仿真軌跡東向定位誤差Fig.5 The east orientation error of simulation trajectory of five navigation schemes

圖6 五種導(dǎo)航方案的仿真軌跡北向定位誤差Fig.6 The simulation trajectory northbound positioning error of five navigation schemes

由圖5-6 可知，所有方案在波束故障時，導(dǎo)航定位精度均有下降。對比方案1 與方案2，對缺失波束信息進行重構(gòu)后的方案2 導(dǎo)航精度遠高于方案1。對比方案2 與方案3，ST-EKF 的濾波效果好于EKF，提高了最終導(dǎo)航精度。對比方案3 與方案4，方案4 的東向與北向的導(dǎo)航精度明顯提升。對比方案4 與方案5，方案5 的東向與北向定位精度進一步提升。在對缺失波束信息補償后但未對噪聲方差矩陣Q和R進行實時優(yōu)化的方案2 與3 和對噪聲方差矩陣Q和R進行實時優(yōu)化的方案4 與5 對比后，對噪聲方差矩陣Q和R進行實時優(yōu)化后再進行卡爾曼濾波的方案能有效抑制運動狀態(tài)改變時產(chǎn)生的誤差峰值，提高了最終導(dǎo)航精度。

五種導(dǎo)航方案的最終導(dǎo)航軌跡對比如圖7 所示。對比圖7 中五種導(dǎo)航方案的軌跡，所有軌跡在fd2、fd3、fd4缺失時軌跡偏移較大，而在fd3、fd4缺失與fd4缺失時，軌跡偏移較小。表1 給出了五種導(dǎo)航方案的均方根誤差統(tǒng)計。

表1 五種導(dǎo)航方案的姿態(tài)誤差、速度誤差、位置誤差的均方根誤差統(tǒng)計Tab.1 Root mean square error statistics of attitude error,velocity error and position error of five navigation schemes

圖7 五種導(dǎo)航方案軌跡對比Fig.7 Trajectory comparison of five navigation schemes

根據(jù)表1 可看出，在對噪聲方差矩陣Q和R進行實時優(yōu)化后再進行組合導(dǎo)航的方案，姿態(tài)誤差與速度誤差均有小幅減少。在進行噪聲方差矩陣Q和R優(yōu)化后，導(dǎo)航方案4 對天向位置誤差的抑制能力較差。本文提出的導(dǎo)航方案5 在一定程度上改善了天向位置誤差較大這一問題，水平兩方向的位置均方根誤差均小于其他導(dǎo)航方案，東向、北向和天向的位置均方根誤差比導(dǎo)航方案1 分別降低了88%、69%、89%，比導(dǎo)航方案2 分別降低了54%、52%、46%，比導(dǎo)航方案3分別降低了47%、38%、6%，比導(dǎo)航方案4 分別降低了22%、26%、36%。總的來說，導(dǎo)航方案5 的位置均方根誤差比導(dǎo)航方案1 降低了70.4%，比導(dǎo)航方案2 降低了51.7%，比導(dǎo)航方案3 降低了38.4%，比導(dǎo)航方案4 降低了26.7%。對于天向速度與天向位置兩者誤差較小的情況，是由于仿真條件設(shè)定載體為巡航模式，天向速度設(shè)定為0 m/s，并且在進行了缺失波束重構(gòu)后，量測信息是能夠提供天向約束的，所以在導(dǎo)航方案2～5 中天向速度與天向位置具有較高的精度，而未進行缺失信息重構(gòu)的導(dǎo)航方案1 對于天向速度與天向位置的精度較差。實驗結(jié)果證明這一方案有效提高了在波束故障條件下基于ST-EKF 的SINS/DVL 緊組合導(dǎo)航系統(tǒng)的導(dǎo)航精度。

5 結(jié)論

本文針對基于EKF 的SINS/DVL 緊組合導(dǎo)航系統(tǒng)受外部環(huán)境因素及波束故障影響，會出現(xiàn)精度下降甚至濾波發(fā)散的問題，將ST-EKF 應(yīng)用于SINS/DVL 緊組合導(dǎo)航系統(tǒng)。對波束的頻移信息重構(gòu)后，因引入慣導(dǎo)解算的載體速度信息，導(dǎo)致量測噪聲方差矩陣的噪聲特性發(fā)生變化的問題，使用粒子群優(yōu)化算法實時優(yōu)化系統(tǒng)的噪聲方差矩陣Q和R，并根據(jù)四波束Janus配置的結(jié)構(gòu)特性，設(shè)計了適用于SINS/DVL 緊組合導(dǎo)航系統(tǒng)的目標函數(shù)。仿真實驗結(jié)果表明：本文方法提高了波束故障條件下基于ST-EKF 的SINS/DVL 緊組合導(dǎo)航系統(tǒng)的導(dǎo)航精度。