任泉林 臧鐵鋼 崔海程

（南京航天航空大學(xué)機(jī)電學(xué)院 江蘇省南京市 210016）

1 前言

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

2 移動(dòng)機(jī)器人導(dǎo)航系統(tǒng)設(shè)計(jì)

2.1 總體設(shè)計(jì)目標(biāo)及設(shè)計(jì)方案

設(shè)計(jì)目標(biāo)：移動(dòng)機(jī)器人導(dǎo)航系統(tǒng)能在糧倉(cāng)庫(kù)區(qū)工作環(huán)境中快速定位，定位準(zhǔn)確，并對(duì)清潔、巡檢兩種不同工作模式進(jìn)行所需要的有避障的路徑規(guī)劃。

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

2.2 環(huán)境地圖表示

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

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

2.3 移動(dòng)機(jī)器人定位方案

本文設(shè)計(jì)的移動(dòng)機(jī)器人導(dǎo)航系統(tǒng)，采用地圖匹配定位，傳感器為單線激光雷達(dá)和里程計(jì)。

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

自適應(yīng)粒子濾波定位算法的主要步驟如下：

2.2.1 初始化粒子群

通過隨機(jī)采樣的方式初始化粒子群，每個(gè)粒子的權(quán)重相等，即為1/N。

2.2.2 運(yùn)動(dòng)模型預(yù)測(cè)

將機(jī)器人運(yùn)動(dòng)模型應(yīng)用到每個(gè)粒子上，得到t 時(shí)刻各個(gè)粒子的位姿。即根據(jù)上一時(shí)刻樣本集合St-1中機(jī)器人的位姿采樣Xt-1、運(yùn)動(dòng)信息at-1和運(yùn)動(dòng)模型（即狀態(tài)轉(zhuǎn)移分布P(Xt|Xt-1, at-1)）,預(yù)測(cè)t 時(shí)刻移動(dòng)機(jī)器人位姿Xt，這樣就得到了最新的粒子集合St。

2.2.3 觀測(cè)模型更新

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

2.2.4 粒子群重采樣

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

2.2.5 計(jì)算最佳位姿

通過計(jì)算更新粒子的分布期望值，估計(jì)出移動(dòng)機(jī)器人平臺(tái)位姿。

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

圖1：粒子濾波定位過程

2.3 導(dǎo)航系統(tǒng)路徑規(guī)劃模塊設(shè)計(jì)

2.3.1 點(diǎn)對(duì)點(diǎn)路徑規(guī)劃

點(diǎn)對(duì)點(diǎn)路徑規(guī)劃是預(yù)設(shè)起點(diǎn)和終點(diǎn)，根據(jù)已知的靜態(tài)環(huán)境，通過相應(yīng)的路徑規(guī)劃算法，找到一條在時(shí)間性能、安全性等方面一個(gè)參數(shù)或幾個(gè)參數(shù)最優(yōu)的路徑。點(diǎn)對(duì)點(diǎn)路徑規(guī)劃的用途是控制機(jī)器人以最快速度到達(dá)指定目標(biāo)點(diǎn)，其流程為：預(yù)設(shè)終點(diǎn)、進(jìn)行全局規(guī)劃、實(shí)時(shí)環(huán)境感知判斷是否到達(dá)終點(diǎn)及是否有動(dòng)態(tài)障礙物、控制機(jī)器人沿規(guī)劃路徑運(yùn)動(dòng)、到達(dá)終點(diǎn)。

其中的路徑規(guī)劃算法，本文采用A*算法[5]。A-star 算法是一種啟發(fā)式搜索算法，該算法通過成本函數(shù)來找到最優(yōu)路徑，成本函數(shù)的計(jì)算公式為：

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

采用曼哈頓距離作為啟發(fā)式函數(shù)，限制了路徑規(guī)劃過程中只允許沿上下左右四個(gè)方向移動(dòng)。

圖2 為本文點(diǎn)對(duì)點(diǎn)路徑規(guī)劃效果圖，綠線為搜尋到的連接起點(diǎn)和終點(diǎn)規(guī)避障礙物的最優(yōu)路徑。

