基于狹窄通道路徑樹的快速航路規(guī)劃算法*

冉華明

(中國(guó)電子科技集團(tuán)公司航空電子信息系統(tǒng)技術(shù)重點(diǎn)實(shí)驗(yàn)室,成都 610036)

0 引 言

無(wú)人機(jī)在城市環(huán)境中進(jìn)行低空飛行時(shí)所處的環(huán)境十分復(fù)雜，存在大量密集不規(guī)則障礙等，這些障礙之間可能存在狹小的縫隙，合理地利用這些縫隙規(guī)劃無(wú)人機(jī)的低空飛行航路，在保證無(wú)人機(jī)安全的同時(shí)縮短無(wú)人機(jī)的航程，可有效提升無(wú)人機(jī)在城市環(huán)境中執(zhí)行各類任務(wù)的效能和安全性[1-3]。

無(wú)人機(jī)航路規(guī)劃是指無(wú)人機(jī)在特定規(guī)劃環(huán)境下規(guī)劃制定從初始位置到目標(biāo)位置滿足安全性、時(shí)效性等性能指標(biāo)的最優(yōu)飛行路徑。目前常用的航路規(guī)劃算法主要有遺傳算法、蟻群算法、快速擴(kuò)展隨機(jī)樹(Rapidly-exploring Random Tree,RRT)算法和A*算法等[4-11]。遺傳算法[5-6]通過(guò)模擬自然界生物交叉遺傳、個(gè)體變異、優(yōu)勝劣汰等進(jìn)化過(guò)程進(jìn)行航路規(guī)劃搜索，不需要確定的模型，算法魯棒性強(qiáng)，但在進(jìn)行復(fù)雜航路規(guī)劃時(shí)存在編碼困難復(fù)雜、搜索時(shí)間長(zhǎng)、收斂性能差等問(wèn)題。蟻群算法[7-8]具有正反饋、分布計(jì)算等優(yōu)點(diǎn)，但算法初始信息素設(shè)定和信息素更新規(guī)則難以確定。RRT算法[9-10]通過(guò)隨機(jī)采樣避免了對(duì)空間的建模過(guò)程，同時(shí)可以避免對(duì)一些非完整航路規(guī)劃約束進(jìn)行建模，適合在復(fù)雜約束環(huán)境下的航路規(guī)劃，但RRT算法不一定得到最優(yōu)航路，同時(shí)隨機(jī)搜索會(huì)消耗較大的無(wú)效計(jì)算資源。A*算法[11-12]是一種啟發(fā)式搜索算法，需要將整個(gè)規(guī)劃環(huán)境柵格化，而柵格的顆粒度對(duì)航路整體的精度和搜索計(jì)算時(shí)間有著很大的影響，較小的顆粒度往往會(huì)得到較高精度的航路，但其計(jì)算時(shí)間卻會(huì)大大增加，因此需要選擇一個(gè)合適的柵格顆粒度來(lái)權(quán)衡航路精度與計(jì)算時(shí)間的需求沖突，這就使得該方法一般只能找到較優(yōu)解，而非最優(yōu)解。

在復(fù)雜環(huán)境中，由于障礙位置、形狀大小等因素，各障礙之間可能存在一定的縫隙，即狹窄通道。當(dāng)前常用的航路規(guī)劃方法在求解存在狹窄通道場(chǎng)景下的航路規(guī)劃問(wèn)題時(shí)還存在一些不足之處。

一是計(jì)算時(shí)間長(zhǎng)。由于狹窄通道的寬度很窄，長(zhǎng)度較長(zhǎng)，A*算法需要很小的柵格顆粒度才能將狹窄通道與周圍的障礙環(huán)境區(qū)分開，這將大大增加航路搜索的時(shí)間；RRT算法在狹窄通道擴(kuò)展時(shí)很容易擴(kuò)展到障礙范圍內(nèi)，需耗費(fèi)大量時(shí)間才能得到一條通過(guò)狹窄通道的隨機(jī)擴(kuò)展樹。

二是規(guī)劃效果差。當(dāng)規(guī)劃環(huán)境還存在除了狹窄通道之外的其他通道時(shí)，航路規(guī)劃算法往往會(huì)得到一條未穿過(guò)狹窄通道的航路，而該航路的性能比穿過(guò)狹窄通道的航路往往要差很多；當(dāng)復(fù)雜環(huán)境只存在狹窄通道時(shí)，航路規(guī)劃算法可能會(huì)搜索不到穿過(guò)狹窄通道的航路，導(dǎo)致航路規(guī)劃失敗。

