基于改進(jìn)蟻群算法的無(wú)人機(jī)三維路徑規(guī)劃

唐熙，王海寶，羅強(qiáng)

（404120 重慶市 重慶三峽學(xué)院 智能山地農(nóng)機(jī)技術(shù)研究中心）

0 引言

無(wú)人機(jī)的路徑規(guī)劃算法是實(shí)現(xiàn)無(wú)人機(jī)自主飛行的重要研究方向。與傳統(tǒng)地面機(jī)器人相比，無(wú)人機(jī)在路徑規(guī)劃中不僅要考慮時(shí)間和動(dòng)力學(xué)的約束，還要考慮地形可能導(dǎo)致的碰撞[1]。因此學(xué)者們提出了許多三維環(huán)境下的路徑規(guī)劃方法，如人工勢(shì)場(chǎng)法[2]、螢火蟲(chóng)算法[3]、RRT 算法[4]等。

蟻群算法是模擬螞蟻覓食提出的一種算法，被廣泛應(yīng)用于解決旅行商問(wèn)題[5]。Huang 等[6]考慮到無(wú)人機(jī)在路徑規(guī)劃時(shí)的靜態(tài)威脅和動(dòng)態(tài)威脅，在蟻群算法的基礎(chǔ)上為障礙物添加排斥力場(chǎng)，并將斥力作為性能考察指標(biāo)，改進(jìn)后的算法能生成更加光滑的低成本路徑；Zhang 等[7]利用插值法對(duì)地圖進(jìn)行柵格化處理，根據(jù)坡度閾值將三維地圖轉(zhuǎn)化為二維地圖，利用并行技術(shù)加快蟻群算法；袁夢(mèng)順等[8]通過(guò)將人工勢(shì)場(chǎng)法與狀態(tài)轉(zhuǎn)移策略進(jìn)行融合，設(shè)置了雙精英螞蟻策略，提高了算法的全局搜索能力；劉雨青等[9]利用Dijkstra 算法進(jìn)行信息素初始化，根據(jù)不同路徑點(diǎn)能量消耗的差異來(lái)改進(jìn)啟發(fā)式函數(shù)，并采用貝塞爾曲線進(jìn)行平滑處理，規(guī)劃出的路徑較傳統(tǒng)蟻群算法能耗比降低了20%；王飛等[10]構(gòu)造了新的啟發(fā)函數(shù)，利用優(yōu)質(zhì)螞蟻更新策略，自適應(yīng)調(diào)整信息素?fù)]發(fā)因子，比傳統(tǒng)蟻群算法路徑長(zhǎng)度減少10%，迭代次數(shù)減少了40%左右，但是時(shí)間是傳統(tǒng)蟻群算法的2 倍。

本文在現(xiàn)有蟻群算法的基礎(chǔ)上以算法運(yùn)行時(shí)間和最優(yōu)路徑長(zhǎng)度為優(yōu)化目標(biāo)，采用改變搜索窗口大小的方式，根據(jù)無(wú)人機(jī)工作的最優(yōu)高度范圍改進(jìn)啟發(fā)式函數(shù)，通過(guò)2 組實(shí)驗(yàn)對(duì)本文改進(jìn)算法與蟻群算法、王飛等[10]的算法（下文稱算法W）進(jìn)行比較，并得出結(jié)論。

1 三維模型建立

1.1 三維環(huán)境建模

首先以A為坐標(biāo)原點(diǎn)建立一個(gè)ABCD-EFGH三維空間坐標(biāo)系，其中無(wú)人機(jī)橫向移動(dòng)的方向?yàn)閄軸，縱向移動(dòng)的方向?yàn)閅軸，垂直移動(dòng)方向?yàn)閆軸，沿X軸方向?qū)E進(jìn)行K次等分，即可獲得K+1 個(gè)平面，如圖1 所示。利用地形高程信息中的橫縱坐標(biāo)與XY軸坐標(biāo)對(duì)應(yīng)，將高度信息存儲(chǔ)在如式（1）所示矩陣T 中，則地形高度的頂點(diǎn)在坐標(biāo)系中可以用（X，Y，T[m，n]）表示，連接相鄰坐標(biāo)即可表示整個(gè)地圖的地形。

圖1 三維模型Fig.1 Three-dimensional model

1.2 搜索模式

