有障礙區(qū)域的多無人機(jī)多目標(biāo)點路徑規(guī)劃*

肖春暉，鄒媛媛，李少遠(yuǎn)

0 引 言

無人機(jī)具有敏捷、地面保障要求低、安全風(fēng)險系數(shù)小等特性，且運行成本較低，近年來已廣泛地應(yīng)用于民用及軍事等多個領(lǐng)域，如電力巡檢、農(nóng)業(yè)灌溉、物流配送、遙感[1-4]及情報搜集、偵查監(jiān)視、通信中繼、對地打擊等[5-8]. 在無人機(jī)的運動控制過程中，路徑規(guī)劃是重要的一部分.一般而言,無人機(jī)路徑規(guī)劃的目標(biāo)是在到達(dá)目標(biāo)點的同時盡可能最小化燃料消耗、時間消耗等因素. 在二維環(huán)境中的無人機(jī)路徑規(guī)劃類似于一般地面機(jī)器人路徑規(guī)劃[9]. 常規(guī)無人機(jī)路徑規(guī)劃的現(xiàn)有解決方案主要有Dijkstra算法[10]、基于啟發(fā)式的搜索算法A*和D*和各種其他元啟發(fā)式算法[11]、基于采樣的路徑搜索方式，包括快速擴(kuò)展隨機(jī)樹算法(RRT)[12]和隨機(jī)路標(biāo)圖算法(PRM)[13]. 對于有障礙物的情況，文獻(xiàn)[14]通過模擬人類躲避障礙的思想，制定了五項飛行規(guī)則，提出了基于廣度優(yōu)先和RRT結(jié)合的算法，文獻(xiàn)[15]提出了切向量矢量場引導(dǎo)和李雅普諾夫矢量場引導(dǎo)的無人機(jī)路徑規(guī)劃算法，文獻(xiàn)[16]則設(shè)計了基于神經(jīng)網(wǎng)絡(luò)和反步法的狀態(tài)反饋算法.

對于有多個目標(biāo)點需要訪問的無人機(jī)路徑規(guī)劃,可以將其轉(zhuǎn)化為旅行商問題(traveling salesman problem, TSP)或車輛路徑問題(vehicle routing problem, VRP). 文獻(xiàn)[17]針對無人機(jī)運輸任務(wù)提出了兩個多旅途VRP，分別使用了混合整數(shù)線性規(guī)劃算法和模擬退火算法，以解決時間窗約束及時間窗和預(yù)算約束問題，并通過數(shù)值仿真與效果分析進(jìn)行了算法驗證. 文獻(xiàn)[18]針對具有曲率約束的無人機(jī)的TSP，提出了一種基于自組織映射網(wǎng)絡(luò)(self organizing map, SOM)的非監(jiān)督式學(xué)習(xí)算法，解決了鄰域約束的多個體Dubins旅行商問題，并能夠在傳統(tǒng)的計算資源上以秒為單位快速地提供解決方案，使得該方法適合于在線規(guī)劃. 上述工作均未考慮障礙區(qū)域，但實際中存在的飛行障礙往往導(dǎo)致現(xiàn)有方案規(guī)劃的路徑不可行.

本文考慮解決有障礙區(qū)域限制的多無人機(jī)遍歷多目標(biāo)點的路徑規(guī)劃問題，難點主要體現(xiàn)在兩方面：一方面是如何求解每個無人機(jī)的訪問順序和各點的航向角，得到適用于無人機(jī)的路徑；另一方面是如何處理路徑與障礙區(qū)域的約束. 本文的結(jié)構(gòu)如下：首先建立了無人機(jī)模型，確定無人機(jī)路徑所用曲線；其次確定了優(yōu)化問題. 針對該優(yōu)化問題，使用改進(jìn)的遺傳算法求得所需要的無人機(jī)數(shù)量及各無人機(jī)對應(yīng)的路徑，并判斷所獲得路徑是否存在障礙區(qū)域約束，若存在則使用Dubins-RRT算法進(jìn)行調(diào)優(yōu)以得到滿足約束的路徑. 最后，通過仿真算法案例證明了本文方法的有效性.

1 無人機(jī)模型

選取小型固定翼多無人機(jī)(unmanned aerial vehicle，UAV)作為本文的研究對象，為保證規(guī)劃的路徑平滑可飛，便于UAV后續(xù)的運動控制，使用Dubins模型來描述固定翼UAV簡化的動力學(xué)特性，滿足了無人機(jī)的曲率約束[19].

