曾國(guó)奇, 趙民強(qiáng), 劉方圓, 丁文銳

(1. 北京航空航天大學(xué)無(wú)人駕駛飛行器設(shè)計(jì)研究所, 北京 100191;2. 北京航空航天大學(xué)電子信息工程學(xué)院, 北京 100191)

?

基于網(wǎng)格PRM的無(wú)人機(jī)多約束航路規(guī)劃

曾國(guó)奇1, 趙民強(qiáng)2, 劉方圓2, 丁文銳1

(1. 北京航空航天大學(xué)無(wú)人駕駛飛行器設(shè)計(jì)研究所, 北京 100191;2. 北京航空航天大學(xué)電子信息工程學(xué)院, 北京 100191)

目前，無(wú)人機(jī)航路規(guī)劃技術(shù)存在約束條件少,不能滿足實(shí)際飛行需求,規(guī)劃效率低等不足,文章主要針對(duì)上述問(wèn)題提出改進(jìn)措施。首先,將航路約束條件進(jìn)行分類,提出了基本約束、平臺(tái)安全約束和鏈路載荷約束,并對(duì)約束條件建模,完善無(wú)人機(jī)航路約束模型。為了提高無(wú)人機(jī)航路規(guī)劃效率,提出了網(wǎng)格概率地圖法(gridprobabilisticroadmap,GPRM),利用約束模型構(gòu)建代價(jià)函數(shù),實(shí)現(xiàn)無(wú)人機(jī)航路的多約束快速航路規(guī)劃。GPRM的實(shí)驗(yàn)仿真表明，GPRM規(guī)劃效率相比較傳統(tǒng)PRM有顯著提升,同時(shí)規(guī)劃結(jié)果更加符合實(shí)際任務(wù)需求,證明基于GPRM的無(wú)人機(jī)航路規(guī)劃具有一定的工程應(yīng)用價(jià)值。

無(wú)人機(jī); 航路規(guī)劃; 網(wǎng)格概率地圖法; 多約束; 鏈路載荷

0　引　言

近年來(lái),無(wú)人機(jī)技術(shù)取得了迅速發(fā)展,其指揮控制系統(tǒng)正在朝著自主化、智能化方向發(fā)展,無(wú)人機(jī)航路規(guī)劃是實(shí)現(xiàn)無(wú)人機(jī)自主化的重要技術(shù)環(huán)節(jié)。無(wú)人機(jī)航路規(guī)劃是指無(wú)人機(jī)在得到任務(wù)信息后根據(jù)執(zhí)行任務(wù)的環(huán)境信息規(guī)劃出滿足一系列約束條件和性能指標(biāo)的最優(yōu)或較優(yōu)的可行航路。高效的航路規(guī)劃系統(tǒng)可以使無(wú)人機(jī)在惡劣環(huán)境中安全、高效地完成任務(wù)。因此,無(wú)人機(jī)航路規(guī)劃技術(shù)吸引了越來(lái)越多學(xué)者的關(guān)注。

航路規(guī)劃算法通常分為幾何方法、數(shù)學(xué)規(guī)劃方法、人工勢(shì)場(chǎng)法[1]和智能優(yōu)化方法[2]。幾何方法一般指基于圖論的搜索方法,如基于概略圖的通視圖法、Voronoi法[3]、概率地圖法(probabilisticroadmapmethod,PRM)、A*算法,以及基于柵格的搜索方法。數(shù)學(xué)規(guī)劃方法包括動(dòng)態(tài)規(guī)劃法、非線性規(guī)劃法等。人工勢(shì)場(chǎng)法在規(guī)劃空間構(gòu)建人工勢(shì)場(chǎng),通過(guò)勢(shì)場(chǎng)力的作用來(lái)求解。智能優(yōu)化方法有進(jìn)化算法[4-5]、蟻群算法[6-7]等。

PRM因具有實(shí)現(xiàn)簡(jiǎn)單,幾何計(jì)算需求小,擴(kuò)展性好而被廣泛應(yīng)用于路徑規(guī)劃問(wèn)題[8]。PRM的基本思想最初由文獻(xiàn)[8-9]提出,主要用來(lái)解決機(jī)器人的運(yùn)動(dòng)規(guī)劃問(wèn)題和其他類似問(wèn)題。文獻(xiàn)[10]對(duì)PRM中的距離參數(shù)和局部規(guī)劃器的選取進(jìn)行了研究,提出了一般性的參數(shù)選取指導(dǎo)。針對(duì)不同的應(yīng)用,對(duì)PRM的改進(jìn)包括提高PRM在障礙聚集區(qū)的性能[11],提高PRM圖的連通性[12]等。文獻(xiàn)[13-14]將PRM應(yīng)用于類車機(jī)器人的路徑規(guī)劃。文獻(xiàn)[15]利用PRM解決了自主水下航行器的路徑規(guī)劃。文獻(xiàn)[16]利用改進(jìn)的PRM解決了無(wú)人機(jī)低空突防的航跡規(guī)劃問(wèn)題。

