SVR輔助SINS-DVL的水下機器人組合導航方法

魏奧博，鄭 榮

(1. 中國科學院沈陽自動化研究所機器人學國家重點實驗室，遼寧 沈陽 110016；2. 中國科學院機器人與智能制造創新研究院，遼寧 沈陽 110016；3. 中國科學院大學，北京 100049)

0 引 言

隨著人類對海洋的開發日益增多，水下機器人越來越多被用來執行水下任務，如海洋礦產勘探，海洋數據收集等。精確可靠的導航系統是水下機器人成功完成任務不可或缺的部分[1]。

由于捷聯慣導（SINS）具有高可靠性和完全獨立等優點，SINS被廣泛應用在水下機器人導航中。SINS通過慣導計算機采集的慣性測量單元（IMU）的輸出信息，進行數值積分求解水下機器人的姿態、速度和位置等導航參數。隨著時間推移，IMU中的陀螺和加速度計的誤差會逐漸增加，這將導致SINS的導航誤差會逐漸增大。為了提高導航性能，有必要引入輔助傳感器來限制誤差增長。目前水下機器人導航系統多采用由多個導航傳感器組成的綜合導航系統[2]。多普勒計程儀（DVL）是一種高精度的速度測量儀器。由SINS和DVL結合的組合導航系統，于20世紀初在丹麥的MARIDAN AUV上成功應用后，在水下機器人領域的應用越來越廣泛[3]。

DVL是根據多普勒效應，測量出水下機器人相對于海底或者海流的速度。通過卡爾曼濾波器（KF）采用集中濾波的方式將DVL提供的高精度的速度數據和SINS的數據進行融合能夠有效抑制導航系統的累計誤差。由于DVL數據的更新頻率相對于SINS較低，會影響AUV的組合導航精度[4]。對于DVL處在對底工作模型的情況，由于DVL對海底環境的依賴會使DVL在使用過程中出現以下問題[4]：

1）由于海底地形變化導致DVL返回數據頻率不固定。

2）由于海底存在強烈的吸波物質，導致DVL發出的聲波無法被返回。

3）當海底存在海溝超出了DVL的量測范圍，導致DVL信號丟失。

當上述情況發生時，濾波器無法得到頻率穩定、精度可靠的DVL數據，情況嚴重時會導致AUV測速數據無效，這不僅會影響濾波器的穩定，同時會帶來較大的導航誤差。解決上述問題主要有2種方法：1種為隔離法，即直接隔離數據失效的DVL；第2種方法為替換法，即替換掉DVL的測速數據[4]。采用第1種方法，組合導航系統將變為慣性導航，導航精度會大大降低。第2種方法是目前大部分學者采用的。考慮到硬件成本，一般不采用通過增加傳感器的方式解決上述問題。目前大多數情況下，通過設計一個估計器的方式來處理頻率不穩以及數據失效等問題。Semeniuk和Noureldin等[5]提出了一種基于人工智能的分段預測器來克服GPS衛星信號失效的問題。Hasan等[6]介紹了一種神經網絡模糊系統，用于預測GPS信號失效時INS的誤差。這些方法均可以解決傳感器信號不穩的問題，而不用了解傳感器的特性。基于上述智能方法在GPS/INS組合導航方面的廣泛使用，Zhu等[7]通過采用線性的偏最小二乘回歸估計器（PLSR）和支持向量（SVR）估計器相結合的方法，建立了慣導速度與多普勒速度輸出的映射關系，在DVL數據失效后實時估計水下機器人對底的速度。該方法通過仿真得到了較好的效果。該方法中PLSR算法隨著時間推移，當新信息加入時新信息對于擬合的影響較小。對于SVR算法，在線性過程中和PLSR算法共同使用時增加了計算機負荷造成了資源的不必要浪費。當水下機器人運動過程發生非線性過程時，如由勻速到加速的轉換過程，由于先前的訓練的數據處于線性過程，在線性到非線性的轉換過程起始時SVR表現并不十分好。

針對DVL相對于SINS數據刷新頻率不穩影響組合導航精度和由于一段時間內DVL數據失效導致組合導航精度降低等問題，本文提出一種SVR輔助SINS/DVL組合導航方法，并針對組合導航過程中由于運動變化引起的非線性問題，提出了一種動態改變SVR訓練數據集尺度和樣本的方法解決該問題。

1 SINS/DVL組合導航系統

水下機器人組合導航系統是由SINS和一些輔助傳感器構成，輔助傳感器是提高SINS精度必不可少的條件。水下機器人組合導航系統中常用的輔助傳感器包括DVL、深度計、全球定位系統（GPS）。由于GPS在水下無法使用，所以水下機器人在水下航行時無法通過GPS進行校準，長航程水機器人會定期浮上水面通過GPS對整個導航系統進行校準。深度計一般用來提供深度信息。多傳感器輔助的組合導航系統一般采用聯邦濾波算法[8]。本文主要討論SINS/DVL組合導航部分。

