陳錦濤 李鴻一 任鴻儒 魯仁全

隨著城市化進(jìn)程加速,高層建筑數(shù)量急劇增加,而高層建筑火災(zāi)事故給現(xiàn)有的消防救援技術(shù)與設(shè)備帶來(lái)了新的挑戰(zhàn)[1-2].現(xiàn)有的消防舉高設(shè)備通常無(wú)法觸及高于18 層的起火樓層,消防官兵負(fù)重登樓代價(jià)大[3-4],建筑內(nèi)部環(huán)境復(fù)雜,充滿高溫?zé)焿m,人員疏散極其困難[5].此時(shí),基于多無(wú)人機(jī) (Multi-unmanned aerial vehicles,Multi-UAVs) 協(xié)同控制技術(shù)的高層消防救援方法,在高層建筑消防滅火中的應(yīng)用愈發(fā)廣泛[6-8].多無(wú)人機(jī)可通過(guò)破窗,迅速進(jìn)入高層火災(zāi)現(xiàn)場(chǎng),嘗試撲滅初期火災(zāi)[9];在室內(nèi)飛行時(shí)實(shí)現(xiàn)自主避障、自主探測(cè)火點(diǎn)及被困人員;可通過(guò)揚(yáng)聲器、照明燈等輔助設(shè)備協(xié)助被困人員撤離[10].多無(wú)人機(jī)協(xié)同控制現(xiàn)已成為控制領(lǐng)域的研究熱點(diǎn)[11].在高層火災(zāi)現(xiàn)場(chǎng)的室內(nèi)環(huán)境進(jìn)行多無(wú)人機(jī)協(xié)同路徑規(guī)劃、協(xié)同救援的研究,其難點(diǎn)在于規(guī)劃空間小、避障裕度低、限制條件多[12]等.

無(wú)人機(jī)路徑規(guī)劃算法主要有可視圖法、柵格法、快速遍歷隨機(jī)樹(shù)法(Rapidly-exploring random tree,RRT)等[13].其中,RRT 算法因不需要對(duì)環(huán)境進(jìn)行精確建模,搜索性較強(qiáng),以及對(duì)時(shí)變環(huán)境適應(yīng)性良好等優(yōu)勢(shì)[14],在路徑規(guī)劃領(lǐng)域中有著廣泛的應(yīng)用.對(duì)于RRT 算法及其改進(jìn)型算法,文獻(xiàn)[15]在前人的研究成果上對(duì)RRT 算法進(jìn)行了總結(jié),主要包括雙向RRT (Bidirectional RRT,Bi-RRT)、偏向目標(biāo)RRT (RRT-Goalbiasing) 以及RRT-Connect 算法,并以此為基礎(chǔ)提出了基于人工勢(shì)場(chǎng)法的改進(jìn)RRT 算法,一定程度上提高了算法的路徑規(guī)劃效率,減少了路徑規(guī)劃時(shí)間.文獻(xiàn)[16-17]研究了動(dòng)態(tài)步長(zhǎng)RRT 路徑規(guī)劃算法,提高了隨機(jī)樹(shù)擴(kuò)展的效率.然而,上述改進(jìn)算法為提高到達(dá)終點(diǎn)的速度,傾向于采用降低搜索廣度的方法對(duì)RRT 算法進(jìn)行優(yōu)化,這樣的優(yōu)化對(duì)于復(fù)雜的高層室內(nèi)火災(zāi)環(huán)境來(lái)講是難以適用的.有學(xué)者[18-21]曾提出在確定的位置上增加少量隨機(jī)樹(shù)以解決此問(wèn)題,但該方法更傾向于通過(guò)人工干預(yù)的形式選擇輔助隨機(jī)樹(shù)根節(jié)點(diǎn)的位置,不適合根據(jù)高層火災(zāi)室內(nèi)環(huán)境變化頻繁重規(guī)劃.針對(duì)以上問(wèn)題,本文提出一種改進(jìn)的插入隨機(jī)中間樹(shù)的多樹(shù)RRT 算法,下稱RRT 森林算法.

考慮路徑的可行性,現(xiàn)有的RRT 算法普遍存在路徑與障礙物距離過(guò)近的問(wèn)題.目前常見(jiàn)的解決方法是把地圖進(jìn)行膨脹處理再進(jìn)行路徑規(guī)劃[22],但對(duì)于需要實(shí)時(shí)進(jìn)行路徑規(guī)劃的復(fù)雜高層消防環(huán)境,對(duì)地圖進(jìn)行預(yù)處理會(huì)降低規(guī)劃效率.文獻(xiàn)[23]提出一種人工勢(shì)場(chǎng)優(yōu)化的RRT 算法,但這種方法需要計(jì)算地圖中的勢(shì)場(chǎng),需要反復(fù)計(jì)算微分,運(yùn)算效率仍不高.因此,研究一種能確保無(wú)人機(jī)與障礙物之間保持安全距離的碰撞檢測(cè)程序十分有意義.

此外,RRT 算法得到的路徑通常不是最優(yōu)路徑[15],而是一條有冗余點(diǎn)的路徑.在文獻(xiàn)[24]中,Jeong 等提出Quick-RRT*算法,在每一次產(chǎn)生子節(jié)點(diǎn)時(shí)都在給定的深度范圍內(nèi),尋找無(wú)障礙物相隔的父節(jié)點(diǎn)進(jìn)行連接,減少冗余點(diǎn)的產(chǎn)生,但其路徑受制于RRT 生成的節(jié)點(diǎn),易出現(xiàn)不合理的拐角.針對(duì)這一問(wèn)題,本文提出兩次動(dòng)態(tài)規(guī)劃(Dynamic programming,DP)對(duì)路徑進(jìn)行優(yōu)化的方法,采取“規(guī)劃-插值-規(guī)劃”的策略取得可行性更高的路徑.

基于對(duì)上述國(guó)內(nèi)外研究現(xiàn)狀的調(diào)研與分析,綜合考慮高層消防多無(wú)人機(jī)路徑規(guī)劃時(shí)間緊迫、環(huán)境復(fù)雜、需要頻繁對(duì)未執(zhí)行的局部路徑進(jìn)行重規(guī)劃[25],以及需要同時(shí)規(guī)劃多條路徑的特性,本文提出RRT森林算法,并通過(guò)改進(jìn)的障礙物接近檢測(cè)方法,以及動(dòng)態(tài)規(guī)劃的兩次優(yōu)化,提高算法的可行性.主要工作歸納如下.

