基于A*和蟻群算法的移動機(jī)器人多目標(biāo)路徑規(guī)劃方法

李孟錫，何博俠，周 俁

(南京理工大學(xué)機(jī)械工程學(xué)院,江蘇 南京 210094)

0 引言

移動機(jī)器人多目標(biāo)路徑規(guī)劃,是指在環(huán)境中找到一條經(jīng)過所有目標(biāo)點(diǎn)且安全無碰撞的最優(yōu)路徑[1]。移動機(jī)器人多目標(biāo)路徑規(guī)劃通?？梢苑譃?個問題[2]：任意兩目標(biāo)間最短路徑規(guī)劃問題；多目標(biāo)全局最優(yōu)路徑問題。常用二維柵格地圖最短路徑規(guī)劃算法有Dijkstra算法[3]、最佳優(yōu)先搜索(BFS)算法、A*算法[4]、Floyd[5]算法和D*算法[2]。其中，A*算法搜索的路徑綜合考慮了當(dāng)前點(diǎn)與起點(diǎn)及目標(biāo)點(diǎn)的代價(jià)值，是目前最常用的路徑規(guī)劃算法。但該算法也有一定缺陷，在擴(kuò)展節(jié)點(diǎn)時，該算法將相鄰的所有節(jié)點(diǎn)都加入搜索列表，造成大量的無用計(jì)算。多目標(biāo)點(diǎn)全局最優(yōu)路徑問題可以轉(zhuǎn)換為TSP(traveling salesman problem)問題，TSP 問題通常采用啟發(fā)式算法求解1個較優(yōu)解。常用的求解方法有粒子群算法[6]、遺傳算法[7]、模擬退火算法[8]和蟻群算法[9]等。蟻群算法將螞蟻覓食過程抽象為數(shù)學(xué)模型求解TSP問題。優(yōu)點(diǎn)是編程簡單、魯棒性強(qiáng)，缺點(diǎn)是易陷入局部最優(yōu)值。針對該缺點(diǎn)，文獻(xiàn)[10]提出一種動態(tài)蟻群遺傳算法，通過將蟻群算法與遺傳算法動態(tài)融合，避免算法陷入局部最優(yōu)解。文獻(xiàn)[11]通過將蟻群算法與貪心算法相結(jié)合，并引入變異算子進(jìn)行改進(jìn)。

本文提出一種基于啟發(fā)信息擴(kuò)展節(jié)點(diǎn)的A*算法與混合蟻群算法相結(jié)合的方法，求解移動機(jī)器人多目標(biāo)路徑規(guī)劃問題。針對A*算法擴(kuò)展節(jié)點(diǎn)時，會造成無用計(jì)算的缺點(diǎn)，通過基于啟發(fā)信息的擴(kuò)展函數(shù)進(jìn)行改進(jìn)，減少無用節(jié)點(diǎn)計(jì)算，提升規(guī)劃效率。針對蟻群算法易陷入局部最優(yōu)解的缺點(diǎn)，提出一種混合蟻群算法。通過自適應(yīng)調(diào)整方法，動態(tài)調(diào)整信息素?fù)]發(fā)因子，將粒子群算法思想融入蟻群算法，并采用自適應(yīng)交叉變異策略改進(jìn)蟻群算法，避免陷入局部最優(yōu)解，縮短移動機(jī)器人行進(jìn)路程。

1 最短路徑規(guī)劃

1.1 基本A*算法

常用的柵格地圖路徑規(guī)劃方法遵循最小代價(jià)原則，具體過程為從起點(diǎn)開始，不斷擴(kuò)展搜索節(jié)點(diǎn)，計(jì)算各個已探索柵格節(jié)點(diǎn)的代價(jià)值，更新節(jié)點(diǎn)的狀態(tài)，直到搜索到目標(biāo)后停止，并按照搜索時標(biāo)記的節(jié)點(diǎn)最小代價(jià)得到最優(yōu)路徑。A*算法、Dijkstra算法和BFS算法流程基本相同，不同點(diǎn)在于代價(jià)函數(shù)不同。

A*算法綜合了 Dijkstra 算法和 BFS 算法的優(yōu)點(diǎn)，其代價(jià)函數(shù)兼容了兩者的思路：

f(n)=g(n)+h(n)

(1)

n為路徑中的柵格節(jié)點(diǎn)；g(n)為到達(dá)當(dāng)前點(diǎn)最短路徑代價(jià)函數(shù)；h(n)為達(dá)到目標(biāo)點(diǎn)最短路徑代價(jià)函數(shù)。