目前，PRM在解決無(wú)人機(jī)航路規(guī)劃時(shí)還存在著一些不足。首先,對(duì)無(wú)人機(jī)航路規(guī)劃的約束條件考慮不夠完善[9-10],沒(méi)有充分考慮無(wú)人機(jī)實(shí)際飛行需求,如對(duì)通訊鏈路的要求。其次,無(wú)人機(jī)規(guī)劃空間大,PRM在學(xué)習(xí)階段將消耗較多時(shí)間,限制了PRM在大范圍無(wú)人機(jī)航路規(guī)劃中的應(yīng)用。

針對(duì)上述存在的問(wèn)題,本文首先結(jié)合實(shí)際需求,對(duì)真實(shí)環(huán)境中無(wú)人機(jī)航跡的約束條件進(jìn)行分類和建模。其次,對(duì)傳統(tǒng)PRM進(jìn)行改進(jìn),提出網(wǎng)格PRM的方法,在三維空間進(jìn)行采樣,構(gòu)建了三維的規(guī)劃空間,應(yīng)用網(wǎng)格數(shù)據(jù)結(jié)構(gòu),實(shí)現(xiàn)了無(wú)人機(jī)多約束條件下航路的快速規(guī)劃。

1　航路規(guī)劃的約束條件

無(wú)人機(jī)在復(fù)雜環(huán)境中進(jìn)行航路規(guī)劃時(shí)必須滿足以下約束條件。第一,無(wú)人機(jī)基本約束,包括地形、人工禁飛區(qū)、無(wú)人機(jī)最小直飛距離與轉(zhuǎn)彎半徑、無(wú)人機(jī)最大爬升角等約束;第二,無(wú)人機(jī)平臺(tái)安全約束,包括雷達(dá)威脅源、高炮威脅源等約束;第三,無(wú)人機(jī)鏈路載荷約束,如鏈路與干擾等。

根據(jù)上述約束條件對(duì)航路質(zhì)量評(píng)價(jià)的影響,以上約束條件的代價(jià)函數(shù)又可分為3類：第1類為剛性代價(jià)函數(shù),包括地形、禁飛區(qū)、航跡段直飛距離、最小轉(zhuǎn)彎半徑和最大爬升角約束,該指標(biāo)航路的評(píng)價(jià)是二值的,即可行或不可行；第2類為成本型代價(jià)函數(shù),如雷達(dá)威脅源,函數(shù)值越小航路評(píng)價(jià)越好；第3類為效益型代價(jià)函數(shù),如鏈路與干擾,函數(shù)值越大航路評(píng)價(jià)越好。

2　約束模型

根據(jù)3類約束條件對(duì)航路質(zhì)量評(píng)價(jià)的影響,對(duì)不同約束條件進(jìn)行建模后獲得各約束條件下的代價(jià)模型與總的代價(jià)模型。

2.1基本約束數(shù)學(xué)量與符號(hào)使用規(guī)范

2.1.1地形

記航跡段E上航點(diǎn)距地面高度的集合為H={h(p)|p∈E},ch為航跡段E的地形威脅代價(jià),hmin為最小安全飛行高度,則有

(1)

2.1.2禁飛區(qū)

由于航跡段E不能穿越禁飛區(qū),因此有代價(jià)函數(shù)

(2)

式中,cr為無(wú)人機(jī)的禁飛區(qū)代價(jià)。

2.1.3直飛距離與轉(zhuǎn)彎半徑

為滿足最小直飛距離和最小轉(zhuǎn)彎半徑約束,需要同時(shí)檢測(cè)當(dāng)前航跡段與前后相鄰航跡段的關(guān)系。

假設(shè)無(wú)人機(jī)的轉(zhuǎn)彎半徑恒定,如圖1所示,其中AB、BC、CD分別為連續(xù)航點(diǎn)形成的航跡在水平面的投影,rmin為最小轉(zhuǎn)彎半徑,EF為航跡段BC中的無(wú)人機(jī)直飛距離。α,β分別為航跡AB與BC、BC與CD的夾角。則無(wú)人機(jī)在BC航跡段的直飛距離為

(3)

式中,l為BC段的長(zhǎng)度。設(shè)lmin為需要滿足的最小直飛距離,cs為最小直飛距離與轉(zhuǎn)彎半徑代價(jià),則有

(4)

圖1　轉(zhuǎn)彎半徑約束示意圖Fig.1　Constraints of turning radius

2.1.4最大爬升角

記航跡段E的爬升角為θ,則航跡段E的爬升角代價(jià)為

(5)

式中,θmax為最大爬升角。

2.2平臺(tái)安全約束

無(wú)人機(jī)平臺(tái)安全約束包括雷達(dá)威脅源、高炮威脅源等,本來(lái)主要針對(duì)雷達(dá)威脅源代價(jià)進(jìn)行分析,將被雷達(dá)偵測(cè)到的回波能量轉(zhuǎn)化為無(wú)人機(jī)航路的代價(jià)。

