摘 要： 針對(duì)同步定位與地圖構(gòu)建SLAM(Simultaneous Localization and Mapping)算法中，單個(gè)傳感器提供的信息受到傳感器本身特性和周?chē)h(huán)境制約的局限性，提出了在僅聲納更新的SLAM算法同時(shí)，引入航向和速度的多傳感器更新，使得自主式水下機(jī)器人AUV（Autonomous Underwater Vehicle）的定位和構(gòu)圖更精確。通過(guò)對(duì)實(shí)驗(yàn)結(jié)果的分析驗(yàn)證了多傳感器更新的SLAM算法的有效性。
關(guān)鍵詞： 同步定位與地圖構(gòu)建；多傳感器；自主式水下機(jī)器人
同步定位與地圖構(gòu)建(SLAM)算法指自主式水下機(jī)器人[1-2](AUV)在未知水下環(huán)境中運(yùn)動(dòng)時(shí)，利用自身攜帶的各種傳感器識(shí)別周?chē)沫h(huán)境特征，增量式地創(chuàng)建環(huán)境地圖，同時(shí)用該地圖實(shí)現(xiàn)自身位置的定位[3-4]。因此SLAM算法是水下機(jī)器人真正實(shí)現(xiàn)自主導(dǎo)航的關(guān)鍵。
水下機(jī)器人在實(shí)際的導(dǎo)航與定位中，只采用單個(gè)傳感器提供的信息容易受到本身特性及周?chē)h(huán)境的制約，使得部分測(cè)量信息不可靠，難以滿(mǎn)足水下機(jī)器人導(dǎo)航需要。針對(duì)單個(gè)傳感器感知周?chē)h(huán)境存在的這些缺陷[5]，采用多個(gè)傳感器可以獲得更可靠的量測(cè)，可以有效地補(bǔ)償單個(gè)傳感器帶來(lái)的誤差和不穩(wěn)定性[6]。
通過(guò)AUV自身搭載的各種傳感器，可以獲得速度、加速度、角速度和角加速度。為了得到更優(yōu)的估計(jì)，可以采用融合算法(卡爾曼濾波)對(duì)不同傳感器提供的同一個(gè)量測(cè)進(jìn)行融合。應(yīng)用到SLAM算法中，即在未知的水下環(huán)境中，為了實(shí)現(xiàn)精確的機(jī)器人定位和環(huán)境地圖，以卡爾曼濾波理論和SLAM算法為核心，在僅聲納更新的SLAM算法的基礎(chǔ)上，提出航向和速度的更新，以此獲得更加精確的機(jī)器人位姿，并在此基礎(chǔ)上構(gòu)建周?chē)h(huán)境地圖。
1 多傳感器更新的EKF-SLAM算法
1.1 算法流程圖
本文采用基于擴(kuò)展卡爾曼濾波EKF(Extended Kalman Filter)的SLAM算法，其中機(jī)器人的狀態(tài)向量Xv=(xv，yv，Φv，vx，vy)T[7]，其中前兩個(gè)分量為x、y方向的位置，Φ為機(jī)器人的航向角，vx、vy分別為機(jī)器人x、y方向的速度。多傳感器更新的SLAM算法的流程圖如圖1所示。
1.2 偽代碼描述
/* SLAM 算法 */
gloabl X Px; //全局狀態(tài)變量X，P
[X, Px]=initialize(0); //初始化
WHILE sensors_data DO
[X, Px]=predict (Head, Vx, Vy, Wn, dt);
//預(yù)測(cè)階段，Vx,Vy從DVL傳感器獲得, Wn從GYRO傳感器中獲得, head從AHRS傳感器中獲得,dt是預(yù)測(cè)間隔
[X, Px]=update(Head, Vx,Vy);//更新階段
IF sonar_data THEN
DO
[Zf, Zn]=data_association(Z,GATE1,GATE2);
//數(shù)據(jù)關(guān)聯(lián)階段，GATE1是進(jìn)行數(shù)據(jù)關(guān)聯(lián)的最大距離, GATE2 是建立新地圖特征的最小距離
IF ~empty(Zf) THEN
DO [X, P]=update_sonar(Zf);
//Zf是關(guān)聯(lián)上的特征，即地圖中已有特征，用于更新
ELSE ~empty(Zn) THEN
[X, Px]=augment_sonar(Zn);
//Zn是未關(guān)聯(lián)上的特征，即地圖中沒(méi)有的特征，用于擴(kuò)充
END IF
END IF
END WHILE
1.3 航向速度傳感器觀測(cè)更新
多傳感器更新的SLAM算法中引入了速度航向的更新，即從傳感器中獲得觀測(cè)值，結(jié)合EKF理論來(lái)更新航向和速度，得到的機(jī)器人位姿更為精確。航向和速度更新中，傳感器的觀測(cè)向量包含航向、橫向速度和縱向速度三個(gè)分量。其中，傳感器的觀測(cè)向量和觀測(cè)矩陣為：
軌跡線表示推位軌跡，“+”軌跡線由SLAM算法得到。圖中黑點(diǎn)是通過(guò)SLAM算法最終構(gòu)建的全局環(huán)境地圖，圓圈為試驗(yàn)中預(yù)設(shè)的10個(gè)特征點(diǎn)，從構(gòu)建的全局環(huán)境地圖中可以明顯地看出湖岸的形狀，符合實(shí)際的試驗(yàn)環(huán)境。
由圖3可以看出，隨著AUV的航行，推位軌跡出現(xiàn)了發(fā)散現(xiàn)象。僅聲納更新的SLAM算法的軌跡明顯改善了航位推算算法軌跡的發(fā)散現(xiàn)象，但在航程的最后階段SLAM算法的軌跡出現(xiàn)倒退，這是由于數(shù)據(jù)關(guān)聯(lián)時(shí)特征的錯(cuò)誤關(guān)聯(lián)，使得機(jī)器人的軌跡偏離原先的方向。而由圖4可以看出，引入了多傳感器更新的SLAM算法的軌跡有效地改善了圖3中推位軌跡的發(fā)散現(xiàn)象，其軌跡最終也收斂于GPS軌跡。這是由于在聲納更新的同時(shí)引入了航向和速度更新，使得AUV的定位更精確，在此基礎(chǔ)上進(jìn)行預(yù)測(cè)、觀測(cè)和更新，可以獲得最優(yōu)的狀態(tài)估計(jì)，最終得到的SLAM軌跡更接近AUV的實(shí)際航行路徑。
由圖5和圖6可以明顯看出，多傳感器更新的SLAM算法的軌跡效果明顯優(yōu)于僅聲納更新的SLAM算法軌跡，更接近AUV真實(shí)的航行路徑，并最終收斂于GPS軌跡。多傳感器更新的SLAM算法軌跡偏差也始終小于僅聲納更新的SLAM算法，且多傳感器更新的SLAM算法的機(jī)器人定位的最大偏差小于航程的1.3%，能夠滿(mǎn)足水下機(jī)器人導(dǎo)航的要求，構(gòu)建的全局環(huán)境地圖與實(shí)際環(huán)境相符，驗(yàn)證了多傳感器更新的SLAM算法的有效性。
本文主要在僅聲納更新的SLAM算法的基礎(chǔ)上，介紹了基于EKF的多傳感器更新的SLAM算法的流程圖和偽代碼。最后通過(guò)實(shí)驗(yàn)結(jié)果從定位誤差和構(gòu)圖精度上分析可以看到后者明顯優(yōu)于前者，從而驗(yàn)證了多傳感器更新的SLAM算法的有效性，可滿(mǎn)足水下機(jī)器人自主導(dǎo)航的要求。
參考文獻(xiàn)
[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é)報(bào)(自然科學(xué)版)，2003，33（2）：33-36.