基于AKF 的AUV 對接回收組合導航研究*

包靈卉，曾慶軍，朱志宇，戴曉強，趙 強

（江蘇科技大學電子信息學院，江蘇 鎮江 212003）

0 引言

隨著水下機器人的發展，我國對海洋的開發技術已達到世界前列。由于AUV 的運動靈活且范圍大，國內外各科研機構多使用AUV 進行水下作業。而面對復雜的水下作業，AUV 需要多次返回支持平臺，完成能源對接及數據交換等工作，以保證完成水下作業任務。精準導航是保證AUV 安全對接回收的重要條件，常用捷聯慣性導航系統（Strapdown Inertial Navigation systems，SINS）作為主要的導航系統，由其他傳感器作為輔助進行組合導航。但這種組合導航可能存在水下無信號或電磁波傳播困難等問題，因此，水聲定位逐步變成了各國海洋研究領域的熱點［1-2］。超短基線定位系統（Ultra-Short Baseline，USBL）由于基陣尺寸小且使用范圍廣，被廣泛應用于水下乃至深海導航定位［3］。

目前，在組合導航信息融合中，Sage-Husa 自適應卡爾曼濾波算法雖然可以實時估計噪聲統計特性，但其濾波的收斂性差，達不到滿意效果。文獻［10］提出一種基于人工蜂群算法（ABC）改進的徑向基函數（RBF）神經網絡增強自適應無跡卡爾曼濾波算法（AUKF），但是其公式復雜，在實際情況下很難實現。文獻［11］提出一種改進的自適應擴展卡爾曼濾波算法，只需要實時估計對濾波影響大的測量噪聲協方差陣，但每個采樣周期都對其進行估計，加深了計算的復雜度，濾波的實時性難以得到保障。

針對自主水下機器人的導航系統，采取了多傳感器組合導航的方式，主要研究了AUV 進入聲學作用范圍時的基于USBL 輔助SINS 的組合導航，USBL 定位系統采用改進的高斯測距算法，解決了距離計算中存在的奇異情況，并結合SINS、DVL 和深度計等傳感器建立改進的基于斜距的組合導航數學模型，然后設計了一種改進的自適應濾波算法，以簡化的Sage-Husa 自適應濾波算法為框架，引入濾波收斂性判據及強跟蹤濾波思想，在一定程度上改善了濾波收斂性和穩定性。而且通過仿真實驗結合實際AUV 系統，證得AUV 導航系統采用改進的自適應濾波算法能安全回收。

1 AUV 對接回收中USBL 定位系統工作原理

在AUV 的對接回收過程中，對接流程分為4個階段，分別為直線歸位、直線跟蹤、直線對接和慣性對接4 個階段，重點研究的是USBL 作用范圍內的組合導航，采用基于USBL 組合導航的方式進行導航，主要在直線歸位階段和直線跟蹤階段。在這個過程中，需要利用組合導航提供的速度、位置、姿態信息來調整AUV 與對接裝置的位置，使得AUV在消耗能量較少的情況下，到達對接塢的中軸線上。在直線對接階段，AUV 的重心已經行駛到中軸線上，且離對接口相距3 m~5 m，此時利用三維視場重建和視覺導航提供的相對位置和姿態信息來微調AUV 的狀態，保證AUV 沿中軸線航行，且艏向角指向對接口。最后在慣性對接階段，關閉所有推進器，依靠慣性駛入對接塢［6］。

圖1 AUV 回收對接流程框圖

USBL 定位系統由聲學基陣和接收器或應答器組成，聲學基陣安裝在回收塢上，包括水聽器和換能器，AUV 端為應答器。系統通過測定水聽器的相位差來計算AUV 到回收塢的水平和垂直角度，再通過測定聲波的傳播時間來確定AUV 到回收塢的距離，最終得到AUV 相對于換能器的位置信息［7-8］。

圖2 改進的高斯距離迭代原理圖

采用純距離定位的方法，由于純距離定位法的定位方法是解算航跡的幾何路徑，由此獲取距離量即可，只需要USBL 的測距功能，簡化了系統的結構，并增強了系統的穩定性。純距離定位法采用了改進的高斯距離迭代法，其步驟如下：

步驟1 將AUV 的航跡分割成若干條連接的直線段，初始直線段的初始點已知，在每條直線上的每個位置點測得AUV 到聲源發射器的距離。