1 ) 提出RRT 森林算法.通過(guò)提升隨機(jī)樹(shù)森林的搜索廣度,提升算法搜索效率.與文獻(xiàn)[18-21]不同,RRT 森林算法以樹(shù)間連接優(yōu)先的策略,降低搜索后期節(jié)點(diǎn)數(shù)量對(duì)遍歷效率的影響,從而適應(yīng)高層火災(zāi)救援任務(wù)中頻繁的路徑重規(guī)劃;同時(shí),RRT 森林算法通過(guò)多樹(shù)連接進(jìn)行尋路的設(shè)計(jì),實(shí)現(xiàn)多起點(diǎn)多終點(diǎn)的多無(wú)人機(jī)協(xié)同路徑規(guī)劃.

2 ) 提出障礙物接近檢測(cè)方法.通過(guò)檢測(cè)包絡(luò)線控制點(diǎn)以及包絡(luò)線內(nèi)部的隨機(jī)點(diǎn),考慮無(wú)人機(jī)的體積,使得路徑更遠(yuǎn)離障礙物.

3 ) 提出基于動(dòng)態(tài)規(guī)劃的冗余點(diǎn)移除方法.通過(guò)對(duì)路徑中的節(jié)點(diǎn)以最優(yōu)的方式重新連接,刪除其中的冗余點(diǎn),分離多無(wú)人機(jī)重合的路徑,并得到節(jié)點(diǎn)稀疏且路程最短的路徑.

本文算法流程如圖1 所示.第1 節(jié)分析了基本RRT 算法及雙向RRT 算法的優(yōu)缺點(diǎn);第2 節(jié)和第3 節(jié)分別闡述了RRT 森林算法和基于動(dòng)態(tài)規(guī)劃的冗余點(diǎn)移除方法;第4 節(jié)為RRT 森林算法的仿真研究;第5 節(jié)為結(jié)論.

1 RRT 算法及其改進(jìn)

為滿足高層消防多無(wú)人機(jī)協(xié)同路徑規(guī)劃中的時(shí)間緊迫性要求,本文提出RRT 森林算法.RRT 森林算法通過(guò)增加隨機(jī)樹(shù)的數(shù)量達(dá)到提升搜索廣度的目的,從而提升算法在復(fù)雜環(huán)境下的路徑搜索效率.在闡述RRT 森林算法之前,首先對(duì)基本RRT 算法與雙向RRT 算法進(jìn)行簡(jiǎn)單的描述.

1.1 基本RRT 算法

RRT 算法是由Lavalle 于1998 年提出的一種基于隨機(jī)采樣的快速路徑規(guī)劃算法,是一種概率完備算法[15,26].該算法的優(yōu)點(diǎn)是節(jié)點(diǎn)擴(kuò)展不需要進(jìn)行預(yù)處理,根據(jù)當(dāng)前的環(huán)境信息快速搜索可行路徑,可用于無(wú)人機(jī)路徑規(guī)劃上.

RRT 算法的基本思想是: 以起點(diǎn)為根節(jié)點(diǎn),通過(guò)在地圖上隨機(jī)采樣,并以最近的節(jié)點(diǎn)為父節(jié)點(diǎn),產(chǎn)生相應(yīng)的子節(jié)點(diǎn),使隨機(jī)樹(shù)不斷擴(kuò)展.當(dāng)隨機(jī)樹(shù)接近終點(diǎn)并與終點(diǎn)連接,即完成路徑搜索.隨機(jī)樹(shù)的采樣及搜索過(guò)程如圖2 所示.

圖2 基本RRT 算法采樣及搜索過(guò)程Fig.2 Sampling and exploring process of basic RRT

基本RRT 算法的優(yōu)點(diǎn)在于不需要對(duì)環(huán)境模型進(jìn)行復(fù)雜處理,即可進(jìn)行路徑規(guī)劃.但傳統(tǒng)RRT 算法的缺點(diǎn)主要有:

1 ) 由于RRT 算法是通過(guò)隨機(jī)采樣驅(qū)動(dòng)尋找路徑,得到的路徑曲折,通常不是最優(yōu)路徑;

2 ) RRT 算法在隨機(jī)采樣后需要遍歷隨機(jī)樹(shù)上現(xiàn)有的節(jié)點(diǎn),之后選擇最近的節(jié)點(diǎn)vnear,方能在vnear附近朝著采樣點(diǎn)vrand的方向前進(jìn)一小步到達(dá)vnew.然而,在復(fù)雜環(huán)境下,路徑搜索之初,隨機(jī)樹(shù)上節(jié)點(diǎn)少,覆蓋范圍小,生成vnew的成功率顯著降低,隨機(jī)樹(shù)在小范圍內(nèi)重復(fù)搜索,會(huì)導(dǎo)致路徑規(guī)劃收斂緩慢.

高層建筑火災(zāi)的室內(nèi)環(huán)境往往是復(fù)雜時(shí)變環(huán)境,需要頻繁重規(guī)劃路徑的環(huán)境,因此我們需要解決傳統(tǒng)RRT 算法前期搜索效率低的問(wèn)題,使之能夠高效完成復(fù)雜環(huán)境下的路徑搜索.

1.2 雙向RRT 算法

雙向RRT 算法與基本RRT 算法不同之處在于,以起點(diǎn)和終點(diǎn)為兩個(gè)根節(jié)點(diǎn),分別建立隨機(jī)樹(shù)進(jìn)行搜索,直到雙方節(jié)點(diǎn)彼此接近,最終將兩棵樹(shù)最接近的子節(jié)點(diǎn)連接[26].雙向RRT 兩棵樹(shù)連接示意圖如圖3 所示.

圖3 雙向RRT 連接過(guò)程Fig.3 Connecting process of bidirection-RRT

