基于蟻群算法的四旋翼航跡規(guī)劃

莫宏偉，馬靖雯

(哈爾濱工程大學(xué) 自動(dòng)化學(xué)院，黑龍江 哈爾濱 150001)

?

基于蟻群算法的四旋翼航跡規(guī)劃

莫宏偉，馬靖雯

(哈爾濱工程大學(xué) 自動(dòng)化學(xué)院，黑龍江 哈爾濱 150001)

摘要：由于四旋翼無人機(jī)(UAV)自身的特點(diǎn)和其復(fù)雜的飛行環(huán)境，考慮到全球定位系統(tǒng)(GPS)定位的精度，在環(huán)境模型方面，建立了一個(gè)基于高程圖的三維環(huán)境模型，減小了碰到障礙物的概率。在規(guī)劃算法方面，大部分現(xiàn)有的路徑規(guī)劃算法只能規(guī)劃二維平面路徑，而一般的三維規(guī)劃算法，大多數(shù)運(yùn)算算法復(fù)雜，需要很大的存儲(chǔ)空間，同時(shí)難以進(jìn)行全局路徑規(guī)劃。該蟻群算法具有分布式計(jì)算、群體智能等優(yōu)勢(shì)，在路徑規(guī)劃上有很大潛力。但在應(yīng)用基本三維蟻群算法進(jìn)行航跡搜索時(shí)，兩平面直接相連容易使航跡直接穿過障礙物，并且搜索出的航跡節(jié)點(diǎn)較多，適應(yīng)度值過大。針對(duì)這兩個(gè)問題對(duì)算法做出了改進(jìn)，提出了變主方向搜索策略和簡化航跡策略。仿真實(shí)驗(yàn)證明改進(jìn)后的蟻群算法能夠很好地避開障礙物，減小了路徑長度，提高了搜索效率。

關(guān)鍵詞：四旋翼無人機(jī)；航跡規(guī)劃；三維環(huán)境模型；蟻群算法；變主方向搜索策略；簡化航跡策略

中文引用格式：莫宏偉，馬靖雯. 基于蟻群算法的四旋翼航跡規(guī)劃[J]. 智能系統(tǒng)學(xué)報(bào)， 2016, 11(2): 216-225.

英文引用格式：MO Hongwei， MA Jingwen. Four-rotor route planning based on the ant colony algorithm[J]. CAAI transactions on intelligent systems, 2016, 11(2): 216-225.

UAV三維航跡規(guī)劃區(qū)域廣，搜素空間巨大，所以要想找到一條最優(yōu)的飛行軌跡就必定需要占用較長的收斂時(shí)間，同時(shí)還需要較大的存儲(chǔ)空間，這對(duì)大部分無人機(jī)實(shí)際應(yīng)用來說是不怎么實(shí)際的做法。近些年來，國外一些專家對(duì)航路規(guī)劃算法歸結(jié)出了三類主要的方法：傳統(tǒng)算法，如柵格法、Voronoi圖法；智能優(yōu)化算法，如遺傳算法、蟻群算法；其他算法，如動(dòng)態(tài)規(guī)劃算法。傳統(tǒng)算法對(duì)障礙物要求比較理想化，對(duì)實(shí)際地形規(guī)劃出的結(jié)果影響很大。動(dòng)態(tài)規(guī)劃算法，在局部路徑上可以達(dá)到最優(yōu)及適用于動(dòng)態(tài)地圖，但不能確保全局路徑上也可以實(shí)現(xiàn)。 智能優(yōu)化算法[1-4]，如遺傳算法，是一種依據(jù)生物界的進(jìn)化規(guī)律演化而來的一種方法，它最重要的一個(gè)特點(diǎn)是不受函數(shù)求導(dǎo)的限制，在隱并行性方面和全局搜索方面有很大優(yōu)勢(shì)，在穩(wěn)定性方面也有顯著提高，但算法效率慢，不能適用于動(dòng)態(tài)、真實(shí)的地圖。

我國在航路規(guī)劃算法方面也取得了不少的成果，李翊[5]將動(dòng)態(tài)標(biāo)定遺傳算法、自適應(yīng)遺傳算法和大變異遺傳算法結(jié)合在一起，提出了一種改進(jìn)的遺傳算法，避免了陷入局部最優(yōu)解與“早熟”；王玥等[6]提出了基于降落傘型搜索域的變步長航跡點(diǎn)搜索和帶有威脅信息并歸一化后的代價(jià)函數(shù)，改進(jìn)了A*算法，提高了搜索效率；韓超[7]提出了一種改進(jìn)粒子群的航跡規(guī)劃方法，該方法將UAV的航路規(guī)劃問題通過目標(biāo)轉(zhuǎn)換，形成一個(gè)考慮威脅優(yōu)先，路徑優(yōu)化其次的單目標(biāo)航路優(yōu)化問題，并引入局部搜索改進(jìn)粒子群算法求解該問題的收斂性；文獻(xiàn)[8]提出了采用蟻群算法解決機(jī)器人三維路徑規(guī)劃的問題，通過坐標(biāo)變換對(duì)三維環(huán)境進(jìn)行轉(zhuǎn)換，環(huán)境中障礙物抽象成圓形，然后將原點(diǎn)和終點(diǎn)的空間劃分成三維立體網(wǎng)格，在原點(diǎn)和終點(diǎn)之間的每一個(gè)平面上選定一個(gè)點(diǎn)作為航跡節(jié)點(diǎn)，從而找到有效的路徑，但該方法必須首先進(jìn)行坐標(biāo)變換，有誤差；胡中華[9]不用進(jìn)行坐標(biāo)轉(zhuǎn)換，采用在狀態(tài)轉(zhuǎn)移策略中引入導(dǎo)引因子和設(shè)定最大航跡節(jié)點(diǎn)數(shù)的方式來實(shí)行航跡規(guī)劃，從而解決標(biāo)準(zhǔn)蟻群算法不易找到目標(biāo)節(jié)點(diǎn)的問題。到目前為止，我國在航路規(guī)劃技術(shù)方面的研究正朝著實(shí)用性、實(shí)時(shí)性、智能化的方向發(fā)展。