1.1 Dubins模型

Dubins模型的公式如下所示：

(1)

式中，(xA,yA)為UAV在二維平面直角坐標(biāo)系下的橫縱坐標(biāo)，θA為UAV的轉(zhuǎn)向角，vA和rA分別為UAV的飛行速度和最小轉(zhuǎn)彎半徑，uA為控制轉(zhuǎn)向的輸入.(xA,yA,θA)∈R3表示Dubins運動體的狀態(tài)，可以稱之為Dubins狀態(tài)(Dubins Configuration). 通過Dubins模型可以看出，當(dāng)uA>0時，Dubins運動體相對于當(dāng)前運動方向左轉(zhuǎn)彎，而uA<0時，Dubins運動體相對于當(dāng)前運動方向向右轉(zhuǎn)彎，當(dāng)uA=0時，Dubins運動體保持當(dāng)前運動方向直行. 同時，rA限制了Dubins運動體轉(zhuǎn)彎的最小半徑，當(dāng)|uA|=1時，表示Dubins運動體以最小轉(zhuǎn)彎半徑向左或者向右轉(zhuǎn).

1.2 Dubins路徑

Dubins路徑表示從起始Dubins狀態(tài)到目標(biāo)Dubins狀態(tài)的符合Dubins動力學(xué)模型的最短路徑.文獻(xiàn)[19]證明，當(dāng)給定任意起始Dubins狀態(tài)和目標(biāo)Dubins狀態(tài)時，存在六種類型的最短路徑，根據(jù)運動模式的不同稱它們?yōu)? RSR, LSL, RSL, LSR, RLR, LRL. 其中，R、L、S分別代表右轉(zhuǎn)、左轉(zhuǎn)、直行. 圖1分別給出了在這兩個Dubins狀態(tài)下的RSR路徑和RLR路徑示例，其中SP表示起始Dubins狀態(tài)，TP表示目標(biāo)Dubins狀態(tài). 在尋找兩個Dubins狀態(tài)之間的Dubins路徑時，需要計算這六種Dubins路徑各自的長度(部分情況下可能存在某些種類路徑不存在)，其中長度最短的路徑就是這兩個Dubins狀態(tài)下的Dubins路徑.

圖1 Dubins路徑Fig.1 Dubins path

2 問題形成

2.1 問題描述

指揮中心擁有K個UAV，UAV需要到達(dá)N個目標(biāo)點并裝載一定量的任務(wù)物品以完成任務(wù). 安排合理科學(xué)的UAV遍歷路徑，使得一定數(shù)量的UAV從指揮中心出發(fā)，在總飛行成本最低的目標(biāo)下，完成任務(wù)后再返回到指揮中心.

UAV在飛行的過程中，可能遇到影響其飛行的障礙物或禁飛區(qū)域(兩者統(tǒng)稱為障礙區(qū)域)，所規(guī)劃的路徑應(yīng)該避開這些障礙區(qū)域.

為構(gòu)建問題的數(shù)學(xué)模型，做出一定的假設(shè)：只有一個指揮中心，且所有的UAV均以指揮中心為起點，執(zhí)行完各自的任務(wù)后返回指揮中心；每一個任務(wù)點對于UAV執(zhí)行該點任務(wù)時所需要的裝備、物資是已知且不變的，且小于UAV的額定載重量指揮中心和任務(wù)點的位置已知；障礙區(qū)域提前已知且靜態(tài)的.

2.2 數(shù)學(xué)模型

為了更方便地描述模型，首先給出使用到的符號的定義.

N：任務(wù)點集合；

K：UAV集合，下標(biāo)用表示；

O：指揮中心，下標(biāo)用表示；

C：UAV單位距離所耗費的飛行成本；

dij：從任務(wù)點i到j(luò)的Dubins距離；

qi：執(zhí)行任務(wù)點處任務(wù)所需的裝備物資量；

Q：UAV的額定裝備物資載重量；

D：UAV飛行距離上限；

Obs：障礙區(qū)域集合，每個障礙區(qū)域用圓來表示；

Φik：當(dāng)xijk=1時，UAVk在任務(wù)點i的航向角.

針對該問題，給定目標(biāo)函數(shù)為：

(2)

s.t.

(3)

(4)

(5)

(6)

(7)

(8)

path(X,Y,Φ)Obs

(9)

