張嘉誠，劉文匯，袁吳越，王澤坤，巢 淵

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

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

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

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

1 安防機器人實驗平臺

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

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

2.1 定位方法設(shè)計

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

2.2 建圖方法設(shè)計

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

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

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

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

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

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

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

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

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

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

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

4 實驗與分析

4.1 定位方法驗證

用于測試機器人實驗平臺的真實家庭住宅場景如圖3所示。ACML算法在該場景的實現(xiàn)效果如圖4所示，紅色箭頭表示粒子對機器人位姿的估計信息。可以看出，實驗平臺能夠快速準確地定位到機器人的全局位置，圖中粒子聚集在一小塊區(qū)域即為理想定位情況。

4.2 建圖方法性能對比與分析

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

圖2 動態(tài)窗口算法實現(xiàn)流程圖

圖3 用于測試實驗平臺的真實場景

圖4 ACML算法可視化模型

圖5 GMapping建圖效果

圖6 Cartographer建圖效果

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

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

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

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

5 結(jié)論

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

圖8 到達目標點結(jié)果

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