進(jìn)行路徑搜索時(shí)采取節(jié)點(diǎn)前進(jìn)方式。無(wú)人機(jī)起飛點(diǎn)為Us，目標(biāo)點(diǎn)為Ue，沿著X軸方向前進(jìn)。從起始點(diǎn)所在平面K1開(kāi)始，探索在K2平面可行區(qū)域內(nèi)最優(yōu)的下一節(jié)點(diǎn)位置，以此往復(fù)，總共探索K個(gè)平面。無(wú)人機(jī)在一定時(shí)間內(nèi)的最大橫移和最大縱移動(dòng)為Ymax和Zmax，則無(wú)人機(jī)的搜索范圍為（2Ymax+1）×（2Zmax+1）。搜索范圍的大小對(duì)算法的運(yùn)行時(shí)間有極大影響，減小搜索范圍可以縮短蟻群算法運(yùn)行時(shí)間，但搜索范圍太小會(huì)導(dǎo)致蟻群算法陷入局部最優(yōu)。傳統(tǒng)蟻群算法搜索范圍是一個(gè)固定值，缺乏靈活性，本文采用動(dòng)態(tài)調(diào)整的方法。開(kāi)始時(shí)，讓無(wú)人機(jī)在一個(gè)較大的范圍內(nèi)搜索，在距離目標(biāo)點(diǎn)一定范圍內(nèi)后，減小搜索范圍（如圖2 所示），加快算法運(yùn)行時(shí)間，使整體算法性能達(dá)到最優(yōu)。

圖2 搜索模式Fig.2 Search mode

1.3 距離計(jì)算公式

在三維空間中，（xi，yi，zi）是螞蟻當(dāng)前的坐標(biāo)，（xj，yj，zj）是下一點(diǎn)的坐標(biāo)，（xe，ye，ze）為目標(biāo)點(diǎn)的坐標(biāo)。

螞蟻從當(dāng)前點(diǎn)到下一點(diǎn)距離

螞蟻從當(dāng)前點(diǎn)到目標(biāo)點(diǎn)距離

2 改進(jìn)蟻群算法

2.1 啟發(fā)式函數(shù)改進(jìn)

啟發(fā)式函數(shù)是蟻群算法的核心，設(shè)計(jì)一個(gè)符合當(dāng)前模型的啟發(fā)式函數(shù)能提高蟻群算法的效率。螞蟻m從當(dāng)前節(jié)點(diǎn)到下一個(gè)目標(biāo)點(diǎn)會(huì)依據(jù)路徑上的信息素值和啟發(fā)式信息計(jì)算狀態(tài)轉(zhuǎn)移概率[11]，其表達(dá)式為

式中：α——信息素啟發(fā)因子；β——期望啟發(fā)因子；ηij——啟發(fā)式信息函數(shù)；τij——信息素函數(shù)。

啟發(fā)式信息函數(shù)為

式中：M——距離系數(shù)，用于調(diào)整距離在啟發(fā)式函數(shù)中的權(quán)重。

傳統(tǒng)蟻群算法僅考慮與下一節(jié)點(diǎn)的距離，這樣容易導(dǎo)致蟻群陷入局部最優(yōu)。本文采用當(dāng)前節(jié)點(diǎn)與下一節(jié)點(diǎn)和目標(biāo)點(diǎn)的距離來(lái)引導(dǎo)螞蟻選擇距離目標(biāo)點(diǎn)更近的下一節(jié)點(diǎn)，促使其向最優(yōu)路徑方向前進(jìn)。

無(wú)人機(jī)在空中飛行也會(huì)受到地形的影響，如低空突防[12]、植保作業(yè)[13]會(huì)跟隨地形進(jìn)行飛行。之前的三維蟻群算法只是將高度作為一個(gè)安全狀態(tài)看待，無(wú)人機(jī)飛行高度大于地形高度即為安全，反之為不可飛行區(qū)域。但是在無(wú)人機(jī)實(shí)際使用過(guò)程中，會(huì)存在一個(gè)最適工作高度，通過(guò)這一特性設(shè)置高度適應(yīng)函數(shù)Sij，讓無(wú)人機(jī)在執(zhí)行任務(wù)過(guò)程中在最適高度飛行，以保證無(wú)人機(jī)有更好的作業(yè)條件，具體表達(dá)式為

式中：Fmin——安全狀態(tài)下的最小離地高度；Wmax、Wmin——無(wú)人機(jī)最適飛行高度的最大值和最小值；Ti——當(dāng)前位置的地形高度；dz——飛行高度。