其中，式(2)為目標(biāo)函數(shù)；式(3)保證了每次任務(wù)都以指揮中心為起點和終點；式(4)保證每個任務(wù)都被執(zhí)行且僅執(zhí)行一次；式(5)表示額定載重量約束；式(6)表示所有使用的UAV數(shù)量少于指揮中心所擁有的數(shù)量；式(7)表示任務(wù)點的總數(shù)為N；式(8)表示UAV最大距離約束；式(9)表示障礙區(qū)域約束，即形成的所有路徑都不經(jīng)過障礙區(qū)域.

3 問題求解

在節(jié)2.2中形成的NP-hard的優(yōu)化問題中，由于約束條件復(fù)雜繁多，問題規(guī)模較大，同時決策變量具有離散性，且Dubins路徑具有組合性，使得傳統(tǒng)的優(yōu)化算法不便于求解該問題. 遺傳算法具有強(qiáng)大的并行搜索能力，自學(xué)習(xí)性、自組織性和自適應(yīng)性強(qiáng)，并且不易陷入局部最優(yōu). 本節(jié)首先使用遺傳算法求解優(yōu)化問題得到路徑，對得到的路徑進(jìn)行障礙區(qū)域檢測，若不滿足障礙約束，則通過Dubins-RRT算法進(jìn)行調(diào)優(yōu)以滿足約束.

3.1 改進(jìn)的遺傳算法

遺傳算法主要包括染色體編碼、初始化、適應(yīng)性函數(shù)設(shè)置、選擇、進(jìn)化等步驟. 在交叉步驟中，通常是對染色體中的元素隨機(jī)交叉，本文在交叉過程中通過設(shè)計新的交叉算子以改進(jìn)遺傳算法，在保留了父代優(yōu)秀染色體的同時加快了收斂速度. 改進(jìn)的遺傳算法各個步驟如下.

1)編碼

在遺傳算法中，一條染色體就是一個解.由于xijk,yki,Φik均存在不少的約束條件，設(shè)定一個合適的染色體編碼直接處理掉這些約束尤為重要. 使用[S,φ]來表示一個解，它們都是N+K+1維的向量，其中S表示UAV對于V的訪問順序，φ表示UAV在S所對應(yīng)點處的航向角.S由K+1個0(首尾一定是0)和1到N的排列組成，其中0表示指揮中心，自然數(shù)表示任務(wù)點，兩個0之間的元素代表了一條UAV訪問路徑的順序. 例如，S=[0,4,1,0,2,5,0,3,0]，[0,4,1,0]代表UAV1從指揮中心->任務(wù)點4->任務(wù)點1->指揮中心的路徑，[0,2,5,0]以及[0,3,0]表示UAV2和UAV3的路徑. 如果S中包含兩個連續(xù)的0即[0,0]，則表示該UAV未被使用.

2)初始化

種群數(shù)量popsize、迭代次數(shù)maxiter都與N、K有關(guān)系，通常情況下兩種參數(shù)設(shè)置在50～200之間，此外，還有交叉概率、變異概率也需要根據(jù)實際調(diào)試結(jié)果進(jìn)行修正.

3)適應(yīng)度函數(shù)

4)選擇

選擇操作以一定概率從舊群體中選擇個體到新群體. 選擇個體的概率與適合度值相關(guān). 個體適應(yīng)度越大，被選中的概率越大.

5)交叉與變異

交叉時將[S,φ]綁定，同時進(jìn)行交換. 為了保留父代的優(yōu)秀子路徑，設(shè)計新的交叉算子，具體步驟如下：

Step1:分別在兩個父染色體上隨機(jī)選取一段子路徑.如圖2所示.

圖2 隨機(jī)選擇子路徑Fig.2 Randomly select subpath

Step2：前置父代中被選擇的子路徑，如圖3所示.

圖3 前置子路徑Fig.3 Put subpath in front

Step3：將父染色體1中子路徑A保留，然后將父染色體2中子路徑A沒有的自然數(shù)以及最后一位0按照父染色體2的順序添加到子路徑A的后面，同樣方法得到子染色體2. 如圖4所示.

圖4 初步得到子染色體Fig.4 Preliminary chromosome

Step4：對于子染色體1，空缺的0(對應(yīng)的航向角隨機(jī)生成)隨機(jī)插入任何一個位置，并計算插入每個位置時對應(yīng)的適應(yīng)度函數(shù)，選擇最優(yōu)的染色體作為最終得到的子染色體1，同樣的方法得到子染色體2.

變異算子則只改變航向角的值，隨機(jī)選擇幾個航向角進(jìn)行變異，若得到更優(yōu)的染色體則進(jìn)行替換.

