基于傳感器信息的AUV海底地形跟蹤研究

徐紅麗，陳 鞏

（中國科學院 沈陽自動化研究所，沈陽 110016）

隨著人們對海洋的開發日益深入，現階段人們對自主水下機器人AUV（autonomous underwater vehicle）的使用提出了更高的要求，希望AUV能夠完成例如海底地形跟蹤、海底礦產資源勘探拍照、定高航行等任務，這些任務要求AUV距離海底高度不大于5 m，航速小于兩節，另外由于海底地形的未知性和復雜性，不能提前對AUV進行路徑規劃，這就要求AUV必須進行近海底實時避碰，目前國內外使用的大多數AUV都是針對特定的任務進行設計的，針對海底定高航行和海底地形跟蹤方面的AUV研究相對還比較少，因此這個問題有待深入研究[1]。

文獻[2]中美國伍茲霍爾海洋研究所研制的“ABE”AUV僅通過使用安裝在AUV艏部的高度計分析局部海底地形變化，進行了海底地形跟蹤的仿真實驗；文獻[3]伊朗德黑蘭大學通過將偽譜法與非線性輸出理論相結合改進了AUV控制器，并將海底地形跟蹤的問題轉換成軌跡跟蹤，通過仿真驗證了方法的可行性；文獻[4]葡萄牙波爾圖大學系統與機器人研究所通過采用卡爾曼濾波、環境估測技術與基于李雅普諾夫穩定性的導航技術，通過對AUV攜帶的高度計信息進行數據擬合，通過線性回歸理論完成了海底地形的跟蹤仿真，并采用美國的“REMUS”系列AUV完成了海底地形跟蹤的試驗驗證。

本文研究了基于高度計與深度計信息的AUV海底地形跟蹤的問題，著重解決在復雜海洋環境下通過傳感器信息完成AUV對海底地形坡度的估計，最終通過仿真對算法進行驗證。

1 AUV運動學建模

圖1所示為AUV對海底地形進行跟蹤時的航行示意圖，當AUV在進行海底跟蹤時，此時只需考慮對AUV垂直面的控制，這里只需考慮AUV升沉速度（w）、縱傾角（q）、縱傾角速率（θ）和水深值（S）等。假設AUV是以固定的速度前進，則設定系統運動模型為

圖1 AUV底跟蹤航行示意Fig.1 AUV bottom-following sailing schematic

此時AUV的運動學方程為

軸向力方程：

垂向力方程：

縱傾力矩方程：

轉首（偏航）力矩方程：

輔助方程：

式中各個參數含義如表1所示。

表1 符號及參數說明Tab.1 Symbol and parameter description

2 擴展卡爾曼濾波

對海底地形的估計，主要依靠的傳感器是高度計和深度計。由于高度計和深度計都存在較多的背景噪聲和較大的測量誤差，在對海底地形進行估計之前，需要對高度計和深度計數據進行濾波。這里采用擴展卡爾曼濾波EKF（extended kalman filter），擴展卡爾曼濾波就是在有隨機干擾和噪聲的情況下，通過采用線性最小方差估計的方法給出狀態的最優估算值，擴展卡爾曼濾波是在當前統計的基礎上給出最接近目標真值的估算值[5]。擴展卡爾曼濾波的計算過程如下：

1）初始化

2）預測

3）修正

式中：X為進行濾波的量；A為系統矩陣；B為控制輸入矩陣；P為估計（誤差協方差矩陣）；Q為預測值的協方差；R為測量值的協方差；H為觀測矩陣，也叫量測矩陣；Kg為中間變量。Q/（Q+R）的值是卡爾曼增益的收斂值，卡爾曼增益越小，預測值越可靠，最優化角度越接近預測值，反之說明測量值越可靠，最優化角度越接近測量值。在本課題中，Z表示高度計和深度計的實際測量值，參數A、B、Q是由選擇的系統模型決定的，參數R是由傳感器自身決定的，因此可以通過卡爾曼濾波對高度計和深度計數值進行濾波[6]。

3 最小二乘法

由于海底地形起伏不定，這里采用最小二乘法對海底地形坡度進行估計，最小二乘法（又稱最小平方法）是一種數學優化方法，它通過將誤差的平方和最小化從而確定數據的最優或者次優匹配函數[7]。利用最小二乘法可以比較方便高效地求得未知的數據，并使結果和實際數據之間的誤差的平方和最小。選取N個水深值，通過最小二乘擬合，建立時間與水深值之間的函數關系S=a+bt，其中a和b為待估參數，b為當前地形的斜率。通過最小二乘擬合，便可以求得待估參數，計算方法如下：

如果沒有測量誤差，只需要2組不同的t和S，就可以求出a和b。但是由于每次測量中總存在隨機誤差，即：

式中：Si為測量數據；si為真值；vi為隨機誤差。

顯然，將每次測量誤差相加，可構成總誤差：

如何使測量的總誤差最小，選擇不同的評判標準會獲得不同的方法，當采用每次測量的平方和最小時，即：

利用求極值的方法得：

解上述方程組，可得：

