魏 江，王建軍 ，王 健，2，秦春霞，梅少輝

1.西北工業(yè)大學(xué) 電子信息學(xué)院，西安 710129

2.西北工業(yè)大學(xué) 第365研究所，西安 710129

1 引言

近年來(lái)，無(wú)人機(jī)航跡規(guī)劃問(wèn)題成為國(guó)內(nèi)外學(xué)者研究的熱點(diǎn)，尤其在軍事、搶險(xiǎn)救災(zāi)和測(cè)繪等領(lǐng)域尤為重要。為了提高無(wú)人機(jī)的生存機(jī)率并充分發(fā)揮其作戰(zhàn)優(yōu)勢(shì)，必須選擇一條能夠快速安全到達(dá)目標(biāo)且最大可能地完成任務(wù)并返航的航跡，因此航跡規(guī)劃成為無(wú)人機(jī)相關(guān)研究中的重要內(nèi)容。許多相關(guān)研究者將A*算法[1-2]、人工勢(shì)場(chǎng)法[3-4]、概率地圖算法[5]、遺傳算法[6-7]、快速隨機(jī)搜索樹算法[8]等較為成熟的算法應(yīng)用于航跡規(guī)劃，這些算法在三維航跡規(guī)劃中都存在一定的局限性。蟻群算法自從在著名的旅行商問(wèn)題（TSP）和工件排序問(wèn)題上取得成效以來(lái)，已經(jīng)陸續(xù)用于解決車輛調(diào)度、路徑規(guī)劃、航跡規(guī)劃等問(wèn)題，該算法具備分布式計(jì)算、群體智能、正反饋等優(yōu)點(diǎn)[9]，在三維航跡規(guī)劃中有著廣泛的應(yīng)用[10-11]。

文獻(xiàn)[12]在蟻群算法的啟發(fā)函數(shù)中加入安全航行因素，使得無(wú)人機(jī)能夠有效避開(kāi)障礙，并且考慮了當(dāng)前節(jié)點(diǎn)到目標(biāo)節(jié)點(diǎn)的距離，提高了搜索的方向性，但是此算法在航跡規(guī)劃初期，會(huì)消耗大量的時(shí)間。文獻(xiàn)[13]針對(duì)蟻群算法在尋路初期需要大量時(shí)間來(lái)確定初始可行解的缺點(diǎn)，通過(guò)改變初始信息素的分布，提高了航跡搜索初始階段的搜索效率，但是對(duì)算法整體的搜索效率提升相對(duì)較小。文獻(xiàn)[14]對(duì)初始信息素分布和啟發(fā)函數(shù)都進(jìn)行了改進(jìn)，但是對(duì)于復(fù)雜度較高的規(guī)劃空間，仍存在收斂速度慢、拐點(diǎn)較多、航跡不平滑的問(wèn)題，另外還會(huì)出現(xiàn)較多不必要的拐彎，使得無(wú)人機(jī)在飛行時(shí)會(huì)頻繁地變更方向，不利于無(wú)人機(jī)飛行，本文在文獻(xiàn)[14]的基礎(chǔ)上提出了一種基于改進(jìn)蟻群算法的無(wú)人機(jī)（UAV）三維航跡規(guī)劃方法，改進(jìn)了航跡規(guī)劃的局部搜索策略和初始信息素調(diào)整因子，并在啟發(fā)函數(shù)中加入路徑偏移因子，降低了航跡規(guī)劃空間的復(fù)雜度，提高了蟻群算法的搜索效率和收斂速度。

2 三維空間建模

在基于蟻群算法的無(wú)人機(jī)三維航跡規(guī)劃問(wèn)題中，大多數(shù)研究是針對(duì)較小的全局搜索空間（20×20 的網(wǎng)格）進(jìn)行，網(wǎng)格分辨率一般為1 km[14-15]，這樣規(guī)劃的航跡精度太低。如果利用高分辨率DEM 數(shù)字地圖（90 m 分辨率），20×20的網(wǎng)格只有1.8 km2，這與無(wú)人機(jī)實(shí)際的飛行范圍相差太大，沒(méi)有實(shí)用性，因此需要增加網(wǎng)格數(shù)量。文中利用90 m 分辨率的地圖，網(wǎng)格數(shù)量擴(kuò)大到100×100，也就是9 km2的地圖中規(guī)劃航跡。

