搜救環(huán)境中的仿生蛇形機(jī)器人SLAM 方法

王超杰，蘇 中，連曉峰，趙 旭

（1.北京信息科技大學(xué) 高動(dòng)態(tài)導(dǎo)航技術(shù)北京市重點(diǎn)實(shí)驗(yàn)室，北京100101；2.北京工商大學(xué) 計(jì)算機(jī)與信息工程學(xué)院，北京100048；3.北京理工大學(xué) 自動(dòng)化學(xué)院，北京100084）

0 引 言

與傳統(tǒng)的移動(dòng)機(jī)器人相比，蛇形機(jī)器人具有較高的運(yùn)動(dòng)穩(wěn)定性和較強(qiáng)的環(huán)境適應(yīng)能力，不僅可以在地震、礦災(zāi)等環(huán)境中進(jìn)行災(zāi)害搜救工作，也可以應(yīng)用于空間探索等領(lǐng)域［1，2］。

SLAM 問題在智能移動(dòng)機(jī)器人領(lǐng)域的重要性受到了廣泛的關(guān)注，出現(xiàn)了許多較為成熟的理論和實(shí)現(xiàn)方法，如EKFSLAM、FastSLAM、DP－SLAM 等［3－6］。SLAM 算法的計(jì)算和實(shí)現(xiàn)需要的運(yùn)算量較大，難以滿足實(shí)時(shí)性需求。在此情況下，本文提出一種MiniSLAM 算法，其運(yùn)算量少、實(shí)時(shí)性高，可應(yīng)用于蛇形機(jī)器人復(fù)雜地形環(huán)境的地圖構(gòu)建需要。

1 SLAM 問題描述

SLAM 最早由Smith等提出：在未知環(huán)境中移動(dòng)機(jī)器人從未知的位置出發(fā)，利用傳感器探測的環(huán)境信息建立環(huán)境地圖并同時(shí)對(duì)自身位置定位的過程。在SLAM 問題中，機(jī)器人的位置、環(huán)境地圖以及傳感器數(shù)據(jù)都具有不同程度的不確定性，概率機(jī)器人學(xué)采用概率模型可精確的描述這種不確定性［7］。為了降低問題的復(fù)雜度，假定SLAM 問題服從一階Markov過程，因此可以采用動(dòng)態(tài)貝葉斯網(wǎng)絡(luò)來描述SLAM 問題中的狀態(tài)轉(zhuǎn)移過程，如圖1所示。

圖1 SLAM 的狀態(tài)轉(zhuǎn)移過程

其中，環(huán)境地圖M 中包含一系列的環(huán)境特征M ＝｛m1，m2…mN｝。xt＝ （x，y，θ）表示t時(shí)刻機(jī)器人的位姿，x 和y 為二維笛卡爾中機(jī)器人的位置，θ為機(jī)器人的方向角。xt＝ ｛x1，x2…xt｝為初始時(shí)刻到t時(shí)刻機(jī)器人的運(yùn)動(dòng)軌跡，ut＝ ｛u1，u2…ut｝和Zt＝ ｛z1，z2…zt｝表示t時(shí)刻之前的控制輸入量和觀測量，ut代表t－1時(shí)刻到t時(shí)刻的控制輸入量，zt表示t時(shí)刻的觀測量，nt＝ ｛n1，n2…nt｝為觀測量對(duì)應(yīng)的特征標(biāo)識(shí)。

在一階Markov過程假設(shè)下，根據(jù)條件獨(dú)立性，可得

SLAM 問題的運(yùn)動(dòng)模型可表示為

SLAM 問題的觀測模型可表示為

其中：εt——t時(shí)刻的運(yùn)動(dòng)噪聲，服從N（0，Qt），δt為t時(shí)刻觀測噪聲，服從N（0，Rt）。SLAM 問題的目標(biāo)在于從帶噪聲的控制量ut和觀測量Zt中估計(jì)出環(huán)境地圖M 和機(jī)器人實(shí)時(shí)位姿xt。

2 MiniSLAM 算法

根據(jù)蛇形機(jī)器人環(huán)境搜索的需要，本文提出一種MiniSLAM 算法來提高地圖建立的實(shí)時(shí)性，其主要由兩部分組成：位姿更新和地圖更新。其中，采用Monte－Carlo算法實(shí)現(xiàn)當(dāng)前激光掃描與地圖的匹配來進(jìn)行位姿更新，采用改進(jìn)的Bresenham 算法進(jìn)行地圖更新。

2.1 Monte－Carlo算法

