彭 湘，向鳳紅，毛劍琳

(昆明理工大學(xué) 信息工程與自動(dòng)化學(xué)院，昆明 650500)

1 引 言

近年來(lái)，隨著人工智能領(lǐng)域的逐步擴(kuò)張[1]，智能機(jī)器人在各個(gè)行業(yè)都得到了較為廣泛的應(yīng)用.移動(dòng)機(jī)器人的路徑規(guī)劃技術(shù)是機(jī)器人學(xué)[2]中一個(gè)重要分支[3]，是指機(jī)器人在存在障礙物的解空間內(nèi)運(yùn)動(dòng)，搜尋到抵達(dá)目標(biāo)點(diǎn)的最優(yōu)解[4]或次優(yōu)解，該最優(yōu)解或次優(yōu)解能夠根據(jù)人類(lèi)期望的機(jī)器人性能決定，如路徑最優(yōu)、能耗最優(yōu)[5]以及路徑最平滑等，也可以是一個(gè)或多個(gè)約束條件的加權(quán).

移動(dòng)機(jī)器人的路徑規(guī)劃方法根據(jù)應(yīng)用環(huán)境可區(qū)分為全局和局部環(huán)境下的路徑規(guī)劃，針對(duì)不同的環(huán)境下的路徑規(guī)劃采用的方法也有所差別.全局路徑規(guī)劃[6]方法主要有蟻群算法、A*算法[7]、遺傳算法等，局部路徑規(guī)劃方法[8]主要有人工勢(shì)場(chǎng)法、DWA算法等.在實(shí)際應(yīng)用中，單一算法通常僅能處理某一限定環(huán)境下的路徑規(guī)劃問(wèn)題，例如蟻群算法，它是一種通過(guò)模擬蟻群覓食規(guī)則的群體智能優(yōu)化算法[9]，在靜態(tài)路徑規(guī)劃問(wèn)題中能夠表現(xiàn)出良好的魯棒性、正反饋性和并行性，但蟻群算法在應(yīng)用到局部環(huán)境時(shí)，會(huì)出現(xiàn)搜索停滯或與動(dòng)態(tài)障礙物碰撞等問(wèn)題.同理，將人工勢(shì)場(chǎng)法等局部路徑規(guī)劃算法用于全局環(huán)境時(shí)，也易出現(xiàn)收斂速度慢、存在局部最優(yōu)解等弊端.單一算法在處理路徑規(guī)劃問(wèn)題時(shí)，由于算法本身的缺陷常常導(dǎo)致規(guī)劃的路徑并非最優(yōu)解，近年來(lái)學(xué)者們?cè)谘芯柯窂揭?guī)劃問(wèn)題時(shí)常將不同算法融合[10]，通過(guò)算法之間優(yōu)劣互補(bǔ)，從而得到較優(yōu)的規(guī)劃結(jié)果.文獻(xiàn)[11]提出將A*算法與蟻群算法融合，利用A*算法的評(píng)價(jià)函數(shù)改進(jìn)啟發(fā)信息函數(shù)，提高了算法的運(yùn)行效率和路徑的平滑度.但算法僅在全局環(huán)境下進(jìn)行仿真實(shí)驗(yàn)，未考慮局部環(huán)境下躲避動(dòng)態(tài)障礙物的問(wèn)題，因此實(shí)際適用范圍較窄.文獻(xiàn)[12]將人工勢(shì)場(chǎng)法的力信息轉(zhuǎn)換為蟻群信息素，改善了算法收斂速度慢和易陷入局部最優(yōu)解等缺點(diǎn)，但規(guī)劃的路徑仍存在較多拐點(diǎn)，路徑平滑程度較低.

