徐紅麗，陳 鞏

（中國科學(xué)院 沈陽自動(dòng)化研究所，沈陽 110016）

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

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

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

1 AUV運(yùn)動(dòng)學(xué)建模

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

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

此時(shí)AUV的運(yùn)動(dòng)學(xué)方程為

軸向力方程：

垂向力方程：

縱傾力矩方程：

轉(zhuǎn)首（偏航）力矩方程：

輔助方程：

式中各個(gè)參數(shù)含義如表1所示。

表1 符號及參數(shù)說明Tab.1 Symbol and parameter description

2 擴(kuò)展卡爾曼濾波

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

1）初始化

2）預(yù)測

3）修正

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

3 最小二乘法

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

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

式中：Si為測量數(shù)據(jù)；si為真值；vi為隨機(jī)誤差。

顯然，將每次測量誤差相加，可構(gòu)成總誤差：

如何使測量的總誤差最小，選擇不同的評判標(biāo)準(zhǔn)會(huì)獲得不同的方法，當(dāng)采用每次測量的平方和最小時(shí)，即：

利用求極值的方法得：

解上述方程組，可得：

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

4 仿真驗(yàn)證

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

4.1 海底地形坡度估計(jì)仿真

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

圖2 海底地形及AUV測量數(shù)據(jù)Fig.2 AUV submarine topography and measurement data

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

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

4.2 海底地形跟蹤仿真

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

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

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

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

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

設(shè)定AUV航速為2節(jié)，定高5 m，如圖6所示為AUV航行示意圖，虛線表示AUV通過滑?？刂坪蟮暮叫新肪€，實(shí)線表示海底地形。

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

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

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

5 結(jié)語

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

[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]陳志敏，薄煜明，吳盤龍，等.基于新型粒子群優(yōu)化的粒子濾波雷達(dá)目標(biāo)跟蹤算法[J].信息與控制，2012，41（4）：413-418.

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