本文研究四旋翼無人機(jī)航跡規(guī)劃時(shí)對(duì)上述文獻(xiàn)各有借鑒，并將基本蟻群算法做了改進(jìn)，減少了多余航跡節(jié)點(diǎn)，優(yōu)化了航跡。

1基于柵格圖法的環(huán)境模型構(gòu)建

柵格圖法[10-11]，是把自由空間C劃分成一系列規(guī)則的單元格，根據(jù)單元格中是否有障礙物和障礙物的覆蓋面積來判斷。機(jī)器人路徑規(guī)劃中柵格圖法得到廣泛應(yīng)用[12-13]。

在對(duì)柵格單元?jiǎng)澐謺r(shí)有兩種方案：一種是只要單元格內(nèi)有障礙物就判定該單元格是不能通過的；另一種方案是根據(jù)單元格內(nèi)障礙物覆蓋面積來判斷單元格是否可通過，即單元格內(nèi)障礙物占的面積如果大于一定比例就認(rèn)為該單元格是不可通過，否則就認(rèn)為是可以通過的。第二種方案比第一種方案找到最優(yōu)解的可能性高，但無人機(jī)碰到障礙物的可能性也更高，因此選擇第一種方案的更多一些。

在使用柵格圖法進(jìn)行路徑搜索時(shí)需要考慮的一個(gè)重要問題就是分辨率的問題，即每一個(gè)單元格的長寬高值。分辨率過高時(shí)計(jì)算量過大，不符合實(shí)際應(yīng)用；而分辨率過低，規(guī)劃出的最優(yōu)解偏離理論最優(yōu)解過大。

圖1　二維柵格圖Fig.1　Two-dimensional raster

圖2　三維柵格圖Fig.2　Three-dimensional raster

根據(jù)自由空間的維數(shù)，柵格圖法可以分為二維柵格圖法和三維柵格圖法。二維柵格圖是一個(gè)平面圖形如圖1，三維柵格圖是一個(gè)三維模魔方形如圖2。二維柵格圖中，設(shè)X軸最大值為Xmax，最小值為Xmin，X軸劃分單位大小為 XGrid，Y軸最大值為Ymax，最小值為Ymin，Y軸劃分單位大小為YGrid，則二維柵格圖總的網(wǎng)格數(shù)目N為

(1)

對(duì)于三維柵格圖，設(shè)X軸最大值Xmax，最小值Xmin，X軸劃分單位大小為XGrid，Y軸最大值Ymax，最小值 Ymin，Y軸劃分單位大小為 YGrid，Z軸最大值Zmax，最小值Zmin，Z軸劃分單位大小為ZGrid，則二維柵格圖總的網(wǎng)格數(shù)目N為

(2)

對(duì)比式(1)和(2)可以看出二維柵格圖和三維柵格圖計(jì)算量的差別。

本文選用三維柵格圖法來劃分規(guī)劃空間。在使用三維柵格圖作為環(huán)境模型時(shí)就必須考慮到三維柵格的分辨率問題，分辨率既不能太大也不能太小。

考慮三維柵格分辨率時(shí)，首先要考慮的問題就是四旋翼無人機(jī)的體積。要保證四旋翼無人機(jī)在飛行過程中能夠不碰到障礙物，柵格的長、寬、高要大于四旋翼的長、寬、高，另外還要考慮到誤差。本文使用的四旋翼無人機(jī)的旋翼軸長55 cm，起落架高約25 cm。另外一方面，設(shè)定三維柵格分辨率時(shí)還要考慮四旋翼的定位方式。GPS定位是目前最常用的定位方式之一，在四旋翼無人機(jī)上安裝一個(gè)GPS導(dǎo)航模塊就可以通過接收GPS信號(hào)，進(jìn)而判斷四旋翼無人機(jī)的當(dāng)前位置，所以本文選擇使用GPS定位。GPS定位時(shí)，緯度上每變化0.001′約為1.837 m，經(jīng)度上每變化0.001′約為1.281 m，GPS定位時(shí)會(huì)存在1～2 m的誤差。

綜上考慮上述兩個(gè)方面的因素，本文最終選定以緯度值作為橫坐標(biāo)，單位長度為0.002′，約3.674 m；經(jīng)度值作為縱坐標(biāo)，單位長度為0.003′，約3.843 m；以地面障礙物的高度作為豎坐標(biāo)，垂直于海平面，豎坐標(biāo)單位長度為1 m。這樣劃分三維柵格單元，就可以兼顧四旋翼的尺寸，又考慮到了GPS定位的誤差，從而可以減小碰到障礙物的概率?，F(xiàn)以學(xué)校部分地圖為基礎(chǔ)，繪制了基于三維柵格圖的三維環(huán)境地圖。地圖所在的位置緯度起始點(diǎn)為45°46.7839′，終止點(diǎn)為45°46.983 9′；經(jīng)度起始點(diǎn)為126°40.217 2′，終止點(diǎn)為126°40.517 2′；地面水平高度為0。地圖大小為(0.002′×100)×(0.003′×100)×(1m×40)。為方便MATLAB作圖，緯度和經(jīng)度上省略度部分上的值，只寫分位，也就是說緯度方向上起始點(diǎn)和終止點(diǎn)坐標(biāo)分別為46.783 9′和46.983 9′，經(jīng)度方向上起始點(diǎn)和終止點(diǎn)坐標(biāo)分別為40.2172′和40.5172′。繪制的三維環(huán)境的三維圖和投影圖分別如圖3、4所示。

圖3　三維圖Fig.3　Three-dimensional map

2三維蟻群算法

三維蟻群算法解決的問題主要是三維航跡規(guī)劃問題，該模型和基本的蟻群算法模型有很多不同之處。在原蟻群算法及改進(jìn)算法的基礎(chǔ)之上對(duì)其進(jìn)行了變形，在變形時(shí)需要考慮到航跡規(guī)劃的環(huán)境模型。在上文中，已經(jīng)選定了以緯度值作為橫坐標(biāo)，以經(jīng)度值作為縱坐標(biāo)，以高度值作為豎坐標(biāo)的三維柵格圖作為環(huán)境模型。因?yàn)橹苯釉谌S柵格圖中對(duì)所有的柵格進(jìn)行搜索找到最優(yōu)路徑需要的計(jì)算量和計(jì)算時(shí)間很大，所以本文采用的方法并不是對(duì)所有的柵格進(jìn)行搜索，而是對(duì)部分柵格進(jìn)行搜索以找到最優(yōu)航跡。