2.2 初始信息素優(yōu)化

蟻群算法在初期的搜索是隨機(jī)的，初始化的信息素值對(duì)其有很大的影響。普通蟻群算法初始化信息素值是開(kāi)始時(shí)設(shè)置一個(gè)統(tǒng)一的值。統(tǒng)一的信息素值會(huì)影響算法的收斂速度，為解決這一問(wèn)題，在沿起始點(diǎn)到目標(biāo)點(diǎn)的直線距離范圍內(nèi)的空間設(shè)置初始有利區(qū)域，該區(qū)域中初始信息素值高于其他區(qū)域的信息素初始值，表達(dá)式為

式中：τ0——信息數(shù)初始值；dse——起始點(diǎn)到終點(diǎn)的距離；P——dse上在無(wú)人機(jī)搜索范圍內(nèi)的點(diǎn)集合。

沿初始點(diǎn)到目標(biāo)點(diǎn)設(shè)置不均勻的信息素分布[14]，有利于引導(dǎo)螞蟻朝著目標(biāo)點(diǎn)的方向前進(jìn)，加快收斂速度。

2.3 精英蟻群信息素更新

信息素更新包括信息素?fù)]發(fā)和信息素增強(qiáng)。信息素?fù)]發(fā)有利于探索未知區(qū)域，避免陷入局部最優(yōu)，信息素增強(qiáng)是增加路徑上的信息素值，加快收斂速度。當(dāng)螞蟻在完成節(jié)點(diǎn)間探索后，當(dāng)前節(jié)點(diǎn)的信息素會(huì)按照設(shè)置的信息素衰減系數(shù)ζ進(jìn)行揮發(fā)，更新公式為

當(dāng)螞蟻完成一條路徑的探索后會(huì)將本次路徑長(zhǎng)度與整個(gè)蟻群的最短路徑進(jìn)行比較，選出最短路徑，在該路徑上進(jìn)行信息素增強(qiáng)，增大后續(xù)螞蟻選擇該路徑的概率，采用蟻周模型更新信息素，更新公式為

式中：length(m)——第m只螞蟻經(jīng)過(guò)的路徑長(zhǎng)度；ρ——信息素系數(shù)；N——信息素常數(shù)。

為防止信息素過(guò)小，收斂速度慢，信息素過(guò)大，陷入局部最優(yōu)，限定τij∈[τmin,τmax]。

2.4 改進(jìn)算法流程

算法實(shí)現(xiàn)步驟：

（1）初始環(huán)境建模，設(shè)定起始點(diǎn)，目標(biāo)點(diǎn)，設(shè)置蟻群數(shù)量，最大迭代次數(shù)等參數(shù)；

（2）信息素的初始化，根據(jù)式（8）計(jì)算初始值，并進(jìn)行不均勻的信息素分配；

（3）蟻群探索。探索路徑，根據(jù)狀態(tài)轉(zhuǎn)移概率式（4）求得下一節(jié)點(diǎn)，并按照式（9）進(jìn)行揮發(fā)，直到探索完所有節(jié)點(diǎn)；

（4）判斷蟻群是否完成路徑探索，是進(jìn)行步驟5，否返回步驟3，直到蟻群完成探索；

（5）對(duì)所有到達(dá)目標(biāo)點(diǎn)的螞蟻?zhàn)哌^(guò)的路徑進(jìn)行信息素更新，并按照式（10）、式（11）增加最短路徑各節(jié)點(diǎn)的信息素；

（6）判斷蟻群的迭代次數(shù)是否達(dá)到設(shè)定值，是進(jìn)行步驟7，否進(jìn)行步驟2，直到完成迭代；

（7）輸出最優(yōu)路徑長(zhǎng)度，算法結(jié)束。

3 仿真與分析

算法運(yùn)行環(huán)境為：處理器為AMD Ryzen 5800X，內(nèi)存16 GB，MATLAB R2021a，采用本文改進(jìn)蟻群算法、算法W 和傳統(tǒng)蟻群算法進(jìn)行對(duì)比實(shí)驗(yàn)，每種算法進(jìn)行20 次實(shí)驗(yàn)，隨機(jī)抽取其中5組作為最終結(jié)果，并選擇出路徑最短的一組對(duì)比其迭代次數(shù)。蟻群初始參數(shù)設(shè)置如表1 所示。