圖2：點(diǎn)對(duì)點(diǎn)路徑規(guī)劃實(shí)驗(yàn)圖

2.3.2 地圖全覆蓋路徑規(guī)劃

全覆蓋路徑規(guī)劃用于移動(dòng)機(jī)器人糧倉(cāng)庫(kù)區(qū)清潔作業(yè)，清潔作業(yè)要求盡可能多地覆蓋地圖中的空閑區(qū)域，且能繞開障礙物，為了工作的效率和減少開支，地圖的路徑遍歷要盡可能減少的重復(fù)路徑和路徑總長(zhǎng)。

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

圖3：全覆蓋路徑規(guī)劃效果圖

圖3：巡檢路徑規(guī)劃示意圖

2.3.3 巡檢路徑規(guī)劃

該部分要實(shí)現(xiàn)的功能是：錄制移動(dòng)機(jī)器人的巡檢路徑，驅(qū)動(dòng)機(jī)器人從地圖上任意位置到達(dá)巡檢路徑起點(diǎn)然后執(zhí)行對(duì)巡檢路徑的循跡。導(dǎo)航系統(tǒng)定位模塊可以得到移動(dòng)機(jī)器人在地圖坐標(biāo)系下的位姿，包括位置坐標(biāo)(x,y,z)和姿態(tài)(roll,pitch,yaw),由于本文研究的是在2D 下的導(dǎo)航系統(tǒng)，故z=0,roll,pitch=0。所以錄制機(jī)器人的巡檢路徑，只需隔一段時(shí)間間隔內(nèi)記錄移動(dòng)機(jī)器人當(dāng)前x,y,yaw 值即可。

移動(dòng)機(jī)器人循跡路徑規(guī)劃分為兩步，第一，判斷機(jī)器人當(dāng)前位置是否為記錄路徑的起點(diǎn)，若不是，則指引機(jī)器人到達(dá)錄制的循跡路徑的起點(diǎn)。第二步，到達(dá)起點(diǎn)后進(jìn)行循跡，此時(shí)路徑即為記錄的循跡路徑。循跡路徑是提前錄制的，在執(zhí)行循跡時(shí)，路徑上可能有新出現(xiàn)的障礙物，所以在執(zhí)行循跡過程，以5Hz 的頻率判斷錄制路徑上是否有障礙物，若有障礙物，則以安全距離繞開障礙物，繞開障礙物后繼續(xù)執(zhí)行循跡。圖3 展示了本文設(shè)計(jì)的巡檢路徑規(guī)劃算法示意圖，其中綠線為提前錄制的路徑，黑線表示在錄制路徑后人為添加的障礙物，紅線表示機(jī)器人循跡路徑結(jié)果，循跡過程中會(huì)主動(dòng)躲避新出現(xiàn)的障礙物，循跡按照時(shí)間優(yōu)先原則進(jìn)行，直角拐彎變?yōu)閳A弧拐彎。

2.4 總結(jié)與展望

本文針對(duì)機(jī)器人導(dǎo)航部分的定位、路徑規(guī)劃兩大部分進(jìn)行研究設(shè)計(jì)，開發(fā)出了一套適用于簡(jiǎn)單糧倉(cāng)庫(kù)區(qū)環(huán)境下的移動(dòng)機(jī)器人導(dǎo)航系統(tǒng)，效果良好，可適用于糧倉(cāng)巡檢、清潔工作，一機(jī)多用，大大減少糧倉(cāng)的投入。

在本文的設(shè)計(jì)中，地圖模型是2D 柵格地圖，相當(dāng)于實(shí)際環(huán)境中的一定高度的水平切片，并不能完整的表示整個(gè)環(huán)境。因此在復(fù)雜環(huán)境下如環(huán)境中有大量動(dòng)態(tài)建筑物及高度不同的大量障礙物時(shí)，本文設(shè)計(jì)的導(dǎo)航系統(tǒng)效果勉強(qiáng)滿足正常使用需求。因此優(yōu)化復(fù)雜環(huán)境中導(dǎo)航系統(tǒng)的表現(xiàn)是后續(xù)工作的主要研究方向。