Lien等[13]采用中軸線采樣方法，對(duì)隨機(jī)路標(biāo)圖(Probabilistic Roadmap,PRM)算法進(jìn)行了改進(jìn)，解決其難以覆蓋狹窄通道的問(wèn)題，但該方法只能實(shí)現(xiàn)直線型狹窄通道環(huán)境下的航路規(guī)劃，難以實(shí)現(xiàn)多個(gè)障礙之間存在復(fù)雜狹窄通道環(huán)境下的航路規(guī)劃。鐘建冬、Wei等人[14-16]提出了一種星形試驗(yàn)法，實(shí)現(xiàn)對(duì)狹窄通道的識(shí)別能力，并提出了三樹RRT算法和混合概率路標(biāo)圖法，實(shí)現(xiàn)機(jī)器人在狹窄通道路徑環(huán)境中的路徑規(guī)劃，但該算法存在狹窄通道識(shí)別收斂慢、無(wú)法利用多個(gè)狹窄通道的問(wèn)題。

本文在第1節(jié)中參考Voronoi法[17]求解復(fù)雜環(huán)境中各障礙之間的狹窄通道，基于此構(gòu)建穿過(guò)所有障礙之間可行縫隙的狹窄通道路徑樹；在第2節(jié)中對(duì)雙向RRT算法進(jìn)行了改進(jìn)，結(jié)合狹窄通道路徑樹構(gòu)建復(fù)雜環(huán)境下的快速航路規(guī)劃模型，快速有效地規(guī)劃出復(fù)雜環(huán)境中的航路；最后在第3節(jié)中通過(guò)仿真對(duì)算法進(jìn)行了驗(yàn)證。

1 復(fù)雜環(huán)境預(yù)處理

航路規(guī)劃環(huán)境中各種障礙之間存在狹窄通道，通過(guò)復(fù)雜航路規(guī)劃環(huán)境預(yù)處理，獲取能充分利用障礙間的狹窄通道的狹窄通道路徑樹，為快速有效規(guī)劃出穿越各狹窄縫隙的航路奠定基礎(chǔ)，如圖1所示。

圖1 狹窄通道示意圖

1.1 鄰近障礙間狹窄通道

通過(guò)判斷鄰近障礙之間的空間位置關(guān)系，確定和計(jì)算所有障礙之間的狹窄通道路徑。

設(shè)復(fù)雜環(huán)境中第i個(gè)障礙的頂點(diǎn)為[xi1,yi1;…;xin,yin;…;xiNi,yiNi]，其中，Ni表示i個(gè)障礙的頂點(diǎn)數(shù)量，xin,yin表示第i個(gè)障礙的第n個(gè)頂點(diǎn)的橫坐標(biāo)和縱坐標(biāo)。

設(shè)障礙i的第n條邊為[xin,yin;xin+1,yin+1]，障礙j的第m條邊為[xjm,yjm;xjm+1,yjm+1]，則這兩條邊的交點(diǎn)計(jì)算公式為[18]

(1a)

(1b)

式中：ti、tj表示交點(diǎn)距離兩條邊起點(diǎn)的比例。則障礙i的第n條與障礙j的第m條邊相交的判斷條件為[18]

rank(A)=2∩0≤ti≤1∩0≤tj≤1 。

(2)

式中：rank()為矩陣的秩。

通過(guò)射線法[19]判斷障礙的頂點(diǎn)是否在另外一個(gè)障礙內(nèi)，通過(guò)式(2)判斷兩個(gè)障礙之間是否存在相交的邊來(lái)確定障礙之間的關(guān)系。若兩個(gè)障礙有邊相交或者有頂點(diǎn)在另一個(gè)障礙范圍內(nèi)，則兩個(gè)障礙相交或包含；否則，兩個(gè)障礙相離。

將障礙的頂點(diǎn)和邊作為障礙的對(duì)象qin，其中i表示障礙的編號(hào)，n表示該障礙上對(duì)象的編號(hào)。對(duì)于相離的兩個(gè)障礙i和j，基于Voronoi圖求解兩個(gè)障礙之間的最短距離對(duì)象對(duì)〈qin,qjm〉，即第i個(gè)障礙上的第n個(gè)對(duì)象與第j個(gè)障礙上的第m個(gè)對(duì)象之間的距離最短[17]。若qin和qjm之間的最短連線的距離小于狹窄通道的門限值，則最短連線的垂直平分線即為兩個(gè)障礙間的狹窄通道路徑。鄰近障礙間的狹窄通道計(jì)算流程如圖2所示。

圖2 鄰近障礙間的狹窄通道計(jì)算流程

1.2 狹窄通道路徑樹