Monte－Carlo算法是一種基于貝葉斯濾波理論，利用粒子近似機(jī)器人在地圖上位置的算法，初始假定均勻的分布于環(huán)境空間中，隨著機(jī)器人移動(dòng)，粒子也做相應(yīng)移動(dòng)，機(jī)器人每次完成環(huán)境感知后，對(duì)這些粒子重新采樣。算法結(jié)束時(shí)，粒子會(huì)收斂于機(jī)器人的實(shí)際位置。Monte－Carlo算法包含以下幾個(gè)階段：

預(yù)測階段：利用運(yùn)動(dòng)模型以概率密度函數(shù)的形式來預(yù)測當(dāng)前機(jī)器人的位姿。假設(shè)當(dāng)前的狀態(tài)xk僅依賴于之前的狀態(tài)xk－1和已知的控制輸入uk－1，該運(yùn)動(dòng)模型被認(rèn)定為條件密度p（xk｜xk－1，uk－1），對(duì)于一階馬爾可夫過程，先驗(yàn)概率密度可通過積分得到

更新階段：根據(jù)測量模型集成傳感器信息來獲得后驗(yàn)概率密度p（xk｜Zk）。假定對(duì)于xk測量值z(mì)k與之前的測量值Zk－1是條件獨(dú)立的。測量模型以似然函數(shù)p（zk｜xk）的形式給出，這種形式表示在觀測值z(mì)k情況下，機(jī)器人位于xk的可能性。通過貝葉斯公式更新先驗(yàn)概率值，得到后驗(yàn)概率密度

2.2 Bresenham 地圖更新改進(jìn)算法

通過Monte－Carlo算法可以得到每個(gè)時(shí)刻機(jī)器人所在地圖上的位置Ps，通過坐標(biāo)變換可獲得激光探測的障礙物坐標(biāo)Pi。為實(shí)時(shí)得到完整的環(huán)境地圖，提出一種基于Bresenham 算法的環(huán)境地圖更新改進(jìn)算法［8－11］。

圖2為Bresenham地圖更新算法流程，其中Ps為激光位置坐標(biāo)，Pi為激光一次240°掃描范圍內(nèi)某一障礙物坐標(biāo)，map為地圖的起始位置，m＿size為地圖的尺寸大小，swap（）是交換函數(shù)，需要在地圖中更新Ps與Pi直線間像素點(diǎn)的值。將所有的激光掃描點(diǎn)集通過Bresenham 地圖更新算法更新到地圖中，獲得最終的環(huán)境地圖。

3 實(shí)驗(yàn)平臺(tái)

3.1 蛇形機(jī)器人簡介

本實(shí)驗(yàn)所采用的仿生蛇形機(jī)器人實(shí)物圖和結(jié)構(gòu)圖分別如圖3和圖4所示，該機(jī)器人具有如下幾個(gè)特點(diǎn)：①采用3D 打印而成，既縮短了加工周期又節(jié)約了成本；②通過ADAMS軟件仿真，進(jìn)行了機(jī)械結(jié)構(gòu)設(shè)計(jì)，具有6 個(gè)正交關(guān)節(jié)和1個(gè)分體機(jī)構(gòu)，腿部具有變形機(jī)構(gòu)，可以進(jìn)行站立、臥倒、直行、蜿蜒、蠕動(dòng)、分體、翻滾等步態(tài)；③機(jī)器人采用12V，1500mA 的電池供電，確保機(jī)器人能夠連續(xù)運(yùn)動(dòng)半小時(shí)以上；④頭部安裝有激光測距儀，可進(jìn)行環(huán)境探測。

圖2 Bresenham 地圖更新算法流程

圖3 仿生蛇形機(jī)器人實(shí)物

圖4 仿生蛇形機(jī)器人結(jié)構(gòu)

3.2 激光測距儀

實(shí)驗(yàn)所用激光測距儀型號(hào)為URG－04LX，其具有尺寸小、重量輕 （160g）、功耗低等優(yōu)點(diǎn)。其性能參數(shù)見表1。

3.3 通信模塊

實(shí)驗(yàn)過程中，通信模塊采用的是串口轉(zhuǎn)WIFI模塊USR－WIFI232－T，激光測距儀通過串口與其連接，這樣激光采集的數(shù)據(jù)就可通過網(wǎng)口傳輸?shù)絇C 端，PC 端通過對(duì)數(shù)據(jù)進(jìn)行處理進(jìn)行實(shí)時(shí)的地圖繪制。圖5 為激光與串口轉(zhuǎn)WIFI模塊的連接。

表1 URG－04LX激光測距儀性能參數(shù)

圖5 URG－04LX與USR－WIFI232－T連接