針對(duì)以上問(wèn)題，為了兼顧全局與局部環(huán)境下的移動(dòng)機(jī)器人路徑規(guī)劃，鑒于實(shí)際應(yīng)用中機(jī)器人的行走環(huán)境往往是未知的，本文提出一種針對(duì)可能同時(shí)存在任意環(huán)境，即移動(dòng)機(jī)器人在未知環(huán)境下的路徑規(guī)劃方法.通過(guò)融合蟻群算法與人工勢(shì)場(chǎng)法，將未知環(huán)境分為全局和局部環(huán)境進(jìn)行分層規(guī)劃，即首先采用蟻群算法進(jìn)行全局路徑規(guī)劃.在移動(dòng)機(jī)器人前進(jìn)的過(guò)程中，若機(jī)器人自身攜帶的傳感器檢測(cè)到將要與動(dòng)態(tài)障礙物碰撞，此時(shí)調(diào)用局部避障方法-人工勢(shì)場(chǎng)法進(jìn)行局部避障.避障結(jié)束后返回全局規(guī)劃，最后，對(duì)輸出路徑進(jìn)行平滑優(yōu)化.規(guī)劃步驟分成環(huán)境建模、搜索路徑、優(yōu)化處理3部分進(jìn)行.

2 環(huán)境建模

柵格法[13]環(huán)境建模.假設(shè)移動(dòng)機(jī)器人所處環(huán)境為柵格規(guī)模大小M2(M=1，2，…，n)的二維有限空間C，即C?M2.此工作空間被劃分為自由空間Cfree和障礙空間Cobs.Cfree∪Cobs=C.為了區(qū)分不同種類(lèi)柵格，用不同的值在柵格矩陣中抽象表示.Cfree中柵格的值表示0，Cobs中包含有靜態(tài)障礙物與可移動(dòng)障礙物，用1表示靜態(tài)障礙物，2表示可移動(dòng)障礙物.柵格法是一種經(jīng)典的建模方法，由于柵格法只能近似地表示實(shí)際環(huán)境，因此實(shí)際應(yīng)用中需對(duì)障礙物進(jìn)行膨脹處理，即：對(duì)實(shí)際大小小于單位柵格的障礙物按單位柵格大小處理.令柵格圖的左下角為坐標(biāo)原點(diǎn)，創(chuàng)建笛卡爾坐標(biāo)系，橫軸為X，縱軸為Y，對(duì)M2個(gè)柵格從上至下、由左往右編號(hào)，依次為1，2，…，M2，當(dāng)移動(dòng)機(jī)器人處于某一柵格Pi時(shí)，若以Pi為中心柵格的八鄰域柵格內(nèi)存在自由柵格，移動(dòng)機(jī)器人便能夠?qū)⑦@些自由柵格加入候選可移動(dòng)?xùn)鸥裰?令任意一個(gè)柵格坐標(biāo)為(xi，yi)，移動(dòng)機(jī)器人的行走半徑Rrobot=1，(xi，yi)與第i個(gè)柵格間的關(guān)系可用公式(1)表示：

(1)

進(jìn)行分層路徑規(guī)劃前，對(duì)工作柵格空間C作如下假設(shè)：1)全局環(huán)境中障礙物為靜態(tài)障礙物，障礙柵格在整個(gè)規(guī)劃過(guò)程中保持靜止，移動(dòng)機(jī)器人在進(jìn)行全局規(guī)劃前已知全局環(huán)境障礙物分布情況；2)局部環(huán)境中含有動(dòng)態(tài)障礙物，障礙物柵格狀態(tài)發(fā)生不確定性變化，障礙物運(yùn)動(dòng)軌跡未知；3)移動(dòng)機(jī)器人自身攜帶有傳感器，可以實(shí)時(shí)掃描一定范圍內(nèi)環(huán)境變壞情況.

3 基于蟻群算法的全局路徑規(guī)劃

移動(dòng)機(jī)器人的路徑規(guī)劃過(guò)程是給定機(jī)器人某一任務(wù)，機(jī)器人依靠算法的決策和自身攜帶的傳感器感知環(huán)境信息[14]，在給定解空間內(nèi)完成該任務(wù).蟻群算法(ACO)最早由Marco Dorigo等人1991年提出[15]，是一種經(jīng)典的求解最優(yōu)問(wèn)題的算法，大自然中螞蟻的覓食機(jī)制與機(jī)器人路徑搜索具有較高的相似度，使得蟻群算法在路徑規(guī)劃領(lǐng)域有較好的運(yùn)用.蟻群算法規(guī)則簡(jiǎn)單，在給定空間，螞蟻個(gè)體夾帶信息素，由單個(gè)螞蟻個(gè)體組成蟻群，通過(guò)群間相互合作、競(jìng)爭(zhēng)與信息素交換，經(jīng)過(guò)揮發(fā)系數(shù)和信息素等的反饋?zhàn)饔茫瑥亩鴮ふ业绞澄镌?但蟻群算法也存在容易陷入“死鎖”，前期搜尋路徑存在“盲目性”，收斂時(shí)間長(zhǎng)、易出現(xiàn)局部最優(yōu)解等弊端[16].