2.1 空間劃分

航跡規(guī)劃空間被視為一個(gè)三維立方體空間ABCDEFGH，如圖1 所示，在三維空間中建立直角坐標(biāo)系O-XYZ，平面ABCD位于XOY平面上，A位于坐標(biāo)原點(diǎn)。根據(jù)DEM的特性，可以將規(guī)劃空間沿著Z軸方向劃分成l（通過(guò)式（1）計(jì)算）個(gè)平面，每個(gè)平面又被劃分為n×m的網(wǎng)格，這樣規(guī)劃空間被劃分成n×m×l個(gè)網(wǎng)格。

其中Vc為無(wú)人機(jī)最大爬升率，V為無(wú)人機(jī)最小巡航速度，Δx為DEM 分辨率，z(i,j)為ABCD平面上每個(gè)網(wǎng)格對(duì)應(yīng)的高度值，m和n由選取的DEM決定。

圖1 三維空間劃分示意圖

2.2 局部搜索策略

無(wú)人機(jī)在三維空間中飛行節(jié)點(diǎn)的選取由約束條件確定，該約束一般包括：航跡距離、最小航跡段、最大拐彎角、最大爬升角等[16-17]。一般基于蟻群算法的航跡規(guī)劃問(wèn)題中，局部搜索空間為26個(gè)節(jié)點(diǎn)（三維）或8個(gè)節(jié)點(diǎn)（二維）且搜索步長(zhǎng)固定為1[18-19]，這種搜索策略當(dāng)全局搜索空間的柵格數(shù)量增加時(shí)，搜索時(shí)間就會(huì)大大增加。本文算法在DEM 數(shù)字地圖中，根據(jù)無(wú)人機(jī)飛行節(jié)點(diǎn)的約束條件，確定搜索步長(zhǎng)。

圖2 所示為節(jié)點(diǎn)擴(kuò)展圖，在XOY平面內(nèi)，A為無(wú)人機(jī)飛行航跡的上一個(gè)節(jié)點(diǎn)，O為當(dāng)前節(jié)點(diǎn)，BCD為待擴(kuò)展節(jié)點(diǎn)，被稱作局部搜索空間，R為無(wú)人機(jī)轉(zhuǎn)彎半徑，假設(shè)A-O-B為已規(guī)劃的航跡，由于無(wú)人機(jī)機(jī)動(dòng)性能的約束，在E-O-F段的實(shí)際飛行軌跡為，因此最小航跡為2×|EO|，Ф為轉(zhuǎn)彎角度，根據(jù)幾何關(guān)系tan(Ф2)=|EO|/R，因此可得最小航跡為：

圖2 節(jié)點(diǎn)擴(kuò)展示意圖

已知某無(wú)人機(jī)的最大轉(zhuǎn)彎角度Фmax=45°，最小轉(zhuǎn)彎半徑Rmin=200 m，則最小航跡Lmin=165.67 m，DEM數(shù)字高程數(shù)據(jù)的分辨率為Δx=Δy=92.662 4 m，因此在一個(gè)平面內(nèi)節(jié)點(diǎn)的選取分為圖3（a）所示5種情況，箭頭所指方向?yàn)轱w行方向。根據(jù)不同的無(wú)人機(jī)參數(shù)和地形分辨率，可確定當(dāng)前節(jié)點(diǎn)和待擴(kuò)展節(jié)點(diǎn)的位置關(guān)系U與待擴(kuò)展節(jié)點(diǎn)的集合V的映射關(guān)系為：

式中i表示當(dāng)前節(jié)點(diǎn)與待擴(kuò)展節(jié)點(diǎn)的位置關(guān)系情況個(gè)數(shù)。

圖3 待擴(kuò)展節(jié)點(diǎn)選取示意圖