與基本RRT 算法相比,雙向RRT 算法在終點(diǎn)處增加一隨機(jī)樹(shù),從起點(diǎn)和終點(diǎn)同時(shí)搜索路徑,在搜索速度上具有一定優(yōu)勢(shì),一定程度上增大了搜索初期的搜索廣度.但隨著環(huán)境復(fù)雜度的提高,雙向RRT 的兩個(gè)隨機(jī)樹(shù),在搜索初期仍會(huì)被局限在小范圍內(nèi),不能滿足高層消防救援中頻繁進(jìn)行路徑重規(guī)劃的需求.

2 RRT 森林算法

為提升RRT 算法在高層火災(zāi)現(xiàn)場(chǎng)內(nèi)部的路徑搜索效率,適應(yīng)高層消防救援中頻繁進(jìn)行路徑重規(guī)劃的任務(wù)需求,本文提出RRT 森林算法,在可行區(qū)域內(nèi)隨機(jī)選點(diǎn)生成隨機(jī)樹(shù),并與其他隨機(jī)樹(shù)相連接,從而得出多組路徑.

通過(guò)多樹(shù)優(yōu)化的RRT 算法的思路并非首次提出,受雙向RRT 算法增加隨機(jī)樹(shù)提升搜索廣度的啟發(fā),鐘建冬[18]提出可以將RRT 算法從兩棵隨機(jī)樹(shù)進(jìn)一步擴(kuò)展為三棵樹(shù),直至多樹(shù).Devaurs 等[20]利用多樹(shù)的特性對(duì)Transition-RRT 進(jìn)行優(yōu)化,通過(guò)給定根節(jié)點(diǎn)的方式在地圖中生成多個(gè)隨機(jī)樹(shù),提升了搜索效率.然而,上述文獻(xiàn)中的多樹(shù)RRT 算法為隨機(jī)樹(shù)擴(kuò)展優(yōu)先的形式,這會(huì)導(dǎo)致在搜索連接對(duì)象時(shí)遍歷量較大.對(duì)此,本文提出的RRT 森林算法采用樹(shù)間連接優(yōu)先策略,能夠有效減少遍歷量,進(jìn)而提升搜索效率.另外,針對(duì)文獻(xiàn)[20]中導(dǎo)出路徑需要經(jīng)過(guò)所有隨機(jī)樹(shù)的根節(jié)點(diǎn)、路徑合理性較差的問(wèn)題,本文通過(guò)設(shè)計(jì)一種適用于多樹(shù)RRT 的路徑導(dǎo)出方法予以解決.綜上所述,本文所提出的RRT森林算法具有以下特點(diǎn): 1)隨機(jī)樹(shù)連接優(yōu)先,通過(guò)檢查vnear判斷是否可以與其他隨機(jī)樹(shù)連接,一旦進(jìn)行連接,則不再擴(kuò)展隨機(jī)樹(shù);2) 利用多隨機(jī)樹(shù)的特點(diǎn)進(jìn)行多起點(diǎn)多終點(diǎn)的路徑規(guī)劃;3)導(dǎo)出路徑不必經(jīng)過(guò)每棵樹(shù)的根節(jié)點(diǎn),因此能夠取得節(jié)點(diǎn)更少的路徑.

RRT 森林算法的主要工作原理是: 在地圖中增加隨機(jī)點(diǎn),并以這些隨機(jī)點(diǎn)以及原有的起點(diǎn)和終點(diǎn)為根節(jié)點(diǎn),各自生成隨機(jī)樹(shù).隨著隨機(jī)樹(shù)的生長(zhǎng),地圖上可行的區(qū)域會(huì)迅速被隨機(jī)樹(shù)覆蓋,就像一片森林,因此稱這種算法為RRT 森林算法.基于該算法的特性,隨機(jī)樹(shù)之間可以通過(guò)樹(shù)枝與其他樹(shù)相互連接,最終將起點(diǎn)與終點(diǎn)連通,從而完成路徑搜索.RRT 森林算法分為搜索過(guò)程、隨機(jī)樹(shù)之間的連接與合并過(guò)程以及路徑導(dǎo)出過(guò)程三個(gè)部分.

2.1 RRT 森林算法搜索過(guò)程

定義一個(gè)由k架無(wú)人機(jī)完成的多起點(diǎn)多終點(diǎn)的高層消防多無(wú)人機(jī)協(xié)同路徑規(guī)劃任務(wù),記地圖為M.設(shè)需要規(guī)劃n(n ≥k) 條路徑,記第i條路徑的起點(diǎn)、終點(diǎn)分別為,其中,i=1,2,···,n,,并滿足以下假設(shè):

假設(shè) 1.考慮到多無(wú)人機(jī)路徑可能出現(xiàn)交叉的問(wèn)題,每架無(wú)人機(jī)之間以及無(wú)人機(jī)與地面之間至少需保持α米距離.無(wú)人機(jī)的高度隨地面起伏而變化,當(dāng)需要改變高度時(shí),無(wú)人機(jī)會(huì)加強(qiáng)對(duì)附近無(wú)人機(jī)的監(jiān)測(cè).

注 1.若地圖中某處v∈M可行高度范圍為[v.floor,v.ceil],暫且假設(shè)該范圍足夠讓n架無(wú)人機(jī)在不同高度同時(shí)飛行,k架無(wú)人機(jī)從低到高排列時(shí),第j(j=1,2,···,k) 架無(wú)人機(jī)在v點(diǎn)的飛行高度vj.z可表示為

而第j架無(wú)人機(jī)設(shè)定的距地面高度則等于 (j+1)α.另外,由式(1)可得到k架無(wú)人機(jī)飛行所需垂直空間條件為

滿足條件(2)方可保證多無(wú)人機(jī)能同時(shí)通過(guò)該點(diǎn).因此,路徑規(guī)劃使用的灰度地圖中,不滿足條件(2)的區(qū)域視作不可行區(qū)域.

現(xiàn)用RRT 森林算法搜索n條路徑,其過(guò)程如下:

