安防機(jī)器人的自主導(dǎo)航方法設(shè)計(jì)與實(shí)現(xiàn)

張嘉誠(chéng)，劉文匯，袁吳越，王澤坤，巢 淵

（江蘇理工學(xué)院 機(jī)械工程學(xué)院，江蘇 常州 213001）

室內(nèi)安防機(jī)器人通常工作在半結(jié)構(gòu)化環(huán)境中，其環(huán)境布局多為長(zhǎng)條形、回字形走廊，由平整且垂直于地面墻壁組成的空間、允許人通過的寬敞通道、桌椅支撐腿組成的狹窄低矮通道以及連接樓層之間的臺(tái)階等構(gòu)成。為適應(yīng)該環(huán)境下機(jī)器人的移動(dòng)，其底盤發(fā)展出了如輪式、履帶式、腿足式等多種形式。其中，麥克納姆輪（Mecanum）由若干個(gè)與輪轂軸線存在45°偏置角的輥?zhàn)訃@中心輪組成，輥?zhàn)优c地面產(chǎn)生與輥?zhàn)虞S方向相反的摩擦力，該摩擦力可分解為沿底盤前進(jìn)方向和垂直于底盤前進(jìn)方向的兩個(gè)分力，通過調(diào)整分力大小可以實(shí)現(xiàn)底盤全向移動(dòng)。

移動(dòng)安防機(jī)器人在未知室內(nèi)環(huán)境下工作時(shí)，需要自主決策運(yùn)動(dòng)路徑、巡視整個(gè)空間范圍并構(gòu)建完整環(huán)境地圖，以實(shí)現(xiàn)無人干預(yù)的自主導(dǎo)航。而上述環(huán)境通常具有面積小、結(jié)構(gòu)化、動(dòng)態(tài)情況少等特點(diǎn)，因而更側(cè)重于依靠機(jī)器人自身傳感器實(shí)現(xiàn)靜態(tài)環(huán)境的地圖構(gòu)建與導(dǎo)航。近年來，同步定位與建圖（simultaneous localization and mapping，SLAM）技術(shù)廣受關(guān)注，它有效推進(jìn)了對(duì)復(fù)雜場(chǎng)景的描述和理解，使導(dǎo)航更加趨于智能化。由于實(shí)現(xiàn)準(zhǔn)確導(dǎo)航與避障的前提是獲取精確的室內(nèi)二維柵格地圖；因此，本文應(yīng)用基于SLAM方法的定位與導(dǎo)航技術(shù)，實(shí)現(xiàn)精確定位與室內(nèi)地圖的構(gòu)建。常用SLAM包含視覺與激光雷達(dá)兩個(gè)方向：經(jīng)典視覺SLAM算法有ORB-SLAM[1]、SVOSLAM[2]等，其實(shí)現(xiàn)方式主要有基于點(diǎn)云數(shù)據(jù)、深度相機(jī)方法與位姿變化比較方法的距離計(jì)算，以及基于單目、雙目或魚眼相機(jī)方法[3]等；激光雷達(dá)SLAM技術(shù)目前較為成熟，經(jīng)典方法有GMapping[4]、Hector[5]、Karto[6]和Cartographer[7]等，能夠?qū)崿F(xiàn)2D和3D建圖[8]。

安防機(jī)器人在對(duì)室內(nèi)環(huán)境建圖過程中，需通過算法規(guī)劃全局路徑與局部路徑。常用全局路徑規(guī)劃算法包括Dijkstra算法、RRT算法與A*算法等。Dijkstra算法通過集合間的判斷與遍歷，以廣度優(yōu)先尋找最短路徑，由于遍歷結(jié)果較多導(dǎo)致效率較低；RRT算法（Rapidly Exploring Random Tree）路徑規(guī)劃具有快速性與隨機(jī)性，但隨機(jī)點(diǎn)的生成過程易陷入局部最優(yōu)；A*算法采用啟發(fā)式搜索方法，在使搜索方向靠向目標(biāo)點(diǎn)的同時(shí)，兼顧了優(yōu)先搜索路徑代價(jià)較小的點(diǎn)，省去無意義節(jié)點(diǎn)的計(jì)算，從而能夠有效提高算法效率。全局路徑規(guī)劃能夠獲取完整路徑軌跡，但其不適用于快速避障，且路徑規(guī)劃計(jì)算量較大，若因局部路徑變化而反復(fù)調(diào)整全局路徑還將降低導(dǎo)航效率。局部路徑規(guī)劃可使用全局地圖的一部分來計(jì)算最佳運(yùn)行軌跡，從而減小計(jì)算量，提高機(jī)器人的反應(yīng)能力。應(yīng)用動(dòng)態(tài)窗口算法可實(shí)現(xiàn)局部路徑規(guī)劃，提高導(dǎo)航效率。

