基于SLAM算法的AUV自主導(dǎo)航仿真研究*

趙 輝，杜航原，張 虎

(1.中國移動通信集團(tuán)山西有限公司，山西太原030032;2.山西大學(xué) 計(jì)算機(jī)學(xué)院，山西 太原030006)

海洋空間蘊(yùn)藏著豐富的海洋生物、礦產(chǎn)資源和各種能源，為人類可持續(xù)發(fā)展提供了大量物質(zhì)基礎(chǔ)。對海洋的研究和合理開發(fā)，成為了人類社會和經(jīng)濟(jì)發(fā)展的一項(xiàng)重大課題。但由于海洋空間能見度低、環(huán)境復(fù)雜等特點(diǎn)，人類對海洋的探索面臨重重困難。伴隨著人類對海洋認(rèn)識、開發(fā)、利用和保護(hù)的逐步深入，水下航行器作為人類探索海洋的有力助手經(jīng)歷了從誕生到發(fā)展再到應(yīng)用的過程。其中，自主式水下航行器因其具有安全、實(shí)時、體積小、重量輕、耗電小等優(yōu)勢，在軍事、科考和工程領(lǐng)域都得到了廣泛應(yīng)用。目前，AUV制造過程所需的材料、結(jié)構(gòu)等相關(guān)技術(shù)已基本得到解決，但隨著AUV向遠(yuǎn)航程、大深度、多功能、長時間趨勢發(fā)展，仍有一些關(guān)鍵問題亟需研究和解決。其中水下導(dǎo)航技術(shù)對于AUV的發(fā)展至關(guān)重要，導(dǎo)航定位信息的精確度直接決定AUV能否安全高效的完成水下任務(wù)，實(shí)現(xiàn)自主導(dǎo)航成為決定AUV發(fā)展與應(yīng)用的關(guān)鍵技術(shù)之一。常見的AUV導(dǎo)航技術(shù)主要有航位推算法［1］、基于聲信號的長基線和短基線導(dǎo)航［2］、基于重力場和磁力場的地球物理導(dǎo)航［3］，還有將多種導(dǎo)航技術(shù)進(jìn)行組合互補(bǔ)的組合導(dǎo)航［4］等。在實(shí)際使用中，由于水下導(dǎo)航作業(yè)時間長、環(huán)境復(fù)雜、信源稀少以及隱蔽性要求高的特點(diǎn)，這些傳統(tǒng)導(dǎo)航方式在精度、可靠性和成本上難以滿足AUV完全自主導(dǎo)航的需要。

起源于機(jī)器人領(lǐng)域的同步定位與地圖創(chuàng)建算法解決了這樣一個問題:處于未知環(huán)境中的機(jī)器人，由一個存在不確定性(初始誤差)的起點(diǎn)出發(fā)，在行進(jìn)過程中利用自身配備的傳感器對環(huán)境進(jìn)行不斷感知，增進(jìn)式的構(gòu)建環(huán)境地圖，同時利用地圖確定機(jī)器人自身位置［5］。SLAM算法具有無須先驗(yàn)環(huán)境地圖、傳感器結(jié)構(gòu)相對簡單以及易于實(shí)施等優(yōu)點(diǎn)，在水下導(dǎo)航定位技術(shù)中極具發(fā)展?jié)摿涂臻g，對于提高AUV的自主性有著重大意義。本文將SLAM算法用于AUV自主導(dǎo)航，在EKF框架下實(shí)現(xiàn)了AUV自身位置與環(huán)境特征狀態(tài)的同時估計(jì)，仿真實(shí)驗(yàn)表明這種導(dǎo)航算法能夠充分利用環(huán)境信息，有效提高了導(dǎo)航精度。

1 SLAM導(dǎo)航系統(tǒng)原理

