動(dòng)態(tài)環(huán)境下機(jī)器人路徑的新型啟發(fā)蟻群規(guī)劃

賀道坤，周 南

（1.南京信息職業(yè)技術(shù)學(xué)院智能制造學(xué)院，江蘇南京 210023；2.湖南大學(xué)金融與統(tǒng)計(jì)學(xué)院，湖南長沙 410082;3.長沙商貿(mào)旅游職業(yè)技術(shù)學(xué)院湘商學(xué)院，湖南長沙 410116）

1 引言

智能機(jī)器人是人類文明進(jìn)步的標(biāo)志性產(chǎn)物，機(jī)器人可以代替人類完成服務(wù)型、重復(fù)型、勞動(dòng)力密集型、危險(xiǎn)型等工作，從而實(shí)現(xiàn)節(jié)約人工成本、保護(hù)人類安全、提高工作效率等目的[1]。對于移動(dòng)機(jī)器人，路徑規(guī)劃是機(jī)器人完成其余工作的基礎(chǔ)，路徑規(guī)劃的質(zhì)量決定了完成其余任務(wù)的效率甚至決定了任務(wù)成敗[2]。因此，研究機(jī)器人路徑規(guī)劃問題具有重要的現(xiàn)實(shí)意義。

機(jī)器人路徑規(guī)劃主要關(guān)注可行性和最優(yōu)性兩個(gè)方面，其中可行性是指安全無碰、路徑平滑可跟蹤等方面，最優(yōu)性是指路徑滿足最短、最平滑等指標(biāo)，其中最優(yōu)性是以可行性為基礎(chǔ)的。根據(jù)環(huán)境中障礙物的運(yùn)動(dòng)情況，路徑規(guī)劃分為靜態(tài)規(guī)劃和動(dòng)態(tài)規(guī)劃；根據(jù)對環(huán)境信息的掌握程度，路徑規(guī)劃分為全局規(guī)劃和局部規(guī)劃[3]。

從路徑規(guī)劃方法的角度講，可以分為傳統(tǒng)方法和智能方法，傳統(tǒng)方法包括自由空間法、拓?fù)鋱D法、快速搜索隨機(jī)樹[4]、A*算法、D*[5]算法、人工勢場法等；智能方法包括人工魚群算法、蟻群算法、粒子群算法、遺傳算法等[6]。

文獻(xiàn)[7]針對RRT算法路徑冗余問題，首先提出了Quick?RRT算法縮短了RRT初始路徑，并進(jìn)一步提出了雙樹Quick?RRT算法，縮短了路徑程度并提高了收斂速度。文獻(xiàn)[8]將粒子群算法融入到狼群算法中，提出了改進(jìn)狼群算法并應(yīng)用于路徑規(guī)劃，在一定程度上縮短了路徑長度。

文獻(xiàn)[9]將勢場力構(gòu)造為一種新的啟發(fā)信息引入到蟻群算法中，而后使用三次B樣條曲線對路徑進(jìn)行平滑，與傳統(tǒng)蟻群算法比，改進(jìn)算法規(guī)劃的路徑更短，拐點(diǎn)數(shù)量更少。

上述路徑規(guī)劃算法中，智能規(guī)劃算法因其普遍適用性好、路徑質(zhì)量高等優(yōu)點(diǎn)而成為主流方法。但是，智能規(guī)劃算法得到的路徑質(zhì)量取決于算法優(yōu)化性能，因此智能規(guī)劃算法研究的本質(zhì)是對算法優(yōu)化性能的提升。

這里研究了動(dòng)態(tài)環(huán)境下機(jī)器人的路徑規(guī)劃問題，首先提出基于新型啟發(fā)蟻群算法的全局路徑規(guī)劃方法，機(jī)器人沿最優(yōu)路徑時(shí)進(jìn)行動(dòng)態(tài)障礙物探測，當(dāng)檢測到障礙物時(shí)根據(jù)碰撞類型實(shí)施正面避撞和側(cè)面避撞。上述方案既保證了機(jī)器人沿最優(yōu)路徑行駛，也保證了機(jī)器人行駛安全。

2 路徑規(guī)劃問題描述與建模

