基于羅經(jīng)/DVL/USBL組合導(dǎo)航系統(tǒng)的DVL在線標定算法

張 舸，謝陽光，伊國興，李學(xué)成，王 尚

（1.哈爾濱工業(yè)大學(xué) 空間控制與慣性技術(shù)研究中心，哈爾濱 150001；2.中國航空工業(yè)集團公司 西安飛行自動控制研究所，西安 710065；3.中海輝固地學(xué)服務(wù)（深圳）有限公司，深圳 518067）

隨著人類對海洋資源的開發(fā)與利用，海底地形勘探、管線巡檢、資源開發(fā)、深海作戰(zhàn)等都對水下定位導(dǎo)航技術(shù)提出了新的要求[1,2]，基于多傳感器數(shù)據(jù)融合的水下組合導(dǎo)航系統(tǒng)是實現(xiàn)水下高精度導(dǎo)航的重要研究方向[3-5]。作為水下導(dǎo)航的重要傳感設(shè)備，基于聲學(xué)的多普勒計程儀(Doppler Velocity Log,DVL)通過向水底發(fā)射聲波并分析其回波的多普勒頻移來解算水下運載體相對于水底的速度，其測量誤差主要由刻度因數(shù)誤差和安裝角度誤差組成，對這兩項誤差參數(shù)的精確估計可以極大程度提高水下導(dǎo)航的精度。

J.Gong 等人提出了基于卡爾曼濾波器(Kalman Filter,KF)的DVL 在線標定算法，將DVL 的量測速度與捷聯(lián)慣導(dǎo)系統(tǒng)(Strap-down Inertial Navigation System,SINS)的速度輸出進行模型解算，獲得誤差參數(shù)的在線估計，標定后定位精度優(yōu)于0.5%航程，但該算法的估計精度隨著時間的增長逐漸降低[6]。J.Liu 等人在此基礎(chǔ)上提出了基于全球?qū)Ш叫l(wèi)星系統(tǒng)(Global Navigation Satellite System,GNSS)輔助下的三點在線標定方法，將終點定位精度提高至0.07%航程，但是該方法需要水下運載體進行兩次上浮，不適用于大深度工作環(huán)境[7]。P.Liu 在此基礎(chǔ)上提出了SINS/GNSS 輔助下基于KF 的DVL 在線標定算法，并通過可觀測性矩陣分析了標定軌跡對DVL 誤差參數(shù)估計精度的影響[8]。B.Xu 等人提出了SINS/GNSS 輔助下基于四元數(shù)的DVL標定算法，使安裝誤差角的估計精度得到大幅提高[9]。以上研究均是基于速度觀測進行的DVL 標定，其估計精度和穩(wěn)定性均受量測噪聲影響，而B.Xu 等人提出了基于魯棒不變擴展卡爾曼濾波算法的DVL 標定方法，通過引入李群理論有效降低了由DVL 量測異常值對標定精度的影響，有效提升了基于速度觀測進行DVL標定的精度[10]。徐曉蘇等人提出了SINS/GNSS輔助下基于梯度下降四元數(shù)的DVL 標定算法，在位置觀測標定方法下安裝角度誤差估計精度提高至0.02 °度以內(nèi)[11]。不同于以上傳統(tǒng)的標定算法，J.Liu等人提出了基于遺傳算法的DVL 誤差參數(shù)標定方法[12]，D.Li 等人提出了基于粒子群優(yōu)化的組合導(dǎo)航系統(tǒng)的DVL 標定方法[13]，B.Wang 等人提出了一種在SINS 輔助下基于GA-SVR 算法的回歸預(yù)測器的無模型DVL 標定算法[14]，三者均能獲得較高的估計精度，但對水下運載體的計算資源要求較高，且均為離線方法，在工程使用中仍有困難。