2 ) 開(kāi)始隨機(jī)搜索.搜索過(guò)程采用樹(shù)間連接優(yōu)先的策略.對(duì)于隨機(jī)樹(shù)集合T,在每次迭代中逐一進(jìn)行搜索.首先,對(duì)于當(dāng)前活動(dòng)樹(shù)Tme采樣得到隨機(jī)點(diǎn)vrand,并查找節(jié)點(diǎn)vnear∈Tme,使得vrand與vnear距離最短;其次,先判斷vnear在給定范圍內(nèi)是否有非活動(dòng)隨機(jī)樹(shù)的節(jié)點(diǎn)vconnect,并確定vnear與vconnect之間有無(wú)障礙物,若存在連接無(wú)障礙的vconnect,則連接vnear與vconnect,并結(jié)束對(duì)當(dāng)前樹(shù)的搜索;反之,按照基本RRT 的方式沿vnear到vrand方向,以步長(zhǎng)s,嘗試在Tme上新建一個(gè)子節(jié)點(diǎn)vnew,并結(jié)束對(duì)當(dāng)前樹(shù)的搜索.RRT 森林算法搜索路徑分為連接和生長(zhǎng)兩種工作模式,其工作原理如圖4所示,其中圖4(a)為隨機(jī)樹(shù)之間的連接過(guò)程,圖4(b)為隨機(jī)樹(shù)的生長(zhǎng)過(guò)程.

圖4 RRT 森林算法兩種工作模式Fig.4 Two working modes of RRT-forest algorithm

3 ) 當(dāng)所有隨機(jī)樹(shù)均嘗試進(jìn)行一次搜索后,逐一判斷樹(shù)Tk ∈T是否已經(jīng)包含所有起點(diǎn)與終點(diǎn),若已全部包含,則搜索完成,否則繼續(xù)進(jìn)行下一次迭代.若算法一直無(wú)法達(dá)到搜索完成條件,則迭代會(huì)在迭代次數(shù)達(dá)到最大迭代次數(shù)m時(shí)停止并顯示錯(cuò)誤信息.RRT 森林算法隨機(jī)樹(shù)搜索過(guò)程偽代碼如算法1 所示.

算法 1.RRT 森林算法隨機(jī)樹(shù)搜索過(guò)程

2.2 多隨機(jī)樹(shù)的連接與合并

對(duì)比于雙向RRT 的連接方式,多隨機(jī)樹(shù)之間的連接方式增加了合并步驟,以防止節(jié)點(diǎn)連接關(guān)系混亂.由于RRT 森林算法中每棵隨機(jī)樹(shù)需要進(jìn)行多次連接,因此不得不考慮隨機(jī)樹(shù)被連成閉環(huán)的問(wèn)題.從路徑規(guī)劃的角度看,一旦隨機(jī)樹(shù)被連接成環(huán),在結(jié)束搜索后,導(dǎo)出路徑時(shí)會(huì)陷入死循環(huán).而從隨機(jī)樹(shù)的樹(shù)狀結(jié)構(gòu)看,隨機(jī)樹(shù)連接成環(huán)將會(huì)破壞樹(shù)狀結(jié)構(gòu),導(dǎo)致部分節(jié)點(diǎn)連接關(guān)系混亂.

為了避免多隨機(jī)樹(shù)連接成環(huán)狀造成路徑規(guī)劃出錯(cuò)的問(wèn)題,在連接的同時(shí)將兩棵隨機(jī)樹(shù)合并,從而防止兩棵樹(shù)之間多次連接.此外,兩棵隨機(jī)樹(shù)的合并需要重新選擇父節(jié)點(diǎn).根據(jù)隨機(jī)樹(shù)的樹(shù)狀結(jié)構(gòu),可以將任意節(jié)點(diǎn)定義為新的根節(jié)點(diǎn),但需要先調(diào)整新根節(jié)點(diǎn)到原根節(jié)點(diǎn)之間各段的連接關(guān)系,再對(duì)其他節(jié)點(diǎn)重新建立連接關(guān)系.隨機(jī)樹(shù)之間的連接過(guò)程如圖4(a)所示,具體操作如下:

2.3 多路徑的導(dǎo)出

多無(wú)人機(jī)協(xié)同執(zhí)行高層消防任務(wù)時(shí),往往需要為多架無(wú)人機(jī)分別導(dǎo)出路徑,本文結(jié)合RRT 森林算法的特點(diǎn),設(shè)計(jì)了一種導(dǎo)出RRT 多路徑的方法.主要工作原理如圖5 和算法2 所示.

圖5 多路徑的導(dǎo)出方法Fig.5 Export method for multiple paths

由于存在多條路徑,因此每條路徑是分別確定的.而相較于文獻(xiàn)[20]所述的方法,本文所導(dǎo)出的路徑可不經(jīng)過(guò)全部根節(jié)點(diǎn).具體工作原理為: 對(duì)于第i架無(wú)人機(jī),其起點(diǎn)為,終點(diǎn)為,所在隨機(jī)樹(shù)被提取,記為T*.

定義序列Rfrom和Rto,分別從起點(diǎn)和終點(diǎn)開(kāi)始,向T*的根節(jié)點(diǎn)的方向,不斷尋找父節(jié)點(diǎn)并記錄,直到Rfrom和Rto出現(xiàn)重復(fù)元素.然后令R=Rfrom∪Rto,即完成路徑導(dǎo)出.路徑導(dǎo)出過(guò)程的偽代碼如算法2 所示.

算法 2.路徑導(dǎo)出過(guò)程

注 2.在程序?qū)崿F(xiàn)的過(guò)程中,Rfrom和Rto記錄的是各節(jié)點(diǎn)在樹(shù)T*上的編碼,因此,需要在樹(shù)T*上根據(jù)R中的編碼查出具體的路徑坐標(biāo)并組合成序列Pi,即為第i架無(wú)人機(jī)的路徑.

2.4 改進(jìn)的障礙物接近檢測(cè)方法