1.1 系統模型

組合導航一般有直接濾波算法和間接濾波算法。直接濾波具有簡單明了容易實現的特點，但該方法的系統方程一般為非線性，需要廣義濾波進行處理，而且采用的狀態變量是導航的輸出參數，有時差值會比較大（如姿態角），給計算帶來一定困難，影響估計的準確性，不適合有較高精度要求的場合。間接濾波算法以各子系統的誤差量作為系統狀態變量，即以導航輸出參數的誤差作為濾波估計對象。在間接法濾波中，卡爾濾波器接收導航子系統對同一導航輸出參數的差值，并經濾波計算得到誤差量的最優估計[9]。本文采用間接濾波算法，算法示意圖如圖1所示。

圖1 間接濾波算法Fig. 1 Indirect filtering algorithm

考慮建立慣性導航系統模型，采用狀態參數為17維，包括8個導航解誤差（經緯度、速度、姿態誤差）、3個陀螺漂移誤差、3個加速度誤差、DVL對地速度、偏流角、刻度系數誤差。

系統變量：

式中的變量是基于導航坐標系n系下建立的。

狀態方程：

式中F為系統狀態轉移矩陣W為系統噪聲矩陣。本文取SINS和DVL的東向和北向速度的差值：

量測方程：

式中：H為系統量測方程，V為觀測噪聲并滿足高斯白噪聲特性。本文的濾波方法采用卡爾曼濾波。

從式（1）可以看出，濾波器的狀態量均為微小量，濾波過程中傳感器數據刷新頻率發生變化或傳感器數據在一段時間內無效均會造成濾波器不穩定，發生誤差過大，濾波不收斂等現象，造成一段時間內導航誤差過大。

2 變訓練數據集的SVR預測方法

2.1 支持向量回歸（SVR）原理

SVR是在Vapnik提出的支持向量機的概念的基礎上拓展而來的[10]。給出樣本訓練集為：D={(xi,yi)}，i=1,2,...,n。其中n為樣本數目，xi為輸入指標，yi為輸出指標。SVR回歸的基本思想是利用非線性函數將輸入空間的數據映射到高維空間中去并在該高維空間進行回歸，回歸方程為：

式中：ω為權值矢量；b為偏差；φ(x)為將輸入映射到高維空間的非線性函數。以不敏感損失函數參數ε作為結構風險最小化的估計問題，并引入非負松弛變量δ和δ*和懲罰因子C，C控制對超出誤差樣本的懲罰程度，則需要優化的目標函數為：

綜合目標函數和約束條件，建立拉格朗日方程，引入核函數和二次規劃求解拉格朗日方程的SVR回歸模型為：

其中核函數

2.2 SINS/DVL組合導航訓練集的構造

根據DVL在數據有效時的速度數據和慣導相同時刻的速度數據建立訓練樣本

對于通過SVR回歸曲線而言，如果擬合曲線的時間序列過長，會導致在在當前時刻擬合精度較低并影響濾波器穩定。所以在SVR回歸過程中采用隨時間移動的窗口，隨著時間的移動，窗口的右邊不斷有新數據加入，窗口的左邊不斷有數據剔除。

由于在AUV運動過程中，AUV變速時會存在一個非線性過程。當AUV發生變速時由于當前時刻SVR訓練集中的數據大部分屬于AUV平穩運動的過程，并且集中的數據存在一定的噪聲，所以SVR回歸無法快速跟蹤AUV變速這一過程。由于物理原因和傳感器的極限不能提高DVL數據的刷新頻率，不能使這一段時間內訓練集的數據迅速變化為變速過程中的數據。根據上述原因，提出一種改變訓練集大小的方法來解決這一問題。該方法在檢測到水下機器人存在突然變速的行為時，迅速減小訓練集的大小，快速剔除平穩運動過程中的訓練數據，使變速后的數據樣本快速充滿訓練集，更快速回歸變速后的曲線。

設數據集的大小為N，引入訓練集尺度因子βt，βt表示當前時刻計算得到的尺度因子。經過仿真分析，給出βt的表達式：

式中：Δt表示相對于AUV平穩運動持續時間的比值，Δv 表示速度變化率，Δa表示加速度變化率。

設穩定時間單位時間長度為t0，tΔ為當Δa＜Δa0時持續的時間，當Δa＞Δa0時tΔ=0，由于數據存在噪聲所以Δa0為根據水下機器人變加速能力大小設定的一個小值。

式中μ＞1為比例系數。

此時尺度因子為：