3.1 屏蔽U型陷阱措施

在全局路徑規(guī)劃開(kāi)始前，移動(dòng)機(jī)器人需要識(shí)別Cobs內(nèi)的U型陷阱.U型陷阱是指柵格地圖中以n個(gè)自由柵格為中心，若自由柵格左右兩側(cè)柵格均為障礙柵格，并且在兩側(cè)柵格的任意一端有障礙柵格將兩側(cè)柵格相連接，從而將此區(qū)域柵格組成一個(gè)類(lèi)似字母U的半封閉空間.本文采用分層路徑規(guī)劃方法，由于局部環(huán)境中動(dòng)態(tài)障礙物運(yùn)動(dòng)狀態(tài)未知，因此在局部環(huán)境中無(wú)法進(jìn)行U型環(huán)境識(shí)別，移動(dòng)機(jī)器人在進(jìn)行路徑規(guī)劃前僅需對(duì)全局環(huán)境下的障礙柵格進(jìn)行U型陷阱識(shí)別.

螞蟻在解空間C內(nèi)進(jìn)行搜索，環(huán)境中存在U型陷阱.螞蟻搜索時(shí)進(jìn)入U(xiǎn)型陷阱，若能夠逃脫，螞蟻需要退出U型陷阱這一過(guò)程，才能繼續(xù)進(jìn)行搜索.若無(wú)法逃脫，便會(huì)導(dǎo)致螞蟻困在U型環(huán)境內(nèi)，陷入“死鎖”，從而停止搜索.考慮到U型陷阱必然導(dǎo)致螞蟻搜索時(shí)間變長(zhǎng)、搜索不到最優(yōu)解或直接停止搜索，為此提出一種屏蔽U型陷阱措施.螞蟻在搜索時(shí)將U型陷阱視為障礙物，從而避開(kāi)U型陷阱搜索.識(shí)別U型陷阱的方法如下：

(2)

其中：i為任意序號(hào)的柵格，x表示大于0的整數(shù)；M為柵格地圖橫縱坐標(biāo)最大值，n為整數(shù)，n=1…nmax，nmax=[1，M-1]；1為柵格矩陣中障礙柵格的值.當(dāng)柵格之間的關(guān)系滿足公式(2)所示條件時(shí)，即為U型環(huán)境.

3.2 全局規(guī)劃過(guò)程

3.2.1 ACO路徑規(guī)劃

傳統(tǒng)蟻群算法(ACO)進(jìn)行全局路徑規(guī)劃步驟如下：

1)初始化：柵格法環(huán)境建模，設(shè)置起始點(diǎn)S、目標(biāo)點(diǎn)G，將S加入禁忌表Tabu中；初始化蟻群算法參數(shù)，包括螞蟻個(gè)數(shù)m，啟發(fā)因子α，信息素因子β，信息素增強(qiáng)系數(shù)Q，局部信息素?fù)]發(fā)系數(shù)ε和全局信息素?fù)]發(fā)系數(shù)ρ，螞蟻?zhàn)畲蟮螖?shù)Ncmax等；

(3)

其中：ηij為啟發(fā)信息，ηij=1/dij；τij為路徑i→j上螞蟻留下的信息素濃度；α和β分別為信息素因子和啟發(fā)因子；allowedk為八鄰域內(nèi)候選可移動(dòng)?xùn)鸥窦?

3)更新Tabu；

4)重復(fù)步驟1)至步驟3)直到螞蟻抵達(dá)目標(biāo)點(diǎn)；

5)信息素更新：

a)局部更新：螞蟻每更新一次路徑，便對(duì)路徑上的局部信息素進(jìn)行更新，更新方法如公式(4)所示：

