李 超，徐東勛，袁昌斌

（中國海洋大學(xué) 信息科學(xué)與工程學(xué)院,山東 青島266100）

同步定位與地圖構(gòu)建(SLAM)算法指自主式水下機(jī)器人[1-2](AUV)在未知水下環(huán)境中運動時，利用自身攜帶的各種傳感器識別周圍的環(huán)境特征，增量式地創(chuàng)建環(huán)境地圖，同時用該地圖實現(xiàn)自身位置的定位[3-4]。因此SLAM算法是水下機(jī)器人真正實現(xiàn)自主導(dǎo)航的關(guān)鍵。

水下機(jī)器人在實際的導(dǎo)航與定位中，只采用單個傳感器提供的信息容易受到本身特性及周圍環(huán)境的制約，使得部分測量信息不可靠，難以滿足水下機(jī)器人導(dǎo)航需要。針對單個傳感器感知周圍環(huán)境存在的這些缺陷[5]，采用多個傳感器可以獲得更可靠的量測，可以有效地補(bǔ)償單個傳感器帶來的誤差和不穩(wěn)定性[6]。

通過AUV自身搭載的各種傳感器，可以獲得速度、加速度、角速度和角加速度。為了得到更優(yōu)的估計，可以采用融合算法 (卡爾曼濾波)對不同傳感器提供的同一個量測進(jìn)行融合。應(yīng)用到SLAM算法中，即在未知的水下環(huán)境中，為了實現(xiàn)精確的機(jī)器人定位和環(huán)境地圖，以卡爾曼濾波理論和SLAM算法為核心，在僅聲納更新的SLAM算法的基礎(chǔ)上，提出航向和速度的更新，以此獲得更加精確的機(jī)器人位姿，并在此基礎(chǔ)上構(gòu)建周圍環(huán)境地圖。

1 多傳感器更新的EKF-SLAM算法

1.1 算法流程圖

本文采用基于擴(kuò)展卡爾曼濾波EKF(Extended Kalman Filter)的SLAM算法，其中機(jī)器人的狀態(tài)向量Xv=(xv，yv，φv，vx，vy)T[7]，其中前兩個分量為 x、y 方向的位置，φ為機(jī)器人的航向角，vx、vy分別為機(jī)器人 x、y方向的速度。多傳感器更新的SLAM算法的流程圖如圖1所示。

1.2 偽代碼描述

1.3 航向速度傳感器觀測更新

多傳感器更新的SLAM算法中引入了速度航向的更新，即從傳感器中獲得觀測值，結(jié)合EKF理論來更新航向和速度，得到的機(jī)器人位姿更為精確。航向和速度更新中，傳感器的觀測向量包含航向、橫向速度和縱向速度三個分量。其中，傳感器的觀測向量和觀測矩陣為：

其中，θ為傳感器羅盤提供的角度，vx、vy為傳感器 DVL采集的速度。

由 EKF公式，新息和新息協(xié)方差如式（3）和式（4）所示：

其中，Rd為高斯白噪聲，代表角速度和速度傳感器的觀測噪聲，具體形式如式（5）所示。

由EKF的更新方程可得卡爾曼增益、更新后狀態(tài)向量X和協(xié)方差矩陣P分別為：

2 湖試實驗和結(jié)果分析

本文試驗平臺是中國海洋大學(xué)UVL實驗室自主設(shè)計的“C-Ranger”號 AUV，如圖 2 所示。 “C-Ranger”采用開架式結(jié)構(gòu)設(shè)計，由上下兩個電子艙和5個水下推進(jìn)器組成，AUV 搭載有自主導(dǎo)航所需的 DVL、Gyro、HARS、GPS等多種傳感器。本次試驗是在中國海洋大學(xué)約60 m×35 m的湖中進(jìn)行，為了使聲納能夠掃描到湖岸邊的環(huán)境特征，將聲納掃描距離設(shè)置為50 m，角度掃描范圍設(shè)置為 120°。