在AUV行進(jìn)和作業(yè)過程中，其所處外部海洋環(huán)境中存在大量實(shí)體(如珊瑚、巖石、沉船等)，在對這些物體進(jìn)行幾何近似提取為特征點(diǎn)的基礎(chǔ)上，AUV通過裝備的傳感器不斷感知各個特征點(diǎn)相對于AUV的距離和角度信息，并利用這些信息形成對環(huán)境的稀疏描述，即特征地圖［6］。在SLAM框架下，如果將坐標(biāo)原點(diǎn)取于AUV的初始位置，那么隨著AUV對環(huán)境的不斷探索就可以增進(jìn)式地獲取相對于起始點(diǎn)的完整特征地圖，同時得到這一過程中每一時刻AUV的位姿狀態(tài)估計(jì)。

令xv(k)表示k時刻AUV的位姿狀態(tài);uv(k)表示k時刻AUV的控制輸入變量;z(k)表示AUV在行進(jìn)過程中利用傳感器獲取的外部環(huán)境觀測信息;L(k)=［l1(k)，l2(k)，…，lN(k)］表示k時刻用于描述環(huán)境的特征地圖，其中l(wèi)i(k)為地圖中第i個特征，N為地圖中包含的特征數(shù)目，假設(shè)環(huán)境保持靜止，則有 lik+1=li(k)=li=［xi，yi］T。SLAM 算法就是實(shí)現(xiàn)每一時刻對AUV狀態(tài)和環(huán)境地圖特征的聯(lián)合估計(jì)，即對如下聯(lián)合后驗(yàn)概率密度的估計(jì):

在過去的研究中，往往將定位和構(gòu)圖分割處理，進(jìn)行獨(dú)立實(shí)現(xiàn)，即地圖構(gòu)建是已知AUV位置的地圖構(gòu)建，定位是已知環(huán)境地圖的定位。然而在SLAM方法中，定位與構(gòu)圖被融為一體。隨著AUV不斷獲取環(huán)境特征的相對觀測，所有特征位置的估計(jì)是相關(guān)的，因?yàn)樗鼈兓谙嗤腁UV位姿估計(jì)。這意味著:要獲得完全一致的SLAM解法，則必須對AUV位姿與地圖中所有特征進(jìn)行全狀態(tài)估計(jì)。將AUV系統(tǒng)和環(huán)境地圖定義在一個二維坐標(biāo)系下，使用一組包含AUV的狀態(tài)向量和地圖特征向量的聯(lián)合狀態(tài)向量表達(dá)空間信息，在AUV探索過程中利用擴(kuò)展卡爾曼濾波(extended kalman filter，EKF)融合傳感器觀測信息，對聯(lián)合狀態(tài)進(jìn)行最優(yōu)準(zhǔn)則估計(jì)。每一時刻AUV獲得新的環(huán)境特征后，要將新特征加入地圖，因此聯(lián)合狀態(tài)向量的維數(shù)不固定。聯(lián)合狀態(tài)向量的協(xié)方差矩陣中保存了AUV狀態(tài)分量之間、狀態(tài)與環(huán)境特征之間以及兩個環(huán)境特征之間的相關(guān)信息，這些信息是聯(lián)系A(chǔ)UV狀態(tài)和環(huán)境地圖的紐帶，同時協(xié)方差矩陣中包含的相關(guān)性也保證了特征地圖的收斂性［7］。

2 系統(tǒng)處理流程

在EKF迭代估計(jì)框架下，基于SLAM的AUV導(dǎo)航算法執(zhí)行過程主要包括預(yù)測、觀測和更新三個部分，是一個遞歸的過程，算法流程如圖1所示。

圖1 SLAM導(dǎo)航系統(tǒng)結(jié)構(gòu)圖