由雷達(dá)特性方程可知,雷達(dá)接收到的回波功率為

(6)

式中,Pt為雷達(dá)發(fā)射功率;Gt、Gr分別為雷達(dá)發(fā)射和接收天線增益;λ為雷達(dá)波長(zhǎng);σ為無(wú)人機(jī)散射截面積;R為無(wú)人機(jī)與雷達(dá)距離。

記雷達(dá)的最小可檢測(cè)信號(hào)功率為Simin,則由式(6)可得雷達(dá)最遠(yuǎn)作用距離

(7)

假設(shè)無(wú)人機(jī)的雷達(dá)散射截面積σ為常數(shù)時(shí),代入式(6)有

(8)

對(duì)式(8)進(jìn)行歸一化,并去除極點(diǎn)得到距離雷達(dá)dr時(shí)的雷達(dá)威脅模型

(9)

記航跡段E上航點(diǎn)p距雷達(dá)距離為dr(p),則有航跡段E的雷達(dá)威脅源代價(jià)為

(10)

2.3數(shù)據(jù)鏈路與干擾

距鏈路或干擾發(fā)射天線距離為R處的信號(hào)功率密度為

(11)

記Smin為鏈路或干擾的最小有效功率,則可得鏈路或干擾的最大作用距離為

(12)

代入式(11)得

(13)

對(duì)式(13)進(jìn)行歸一化,并去除極點(diǎn)，得到距離鏈路或干擾d 時(shí)的模型

(14)

式中,L(d)和I(d)分別為鏈路和干擾模型,應(yīng)用信噪比模型,記航跡段E上一航點(diǎn)p,有一個(gè)鏈路信號(hào)源,多個(gè)干擾源的情況下,點(diǎn)p處鏈路與干擾模型為

(15)

式中,LI(p)為p點(diǎn)的鏈路與干擾代價(jià);dI_k(p)為p點(diǎn)距第k個(gè)干擾源的距離;dL(p)為p點(diǎn)距鏈路的距離。則航跡段E的鏈路與干擾的代價(jià)為

(16)

2.4航路總代價(jià)模型

由式(1)、式(2)、式(4)、式(5)、式(10)和式(16)得到無(wú)人機(jī)航路代價(jià)模型為

CT=ch+cr+cs+cp+ce+wt·ct+wl·cl=

wl·LI(d(p)))dp

(17)

式中,CT為無(wú)人機(jī)航路的總代價(jià);ce為該航路段的歐式距離;wt為對(duì)應(yīng)雷達(dá)威脅的權(quán)值系數(shù);wl為對(duì)應(yīng)鏈路與干擾的權(quán)值系數(shù)。

3　航路規(guī)劃

3.1PRM算法分析

PRM算法是一種基于取樣的規(guī)劃算法,被廣泛應(yīng)用于地面機(jī)器人的路徑規(guī)劃。PRM算法分為兩個(gè)階段:第一階段是學(xué)習(xí)階段即地圖的構(gòu)造階段；第二階段是查詢階段[9]。

在學(xué)習(xí)階段,PRM算法漸進(jìn)增地、隨機(jī)地構(gòu)建出C空間中避撞點(diǎn)和避撞路徑組成的路線圖G(V,E), 其中V為圖2中節(jié)點(diǎn)的集合,E為避撞路徑(圖2中的邊)的集合。

在查詢階段,PRM算法采用圖搜索算法在圖2中搜索從起點(diǎn)到終點(diǎn)的最優(yōu)路徑,從而將最優(yōu)路徑搜索問(wèn)題變成圖搜索問(wèn)題。查詢階段分為兩步,首先將查詢的起始點(diǎn)s和終點(diǎn)g分別連接到航路圖中的s′和g′,然后對(duì)航路圖進(jìn)行搜索,找到連接s′和g′的一系列的邊,這樣,就是實(shí)現(xiàn)了從s到g的航線的搜索。圖2為PRM算法在簡(jiǎn)單環(huán)境下求得的航跡。

圖2　PRM圖和規(guī)劃結(jié)果Fig.2　PRM diagram and results of plan

PRM算法在二維規(guī)劃空間有著良好的性能,在地面機(jī)器人的航路規(guī)劃中較多的應(yīng)用,但是由于無(wú)人機(jī)的規(guī)劃空間由二維拓展到三維,圖2中的節(jié)點(diǎn)數(shù)與邊數(shù)呈指數(shù)上升,該方法在無(wú)人機(jī)航路規(guī)劃時(shí)存在性能較低的缺陷。為提高大范圍無(wú)人機(jī)航路規(guī)劃的性能,本文提出網(wǎng)格PRM算法。

3.2網(wǎng)格PRM算法學(xué)習(xí)階段

在大范圍無(wú)人機(jī)航路規(guī)劃應(yīng)用中,由于規(guī)劃空間的高度尺度要遠(yuǎn)小于長(zhǎng)寬尺度,采用等高度間隔采樣,提出基于分層的地圖構(gòu)建。將規(guī)劃區(qū)域以高度間隔dinterval進(jìn)行分層,在每層的概率地圖構(gòu)造中將空間再劃分為邊長(zhǎng)為L(zhǎng)gridsize的正方形網(wǎng)格,在網(wǎng)格種以每單位面積的密度進(jìn)行隨機(jī)采樣。

