程 琪，荊 濤，于志游

（中國民航大學(xué)航空自動化學(xué)院，天津 300300）

利用三次樣條改進蟻群算法的無人機航路規(guī)劃

程 琪，荊 濤，于志游

（中國民航大學(xué)航空自動化學(xué)院，天津 300300）

針對無人機在二維平面自動飛行中轉(zhuǎn)彎角度過大、路徑規(guī)劃困難的問題，研究了蟻群算法在復(fù)雜環(huán)境下航路規(guī)劃中的應(yīng)用，利用鏈接圖簡潔的特點建立空間模型，對無人機的飛行環(huán)境和航跡代價進行了描述，并結(jié)合三次樣條插值函數(shù)與蟻群算法，提出了改進蟻群算法，對無人機飛行路徑進行優(yōu)化，并給出算法軟件流程；利用MATLAB進行了仿真實驗，得出了最優(yōu)的航路，算法具有較好的穩(wěn)定性和魯棒性，對軌跡中不可飛的尖角進行了平滑處理，使得航路為曲線軌跡，滿足無人機工作的性能要求，減少無人機在飛行中的代價損耗，驗證了該優(yōu)化算法在無人機航路規(guī)劃中的可行性。

蟻群算法；三次樣條插值函數(shù)；航路規(guī)劃；鏈接圖；Dijkstra算法

0 引言

隨著物流業(yè)的快速發(fā)展，無人機低成本和三維飛行能力使其在貨物投遞方面優(yōu)于人工投遞，其中無人機在已知固定區(qū)域進行最優(yōu)航路規(guī)劃（Path Planning）是無人機任務(wù)規(guī)劃中的重要內(nèi)容。

目前較常見的航路規(guī)劃算法有Dijkstra算法，A*算法、人工勢場法等，其中Dijkstra算法是一種經(jīng)典的單源點的路徑規(guī)劃算法，具有簡單便捷的特點，易于無人機軟件實現(xiàn)。A*算法可以保證距離上的最短，但是無法兼顧例如轉(zhuǎn)彎半徑等其他性能指標。蟻群算法（Ant Algorithm）是一種基于種群尋優(yōu)的概率搜索算法，是眾多螞蟻協(xié)同搜尋后得出一條最優(yōu)路徑，蟻群算法正反饋的特點使其具有較好的魯棒性和全局規(guī)劃能力。三次樣條插值函數(shù)具有二階連續(xù)導(dǎo)數(shù)，可以解決如飛機外形的理論模型、舶體放樣等型值線等要求的二階光滑度［1］，在路徑規(guī)劃中可以作去除尖角處理。

無人機路徑規(guī)劃由于約束條件的復(fù)雜性和任務(wù)的風(fēng)險代價而成為無人機飛行任務(wù)中的難題，目前國內(nèi)外學(xué)者大多采用圖論將復(fù)雜的環(huán)境用幾何圖形構(gòu)建的模型來進行研究，但基于幾何圖形的路徑存在折角或在路徑中有不可飛的尖角，不滿足無人機本身的機動性能。文獻［2］在路徑的折角處利用圓弧擬合，通過構(gòu)造角平分線為輔助線，以固定點為圓心作弧，與折角的兩邊相切，實現(xiàn)圓滑處理，該算法簡單但缺乏整體性，在折角較大的地方很難實現(xiàn)較好的擬合；文獻［3］中利用MATLAB的樣條工具箱和最優(yōu)化工具箱，取相鄰路徑段中點為兩邊節(jié)點，通過序列二次規(guī)劃求得中點節(jié)點的最優(yōu)解，連接3個節(jié)點繪制曲線來擬合路徑中的尖角，但構(gòu)造二次規(guī)劃的函數(shù)過于復(fù)雜，求解過程繁瑣。本文在文獻［2］和文獻［3］的基礎(chǔ)上，針對無人機投遞中對象區(qū)域固定，通過構(gòu)建鏈接圖（MAKLINK空間模型），提出了一種將三次樣條插值函數(shù)和蟻群算法相結(jié)合的改進算法。在Dijkstra算法構(gòu)建的粗略最短路徑基礎(chǔ)上，利用蟻群算法將空間模型中的航路點離散化，指引搜索下一段航路，并通過追趕法求解方程組，整理出三次樣條插值函數(shù)，將蟻群搜索出的航路點帶入三次樣條插值函數(shù)重新求解，繪制曲線，將航路中的尖角平滑，從而滿足無人機飛行的轉(zhuǎn)彎半徑要求，最后在仿真實驗中證明了改進蟻群算法具有較好的可行性。