RRT 算法常用的碰撞檢測(cè)是通過(guò)檢測(cè)地圖中某點(diǎn)的灰度值判斷該點(diǎn)是否在障礙物上,對(duì)于線段的碰撞檢測(cè),則從起點(diǎn)到終點(diǎn)逐個(gè)點(diǎn)檢測(cè)是否在障礙物上,一旦發(fā)現(xiàn)線段上某個(gè)點(diǎn)在障礙物上,則判斷線段存在碰撞.特別地,對(duì)于變步距RRT,可以通過(guò)碰撞檢測(cè)程序反饋推薦步長(zhǎng)進(jìn)行變步距搜索,一定程度上提高了RRT 的搜索效率.然而,由于碰撞檢測(cè)未能考慮無(wú)人機(jī)的體積,加之地圖可能存在誤差,在實(shí)際執(zhí)行中,無(wú)人機(jī)存在與障礙物剮蹭的風(fēng)險(xiǎn).對(duì)于這個(gè)問(wèn)題,常用的解決方法多是通過(guò)圖像處理技術(shù)對(duì)圖像中障礙物進(jìn)行膨脹處理,這需要花費(fèi)時(shí)間對(duì)圖像進(jìn)行預(yù)處理,對(duì)于需要頻繁進(jìn)行部分路徑重規(guī)劃的高層消防多無(wú)人機(jī)系統(tǒng)來(lái)說(shuō)是難以實(shí)現(xiàn)的.基于上述情況,本文提出一種改進(jìn)的障礙物接近檢測(cè)方法,其思想是將單點(diǎn)檢測(cè)轉(zhuǎn)化為一種對(duì)包絡(luò)線外緣主要控制點(diǎn)的檢測(cè),在考慮無(wú)人機(jī)體積的同時(shí),盡可能減少路徑規(guī)劃速率的損失.

待測(cè)點(diǎn)vtest在灰度地圖M上判斷是否可行步驟如下: 首先,地圖M以深色表示不可行區(qū)域,定義安全距離r;然后,取無(wú)人機(jī)包絡(luò)線的頂點(diǎn),以及在這些頂點(diǎn)所圍成的區(qū)域內(nèi),根據(jù)計(jì)算機(jī)算力,隨機(jī)選取測(cè)試點(diǎn)=1,2,···,k,加上待測(cè)點(diǎn)vtest自身;最后通過(guò)分析在M上這些測(cè)試點(diǎn)的取值,判定待測(cè)點(diǎn)vtest是否可行.當(dāng)且僅當(dāng)上述點(diǎn)全部位于可行區(qū)域時(shí),待測(cè)點(diǎn)vtest可行.

改進(jìn)的障礙物接近檢測(cè)圖如圖6 所示,假設(shè)采用的是四旋翼無(wú)人機(jī),圖中待測(cè)點(diǎn)記為vtest,圓表示無(wú)人機(jī)的旋翼,淺色實(shí)線段組成無(wú)人機(jī)的框架,正方形點(diǎn)表示無(wú)人機(jī)包絡(luò)線的頂點(diǎn),記為vA,vB,vC,vD,連接這四個(gè)點(diǎn)的虛線框?yàn)闊o(wú)人機(jī)包絡(luò)線,中間正方形點(diǎn)表示無(wú)人機(jī)的中心位置,即vO,而三個(gè)“*”號(hào)點(diǎn)則是隨機(jī)選取的測(cè)試點(diǎn)=1,2,3.圖6中,雖然中心點(diǎn)vO為可行點(diǎn),但vB,vD和一個(gè)白色“*”號(hào)表示的隨機(jī)點(diǎn)位于不可行區(qū)域,因此,本例中待測(cè)點(diǎn)vtest不可行.

圖6 改進(jìn)的障礙物接近檢測(cè)示意圖Fig.6 Schematic diagram of the improved obstacle proximity detection

對(duì)于線段碰撞檢測(cè),從線段的一端向另一端逐步進(jìn)行上述的障礙物檢測(cè),其中每一步間隔一個(gè)安全距離r.當(dāng)檢測(cè)到第k步不可行時(shí)即停止檢測(cè),并返回可行步長(zhǎng)=r(k-1).

在RRT 森林算法中應(yīng)用此種障礙物檢測(cè)方法,可以在未經(jīng)膨脹處理的地圖中進(jìn)行路徑規(guī)劃,同時(shí)不致使無(wú)人機(jī)過(guò)于接近障礙物而發(fā)生碰撞,提高了路徑的可行性.

3 基于動(dòng)態(tài)規(guī)劃的冗余點(diǎn)移除方法

在高層火災(zāi)所造成的未知、時(shí)變且復(fù)雜的環(huán)境中,直接使用RRT 森林算法搜索的多路徑會(huì)出現(xiàn)以下問(wèn)題: 1) 路徑非最優(yōu),會(huì)造成不必要的時(shí)間和能力開(kāi)銷;2) 路徑會(huì)出現(xiàn)重疊,雖然每架無(wú)人機(jī)以不同的高度飛行,但無(wú)人機(jī)仍會(huì)相互影響.因此,有必要移除路徑中的冗余點(diǎn),并減少路徑重疊,實(shí)現(xiàn)路徑的進(jìn)一步優(yōu)化.

3.1 動(dòng)態(tài)規(guī)劃

動(dòng)態(tài)規(guī)劃是上世紀(jì)50 年代美國(guó)學(xué)者Bellman提出的一種最優(yōu)化規(guī)劃方法[27].動(dòng)態(tài)規(guī)劃用一個(gè)遞推關(guān)系式,使決策過(guò)程連續(xù)地轉(zhuǎn)移,并將一個(gè)多步最優(yōu)控制問(wèn)題轉(zhuǎn)化為多個(gè)一步最優(yōu)控制問(wèn)題,從而簡(jiǎn)化求解過(guò)程.對(duì)于N級(jí)決策過(guò)程,其動(dòng)態(tài)方程為

其中,u(k) 為第k級(jí)的容許控制;x(k) 為當(dāng)前狀態(tài).利用動(dòng)態(tài)規(guī)劃解路徑規(guī)劃問(wèn)題,就是要找到?jīng)Q策序列u*(k),使得代價(jià)函數(shù)

達(dá)到最小.

本文將RRT 森林算法搜索到的路徑節(jié)點(diǎn)作為容許控制集,通過(guò)動(dòng)態(tài)規(guī)劃確定在容許控制集中最優(yōu)的決策序列,從而刪除其余的冗余點(diǎn),實(shí)現(xiàn)對(duì)多無(wú)人機(jī)路徑的優(yōu)化.

3.2 刪除冗余點(diǎn)和路徑優(yōu)化

根據(jù)三角不等式,任取不共線的三點(diǎn)A,B,C,則三點(diǎn)之間相互連接的線段長(zhǎng)度,必然滿足

當(dāng)且僅當(dāng)A,B,C共線時(shí),有