圖3為兩種算法在學(xué)習(xí)階段的流程對(duì)比圖。

圖3　兩種算法在學(xué)習(xí)階段的流程對(duì)比圖Fig.3　Comparison of two methods during the learning phase

通過(guò)對(duì)比可以看出,網(wǎng)格PRM算法相比于傳統(tǒng)PRM算法增加了對(duì)規(guī)劃空間的網(wǎng)格劃分,同時(shí)丟棄了對(duì)路徑的地形躲避檢測(cè)。在完成地圖的構(gòu)造之后,將地圖中的邊定義為當(dāng)前節(jié)點(diǎn)與當(dāng)前節(jié)點(diǎn)所在網(wǎng)格的相鄰網(wǎng)格中的所有節(jié)點(diǎn)構(gòu)成的邊。因此,通過(guò)將空間劃分為網(wǎng)格,使得節(jié)點(diǎn)的鄰接關(guān)系轉(zhuǎn)化為網(wǎng)格的鄰接關(guān)系。

采用基于網(wǎng)格的PRM的構(gòu)造方法主要有兩大優(yōu)勢(shì)。第一,將學(xué)習(xí)階段需要大量運(yùn)算量的路徑地形躲避檢測(cè)推遲到查詢階段,加快學(xué)習(xí)階段速度；同時(shí),由于查詢階段只檢測(cè)最可行的路徑,大大減少了路徑的地形避讓檢測(cè)次數(shù),算法速度加快。第二,將采樣點(diǎn)根據(jù)網(wǎng)格劃分,只需要保存網(wǎng)格的關(guān)系,將點(diǎn)的關(guān)系由網(wǎng)格關(guān)系導(dǎo)出,大大減小存儲(chǔ)空間；同時(shí),在查詢階段可以根據(jù)網(wǎng)格關(guān)系,結(jié)合無(wú)人機(jī)飛行約束條件丟棄無(wú)用網(wǎng)格中點(diǎn)的擴(kuò)展,進(jìn)一步加快查詢速度。

3.3網(wǎng)格PRM算法查詢階段

為了加快PRM算法查詢階段的速度,在查詢階段應(yīng)用A*算法進(jìn)行搜索。

3.3.1A*搜索算法分析

A*算法在搜索過(guò)程中保留兩個(gè)節(jié)點(diǎn)表：一是open表,表中存放在節(jié)點(diǎn)擴(kuò)展過(guò)程中已經(jīng)出現(xiàn)但還沒(méi)有檢查過(guò)的節(jié)點(diǎn);二是close表,表中存放在節(jié)點(diǎn)搜索過(guò)程中已經(jīng)檢查過(guò)的節(jié)點(diǎn)。為了提高節(jié)點(diǎn)搜索效率,對(duì)open表中存放的節(jié)點(diǎn)以一定的權(quán)值進(jìn)行排列,使open表中最有希望的節(jié)點(diǎn)總是最先取出的。

在傳統(tǒng)PRM圖上應(yīng)用A*算法在節(jié)點(diǎn)擴(kuò)展時(shí)需要計(jì)算節(jié)點(diǎn)的每個(gè)相鄰節(jié)點(diǎn)?？紤]到無(wú)人機(jī)的飛行動(dòng)力約束,節(jié)點(diǎn)的有效擴(kuò)展區(qū)域如圖4所示。

圖4　節(jié)點(diǎn)有效擴(kuò)展區(qū)域Fig.4　Node effective extension area

圖4中球心Pn點(diǎn)為當(dāng)前的節(jié)點(diǎn),Pn-1點(diǎn)為Pn的上一節(jié)點(diǎn),則圖中紅色球面與藍(lán)色球面形成的球殼Rall是傳統(tǒng)PRM算法中節(jié)點(diǎn)Pn的節(jié)點(diǎn)擴(kuò)展區(qū)域,以球心為頂點(diǎn)的四棱錐與球殼的相交區(qū)域Rvalid是考慮無(wú)人機(jī)飛行性能后的節(jié)點(diǎn)有效擴(kuò)展區(qū)域。

A*算法在節(jié)點(diǎn)擴(kuò)展階段計(jì)算每一相鄰節(jié)點(diǎn)的航路代價(jià)并將其放入open表中,由于傳統(tǒng)PRM中的只包含節(jié)點(diǎn)是否相鄰的信息,因此不能利用無(wú)人機(jī)飛行約束對(duì)節(jié)點(diǎn)擴(kuò)展進(jìn)行有效縮減。網(wǎng)格PRM將節(jié)點(diǎn)分層并分塊存儲(chǔ),可以通過(guò)快速剔除無(wú)效擴(kuò)展塊來(lái)加速A*算法的擴(kuò)展速度。