在工程應(yīng)用中，出于對水下設(shè)備成本控制的考慮，采用了羅經(jīng)代替慣導(dǎo)，與DVL 和超短基線定位系統(tǒng)（Ultra Short Base Line，USBL）組合導(dǎo)航。在該組合中，由于實際工況下USBL 的工作穩(wěn)定性往往欠佳，且傳感器數(shù)據(jù)波動較大，無法通過其位置差分獲得有效的運載體速度數(shù)據(jù)，不能為標定DVL 提供有效的速度觀測條件，因此無法采用傳統(tǒng)基于速度觀測的方法進行 DVL 誤差參數(shù)的標定。本文基于羅經(jīng)/DVL/USBL 的組合方式，將DVL 的誤差參數(shù)通過羅經(jīng)量測傳播到航位推算位置信息中，并通過設(shè)計的雙環(huán)路卡爾曼濾波器以基于USBL 位置觀測的方式估計得到DVL 的誤差參數(shù)和導(dǎo)航位置信息。

1 DVL 誤差模型參數(shù)化表征

1.1 DVL 量測輸出模型建模

DVL 是水下運載體的速度量測單元，根據(jù)其工作深度的不同輸出水下運載體相對于水底或水流速度。本文涉及的遙控無人潛水器（Remote Operated Vehicle，ROV）工作在深度較大的海域，與海底之間的距離滿足跟蹤模式的工作閾值，DVL 輸出ROV 相對于海底的速度信息。

DVL 的理想輸出是基于 DVL 量測坐標系O-X d Yd Zd下水下運載體的三軸線速度Vd，當只考慮DVL 的安裝偏角誤差和刻度因數(shù)誤差時，其與載體坐標系O-X bY b Zb下的理想速度Vb之間滿足：

式中，kδ為DVL 的刻度因數(shù)誤差，反映了DVL 輸出值與真實值之間的比例誤差，是傳感器的固有特性，這里考慮三個方向的刻度因數(shù)誤差為相同值。δ A為DVL 的安裝角度誤差矩陣，反映了由DVL 量測坐標系與載體坐標系各軸之間的偏角θdx、θdy、θdz產(chǎn)生的誤差，其定義如式(2)所示，當ROV 處于定深度低速航行時，安裝角度誤差只考慮航向偏角的影響。

綜上，DVL 的量測輸出可表示為：

1.2 DVL 誤差傳播模型建模

根據(jù)上述的DVL 模型，對DVL 的標定即對刻度因數(shù)誤差δk和z 軸安裝角度誤差θdz的估計。將這兩個DVL 誤差參數(shù)轉(zhuǎn)化為組合導(dǎo)航系統(tǒng)狀態(tài)變量的一部分，并分析其傳播過程，則可以實現(xiàn)在ROV 組合導(dǎo)航解算的同時完成DVL 的在線標定。

給定初始位置，以DVL 和羅經(jīng)的輸出作為數(shù)據(jù)來源進行航位推算，在忽略其他影響因素的情況下，航位推算得到的位置與真實位置P之間的誤差δ P可以認為是由DVL 的兩個誤差參數(shù)引起的，即DVL 的誤差參數(shù)最終向組合導(dǎo)航系統(tǒng)位置誤差傳播。

組合導(dǎo)航系統(tǒng)的位置誤差變化率如下所示：

其中，和分別表示基于DVL 輸出計算得到的導(dǎo)航坐標系下的東向速度和北向速度。L和h分別表示ROV 當前所處的緯度和高度。Rm和Rn分別表示當前位置的子午圈地球曲率半徑和卯酉圈地球曲率半徑。

在式(4)中，位置誤差轉(zhuǎn)換矩陣和速度誤差轉(zhuǎn)換矩陣均可由導(dǎo)航系統(tǒng)當前的航位推算、DVL 速度量測等信息計算獲得，位置誤差δ P在計算中取自上一時刻的迭代值，導(dǎo)航坐標系下速度誤差δVn的計算如式(8)所示。式中為基于羅經(jīng)輸出計算的方向余弦矩陣。Cφ為航向角量測噪聲轉(zhuǎn)換矩陣。wφ為羅經(jīng)的航向角量測噪聲，是標準差為σφ的高斯白噪聲。

其中，航向角量測噪聲轉(zhuǎn)換矩陣Cφ是考慮羅經(jīng)存在航向角量測噪聲δφ時對方向余弦矩陣造成的影響，二者滿足如下關(guān)系：

式中，φ表示航向角，θ表示俯仰角，γ表示滾轉(zhuǎn)角。