圖4　投影圖Fig.4　Projection map

2.1算法的數(shù)學(xué)模型

在進(jìn)行路徑搜索時(shí)，已知起始點(diǎn)S所在的柵格的坐標(biāo)值(Xstart,Ystart,Zstart)，終止點(diǎn)所在柵格的坐標(biāo)值(Xend,Yend,Zend)，至于柵格單元的大小則是根據(jù)上文劃分的大小，即XGrid=0.003′，YGrid=0.002′，ZGrid=1 m。航跡規(guī)劃時(shí)需要先選定X方向或Y方向作為四旋翼無人機(jī)運(yùn)行的主方向。選擇主方向的方法為：比較起始點(diǎn)和終止點(diǎn)橫縱坐標(biāo)的變化值大小，即比較(Xstart-Xend)/XGrid和(Ystart-Yend)/YGrid的大小，如果(Xstart-Xend)/XGrid大于(Ystart-Yend)/YGrid，則選擇X方向作為主方向；否則選擇Y方向?yàn)橹鞣较颉?/p>

選定主方向后沿主方向前進(jìn)方向，例如已經(jīng)選定X方向?yàn)橹鞣较?，沿X軸方向從Xstart到 Xend劃分成n=|Xstart-Xend|+1個(gè)平面(如圖5所示)，編號(hào)為Π1，Π2，Π3，…，Πn，那么四旋翼無人機(jī)航跡就分成(n-1) 段。假設(shè)四旋翼無人機(jī)運(yùn)行至第i個(gè)平面 Πi上的一點(diǎn)(Xi,Yi,Zi) 處，那么下一個(gè)運(yùn)行的柵格就在Πi+1上，下一個(gè)柵格坐標(biāo)選擇的具體步驟為：X方向上直接以平面Πi+1的橫坐標(biāo)作為下一個(gè)節(jié)點(diǎn)的橫坐標(biāo)，即新的X坐標(biāo)值為Xi+1；Y方向和Z方向坐標(biāo)值的選擇不是直接選擇的，而是在平面Πi+1選擇沒有障礙物的柵格放入數(shù)組Allowedi中；否則被舍棄。然后從中選擇一個(gè)柵格點(diǎn)作為下一個(gè)運(yùn)行柵格。平面上任意一個(gè)柵格(X,Y,Z)作為下一個(gè)運(yùn)行柵格的概率計(jì)算為

(3)

式中：τ(X,Y,Z)是平面Πi+1上坐標(biāo)為(X,Y,Z)的柵格的信息素值。H(X,Y,Z)是平面Πi+1上坐標(biāo)為(X,Y,Z)的柵格的啟發(fā)函數(shù)，其計(jì)算公式為

(4)

式中：D(X,Y,Z)是當(dāng)前點(diǎn)和(X,Y,Z)的路徑長度，這可以促使螞蟻盡可能選則距離當(dāng)前點(diǎn)最近的點(diǎn)，計(jì)算公式為

(5)

S(X,Y,Z)表示安全性因素，促使螞蟻選擇安全點(diǎn)。當(dāng)前柵格(Xi,Yi,Zi)和(X,Y,Z)是不可連通的，或者柵格(X,Y,Z)內(nèi)有障礙物，則稱該柵格是不可達(dá)的。S(X,Y,Z)的計(jì)算公式為

(6)

式中：Q(X,Y,Z)為柵格(X,Y,Z)到終止柵格(Xend,Yend,Zend)的距離，Q(X,Y,Z)可以促使螞蟻選擇離目標(biāo)柵格更近的柵格，其計(jì)算公式為

(7)

式中：ω1、ω2、ω3是系數(shù)，其大小代表了上述各因素的重要性程度，系數(shù)值越大代表該項(xiàng)越重要；否則說明該項(xiàng)越不重要。

圖5　主方向選取Fig.5　Select the main direction

蟻群算法中螞蟻是根據(jù)信息素濃度來進(jìn)行搜索路徑的，在每走完一段或全部路徑時(shí)，就要對(duì)信息素值進(jìn)行更新，所以信息素初始值設(shè)定和更新對(duì)于蟻群算法是否能夠成功搜索具有重要影響。本文把信息素值存儲(chǔ)在三維空間一系列離散的點(diǎn)中，然后對(duì)這些離散點(diǎn)的信息素值進(jìn)行更新，所以對(duì)于每一個(gè)柵格來說都有一個(gè)信息素值，這個(gè)信息素值就代表了該柵格對(duì)螞蟻的吸引程度，每個(gè)柵格的信息素值在螞蟻經(jīng)過之后都要進(jìn)行更新。

信息素的更新分為局部更新和全局更新兩部分。局部更新是指只要有螞蟻經(jīng)過某柵格，該柵格的信息素值就會(huì)被更新，更新后柵格的信息素值會(huì)被減少，這樣這個(gè)柵格在以后的搜索中被選中的概率就被降低，而相應(yīng)地增加其他未被搜索的柵格被搜索的概率，這樣就可以達(dá)到全局搜素的目的。局部搜索的信息素更新公式為

(8)

式中：ζ表示信息素衰減系數(shù)，τX,Y,Z表示柵格(X,Y,Z)的信息素值。

全局信息素更新是指螞蟻完成一條航跡搜索時(shí)，計(jì)算該路徑的適應(yīng)度值，從現(xiàn)有的搜索到的路徑中選擇出最短的航跡，更新適應(yīng)度值最小的航跡所經(jīng)過的所有柵格的信息素值，信息素更新公式為

(9)

(10)

式中：length(m)表示螞蟻m經(jīng)過的路徑長度；ρ表示信息素?fù)]發(fā)系數(shù)；K為系數(shù)。

2.2算法的主要流程