3.3.2基于網(wǎng)格的A*算法節(jié)點(diǎn)擴(kuò)展

利用網(wǎng)格PRM圖的節(jié)點(diǎn)結(jié)構(gòu),得到節(jié)點(diǎn)的擴(kuò)展流程如下。

(1) 計(jì)算當(dāng)前節(jié)點(diǎn)所在網(wǎng)格的位置(x,y)和節(jié)點(diǎn)所在層z,根據(jù)無(wú)人機(jī)的飛行性能約束,快速計(jì)算當(dāng)前節(jié)點(diǎn)的有效擴(kuò)展塊。根據(jù)當(dāng)前節(jié)點(diǎn)是否為起始起點(diǎn),可將擴(kuò)展過(guò)程分為兩類情況。

① 如果當(dāng)前節(jié)點(diǎn)為起始節(jié)點(diǎn)P0,則當(dāng)前節(jié)點(diǎn)的下一節(jié)點(diǎn)為層z、z+1、z-1中的網(wǎng)格位置為(x-1,y-1)、(x,y-1)、(x+1,y-1)、(x-1,y)、(x+1,y)、(x-1,y+1)、(x,y+1)、(x+1,y+1)中的節(jié)點(diǎn),如圖5所示。

圖5　起始節(jié)點(diǎn)的塊擴(kuò)展Fig.5　Starting nodes grid extension area

② 如果當(dāng)前節(jié)點(diǎn)Pn不是起始節(jié)點(diǎn),則計(jì)算當(dāng)前節(jié)點(diǎn)的上一節(jié)點(diǎn)Pn-1所在的網(wǎng)格位置(x,y)和節(jié)點(diǎn)所在的層z,根據(jù)上一節(jié)點(diǎn)所在網(wǎng)格的位置和層可以出現(xiàn)圖6所示情況。

圖6　普通節(jié)點(diǎn)的塊擴(kuò)展Fig.6　Ordinary nodes grid extension area

圖5、圖6中,藍(lán)色網(wǎng)格為當(dāng)前節(jié)點(diǎn)所在網(wǎng)格,紅色網(wǎng)格為當(dāng)前節(jié)點(diǎn)的上一節(jié)點(diǎn)所在網(wǎng)格,綠色網(wǎng)格為當(dāng)前節(jié)點(diǎn)的擴(kuò)展節(jié)點(diǎn)所在的網(wǎng)格。

(2) 在得到節(jié)點(diǎn)的擴(kuò)展網(wǎng)格后,將擴(kuò)展網(wǎng)格中的所有節(jié)點(diǎn)作為當(dāng)前節(jié)點(diǎn)的下一節(jié)點(diǎn)作為A*搜索時(shí)的擴(kuò)展節(jié)點(diǎn)。

采用新的基于網(wǎng)格的節(jié)點(diǎn)擴(kuò)展方法的優(yōu)勢(shì)有:一是,可以有效減少由于不符合最小航跡段長(zhǎng)度要求和較長(zhǎng)航跡段的節(jié)點(diǎn)的無(wú)效擴(kuò)展；二是,可以減少由于不符合無(wú)人機(jī)最小轉(zhuǎn)彎半徑的節(jié)點(diǎn)的無(wú)效擴(kuò)展；三是,可以減少不符合無(wú)人機(jī)最大爬升角的航點(diǎn)的無(wú)效擴(kuò)展。

3.3.3A*算法的函數(shù)選取

在得到當(dāng)前節(jié)點(diǎn)的擴(kuò)展節(jié)點(diǎn)后對(duì)所有的擴(kuò)展節(jié)點(diǎn)進(jìn)行評(píng)價(jià)。選取式(17)作為代價(jià)函數(shù),保證了規(guī)劃航跡的準(zhǔn)確性和有效性。選取較大的啟發(fā)函數(shù)可以加速算法的收斂,但過(guò)大的啟發(fā)函數(shù)將使規(guī)劃失去最優(yōu)解,為了保證航路的確定性,將歐氏距離作為啟發(fā)函數(shù)。由式(17)可知,即使在不同的權(quán)重因子的情況下,可以保證啟發(fā)函數(shù)將嚴(yán)格小于代價(jià)函數(shù),保證了航路規(guī)劃的確定性。假設(shè)當(dāng)前節(jié)點(diǎn)為Pcur,上一節(jié)點(diǎn)為Ppre,終點(diǎn)為Pg,則Pcur的代價(jià)為

(18)

Pcur的優(yōu)先級(jí)為

Pcur·priority=Pcur·cost+ce(Pcur,Pg)

(19)

3.4算法復(fù)雜度分析

現(xiàn)對(duì)網(wǎng)格PRM和傳統(tǒng)PRM法算法復(fù)雜度進(jìn)行分析。設(shè)規(guī)劃區(qū)域采樣點(diǎn)數(shù)為N,傳統(tǒng)PRM法在學(xué)習(xí)階段每一節(jié)點(diǎn)可平均形成M條可行路徑,同時(shí)網(wǎng)格PRM法在采樣區(qū)域采樣個(gè)數(shù)為m,設(shè)最終規(guī)劃結(jié)果節(jié)點(diǎn)數(shù)為n。