τij(t+1)=(1-ε)τij(t)+ετ0

(4)

其中：ε為局部信息素?fù)]發(fā)系數(shù)，ε∈(0，1]；τ0為初始信息素值，用單位矩陣表示.

b)全局更新：在m只螞蟻結(jié)束一次迭代以后，僅對(duì)此次迭代搜尋到最優(yōu)路徑上的信息素進(jìn)行全局更新：

τij(t+1)=(1-ρ)τij+ρΔτij(t，t+1)

(5)

(6)

其中：ρ為全局信息素?fù)]發(fā)系數(shù)，ρ∈(0，1]；Δτij(t，t+1)為1個(gè)時(shí)間間隔Δt內(nèi)最優(yōu)路徑中各節(jié)點(diǎn)的信息素增量；Lbest為本次迭代搜索到的最優(yōu)路徑長(zhǎng)度.此步驟通過(guò)加強(qiáng)Lbest上各節(jié)點(diǎn)的信息素濃度，從而產(chǎn)生正反饋.

6)若Nc=Ncmax，全局規(guī)劃結(jié)束；否則清空禁忌表，Nc=Nc+1，返回步驟(2)，直到Nc=Ncmax.

3.2.2 IACO

針對(duì)ACO在路徑規(guī)劃問(wèn)題中存在的全局性較差、收斂時(shí)間長(zhǎng)和容易陷入局部最優(yōu)解等問(wèn)題，對(duì)ACO作以下改進(jìn)：

1)改進(jìn)啟發(fā)函數(shù)：由于ACO啟發(fā)函數(shù)的確定僅考慮了節(jié)點(diǎn)i→j之間的歐式距離dij，蟻群缺少目標(biāo)點(diǎn)的向?qū)ё饔?，因此前期搜索較為盲目，導(dǎo)致收斂速度緩慢，極易陷入局部最優(yōu).故本文從算法的全局性出發(fā)，設(shè)計(jì)新的啟發(fā)函數(shù)：

(7)

其中：A?[0，+∞).

改進(jìn)啟發(fā)函數(shù)加入了候選節(jié)點(diǎn)j到G的歐式距離djG，使螞蟻在選擇下一節(jié)點(diǎn)時(shí)受目標(biāo)點(diǎn)的影響，目標(biāo)點(diǎn)在整個(gè)搜索過(guò)程中對(duì)螞蟻起持續(xù)的向?qū)ё饔?能夠加快算法的尋優(yōu)功效，同時(shí)還加強(qiáng)了算法的全局性.

2)信息素闕值限制：借鑒最大最小螞蟻系統(tǒng)[17](MMAS)思想，限制信息素的大小范圍，預(yù)防收斂過(guò)程中因信息素過(guò)少而導(dǎo)致收斂速度慢或直接停止搜索，或因信息素積累過(guò)多而出現(xiàn)收斂過(guò)快，導(dǎo)致陷入局部最優(yōu)解的情況.

(8)

其中：τmax和τmin為設(shè)置的信息素闕值，若信息素超過(guò)設(shè)定值，將會(huì)按照公式(8)重置.

4 局部路徑規(guī)劃

4.1 人工勢(shì)場(chǎng)算法描述

人工勢(shì)場(chǎng)法(APF)[18-19]的思想是，假設(shè)空間C內(nèi)存在虛擬勢(shì)場(chǎng)U，U中目標(biāo)點(diǎn)對(duì)移動(dòng)機(jī)器人產(chǎn)生一引力勢(shì)場(chǎng)Uatt，障礙物對(duì)機(jī)器人產(chǎn)生一斥力勢(shì)場(chǎng)Urep，且U=Uatt+Urep.移動(dòng)機(jī)器人在Uatt和Urep的共同作用下，從而規(guī)劃出一條無(wú)碰路徑.

其中：Uatt表示為：

(9)

其中：Katt表示引力勢(shì)場(chǎng)位置增益系數(shù)；d2(P，G)表示當(dāng)前位置P到G的歐氏距離.