步驟2 將AUV 航跡中的一段與回收塢上聲源發射器的位置信息放到二維平面上，形成一個二維平面關系圖，設Ps點坐標為Ps（X0，Y0），任取AUV航跡上的3 個點p1（X1，Y1），p2（X2，Y2）和p3（x，y），首先列出3 個點與Ps點的距離關系，然后根據p1位置到p3位置的時間t13和p2位置到p3位置的時間t23與速度和角度的關系，用p3點的橫坐標和縱坐標代替p1和p2點的橫坐標和縱坐標，最終整理可得關于x，y 的方程組

將式（1）寫成矩陣形式，通過最小二乘法進行求解，由于求解過程中可能存在奇異的情況。引入改進的高斯距離迭代算法進行迭代求解，式（1）相當于坐標（x，y）與已知坐標的距離函數，令

式中，i=1，2，3。

步驟3 為了減小速度和位置的誤差，用泰勒級數展開進行線性化，首先設定AUV 的位置初始值和迭代閾值，其次用最小二乘法求出AUV 位置的修正量，將修正量與迭代閾值進行比較，若修正量小于迭代閾值，則停止迭代；否則采用信賴域的方法來更新步長，繼續迭代。

首先確定一個位移長度（信賴域半徑），并以當前迭代點為中心點，以此位移長度為半徑確定一個閉球區域（信賴域）。然后求解這個區域內“信賴域子問題”的最優點，若此最優點能夠使式（4）有足夠的下降量，則接受此最優點為新的位移，同時需要保持或擴大信賴域半徑繼續迭代。否則，說明二次模型與目標函數的近似度沒有達到目標，需要縮小信賴域半徑，再通過求解新的信賴域子問題得到新的最優點作為新的位移。

循環執行步驟2、步驟3 進行新一輪的迭代計算，直到式（4）滿足為止。

步驟4 取計算出的直線段的末端點作為下一直線段的起始點，繼續進行下一直線段的迭代計算。

2 基于USBL 的組合導航建模

2.1 基于USBL 組合導航系統狀態方程設計

將SINS 系統解算的速度、深度及到達距離差和斜距分別與DVL、深度計及USBL 系統的信息做差，作為濾波器的量測值進行濾波融合。綜上所述設計出基于到達距離差和斜距的SINS/USBL/DVL/深度計組合導航子系統原理框圖，如圖3 所示［12］。

圖3 SINS/USBL/DVL/深度計組合導航系統原理圖

根據改進的基于斜距USBL 的誤差模型，選取到達距離差和斜距誤差作為狀態量，USBL 系統狀態變量為：

2.2 基于USBL 的組合導航系統量測方程設計

在基于USBL 的組合導航系統中，提高定位的精度以及實時性是最關鍵的問題，而在整個系統中，有以下幾個亟待解決的問題：

1）在USBL 系統中，發射器端有一個由水聽器組成的基陣，整個基陣尺寸較小，易在安裝時發生偏移，則會對后續的導航定位產生較大的影響。

2）對于USBL 系統中純距離定位法而言，計算過程中使用的最小二乘法會存在奇異值的情況，需要解決其中的奇異值，增強純距離定位法的精確性。

3）對于SINS/USBL/DVL/深度計組合導航系統而言，將SINS 系統與各輔助系統進行融合，必須解決引入USBL 系統帶來的濾波發散問題，以及歷史信息對當前濾波值的影響，并保證濾波的實時性，使得濾波收斂。

3 基于USBL 的組合導航系統改進的自適應卡爾曼濾波算法設計

將SINS 系統與DVL、深度計及USBL 系統的信息做差，作為濾波器的量測值進行濾波融合，在濾波器內采用改進的自適應卡爾曼濾波算法進行濾波。基于USBL 的組合導航改進的AKF 算法設計如下：

1）狀態預測

式中，X（k|k-1）為k 時刻系統預測狀態向量，F（k|k-1）為k 時刻系統預測狀態轉移矩陣，X（k-1）為k-1時刻狀態向量，G（k|k-1）為k 時刻系統噪聲傳播矩陣，W（k-1）為k-1 時刻噪聲矩陣。

2）新息序列更新

3）狀態預測協方差更新

4）濾波增益矩陣更新

從式（16）和式（17）可以看出，濾波增益和系統狀態估計協方差與測量噪聲協方差R（k）相聯系，R（k）對濾波的影響更大。所以對測量噪聲協方差R（k）進行估算，從而減小計算量，提高實時性。

7）濾波的收斂性判據

在濾波融合時，需要判斷當前濾波是否收斂，若滿足收斂條件，則R（k）=R（k-1）。否則，需要由式（18）估計新的R（k）值。

8）自適應漸消因子

式（23）中，β≥1 是為了使狀態估計值更平滑引入的一個弱化因子。式中，ρ 為遺忘因子，V0為殘差的協方差。