表1 蟻群初始參數(shù)Tab.1 Initial parameters of ant colony

3.1 實(shí)驗(yàn)1

隨機(jī)生成一個(gè)20 km×20 km×2 km 的柵格地圖，設(shè)置起飛點(diǎn)為（1，11，3），目標(biāo)點(diǎn)為（21，10，5）。傳統(tǒng)蟻群算法、算法W 和本文改進(jìn)算法的實(shí)驗(yàn)結(jié)果分別如圖3—圖5 所示，3 種算法最優(yōu)路徑長(zhǎng)度如圖6 所示。實(shí)驗(yàn)結(jié)果見(jiàn)表2。

圖3 蟻群算法Fig.3 Ant colony algorithm

圖4 算法WFig.4 Algorithm W

圖5 本文改進(jìn)算法Fig.5 Improved algorithm

圖6 最優(yōu)路徑長(zhǎng)度迭代次數(shù)變化曲線Fig.6 Variation curve of iterations number of optimal path length

由圖3—圖5 和表2 可知，本文的改進(jìn)算法最優(yōu)路徑長(zhǎng)度比蟻群算法減少了19%，比算法W 減少了14%；運(yùn)行時(shí)間比蟻群算法減少了27%，比算法W 減少了52%。根據(jù)最適高度改進(jìn)啟發(fā)式函數(shù)后，改進(jìn)算法尋找的路徑更加跟隨地形，不均勻的信息素分布加快了算法的收斂速度，在改進(jìn)算法搜索窗口大小后，算法整體運(yùn)行的時(shí)間也明顯減小。

3.2 實(shí)驗(yàn)2

為檢驗(yàn)算法的通用性，驗(yàn)證其在更加復(fù)雜的環(huán)境下的性能，采用30 km×30 km×2 km 的柵格地圖，設(shè)置起飛點(diǎn)為（1，20，5），目標(biāo)點(diǎn)為（31，19，5）。傳統(tǒng)蟻群算法、算法W 和本文改進(jìn)算法的實(shí)驗(yàn)結(jié)果分別如圖7—圖9 所示，3 種算法的路徑長(zhǎng)度與迭代次數(shù)對(duì)比如圖10 所示。實(shí)驗(yàn)結(jié)果見(jiàn)表3。

表3 仿真實(shí)驗(yàn)結(jié)果Tab.3 Simulation experiment results

圖7 蟻群算法Fig.7 Ant colony algorithm

圖8 算法WFig.8 Algorithm W

圖9 本文改進(jìn)算法Fig.9 Improved algorithm

圖10 最優(yōu)路徑長(zhǎng)度迭代次數(shù)變化曲線Fig.10 Variation curve of iteration number of optimal path length

結(jié)合圖7—圖9 和表3 可知，本文的改進(jìn)算法得到的最優(yōu)路徑長(zhǎng)度比蟻群算法減少了13%，比算法W 減少了9%；運(yùn)行時(shí)間比蟻群算法減少了31%，比算法W 減少了60%。在更加復(fù)雜的環(huán)境中，本文改進(jìn)算法依然可以找到最優(yōu)路徑，且所需時(shí)間大幅度減少。通過(guò)2 個(gè)實(shí)驗(yàn)結(jié)果可知，改進(jìn)后的算法在不同環(huán)境下都能搜索到最優(yōu)路徑，且所需時(shí)間大大減少，證明了改進(jìn)算法的優(yōu)越性和可行性。

4 總結(jié)

本文為解決蟻群算法在求解無(wú)人機(jī)路徑規(guī)劃問(wèn)題中遇到的運(yùn)行時(shí)間長(zhǎng)、局部最優(yōu)等問(wèn)題，采用不均勻的初始化信息素，在蟻群搜索過(guò)程中改變搜索窗口大小,根據(jù)無(wú)人機(jī)工作的最優(yōu)工作高度改進(jìn)啟發(fā)式函數(shù)，對(duì)達(dá)到目的地最短的螞蟻進(jìn)行信息素更新等方法對(duì)蟻群算法進(jìn)行改進(jìn)。通過(guò)2組對(duì)比實(shí)驗(yàn)表明，改進(jìn)后的蟻群算法比傳統(tǒng)蟻群算法和算法W 最優(yōu)路徑長(zhǎng)度更短，運(yùn)行時(shí)間更短。