1 飛行環(huán)境描述

1.1 建立MAKLINK圖

無人機在執(zhí)行空中投遞任務(wù)時，飛行高度可近似為保持不變，認為無人機在飛行過程中保持恒速飛行。定義環(huán)境中橫向X方向和縱向Y方向，并以一定單位量m進行等量劃分，則可以得到（X／m）×（Y／m）的網(wǎng)格圖。在網(wǎng)格中標注固定區(qū)域的頂點坐標，并用鏈接線相連，就構(gòu)成了威脅區(qū)域，為保證無人機能夠成功完成飛行任務(wù)，需要在航路規(guī)劃中規(guī)避威脅區(qū)域。以多邊形的威脅區(qū)域為基準，將網(wǎng)格區(qū)域劃分為N個包括威脅區(qū)域在內(nèi)的凸多邊形，相鄰區(qū)域之間用鏈接線隔開，用迭代法計算出鏈接線的中點坐標。投遞任務(wù)中設(shè)定飛行起點S、終點T和中途停留點P，依據(jù)設(shè)定點所屬區(qū)域的劃分，相同區(qū)域內(nèi)的鏈接線中點之間相互連接，構(gòu)成MAKLINK圖，其中的鏈接線組成了可選航路段集合，各中點和設(shè)定點組成了可選航路點集合。在MAKLINK圖中，起點S連接的第一個航路段規(guī)定無人機的初始航向，終點T連接的最后一個航路段規(guī)定無人機的終止航向。

1.2 航跡代價描述

由于飛行環(huán)境的多樣性，無人機在完成飛行任務(wù)的過程中需要考慮不同因素對航跡優(yōu)劣程度的影響［4］。根據(jù)威脅區(qū)域在MAKLINK圖中的分布以及可選航路段的特征，可以將航跡規(guī)劃的優(yōu)劣程度用航跡代價來表征，航跡代價J［5］可分為受威脅區(qū)域影響的威脅代價Jthreat和受航路段距離影響的路程代價Jdist。

二維環(huán)境模型中的威脅區(qū)域均為多邊形，無人機在飛行過程中距離威脅區(qū)域邊界越近，受威脅的程度越大，威脅代價與航路段上的點到威脅區(qū)域邊界的垂直距離d［（x，y），（m，n）］的成反比：

無人機在勻速飛行的過程中，會產(chǎn)生燃油消耗，隨著飛行距離的增加，燃油消耗也相應(yīng)地增加，因此路程代價與航路段的長度Li成正比：

綜合考慮航路軌跡的優(yōu)劣程度，航跡代價J表示為：

式中，k1和k2分別代表威脅代價和路程代價的權(quán)重，且k1＋k2＝1；M為航路段的數(shù)量，N為一條航路段上計算威脅代價的航路點的數(shù)量；在d（（x，y），（m，n））中，（x，y）為航路段上點的坐標，過（x，y）點作與航路段垂直的輔助線，（m，n）為輔助線與威脅區(qū)域邊界的交點。

利用航跡代價將MAKLINK圖構(gòu)造成帶權(quán)無向網(wǎng)絡(luò)圖G＝（V，E，W），其中V為所有航路點的集合，E為每段航路的集合，W為每段航路的權(quán)值：

2 Dijkstra算法規(guī)劃粗略路徑

在飛行區(qū)域的鏈接圖中利用Dijkstra算法找出粗略的最短路徑［6］，Dijkstra算法基本步驟如下：