1 安防機(jī)器人實(shí)驗(yàn)平臺(tái)

本文設(shè)計(jì)的安防機(jī)器人實(shí)驗(yàn)平臺(tái)，由四個(gè)直流減速編碼電機(jī)驅(qū)動(dòng)麥克納姆輪實(shí)現(xiàn)全向移動(dòng)，配備12 000 mAH電池向機(jī)器人系統(tǒng)供電。以樹莓派作為核心數(shù)據(jù)處理器；TF卡用于存儲(chǔ)樹莓派鏡像和地圖等信息；OpenCRP4是實(shí)驗(yàn)平臺(tái)控制器；用RPLidar A1型激光雷達(dá)進(jìn)行環(huán)境掃描與信息采集，并將結(jié)果傳輸給樹莓派。由安裝了ubuntu 18.04虛擬機(jī)的上位機(jī)（計(jì)算機(jī)端）通過WiFi模塊實(shí)現(xiàn)與樹莓派的通訊，再由樹莓派將采集到的數(shù)據(jù)信息傳給計(jì)算機(jī)端，計(jì)算機(jī)將相應(yīng)的控制指令傳送給機(jī)器人。

2 安防機(jī)器人定位與建圖方法設(shè)計(jì)

2.1 定位方法設(shè)計(jì)

自適應(yīng)蒙特卡洛定位算法（Adaptive Monte Carlo Localization，AMCL）是基于粒子濾波的一種不受環(huán)境限制的2D概率定位算法，它是對(duì)僅基于已知環(huán)境地圖的估計(jì)機(jī)器人位姿的蒙特卡洛算法的一種改進(jìn)。在ROS系統(tǒng)中使用AMCL算法來跟蹤估計(jì)任意2D環(huán)境下機(jī)器人的位姿。AMCL算法通過一系列加權(quán)粒子來表示機(jī)器人估計(jì)位姿，當(dāng)機(jī)器人移動(dòng)到下一個(gè)位置時(shí)，新粒子根據(jù)上一時(shí)刻粒子的權(quán)重重新生成，且權(quán)重越高生成概率越大，在舍棄低權(quán)重粒子的同時(shí)能夠隨機(jī)生成新的粒子。粒子的權(quán)重在機(jī)器人運(yùn)行過程中通過運(yùn)動(dòng)模型、傳感器模型和實(shí)時(shí)數(shù)據(jù)不斷改變。通過粒子的不斷迭代就能使其逐漸收斂到一個(gè)區(qū)域，從而獲得機(jī)器人最大概率的位姿。通過重采樣和庫(kù)爾貝克-萊布勒散度方法交替進(jìn)行采樣[10-11]。

2.2 建圖方法設(shè)計(jì)

GMapping算法的實(shí)現(xiàn)基于濾波SLAM框架，在獲知傳感器與里程計(jì)數(shù)據(jù)的情況下推算機(jī)器人位姿和地圖信息，結(jié)合條件聯(lián)合概率分布與貝葉斯公式[12]，可表示為以下概率形式：

其中，算法參數(shù)描述為：M-地圖，U-里程計(jì)信息，X-機(jī)器人位姿，Z-傳感器數(shù)據(jù)。