2.1 問題描述與環(huán)境模型

機(jī)器人路徑規(guī)劃描述為，規(guī)劃出一條由起點(diǎn)到目標(biāo)點(diǎn)的路徑，該路徑在滿足無碰、可跟蹤等約束條件下，需實(shí)現(xiàn)路徑最短、最平滑等最優(yōu)化指標(biāo)。路徑規(guī)劃的前提是環(huán)境建模，這里建立二維環(huán)境的柵格模型。

首先依據(jù)機(jī)器人高度和工作環(huán)境中的障礙物高度分布，將三維環(huán)境在二維中進(jìn)行投影，從而將工作環(huán)境簡化為二維。

使用方形柵格對二維環(huán)境進(jìn)行分割，得到二維環(huán)境的柵格模型。

將機(jī)器人的最大半徑記為rrob，所有障礙物按原形狀向外擴(kuò)展尺寸rrob，則后續(xù)處理中可以將機(jī)器人視為質(zhì)點(diǎn)。

在柵格模型中，當(dāng)柵格中包含障礙物時(shí)，即使該障礙物未占滿柵格，也將該柵格視為障礙物柵格。

障礙物分布，如圖1（a）所示。其柵格模型，如圖1（b）所示。

圖1 柵格環(huán)境模型Fig.1 Grid Model of Environment

將圖1（b）中的障礙物柵格屬性賦值為1，可自由行駛柵格的屬性賦值為0，則柵格模型可以使用0?1矩陣表示為：

之所以將柵格環(huán)境模型轉(zhuǎn)化為0?1矩陣形式，是因?yàn)閳D1（b）所示形式機(jī)器人無法辨識(shí)和識(shí)別，而機(jī)器人對矩陣形式是可以識(shí)別的。

2.2 路徑評價(jià)指標(biāo)

這里進(jìn)行路徑規(guī)劃具有兩個(gè)優(yōu)化目標(biāo)，第一個(gè)目標(biāo)為規(guī)劃路徑最短，描述為：

式中：f1—路徑長度；S—起點(diǎn)；G—目標(biāo)點(diǎn)；dij—柵格ij間距離。

第二個(gè)目標(biāo)為規(guī)劃路徑平滑性最好，使用拐點(diǎn)數(shù)量路徑平滑性進(jìn)行評價(jià)。

3 全局路徑規(guī)劃

本節(jié)首先對傳統(tǒng)蟻群算法進(jìn)行簡介，并針對蟻群算法的性能缺陷進(jìn)行改進(jìn)，給出了初始信息素梯度分布策略，并引入了新型啟發(fā)信息，提出了新型啟發(fā)蟻群算法。

3.1 傳統(tǒng)蟻群算法與問題分析

在柵格環(huán)境中，機(jī)器人可選柵格分為四叉樹和八叉樹兩種情況，四叉樹是指可選柵格為前后左右四個(gè)柵格，八叉樹指可選柵格為前、后、左、右、左前、左后、右前、右后等八個(gè)柵格，這里選擇八叉樹模式。

將螞蟻m迭代t次時(shí)所在柵格記為i，則其對鄰域內(nèi)自由柵格j的選擇概率(t)為［10］：

式中：τij(t)—柵格ij間的信息素濃度；α—信息素因子；ηij(t)—柵格ij間的啟發(fā)信息；β—啟發(fā)因子；allowedi—柵格i鄰域可選柵格集合。

蟻群算法每完成一次迭代，則進(jìn)行路徑上的信息素濃度更新，為：

式中：ρ—揮發(fā)系數(shù)；△τij(t)—路徑ij上遺留的信息素濃度；△(t)?螞蟻k在路徑ij上遺留的信息素濃度；M—螞蟻數(shù)量；Lk—螞蟻k的路徑長度。

傳統(tǒng)蟻群算法應(yīng)用于柵格環(huán)境下的路徑規(guī)劃存在以下問題：

（1）初始信息素為均勻分布，使得蟻群算法在算法初期類似于隨機(jī)搜索，算法搜索效率低、收斂速度慢；

針對上述問題，這里通過信息素的梯度分布初始化方法，解決了算法初期搜索效率低的問題；通過引入新的啟發(fā)信息，解決了路徑多目標(biāo)優(yōu)化問題。