系統趨向平穩狀態時，增益矩陣K（k）一般是接近極小值的，可預報殘差V（k）則會在系統狀態發生突變時增大，但從式（16）看出，增益矩陣K（k）不會隨V（k）變化而發生相應的變化，此時濾波器無法對突變狀態作跟蹤處理。為了避免以上缺點，使濾波器具有強跟蹤濾波的特性，以簡化的Sage-Husa 自適應濾波算法作為基礎模型，在線調整狀態預測協方差和相應的增益陣。

強跟蹤濾波器在線選擇一個適當的濾波增益陣使：

相當于將不同時刻的殘差序列作正交，其原理是為使得預報殘差時刻具有類似于高斯白噪聲的性質，在模型參數失配情況下，實時調整增益矩陣K（k）。

表1 狀態估計各參數標準差

4 組合導航系統算法仿真驗證

4.1 組合導航算法對比仿真

為了驗證建立的基于USBL 系統輔助SINS 的SINS/USBL/DVL/深度計組合導航系統的誤差模型和改進的自適應卡爾曼濾波算法下的性能，在MATLAB 下模擬環境進行仿真。在MATLAB 建立了SINS/USBL/DVL/深度計組合導航系統的模型，分別采用AKF 仿真和改進的AKF 算法仿真，并進行對比。

設置仿真參數條件：陀螺常值漂移為0.05°/h，隨機偏移為0.05°/h；加速度計常值偏置：50 ug，隨機誤差為50 ug；DVL 精度為0.1 m/s；初始姿態角：縱搖為0°，橫搖為0°，航向為30°；初始速度為1 m/s；初始姿態誤差中縱搖誤差為0.001 5°，橫搖誤差為0.001 5°，航向誤差為0.001 5°；深度計誤差為0.3 m；初始速度誤差為0.018 m/s；初始位置誤差中緯度誤差為1.2 m，經度誤差為1.2 m，高度誤差為0.6 m；仿真結果如圖4 所示，輸出的變量主要有東北天向速度誤差和經緯度、高度誤差。

圖4 組合導航兩種濾波算法誤差對比圖

圖4 為組合導航兩種濾波算法誤差對比圖，其中，藍色為組合導航在AKF 下的速度和位置誤差，紅色為組合導航在改進的AKF 下的速度和位置誤差。表1 為兩種算法下狀態估計各參數的標準差，可以明顯地看出，AKF 濾波作用下的導航系統東北天向速度誤差及位置誤差都得到了改善，其主要原因在于，提出的改進的自適應濾波算法，以簡化的Sage-Husa 自適應濾波算法為框架，在濾波收斂性判據和強跟蹤濾波思想雙重作用下，改善了濾波收斂性和穩定性。

4.2 USBL 純距離定位法輔助SINS 定位仿真

當AUV 行駛至USBL 作用的范圍時，通過聲學信號測量AUV 到配備有發射器的回收塢的斜距，再結合深度計測量的深度信息，通過改進的高斯距離迭代法進行定位，用得到的位置信息去修正SINS的位置信息，以改進的SINS/USBL/DVL/深度計組合導航方式導航定位。

仿真時間1 400 s，假設AUV 在500 s 后進入USBL 作用的范圍，通過改進的高斯距離迭代法對SINS 導航進行輔助定位，分別對有無改進的高斯距離迭代法輔助SINS 定位進行對比仿真實驗，用水平斜距誤差率來衡量定位精度，即水平方向位置計算值與真實值之差，除以真實斜距值，水平誤差率越小，其精度越高，其計算公式如下：

仿真結果如圖5 所示。

圖5 有無改進的高斯距離迭代輔助定位的水平斜距誤差率對比

如圖5 所示，整個仿真時間為1 400 s，設定AUV 在500 s之后進入USBL 作用范圍，即500 s~1 400 s之間在改進的高斯距離迭代法輔助SINS 定位下，表2 則截取500 s后的5 個時刻，600 s、800 s、1 000 s、1 200 s及1 400 s時刻的水平斜距誤差率進行對比，水平斜距誤差率降低了90%左右，而且不產生發散，一直保持穩定。

5 結論

針對AUV 在對接回收中基于斜距的SINS/U S-BL/DVL/深度計組合導航的多傳感器融合問題，研究表明，改進的高斯距離迭代法輔助SINS 的定位導航的變量參數更小，精度和穩定性更高，其主要原因在于，改進的高斯距離迭代法解決了計算中矩陣可能存在的奇異情況。且提出的改進的自適應卡爾曼濾波算法，能夠在AUV 對接回收導航系統中起到較大的改進作用，使得AUV 的回收成功率變大。

表2 500 s后5 個時刻的水平誤差率對比