在求解完復(fù)雜航路規(guī)劃環(huán)境中所有障礙之間的狹窄通道路徑后，綜合所有狹窄通道路徑信息、障礙范圍信息，通過(guò)判斷所有狹窄通道路徑與各障礙范圍的關(guān)系，剔除被障礙覆蓋的狹窄通道，計(jì)算在障礙范圍外的所有狹窄通道路徑集合；再通過(guò)將相交或者相互之間距離小于一定閾值的狹窄通道路徑聯(lián)合起來(lái)組成復(fù)雜航路規(guī)劃環(huán)境中的狹窄通道路徑樹。

對(duì)每一條狹窄通道路徑，通過(guò)射線法[19]判斷其起點(diǎn)和終點(diǎn)是否在同一障礙的范圍內(nèi)，若在障礙范圍內(nèi)，則剔除該狹窄路徑；否則，計(jì)算狹窄通道路徑與障礙的交點(diǎn)信息，保留在障礙范圍外的狹窄通道路徑作為新的狹窄通道路徑。當(dāng)完成對(duì)所有狹窄通道路徑的判斷后，得到在障礙范圍外的新的狹窄通道路徑集合。

通過(guò)式(2)判斷各狹窄通道路徑之間是否相交，若相交，則相交的兩條狹窄通道路徑可構(gòu)成一棵狹窄通道路徑樹：

對(duì)還未加入狹窄通道路徑樹上的狹窄通道路徑，計(jì)算其與已有狹窄通道路徑樹的相交情況，若相交，則將該狹窄路徑加入到該狹窄通道路徑樹上。

當(dāng)存在多棵不相交的狹窄通道路徑樹時(shí)，計(jì)算每棵樹上節(jié)點(diǎn)距離其他樹的最近距離，若距離小于最小距離閾值，則將兩棵樹合并為一棵新的樹。點(diǎn)[x,y]距離狹窄通道路徑樹上第i個(gè)樹枝[xi1,yi1;xi2,yi2]的最短距離dmin為

(3)

求解狹窄通道路徑樹的流程如圖3所示。

圖3 狹窄通道路徑樹求解流程

2 快速航路規(guī)劃模型

通過(guò)復(fù)雜環(huán)境預(yù)處理確定航路規(guī)劃環(huán)境的狹窄通道路徑樹后，結(jié)合雙向RRT算法，構(gòu)建復(fù)雜環(huán)境下的快速航路規(guī)劃模型，快速規(guī)劃出復(fù)雜環(huán)境中的航路。

2.1 雙向RRT算法

RRT算法通過(guò)隨機(jī)搜索的方式，能夠在障礙物復(fù)雜的高維非凸空間中快速搜索到一條路徑[9]。雙向RRT算法從航路規(guī)劃的起點(diǎn)和終點(diǎn)分別生成一棵搜索樹，同時(shí)不斷對(duì)兩棵搜索樹進(jìn)行擴(kuò)展，當(dāng)兩棵搜索樹相連后就形成一棵連接航路規(guī)劃起點(diǎn)和終點(diǎn)的航路樹，并對(duì)航路樹進(jìn)行刪減平滑生成航路，提升了航路規(guī)劃的速度[10]。

如圖4所示，以航路規(guī)劃的起點(diǎn)為起始節(jié)點(diǎn)ninit生成搜索樹1，在航路規(guī)劃環(huán)境中隨機(jī)生成點(diǎn)nrand，并選擇搜索樹1上距隨機(jī)點(diǎn)nrand最近的點(diǎn)nnear，從該點(diǎn)向隨機(jī)點(diǎn)nrand的方向擴(kuò)展一個(gè)搜索步長(zhǎng)ε，得到一個(gè)新的節(jié)點(diǎn)nnew。當(dāng)新的節(jié)點(diǎn)不在威脅范圍內(nèi)時(shí)，則將該節(jié)點(diǎn)添加到搜索樹1上，通過(guò)該方法實(shí)現(xiàn)搜索樹1的不斷擴(kuò)展。當(dāng)搜索樹新擴(kuò)展的節(jié)點(diǎn)與另一棵樹的節(jié)點(diǎn)的最短距離小于搜索步長(zhǎng)時(shí)，則連接兩棵樹上最近的節(jié)點(diǎn)，生成一棵航路樹。

圖4 搜索樹和航路樹示意圖

2.2 結(jié)合狹窄通道路徑樹的雙向RRT算法