為了衡量UAV數(shù)量、任務(wù)點數(shù)量、種群數(shù)量等參數(shù)對算法的影響，對本節(jié)介紹的改進(jìn)的遺傳算法進(jìn)行簡要的時間復(fù)雜度分析. 在編碼過程中提到染色體中S、φ都是N+K+1維，為了簡便，記n=N+K+1.求適應(yīng)度階段的時間復(fù)雜度為O(popsize×n)，選擇階段為O(popsize×logpopsize),交叉階段為O(popsize2),變異階段為O(popsize).故對整個改進(jìn)遺傳算法來說，其時間復(fù)雜度為:

O(maxiter)×

[O(popsize×n)+O(popsize×logpopsize)+

O(popsize2)+O(popsize)]=

O[maxiter×popsize×(popsize+n)]

因此，可以看出，迭代次數(shù)、種群數(shù)量、任務(wù)點數(shù)量、UAV數(shù)量都是影響改進(jìn)遺傳算法的因素，其中，由于種群數(shù)量是二次項，所以為影響整個算法時間復(fù)雜度的最重要因素.

3.2 障礙區(qū)域檢測方法

為了方便描述，將由兩個Dubins狀態(tài)得到的Dubins路徑稱為“基本Dubins路徑”，由多個“基本Dubins路徑”連接得到的稱為“復(fù)合Dubins路徑”. 顯然，每個UAV的路徑都是復(fù)合Dubins路徑. 所以，要對UAV的路徑進(jìn)行障礙區(qū)域檢測，只需要對每個基本Dubins路徑進(jìn)行檢測，而每個基本Dubins路徑都是有線段和弧組成的，所以只需要對每條線段和弧進(jìn)行檢測.

對線段來說，首先判斷線段所在的直線是否與障礙區(qū)域有交點，如果有交點則進(jìn)一步判斷交點是否在線段上；對于弧來說，首先補(bǔ)全弧所在的圓，判斷該圓是否與障礙區(qū)域相交，若相交則進(jìn)一步判斷交點是否在弧上. 圖5給出了具體實例，黃色代表障礙區(qū)域，紅色實線代表線段和弧，紅色虛線代表輔助的直線和圓，黑色實心點表示輔助線與障礙區(qū)域的交點，藍(lán)色實心點表示線段和弧與障礙區(qū)域的交點.

圖5 對線段和弧的障礙區(qū)域檢測Fig.5 Detection for line and arc

結(jié)合對線段和弧進(jìn)行障礙檢測的方法，就可以對基本Dubins路徑進(jìn)行障礙區(qū)域檢測. 圖6給出了針對兩類基本Dubins路徑檢測的實例，一類是針對弧-直線-弧(Circle-Straight-Circle, CLC，包含LSL,LSR,RSL,RSR)種類曲線的障礙區(qū)域檢測方法，另一類是針對弧-弧-弧(Circle-Circle-Circle, CCC，包含LRL,RLR)種類曲線的障礙區(qū)域檢測方法.

圖6 對基本Dubins路徑的障礙區(qū)域檢測Fig.6 Detection for basic Dubins path

3.3 路徑調(diào)優(yōu)

對得到的UAV路徑中的每一段基本Dubins路徑進(jìn)行障礙區(qū)域檢測，然后對經(jīng)過障礙區(qū)域的基本Dubins路徑進(jìn)行重新規(guī)劃，以得到安全路徑.

RRT算法是一種速度較快的路徑規(guī)劃算法，能快速擴(kuò)展以充分地搜索可行(無障礙)空間，并構(gòu)建一個從初始點向目標(biāo)點探尋可行路徑的樹T(V;E)，其中V代表節(jié)點，E代表邊. RRT算法中用于探索空間的并非一定是二維頂點，也可以是Dubins狀態(tài)，邊也可以是Dubins曲線，該樹用T(Vd;Ed)來表示，在擴(kuò)展過程中Vd和Ed不斷增加. 為此，本文針對Dubins路徑改進(jìn)RRT算法，稱其為“Dubins-RRT”算法. 算法流程如下：

1)初始化樹，T(Vd;Ed),dinit作為樹的根節(jié)點，設(shè)定選擇目標(biāo)Dubins狀態(tài)dgoal作為隨機(jī)目標(biāo)點的概率pg;

2)是否到達(dá)目標(biāo)dgoal，否則專向步驟3)，是則表明樹已構(gòu)造完成，轉(zhuǎn)向步驟7);