為DVL 誤差參數(shù)矩陣，其定義為：

其中，誤差參數(shù)矩陣中各元素的值為：

式中，D1為DVL 誤差參數(shù)傳遞矩陣，D2為航位推算量測噪聲傳遞矩陣，定義分別如式(13)(14)所示：

其中，VdT是DVL 量測數(shù)據(jù)轉(zhuǎn)換矩陣，其定義為：

根據(jù)式(12)可知，DVL 的誤差參數(shù)經(jīng)過轉(zhuǎn)化最終傳播到位置誤差的解算迭代過程中。

當組合導(dǎo)航解算迭代完成后，通過式(16)可以得到DVL 的刻度因數(shù)誤差和z 軸安裝角度誤差。

2 基于位置觀測的DVL 標定與組合導(dǎo)航算法

如圖1 所示，為基于位置觀測的DVL 在線標定與組合導(dǎo)航算法框圖。算法采用雙環(huán)路卡爾曼濾波器架構(gòu)，內(nèi)環(huán)進行組合導(dǎo)航位置解算，外環(huán)進行DVL的誤差參數(shù)估計，通過基于位置觀測的DVL 誤差傳播模型設(shè)計卡爾曼濾波器，在實現(xiàn)ROV 的組合導(dǎo)航的同時進行DVL 的在線標定。

圖1 DVL 在線標定組合導(dǎo)航系統(tǒng)框圖Fig.1 Block diagram of DVL online calibration integrated navigation system

算法過程中，由羅經(jīng)和DVL 組成的航位推算進行ROV 的位置更新與迭代，并與USBL 的位置量測在卡爾曼濾波器中進行數(shù)據(jù)融合進而得到平滑且不發(fā)散的組合導(dǎo)航位置。與此同時，基于USBL 的位置觀測，卡爾曼濾波器將傳播到航位推算中的DVL 誤差參數(shù)進行估計解算并反饋給ROV 的速度解算過程，在不斷的迭代中，算法逐漸穩(wěn)定，濾波器將輸出滿足精度要求的DVL 誤差參數(shù)估計值。

2.1 狀態(tài)方程

選擇DVL 的誤差參數(shù)向量和ROV 的位置誤差作為狀態(tài)變量，即：

式中，Lδ、δλ和hδ分別為ROV 航位推算位置的緯度誤差、經(jīng)度誤差和高度誤差。

則DVL 在線標定組合導(dǎo)航系統(tǒng)的狀態(tài)方程為：

2.2 量測方程

2.3 可觀測性分析

根據(jù)分段線性定常系統(tǒng)（PWCS）的可觀測性分析方法，系統(tǒng)被分為若干個時間段，在每一個時間段內(nèi)，狀態(tài)轉(zhuǎn)移矩陣和量測矩陣均為常值，因此可以根據(jù)系統(tǒng)的狀態(tài)轉(zhuǎn)移矩陣和量測矩陣得到k時段導(dǎo)航系統(tǒng)的可觀測性矩陣為：

組合導(dǎo)航系統(tǒng)的全局可觀測性矩陣可由各時段的可觀測性矩陣構(gòu)成，如下所示：

當系統(tǒng)采樣時間為1 s，ROV 以1 m/s 的前向速度并保持30 °航向角進行7200 s 勻速直線運動時，對其全局可觀測性矩陣進行SVD 分解，得到5 個奇異值分別為170.57、84.87、84.84、52.01 和0.03。

如圖2 所示，為DVL 在線標定組合導(dǎo)航系統(tǒng)全局可觀測性矩陣Qo經(jīng)過SVD 分解后所得右奇異值矩陣V中各奇異值對應(yīng)其正交基元素的取值分布。其中橫坐標表示奇異值的序號，即矩陣V的列序號，縱坐標表示各列向量元素的取值。根據(jù)系統(tǒng)狀態(tài)變量的順序，圖中的藍色、橙色、黃色、紫色和綠色柱形分別代表刻度因數(shù)誤差、安裝角度誤差、緯度誤差、經(jīng)度誤差和高度誤差相關(guān)量。由柱形圖的分布可以得知，第一個奇異值170.57 對應(yīng)刻度因數(shù)誤差的可觀測度，第二奇異值84.87 對應(yīng)經(jīng)度誤差的可觀測度，第三個奇異值84.84 對應(yīng)緯度誤差的可觀測度，第四個奇異值52.01 對應(yīng)高度誤差的可觀測度，第五個奇異值0.03對應(yīng)安裝角度誤差的可觀測度。由于系統(tǒng)狀態(tài)變量的量綱不同，其奇異值的大小不能直接作為比較其可觀測度的依據(jù)，只能證明在該條件下系統(tǒng)的五個狀態(tài)變量是完全可觀的。