障礙物的斥力與機(jī)器人和障礙物間的距離有關(guān)，距離越短，斥力越大.環(huán)境中通常存在多個(gè)障礙物，假設(shè)環(huán)境中有n個(gè)障礙，第i個(gè)障礙產(chǎn)生的斥力勢(shì)場(chǎng)Urep表示為：

(10)

其中：Krep為斥力勢(shì)場(chǎng)位置增益系數(shù)，Krep?C；d(P，Oi)表示P到第i個(gè)障礙的歐氏距離；d0表示障礙物斥力場(chǎng)的影響距離，d0?C.對(duì)Uatt和Urep求導(dǎo)，可計(jì)算機(jī)器人在P處的引力和斥力值分別為：

引力：

Fatt(P)=-?Uatt(P)=Krepd(P，G)

(11)

斥力為：

(12)

移動(dòng)機(jī)器人在位置P受到的合力F大小為：

(13)

局部規(guī)劃過(guò)程中，合力F=0處，移動(dòng)機(jī)器人所受斥力與引力大小相等，可能會(huì)因此陷入局部極小值而停止搜索.此時(shí)，由于全局路徑規(guī)劃過(guò)程已經(jīng)結(jié)束，因此當(dāng)機(jī)器人遭遇此種狀況時(shí)，可以沿全局規(guī)劃完成的路徑行走，此時(shí)便可跳出局部極小值點(diǎn)，繼續(xù)搜索.

4.2 局部路徑規(guī)劃過(guò)程

依靠移動(dòng)機(jī)器人所攜帶傳感器檢測(cè)到的環(huán)境變化情況，本文借鑒預(yù)測(cè)控制滾動(dòng)原理[20].令機(jī)器人從柵格i到下一個(gè)柵格j的運(yùn)動(dòng)時(shí)間為Δt，通過(guò)規(guī)劃過(guò)程中不斷更新地圖環(huán)境信息，通過(guò)不斷變化的環(huán)境預(yù)測(cè)障礙物的運(yùn)動(dòng)軌跡，并判斷在下個(gè)窗口內(nèi)是否會(huì)與動(dòng)態(tài)障礙物相撞.局部路徑規(guī)劃流程如圖1所示.

圖1 局部規(guī)劃過(guò)程

5 拐角優(yōu)化

由于路徑是在柵格地圖中生成，因此生成的路徑必然會(huì)出現(xiàn)拐角，為了避免機(jī)器人不必要的能量損失，使用中點(diǎn)替代策略，對(duì)生成的路徑做尖峰處理，處理后得到一條平滑路徑.平滑步驟如下：

Step 1.令路徑中任意兩相鄰線段交點(diǎn)坐標(biāo)為(xold，yold)，夾角為θ；

Step 2.設(shè)定角度闕值φ，判斷θ是否大于φ，若否則取兩線段的中點(diǎn)為(xnew1，ynew1)、(xnew2，ynew2)、連接兩點(diǎn)成為新線段，若是，返回Step1，繼續(xù)比較下一節(jié)點(diǎn)；

Step 3.判斷新線段與鄰邊的夾角θ1、θ2是否大于角度闕值φ，若是，則(xnew1，ynew1)、(xnew2，ynew2)代替(xold，yold)成為新的節(jié)點(diǎn)，若否，繼續(xù)Step 2；

Step 4.判斷整條路徑是否比較完畢，若是，優(yōu)化結(jié)束，否則，轉(zhuǎn)到Step 1.

6 結(jié)果分析

6.1 分層路徑規(guī)劃流程

移動(dòng)機(jī)器人進(jìn)行分層路徑規(guī)劃，規(guī)劃步驟如下：

Step 1.初始化參數(shù)；

Step 2.識(shí)別全局環(huán)境及全局環(huán)境中的U型陷阱；

Step 3.采用IACO進(jìn)行全局路徑規(guī)劃；

Step 4.若檢測(cè)到未知障礙物，調(diào)用APF進(jìn)行局部規(guī)劃，避障結(jié)束，返回Step 2，否則，繼續(xù)進(jìn)行局部規(guī)劃；

Step 5.到達(dá)目標(biāo)點(diǎn)，輸出路徑，否則轉(zhuǎn)回Step 2；