3.4.1學(xué)習(xí)階段

傳統(tǒng)PRM:由于傳統(tǒng)PRM在學(xué)習(xí)階段每次采樣都進(jìn)行路經(jīng)檢測(cè),因此時(shí)間復(fù)雜度為O(N2)。

網(wǎng)格PRM:網(wǎng)格PRM法在學(xué)習(xí)階段不進(jìn)行路經(jīng)檢測(cè),時(shí)間復(fù)雜度為O(N)。

3.4.2查詢階段

傳統(tǒng)PRM:由于傳統(tǒng)PRM在查詢階段不能對(duì)搜索進(jìn)行剪枝,因此時(shí)間復(fù)雜度為O(Mn)。

網(wǎng)格PRM:網(wǎng)格PRM法在查詢階段平均每次僅擴(kuò)展6～9個(gè)網(wǎng)格分節(jié)點(diǎn),假設(shè)平均為8個(gè)網(wǎng)格,則時(shí)間復(fù)雜度為O((8m)n)。通過(guò)對(duì)高度間隔dinterval及網(wǎng)格邊長(zhǎng)為L(zhǎng)gridsize的值進(jìn)行選取應(yīng)有關(guān)系:

M≈26m

(19)

式(19)表示網(wǎng)格PRM法中的網(wǎng)格中節(jié)點(diǎn)的可行路徑與傳統(tǒng)PRM法中節(jié)點(diǎn)的可行路徑數(shù)量想當(dāng)。則網(wǎng)格PRM法時(shí)間復(fù)雜度約為O((M/3)n)。

通過(guò)比較可知網(wǎng)格PRM法較傳統(tǒng)PRM法在學(xué)習(xí)階段和查詢階段時(shí)間復(fù)雜度有巨大優(yōu)勢(shì)。

4　實(shí)驗(yàn)驗(yàn)證

選取100km×100km的地形,使用Matlab繪制地形高度圖,如圖7所示,在地圖的右下方存在高度3 000m的山脈。

圖7　高度地形圖Fig.7　Topographic map

設(shè)置采樣高度間隔dinterval=250m,網(wǎng)格邊長(zhǎng)Lgridsize=5km,每平方公里采樣密度為5個(gè)。無(wú)人機(jī)最小直飛航跡段為ls=5km,最小轉(zhuǎn)彎半徑為rmin=1km。起始點(diǎn)坐標(biāo)為(1km,1km,0.28km),終點(diǎn)坐標(biāo)為(99km,99km,1.32km)。在沒(méi)有禁飛區(qū)、雷達(dá)威脅源,且不考慮數(shù)據(jù)鏈路的情況下,得到規(guī)劃航跡如圖8所示。

圖8　場(chǎng)景1規(guī)劃結(jié)果Fig.8　Planning results of scene 1

從圖8可看出，當(dāng)環(huán)境簡(jiǎn)單時(shí),規(guī)劃的航路為避開地形的從起始點(diǎn)到終點(diǎn)的近似直線。

分別在(86km,78km)和(12km,20km)處設(shè)置半徑為12km的禁飛區(qū),得到航跡如圖9所示。

圖9　場(chǎng)景2規(guī)劃結(jié)果Fig.9　Planning results of scene 2

從圖9可以看出，規(guī)劃出的無(wú)人機(jī)航路成功避開了人工禁飛區(qū),但無(wú)人機(jī)航路較圖8更長(zhǎng)。

在以上基礎(chǔ)上增加雷達(dá)威脅源,對(duì)于小型無(wú)人機(jī),雷達(dá)對(duì)其可偵察最大半徑較常規(guī)目標(biāo)縮小,因此在(35km,68km,0.482km)處設(shè)置半徑為36km的雷達(dá)威脅,分別在不同的雷達(dá)威脅權(quán)重時(shí)得到如圖10所示航跡。航跡1和航跡2分別為權(quán)重因子取值為5.0和1.0時(shí)的規(guī)劃航跡。

圖10　場(chǎng)景3和場(chǎng)景4規(guī)劃結(jié)果Fig.10　Planning results of scene 3 and scene 4

由圖10可以看出,當(dāng)航路的雷達(dá)威脅權(quán)重因子為1.0時(shí)航路掠過(guò)雷達(dá)威脅源,當(dāng)雷達(dá)威脅權(quán)重因子為5.0時(shí)航路完全避開了威脅源,但較前者航路長(zhǎng)度增加。因此,設(shè)置不同的雷達(dá)威脅權(quán)重因子可在航路的安全性和其他因素之間進(jìn)行權(quán)衡。

當(dāng)考慮無(wú)人機(jī)鏈路安全的因素,在(17km,92km,1.1km)處設(shè)置半徑為109km的鏈路,在(64km,36km,0.738km)處設(shè)置半徑為41km的干擾源,得到如圖11所示航跡。