式（1）將機(jī)器人的軌跡估計(jì)轉(zhuǎn)化為一個(gè)增量估計(jì)的問題。其中：p(Xt|Xt-1,Ut)為基于前一時(shí)刻下機(jī)器人的位姿狀態(tài)和這一時(shí)刻下里程計(jì)信息估計(jì)當(dāng)前時(shí)刻機(jī)器人位姿狀態(tài)；p(X1:t-1|U1:t-1,Z1:t-1)使用上一時(shí)刻的粒子群表示前一時(shí)刻下機(jī)器人的位姿狀態(tài)；每個(gè)粒子都使用p(Xt|Xt-1,Ut)作為運(yùn)動(dòng)學(xué)模型進(jìn)行狀態(tài)傳播，來得到粒子對(duì)應(yīng)的預(yù)測(cè)軌跡；對(duì)每一個(gè)傳播之后的粒子，使用p(Zt|Xt)作為觀測(cè)模型進(jìn)行權(quán)重計(jì)算，并做歸一化計(jì)算；p(M|X1:t,Z1:t)表示使用當(dāng)前時(shí)刻的機(jī)器人位姿和當(dāng)前時(shí)刻的傳感器數(shù)據(jù)進(jìn)行地圖構(gòu)建。至此，可完成基于GMapping的地圖構(gòu)建過程。

3 安防機(jī)器人路徑規(guī)劃方案設(shè)計(jì)

3.1 全局路徑規(guī)劃方案設(shè)計(jì)

A*算法通過節(jié)點(diǎn)化的地圖，應(yīng)用啟發(fā)式算法得到到達(dá)終點(diǎn)的最少代價(jià)的路徑。在該地圖中，每個(gè)柵格被標(biāo)記為可通過狀態(tài)和不可通過的節(jié)點(diǎn)。通過定位運(yùn)算獲取自身在全局地圖中的位置，并指定全局地圖中的某一點(diǎn)作為導(dǎo)航終點(diǎn)。計(jì)算過程中取水平方向和豎直方向之間相鄰節(jié)點(diǎn)的距離為1，對(duì)角線方向的距離為1.4。定義節(jié)點(diǎn)A為起點(diǎn)，B為終點(diǎn)。定義數(shù)組open_set和close_set分別用于存儲(chǔ)待處理節(jié)點(diǎn)和已處理節(jié)點(diǎn)。定義評(píng)價(jià)函數(shù)：

其中：dist_abs表示不考慮障礙物所占節(jié)點(diǎn)情況下兩點(diǎn)之間的距離；dist_estimate為考慮障礙物時(shí)的距離。Cost函數(shù)值越小表示機(jī)器人移動(dòng)到目標(biāo)點(diǎn)的代價(jià)越小。在計(jì)算Cost函數(shù)時(shí)，僅需考慮從點(diǎn)到點(diǎn)之間的移動(dòng)代價(jià)，因此，選擇曼哈頓距離作為計(jì)算距離有利于提高路徑計(jì)算效率。

基于上述理論，A*算法的實(shí)現(xiàn)流程如圖1所示。

圖1 A＊算法實(shí)現(xiàn)流程圖

3.2 局部路徑規(guī)劃方案設(shè)計(jì)

動(dòng)態(tài)窗口算法是在ROS中常用的局部路徑規(guī)劃方法，其原理為：在速度空間(v,ω)中采樣多組速度，并模擬在這些速度下一定時(shí)間內(nèi)的軌跡；在得到多組軌跡后，對(duì)這些軌跡進(jìn)行評(píng)價(jià)，選取最優(yōu)軌跡對(duì)應(yīng)的速度來驅(qū)動(dòng)機(jī)器人運(yùn)動(dòng)。算法突出動(dòng)態(tài)窗口的特點(diǎn)，依據(jù)移動(dòng)機(jī)器人加減速性能，限定速度采樣空間在一個(gè)以當(dāng)前速度為中心、以諸多限制下能達(dá)到的最大速度為左右區(qū)間長(zhǎng)度的動(dòng)態(tài)窗口范圍內(nèi)。使用全向移動(dòng)小車進(jìn)行基于動(dòng)態(tài)窗口算法的局部路徑規(guī)劃，其流程如圖2所示。

4 實(shí)驗(yàn)與分析

4.1 定位方法驗(yàn)證