每一時刻AUV的SLAM導(dǎo)航系統(tǒng)數(shù)據(jù)處理流程為:在預(yù)測階段，基于上一時刻的聯(lián)合狀態(tài)估計(jì)值，利用控制輸入變量及AUV運(yùn)動模型和觀測模型完成當(dāng)前AUV位姿和地圖特征狀態(tài)的預(yù)測值;在觀測階段，一方面通過傳感器獲得對外部環(huán)境的感知信息，并進(jìn)行特征的識別和抽取，另一方面還需判斷觀測信息與環(huán)境地圖中已有特征的關(guān)聯(lián)關(guān)系以及多個觀測是否源于同一物理實(shí)體，即數(shù)據(jù)關(guān)聯(lián)過程;在更新階段，利用數(shù)據(jù)關(guān)聯(lián)結(jié)果將新特征加入地圖，對于地圖中已有特征的觀測值，計(jì)算實(shí)際觀測與估計(jì)觀測間的新息，利用新息對AUV狀態(tài)和地圖進(jìn)行更新。在AUV的行進(jìn)和探索過程中，系統(tǒng)不斷進(jìn)行“估計(jì)-校正”的迭代循環(huán)，克服了誤差累積，能夠獲得更加準(zhǔn)確的自身定位和環(huán)境地圖。

3 仿真實(shí)驗(yàn)及結(jié)果分析

利用仿真實(shí)驗(yàn)將基于SLAM的AUV導(dǎo)航算法與航位推算法進(jìn)行定位精度的比較，根據(jù)仿真結(jié)果分析算法性能。

3.1 仿真環(huán)境

仿真的硬件環(huán)境為Intel(R)Core2 Duo CPU T7250@2.00 GHz，2G RAM，Windows XP 操作系統(tǒng)，軟件利用 Matlab R2007b編寫。AUV的探索仿真環(huán)境設(shè)為圖2所示的平面矩形海域，環(huán)境中分布著一定數(shù)量的環(huán)境特征點(diǎn)，用“*”表示，AUV由(0 m，0 m)點(diǎn)出發(fā)(其初始位置的確定可借助GPS實(shí)現(xiàn))，沿圖中綠色曲線逆時針勻速運(yùn)動2周，在運(yùn)動過程中傳感器可以對前向180度范圍內(nèi)的特征點(diǎn)進(jìn)行觀測，其最大測量距離為100 m。

設(shè)AUV的運(yùn)動方程為:

式中:dt為兩時刻間的間隔時間;Uk和α(k)為k時刻的控制輸入量;L代表AUV的長度;wx，wy和wψ為系統(tǒng)噪聲。

觀測方程為:

式中:(xV(k)，yV(k))和(xi(k)，yi(k))分別為 k時刻AUV的坐標(biāo)和第i個路標(biāo)的坐標(biāo);wr和wθ為觀測噪聲。設(shè)AUV 長度為4 m，初始狀態(tài)為 X(0)v=［0，0，0］T，運(yùn)行線速度為3 m/s，最大舵角為30°;線速度誤差為0.2 m/s，角度誤差為 0.2 °/s;傳感器測距誤差為1 m/s，測角誤差為0.5°/s，這些誤差包含傳感器自身噪聲以及特征提取過程引入的誤差。

圖2 仿真環(huán)境

3.2 仿真結(jié)果分析

為避免實(shí)驗(yàn)隨機(jī)性對結(jié)果造成的影響，進(jìn)行20次獨(dú)立重復(fù)實(shí)驗(yàn)，利用實(shí)驗(yàn)結(jié)果數(shù)據(jù)的統(tǒng)計(jì)值進(jìn)行分析，引入均方根誤差(root mean squared error，RMSE)對算法性能進(jìn)行比較。

圖3是使用航位推算法產(chǎn)生的AUV定位RMSE隨時間變化情況，可以看出由于系統(tǒng)誤差和觀測誤差的存在，定位誤差不斷累積，總體上呈上升趨勢，最終得到的推位結(jié)果十分不可靠。圖4是使用SLAM導(dǎo)航算法對AUV狀態(tài)估計(jì)的RMSE隨時間變化曲線，從總體上看，后半段的定位誤差相比前半段有所降低，這說明AUV在完成一個探索閉環(huán)后再次對環(huán)境進(jìn)行探索的重定位過程可以進(jìn)一步提高估計(jì)精度。SLAM算法的定位誤差相比航位推算法的誤差大大降低，誤差的增長速度也受到有效控制，證明算法的估計(jì)精度較高，并且具有較好的收斂性。

圖3 航位推算定位誤差

圖4 SLAM定位誤差