在充分考慮了評(píng)價(jià)函數(shù)、環(huán)境模型和三維蟻群算法模型這些內(nèi)容之后，下一步需要考慮的就是航跡規(guī)劃實(shí)現(xiàn)的問題。本文中的程序?qū)崿F(xiàn)主要分為參數(shù)初始化設(shè)置、航跡搜索和圖形繪制3個(gè)主要部分，其具體每一步如下所述。

1)參數(shù)初始化設(shè)置

在進(jìn)行航跡規(guī)劃實(shí)現(xiàn)時(shí)，首先要確定的就是各項(xiàng)參數(shù)設(shè)置問題。這些參數(shù)設(shè)置包括起始點(diǎn)的確定、主方向的選取、種群數(shù)的確定、迭代次數(shù)的選擇、航跡搜尋范圍選取、信息素初始化設(shè)置。

①起始點(diǎn)的確定。當(dāng)把四旋翼無人機(jī)放置在規(guī)劃空間中的某一點(diǎn)時(shí)，放置四旋翼的位置不一定恰好就是柵格圖中柵格的位置，這時(shí)就需要把該四旋翼無人機(jī)所在的柵格的柵格坐標(biāo)作為航跡規(guī)劃的起始點(diǎn)。那么，三維柵格地圖原點(diǎn)坐標(biāo)為(XGridstart,YGridstart,ZGridstart)，四旋翼的放置位置(Slat,Slon,Sh)和其所在柵格的柵格坐標(biāo)位置(Xstart,Ystart,Zstart)的關(guān)系為

(11)

(12)

(13)

式中：ceil表示向正無窮方向取整。

②主方向選取。主方向的選取主要就是以緯度方向?yàn)橹鞣较蜻€是以經(jīng)度方向?yàn)橹鞣较虻膯栴}。選擇兩個(gè)方向之中柵格變化數(shù)最多的方向作為四旋翼無人機(jī)航跡規(guī)劃主方向。主方向選定后還要知道主方向上從起始點(diǎn)到終止點(diǎn)坐標(biāo)值變化趨勢(shì)，即坐標(biāo)值是增加還是減少。如果坐標(biāo)值增加，那么當(dāng)從當(dāng)前平面到下一平面上主方向上坐標(biāo)值加1個(gè)單位值；否則坐標(biāo)值減1個(gè)單位值。假設(shè)A方向?yàn)橹鞣较颍敲磸摩癷到Πi+1坐標(biāo)變化為

(14)

③種群數(shù)選取。

④迭代次數(shù)的選擇。

⑤航跡搜尋范圍選取。如果主方向是A，非主方向的那個(gè)緯度或經(jīng)度方向是B，那么從平面Πi到平面Πi+1，對(duì)于B方向上以Yi為中心從Yi-bcmax到Y(jié)i+bcmax范圍內(nèi)的點(diǎn)都是可選擇作為Yi+1點(diǎn)。同樣，對(duì)于Z方向上來說以Zi為中心從Zi-hcmax到Zi+hcmax范圍內(nèi)的點(diǎn)都是可選擇作為Zi+1點(diǎn)。

⑥信息素初始化。在初始時(shí)刻，把三維柵格環(huán)境地圖中的所有柵格信息素值設(shè)置為固定值。

2)航跡搜索

航跡搜索前首先需要確定航跡所選的所有的節(jié)點(diǎn)都要在規(guī)劃空間中，另外在航跡高度上也要有一定的限制。經(jīng)過觀察發(fā)現(xiàn)，從地面到距地面2 m范圍內(nèi)障礙物主要是人和車，這些障礙物多是動(dòng)態(tài)的，四旋翼在這個(gè)高度范圍內(nèi)飛行容易危害行人和車的正常行動(dòng)。在距地面2～5 m的范圍內(nèi)主要是較矮的樹木，在這個(gè)空間中進(jìn)行飛行時(shí)也較容易碰到障礙物，但總的來說還算安全。本文所研究的航跡規(guī)劃主要是在高于地面2 m的范圍內(nèi)進(jìn)行的。如果無人機(jī)起始點(diǎn)在地面就先讓四旋翼飛行距地面一定高度Hmin,飛行過程中也限制四旋翼飛行在這個(gè)高度之上。

在航跡搜索過程中，假設(shè)PopNum只螞蟻中的第k只螞蟻已運(yùn)行至平面Πi上的點(diǎn)(Xi,Yi,Zi)處，搜索在平面Πi+1上以(Xi+1,Yi,Zi)為中心的count=(2×bcmax+1)×(2×hcmax+1)個(gè)點(diǎn)。將count個(gè)柵格中所有的可行柵格，即沒有障礙物的柵格放入數(shù)組Allowedi中。如果數(shù)組Allowedi為空，那么將當(dāng)前點(diǎn)(Xi,Yi,Zi)的Zi坐標(biāo)值加1，即當(dāng)前點(diǎn)坐標(biāo)變?yōu)?Xi,Yi,Zi+1)，重新搜索平面上的可行柵格，直到數(shù)組Allowedi不為空。從數(shù)組Allowedi中按照輪盤賭法選出一個(gè)可行的柵格作為平面Πi+1上的航跡節(jié)點(diǎn)。下一步就是對(duì)平面Πi上的節(jié)點(diǎn)進(jìn)行局部信息素更新。

對(duì)上述內(nèi)容重復(fù)進(jìn)行，直到到達(dá)平面Πn-1，平面Πi上的節(jié)點(diǎn)(Xn-1,Yn-1,Zn-1)直接與平面Πn上的節(jié)點(diǎn)(Xend,Yend,Zend)即終止點(diǎn)相連，這樣就構(gòu)成了一條完整的航跡。

按照適應(yīng)度值函數(shù)計(jì)算每條航跡的適應(yīng)度值，比較找出最小適應(yīng)度值，而最小適應(yīng)度對(duì)應(yīng)的航跡即為當(dāng)前最優(yōu)航跡。

將上述過程迭代N次，這樣就找到了迭代N次的最優(yōu)航跡。

3)圖形繪制