用于測(cè)試機(jī)器人實(shí)驗(yàn)平臺(tái)的真實(shí)家庭住宅場(chǎng)景如圖3所示。ACML算法在該場(chǎng)景的實(shí)現(xiàn)效果如圖4所示，紅色箭頭表示粒子對(duì)機(jī)器人位姿的估計(jì)信息?？梢钥闯?，實(shí)驗(yàn)平臺(tái)能夠快速準(zhǔn)確地定位到機(jī)器人的全局位置，圖中粒子聚集在一小塊區(qū)域即為理想定位情況。

4.2 建圖方法性能對(duì)比與分析

本文基于slam_mapping ROSpackage[13]實(shí)現(xiàn)SLAM功能，通過可視化軟件實(shí)時(shí)監(jiān)測(cè)機(jī)器人建圖過程。應(yīng)用Cartographer算法與GMapping算法針對(duì)圖3場(chǎng)景進(jìn)行仿真測(cè)試，結(jié)果分別如圖5、圖6和表1所示。

圖2 動(dòng)態(tài)窗口算法實(shí)現(xiàn)流程圖

圖3 用于測(cè)試實(shí)驗(yàn)平臺(tái)的真實(shí)場(chǎng)景

圖4 ACML算法可視化模型

圖5 GMapping建圖效果

圖6 Cartographer建圖效果

表1 室內(nèi)環(huán)境下建圖算法比較

實(shí)驗(yàn)結(jié)果表明：Cartographer算法計(jì)算量較大，建圖耗費(fèi)時(shí)間較長(zhǎng)，不宜在室內(nèi)安防場(chǎng)景中使用；而GMapping算法適合小范圍且地面平坦環(huán)境，能以較低設(shè)備成本獲得精度較高的地圖，但算法依賴?yán)锍逃?jì)，且缺少回環(huán)檢測(cè)，因此更適用于地面平坦的室內(nèi)環(huán)境[14]；使用GMapping算法進(jìn)行建圖，在沒有回環(huán)檢測(cè)的情況下能夠清晰地檢測(cè)到建圖空間的邊界；而Cartographer算法建圖需要通過一系列子圖對(duì)比回環(huán)檢測(cè)，在實(shí)際操作過程中要對(duì)單一區(qū)域重復(fù)檢測(cè)才能使得某一塊區(qū)域的環(huán)境顯示清晰，因而影響了建圖效率。

4.3 路徑規(guī)劃方法驗(yàn)證

綜合考慮搜索效率和路徑有效性，本文基于navigation_melodic_devel導(dǎo)航框架功能包[15]，采用A*算法實(shí)現(xiàn)機(jī)器人在如圖3場(chǎng)景的全局路徑規(guī)劃與導(dǎo)航，結(jié)果如圖7、圖8所示。圖7中紅色路徑表示全局路徑，綠色路徑表示局部規(guī)劃路徑。圖8表示實(shí)驗(yàn)平臺(tái)能夠根據(jù)上位機(jī)端發(fā)布的導(dǎo)航點(diǎn)，自主規(guī)劃出一條通往該點(diǎn)的路徑，從而驗(yàn)證了本文路徑規(guī)劃方法的有效性。

5 結(jié)論

圖7 路徑規(guī)劃結(jié)果

圖8 到達(dá)目標(biāo)點(diǎn)結(jié)果

為了實(shí)現(xiàn)室內(nèi)安防機(jī)器人的建圖與導(dǎo)航，本文設(shè)計(jì)了一種針對(duì)室內(nèi)場(chǎng)景的通用移動(dòng)安防機(jī)器人平臺(tái)。通過機(jī)器人麥克納姆輪底盤與實(shí)驗(yàn)平臺(tái)、定位、建圖與路徑規(guī)劃方案等的設(shè)計(jì)，采用自適應(yīng)蒙特卡洛定位算法、基于GMapping算法的定位與建圖方法、基于A*算法的全局路徑規(guī)劃方法，較好地實(shí)現(xiàn)了定位、建圖與全局路徑規(guī)劃功能，并在導(dǎo)航過程中通過動(dòng)態(tài)窗口算法實(shí)現(xiàn)避障。實(shí)驗(yàn)結(jié)果驗(yàn)證了所提出方法的有效性，顯示出本文方法在建圖與導(dǎo)航效率方面優(yōu)于對(duì)比算法，具有良好的應(yīng)用前景。