RRT 森林算法搜索產(chǎn)生的較曲折的路徑,可以利用上述原理優(yōu)化.然而,盲目刪除冗余點(diǎn),未必能得到最優(yōu)路徑.因此,本文提出基于動(dòng)態(tài)規(guī)劃的冗余點(diǎn)移除方法.

定義兩點(diǎn)間指標(biāo)函數(shù)

對(duì)于第i條原路徑Pi,將起點(diǎn)到終點(diǎn)之間每一個(gè)點(diǎn)獨(dú)立作為一級(jí),對(duì)每一級(jí)計(jì)算指標(biāo)函數(shù),并記錄指標(biāo)函數(shù)極小的上一節(jié)點(diǎn)索引.計(jì)算完畢后,從終點(diǎn)根據(jù)上一節(jié)點(diǎn)的索引往前查找,即得優(yōu)化后的路徑索引序列.根據(jù)索引序列查找Pi中對(duì)應(yīng)點(diǎn)的坐標(biāo),即得優(yōu)化后的路徑.動(dòng)態(tài)規(guī)劃移除冗余點(diǎn)的偽代碼見(jiàn)算法3.

算法 3.動(dòng)態(tài)規(guī)劃移除冗余點(diǎn)

第一次動(dòng)態(tài)規(guī)劃是為RRT 森林算法生成的路徑刪除冗余點(diǎn),優(yōu)化后的路徑顯著縮短,但仍由原路徑的部分節(jié)點(diǎn)連接而成,易出現(xiàn)不必要的繞行以及路徑節(jié)點(diǎn)重疊,這需要進(jìn)一步優(yōu)化.而經(jīng)過(guò)第一次動(dòng)態(tài)規(guī)劃所得的路徑,節(jié)點(diǎn)比較稀疏,需要對(duì)路徑進(jìn)行插值處理,得到節(jié)點(diǎn)較密集的路徑用于第二次動(dòng)態(tài)規(guī)劃.

對(duì)于路徑Pi,經(jīng)過(guò)第一次動(dòng)態(tài)規(guī)劃共有ni個(gè)航段,可擬合成一個(gè)分段函數(shù),但由于路徑中x與y不能唯一映射,故采取從起點(diǎn)到該點(diǎn)的里程lj為參數(shù),其中j=1,2,···,ni,將路徑按參數(shù)方程的形式擬合成x(l) 與y(l) 如下:

按一定步長(zhǎng) Δl取參數(shù)l,代入式(10)和式(11)中分別求得對(duì)應(yīng)的x坐標(biāo)和y坐標(biāo),即得到插值后的路徑.

對(duì)于經(jīng)過(guò)插值處理后的路徑,再一次進(jìn)行動(dòng)態(tài)規(guī)劃,即可裁去中不合理的轉(zhuǎn)角,并解決絕大部分的路徑重合問(wèn)題.

注 3.在實(shí)際高層消防任務(wù)中進(jìn)行路徑規(guī)劃時(shí),所取步長(zhǎng) Δl不宜過(guò)短,否則會(huì)引發(fā)動(dòng)態(tài)規(guī)劃的維數(shù)災(zāi)難,導(dǎo)致規(guī)劃時(shí)間過(guò)長(zhǎng),無(wú)法及時(shí)對(duì)路徑進(jìn)行重規(guī)劃.

4 仿真研究

本節(jié)對(duì)上述基于RRT 森林算法的路徑規(guī)劃方法進(jìn)行仿真研究.根據(jù)障礙物的復(fù)雜程度以及高層消防任務(wù)所面臨的實(shí)際環(huán)境可分為復(fù)雜環(huán)境和實(shí)際簡(jiǎn)單環(huán)境.復(fù)雜環(huán)境以迷宮作為地圖,實(shí)際環(huán)境以實(shí)際高層住宅平面圖作為地圖.又根據(jù)路徑規(guī)劃的起點(diǎn)和終點(diǎn)組合的數(shù)量,分為單一路徑規(guī)劃和多路徑協(xié)同規(guī)劃.其中,單一路徑規(guī)劃主要以復(fù)雜環(huán)境下路徑規(guī)劃為主;而多路徑規(guī)劃是在實(shí)際環(huán)境中進(jìn)行3 架無(wú)人機(jī)飛越5 個(gè)航段的協(xié)同路徑規(guī)劃.

仿真研究共分為三部分: 1)對(duì)RRT 森林算法與基本RRT 算法、雙向RRT 算法在復(fù)雜環(huán)境下的搜索效率進(jìn)行對(duì)比,并給出多起點(diǎn)多終點(diǎn)路徑規(guī)劃表現(xiàn);2)對(duì)比RRT 森林算法生成的路徑經(jīng)過(guò)各次動(dòng)態(tài)規(guī)劃優(yōu)化后的效果;3) RRT 森林算法使用改進(jìn)和未改進(jìn)的障礙物接近檢測(cè)的對(duì)比效果.

在仿真圖中,加粗的黑色線條表示結(jié)果路徑,通過(guò)不同的線型區(qū)分多路徑,淺色細(xì)實(shí)線表示RRT 森林算法產(chǎn)生的隨機(jī)樹(shù)枝干.圓點(diǎn)為起點(diǎn),五角星為終點(diǎn),若兩者重疊,則表示給定的中途點(diǎn).

4.1 RRT 森林算法與其他RRT 算法搜索效率比較

為了測(cè)試RRT 森林算法的路徑規(guī)劃效率,本文對(duì)不同隨機(jī)樹(shù)數(shù)量的RRT 森林算法與基本RRT 和雙向RRT 在復(fù)雜環(huán)境下進(jìn)行比較.其中,隨機(jī)樹(shù)數(shù)量取NTree=1,2,5,10,20,50,隨機(jī)樹(shù)數(shù)量為1 時(shí)算法相當(dāng)于基本RRT,為2 時(shí)相當(dāng)于雙向RRT,其余為RRT 森林算法,其他參數(shù)保持一致.考慮到RRT 算法的固有隨機(jī)性,對(duì)各種算法分別進(jìn)行不少于1 000 次實(shí)驗(yàn),通過(guò)箱形圖展示測(cè)試結(jié)果.