圖5是使用基于SLAM的導(dǎo)航算法對AUV在整個行進(jìn)過程中傳感器觀測到的特征的位置估計(jì)誤差。由于AUV初始狀態(tài)中存在不確定性，并且在AUV行駛過程中只能對特征進(jìn)行有限次數(shù)的觀測，因此環(huán)境特征的位置估計(jì)也受到AUV狀態(tài)估計(jì)誤差的影響。由圖5可以看出，各特征的定位誤差保持在較低水平，說明SLAM導(dǎo)航算法能夠獲得比較可靠的構(gòu)圖精度。誤差分布的總體趨勢是位于AUV行駛路線后半段的特征位置估計(jì)誤差相對較大，這與AUV狀態(tài)估計(jì)誤差的分布情況基本一致?？紤]到AUV的探索范圍、控制誤差和傳感器觀測誤差的大小以及傳感器間同步問題，該SLAM導(dǎo)航算法獲得的定位和環(huán)境構(gòu)圖精度是可接受的。

仿真實(shí)驗(yàn)僅以驗(yàn)證算法的可行性和有效性為目的，因此假設(shè)數(shù)據(jù)關(guān)聯(lián)完全正確，并且只選用了較小的區(qū)域作為仿真環(huán)境，而實(shí)際當(dāng)中需要考慮水下環(huán)境非結(jié)構(gòu)特性對傳感器觀測、特征提取以及數(shù)據(jù)關(guān)聯(lián)等造成的影響。另外，根據(jù)實(shí)際任務(wù)的需要，AUV可能需要進(jìn)行大范圍、長時間的航行，這些對系統(tǒng)的穩(wěn)定性和計(jì)算效率提出了更高的要求，同時也為SLAM算法在水下自主導(dǎo)航中的應(yīng)用研究提供了更加廣闊的前景。

圖5 環(huán)境特征定位誤差

4 結(jié)論

本文基于同步定位與地圖創(chuàng)建的原理，建立了AUV導(dǎo)航系統(tǒng)的狀態(tài)模型和觀測模型，利用卡爾曼濾波框架實(shí)現(xiàn)了未知環(huán)境下AUV的自主定位導(dǎo)航。利用MATLAB進(jìn)行了區(qū)域探索仿真實(shí)驗(yàn)，試驗(yàn)結(jié)果表明:在存在控制誤差和觀測誤差的情況下，基于SLAM的導(dǎo)航算法獲得了較高的定位精度，并且具有良好的收斂性，證明這種導(dǎo)航算法具有廣闊的應(yīng)用前景。另外，如何利用SLAM算法在大規(guī)模、非結(jié)構(gòu)化以及動態(tài)性的水下環(huán)境中實(shí)現(xiàn)AUV自主定位是今后將要進(jìn)一步研究的內(nèi)容。

［1］Elbert S.Maloney Editer Dutton’s Navigation and Piloting［D］.Annapolis，M D Naval Institute Press，1985.

［2］張立川，徐德民，劉明雍，等.基于移動長基線的多AUV 協(xié)同系統(tǒng)［J］.機(jī)器人，2009，31(6):581 －593.

［3］寇義民，溫奇詠.一種基于地磁場曲面線性化擬合的快速導(dǎo)航定位方法［J］.宇航學(xué)報(bào)，2009，30(2):498 －501.

［4］楊洋.GPS/SINS深組合導(dǎo)航中的關(guān)鍵技術(shù)研究［D］.南京:南京理工大學(xué)，2012.

［5］孫鳳池，黃亞樓，康葉偉.基于視覺的移動機(jī)器人同時定位與建圖研究進(jìn)展［J］.控制理論與應(yīng)用，2010，27(4):488－494.

［6］宋宇，李慶玲，康軼非，等.平方根容積 Rao-Blackwillised粒子濾波 SLAM 算法［J］.自動化學(xué)報(bào)，2014，40(2):357－367.

［7］Camini M W M D，Paul N，Hugh F D W.A solution to the simultaneous and map building(SLAM)problem［J］.IEEE Transactions on Robotics and Automation，2001，17(3):229－241.