利用已經(jīng)測(cè)試到的環(huán)境模型的經(jīng)度、緯度和高度數(shù)據(jù)繪制三維環(huán)境地圖，然后在三維環(huán)境地圖中繪制最優(yōu)航跡。到此為止，本文就完成了基于蟻群算法的航跡規(guī)劃與仿真。

2.3算法的改進(jìn)

2.3.1變主方向搜索策略

基本三維蟻群算法進(jìn)行航跡規(guī)劃時(shí)，將Πn-1平面上節(jié)點(diǎn)和Πn平面上的節(jié)點(diǎn)即終點(diǎn)直接相連容易穿過障礙物。因此首先需要判斷兩點(diǎn)間的連通性，其具體過程如下。

假設(shè)在三維空間存在兩點(diǎn)(X1,Y1,Z1)和(X2,Y2,Z2)。兩點(diǎn)間的空間直線方程為

(15)

由式(15)可知，若已知X的值，那么

(16)

同理，若已知Y的值，那么

(17)

想要知道空間中兩點(diǎn)是否連通，首先要判斷空間直線經(jīng)過哪些柵格，然后判斷這些柵格所在的經(jīng)緯度位置的障礙物高度和直線高度關(guān)系，如果障礙物高度高于直線高度，那么這條直線經(jīng)過障礙物，也就是說空間中兩點(diǎn)是不可連通的，否則就是可連通的。

對(duì)于空間兩點(diǎn)，根據(jù)兩點(diǎn)的X坐標(biāo)和Y坐標(biāo)間大小關(guān)系，可以分為以下6種關(guān)系：

a)X1=X2，Y1和Y2關(guān)系任意；

b)Y1=Y2，X1和X2關(guān)系任意；

c)X1>X2，Y1>Y2；

d)X1>X2，Y1

e)X1Y2；

f)X1

圖6～11繪制了三維空間兩點(diǎn)間直線關(guān)系在二維平面上的投影的6種情況。分析這6種情況可以發(fā)現(xiàn)，當(dāng)直線與柵格的橫向直線相交時(shí)，這個(gè)交點(diǎn)的上下兩個(gè)柵格都是直線經(jīng)過的柵格；當(dāng)直線與柵格的縱向直線相交時(shí)，那么這個(gè)交點(diǎn)的左右兩個(gè)柵格都是直線經(jīng)過的柵格。所以，當(dāng)需要判斷三維直線投影經(jīng)過哪些柵格時(shí)，只要找出直線與柵格的橫縱坐標(biāo)的交點(diǎn)就可以。

圖6　X1=X2，Y1和Y2關(guān)系任意Fig.6　X1=X2,Y1and Y2 arbitrary relationship

圖7　Y1=Y2，X1和X2關(guān)系任意Fig.7　Y1=Y2,X1and X2 arbitrary relationship

圖8　X1>X2，Y1>Y2Fig.8　X1>X2,Y1 >Y2

圖9　X1>X2，Y1X2,Y1

圖10　X1Y2Fig.10　X1Y2

圖11　X1

變主方向搜索策略的基本思想是：如果Πn-1平面上的節(jié)點(diǎn)和Πn平面上終點(diǎn)是不連通的，那么就以Πn-1平面上的節(jié)點(diǎn)(Xn-1,Yn-1,Zn-1)為起始點(diǎn)，以Πn平面上終點(diǎn)(Xend,Yend,Zend)為終止點(diǎn)，再次搜索航跡。記第一次搜索的主方向上平面劃分為Π1,1，Π1,2，…，Π1,n1，搜索節(jié)點(diǎn)依次為(X1,1,Y1,1,Z1,1)，(X1,2,Y1,2,Z1,2)，…，(X1,n1-1,Y1,n1-1,Z1,n1-1)；同理，第m次搜索的主方向上平面劃分為Πm,1，Πm,2，…，Πm,nm，搜索節(jié)點(diǎn)依次為(Xm,1,Ym,1,Zm,1)，(Xm,2,Ym,2,Zm,2)，…，(Xm,n1-1,Ym,n1-1,Zm,n1-1)。最終的三維航跡為(X1,1,Y1,1,Z1,1)，…，(X1,n1-1,Y1,n1-1,Z1,n1-1)，(X2,2,Y2,2,Z2,2)，…，(X2,n1-1,Y2,n1-1,Z2,n1-1)，…，(Xm,2,Ym,2,Zm,2)，…，(Xm,nm-1,Ym,nm-1,Zm,nm-1)，(Xend,Yend,Zend)。

2.3.2簡化航跡策略

航跡節(jié)點(diǎn)過多意味著四旋翼無人機(jī)在飛行的過程中需要轉(zhuǎn)折次數(shù)過多，采用一定的簡化策略來減少航跡節(jié)點(diǎn)既能減小適應(yīng)度值還能減小航跡主要思路是：假設(shè)變主方向搜素策略所搜索出的一條可行航跡為route=(r1,r2,…,rn)。首先把節(jié)點(diǎn)r1放入新航跡數(shù)組newroute中，判斷r1與rn的連通性，如果是連通的，把r1放入數(shù)組newroute中；否則判斷r1與rn-1的連通性，直到找到一個(gè)點(diǎn)ri與r1是連通的。對(duì)ri進(jìn)行相同的操作，直到終止點(diǎn)。

3三維粒子群算法

3.1粒子群算法的主要流程

粒子群算法進(jìn)行航跡搜索時(shí)分以下幾個(gè)步驟：

1)確定起始點(diǎn)和終止點(diǎn)，對(duì)粒子群種群數(shù)、迭代次數(shù)等參數(shù)進(jìn)行設(shè)定；

2)搜索num條可行航路，根據(jù)適應(yīng)度函數(shù)計(jì)算個(gè)航路的適應(yīng)度值，選取適應(yīng)度值最小的航路R，根據(jù)主方向選取的不同將航路分成d段；

3)根據(jù)主方向的不同對(duì)速度進(jìn)行分段初始化，由初始速度和航路R找出PopNum條航路作為各個(gè)粒子的初始航路；

4)根據(jù)粒子群初始航路計(jì)算各粒子的局部最優(yōu)航路，再比較粒子群的局部最優(yōu)航路找出適應(yīng)度值最小的航路作為全局最優(yōu)航路；