如圖3（b），將待擴(kuò)展節(jié)點(diǎn)沿Z軸方向擴(kuò)展到三維，構(gòu)成局部搜索空間。根據(jù)式（1），三維空間Z軸的分辨率與DEM 數(shù)字高程數(shù)據(jù)的分辨率有關(guān)，通過(guò)圖3（a）所示的節(jié)點(diǎn)選擇情況，如果當(dāng)前節(jié)點(diǎn)在第k個(gè)平面，那么待節(jié)點(diǎn)只能在平面k、k+1、k-1、k+2和k-2內(nèi)選??；局部搜搜空間節(jié)點(diǎn)個(gè)數(shù)為25 個(gè)，搜索步長(zhǎng)為2，因此總體上減少可選節(jié)點(diǎn)的數(shù)量，從而降低航跡搜索空間的復(fù)雜度。

3 蟻群算法

給定圖G=(V,A) ，其中V為頂點(diǎn)集，A為各頂點(diǎn)相互連接組成的邊集，旅行商優(yōu)化問(wèn)題描述為求遍歷所有頂點(diǎn)且距離最短的邊集（每條邊當(dāng)且僅當(dāng)出現(xiàn)一次）的集合[9]。螞蟻依概率在各個(gè)頂點(diǎn)之間轉(zhuǎn)移，在t時(shí)刻，螞蟻k從城市i到城市j的轉(zhuǎn)移概率為：

算法每完成一次迭代后，路徑上的信息素將按照下式進(jìn)行更新：

式中，τij(t+1) 為t+1 時(shí)刻螞蟻在路徑(i,j)上的信息素，ρ為信息素衰減系數(shù)；Δτij表示本次迭代中路徑(i,j)上信息素增量，其初始值為零；表示上一輪結(jié)束的循環(huán)中第k只螞蟻遺留在路徑(i,j)上的信息素。

在Ant-Quantity模型中：

其中，Q為信息素常數(shù)，表示螞蟻遍歷一次所有節(jié)點(diǎn)所釋放的信息素總量；Lk表示螞蟻k遍歷完所有節(jié)點(diǎn)后經(jīng)歷的總路程長(zhǎng)度。

4 基于蟻群算法的三維航跡規(guī)劃

蟻群算法的轉(zhuǎn)移概率由信息素和局部啟發(fā)式函數(shù)構(gòu)成，兩者將航跡長(zhǎng)度作為評(píng)判標(biāo)準(zhǔn)。文獻(xiàn)[20]提出的動(dòng)態(tài)α和β權(quán)重，使初期以局部啟發(fā)信息為重，后期以信息素為重。針對(duì)靜態(tài)α和β權(quán)重，最有效的辦法是改變初始信息素的分布和優(yōu)化啟發(fā)信息，在算法初期能夠兼顧信息素和啟發(fā)信息，在后期主要依據(jù)優(yōu)化的啟發(fā)信息，從而加快算法的收斂速度。

4.1 信息素表示

傳統(tǒng)蟻群算法的初始信息素均勻分布，這樣螞蟻初始時(shí)刻在任意節(jié)點(diǎn)處選擇航跡的概率相等，因此在算法執(zhí)行的初始階段，航跡搜索具有一定盲目性，會(huì)消耗大量時(shí)間，為此在航跡的起點(diǎn)和終點(diǎn)之間建立一條空間直線，通過(guò)三維空間中節(jié)點(diǎn)到該直線的歐氏距離的增加來(lái)改變初始信息素[13]，提高螞蟻在初期的搜索效率。

圖4 中比較了三種衰減函數(shù)對(duì)初始信息素的衰減情況。本文選用y=x-0.5對(duì)初始信息素進(jìn)行衰減，因此初始信息素τ0可表示為：

圖4 三種信息素衰減函數(shù)比較

其中，dijk為三維空間中節(jié)點(diǎn)到起點(diǎn)與終點(diǎn)連線的歐氏距離，為固定的信息素常數(shù)。

4.2 啟發(fā)函數(shù)設(shè)計(jì)