3.2 信息素梯度分布初始化

根據(jù)數(shù)學(xué)知識(shí)，兩點(diǎn)之間線段最短，因此起始柵格S與目標(biāo)柵格G連線周圍的柵格是重點(diǎn)搜索區(qū)域，其余柵格為非重點(diǎn)搜索區(qū)域，命名為一般區(qū)域，如圖2所示。設(shè)置一個(gè)距離閾值dm，當(dāng)柵格i與線段SG距離h≤dm，則柵格i為重點(diǎn)搜索區(qū)域；當(dāng)柵格i與線段SG距離h>dm，則柵格i為一般區(qū)域。

圖2 重點(diǎn)搜索區(qū)域Fig.2 Key Searching Area

一般區(qū)域不是螞蟻的搜索重點(diǎn)，其信息素使用傳統(tǒng)的均勻初始化方法，直接將柵格信息素濃度初始化為τ0。重點(diǎn)區(qū)域是螞蟻的搜索重點(diǎn)，使用梯度初始化方法，其示意圖，如圖3所示。

圖3 梯度初始化方法Fig.3 Gradient Initialization Method

重點(diǎn)搜索區(qū)域和一般區(qū)域的信息素濃度初始化方法為：

式中：τj—柵格j的初始信息素濃度；λ—信息素增量系數(shù)；dSj—起點(diǎn)柵格S與柵格j的距離。

分析式（4）可知，在重點(diǎn)搜索區(qū)域內(nèi)，柵格j與目標(biāo)柵格、起點(diǎn)柵格的距離和越小，則柵格j的信息素濃度越大。因此在重點(diǎn)搜索區(qū)域內(nèi)，信息素濃度以線段SG向外呈現(xiàn)梯度遞減分布。

3.3 平滑度啟發(fā)信息與算法流程

對路徑的平滑度進(jìn)行優(yōu)化，必須構(gòu)造平滑度啟發(fā)信息。路徑平滑度取決于轉(zhuǎn)角大小，機(jī)器人前進(jìn)方向轉(zhuǎn)角越大說明路徑平滑性越差，轉(zhuǎn)角越小說明平滑性越好。

在柵格環(huán)境下，機(jī)器人路徑轉(zhuǎn)角存在0°、45°、90°、135°這4種情況，如圖4所示。

圖4中A方向機(jī)器人轉(zhuǎn)角為0°，B方向機(jī)器人轉(zhuǎn)角為45°，C方向機(jī)器人轉(zhuǎn)角為90°，D方向機(jī)器人轉(zhuǎn)角為135°。依據(jù)前進(jìn)方向的機(jī)器人轉(zhuǎn)角構(gòu)造新的啟發(fā)信息為：

圖4 前進(jìn)方向Fig.4 Forward Direction

式中：μij(t)—轉(zhuǎn)角啟發(fā)信息；θij—柵格i與柵格j間夾角。

將新的啟發(fā)信息引入到螞蟻對柵格的選擇概率中，為：

式中：γ—路徑平滑性因子。

在此需要說明的是，當(dāng)遇到U性障礙物時(shí)，調(diào)用文獻(xiàn)[11]中的陷阱深度標(biāo)記策略。結(jié)合信息素的梯度初始化方法和新啟發(fā)信息原理，制定新型啟發(fā)蟻群算法流程如下。

（1）初始化算法參數(shù)，包括螞蟻數(shù)量M、信息素因子α、啟發(fā)因子β、距離閾值dm；

（2）將所有螞蟻放置在起始柵格S，并按照式（4）進(jìn)行信息素濃度初始化；

（3）螞蟻按照式（6）計(jì)算的概率選擇下一柵格；

（4）當(dāng)所有螞蟻到達(dá)目標(biāo)點(diǎn)，算法完成一次迭代，此時(shí)迭代次數(shù)＋1；

（5）判斷迭代次數(shù)是否達(dá)到最大值，若否則轉(zhuǎn)至（3）；若是則輸出最優(yōu)路徑，算法結(jié)束。

4 動(dòng)態(tài)避障策略整體規(guī)劃方案