5)迭代開始，迭代次數(shù)k=0；

6)粒子群i=0；

7)根據(jù)迭代次數(shù)計(jì)算每一代ω的值，根據(jù)當(dāng)前全局最優(yōu)航路和局部最優(yōu)航路對(duì)粒子群速度進(jìn)行分段更新，更新完速度之后再對(duì)粒子群中各粒子的位置進(jìn)行更新；

8)計(jì)算粒子i的新的航路的適應(yīng)度值與粒子的局部最優(yōu)值進(jìn)行比較，如果當(dāng)前航路的適應(yīng)度值小于粒子i的最優(yōu)航路的適應(yīng)度值，以當(dāng)前航路替換最優(yōu)航路成為粒子i的新的最優(yōu)航路；

9)如果i

10)更新完所有粒子的位置后比較全局最優(yōu)航路適應(yīng)度值和粒子群當(dāng)前局部最優(yōu)航路適應(yīng)度值，如果有某粒子的局部最優(yōu)航路的適應(yīng)度值小于全局最優(yōu)航路的適應(yīng)度值就用該粒子的航路代替全局最優(yōu)航路成為新的全局最優(yōu)航路；

11)如果k

12)繪制三維地圖和三維航跡。

粒子群算法被用來解決四旋翼無人機(jī)航跡規(guī)劃問題，會(huì)存在適應(yīng)度值過大的問題。即使增加種群數(shù)量和迭代次數(shù)也不能很好解決這個(gè)問題，文中提出簡化航跡的策略來減少航跡節(jié)點(diǎn)，降低適應(yīng)度值。

簡化策略的具體實(shí)行步驟與前文中的蟻群算法航跡簡化策略相似，也是先判斷終點(diǎn)和當(dāng)前點(diǎn)的連通性，如果是可連通的則簡化結(jié)束，否則判斷和倒數(shù)第二個(gè)節(jié)點(diǎn)的連通性，如此反復(fù)，直到簡化完成。在粒子群算法中，每次計(jì)算適應(yīng)度值時(shí)不是直接計(jì)算，而是先將航跡簡化，比較各航跡簡化后的適應(yīng)度值，最后保留適應(yīng)度值最小的航路，該最小航路的簡化航路就是最終的航跡，直接在三維環(huán)境中繪制即可。

4 仿真與分析

4.1三維蟻群算法仿真分析

實(shí)驗(yàn)時(shí)，設(shè)定緯度起始點(diǎn)為45°46.963 9′，終止點(diǎn)為45°46.803 9′；經(jīng)度起始點(diǎn)為126°40.502 2′，終止點(diǎn)為126°40.247 2′；地面水平高度為0.25 m。即(46.963 9，40.502 2，0.25)為起點(diǎn)，(46.803 9，40.247 2，0.25)為終點(diǎn)。圖12、13分別是用基本三維蟻群算法搜索到的三維航跡圖形和三維航跡在二維平面上的投影。圖14、15分別是采用了變主方向策略改進(jìn)后的算法搜索得到的三維航跡圖形和三維航跡在二維平面上的投影。

圖12　蟻群算法搜索到的三維航跡圖形Fig.12　Three-dimensional graphics searched by ant colony algorithm

圖13　三維航跡在二維平面上的投影Fig.13　Three-dimensional trajectory projected on a two-dimensional plane

從圖12、13中可以看出，基本蟻群算法搜索到的航跡出現(xiàn)了穿過障礙物的現(xiàn)象，這顯然是不允許的。改進(jìn)后的航跡節(jié)點(diǎn)數(shù)為173個(gè)，航跡長度為808.040 2 m，如果四旋翼以1～2 m/s的速度飛行時(shí)，可以飛行6.733 6～13.467 3′。從圖14、15可以看出，改進(jìn)后的算法滿足航跡規(guī)劃的需要，且能夠有效避免航跡穿過障礙物的現(xiàn)象出現(xiàn)。

圖14　加入變主方向策略后的三維航跡圖Fig.14　Three-dimensional graphics including the strategy of converting the main direction

圖15　加入變主方向策略的航跡二維投影圖Fig.15　The two-dimensional projection of track including the strategy of converting the main direction

選定起點(diǎn)坐標(biāo)為(46.9639，40.5022，0.25)，終點(diǎn)坐標(biāo)為(46.8039，40.2472，0.25)，種群數(shù)PopNum=25，迭代次數(shù) N=50，其他參數(shù)都相等的情況下，對(duì)簡化策略使用前和使用后進(jìn)行對(duì)比。

圖16、17分別使用簡化策略前的三維航跡及三維航跡在二維平面上的投影，航跡經(jīng)過173個(gè)航跡節(jié)點(diǎn)，航跡長度為812.070 6 m。圖18、19是使用簡化策略后的三維航跡及三維航跡在二維平面上的投影，航跡中經(jīng)過5個(gè)航跡節(jié)點(diǎn)，航跡節(jié)點(diǎn)長度為522.765 8 m?？梢钥闯觯喕蟮暮桔E比簡化前的航跡，在有效避開障礙物的同時(shí)極好地改善了三維航跡的形狀，航跡長度大大縮短，更符合實(shí)際的需要。

圖16　未使用簡化策略的三維航跡圖Fig.16　Three-dimensional graphics without simplification strategy

圖17　未使用簡化策略的航跡二維投影圖Fig.17　The two-dimensional projection of track without simplification strategy

圖18　使用簡化策略的三維航跡圖Fig.18　Three-dimensional graphics with simplification strategy

圖19　使用簡化策略的航跡二維投影圖Fig.19　The two-dimensional projection of track with simplification strategy

為了進(jìn)一步評(píng)價(jià)簡化策略實(shí)施對(duì)航跡搜索結(jié)果的影響，圖20、21分別是簡化策略實(shí)施前后適應(yīng)度值統(tǒng)計(jì)圖和搜索時(shí)間統(tǒng)計(jì)圖。

圖20　簡化前后適應(yīng)度值統(tǒng)計(jì)Fig.20　Statistics of fitness value with and without simplification

圖21　簡化前后搜索時(shí)間統(tǒng)計(jì)Fig.21　Statistics of searching time with and without simplification