啟發(fā)函數(shù)是影響蟻群算法收斂速度和穩(wěn)定性的重要因素，啟發(fā)函數(shù)的設(shè)計(jì)直接影響著蟻群算法的性能。在單一航跡規(guī)劃中，需要考慮安全性、航跡約束條件[21]。局部搜索策略中考慮了最小航跡段長(zhǎng)度、最大拐彎角、最大爬升角，因此，啟發(fā)函數(shù)中考慮了安全性和航跡距離，另外本文引入路徑偏移因子，進(jìn)一步約束無(wú)人機(jī)飛行節(jié)點(diǎn)的選擇，使所選擇的節(jié)點(diǎn)盡可能靠近起點(diǎn)和終點(diǎn)的連線上。因此本文引入安全啟發(fā)因子、距離啟發(fā)因子和路徑偏移因子。

（1）安全啟發(fā)因子

為了保障無(wú)人機(jī)的飛行安全，使無(wú)人機(jī)避開(kāi)所有的障礙及威脅，本文引入了安全啟發(fā)因子S。安全啟發(fā)因子表示無(wú)人機(jī)的飛行高度是否大于三維地圖的高度。通過(guò)式（8）計(jì)算：

其中，zk表示待擴(kuò)展節(jié)點(diǎn)的高度，mapij表示待擴(kuò)展節(jié)點(diǎn)對(duì)應(yīng)于三維地圖上的高度。

（2）距離啟發(fā)因子

為了使規(guī)劃的航跡最短，引入距離啟發(fā)因子D。在圖3（a）所示的待擴(kuò)展節(jié)點(diǎn)的選取情況中，盡可能選取航跡最短的節(jié)點(diǎn)，但航跡最短的節(jié)點(diǎn)未必靠近終點(diǎn)，因此綜合考慮當(dāng)前節(jié)點(diǎn)到待擴(kuò)展節(jié)點(diǎn)的距離|pi pi-1|和待擴(kuò)展節(jié)點(diǎn)到終點(diǎn)的距離|pG pi|，所以D通過(guò)下式計(jì)算：

其中，w∈( 0,1) 。pi表示待擴(kuò)展節(jié)點(diǎn)，pi-1表示當(dāng)前節(jié)點(diǎn)，pG表示目標(biāo)節(jié)點(diǎn)。

（3）路徑偏移因子

如果只依靠距離啟發(fā)因子約束航跡長(zhǎng)度，航跡會(huì)有很多折點(diǎn)，有些研究中引入航跡平滑[15]，對(duì)折點(diǎn)進(jìn)行平滑，以滿足無(wú)人機(jī)的機(jī)動(dòng)性，但是對(duì)于較大的拐彎，平滑效果不佳。本文引入路徑偏移因子，讓航跡上所有的點(diǎn)盡可能靠近起點(diǎn)和終點(diǎn)的連線上，減少拐彎。

在圖5 中，假設(shè)起點(diǎn)為pS，p為搜索空間的一點(diǎn)，則p到直線l的距離M為：

圖5 空間點(diǎn)線距離示意圖

安全啟發(fā)因子和距離啟發(fā)因子的共同作用，能讓無(wú)人機(jī)在三維全局搜索空間中選擇安全無(wú)碰撞、全局航跡最優(yōu)的節(jié)點(diǎn)；而路徑偏移因子將節(jié)點(diǎn)約束到起點(diǎn)和終點(diǎn)的連線附近，平滑航跡，減少拐點(diǎn)和拐彎，加快搜索速度。因此本文蟻群算法的啟發(fā)函數(shù)將綜合以上三個(gè)啟發(fā)因子：

4.3 信息素更新

本文采用局部信息素更新和全局信息素更新相結(jié)合的方式對(duì)信息素進(jìn)行更新。

（1）局部信息素更新

每一代螞蟻的個(gè)體經(jīng)過(guò)一個(gè)節(jié)點(diǎn)后，對(duì)該節(jié)點(diǎn)的信息素進(jìn)行衰減，保證其他個(gè)體有更大的機(jī)率訪問(wèn)其他節(jié)點(diǎn)，避免算法陷入局部最優(yōu)。更新公式為：