圖11　場(chǎng)景5規(guī)劃結(jié)果Fig.11　Planning results of scene 5

本文測(cè)試計(jì)算機(jī)配置如下:

CPU:Intel(R)Core(TM)i3M330 @ 2.13GHz

RAM: 4.0GB

傳統(tǒng)PRM法和網(wǎng)格PRM法的航路規(guī)劃耗時(shí)如表1所示。

表1　航跡規(guī)劃時(shí)間

由表1可知,在相同場(chǎng)景下網(wǎng)格PRM比傳統(tǒng)PRM規(guī)劃速度快5～10倍不等。同時(shí),傳統(tǒng)PRM法在航路規(guī)劃時(shí)需要內(nèi)存為1 200 MB左右,網(wǎng)格PRM法所需內(nèi)存僅為90 MB左右,所需運(yùn)行時(shí)空間顯著降低。由于選取的啟發(fā)函數(shù)為歐式距離函數(shù),故兩種算法都為確定性算法,因此規(guī)劃結(jié)果相似。由表1可知,網(wǎng)格PRM法在不同的場(chǎng)景下,規(guī)劃耗時(shí)較短,規(guī)劃航路滿足無(wú)人機(jī)實(shí)際飛行要求,規(guī)劃速度滿足離線規(guī)劃要求。

5　結(jié)　論

本文針對(duì)PRM算法應(yīng)用于三維航跡規(guī)劃時(shí)效率低的問(wèn)題,結(jié)合無(wú)人機(jī)航路規(guī)劃時(shí)的約束條件,對(duì)無(wú)人機(jī)航路規(guī)劃進(jìn)行研究,得到了如下成果:

(1) 對(duì)無(wú)人機(jī)真實(shí)飛行時(shí)的約束條件進(jìn)行分類,對(duì)不同的約束條件分別建立了模型,同時(shí)得到了約束條件的總代價(jià)模型;

(2) 提出了網(wǎng)格PRM算法,實(shí)現(xiàn)了三維空間大范圍環(huán)境下的航路的快速規(guī)劃；

(3) 針對(duì)典型情景進(jìn)行仿真,快速規(guī)劃出了滿足安全性要求的無(wú)人機(jī)航路,證明了本方法的有效性和高效性。

然而，本文中的方法對(duì)于動(dòng)態(tài)的環(huán)境信息不具備處理能力,今后將繼續(xù)改進(jìn)算法以實(shí)現(xiàn)無(wú)人機(jī)局部實(shí)時(shí)的航路重規(guī)劃。同時(shí),本文實(shí)現(xiàn)了起飛點(diǎn)到任務(wù)地點(diǎn)的航路規(guī)劃,在此基礎(chǔ)上將對(duì)無(wú)人機(jī)的載荷規(guī)劃進(jìn)行研究。

[1] Bellini A C, Lu W, Naldi R, et al. Information driven path planning and control for collaborative aerial robotic sensors using artificial potential functions[C]∥Proc.oftheAmericanControlConference, 2014: 590-597.

[2] Mao H B, Tian S, Chao A N, et al.UAVmissionplanning[M]. Beijing: National Defense Industry Press, 2015:82-83.(毛紅保,田松,晁愛農(nóng),等.無(wú)人機(jī)任務(wù)規(guī)劃[M].北京:國(guó)防工業(yè)出版社,2015:82-83.)

[3] Zhang T S, Lu Y, Zhang L, et al. UAV path planning based on improved Voronoi diagram and dynamic weights A*algorithm[J].FireControl&CommandControl,2015(2):156-160.(張?zhí)陨?魯藝,張亮,等.改進(jìn)Voronoi圖和動(dòng)態(tài)權(quán)值A(chǔ)*算法的無(wú)人機(jī)航跡規(guī)劃[J]. 火力與指揮控制, 2015(2):156-160.)

[4] Yan J L. Evolutionary algorithm based route planer for unmanned air vehicles[D]. Nanjing: Nanjing University of Aeronautics and Astronautics, 2008.(嚴(yán)建林. 基于進(jìn)化算法無(wú)人機(jī)航路規(guī)劃技術(shù)研究[D]. 南京:南京航空航天大學(xué), 2008.)

[5] Ozalp N, Sahingoz O K. Optimal UAV path planning in a 3D threat environment by using parallel evolutionary algorithms[C]∥Proc.oftheIEEEInternationalConferenceonUnmannedAircraftSystems, 2013: 308-317.

[6] Li D, Cao Y H, Su Y, et al. Trajectory planning for low attitude penetration based on improved ant colony algorithm[J].JournalofBeijingUniversityofAeronauticsandAstronautics, 2006, 32(3):258-262.(李棟, 曹義華, 蘇媛, 等. 基于改進(jìn)蟻群算法的低空突防航跡規(guī)劃[J]. 北京航空航天大學(xué)學(xué)報(bào), 2006, 32(3):258-262.)