4.1 動(dòng)態(tài)環(huán)境下避障策略

本節(jié)中的動(dòng)態(tài)障礙物場景設(shè)置為運(yùn)動(dòng)方向固定、運(yùn)動(dòng)節(jié)奏與機(jī)器人一致的障礙物，所謂運(yùn)動(dòng)節(jié)奏一致是指障礙物速度與機(jī)器人速度一致。在這種場景設(shè)置下，機(jī)器人與障礙物的碰撞包括正面碰撞和側(cè)面碰撞兩種情況。

（1）正面碰撞與避撞策略。正面碰撞是指機(jī)器人與障礙物相向而行至同一柵格。

為了避免正面碰撞，當(dāng)機(jī)器人探索到正前方障礙物時(shí)，調(diào)用正面避撞策略。正面避撞策略為：以全局最優(yōu)路徑上某一柵格為目標(biāo)點(diǎn)，調(diào)用新型啟發(fā)蟻群算法規(guī)劃出一條由當(dāng)前柵格到避撞柵格的局部路徑。

（2）側(cè)面碰撞與避撞策略。側(cè)面碰撞是指機(jī)器人由45°、90°、135°等方向與障礙物發(fā)生碰撞。當(dāng)機(jī)器人探測到（45～135）°方向內(nèi)的障礙物時(shí)，調(diào)用側(cè)面避撞策略。側(cè)面避撞策略為：機(jī)器人在原柵格等待一個(gè)周期，待障礙物駛出碰撞柵格后，機(jī)器人繼續(xù)沿最優(yōu)路徑行駛。

4.2 機(jī)器人行駛與避撞方案

綜合新型啟發(fā)蟻群算法的全局路徑規(guī)劃方法與動(dòng)態(tài)避撞策略，機(jī)器人在動(dòng)態(tài)環(huán)境中的行駛方案為：首先使用新型啟發(fā)蟻群算法規(guī)劃出一條全局最優(yōu)路徑，而后機(jī)器人沿最優(yōu)路徑行駛；當(dāng)機(jī)器人出現(xiàn)正面碰撞問題時(shí)則調(diào)用正面避撞策略，當(dāng)機(jī)器人出現(xiàn)側(cè)面碰撞問題時(shí)則調(diào)用側(cè)面避撞策略，如此循環(huán)往往復(fù)直至到達(dá)目標(biāo)點(diǎn)。整體方案，如圖5所示。

圖5 機(jī)器人行駛與避撞方案Fig.5 Robot Travel and Anti?Collision Project

5 仿真驗(yàn)證與分析

5.1 全局規(guī)劃路徑驗(yàn)證

首先在20×20規(guī)模柵格環(huán)境中進(jìn)行路徑規(guī)劃，起點(diǎn)柵格S坐標(biāo)為（0.5，19.5），目標(biāo)柵格為（19.5，0.5）。為了進(jìn)行比較，分別使用傳統(tǒng)蟻群算法、文獻(xiàn)[11]多種群蟻群算法、這里新型啟發(fā)蟻群算法進(jìn)行路徑規(guī)劃，為了驗(yàn)證算法性能穩(wěn)定性，每個(gè)算法獨(dú)立運(yùn)行10次。三種算法得到的最優(yōu)路徑，如圖6所示。

圖6 各算法規(guī)劃的路徑Fig.6 Path Planned by Each Algorithm

觀察圖6中3種蟻群算法規(guī)劃的路徑可以直觀看出，這里新型啟發(fā)蟻群算法規(guī)劃的路徑最短，路徑平滑性也最好；文獻(xiàn)[11]的路徑長度和拐點(diǎn)數(shù)量次之，傳統(tǒng)蟻群算法規(guī)劃路徑最長，且觀點(diǎn)數(shù)量最多。為了進(jìn)一步比較算法性能，統(tǒng)計(jì)路徑長度、拐點(diǎn)數(shù)量、收斂次數(shù)等參數(shù)的最優(yōu)值及均值，結(jié)果，如表1所示。

表1 各算法規(guī)劃路徑參數(shù)Tab.1 Path Parameters of Different Algorithm

