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

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

（江蘇科技大學電子信息學院，江蘇 鎮(zhèn)江 212003）

0 引言

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

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

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

1 AUV 對接回收中USBL 定位系統(tǒng)工作原理

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

圖1 AUV 回收對接流程框圖

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

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

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

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

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

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

式中，i=1，2，3。

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

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

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

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

2 基于USBL 的組合導航建模

2.1 基于USBL 組合導航系統(tǒng)狀態(tài)方程設計

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

圖3 SINS/USBL/DVL/深度計組合導航系統(tǒng)原理圖

根據(jù)改進的基于斜距USBL 的誤差模型，選取到達距離差和斜距誤差作為狀態(tài)量，USBL 系統(tǒng)狀態(tài)變量為：

2.2 基于USBL 的組合導航系統(tǒng)量測方程設計

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

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

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

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

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

將SINS 系統(tǒng)與DVL、深度計及USBL 系統(tǒng)的信息做差，作為濾波器的量測值進行濾波融合，在濾波器內采用改進的自適應卡爾曼濾波算法進行濾波?；赨SBL 的組合導航改進的AKF 算法設計如下：

1）狀態(tài)預測

式中，X（k|k-1）為k 時刻系統(tǒng)預測狀態(tài)向量，F(xiàn)（k|k-1）為k 時刻系統(tǒng)預測狀態(tài)轉移矩陣，X（k-1）為k-1時刻狀態(tài)向量，G（k|k-1）為k 時刻系統(tǒng)噪聲傳播矩陣，W（k-1）為k-1 時刻噪聲矩陣。

2）新息序列更新

3）狀態(tài)預測協(xié)方差更新

4）濾波增益矩陣更新

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

7）濾波的收斂性判據(jù)

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

8）自適應漸消因子

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

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

強跟蹤濾波器在線選擇一個適當?shù)臑V波增益陣使：

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

表1 狀態(tài)估計各參數(shù)標準差

4 組合導航系統(tǒng)算法仿真驗證

4.1 組合導航算法對比仿真

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

設置仿真參數(shù)條件：陀螺常值漂移為0.05°/h，隨機偏移為0.05°/h；加速度計常值偏置：50 ug，隨機誤差為50 ug；DVL 精度為0.1 m/s；初始姿態(tài)角：縱搖為0°，橫搖為0°，航向為30°；初始速度為1 m/s；初始姿態(tài)誤差中縱搖誤差為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 為兩種算法下狀態(tài)估計各參數(shù)的標準差，可以明顯地看出，AKF 濾波作用下的導航系統(tǒng)東北天向速度誤差及位置誤差都得到了改善，其主要原因在于，提出的改進的自適應濾波算法，以簡化的Sage-Husa 自適應濾波算法為框架，在濾波收斂性判據(jù)和強跟蹤濾波思想雙重作用下，改善了濾波收斂性和穩(wěn)定性。

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

當AUV 行駛至USBL 作用的范圍時，通過聲學信號測量AUV 到配備有發(fā)射器的回收塢的斜距，再結合深度計測量的深度信息，通過改進的高斯距離迭代法進行定位，用得到的位置信息去修正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%左右，而且不產生發(fā)散，一直保持穩(wěn)定。

5 結論

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

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