[7] Ma G J, Duan H B, Liu S Q. Improved ant colony algorithm for global optimal trajectory planning of UAV under complex environment[J].InternationalJournalofComputerScienceandApplications, 2007，4(3):57-68.

[8] Overmars M H, Mark H. A random approach to motion planning[R].Netherlamd: Utrecht University Repository, 1992.

[9] Kavraki L E, Svestka P, Latombe J C, et al. Probabilistic roadmaps for path planning in high-dimensional configuration spaces[J].IEEETrans.onRoboticsandAutomation,1996,12(4):566-580.

[10] Amato N M, Bayazit O B, Dale L K, et al. Choosing good distance metrics and local planners for probabilistic roadmap methods[C]∥Proc.oftheIEEEInternationalConferenceonRoboticsandAutomation, 1998: 630-637.

[11] Amato N M, Wu Y. A randomized roadmap method for path and manipulation planning[C]∥Proc.oftheIEEEInternationalConferenceonRoboticsandAutomation, 1996: 113-120.

[12] Morales M, Rodriguez S, Amato N M. Improving the connectivity of PRM roadmaps[C]∥Proc.oftheIEEEInternationalConferenceonRoboticsandAutomation, 2003: 4427-4432.

[13] Song G, Amato N M. Randomized motion planning for car-like robots with C-PRM[C]∥Proc.oftheIEEEInternationalConferenceonIntelligentRobotsandSystems, 2001:37-42.

[14] Balakirsky S, Dimitrov D. Single-query, Bi-directional, Lazy roadmap planner applied to car-like robots[C]∥Proc.oftheIEEEInternationalConferenceonRoboticsandAutomation, 2010:5015-5020.

[15] Poppinga J, Birk A, Pathak K, et al. Fast 6-DOF path planning for autonomous underwater vehicles(AUV) based on 3D plane mapping[C]∥Proc.oftheIEEEInternationalSymposiumonSafety,Security,andRescueRobotics, 2011: 345-350.

[15] Li Q R, Wei C, Wu J, et al. Improved PRM method of low altitude penetration trajectory planning for UAVs[C]∥Proc.oftheIEEEChineseGuidance,NavigationandControlConference, 2014: 2651-2654.

Multi-constraints UAV path planning based on grid PRM

ZENG Guo-qi1, ZHAO Min-qiang2, LIU Fang-yuan2, DING Wen-rui1

(1. UAV Research Academy, Beihang University, Beijing 100191, China;2. School of Electronic Information Engineering, Beihang University, Beijing 100191, China)

Nowadaystheunmannedarielvehicle(UAV)’spathplanningtechnologyhasseveraldefects,suchasfewconstraintswhichleadtodissatisfactoryofrealisticflyingdemands,lowefficiencyinpathplanning,thispapermainlydealswiththeseproblems.First,classifytheconstraintsintothreetypes,basicconstraints,platformsafetyconstraintsandlinkloadconstraints.ThenmodeltheconstraintstorefineUAVpathconstrainmodel.Next,toimprovetheplanningefficiencythegridprobabilisticroadmap(GPRM)methodisproposed.Finally,bydevisingacostfunctionusingconstraintmodels,thehighefficiencymulti-constraintspathplanningforUAVisobtained.SimulationsshowthatpathplanningusingGPRMisfasterthanthatusingtraditionalPRM,andalsonoticethatthepathobtainedbyGPRMismorerealisticfortheactualflight,thustheengineerapplicationvalueofthismethodisproved.

unmannedarielvehicle(UAV);pathplanning;gridprobabilisticroadmap(GPRM)method;multi-constraints;linkload

2015-09-20；

2016-05-31；網(wǎng)絡(luò)優(yōu)先出版日期：2016-06-22。

國(guó)家高技術(shù)研究發(fā)展計(jì)劃(863計(jì)劃)(2013AA122103)；中央高?；究蒲袠I(yè)務(wù)費(fèi)專項(xiàng)資金(YWF-14-WRJS-005,YWF-15-WRJS-001,YWF-15-GJSYS-061)資助課題

V279;TP301.6

ADOI:10.3969/j.issn.1001-506X.2016.10.13

曾國(guó)奇(1978-),男,高級(jí)實(shí)驗(yàn)師,博士,主要研究方向?yàn)闊o(wú)人機(jī)測(cè)控、電磁兼容、共形相控陣天線。

E-mail:zengguoqi@buaa.edu.cn

趙民強(qiáng)(1989-),男,碩士,主要研究方向?yàn)闊o(wú)人機(jī)任務(wù)規(guī)劃。

E-mail:beyond21299@163.com

劉方圓(1989-),男,碩士,主要研究方向?yàn)闊o(wú)人機(jī)航路規(guī)劃。

E-mail:1019365767@qq.com

丁文銳(1971-),女,研究員,博士,主要研究方向?yàn)閳D像處理和信號(hào)處理。

E-mail:ding@buaa.edu.cn

網(wǎng)絡(luò)優(yōu)先出版地址：http://www.cnki.net/kcms/detail/11.2422.TN.20160622.1127.006.html