1）在帶權(quán)無向網(wǎng)絡(luò)圖中，把集合V分為集合S和集合T，S存放已標記點，T存放未標記點。

2）初始化時，S中元素為s，T中元素為s以外的所有點。

3）源點s到集合T中的所有航路點的最小權(quán)值總和記錄在數(shù)組dist中，i為數(shù)組dist中最小值dist［i］的航路點，把點i從集合T中取出放入集合S中。

4）更新數(shù)組dist中的值。

5）當所有的航路點都被標記時，即S中包含所有的航路點時算法結(jié)束，否則繼續(xù)步驟3）。當算法結(jié)束時，可以求得一條航跡代價最小的粗略最短路徑。

3 改進蟻群算法

利用經(jīng)典蟻群算法對粗略最短路徑進行優(yōu)化，通過改變航路點的分布，搜索出新的航路點，并通過三次樣條插值函數(shù)對新的航路點重新求解，繪制曲線對折線段的尖角進行平滑處理。

3.1 基本蟻群算法

已知航路點對應(yīng)的鏈接線為leni（i＝1，2，...，d），對leni按照一定的比例系數(shù)k進行分割，則增加了可能航路點的數(shù)量，便于Pi在leni上進行局部調(diào)整，從而得到新的航路點：

對各段鏈接線進行離散化處理，即進行固定距離劃分［7］，劃分長度為ζ，每條鏈接線的劃分數(shù)為：

當前鏈接線上航路點i到下條鏈接線上航路點j的概率Pi，j（t）［8］為：

式中，τi，j（t）為航路點i到航路點j之間的信息素濃度；ηi，j（t）為啟發(fā)函數(shù)，表示螞蟻從航路點i轉(zhuǎn)移到航路點j的期望程度，α表示信息素濃度對螞蟻選擇路徑所起的作用，β為啟發(fā)函數(shù)重要程度因子，其值越大，表示螞蟻會以較大的概率轉(zhuǎn)移到距離短的航路點。I表示螞蟻待訪問航路點的集合。

每一次選擇節(jié)點后，對信息素進行更新：

其中0≤ρ≤1，τ0為信息素的初始值。當一次完整的搜尋結(jié)束，同樣要對信息素進行更新，公式同上，將τ0替換為1／Lmin，其中Lmin為最短路徑值。

3.2 三次樣條插值

定義被插函數(shù)y＝f（x），自變量范圍x∈［a，b］，取n個節(jié)點xi∈［a，b］（i＝1，2，...，n），f（x）在這n個節(jié)點上對應(yīng)的函數(shù)值為yi（i＝1，2，...，n）。若存在一個多項式函數(shù)P（x）使得P（x）＝y(tǒng)i，其中i＝1，2，...，d，則P （x）成為這n個節(jié)點xi的插值函數(shù)。若P（x）滿足在區(qū)間［a，b］內(nèi)有二階導(dǎo)數(shù)，并且在每個相鄰節(jié)點的區(qū)間［xi，xi＋1］（i＝1，2，...，n－1）內(nèi)P（x）為三次多項式，則稱P （x）為（xi，yi）的三次樣條插值函數(shù)。

3.3 三次樣條改進蟻群算法

3.3.1 分段三次插值［9］

函數(shù)在節(jié)點處的導(dǎo)數(shù)已知時，利用分段插值可寫出每個相鄰節(jié)點區(qū)間［xi，xi＋1］（i＝1，2，...，n－1）的樣條函數(shù)：

式中，mi為節(jié)點處的一階導(dǎo)數(shù)值。

3.3.2 構(gòu)造方程組

三次樣條插值函數(shù)滿足在規(guī)定區(qū)間內(nèi)具有連續(xù)二階導(dǎo)數(shù)，因此對P（x）求二階導(dǎo)數(shù)可得到P″i（x），并使得，通過整理對節(jié)點建立方程組：

通過計算，可求出一組mi（i＝1，2，...，d），從而根據(jù)mi確定三次樣條插值函數(shù)。

3.3.3 平滑曲線