圖7 展示了基本RRT 算法、雙向RRT 算法以及NTree=30 的RRT 森林算法在復(fù)雜環(huán)境中的表現(xiàn).從圖7(a)和圖7(b)可見(jiàn),基本RRT 以及雙向RRT 都產(chǎn)生了較多的無(wú)效點(diǎn),且集中在隨機(jī)樹(shù)的根節(jié)點(diǎn)附近,表明搜索被局限在隨機(jī)樹(shù)現(xiàn)有節(jié)點(diǎn)附近.而從圖7(c)中可見(jiàn),RRT 森林算法則產(chǎn)生較少的無(wú)效點(diǎn),且采點(diǎn)比較均勻.在可行區(qū)域內(nèi)的圓點(diǎn),表示中間隨機(jī)樹(shù)初始根節(jié)點(diǎn).這些從隨機(jī)點(diǎn)開(kāi)始搜索的樹(shù),從搜索初期就拓寬了RRT 森林算法的搜索范圍,提升了搜索的廣度,避免了RRT 算法搜索局限在某個(gè)范圍內(nèi),減少了無(wú)效搜索,從而提升了在復(fù)雜環(huán)境中的搜索效率.

圖7 各算法在復(fù)雜環(huán)境下的運(yùn)行結(jié)果Fig.7 Performance of algorithms in complex environment

圖8 用箱形圖展示不同隨機(jī)樹(shù)數(shù)量的RRT 森林算法及基本RRT 算法、雙向RRT 算法的實(shí)驗(yàn)數(shù)據(jù)分布情況.表1 則揭示了圖8(a)中箱形圖的數(shù)據(jù).由表1 結(jié)合圖8(a)分析可得,基本RRT 算法在復(fù)雜環(huán)境下的搜索時(shí)間中位數(shù)為8.00 s,雙向RRT 算法為2.57 s,而RRT 森林算法中,搜索時(shí)間與隨機(jī)樹(shù)的數(shù)量呈先顯著下降,再略微增加的趨勢(shì),當(dāng)NTree=20 時(shí),搜索時(shí)間的中位數(shù)為0.48 s,與基本RRT 算法相比,節(jié)約了94%的搜索時(shí)間.另外,根據(jù)每種算法的上四分位數(shù)與下四分位數(shù)之差,可得搜索時(shí)間的穩(wěn)定性與隨機(jī)樹(shù)數(shù)量呈正相關(guān).從圖8(b)可知,路徑搜索所使用的迭代次數(shù)隨著隨機(jī)樹(shù)數(shù)量的增加而減少.從圖8(c)可知,生成路徑的路徑點(diǎn)數(shù)隨著隨機(jī)樹(shù)的增多先減少后增加.上述實(shí)驗(yàn)數(shù)據(jù)表明,在RRT 森林算法的運(yùn)用中,為避免路徑過(guò)于復(fù)雜,以及不必要的運(yùn)算開(kāi)銷,需要視環(huán)境復(fù)雜程度選擇合適的隨機(jī)樹(shù)數(shù)量.

表1 各算法搜索用時(shí)數(shù)據(jù)(s)Table 1 Statistics of time used in exploring of each algorithm (s)

圖8 各算法在復(fù)雜環(huán)境中的實(shí)驗(yàn)數(shù)據(jù)Fig.8 Statistics among algorithms in complex environment

注 4.圖8 和表1 所述的搜索用時(shí)數(shù)據(jù),包括取隨機(jī)根節(jié)點(diǎn)的時(shí)間和路徑搜索時(shí)間,不包括動(dòng)態(tài)規(guī)劃的運(yùn)算時(shí)間.

RRT 森林算法基于多個(gè)隨機(jī)樹(shù)連接與合并,可支持多路徑規(guī)劃,因此可用于多無(wú)人機(jī)協(xié)同路徑規(guī)劃.圖9 為RRT 森林算法多路徑規(guī)劃的表現(xiàn).

圖9 RRT 森林算法多路徑規(guī)劃Fig.9 Multi-path planning by RRT-forest

從圖9 中可見(jiàn),RRT 森林算法通過(guò)將多隨機(jī)樹(shù)連接合并,達(dá)到同時(shí)兼顧多起點(diǎn)多終點(diǎn)的路徑規(guī)劃目標(biāo),最終達(dá)到所有起點(diǎn)和終點(diǎn)都被包含在同一個(gè)隨機(jī)樹(shù)上,此時(shí)只需按照起點(diǎn)與終點(diǎn)的組合尋找路徑即可.值得注意的是,這些路徑之間往往都存在公共線段,且這些路徑比較曲折,甚至有不必要的繞行,因此無(wú)法直接應(yīng)用于高層消防無(wú)人機(jī)協(xié)同任務(wù)當(dāng)中,需要進(jìn)行刪除冗余點(diǎn)等路徑優(yōu)化處理.

4.2 基于動(dòng)態(tài)規(guī)劃的路徑優(yōu)化

為了測(cè)試動(dòng)態(tài)規(guī)劃對(duì)路徑的優(yōu)化效果,本文分別針對(duì)復(fù)雜環(huán)境下的單路徑和實(shí)際環(huán)境下的多路徑進(jìn)行優(yōu)化處理.

由圖10(a)可知,未經(jīng)處理的路徑非常曲折,且存在冗余點(diǎn).對(duì)路徑進(jìn)行第一次動(dòng)態(tài)規(guī)劃得到圖10(b),發(fā)現(xiàn)許多折線被拉直.顯然,從起點(diǎn)到終點(diǎn)的路程已被縮短,但仍然存在不合理的轉(zhuǎn)角.將第一次動(dòng)態(tài)規(guī)劃的結(jié)果進(jìn)行參數(shù)方程擬合并重新采樣,再次進(jìn)行動(dòng)態(tài)規(guī)劃,得到圖10(c).由圖10(c)可見(jiàn),在轉(zhuǎn)角處能夠緊貼障礙物邊緣,以取得最短的路徑.

圖10 單路徑優(yōu)化過(guò)程Fig.10 Optimization of single path