式中，μ為信息素衰減系數(shù)，是一個(gè)介于(0,1)的參數(shù)，τ0是節(jié)點(diǎn)的初始信息素。

（2）全局信息素更新

一旦所有螞蟻找到一條航路規(guī)劃問(wèn)題的可行解，必須對(duì)信息素做一遍全面更新[22]，更新規(guī)則如下：

式中，ρ表示信息素衰減系數(shù)，Δτijk表示所有螞蟻在航跡上所有節(jié)點(diǎn)(i,j,k)的信息素增量，計(jì)算公式如下：

式中，表示第m只螞蟻在節(jié)點(diǎn)(i,j,k)上的信息素，計(jì)算公式如下：

式中，Q為信息素常數(shù)，表示螞蟻遍歷一次所有節(jié)點(diǎn)所釋放的信息素總量，Lm為一次迭代結(jié)束后的最優(yōu)航跡長(zhǎng)度。

4.4 算法步驟

改進(jìn)蟻群算法的算法步驟如下：

（1）環(huán)境建模，首先將原始的DEM數(shù)字高程數(shù)據(jù)裁剪為無(wú)人機(jī)的實(shí)際飛行空間，導(dǎo)出.xyz 格式數(shù)據(jù)；根據(jù)公式（1）量化地圖高度信息，將無(wú)人機(jī)的實(shí)際飛行空間構(gòu)建成適合蟻群算法的航跡規(guī)劃空間。

（2）初始信息素分配，根據(jù)公式（7），對(duì)均勻分布的初始信息素進(jìn)行重分配。

（3）蟻群算法各參數(shù)初始化并將所有螞蟻置于起點(diǎn)。

（4）根據(jù)公式（4）選擇下一節(jié)點(diǎn)，根據(jù)公式（12）進(jìn)行局部信息素更新。

（5）當(dāng)所有螞蟻完成一次迭代之后，根據(jù)公式（13）進(jìn)行全局信息素更，否則返回步驟（4）。

（6）判斷是否達(dá)到最短的航跡長(zhǎng)度，若是，則輸出最優(yōu)航跡，否則，返回步驟（3）。

4.5 算例分析

為驗(yàn)證改進(jìn)蟻群算法的可行性與有效性，本文采用PC 機(jī)（操作系統(tǒng)：Win10，CPU：i7-8750H，內(nèi)存：8 GB），基于MATLAB 仿真平臺(tái)進(jìn)行對(duì)比實(shí)驗(yàn)。規(guī)劃空間為101×101×44 的網(wǎng)格，分辨率為92.662 4 m，蟻群算法的參數(shù)在經(jīng)典值范圍選取：ρ=0.2 ，μ=0.2 ，α=2.5 ，β=4；螞蟻個(gè)數(shù)m=50。

對(duì)于文獻(xiàn)[14]蟻群算法和本文蟻群算法設(shè)置最大迭代次數(shù)為300 次，在9 種地形中分別進(jìn)行50 次實(shí)驗(yàn)，50次實(shí)驗(yàn)的平均航跡長(zhǎng)度如表1所示，圖6為地形1、地形4和地形7中10次實(shí)驗(yàn)的搜索結(jié)果，圖7為收斂曲線。

表1 航跡長(zhǎng)度比較km

圖6 蟻群算法搜索結(jié)果圖

圖7 收斂曲線圖

