糧倉庫區移動機器人導航系統設計

任泉林 臧鐵鋼 崔海程

（南京航天航空大學機電學院 江蘇省南京市 210016）

1 前言

導航系統是移動機器人的系統核心，賦予機器人適應工作環境，按照工作需求自主移動的能力。移動機器人可采用的導航方式包括：基于磁軌道導航、依靠GPS 導航、依靠慣性原件導航、環境地圖匹配導航、路標導航等[1]。環境地圖的匹配定位導航是本文采用的定位導航方式，該方式需要提前建立環境地圖[2]，導航過程中利用傳感器獲取環境信息，結合環境地圖確定自身位置，然后進行全局路徑規劃導航[3]?，F有的機器人導航系統功能較為單一，并不能適應糧庫的工作環境。糧倉庫區移動機器人導航系統應能滿足以下工作場景的導航工作：日常地面清掃、按照指定路線巡檢以及以最快速度到達人為指定的目標點。為滿足上述要求，本文對現有的基于環境地圖匹配導航系統進行改進，并在Linux 下基于ROS 平臺對導航系統進行功能仿真驗證。

2 移動機器人導航系統設計

2.1 總體設計目標及設計方案

設計目標：移動機器人導航系統能在糧倉庫區工作環境中快速定位，定位準確，并對清潔、巡檢兩種不同工作模式進行所需要的有避障的路徑規劃。

設計方案：本文首先對機器人所用環境地圖進行簡單介紹。針對定位部分，設計了一套基于粒子濾波的自適應蒙特卡洛算法的定位系統。針對路徑規劃部分，首先對已知起點和終點的點對點路徑規劃進行研究，應用A-star 算法，完成該部分路徑規劃功能。對于清潔工作中地圖全覆蓋路徑規劃，采用螺旋線路徑對所有空閑區域進行遍歷，并使用A*算法連接不同的空閑區域，幾乎做到空閑區域全覆蓋。對于巡檢工作所需路徑規劃，設計了一套集巡檢路徑錄制、保存及循跡于一體的路徑規劃器。移動機器人可根據工作模式調用不同的路徑規劃算法來完成任務相應任務。

2.2 環境地圖表示

占據柵格地圖常用于地圖構建、定位和路徑規劃，所以本文中環境地圖采用占據柵格地圖。占用柵格地圖將地圖柵格化，每個柵格的狀態要么占用，要么空閑，要么未知（初始化狀態）。對于每個柵格，用 p(s=1) 來表示Free 狀態的概率，用 p(s=0) 來表示柵格占據狀態的概率,引入兩者的比值作為柵格狀態：

Odd(s)越小表示柵格占用概率越大，反之則表示空閑概率越大。

2.3 移動機器人定位方案

本文設計的移動機器人導航系統，采用地圖匹配定位，傳感器為單線激光雷達和里程計。

定位算法采用自適應粒子濾波定位算法，也稱作自適應蒙特卡羅定位（Adaptive Monte Carlo Localization, AMCL）[5]。算法首先定義包含N 個加權位姿粒子群。任意t 時刻移動機器人位姿用表示。而表示N 個位姿樣本集合S。對應的置信度Bel(Xt)可用公式（1）來描述。每個采樣粒子可用二元組表示是其中第i 個粒子，表示第i 個粒子的權重，且總和為1.機器人可能的位姿用隨機粒子的集合所確定的后驗置信度來表示。

自適應粒子濾波定位算法的主要步驟如下：

2.2.1 初始化粒子群

通過隨機采樣的方式初始化粒子群，每個粒子的權重相等，即為1/N。

2.2.2 運動模型預測

將機器人運動模型應用到每個粒子上，得到t 時刻各個粒子的位姿。即根據上一時刻樣本集合St-1中機器人的位姿采樣Xt-1、運動信息at-1和運動模型（即狀態轉移分布P(Xt|Xt-1, at-1)）,預測t 時刻移動機器人位姿Xt，這樣就得到了最新的粒子集合St。

2.2.3 觀測模型更新

此步驟是對上一步驟得到的最新粒子集合St中的每一個粒子進行打分。傳感器觀測到的數據為st，觀測模型為P(st|Xt)利用公式（2）進行權重計算，然后將得到的權重按照公式（3）進行歸一化處理，得到了t 時刻的粒子集合St。用該粒子集合對移動機器人t 時刻位姿近似表達，每個粒子的權重代表著它接近機器人真實位姿的程度，權重越大，表明該粒子的位姿越接近機器人真實位姿。

2.2.4 粒子群重采樣

該步核心是復制評分高的粒子，淘汰評分小的粒子。從上一步的粒子集合中，采用庫爾貝克-萊布勒散度（Kullback- Leibler Divergence, KLD）采樣，此過程粒子集合總數會根據情況進行自適應調整，不再固定為初始化是的數量N，在定位開始的初期，會增大粒子數以防算法發散，而在跟蹤到移動機器人位置后，僅需很小的粒子數便可繼續追蹤機器人移動，防止計算資源浪費。重采樣過后返回步驟2.2.2。

2.2.5 計算最佳位姿

通過計算更新粒子的分布期望值，估計出移動機器人平臺位姿。