4 實(shí)驗(yàn)與結(jié)果分析

實(shí)驗(yàn)以仿生蛇形機(jī)器人為載體，搭載URG－04LX 激光測距儀和USR－WIFI232－T串口轉(zhuǎn)WIFI模塊，在不同的環(huán)境下驗(yàn)證算法的性能。

4.1 實(shí)驗(yàn)1

首先在相對(duì)規(guī)整的實(shí)驗(yàn)室環(huán)境下驗(yàn)證算法性能，激光測距儀安裝在蛇形機(jī)器人頭部，距地面約15cm，實(shí)驗(yàn)中蛇形機(jī)器人按圖6藍(lán)線軌跡以大約2m／s的速度探測實(shí)驗(yàn)室，箭頭指示機(jī)器人的運(yùn)動(dòng)起點(diǎn)。

圖6 實(shí)驗(yàn)室環(huán)境

實(shí)驗(yàn)室SLAM 如圖7所示。

圖7 實(shí)驗(yàn)室SLAM

SLAM 圖的左邊部分對(duì)應(yīng)于實(shí)驗(yàn)室環(huán)境的正前方部分，實(shí)驗(yàn)所創(chuàng)建的地圖與實(shí)驗(yàn)室的真實(shí)環(huán)境完全一致，圖中虛線顯示的是蛇形機(jī)器人的運(yùn)動(dòng)軌跡，與真實(shí)運(yùn)動(dòng)軌跡相一致，而且軌跡幾乎是閉合的，能夠在開環(huán)的情況下實(shí)現(xiàn)閉環(huán)效果。

4.2 實(shí)驗(yàn)2

為了模擬震后受災(zāi)現(xiàn)場，在實(shí)驗(yàn)室中人工搭建搜救實(shí)驗(yàn)環(huán)境，如圖8所示，地面上堆放著石頭、木板、箱子等雜物來模擬廢墟環(huán)境，實(shí)驗(yàn)中蛇形機(jī)器人按圖中軌跡進(jìn)行環(huán)境探測。圖9為蛇形機(jī)器人在模擬搜救環(huán)境中SLAM 實(shí)驗(yàn)結(jié)果。

圖8 人工搭建模擬搜救實(shí)驗(yàn)環(huán)境

通過對(duì)比模擬的搜救環(huán)境與實(shí)驗(yàn)所創(chuàng)建的地圖可以看出，實(shí)驗(yàn)創(chuàng)建的地圖能準(zhǔn)確的反映出環(huán)境的特征，可以有效的完成搜救環(huán)境中地圖創(chuàng)建的任務(wù)。

為了證明算法計(jì)算量少、實(shí)時(shí)性高，在實(shí)驗(yàn)硬件條件和地圖分辨率相同的情況下，對(duì)MiniSLAM 算法和EKFSLAM 算法、FastSLAM 算法進(jìn)行了比較。表2為3種算法的比較結(jié)果。

圖9 模擬搜救環(huán)境SLAM 實(shí)驗(yàn)結(jié)果

由表2可知，MiniSLAM 算法處理一次掃描的時(shí)間小于激光測距儀的采樣時(shí)間 （100ms），能夠滿足實(shí)時(shí)性的要求。MiniSLAM 算法相比于其它兩種算法，處理一次激光掃描點(diǎn)集的時(shí)間最短，并且不會(huì)隨處理掃描點(diǎn)集的數(shù)量變化而大幅度變化，能夠滿足搜救環(huán)境實(shí)時(shí)性建圖的需求。

表2 不同算法處理一次掃描時(shí)間比較／ms

5 結(jié)束語

本文針對(duì)搜救環(huán)境中蛇形機(jī)器人SLAM 的需求，設(shè)計(jì)了一種簡單有效的MiniSLAM 算法，該算法利用蛇形機(jī)器人運(yùn)動(dòng)過程中激光測距儀采集的數(shù)據(jù)進(jìn)行地圖構(gòu)建。實(shí)驗(yàn)結(jié)果顯示MiniSLAM 算法建圖精度高、計(jì)算量少、實(shí)時(shí)性高，幾乎能夠在開環(huán)的情況下實(shí)現(xiàn)閉環(huán)效果，能夠滿足搜救環(huán)境中的實(shí)時(shí)建圖需求。

下一階段，將會(huì)改進(jìn)算法，致力于實(shí)現(xiàn)閉環(huán)算法，從而能夠?qū)崿F(xiàn)自動(dòng)地檢測閉環(huán)并相應(yīng)地糾正機(jī)器人的運(yùn)動(dòng)軌跡。同時(shí)也會(huì)考慮加入視覺傳感器或里程計(jì)以提高創(chuàng)建地圖的精度。