式中：ρ和η均為比例系數ρ＜1，η＞1，η可以根據水下機器人的機動能力選取，如果機器人機動能力強η相應的增大，反之η取小。θ為比例系數，0.5＜θ＜1。

此時訓練集的大小：

2.3 判斷DVL數據失效

本文提到的DVL數據失效是指DVL數據較短時間內DVL無數據上傳和DVL數據存在野值的情況。若DVL長時間無數據或DVL數據長時間錯誤則視為DVL故障，為避免造成不必要的損失，水下機器人應立即進行相關應急處理，如拋載上浮等。

DVL數據野值檢驗：

式中：vdef為給定的速度誤差值，設置值的大小可以根據DVL的精度進行選取，vsvr為SVR算法估計的速度值，vdvl為多普勒量測的速度值。式（17）成立時則認為該值為野值，若不成立則為正常值。

2.4 SVR輔助的SINS/DVL組合導航流程

DVL正常運行時，由于多普勒數據刷新頻率低，通過SVR回歸預測DVL的速度值，將預測值的頻率相對DVL數據頻率提高，協調SINS和預測數據頻率后，將預測值作為量測值給入濾波器進行濾波處理。處理流程如圖2所示。

由于DVL數據無效時間較為短暫，DVL數據無效時，認為水下機器人不進行較大幅度的加速減速等機動行為。通過前一段時間訓練的SINS速度和DVL速度的對應關系預測當前時刻的速度關系，此時不在計算訓練數據集大小，訓練數據集的大小不再進行變化，處理流程如圖3所示。

圖2 DVL正常工作情況下程序流程圖Fig. 2 The program flow chart of DVL under normal working condition

圖3 DVL不正常工作情況下程序流程圖Fig. 3 The program flow chart of DVL under abnormal working condition

3 仿真分析

為了驗證上述算法的有效性，進行仿真試驗，將變訓練集大小的SVR算法，用于SNS/DVL組合導航系統。表1給出了各傳感器的規格，仿真時間為960 s，圖4顯示了水下機器人運行的軌跡包括經緯度、深度。

仿真中水下機器人按照圖4中箭頭的指向運動。為了驗證充分，水下機器人設定的運動過程：100 s的靜止，10 s的勻加速，100 s的勻速，70 s的下潛，20 s左轉彎，300 s勻速，20 s右轉彎，100 s的勻速，70 s的上浮，100 s的勻速，10 s減速，60 s靜止，仿真結束。仿真中設置SINS數據的刷新周期為0.1 s，DVL工作周期為2 s，仿真過程中對于DVL數據的刷新周期，設置在1.2～0.8 s之間隨機變化。

表1 傳感器精度Tab. 1 Sensor accuracy

圖4 水下機器人運動路徑Fig. 4 The sailing trajectory of the underwater vehicle.

3.1 DVL正常工作時SVR輔助的SINS/DVL組合導航

DVL正常工作時海底地形不平坦，導致DVL返回數據頻率不穩定，通過SVR回歸得到的曲線預測DVL速度值，使DVL數據穩定，并協調SINS和DVL的數據頻率，小幅度的提升DVL數據頻率。

圖5和圖6中的曲線1為SVR輔助情況下的SINS/DVL組合導航曲線，曲線2為不加SVR輔助情況下的SINS/DVL組合導航曲線。可以看出SVR輔助情況下，東向、北向速度及經緯度誤差下降較為明顯。

圖5 東向速度誤差和經度誤差Fig. 5 Eastward velocity error and longitude error

圖6 北向速度誤差和緯度誤差Fig. 6 North velocity error and latitude error

表2 速度及距離誤差數據Tab. 2 Speed and distance error data

從上述圖形和數據中可以看出在SVR輔助的情況下，速度誤差和誤差波動幅度明顯降低，由于路徑規劃過程中航行的方向在不斷變化，位置誤差屬于速度的累積誤差，經度誤差出現了變大的情況也較為合理，經緯度誤差的波動情況也明顯下降，所以穩定DVL數據頻率，通過SVR預測的方法小幅度提升數據頻率對提升導航精度是有效的。在仿真中發現水下機器人的機動行為越少，在SVR輔助的情況下提升精度越高。航行情況如文中所示的情況時東向速度誤差減少65.48%，誤差均方差減少96.65%。北向速度誤差減少29.51%，誤差均方差減少98.10%。SVR回歸對速度有明顯的平滑性，使誤差收斂幅度較大。經緯度誤差是速度誤差的累計，從圖像中看經緯度誤差具有明顯的收斂性，經度誤差收斂性提高37.59%，緯度誤差收斂性提升52.9%。從上述數據可以看出，變訓練集的SVR算法對導航經度提升幅度明顯。同時發現預測數據的頻率提升較少時精度會有所提高，但隨著頻率提升的越高誤差將會急劇增大，所以提升頻率的幅度大小與精度的關系還需要進一步研究。