圖1 展示了本文設計的導航系統定位算法的效果。左圖表示移動機器人上電后，對粒子群進行初始化，得到了初始粒子群，可以看出粒子群分布隨機、雜亂。右圖表示隨著機器人移動，粒子群不停更新迭代，最后收斂于機器人真實位置附近。

圖1：粒子濾波定位過程

2.3 導航系統路徑規劃模塊設計

2.3.1 點對點路徑規劃

點對點路徑規劃是預設起點和終點，根據已知的靜態環境，通過相應的路徑規劃算法，找到一條在時間性能、安全性等方面一個參數或幾個參數最優的路徑。點對點路徑規劃的用途是控制機器人以最快速度到達指定目標點，其流程為：預設終點、進行全局規劃、實時環境感知判斷是否到達終點及是否有動態障礙物、控制機器人沿規劃路徑運動、到達終點。

其中的路徑規劃算法，本文采用A*算法[5]。A-star 算法是一種啟發式搜索算法，該算法通過成本函數來找到最優路徑，成本函數的計算公式為：

g(n)表示當前節點到起點的代價值，h(n)表示啟發式函數，它指引著A-star 算法的路徑搜索方向，加快搜索過程，在算法中起到至關重要的作用。本文中h(n)采用當前節點到終點的曼哈頓距離(Manhattan Distance)來衡量，假設當前節點的坐標為(x,y),終點的坐標為(xgoal, ygoal)，則啟發函數h(n)的衡量計算公式如下：

采用曼哈頓距離作為啟發式函數，限制了路徑規劃過程中只允許沿上下左右四個方向移動。

圖2 為本文點對點路徑規劃效果圖，綠線為搜尋到的連接起點和終點規避障礙物的最優路徑。

圖2：點對點路徑規劃實驗圖

2.3.2 地圖全覆蓋路徑規劃

全覆蓋路徑規劃用于移動機器人糧倉庫區清潔作業，清潔作業要求盡可能多地覆蓋地圖中的空閑區域，且能繞開障礙物，為了工作的效率和減少開支，地圖的路徑遍歷要盡可能減少的重復路徑和路徑總長。

本文設計了一種基于螺旋線路徑的地圖遍歷算法。螺旋線路徑中機器人移動方向只有上、下、左、右，其核心思想是機器人總是希望左轉，例如機器人向上運動到某個柵格處，那么如果該柵格向左的鄰居柵格沒有障礙物且沒有被訪問，則機器人會向左運動，否則機器人會繼續向上運動直到可以左轉[6]。如圖3所示為本文設計的地圖遍歷算法效果圖，本算法將地圖劃分為幾個子區域，對子區域按照螺旋線進行遍歷，當一塊子區域已經遍歷結束時，移動到最近的空閑子區域進行遍歷，直到全部遍歷完成，圖中的綠線即為本算法規劃的地圖全覆蓋路徑，對地圖空閑部分算法覆蓋率達到95%，可滿足清潔工作要求。

圖3：全覆蓋路徑規劃效果圖

圖3：巡檢路徑規劃示意圖

2.3.3 巡檢路徑規劃

該部分要實現的功能是：錄制移動機器人的巡檢路徑，驅動機器人從地圖上任意位置到達巡檢路徑起點然后執行對巡檢路徑的循跡。導航系統定位模塊可以得到移動機器人在地圖坐標系下的位姿，包括位置坐標(x,y,z)和姿態(roll,pitch,yaw),由于本文研究的是在2D 下的導航系統，故z=0,roll,pitch=0。所以錄制機器人的巡檢路徑，只需隔一段時間間隔內記錄移動機器人當前x,y,yaw 值即可。

移動機器人循跡路徑規劃分為兩步，第一，判斷機器人當前位置是否為記錄路徑的起點，若不是，則指引機器人到達錄制的循跡路徑的起點。第二步，到達起點后進行循跡，此時路徑即為記錄的循跡路徑。循跡路徑是提前錄制的，在執行循跡時，路徑上可能有新出現的障礙物，所以在執行循跡過程，以5Hz 的頻率判斷錄制路徑上是否有障礙物，若有障礙物，則以安全距離繞開障礙物，繞開障礙物后繼續執行循跡。圖3 展示了本文設計的巡檢路徑規劃算法示意圖，其中綠線為提前錄制的路徑，黑線表示在錄制路徑后人為添加的障礙物，紅線表示機器人循跡路徑結果，循跡過程中會主動躲避新出現的障礙物，循跡按照時間優先原則進行，直角拐彎變為圓弧拐彎。

2.4 總結與展望

本文針對機器人導航部分的定位、路徑規劃兩大部分進行研究設計，開發出了一套適用于簡單糧倉庫區環境下的移動機器人導航系統，效果良好，可適用于糧倉巡檢、清潔工作，一機多用，大大減少糧倉的投入。

在本文的設計中，地圖模型是2D 柵格地圖，相當于實際環境中的一定高度的水平切片，并不能完整的表示整個環境。因此在復雜環境下如環境中有大量動態建筑物及高度不同的大量障礙物時，本文設計的導航系統效果勉強滿足正常使用需求。因此優化復雜環境中導航系統的表現是后續工作的主要研究方向。