程 琪，荊 濤，于志游

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

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

程 琪，荊 濤，于志游

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

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

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

0 引言

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

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

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

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

1.1 建立MAKLINK圖

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

1.2 航跡代價(jià)描述

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

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

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

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

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

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

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

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

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

2）初始化時(shí)，S中元素為s，T中元素為s以外的所有點(diǎn)。

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

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

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

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

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

3.1 基本蟻群算法

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

對(duì)各段鏈接線進(jìn)行離散化處理，即進(jìn)行固定距離劃分［7］，劃分長(zhǎng)度為ζ，每條鏈接線的劃分?jǐn)?shù)為：

當(dāng)前鏈接線上航路點(diǎn)i到下條鏈接線上航路點(diǎn)j的概率Pi，j（t）［8］為：

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

每一次選擇節(jié)點(diǎn)后，對(duì)信息素進(jìn)行更新：

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

3.2 三次樣條插值

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

3.3 三次樣條改進(jìn)蟻群算法

3.3.1 分段三次插值［9］

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

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

3.3.2 構(gòu)造方程組

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

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

3.3.3 平滑曲線

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

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

該算法流程如圖1所示。

圖1 改進(jìn)蟻群算法流程圖

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

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

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

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

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

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

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

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

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

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

5 結(jié)論

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

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

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

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

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

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

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

［7］張美玉，黃 翰，郝志峰，等.基于蟻群算法的機(jī)器人路徑規(guī)劃［J］.計(jì)算機(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］.北京：國(guó)防工業(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。

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

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