安防機器人的自主導航方法設計與實現

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

（江蘇理工學院 機械工程學院，江蘇 常州 213001）

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

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

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

1 安防機器人實驗平臺

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

2 安防機器人定位與建圖方法設計

2.1 定位方法設計

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

2.2 建圖方法設計

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

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

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

3 安防機器人路徑規劃方案設計

3.1 全局路徑規劃方案設計

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

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

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

圖1 A＊算法實現流程圖

3.2 局部路徑規劃方案設計

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

4 實驗與分析

4.1 定位方法驗證

用于測試機器人實驗平臺的真實家庭住宅場景如圖3所示。ACML算法在該場景的實現效果如圖4所示，紅色箭頭表示粒子對機器人位姿的估計信息?？梢钥闯觯瑢嶒炂脚_能夠快速準確地定位到機器人的全局位置，圖中粒子聚集在一小塊區域即為理想定位情況。

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

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

圖2 動態窗口算法實現流程圖

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

圖4 ACML算法可視化模型

圖5 GMapping建圖效果

圖6 Cartographer建圖效果

表1 室內環境下建圖算法比較

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

4.3 路徑規劃方法驗證

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

5 結論

圖7 路徑規劃結果

圖8 到達目標點結果

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