針對(duì)多條路徑的優(yōu)化效果,本文進(jìn)行了實(shí)際環(huán)境下的多路徑優(yōu)化實(shí)驗(yàn).假設(shè)3 架無(wú)人機(jī)以不同高度飛行,且飛行過(guò)程中保持高度不變.仿真結(jié)果如圖11 所示.由圖9 可發(fā)現(xiàn),處理前的路徑非常曲折,且有路徑重疊的現(xiàn)象.進(jìn)行第一次動(dòng)態(tài)規(guī)劃后的效果如圖11(a)所示,由圖可見(jiàn),許多不合理的轉(zhuǎn)角被移除,但仍然存在個(gè)別交叉與重疊,部分轉(zhuǎn)角仍然過(guò)大.在圖11(b)中,對(duì)各條路徑進(jìn)行擬合并重新采樣后,再次進(jìn)行動(dòng)態(tài)規(guī)劃,并且按照任務(wù)設(shè)定將應(yīng)合并的路徑合并.由圖可見(jiàn),5 條路徑被合并成3條路徑并完全分開(kāi),各段路徑筆直且不存在交點(diǎn),而重疊的路徑僅存在于同一無(wú)人機(jī)飛往與飛離中途點(diǎn)的路徑中.

圖11 多路徑優(yōu)化過(guò)程Fig.11 Optimization of multi-paths

4.3 障礙物接近檢測(cè)改進(jìn)

從圖10 可知,經(jīng)過(guò)兩次動(dòng)態(tài)規(guī)劃得到的路徑,部分線段是緊貼障礙物的,在多無(wú)人機(jī)執(zhí)行高層消防救援任務(wù)時(shí),這樣的路徑可行性仍然較差,因此需要在路徑搜索以及動(dòng)態(tài)規(guī)劃中,使用改進(jìn)的障礙物接近檢測(cè)代替原碰撞檢測(cè),實(shí)現(xiàn)使路徑與障礙物保持一定距離的目標(biāo).對(duì)此,本文對(duì)障礙物接近檢測(cè)與原碰撞檢測(cè)進(jìn)行了對(duì)比實(shí)驗(yàn).

注 5.對(duì)于迷宮地圖,假設(shè)無(wú)人機(jī)直徑rUAV≤20 cm;對(duì)于實(shí)際高層建筑地圖,圖上1 個(gè)像素代表10 mm,假設(shè)無(wú)人機(jī)直徑rUAV≤30 cm,并為每架無(wú)人機(jī)分配不同的飛行高度,在飛行中高度將保持不變.

從圖12(a)可見(jiàn),RRT 森林算法以及動(dòng)態(tài)規(guī)劃中使用未改進(jìn)的碰撞檢測(cè)函數(shù)時(shí),得到的路徑與障礙物的最小距離為0,這意味著無(wú)人機(jī)執(zhí)行這樣的路徑會(huì)與障礙物發(fā)生剮蹭.而從圖12(b)可見(jiàn),使用改進(jìn)的障礙物接近檢測(cè)后,設(shè)置安全距離為10 cm,得到的路徑與障礙物的距離顯著大于使用原碰撞檢測(cè)的算法所生成的路徑.圖12(b)也給出了使用改進(jìn)障礙物接近檢測(cè)方法的RRT 森林算法所得路徑與障礙物的最短距離曲線,可知路徑到障礙物最小距離均大于給定安全距離10 cm.可見(jiàn),改進(jìn)的碰撞檢測(cè)方法顯著提高了路徑的可行性,使得RRT森林算法在未經(jīng)膨脹處理的地圖中,通過(guò)設(shè)置安全距離進(jìn)行路徑規(guī)劃可以得到更可行的路徑.

需要注意的是,當(dāng)安全距離設(shè)置過(guò)大時(shí)會(huì)影響路徑搜索效率,導(dǎo)致搜索失敗率增加,搜索時(shí)間變長(zhǎng),因此需要合理設(shè)置安全距離.在高層建筑室內(nèi),對(duì)于無(wú)人機(jī)仍有比較大的空間,此時(shí)可以設(shè)置更大的安全裕度.圖13 為一般室內(nèi)環(huán)境下,安全距離為15 cm 的規(guī)劃結(jié)果及其與障礙物之間的距離曲線,此時(shí)總規(guī)劃時(shí)間大約為1 s.而對(duì)于圖12 中的復(fù)雜環(huán)境,由于空間狹小,當(dāng)安全距離取10 cm 時(shí)會(huì)使得可行區(qū)域大幅減少,隨機(jī)樹(shù)生長(zhǎng)成功率降低,規(guī)劃時(shí)間會(huì)超過(guò)2 s.

圖13 實(shí)際環(huán)境下改進(jìn)碰撞檢測(cè)效果Fig.13 Result of novel obstacle checking in practical environment

5 結(jié)束語(yǔ)

考慮到多無(wú)人機(jī)協(xié)同執(zhí)行高層室內(nèi)消防救援任務(wù)時(shí)的高度緊迫性,在路徑規(guī)劃方面本文提出RRT森林算法,提高了路徑搜索的速度以及結(jié)果可行性.通過(guò)在RRT 算法中增加中間樹(shù),解決了RRT 算法在前期搜索范圍受限的問(wèn)題,提高了復(fù)雜環(huán)境下的路徑搜索效率、搜索速度及魯棒性,并且為多無(wú)人機(jī)進(jìn)行協(xié)同路徑規(guī)劃提供了重要支撐.通過(guò)設(shè)計(jì)新型碰撞檢測(cè)算法,使得路徑與障礙物始終保持安全距離.應(yīng)用兩次動(dòng)態(tài)規(guī)劃和參數(shù)方程擬合方法,移除了路徑上的冗余節(jié)點(diǎn),增強(qiáng)了規(guī)劃結(jié)果的實(shí)用性.實(shí)驗(yàn)結(jié)果表明,RRT 森林算法可實(shí)現(xiàn)復(fù)雜環(huán)境下快速規(guī)劃多條安全可行的路徑,并能滿足高層消防救援中路徑頻繁重規(guī)劃的需求.

關(guān)于高層消防多無(wú)人機(jī)室內(nèi)協(xié)同路徑規(guī)劃,本文提出的RRT 森林算法能在較短時(shí)間內(nèi)得到可行性較高的路徑,但在無(wú)人機(jī)數(shù)量較多、任務(wù)分配復(fù)雜的情況下,在協(xié)同能力方面仍有提升空間.因此將來(lái)的工作可以致力于提高多無(wú)人機(jī)協(xié)同路徑規(guī)劃的能力.