圖2 DVL 在線標定組合導(dǎo)航系統(tǒng)可觀測度分析Fig.2 Observability analysis of DVL online calibration integrated navigation system

3 仿真驗證

3.1 仿真條件與精度評價指標

為了驗證組合導(dǎo)航系統(tǒng)能夠在進行組合導(dǎo)航位置解算的同時在線估計DVL 的誤差參數(shù)，進行了相關(guān)仿真實驗，仿真條件如下：

a) ROV 的起始位置為東經(jīng) 116.35 °，北緯39.98 °，高度-1000 m；

b) ROV 保持前向速度1 m/s，航向角30 °進行勻速直線運動；

c) DVL 量測噪聲各軸標準差均為0.1 m/s，刻度因數(shù)誤差0.015，z 軸安裝角度誤差0.75 °；

d) 羅經(jīng)量測噪聲各軸標準差均為0.1°；

e) USBL 量測噪聲水平方向標準差為1 m，高度方向標準差為0.1 m；

f) 系統(tǒng)采樣周期為1 s，仿真時間7200 s。

對于導(dǎo)航算法的定位精度，本文同時采用終點相對精度和全局相對精度來進行表征。終點相對精度是指，在導(dǎo)航結(jié)束時由導(dǎo)航算法解算的位置與真實位置之間的距離與總航程之比，該指標僅能反映導(dǎo)航算法在導(dǎo)航終點的定位精度，無法反映導(dǎo)航過程中算法的穩(wěn)定性。全局相對精度是指，在導(dǎo)航過程中的每一個時段，計算其相對定位精度并在導(dǎo)航結(jié)束后對所有時段的導(dǎo)航精度求平均值，如式(24)所示，該指標一定程度上反映了導(dǎo)航算法的精度和穩(wěn)定性。

對于DVL 誤差參數(shù)的估計精度，一方面由算法估計值直接反映，另一方面將估計值對DVL 模型進行校正，并計算校正后的航位推算數(shù)據(jù)，將定位精度與校正前對比可間接反映DVL 誤差參數(shù)的估計結(jié)果。

3.2 仿真結(jié)果與分析

圖3 是仿真實驗中DVL 刻度因數(shù)誤差和z 軸安裝角度誤差的估計結(jié)果，具體數(shù)值如表1 所示。

圖3 DVL 誤差參數(shù)估計Fig.3 Estimation of DVL error parameter

表1 仿真實驗DVL 誤差參數(shù)Tab.1 DVL error parameters in simulation experiment

由表1 可知，在基于組合導(dǎo)航位置觀測的DVL標定算法估計結(jié)果中，刻度因數(shù)誤差最終收斂于0.0157，z 軸安裝角度誤差最終收斂于0.8381 °。與仿真設(shè)定值相比，估計結(jié)果基本達到要求。

傳統(tǒng)基于速度觀測的DVL 標定算法中，z 軸安裝角度誤差的估計值為0.7975 °，略優(yōu)于前者，刻度因數(shù)誤差的估計值為-0.7567，顯然與實際情況不符。由于觀測速度引入了USBL 的圓概率誤差，在ROV 低速行駛的情況下，有效數(shù)據(jù)被誤差淹沒導(dǎo)致該算法無法正確估計刻度因數(shù)誤差的值。

因此基于速度觀測的DVL 標定方式不適用于羅經(jīng)/DVL/USBL 組合導(dǎo)航系統(tǒng)中，而基于位置觀測的DVL 標定方式則能夠有效地規(guī)避直接觀測速度帶來的誤差，采用雙環(huán)路卡爾曼濾波器架構(gòu)通過對組合導(dǎo)航位置的解算與觀測完成對DVL 誤差參數(shù)的有效估計。