Step 6.對(duì)輸出路徑進(jìn)行優(yōu)化處理.

6.2 實(shí)驗(yàn)仿真

為了檢驗(yàn)所提方法的可行性和有效性，在MATLAB 2014a仿真平臺(tái)上進(jìn)行試驗(yàn)仿真.實(shí)驗(yàn)設(shè)置移動(dòng)機(jī)器人的工作環(huán)境為M×M的柵格地圖，S和G的柵格序號(hào)分別為1和M2.對(duì)實(shí)驗(yàn)參數(shù)的設(shè)置如表1所示.

表1 參數(shù)設(shè)定值

為了更好的體現(xiàn)算法的搜索能力和避障效果，使用柵格大小為20×20，并含有多U型陷阱的復(fù)雜環(huán)境地圖，S=1，G=400，障礙物覆蓋率為46%.移動(dòng)機(jī)器人識(shí)別全局環(huán)境中的U型陷阱，將識(shí)別后的U型陷阱用“+”表示，結(jié)果如圖2所示.在搜索時(shí)使用屏蔽U型陷阱措施，避開(kāi)U型陷阱移動(dòng).

實(shí)驗(yàn)1首先在全局環(huán)境下驗(yàn)證IACO進(jìn)行全局規(guī)劃的能力.圖3是全局環(huán)境下ACO與IACO的規(guī)劃結(jié)果，可以看到，IACO能夠成功規(guī)劃出一條全局最優(yōu)路徑，并且避開(kāi)了環(huán)境中所有的U型陷阱.對(duì)比ACO與IACO，IACO明顯改善了規(guī)劃結(jié)果，IACO規(guī)劃的最優(yōu)路徑長(zhǎng)度相較于ACO長(zhǎng)度減少了4.58；從圖4收斂曲線對(duì)比可以明顯看出，振幅較大ACO收斂曲線，在設(shè)定100次迭代內(nèi)無(wú)法收斂，收斂過(guò)程頻繁震蕩，幅度較大，而IACO(點(diǎn)線)在僅11迭代后即收斂，并且，曲線在僅4次震蕩就搜索到最優(yōu)路徑.并且收斂中不存在搜索到最小值后再產(chǎn)生震蕩的過(guò)程，有效地減少了收斂時(shí)間，同時(shí)，IACO規(guī)劃的路徑拐點(diǎn)較少，拐角角度均為鈍角，規(guī)劃結(jié)果體現(xiàn)了IACO優(yōu)越的性能.ACO與IACO對(duì)比結(jié)果如表2所示.

表2 ACO與IACO對(duì)比結(jié)果

為了進(jìn)一步檢驗(yàn)分層規(guī)劃這一方法在未知環(huán)境下規(guī)劃能力，在實(shí)驗(yàn)1的基礎(chǔ)上，實(shí)驗(yàn)2在環(huán)境中加入了動(dòng)態(tài)障礙物A、B，初始位置分別為A(6.5，13.5)，B(7.5，9.5).設(shè)置動(dòng)態(tài)障礙物的運(yùn)動(dòng)機(jī)制為在一個(gè)時(shí)間周期Δt內(nèi)向前運(yùn)動(dòng)一個(gè)柵格，A從上往下運(yùn)動(dòng)，B從左往右運(yùn)動(dòng).

圖5是未知環(huán)境下的避障結(jié)果，可以看到，移動(dòng)機(jī)器人能夠成功避開(kāi)運(yùn)動(dòng)的障礙并尋找到一條最佳路徑.在時(shí)刻1，動(dòng)態(tài)障礙物A由初始位置(6.5，13.5)運(yùn)動(dòng)到(6.5，12.5)，B由位置(7.5，9.5)運(yùn)動(dòng)到(8.5，9.5).移動(dòng)機(jī)器人在此過(guò)程中執(zhí)行分層規(guī)劃，在時(shí)刻1之前，移動(dòng)機(jī)器人按照全局規(guī)劃生成的路徑行進(jìn)，在遭遇動(dòng)態(tài)障礙物A和B時(shí)調(diào)用APF進(jìn)行局部規(guī)劃，規(guī)劃結(jié)果如圖5(a)所示，若時(shí)刻1局部規(guī)劃結(jié)束后，環(huán)境中不存在動(dòng)態(tài)障礙物，則返回全局規(guī)劃.