結(jié)合表1和圖6可以看出：（1）從最優(yōu)路徑的角度看，傳統(tǒng)算法最優(yōu)路徑長度為31.21，拐點(diǎn)數(shù)量為14，收斂迭代次數(shù)為70，在3 種算法中長度最大、拐點(diǎn)數(shù)量最多、迭代次數(shù)最多；其次為文獻(xiàn)[11]規(guī)劃路徑；這里算法的路徑長度最小、拐點(diǎn)數(shù)量最少，迭代次數(shù)最少；（2）從平均水平看，這里算法規(guī)劃的路徑長度均值最小，拐點(diǎn)均值最少，迭代次數(shù)也最少。

這是因?yàn)檫@里新型蟻群算法信息素初始化對重點(diǎn)搜索區(qū)域使用了梯度初始化方法，有效提高了算法初期的搜索效率，所以收斂次數(shù)遠(yuǎn)少于另外兩種算法。另外，這里算法中引入了路徑平滑度這種新型啟發(fā)信息，引導(dǎo)螞蟻盡量沿直線行走，因此路徑拐點(diǎn)數(shù)量明顯減少，同時(shí)減小了路徑長度。因此，這里新型啟發(fā)蟻群算法在全局路徑規(guī)劃中具有優(yōu)越性。

5.2 動(dòng)態(tài)避撞方案驗(yàn)證

在圖6所示的20×20規(guī)模柵格環(huán)境中設(shè)置2個(gè)動(dòng)態(tài)障礙物，障礙物1由柵格H（2.5，16.5）出發(fā)，在柵格H與柵格P（8.5，16.5）之間做直線往返運(yùn)動(dòng)，障礙物1從第3個(gè)周期開始運(yùn)動(dòng)。

障礙物2由柵格Q（17.5，3.5）出發(fā)，沿X軸135°方向做直線運(yùn)動(dòng)，從第10個(gè)周期開始運(yùn)動(dòng)。

機(jī)器人的碰撞檢測位置與避撞結(jié)果，如圖7所示。

在圖7（a）中，機(jī)器人運(yùn)動(dòng)4步后，探測到動(dòng)態(tài)障礙物M1，根據(jù)M1所在柵格及運(yùn)動(dòng)方向，判斷兩者發(fā)生側(cè)面碰撞，且碰撞柵格為P1位置，此時(shí)機(jī)器人調(diào)動(dòng)側(cè)面避撞策略，如圖7（b）所示，機(jī)器人在圖中所示位置等待1個(gè)周期后繼續(xù)前進(jìn)。

圖7 避撞策略驗(yàn)證Fig.7 Clarification of Anti?Collision Strategy

在圖7（c）中，機(jī)器人運(yùn)動(dòng)至所示位置時(shí)，探測到動(dòng)態(tài)障礙物M2，根據(jù)M2所在柵格及運(yùn)動(dòng)方向，判斷兩者會(huì)發(fā)生正面碰撞，且碰撞柵格為P2位置，此時(shí)機(jī)器人調(diào)用正面避撞策略。

以圖7（d）中R柵格為臨時(shí)目標(biāo)點(diǎn)進(jìn)行局部路徑規(guī)劃，局部避撞路徑如圖7（d）所示，機(jī)器人按照局部規(guī)劃路徑行駛實(shí)現(xiàn)避撞。

由以上避撞過程和分析可知，這里制定的正面避撞和側(cè)面避撞策略在設(shè)定動(dòng)態(tài)場景中可以實(shí)現(xiàn)有效避撞，保證了機(jī)器人的行駛安全。

6 結(jié)論

這里研究了機(jī)器人在動(dòng)態(tài)環(huán)境下的路徑規(guī)劃問題，首先使用新型啟發(fā)蟻群算法進(jìn)行全局路徑規(guī)劃，而后制定了機(jī)器人行駛的避撞策略。

經(jīng)驗(yàn)證得出以下結(jié)論：

（1）新型啟發(fā)蟻群算法規(guī)劃的路徑長度短、收斂速度快、平滑性好；

（2）這里制定的正面避撞策略和側(cè)面避撞策略在設(shè)定動(dòng)態(tài)場景下可以實(shí)現(xiàn)有效避撞，保證機(jī)器人安全行駛。