圖4 是仿真實驗中ROV 的真實運動軌跡、USBL量測軌跡、組合導(dǎo)航解算軌跡以及DVL 標定前后航位推算軌跡對比圖。圖中，DVL 標定之前的航位推算軌跡隨著時間的增長逐漸偏離真實軌跡；USBL 的量測軌跡雖然沒有發(fā)散，但圍繞真實軌跡有較大波動；組合導(dǎo)航解算軌跡與真實軌跡具有較高重合度且相對USBL 數(shù)據(jù)更加平滑；DVL 標定后的航位推算軌跡雖未隨時間增長而發(fā)散，但與真實軌跡仍有一定誤差。

圖4 仿真實驗軌跡對比Fig.4 Comparison of tracks in Simulation experiment

圖5 是仿真實驗中組合導(dǎo)航以及DVL 標定前后航位推算解算位置與真實位置之間的誤差。圖中，DVL 標定之前的航位推算軌跡隨著時間的增加，其定位誤差逐漸累積且最終發(fā)散；組合導(dǎo)航與DVL 標定之后航位推算的定位誤差均未發(fā)散，但航位推算的定位誤差略大于組合導(dǎo)航，具體定位精度如表2 所示。

圖5 仿真實驗定位誤差對比Fig.5 Comparison of positioning error in simulation experiment

表2 仿真實驗導(dǎo)航定位精度Tab.2 Accuracy of navigation in simulation experiment

根據(jù)定位精度的計算結(jié)果可以證明，在仿真實驗中DVL 在線標定組合導(dǎo)航算法解算得到的位置具有較高定位精度和算法穩(wěn)定性，同時該算法估計得到的DVL 參數(shù)誤差能夠較好地補償航位推算的定位誤差，使DVL 標定后的航位推算的定位精度大幅提升，并具有良好的算法穩(wěn)定性。

3 海試驗證

3.1 測試條件

為了驗證DVL 在線標定組合導(dǎo)航系統(tǒng)的實用性，2021 年11 月在南海某海域利用ROV 進行了試驗?，F(xiàn)場配置狀態(tài)如圖6 所示。

圖6 海試ROV 傳感器配置Fig.6 Configuration of ROV sensors in sea trial

具體測試傳感器設(shè)備條件如下：

a) CFCV1500 型ROV 設(shè)備，作業(yè)水深可達1500 m，速度最高3 節(jié)；

b) 全國產(chǎn)化320B 型光纖羅經(jīng)設(shè)備，航向角量測精度優(yōu)于0.3 °；

c) RDI 1200kHz navigator 型DVL 設(shè)備，長時精度約為±0.2%±0.1 cm/s；

d) Sonardyne Ranger 2 HPT 5000 型USBL 設(shè)備，定位精度優(yōu)于15 mm。

3.2 測試結(jié)果與分析

圖7 和圖8 分別為海試過程中羅經(jīng)、DVL和USBL的有效量測數(shù)據(jù)，由量測數(shù)據(jù)可知，ROV 在海試過程中保持245 °左右的航向角向西南方向大致以0.2 m/s的速度運動。

圖7 海試羅經(jīng)與DVL 有效量測數(shù)據(jù)Fig.7 Measurement data of compass and DVL in sea trial

圖8 海試USBL 有效量測數(shù)據(jù)Fig.8 Measurement data of USBL in sea trial

圖9 是海試中DVL 刻度因數(shù)誤差和z 軸安裝角度誤差的估計結(jié)果，其中刻度因數(shù)誤差最終收斂于0.0043，z 軸安裝角度誤差最終收斂于-1.9641 °。在無法驗證其真實值的情況下，DVL 誤差參數(shù)的估計精度只能以標定后航位推算的定位精度間接體現(xiàn)。

圖9 海試DVL 誤差參數(shù)估計Fig.9 Estimation of DVL error parameter in sea trial