從圖中可以看出，簡化策略實(shí)施前后花費(fèi)差不多的時(shí)間，與簡化策略實(shí)施前相比，簡化策略實(shí)施后適應(yīng)度值大大減小，從而進(jìn)一步證明了簡化策略實(shí)施的有效性。

4.2三維蟻群算法與三維粒子群算法對(duì)比

基于粒子群算法的航跡規(guī)劃所使用的評(píng)價(jià)函數(shù)和環(huán)境模型與蟻群算法中所使用的評(píng)價(jià)函數(shù)和環(huán)境模型相同。但用傳統(tǒng)粒子群算法實(shí)現(xiàn)航跡規(guī)劃，也存在航跡節(jié)點(diǎn)過多的問題，所以同樣采用了上文提出的簡化策略。將加入簡化策略后的粒子群算法與改進(jìn)后的蟻群算法相比較。

設(shè)定蟻群算法和粒子群算法主要參數(shù)為：

蟻群算法：種群數(shù)取值25，高度系數(shù)取值1，信息素?fù)]發(fā)系數(shù)取0.5；粒子群算法：初始時(shí)選定可行航跡條數(shù)取值50，種群數(shù)量取值20，高度系數(shù)取值1，學(xué)習(xí)因子取2。

起點(diǎn)坐標(biāo)均為(46.9349，40.4407，0.25)，終點(diǎn)坐標(biāo)均為(46.8809，40.3387，0.25)。運(yùn)行20次。得到蟻群算法和粒子群算法的最優(yōu)航跡如圖22、23。

圖22　蟻群算法搜索到的最優(yōu)三維航跡圖形Fig.22　The best three-dimensional graphic searched by Ant colony algorithm

圖23　粒子群算法搜索到的最優(yōu)三維航跡圖形Fig.23　The best three-dimensional graphic searched by Particle swarm optimization

為了評(píng)價(jià)一個(gè)算法的優(yōu)劣，需要用一些指標(biāo)來評(píng)價(jià)，常用的算法指標(biāo)包括：時(shí)間復(fù)雜度、空間復(fù)雜度、正確性、可讀性、健壯性。

結(jié)合本文所研究課題內(nèi)容，重點(diǎn)引入了穩(wěn)定性[14]、誤差率、航跡規(guī)劃搜索時(shí)間這3個(gè)指標(biāo)[15]進(jìn)行對(duì)比。

航跡規(guī)劃時(shí)已經(jīng)有S1，S2，…，Sm共m個(gè)樣本個(gè)體的樣本，樣本最大值Smax，最小值Smin，那么樣本絕對(duì)誤差可以表示為

(18)

樣本的均值描述了樣本的平均情況，表示為

(19)

樣本標(biāo)準(zhǔn)差反映了樣本的離散程度，表示為

(20)

統(tǒng)計(jì)蟻群算法和粒子群算法航跡規(guī)劃適應(yīng)度值如表1所示。從表中可以看出，蟻群算法搜索出的航跡的適應(yīng)度值更為穩(wěn)定一些。

表1　航跡規(guī)劃適應(yīng)度值統(tǒng)計(jì)

誤差率描述了樣本平均值與理論值的偏差程度，反映了算法對(duì)問題的優(yōu)化程度，誤差率越小則表示算法的優(yōu)化性能越好，也就是找到的最優(yōu)值越接近理論最優(yōu)值。誤差率計(jì)算公式為

(21)

式中：S*是理論最優(yōu)值，在本文中就是指理論最優(yōu)航跡。

本文的航跡規(guī)劃過程中，環(huán)境相同時(shí)，以粒子群算法和蟻群算法規(guī)劃處的航跡適應(yīng)度值最小值作為理論最優(yōu)值，以每種算法各自的平均值來計(jì)算兩種算法各自的誤差率。兩種算法的誤差率如表2。

表2　航跡規(guī)劃誤差率統(tǒng)計(jì)

航跡規(guī)劃搜索時(shí)間是指從算法開始到找到最優(yōu)航跡經(jīng)歷的時(shí)間。一般來說，規(guī)劃時(shí)間越少算法優(yōu)化性能越好。表3是蟻群算法和粒子群算法分別進(jìn)行航跡搜索時(shí)的搜索時(shí)間，從表中可以看出，粒子群算法進(jìn)行航跡搜索時(shí)的搜索時(shí)間比蟻群算法的小得多，同時(shí)，蟻群算法的規(guī)劃時(shí)間更穩(wěn)定一些，也間接說明蟻群算法搜索穩(wěn)定性更高一些。

表3　航跡規(guī)劃時(shí)間統(tǒng)計(jì)

從標(biāo)準(zhǔn)差、誤差率、規(guī)劃時(shí)間3個(gè)方面對(duì)兩種算法進(jìn)行對(duì)比和評(píng)估，從對(duì)比和評(píng)估的結(jié)果看來，蟻群算法穩(wěn)定性更高一些，而粒子群算法航跡規(guī)劃時(shí)間更少一些。

5結(jié)束語

本文詳細(xì)介紹了四旋翼無人機(jī)航跡規(guī)劃的三維環(huán)境模型構(gòu)建并結(jié)合蟻群算法在航跡搜索中出現(xiàn)的問題對(duì)算法提出了改進(jìn)，仿真實(shí)驗(yàn)證實(shí)改進(jìn)后的算法減小了適應(yīng)度值，使航跡得到了優(yōu)化。但是根據(jù)搜索時(shí)間的統(tǒng)計(jì)圖可以得出搜索時(shí)間較改進(jìn)前長。本文還與同樣改進(jìn)策略的粒子群算法作比較，得出蟻群算法穩(wěn)定性更高，但是規(guī)劃時(shí)間有點(diǎn)長。因此，在后續(xù)的工作研究中要結(jié)合實(shí)際飛行測(cè)試實(shí)驗(yàn)并將蟻群算法與更多智能算法做橫向?qū)Ρ?，進(jìn)而優(yōu)化參數(shù)以便提高算法的規(guī)劃時(shí)間。

參考文獻(xiàn):