［1］WANG Nan，MA Shugen，LI Bin，et al.Simultaneous localization and mapping based on the hybrid metric－topological map in the ruins environment［J］.Robot，2013，35 （6）：762－768（in Chinese）.［王楠，馬書根，李斌，等.基于拓?fù)涿字苹旌系貓D的廢墟環(huán)境同步定位與地圖構(gòu)建 ［J］.機(jī)器人，2013，35（6）：762－768.］

［2］WANG Nan，MA Shugen，LI Bin，et al.Simultaneous localization and mapping based on morphological characteristics of the seismic damage of the interior ruins ［J］.Chin Sci Bull（Chin Ver），2013，58 （S2）：104－111 （in Chinese）. ［王楠，馬書根，李斌，等.面向廢墟內(nèi)部震害形態(tài)的同步定位與地圖構(gòu)建［J］.通報(bào)學(xué)報(bào)，2013，58 （S2）：104－111.］

［3］TU Gangyi，JIN Shijun，ZHU Xuefen，et al.Particle filter SLAM method for mobile robot［J］.Journal of Southeast University （Natural Science Edition），2010，40 （1）：117－122（in Chinese）.［涂剛毅，金世俊，祝雪芬，等.基于粒子濾波的移動(dòng)機(jī)器人SLAM 算法 ［J］.東南大學(xué)學(xué)報(bào) （自然科學(xué)版），2010，40 （1）：117－122.］

［4］ZHU Jihua，ZHENG Nanning，YUAN Zejian，et al.A SLAM approach by combining ICP algorithm and partial filter［J］.Acta Automatica Sinica，2009，35 （8）：1107－1113 （in Chinese）.［祝繼華，鄭南寧，袁澤劍，等.基于ICP 算法和粒子濾波的未知環(huán)境地圖創(chuàng)建 ［J］.自動(dòng)化學(xué)報(bào)，2009，35（8）：1107－1113.］

［5］ZHANG Jialong.Research on SLAM problem of indoor mobile robots based on particle filter ［D］.Harbin：Harbin Institute of Technology，2011 （in Chinese）. ［張家隆.基于粒子濾波的室內(nèi)移動(dòng)機(jī)器人SLAM 問題研究 ［D］.哈爾濱：哈爾濱工業(yè)大學(xué)，2011.］

［6］ZHAO Xuan，HE Bo，JI Dezhi，et al.Location and tracking of moving objects based on particle filter for mobile robot［J］.Journal of System Simulation，2008，20 （23）：6490－6497 （in Chinese）.［趙璇，何波，吉德志，等.基于粒子濾波的機(jī)器人定位及動(dòng)態(tài)目標(biāo)跟蹤 ［J］.系統(tǒng)仿真學(xué)報(bào)，2008，20 （23）：6490－6497.］

［7］Steux Bruno，EL Hamzaoui Oussama.tinySLAM：A SLAM algorithm in less than 200lines C－Language program ［C］／／11th International Conference on Control，Automation，Robotics and Vision，2010：1975－1979.

［8］Soleimani B，Ashtiani M－H Z，Soleimani B H，et al.A disaster invariant feature for localization ［C］／／IEEE／RSJ International Conference on Intelligent Robots and Systems.Piscataway，USA：IEEE，2010：1096－1101.

［9］Sun R，Ma S，Li B，et al.A simultaneous localization and mapping algorithm in complex environments：SLASEM ［J］.Advanced Robotics，2011，25 （6／7）：941－962.

［10］LIU Shaogang，GUO Yunlong，JIA Heming.Rescue robot simultaneous localization and mapping based on extraction and matching of line features［J］.Journal of Jilin University （Engineering and Technology Edition），2013，43 （4）：1035－1044 （in Chinese）.［劉少剛，郭云龍，賈鶴鳴.基于直線特征提取匹配搜救機(jī)器人的同步定位與地圖構(gòu)建 ［J］.吉林大學(xué)學(xué)報(bào) （工學(xué)版），2013，43 （4）：1035－1044.］

［11］GUO Shuai，MA Shugen，LI Bin，et al.Simultaneous localization and mapping through a Voronoi－diagram－based map representation ［J］.Acta Automatica Sinica，2011，37 （9）：1095－1104 （in Chinese）. ［郭帥，馬書根，李斌，等.基于Voronoi地圖標(biāo)示方法的同步定位與地圖創(chuàng)建 ［J］.自動(dòng)化學(xué)報(bào)，2011，37 （9）：1095－1104.］