A*算法的估值函數(shù)中，h(n)代價(jià)值可使用曼哈頓距離、切比雪夫距離或歐幾里得距離計(jì)算[12]。本文采用歐幾里得距離為作為代價(jià)值。設(shè)當(dāng)前節(jié)點(diǎn)坐標(biāo)為(xn,yn)，目標(biāo)坐標(biāo)為(xg,yg)，歐幾里得距離表示兩坐標(biāo)的最短距離，其公式為

(2)

1.2 基于啟發(fā)信息擴(kuò)展節(jié)點(diǎn)的A*算法

基本A*算法中，將父節(jié)點(diǎn)所有相鄰節(jié)點(diǎn)加入到待搜索列表，造成大量無效計(jì)算，利用基于啟發(fā)信息的節(jié)點(diǎn)擴(kuò)展函數(shù)擴(kuò)展節(jié)點(diǎn)，利用評價(jià)函數(shù)對擴(kuò)展的節(jié)點(diǎn)進(jìn)行評估，減少無用節(jié)點(diǎn)的擴(kuò)展，提高計(jì)算效率。擴(kuò)展節(jié)點(diǎn)評價(jià)函數(shù)描述為

Gnode(n)=knode(n-1)-knode(n)

(3)

knode(n)為擴(kuò)展節(jié)點(diǎn)(xn,yn)的啟發(fā)信息。

設(shè)目標(biāo)點(diǎn)坐標(biāo)為(xg,yg)，其公式為

(4)

如果當(dāng)前節(jié)點(diǎn)的擴(kuò)展節(jié)點(diǎn)有障礙物時，還是按照八鄰域擴(kuò)展；如果當(dāng)前節(jié)點(diǎn)的擴(kuò)展節(jié)點(diǎn)無障礙點(diǎn)時，如果其評價(jià)函數(shù)大于0，則將該點(diǎn)加入待搜索列表，否則，忽略此擴(kuò)展點(diǎn)。擴(kuò)展節(jié)點(diǎn)流程如圖1所示。

圖1 擴(kuò)展節(jié)點(diǎn)流程

2 全局路徑規(guī)劃

2.1 基本蟻群算法

基本蟻群算法中螞蟻根據(jù)路徑上的信息素濃度選擇路徑，距離短的路徑信息素濃度高，經(jīng)過迭代，全局最短路徑就會突顯出來。

假設(shè)目標(biāo)點(diǎn)數(shù)量為n;群體中的螞蟻數(shù)量為m;目標(biāo)點(diǎn)i與j之間的距離為dij;路徑信息素濃度為τij(t)，通常認(rèn)為最初各目標(biāo)點(diǎn)間的具有相同的信息素濃度，于是有τij(0)=τ0;ηij(t)為啟發(fā)函數(shù)，表示螞蟻在時刻t由目標(biāo)點(diǎn)i轉(zhuǎn)移到目標(biāo)點(diǎn)j的期望程度。

(5)

α為信息素啟發(fā)因子，表示信息素濃度的重要程度，即τij(t)的重要程度；β為期望啟發(fā)因子，表示兩目標(biāo)距離的重要程度，即ηij(t)的重要程度；allowk存放未訪問的目標(biāo)點(diǎn)。α+β=1，若α越大，則選擇當(dāng)前信息素濃度較大的路徑概率大，若β較大，則選擇距離當(dāng)前位置較近的目標(biāo)點(diǎn)概率較大。啟發(fā)函數(shù)一般為ηij(t)=1/dij。

在蟻群完成路徑選擇后，需要更新路徑上的信息素濃度更新為

(6)

蟻群算法中信息素可通過3種模型更新，Ant-Cycle(蟻周系統(tǒng))模型利用全局信息更新信息素，性能更好[13]。該模型其信息素更新模型為

(7)

Q為在1次循環(huán)中螞蟻k所釋放的信息素總量；Lk為在該循環(huán)中螞蟻k經(jīng)過的路徑總長；Spathk為螞蟻k經(jīng)過的路徑集合。

2.2 混合蟻群算法

蟻群算法依賴信息素濃度求解，容易陷入局部最優(yōu)解。為解決這一問題，本文提出一種混合蟻群算法，在蟻群算法中融入粒子群算法思想，通過自適應(yīng)調(diào)整蟻群算法中的信息素?fù)]發(fā)因子，與最優(yōu)解進(jìn)行變異交叉操作，增強(qiáng)全局搜索能力，避免過早陷入局部最優(yōu)解。

2.2.1 自適應(yīng)調(diào)整信息素?fù)]發(fā)因子

蟻群算法中，信息素?fù)]發(fā)因子ρ的會影響算法的全局搜索能力與收斂速度，當(dāng)ρ值較小時，可以提高全局搜索能力，但收斂速度會下降；當(dāng)ρ值較大時，收斂速度會增加，但全局搜索能力降低。本文使用自適應(yīng)調(diào)整方法動態(tài)調(diào)整信息素?fù)]發(fā)因子ρ，調(diào)整公式如下：