[1]METEA M, TSAI J. Route planning for intelligent autonomous land vehicles using hierarchical terrain representation[C]//Proceedings of IEEE International Conference on Robotics and Automation. Raleigh, NC, USA, 2010: 1947-1952.

[2]謝奉軍, 張丹平, 黃蕾,等. 旋翼無人機(jī)專利申請(qǐng)現(xiàn)狀與技術(shù)發(fā)展趨勢(shì)分析[C]//第六屆中國航空學(xué)會(huì)青年科技論壇文集. 沈陽, 2014.

[3]LIU Hongyun, JIANG Xiao, JU Hehua. Multi-goal path planning algorithm for mobile robots in grid space[C]//Proceedings of the 25th Chinese Control and Decision Conference. Guiyang, 2013: 2872-2876.

[4]GALCERAN E, CARRERAS M. A survey on coverage path planning for robotics[J]. Robotics and Autonomous Systems, 2013, 61(12): 1258-1276.

[5]李翊, 王朕, 姜鵬, 等. 四旋翼無人飛行器的航跡規(guī)劃[J]. 艦船電子工程, 2014, 34(12): 58-61.

LI Yi， WANG Zhen， JIANG Peng， et al. Path planning of quad-rotor[J]. Ship electronic engineering， 2013, 61(12): 1258-1276.

[6]王玥, 張志強(qiáng), 曹曉文. A*改進(jìn)算法及其在航跡規(guī)劃中的應(yīng)用[J]. 信息系統(tǒng)工程, 2014 (1): 89-91.

[7]韓超, 王贏. 一種基于改進(jìn)PSO的無人機(jī)航路規(guī)劃方法[J]. 艦船電子工程, 2014, 34(4): 49-53.

HAN Chao， WANG Ying. Path planning of UAV based on an improved PSO algorithm[J]. Ship electronic engineering, 2014, 34(4): 49-53.

[8]胡小兵, 黃席樾. 基于蟻群算法的三維空間機(jī)器人的路徑規(guī)劃[J]. 重慶大學(xué)學(xué)報(bào), 2004, 27(8): 132-135.

HU Xiaobing， HUANG Xiyue. Path planning in 3-D space for robot based on ant colony algorithm[J]. Journal of Chongqing university： natural science edition, 2004, 27(8): 132-135.

[9]胡中華. 基于智能優(yōu)化算法的無人機(jī)航跡規(guī)劃若干關(guān)鍵技術(shù)研究[D]. 南京: 南京航空航天大學(xué), 2011: 46-52.

[10]KHATIB O. Real-time obstacle avoidance for manipulators and mobile robots[J]. International Journal of Robotics Research, 1986, 5(1): 90-99.

[11]BAIRD C, ABRARNSON M. A comparison of several digital map-aided navigation technique[C]//The Proceedings of IEEE PLANS. Halifax, Nova Scotia, Canada, 1984: 286-293.

[12]任世軍, 洪炳熔, 黃德海. 一種基于柵格擴(kuò)展的機(jī)器人路徑規(guī)劃方法[J]. 哈爾濱工業(yè)大學(xué)學(xué)報(bào), 2001, 33(1): 68-72.

REN Shijun,HONG Bingrong, HUANG Dehai. A robot path planning algorithm based on grid expansion[J]. Journal of Harbin institute of technology, 2001, 33(1): 68-72.

[13]呂太之, 趙春霞. 基于改進(jìn)概率柵格分解的路徑規(guī)劃算法[J]. 計(jì)算機(jī)工程, 2007, 33(21): 160-162, 165.

LV Taizhi,ZHAO Chunxia. Path planning based on improved probabilistic cell decomposition[J]. Computer engineering, 2007, 33(21): 160-162, 165.

[14]王凌. 智能優(yōu)化算法及其應(yīng)用[M]. 北京: 清華大學(xué)出版社, 2001.

[15]WEISS M A. 數(shù)據(jù)結(jié)構(gòu)與算法分析[M]. 馮舜璽, 譯. 2版. 北京: 機(jī)械工業(yè)出版社, 2004.

莫宏偉，男，1973年生，教授，主持完成國家自然科學(xué)基金等國家、省部級(jí)及橫向課題16項(xiàng)，獲得省科技進(jìn)步獎(jiǎng)兩項(xiàng)，主要研究方向?yàn)樽匀挥?jì)算理論與應(yīng)用，機(jī)器人，機(jī)器學(xué)習(xí)與數(shù)據(jù)挖掘，發(fā)表論文60余篇，其中被SCI檢索11篇，被EI檢索40余篇。

Four-rotor route planning based on the ant colony algorithm

MO Hongwei， MA Jingwen

(College of Automation, Harbin Engineering University, Harbin 150001, China)

Abstract：Given a four-rotor unmanned aerial vehicle’s characteristics and complex flight environment, as well as the accuracy of the global positioning system in the environment model, the establishment of a 3D environment model based on elevation maps has reduced the probability of encountering obstacles. In terms of planning algorithms, most of the existing path planning algorithms can only plan 2D paths. Numerous 3D planning algorithms have complex computations and require much storage space. A global path is also difficult to plan. The advantages of the ant colony algorithm include distributed computing and swarm intelligence. Moreover, this algorithm has great potential in path planning. However, when the fundamental ant colony algorithm is used in a 3D track search, the two directly connected planes easily track straight through obstacles. The track then includes more nodes, and the fitness value becomes too large. The algorithm was improved to address these issues by proposing the strategy of converting the main direction to search and the simplified track strategy. Ant simulation results showed that the improved algorithm could avoid obstacles, reduce path length, and improve search efficiency.

Keywords：four-rotor unmanned aerial vehicle; route planning; three-dimensional environment model; ant colony algorithm; strategy of converting the main direction to search; track simplification strategy

作者簡介：

中圖分類號(hào)：TP18

文獻(xiàn)標(biāo)志碼：A

文章編號(hào)：1673-4785(2016)02-0216-10

通信作者：莫宏偉.E-mail：honwei2004@126.com.

基金項(xiàng)目：黑龍江省杰出青年科學(xué)基金項(xiàng)目(JC1212).

收稿日期：2014-04-01.

DOI:10.11992/tis.201509009