基本蟻群算法依據(jù)搜索概率Pi，j（t）確定了每條鏈接線上的當前航路點POINT（λ），選取從起點S到終點T之間的所有航路段中點Z（λ＋1），由Z（λ＋1，1）構(gòu)成節(jié)點（xi，yi）的橫坐標向量組，Z（λ＋1，2）構(gòu)成節(jié)點（xi，yi）的縱坐標向量組，利用三次樣條插值函數(shù)對路徑重新構(gòu)圖，得到平滑曲線，實現(xiàn)路徑最短化，又將航路中不可飛的尖角平滑處理，滿足無人機的轉(zhuǎn)彎機動性能。

3.3.4 改進蟻群算法

該算法流程如圖1所示。

圖1 改進蟻群算法流程圖

4 仿真實驗

以校園內(nèi)建筑平面圖為例，對無人機執(zhí)行任務(wù)來說，威脅區(qū)域主要是教學(xué)樓所在區(qū)域，依據(jù)此方法，在校園內(nèi)某一范圍內(nèi)的地形平面圖中，假設(shè)平面中教學(xué)樓為多邊形區(qū)域，取一定高度H下的等高二維平面圖，可得無人機飛行環(huán)境的威脅分布圖。在Matlab2013中構(gòu)造MAKLINK圖，對算法進行仿真實驗，試驗中對經(jīng)典蟻群算法的參數(shù)選擇如下：

1）每條鏈路均離散化為10個小路段；

2）螞蟻數(shù)量m＝10，循環(huán)次數(shù)為500次。

經(jīng)典蟻群算法對粗略的最短路徑進行優(yōu)化，隨著螞蟻循環(huán)次數(shù)的增加，航路路徑值和航跡代價值逐漸減小，優(yōu)化路徑如圖2所示。圖2中基本蟻群算法搜索出的最優(yōu)航路和Dijkstra算法的粗略航路可看出明顯對比。

圖2 基本蟻群算法的優(yōu)化航路

改進蟻群算法的程序包括參數(shù)初始化、蟻群搜索、信息素更新、追趕法求解方程組、繪制路徑圖等5個部分，將基本蟻群算法搜索出的新的航路點帶入三次樣條插值函數(shù)進行重新求解，通過增加區(qū)間節(jié)點數(shù)量，繪制出新的航路路徑，如圖3所示，圖中標準的路徑分別為Dijkstra算法的粗略航路、基本蟻群算法搜索出的航路以及三次樣條插值函數(shù)改進的蟻群算法繪制出的最優(yōu)航路。

圖3 改進蟻群算法的最優(yōu)航路

在關(guān)鍵航路點處的部分參數(shù)如表1所示，利用基本蟻群算法對粗略路徑的路徑值進行優(yōu)化，航跡代價明顯降低，部分航路段折角減小，經(jīng)過改進蟻群算法的優(yōu)化，航路段折角被曲線代替，軌跡得到平滑處理，且航跡代價保持最優(yōu)狀態(tài)。

可見改進后的蟻群算法在優(yōu)化路徑值的同時，解決了幾何模型中存在路徑尖角的問題，利用三次樣條曲線平滑航路中的尖角，使無人機具有平滑的飛行軌跡，滿足無人機本身在轉(zhuǎn)彎時的機動性。

5 結(jié)論

無人機航路規(guī)劃是在較復(fù)雜的飛行環(huán)境下，受多種約束條件限制的飛行任務(wù)規(guī)劃，本文利用了MAKLINK圖簡潔明了的特點，通過以校園平面圖為基礎(chǔ)建立二維空間模型，對無人機的飛行環(huán)境進行描述，并定義了無人機在飛行過程中的航跡代價。結(jié)合了三次樣條插值函數(shù)和蟻群算法，提出改進蟻群算法，利用蟻群算法的隨機搜索的特點對飛行模型中的航路點進行離散化，使得蟻群能夠進行全局搜索，也保證航路軌跡的最優(yōu)和穩(wěn)定性，并利用三次樣條插值函數(shù)對航路點坐標重新取值，增加區(qū)間內(nèi)節(jié)點數(shù)量，進行曲線繪制，對航路中不可飛的尖角進行平滑處理。從MATLAB仿真實驗的結(jié)果來看，該方法穩(wěn)定性好，航路軌跡較為平滑，各個航路點的轉(zhuǎn)彎半徑滿足無人機的性能要求，減少無人機在飛行中的代價損耗，在最短的航程條件下完成飛行任務(wù)，對今后無人機航路規(guī)劃以及其他二維空間路徑規(guī)劃提供參考。