其中，b為要求的海底地形的斜率，通過公式可將其轉換成角度。

4 仿真驗證

本文通過Matlab建立了兩類特定的地形，讓AUV在定深/定高模式下航行，通過2組仿真分別驗證了基于EKF和最小二乘法的海底地形坡度估計及海底地形跟蹤。

4.1 海底地形坡度估計仿真

首先通過Matlab建立一個梯形地形，在前200 s的時間內，海底地形為0°，在200 s～400 s的時間海底地形角度為20°，在400 s～600 s的時間內海底地形為0°，在600 s～800 s的時間內海底地形為-20°，在800 s～1000 s的時間內海底地形為0°。AUV航速設定為1節，傳感器采樣周期為0.5 s，給傳感器加入了標準差為1.5的白噪聲，通過EKF對傳感器數據進行濾波，圖2所示為海底地形隨時間的變化關系及AUV測量的數據。圖3所示為通過算法測得的海底地形的角度隨時間變化的曲線。

圖2 海底地形及AUV測量數據Fig.2 AUV submarine topography and measurement data

圖3 海底地形角度估計Fig.3 Seabed topography angle estimation

此時本文通過最小二乘法對海底坡度進行估計，n取值為10，即每10個點計算1次海底地形的坡度，通過圖3可以看出通過算法測量的海底地形的角度與設定角度基本一致，能夠準確預測海底地形的變化趨勢。

4.2 海底地形跟蹤仿真

針對海底地形跟蹤的仿真，本文建立了一種漸升型海底地形用以模擬典型的海底地形。

本文建立了一個長100 m的海底地形，為了建模方便，本文選取z=0表示海平面，z向下為正方向，設定海深為 20 m，在x＜25時海深為 20 m，在 25＜x＜55時海深漸漸變小，在x＞55時海深維持在14 m，如圖4所示。

圖4 漸升型海底地形Fig.4 Gradually rise subsea terrain

在仿真中，設定AUV模型為“潛龍一號”6000 m自主水下機器人，它攜帶8個不同方向的測距聲納，測距聲納分布示意如圖5所示，橢圓形表示潛龍一號載體，S1～S8為聲納編號，相鄰2個聲納之間夾角為 45°，其中 S1，S5～S8可以測量水平面上的障礙物信息，S1～S4可以測量垂直面的障礙物信息，其中S4測距聲納可以對AUV距底高度進行測量。將聲納探測距離設定為15 m，傳感器采樣周期為0.25 s。

圖5 測距聲納分布示意Fig.5 Schematic diagram of sonar

設定AUV航速為2節，定高5 m，如圖6所示為AUV航行示意圖，虛線表示AUV通過滑模控制后的航行路線，實線表示海底地形。

圖6 AUV航行示意Fig.6 AUV Navigation schematic

當AUV的測距聲納探測到前方海底有升高趨勢，AUV及時增大pitch角，當AUV爬坡完成后，此時AUV距離海底的高度大于5 m，AUV減少pitch角，開始下坡，直到AUV距離海底的高度達到5 m，pitch角最終歸零，完成垂直面的滑模控制。圖7所示為AUV的pitch角的變化曲線，pitch角控制在-15°～25°之間。

圖7 AUV pitch角變化曲線Fig.7 AUV pitch angle curve

5 結語

本文針對海底地形坡度估計的問題研究了基于擴展卡爾曼濾波與最小二乘法的海底地形跟蹤算法。仿真和試驗的數據表明，基于擴展卡爾曼濾波和最小二乘法的海底地形坡度估計能夠準確地對海底地形的變化趨勢進行計算及預測，并能順利完成海底地形的跟蹤。

[1]Grasmueck M，Eberli G P，Viggiano D A，et al.Autonomous underwater vehicle（AUV）mapping reveals coral mound distribution，morphology，and oceanography in deep water of the Straits of Florida[J].Geophysical Research Letters，2006，33（23）：430-452.

[2]Singh H，Yoerger D，Bachmayer R，et al.Sonar mapping with the autonomous benthic explorer（ABE）[C]//International Symposium on Unmanned Untethered Submersible Technology University of New Hampshire-matine Systems，1995：367-375.

[3]Kajbafzadeh A M，Jangouk P，Yazdi C A.Anterior urethral valve associated with posterior urethral valves[J].Journal of Pediatric Urology，2005，1（6）：433-435.

[4]Cruz N，Matos A C.The MARES AUV，a modular autonomous robot for environment sampling[C]//OCEANS 2008，IEEE，2008：1-6.

[5]Pi S，He B，Zhang S，et al.Stereo visual SLAM system in underwater environment[C]//OCEANS 2014-TAIPEI.IEEE，2014：1-5.

[6]陳志敏，薄煜明，吳盤龍，等.基于新型粒子群優化的粒子濾波雷達目標跟蹤算法[J].信息與控制，2012，41（4）：413-418.

[7]王星星，吳貞煥，楊國安，等.基于改進粒子群算法的最小二乘影響系數法的理論及實驗研究[J].振動與沖擊，2013，32（8）：100-104.