3.2 DVL數據無效時SVR輔助的SINS/DVL組合導航

在上述仿真運動規劃中，水下機器人在350～550 s為勻速運動，在該時間段內DVL數據無效。DVL數據無效即人為的去掉該時間段內的DVL數據。仿真情況如圖8所示。

圖7和圖8中的曲線1為SVR輔助情況下的SINS/DVL組合導航曲線，曲線2為不加SVR輔助情況下的SINS/DVL組合導航曲線。當DVL數據無效時，單純比較誤差大小是看不出效果的，應該在基礎誤差（即DVL數據無效前一刻的誤差），觀察誤差的波動情況（即觀察誤差在這一段時間內的方差），可以看出相對的誤差增長幅度。圖中曲線1的波動幅度明顯小于曲線2。

圖7 DVL速度無效時的東向速度和經度誤差曲線Fig. 7 The eastward velocity and longitude error curves of DVL when the speed is invalid

圖8 DVL速度無效時的北向速度和緯度誤差曲線Fig. 8 The northern velocity and latitude error curves of DVL when the speed is invalid

從表3中可以看出，當DVL數據失效時，東向速度誤差收斂性提升64.81%，北向速度收斂性提升37.4%，經度誤差收斂性提升57.88%，緯度誤差收斂性提升54.29%。SVR輔助的SINS/DVL組合導航系統，在DVL數據失效時對于導航誤差具有明顯的抑制。

表3 速度及距離誤差均方差Tab. 3 Mean square error of velocity and distance

4 實航數據驗證

為了更加充分地驗證本文提出算法的有效性，采用2017年12月AUV湖上航行試驗的部分導航數據。該AUV長9.5 m，直徑534 mm，空氣中凈重1.5 t，最大下潛深度為200 m。選取數據為前向速度不變AUV先轉彎再勻速的一段數據（航向角由240°～69°并且在轉彎過程中深度值和pitch角有小范圍的波動）。

數據有效時的DVL東向和北向速度曲線如圖9和圖10所示。

圖9 DVL數據正常時的東向速度Fig. 9 Eastward speed when DVL data is normal

圖10 DVL數據正常時東向速度Fig. 10 Northward speed when DVL data is normal

圖9和圖10為SVR回歸的情況，可以看出跟蹤效果較好，在東向速度和北向速度完成變速后東向速度收斂性提升33.47%，北向速度收斂性提升27.99%。在平穩運行的過程中，SVR回歸對曲線具有一定的平滑性，這同時也驗證了仿真過程中經過SVR回歸并適當提高數據頻率會在一定程度上提高導航精度。

在AUV平穩運行過程中，一般DVL數據無效時間較短僅為數秒，并且未在AUV數據中找到連續多次上傳數據均為野值的情況，所以在轉彎階段（15～20 s）和（100～110 s）分別去掉一段DVL速度數據，通過去掉部分數據當作DVL數據無效的情況。在去掉DVL速度數據時，通過比較SVR估計數據和真實量測數據來分析算法的有效性。

圖11中在DVL數據無效階段相對于量測值的誤差均值為0.053 m，為量測值的6.17%。圖12中數據誤差值為0.031 m，為量測值的6.10%。可以看出在DVL數據無效時，SVR回歸能夠較好預測出DVL的數據值，且預測值相對于DVL真實量測誤差較小，并且預測值誤差的數量級已經達到了量測噪聲的量級。

圖11 15～20 s數據無效時的東向速度Fig. 11 15～20 s Eastward speed when data is invalid

圖12 100～110 s數據無效時的北向速度Fig. 12 100～110 s Northward speed when data is invalid

由于SINS數據記錄的不夠充分，并且圖9～圖12中SVR=預測的DVL數據已經能夠說明算法的有效性，所以不再進行經緯度數據的驗證和分析。

5 結 語

本文通過研究水下機器人航行過程中由于海底地形等外部環境的變化帶來的DVL數據頻率變化及失效等問題，提出了一種變訓練集的SVR輔助SINS/DVL組合導航的方法。通過變換訓練集大小提高在水下機器人機動時速度曲線的跟蹤速度，解決了水下機器人運動時帶來的非線性變化，并通過仿真和實際航行數據驗證了該方法能夠解決數據頻率變化和數據無效等問題，增強了導航系統的穩定性，為后續的水下機器人試驗打下基礎。對于變換數據集的方法，確定尺度因子參數的數值是有難度的，仿真中確定了參數的大致范圍，后續將會嘗試一些優化方法確定優化參數具體值，將繼續考慮在DVL數據失效的同時水下機器人機動的情況下，改進該算法并達到抑制導航誤差的目的。