鑒于雙向RRT算法搜索的隨機(jī)性，其很難搜索到處于障礙之間狹窄縫隙的有效樹節(jié)點(diǎn)，而第1節(jié)中設(shè)計(jì)的狹窄通道路徑樹可穿越障礙之間各狹窄縫隙，基于此設(shè)計(jì)了一種結(jié)合狹窄通道路徑樹的雙向RRT算法，通過(guò)判斷環(huán)境預(yù)處理生成的狹窄通道路徑樹與雙向RRT算法生成的兩棵搜索樹之間的空間關(guān)系，將滿足條件的狹窄通道路徑樹添加到搜索樹上，減少雙向RRT算法搜索位于障礙間狹窄縫隙中的樹節(jié)點(diǎn)所耗費(fèi)的時(shí)間，提升路徑搜索的效率和生成的路徑的效能。

在該算法中，將預(yù)處理得到的狹窄通道路徑樹作為一個(gè)樹枝添加到搜索樹上，改變了傳統(tǒng)雙向RRT算法中搜索樹每次只能擴(kuò)展一個(gè)樹節(jié)點(diǎn)的模式，減少了搜索樹在狹窄通道中的搜索時(shí)間，提升了兩棵搜索樹相連生成航路樹的速度。

通過(guò)結(jié)合狹窄通道路徑樹的雙向RRT算法進(jìn)行航路規(guī)劃的流程如圖5所示。

圖5 結(jié)合狹窄通道路徑樹的雙向RRT算法流程

3 仿真與分析

無(wú)人機(jī)低空飛行的航路規(guī)劃環(huán)境中存在4個(gè)障礙，4個(gè)障礙的頂點(diǎn)坐標(biāo)信息如表1所示。

表1 障礙頂點(diǎn)坐標(biāo)

無(wú)人機(jī)航路規(guī)劃的起點(diǎn)坐標(biāo)為(3 km,35 km)，航路規(guī)劃的終點(diǎn)坐標(biāo)為(10 km,60 km)。

通過(guò)復(fù)雜航路預(yù)處理生成的狹窄通道路徑樹如圖6所示，整個(gè)狹窄通道路徑樹由14個(gè)節(jié)點(diǎn)組成，各節(jié)點(diǎn)信息如表2所示。

圖6 狹窄通道路徑樹仿真結(jié)果

表2 狹窄通道路徑樹節(jié)點(diǎn)信息

通過(guò)結(jié)合狹窄通道路徑樹的雙向RRT算法，規(guī)劃出的航路如圖7所示，航路包含5個(gè)關(guān)鍵航路點(diǎn)，具體信息如表3所示。

圖7 航路規(guī)劃仿真結(jié)果

表3 關(guān)鍵航路點(diǎn)信息

對(duì)于同一種仿真情景，采用傳統(tǒng)雙向RRT算法、A*算法進(jìn)行航路規(guī)劃，并與本文提出的算法規(guī)劃出的航路進(jìn)行對(duì)比，結(jié)果如圖8所示，可以看出本文提出的算法能充有效利用障礙間的狹窄通道。

圖8 三種航路規(guī)劃算法仿真對(duì)比

三種算法規(guī)劃出的航路的航程以及規(guī)劃耗時(shí)統(tǒng)計(jì)如表4所示，可以看出本文所提算法在航程及規(guī)劃耗時(shí)方面均優(yōu)于另外兩種算法。

表4 三種航路規(guī)劃算法性能對(duì)比

4 結(jié) 論

本文提出的狹窄通道路徑樹求解方法，基于各障礙之間的幾何關(guān)系，能夠快速生成穿過(guò)復(fù)雜環(huán)境中各障礙之間狹窄通道的路徑樹，實(shí)現(xiàn)對(duì)復(fù)雜航路規(guī)劃環(huán)境的快速有效處理。

本文提出的結(jié)合狹窄通道路徑樹的雙向RRT算法，通過(guò)在航路樹的搜索過(guò)程中添加狹窄通道路徑樹，實(shí)現(xiàn)航路樹在復(fù)雜環(huán)境障礙間的狹窄通道中快速擴(kuò)展，提升航路樹搜索的速度。

仿真結(jié)果表明，基于狹窄通道路徑樹的快速航路規(guī)劃算法相較傳統(tǒng)RRT算法和A*算法，能夠有效地利用復(fù)雜環(huán)境中的狹窄通道，能夠在0.67 s的時(shí)間內(nèi)規(guī)劃出航程最短的航路，規(guī)劃耗時(shí)小于傳統(tǒng)雙向RRT算法所需的1.07 s和A*算法所需的7.21 s,適用于無(wú)人機(jī)在復(fù)雜城市環(huán)境中進(jìn)行低空飛行時(shí)的航路規(guī)劃。