［1］張德豐，等.MATLAB數(shù)值分析與應(yīng)用［M］.北京：國防工業(yè)出版社，2009.

［2］曾 佳，申功璋，等.一種無人機平滑飛行軌跡規(guī)劃方法［J］.系統(tǒng)仿真學(xué)報，2008，20：470-473.

［3］符小衛(wèi)，高曉光，等.一種無人機路徑規(guī)劃算法研究［J］.系統(tǒng)仿真學(xué)報，2004，16（1）：20-21.

［4］華珊珊.無人機航路自動規(guī)劃優(yōu)化方法研究與仿真［J］.計算機仿真，2013，30（4）：45-48.

［5］李 猛，王道波，柏婷婷，等.基于蟻群優(yōu)化算法和人工勢場的無人機航跡規(guī)劃［J］.應(yīng)用科學(xué)學(xué)報，2012，30（2）：216.

［6］李元臣，劉維群.基于Dijkstra算法的網(wǎng)絡(luò)最短路徑分析 ［J］.微計算機應(yīng)用，2004，25（3）：295-298.

［7］張美玉，黃 翰，郝志峰，等.基于蟻群算法的機器人路徑規(guī)劃［J］.計算機工程與應(yīng)用，2005，41（9）：34-37.

［8］Glabowski M，Musznicki B，Nowak P，et al.An algorithm for finding shortest path tree using ant colony optimization metaheuristic ［J］.Advances in Intelligent Systems and Computing，2014，233：317-326.

［9］馬昌鳳.現(xiàn)代數(shù)值分析［M］.北京：國防工業(yè)出版社，2013.

［10］Khanmohammadi S，Zarrin R S.Intelligent path planning for rescue robot［J］.World Academy of Science，Engineering and Technology，2011，5（7）：607-612.

［11］Sullivan T A，Van J D.Multi-objective，multidomain genetic optimization of a hydraulic rescue spreader［J］.Mechanism and Machine Theory，2014，80：35-51.

UAV Path Planning Based on Ant Colony Optimization Improved by Cubic Spline

Cheng Qi，Jing Tao，Yu Zhiyo u

（College of Aeronautical Automation，Civil Aviation University of China，Tianjin 300300，China）

Focus on issues as the sharp of turning angle and difficulty of path planning in Automatic UAV flight in the two－dimensional plane，the paper studies the ant colony algorithm in a complex environment of UAV route planning，and establishes a space model to describe the flight environment of UAV in the use of the MAKLINK graph for the concise characteristics.The cost of the flight environment and track the UAV has been described in conjunction with cubic spline interpolation function and ant colony algorithm to optimize the UAV flight path，and algorithm software flow is shown.The optimal route which has better stability and robustness by using MATLAB simulation experiments is got，and the track route is relatively smooth，turning angle of each waypoint can meet the performance requirements of UAV，the cost of the UAV fighting is reduced corresponding，it is verified that the optimization algorithm of UAV route planning is feasible.

ant colony algorithm；Cubic spline interpolation function；path planning；MAKLINK Graph；Dijkstra algorithm

1671-4598（2016）08-0272-03

10.16526／j.cnki.11－4762／tp.2016.08.074

：TP18

：A

2016-02-29；

：2016-03-27。

國家自然科學(xué)基金（61405246）；天津市創(chuàng)新創(chuàng)業(yè)項目（201510059084）。

程 琪（1994-），男，江蘇宿遷人，大學(xué)生，主要從事電氣工程及其自動化方向的研究。