圖10 是海試中ROV 的USBL 量測軌跡、組合導(dǎo)航解算軌跡以及DVL 標定前后航位推算軌跡對比圖。圖中，USBL 的量測軌跡波動較強，但依據(jù)傳感器的固有特性，其量測結(jié)果始終不會發(fā)散；組合導(dǎo)航解算軌跡與USBL 軌跡具有較高重合度，且比USBL 數(shù)據(jù)更加平滑，可以作為ROV 的真實位置估計；DVL 標定之前的航位推算軌跡隨著時間的增長逐漸偏離USBL 的量測軌跡；DVL 標定后的航位推算軌跡雖然沒有隨時間增長逐漸發(fā)散，但與USBL 量測軌跡和組合導(dǎo)航解算軌跡之間有明顯誤差。

圖10 海試軌跡對比Fig.10 Comparison of tracks in sea trial

圖11 是海試中DVL 標定前后航位推算解算位置與組合導(dǎo)航解算位置之間的誤差。圖中，DVL 標定前的航位推算軌跡隨著時間的增加，其定位誤差逐漸累積且最終發(fā)散；DVL 標定后航位推算的定位誤差得到了顯著抑制，具體定位精度如表3 所示。

圖11 海試定位誤差對比Fig.11 Comparison of positioning error in sea trial

表3 海試導(dǎo)航定位精度Tab.3 Accuracy of navigation in sea trial

根據(jù)USBL 量測軌跡與組合導(dǎo)航解算軌跡的分析可以證明，在海試中DVL 在線標定組合導(dǎo)航算法解算得到的位置具有較高定位精度和算法穩(wěn)定性。根據(jù)定位精度的計算結(jié)果可以證明，在海試中DVL 在線標定組合導(dǎo)航算法估計得到的DVL 參數(shù)誤差能夠較好補償航位推算的定位誤差，標定后航位推算的定位精度大幅提升，并具有良好的算法穩(wěn)定性。

4 結(jié)論

針對大深度水下工作環(huán)境中ROV 組合導(dǎo)航與DVL 標定問題，本文提出了一種基于羅經(jīng)/DVL/USBL組合導(dǎo)航系統(tǒng)的DVL 誤差參數(shù)在線標定算法，通過建立DVL 的刻度因數(shù)誤差和安裝角度誤差的傳播模型，將DVL 的誤差參數(shù)引入基于羅經(jīng)和DVL 的航位推算的迭代中，并以USBL 的位置輸出作為觀測量，對包括DVL 誤差參數(shù)和航位推算定位誤差在內(nèi)的狀態(tài)變量進行卡爾曼濾波，進而實現(xiàn)ROV 組合導(dǎo)航輸出的同時進行DVL 的在線標定。

仿真結(jié)果表明，組合導(dǎo)航位置輸出的終點相對定位精度為0.01%總航程，全局相對定位精度為0.13%總航程；DVL 刻度因數(shù)的估計值為0.0157，估計精度優(yōu)于0.0007；z 軸安裝角度誤差的估計值為0.8381 °，估計精度優(yōu)于0.09 °；以估計值對DVL 進行標定后航位推算的終點相對定位精度由2.03%總航程大幅提高至0.03%總航程，全局相對定位精度由1.90%總航程提高至0.26%總航程。海試結(jié)果表明，組合導(dǎo)航位置輸出與USBL 的位置量測具有較好的重合度，且軌跡曲線較為平滑；DVL 刻度因數(shù)的估計值為0.0043，z 軸安裝角度誤差的估計值為-1.9641 °；以估計值對DVL 進行標定后航位推算的終點相對定位精度由3.05%總航程大幅提高至0.33%總航程，全局相對定位精度由3.55%總航程提高至0.70%總航程。證明本文提出的基于ROV組合導(dǎo)航系統(tǒng)的DVL 在線標定算法能夠在輸出組合導(dǎo)航位置的同時進行DVL 標定，且具有較高的導(dǎo)航定位精度、DVL 誤差參數(shù)估計精度和算法穩(wěn)定性。

本文只對DVL 的z 軸安裝角度誤差進行了估計，同時在誤差傳播函數(shù)推導(dǎo)過程中忽略了安裝角度誤差的二階小量，后續(xù)的研究將在此基礎(chǔ)上建立更精確的誤差參數(shù)傳遞模型，并考慮在復(fù)雜運動軌跡下對DVL的全部安裝角度誤差進行更高精度的估計。