從圖6（a）、（b）、（e）中可以看出，文獻(xiàn)[14]蟻群算法所規(guī)劃的航跡在水平方向出現(xiàn)了很多拐點(diǎn)和較大的拐彎，這些拐點(diǎn)和拐彎在直觀上是不必要的，這使得無(wú)人機(jī)在實(shí)際飛行過(guò)程中，會(huì)較多地變更方向，增加油耗且航跡并非最優(yōu)；本文蟻群算法所規(guī)劃的航跡基本沿著地形趨勢(shì)，落在起點(diǎn)和終點(diǎn)的連線附近，基本沒(méi)有出現(xiàn)明顯的拐點(diǎn)和較大的拐彎，航跡比較平滑，說(shuō)明在啟發(fā)函數(shù)中的路徑偏移因子的作用下，螞蟻選擇航跡的隨機(jī)性降低，將選擇的航跡節(jié)點(diǎn)約束到起點(diǎn)到終點(diǎn)的連線附近，從而減少拐點(diǎn)，使得航跡比較平滑；拐點(diǎn)的出現(xiàn)也說(shuō)明當(dāng)全局搜索空間增加時(shí)，文獻(xiàn)[14]蟻群算法容易陷入局部最優(yōu)，而本文算法全局尋優(yōu)能力較強(qiáng)，避免陷入局部最優(yōu)。從表1 的航跡長(zhǎng)度比較可以看出，本文算法規(guī)劃的航跡更短，在同樣的迭代次數(shù)下，平均縮短24.08%。

從圖7 中可以看出，雖然文獻(xiàn)[14]蟻群算法在前20次迭代中，收斂比較快，但是整個(gè)收斂過(guò)程比較慢，當(dāng)?shù)?60 次時(shí)又出現(xiàn)了快速的收斂，因此收斂過(guò)程不穩(wěn)定。本文蟻群算法在搜索的初始階段，能夠穩(wěn)定較快地向最優(yōu)解靠近，這是由于初始信息素調(diào)整因子的改進(jìn)，結(jié)合路徑偏移因子的約束，本文蟻群算法在收斂速度上有很大提升。50 次實(shí)驗(yàn)在9 種地形中的平均耗時(shí)如表2 所示，可以看出本文蟻群算法運(yùn)行時(shí)間較文獻(xiàn)[14]蟻群算法平均減少11.56%，說(shuō)明改進(jìn)的搜索策略發(fā)揮了重要作用，雖然在啟發(fā)函數(shù)中增加了路徑偏移因子，增加了算法的復(fù)雜度，但是總體上算法搜索效率有所提升，縮短了時(shí)間。表3 為最優(yōu)航跡的節(jié)點(diǎn)個(gè)數(shù)，由于局部搜索策略的改進(jìn)，本文算法所規(guī)劃的航跡節(jié)點(diǎn)個(gè)數(shù)大約是文獻(xiàn)[14]蟻群算法所規(guī)劃航跡節(jié)點(diǎn)個(gè)數(shù)的一半，說(shuō)明了本文蟻群算法搜索效率提升較為明顯。

從圖7 中可以看出，本文蟻群算法只要大約150 次左右就可以基本達(dá)到收斂，而文獻(xiàn)[14]蟻群算法則需要迭代300次以上。為了驗(yàn)證文獻(xiàn)[14]蟻群算法達(dá)到收斂條件時(shí)的迭代次數(shù)，認(rèn)為最短航跡不再發(fā)生變化時(shí)，算法收斂完成，結(jié)果如圖8，從圖中可以看出，文獻(xiàn)[14]蟻群算法需要迭代340次左右才能達(dá)到收斂。

圖8 文獻(xiàn)[14]蟻群算法收斂曲線圖

5 結(jié)束語(yǔ)

本文研究了基于蟻群算法的無(wú)人機(jī)（UAV）三維航跡規(guī)劃方法，并對(duì)局部搜索策略、初始信息素調(diào)整因子和啟發(fā)函數(shù)進(jìn)行改進(jìn)。在三維環(huán)境中進(jìn)行的實(shí)驗(yàn)結(jié)果表明，本文蟻群算法搜索得到的航跡長(zhǎng)度、收斂速度、時(shí)間效率和迭代次數(shù)比文獻(xiàn)[14]蟻群算法都得到提高，在實(shí)際地形中能達(dá)到良好的航跡規(guī)劃效果。航跡規(guī)劃所面臨的環(huán)境存在著復(fù)雜性和多變性，實(shí)際應(yīng)用中靠一種算法很難得到理想的結(jié)果，因此多算法航跡規(guī)劃有待進(jìn)一步深入研究。

表2 平均耗時(shí)比較s

表3 航跡節(jié)點(diǎn)個(gè)數(shù)