3)生成隨機(jī)數(shù)p∈[0,1],若p≤pg則轉(zhuǎn)向步驟4)，否則轉(zhuǎn)向步驟5);

4)選擇目標(biāo)dgoal作為drand，從Vd的所有節(jié)點中，找出離drand最近的節(jié)點，稱為dnear；

5)生成一個隨機(jī)節(jié)點作為drand，從Vd的所有節(jié)點中，找出離drand最近的節(jié)點，稱為dnear；

6)判斷從dnear到drand的路徑lnr是否與障礙區(qū)域沖突，若不沖突則將drand，lnr分別作為節(jié)點和邊添加到隨機(jī)樹中，完成一次樹的擴(kuò)展，轉(zhuǎn)向步驟2)，若沖突則轉(zhuǎn)向步驟3)；

7)從構(gòu)造完成的隨機(jī)樹中，確定一條從dinit到dgoal的路徑.

圖7展示了給定兩個Dubins狀態(tài)以及障礙區(qū)域(黑色實心圓)，使用Dubins-RRT的擴(kuò)展過程，(a)中隨機(jī)樹只新增了兩個節(jié)點和兩條邊，(b)中隨機(jī)樹已有近50個節(jié)點和邊，(c)中隨機(jī)樹有近200個節(jié)點和邊，(d)是算法結(jié)束時隨機(jī)樹的結(jié)果，紅色線條表示滿足障礙約束的路徑.

圖7 Dubins-RRT擴(kuò)展過程Fig.7 Dubins-RRT extension process

4 案例仿真

本章節(jié)通過一個具體仿真案例驗證算法的有效性. 給定指揮中心和25個任務(wù)點，表1給出了其相應(yīng)的坐標(biāo)位置，其中Ti表示第個任務(wù)點，表2給出了任務(wù)的相關(guān)參數(shù)和改進(jìn)遺傳算法的相關(guān)參數(shù).

表1 指揮中心與任務(wù)點的坐標(biāo)Tab.1 Coordinate of center and target points

表2 相關(guān)參數(shù)設(shè)定Tab.2 Related parameters

仿真結(jié)果如圖8～9所示，紅綠藍(lán)三條線分別代表了三個UAV的路徑，黑色圓表示多個障礙區(qū)域. 圖8展示了使用改進(jìn)遺傳算法的結(jié)果，可以看出有部分路徑(T2->T14，T3->T1，T9->T24)違反了障礙區(qū)域約束，經(jīng)過局部調(diào)優(yōu)后得到的結(jié)果如圖9所示.

將本文方案結(jié)果與其他兩種方案進(jìn)行比較，兩種方案分別采用未改進(jìn)的遺傳算法(GA)和蟻群算法(ant colony optimization, ACO)求解優(yōu)化問題后進(jìn)行調(diào)優(yōu). 三種方案的結(jié)果如表3所示. 可以看出，本文方案與GA+Dubins-RRT方案相比，收斂速度提升了77%，避障后飛行成本減少了11.3%；與ACO+Dubins-RRT方案相比，收斂速度提升了122%，避障后飛行成本減少了8.9%. 綜上所述，本文提出的方案在收斂速度提升較大的同時減少了飛行成本.

圖8 求解優(yōu)化問題后路徑Fig.8 Path after solving the optimization problem

圖9 調(diào)優(yōu)后路徑Fig.9 Tuning path

表3 三種方案比較Tab.3 Comparison of three schemes

5 結(jié) 論

針對有障礙區(qū)域的多無人機(jī)多目標(biāo)點路徑規(guī)劃問題，本文采用改進(jìn)的遺傳算法求解優(yōu)化問題得到路徑，并針對不滿足障礙約束的路徑，采用Dubins-RRT算法進(jìn)行調(diào)優(yōu)重規(guī)劃，最終得到符合要求的較優(yōu)路徑. 通過與其他幾種算法的比較，證明了算法的有效性.

當(dāng)任務(wù)點規(guī)定了UAV執(zhí)行任務(wù)的時間段時，還可以在本文的基礎(chǔ)上考慮有時間窗約束的問題. 在對這種優(yōu)化問題求解時，對于無法滿足障礙區(qū)域約束的路徑需要進(jìn)行調(diào)優(yōu)，然而調(diào)優(yōu)后的路徑又可能與時間窗約束矛盾. 因此，如何在考慮障礙區(qū)域的基礎(chǔ)上考慮時間窗約束的處理方法，是進(jìn)一步工作的重點和難點.