移動(dòng)機(jī)器人沿著路徑繼續(xù)往前運(yùn)動(dòng)，在兩個(gè)Δt時(shí)間間隔后，由于時(shí)刻2空間C內(nèi)仍存在動(dòng)態(tài)障礙物，因此需要再次進(jìn)行局部路徑規(guī)劃，時(shí)刻2動(dòng)態(tài)障礙物A向下移動(dòng)了兩個(gè)單位柵格，B向右移動(dòng)了兩個(gè)單位柵格，規(guī)劃結(jié)果如圖5(b)所示.時(shí)刻2結(jié)束后，在一定范圍內(nèi)未檢測(cè)到運(yùn)動(dòng)的障礙物，移動(dòng)機(jī)器人將當(dāng)前所處位置(10.5，7.5)作為起始點(diǎn)，返回全局規(guī)劃，按照?qǐng)D5(b)后半地圖所示規(guī)劃結(jié)果進(jìn)行運(yùn)動(dòng)直到路徑規(guī)劃結(jié)束.

對(duì)比圖5(b)中時(shí)刻2平滑前的曲線和平滑后(點(diǎn)線)的曲線，可以看到，平滑前的路徑中有10個(gè)拐點(diǎn)，對(duì)輸出路徑進(jìn)行平滑以后，路徑長(zhǎng)度較處理前明顯縮短，拐點(diǎn)個(gè)數(shù)也大大減少.為了更直觀的檢驗(yàn)平滑效果，分別觀察每一個(gè)拐點(diǎn)處的平滑結(jié)果，可見(jiàn)平滑后雖然仍舊存在拐點(diǎn)，但拐點(diǎn)角度均較大，幾乎接近180°，在實(shí)際應(yīng)用中不會(huì)造成移動(dòng)機(jī)器人在移動(dòng)過(guò)程中過(guò)多耗能，使得移動(dòng)機(jī)器人能夠流暢地經(jīng)過(guò)尖峰路徑.

7 結(jié) 論

目前，移動(dòng)機(jī)器人單一環(huán)境下的路徑規(guī)劃技術(shù)已經(jīng)較為成熟，研究熱點(diǎn)主要聚焦于存在環(huán)境無(wú)法預(yù)知的路徑規(guī)劃問(wèn)題.針對(duì)并含靜態(tài)與動(dòng)態(tài)障礙物的未知環(huán)境下的路徑規(guī)劃問(wèn)題，提出將蟻群算法與人工勢(shì)場(chǎng)法結(jié)合.

1)首先采用改進(jìn)蟻群算法進(jìn)行全局路徑規(guī)劃，提出一種屏蔽U型陷阱措施，能夠避免在搜索過(guò)程因陷入U(xiǎn)型陷阱而停滯搜索，無(wú)法前進(jìn)的情況；在啟發(fā)信息里加入目標(biāo)點(diǎn)的引導(dǎo)，加強(qiáng)了蟻群的全局搜尋能力；同時(shí)，為了避免螞蟻因信息素值大小而出現(xiàn)的局部最優(yōu)解和結(jié)束搜索的問(wèn)題，從而引進(jìn)了最大最小螞蟻系統(tǒng)的思想，將信息素限制在一定范圍內(nèi).

2)在規(guī)劃過(guò)程中，當(dāng)移動(dòng)機(jī)器人在搜索過(guò)程檢測(cè)到動(dòng)態(tài)障礙物并預(yù)測(cè)到將要與它碰撞時(shí)，調(diào)用人工勢(shì)場(chǎng)法進(jìn)行局部規(guī)劃，規(guī)劃結(jié)束后，將當(dāng)前點(diǎn)作為起始點(diǎn)，返回全局規(guī)劃.

3)最后，將規(guī)劃好的路徑輸出并對(duì)其做優(yōu)化處理.

實(shí)驗(yàn)在多U型環(huán)境下成功規(guī)劃出一條平滑的路徑，驗(yàn)證了所提方法的良好性能.