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

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

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

0 引 言

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

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

1 SLAM 問題描述

SLAM 最早由Smith等提出：在未知環(huán)境中移動機器人從未知的位置出發(fā)，利用傳感器探測的環(huán)境信息建立環(huán)境地圖并同時對自身位置定位的過程。在SLAM 問題中，機器人的位置、環(huán)境地圖以及傳感器數(shù)據(jù)都具有不同程度的不確定性，概率機器人學(xué)采用概率模型可精確的描述這種不確定性［7］。為了降低問題的復(fù)雜度，假定SLAM 問題服從一階Markov過程，因此可以采用動態(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時刻機器人的位姿，x 和y 為二維笛卡爾中機器人的位置，θ為機器人的方向角。xt＝ ｛x1，x2…xt｝為初始時刻到t時刻機器人的運動軌跡，ut＝ ｛u1，u2…ut｝和Zt＝ ｛z1，z2…zt｝表示t時刻之前的控制輸入量和觀測量，ut代表t－1時刻到t時刻的控制輸入量，zt表示t時刻的觀測量，nt＝ ｛n1，n2…nt｝為觀測量對應(yīng)的特征標(biāo)識。

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

SLAM 問題的運動模型可表示為

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

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

2 MiniSLAM 算法

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

2.1 Monte－Carlo算法

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

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

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

2.2 Bresenham 地圖更新改進算法

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

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

3 實驗平臺

3.1 蛇形機器人簡介

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

圖2 Bresenham 地圖更新算法流程

圖3 仿生蛇形機器人實物

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

3.2 激光測距儀

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

3.3 通信模塊

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

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

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

4 實驗與結(jié)果分析

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

4.1 實驗1

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

圖6 實驗室環(huán)境

實驗室SLAM 如圖7所示。

圖7 實驗室SLAM

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

4.2 實驗2

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

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

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

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

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

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

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

5 結(jié)束語

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

下一階段，將會改進算法，致力于實現(xiàn)閉環(huán)算法，從而能夠?qū)崿F(xiàn)自動地檢測閉環(huán)并相應(yīng)地糾正機器人的運動軌跡。同時也會考慮加入視覺傳感器或里程計以提高創(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）.［王楠，馬書根，李斌，等.基于拓撲米制混合地圖的廢墟環(huán)境同步定位與地圖構(gòu)建 ［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］.通報學(xué)報，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）.［涂剛毅，金世俊，祝雪芬，等.基于粒子濾波的移動機器人SLAM 算法 ［J］.東南大學(xué)學(xué)報 （自然科學(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］.自動化學(xué)報，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)移動機器人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）.［趙璇，何波，吉德志，等.基于粒子濾波的機器人定位及動態(tài)目標(biāo)跟蹤 ［J］.系統(tǒng)仿真學(xué)報，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）.［劉少剛，郭云龍，賈鶴鳴.基于直線特征提取匹配搜救機器人的同步定位與地圖構(gòu)建 ［J］.吉林大學(xué)學(xué)報 （工學(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］.自動化學(xué)報，2011，37 （9）：1095－1104.］