圖3和圖4分別為僅聲納更新的SLAM算法的軌跡和地圖以及多傳感器更新的SLAM算法的軌跡和地圖。其中，將由GPS數(shù)據(jù)得到的軌跡線作為一個基準(zhǔn)，用于比較SLAM算法軌跡和推位算法軌跡的效果?！啊痢避壽E線表示推位軌跡，“+”軌跡線由 SLAM算法得到。圖中黑點是通過SLAM算法最終構(gòu)建的全局環(huán)境地圖，圓圈為試驗中預(yù)設(shè)的10個特征點，從構(gòu)建的全局環(huán)境地圖中可以明顯地看出湖岸的形狀，符合實際的試驗環(huán)境。

由圖3可以看出，隨著AUV的航行，推位軌跡出現(xiàn)了發(fā)散現(xiàn)象。僅聲納更新的SLAM算法的軌跡明顯改善了航位推算算法軌跡的發(fā)散現(xiàn)象，但在航程的最后階段SLAM算法的軌跡出現(xiàn)倒退，這是由于數(shù)據(jù)關(guān)聯(lián)時特征的錯誤關(guān)聯(lián)，使得機(jī)器人的軌跡偏離原先的方向。而由圖4可以看出，引入了多傳感器更新的SLAM算法的軌跡有效地改善了圖3中推位軌跡的發(fā)散現(xiàn)象，其軌跡最終也收斂于GPS軌跡。這是由于在聲納更新的同時引入了航向和速度更新，使得AUV的定位更精確，在此基礎(chǔ)上進(jìn)行預(yù)測、觀測和更新，可以獲得最優(yōu)的狀態(tài)估計，最終得到的SLAM軌跡更接近AUV的實際航行路徑。

由圖5和圖6可以明顯看出，多傳感器更新的SLAM算法的軌跡效果明顯優(yōu)于僅聲納更新的SLAM算法軌跡，更接近AUV真實的航行路徑，并最終收斂于GPS軌跡。多傳感器更新的SLAM算法軌跡偏差也始終小于僅聲納更新的SLAM算法，且多傳感器更新的SLAM算法的機(jī)器人定位的最大偏差小于航程的1.3%，能夠滿足水下機(jī)器人導(dǎo)航的要求，構(gòu)建的全局環(huán)境地圖與實際環(huán)境相符，驗證了多傳感器更新的SLAM算法的有效性。

本文主要在僅聲納更新的SLAM算法的基礎(chǔ)上，介紹了基于EKF的多傳感器更新的SLAM算法的流程圖和偽代碼。最后通過實驗結(jié)果從定位誤差和構(gòu)圖精度上分析可以看到后者明顯優(yōu)于前者，從而驗證了多傳感器更新的SLAM算法的有效性，可滿足水下機(jī)器人自主導(dǎo)航的要求。

[1]MILLER P A，F(xiàn)ARRELL J A，ZHAO Y.Autonomous underwater vehicle navigation[J].IEEE Journal of Oceanic Engineering，2010，35(3)：663-678.

[2]BOVIO E，CECCHI D，BARALLI F.Autonomous underwater vehicles for scientific and naval operations[J].Annual Reviews in Control，2006(30)：117-130.

[3]LEONARD J，BENNETT A，SMITH C.Autonomous underwater vehicle navigation[C].MIT Marine Robotics Laboratory Technical Memorandum，1998：1-17.

[4]BAILEY T.Mobile robot localization and mapping in extensive outdoor environments[D].University of Sydney，Australian.Centre for Field Robotics，2002.

[5]DURRANT-WHYTE H.Uncertain geometry in robotics[J].IEEE Transactions on Robotics and Automation，1988(4)：23-31.

[6]譚民，王碩，曹志強(qiáng).多機(jī)器人系統(tǒng)[M].北京：清華大學(xué)出版社，2005.

[7]周福娜，周梅，文成林.多傳感器數(shù)據(jù)融合效果分析[J].河南大學(xué)學(xué)報(自然科學(xué)版)，2003，33（2）：33-36.