(8)

λ∈(0,1)為信息素?fù)]發(fā)因子調(diào)節(jié)系數(shù)；ρmin為ρ的最小值。在迭代初始階段，信息素?fù)]發(fā)因子ρ值較大，收斂速度較快，后期逐漸降低ρ值，搜索隨機(jī)性增強(qiáng)，全局搜索能力得到提升。通過設(shè)定最小值ρmin保證算法收斂速度；通過對信息素?fù)]發(fā)因子的改進(jìn)，提升算法全局搜索能力。

2.2.2 融入粒子群算法思想

粒子群算法通過追蹤全局極值gbest和個體極值pbest更新粒子，向最優(yōu)解方向運(yùn)動。粒子群算法粒子更新公式為

(9)

(10)

w為慣性權(quán)重值；c1、c2為學(xué)習(xí)因子；r1、r2為均勻分布在(0,1)區(qū)間的隨機(jī)數(shù)。

將粒子群算法中通過追蹤個體極值pbest與全局極值gbest求解最優(yōu)解的思想融入到蟻群算法，在算法迭代過程中，螞蟻根據(jù)全局最優(yōu)解與信息素進(jìn)行學(xué)習(xí)。

2.2.3 自適應(yīng)交叉變異策略

通過引入自適應(yīng)交叉變異策略避免算法陷入局部最優(yōu)解，將解空間中所有解分別與最優(yōu)解進(jìn)行交叉變異操作，交叉方式采用部分映射雜交，在1個粒子中隨機(jī)選擇1個區(qū)域進(jìn)行交叉操作，使用該區(qū)域代替待交叉粒子的相應(yīng)區(qū)域，對粒子中有沖突的數(shù)據(jù)采用部分映射法進(jìn)行消除。假設(shè)1個粒子為:Spath0=(1,2,3,4,5,6,7,8,9)(數(shù)字代表城市編號，這串?dāng)?shù)字為城市遍歷順序)，待交叉粒子為Spath1=(9,8,|6,7,5,4|,3,2,1)，假設(shè)交叉區(qū)域?yàn)閨6,7,5,4|，交叉后的結(jié)果為(1，2，6，7，5，4，3，8，9)。該交叉策略可以避免交叉區(qū)域添加到隨機(jī)位置，避免出現(xiàn)較差解。變異操作采用的是隨機(jī)選取2個點(diǎn)進(jìn)行交換，在1～n個節(jié)點(diǎn)中，隨機(jī)地選取第j次訪問的城市節(jié)點(diǎn)，在路徑Spath0中交換第j次和j+1次訪問的節(jié)點(diǎn)，其余不變，此時的路徑為Spath1。例如Spath0=(1,3,2,5,4,9,8,6,7)，j=3，則Spath1=(1,3,5,2,4,9,8,6,7)。與交叉策略相同，變異策略的變異幅度小，避免出現(xiàn)較差解，同時維持解的多樣性。根據(jù)適應(yīng)度值大小更新粒子，如果在交叉變異操作后，求解路徑長度變短，則將交叉變異后的粒子更新該粒子。

2.3 混合蟻群算法流程

通過在蟻群算法中融入粒子群算法思想，并對粒子進(jìn)行交叉變異操作，保留優(yōu)秀粒子，同時增強(qiáng)了粒子的多樣性，蟻群算法根據(jù)信息素和全局最優(yōu)粒子進(jìn)行求解，緩解蟻群算法易陷入局部最優(yōu)解的特性。

混合蟻群算法流程如圖2所示。

圖2 混合蟻群算法流程

3 算法仿真

3.1 基于啟發(fā)信息擴(kuò)展節(jié)點(diǎn)的A*算法仿真

在MATLAB上建立柵格地圖并進(jìn)行仿真實(shí)驗(yàn)，算法仿真對比如圖3所示。圖3中，黑色柵格為環(huán)境障礙物，灰色柵格為已搜索節(jié)點(diǎn)，黑色線條為規(guī)劃路徑，星號為目標(biāo)點(diǎn)，圓點(diǎn)為起始點(diǎn)。對遍歷的柵格數(shù)量，路徑長度進(jìn)行分析。仿真結(jié)果如表1所示。

圖3 算法仿真對比

表1 算法仿真結(jié)果

仿真結(jié)果表明，基于啟發(fā)信息擴(kuò)展節(jié)點(diǎn)的A*算法相對于A*算法，其搜索節(jié)點(diǎn)數(shù)量減少13.18%，提升了路徑規(guī)劃效率。

3.2 混合蟻群算法仿真

為了測試混合蟻群算法的性能，采用TSPLIB標(biāo)準(zhǔn)庫中數(shù)據(jù)集進(jìn)行仿真測試，使用基本蟻群算法與本文的混合蟻群算法進(jìn)行對比，每種算法運(yùn)行30次，在算法仿真中，信息素啟發(fā)因子為α=1.0，期望啟發(fā)因子為β=5.0，信息素?fù)]發(fā)因子初始值為ρ0=0.9，信息素?fù)]發(fā)因子調(diào)節(jié)系數(shù)為λ=0.98，信息素?fù)]發(fā)因子最小值為ρmin=0.5，螞蟻數(shù)量為m=50，初始信息素為C=100，信息素強(qiáng)度為Q=106。

仿真結(jié)果如表2所示，圖4為混合蟻群算法求解Eil51數(shù)據(jù)集的結(jié)果。由實(shí)驗(yàn)結(jié)果可知，基本蟻群算法求解的最優(yōu)路徑和平均路徑與已知最優(yōu)值相差較大，在迭代60次后，路徑長度不再變化，陷入局部最優(yōu)解，而混合蟻群算法求解的最優(yōu)路徑和平均路徑均好于蟻群算法，并且在迭代過程中，路徑長度持續(xù)減小。所以，混合蟻群算法有效改善了蟻群算法易陷入局部最優(yōu)解的問題。

表2 算法仿真結(jié)果

圖4 混合蟻群算法求解Eil51數(shù)據(jù)集結(jié)果

4 實(shí)驗(yàn)

為了驗(yàn)證實(shí)際環(huán)境中算法的有效性，選取建筑物走廊進(jìn)行實(shí)驗(yàn)，機(jī)器人導(dǎo)航實(shí)驗(yàn)平臺如圖5所示。該平臺硬件包括激光雷達(dá)、雙目視覺相機(jī)、IMU、控制系統(tǒng)與機(jī)器人底盤，軟件系統(tǒng)采用ROS(robot operating system)[14]。

圖5 機(jī)器人硬件系統(tǒng)

在已構(gòu)建好的二維柵格環(huán)境地圖基礎(chǔ)上進(jìn)行實(shí)驗(yàn)，地圖分辨率為0.05 m，為了讓機(jī)器人與環(huán)境障礙物保持安全距離，對地圖中的障礙物輪廓做適當(dāng)?shù)呐蛎浱幚?0.60 m)。

實(shí)驗(yàn)參數(shù)與仿真參數(shù)相同。選擇15個目標(biāo)點(diǎn)，進(jìn)行多目標(biāo)全局路徑規(guī)劃對比實(shí)驗(yàn)。任意兩目標(biāo)點(diǎn)間使用基于擴(kuò)展節(jié)點(diǎn)的A*算法規(guī)劃最短路徑。分別使用基本蟻群算法與提出的混合蟻群算法規(guī)劃全局路徑，每種算法運(yùn)行10次，并進(jìn)行對比。

圖6a為基本蟻群算法求解的全局路徑圖，圖6b為混合蟻群算法求解的全局路徑圖，圓點(diǎn)為選定目標(biāo)點(diǎn)，黑色線條為規(guī)劃路徑。

圖6 全局路徑規(guī)劃實(shí)驗(yàn)

實(shí)驗(yàn)結(jié)果如表3所示。由實(shí)驗(yàn)結(jié)果可知，混合蟻群算法求解的最短路徑長度與平均路徑長度均優(yōu)于基本蟻群算法。本文所提出的多目標(biāo)全局路徑規(guī)劃算法可以在柵格地圖環(huán)境中進(jìn)行路徑規(guī)劃，并規(guī)劃出1條較短的全局路徑，且該路徑通過所有目標(biāo)點(diǎn)，可以完成機(jī)器人多目標(biāo)路徑規(guī)劃任務(wù)。

表3 實(shí)驗(yàn)結(jié)果

5 結(jié)束語

本文提出一種針對二維柵格地圖下，移動機(jī)器人多目標(biāo)點(diǎn)路徑規(guī)劃方法。該方法首先采用基于啟發(fā)信息擴(kuò)展節(jié)點(diǎn)的A*算法計(jì)算任意兩目標(biāo)點(diǎn)的最短路徑距離，然后通過混合蟻群算法求解目標(biāo)點(diǎn)遍歷順序。將該方法應(yīng)用于機(jī)器人平臺并進(jìn)行實(shí)驗(yàn)，實(shí)驗(yàn)結(jié)果證明了該方法的有效性。在今后的研究過程中，可以將二維柵格地圖擴(kuò)展為三維地圖，通過改進(jìn)本文方法實(shí)現(xiàn)移動機(jī)器人多目標(biāo)點(diǎn)路徑規(guī)劃。