尚嵩,劉明剛,丁士強(qiáng)
(海軍航空大學(xué),山東煙臺(tái) 264001)
在基于無(wú)人機(jī)的環(huán)境智能感知技術(shù)中,路徑規(guī)劃是非常重要的一環(huán)。無(wú)論使用何種傳感器和計(jì)算方法來(lái)進(jìn)行環(huán)境感知,無(wú)人機(jī)都需要能夠安全無(wú)碰撞地到達(dá)目的地。因此,無(wú)人機(jī)路徑規(guī)劃是完成任務(wù)的關(guān)鍵問(wèn)題之一。根據(jù)應(yīng)用場(chǎng)景的不同,無(wú)人機(jī)路徑規(guī)劃可以分為全局路徑規(guī)劃和局部路徑規(guī)劃。全局路徑規(guī)劃適用于無(wú)人機(jī)工作環(huán)境已知的場(chǎng)景,而局部路徑規(guī)劃則適用于無(wú)人機(jī)工作環(huán)境完全未知或部分未知的場(chǎng)景?;跓o(wú)人機(jī)的環(huán)境智能感知技術(shù)的目的是依靠無(wú)人機(jī)來(lái)完成對(duì)環(huán)境信息的感知。由于通常不存在先驗(yàn)知識(shí),因此局部路徑的效率對(duì)無(wú)人機(jī)任務(wù)完成的效果起著至關(guān)重要的作用[1]。
當(dāng)前應(yīng)用廣泛的基于固定活動(dòng)窗的VFH類算法,通過(guò)設(shè)計(jì)一個(gè)虛擬的視窗,即活動(dòng)窗,來(lái)實(shí)現(xiàn)無(wú)人機(jī)的避障行為[2]。活動(dòng)窗的范圍是固定的,不能根據(jù)無(wú)人機(jī)所處的環(huán)境信息進(jìn)行動(dòng)態(tài)調(diào)整避障距離,這使得算法的環(huán)境適應(yīng)性受到限制,特別是在一些稀疏障礙物和密集障礙物并存的環(huán)境中。如果活動(dòng)窗設(shè)置過(guò)大,會(huì)導(dǎo)致原本存在缺口被認(rèn)定為“不可通行”,使得無(wú)人機(jī)忽略了可通行區(qū)域,在環(huán)境特別復(fù)雜時(shí),還會(huì)使無(wú)人機(jī)陷入局部停滯狀態(tài)。相反,如果活動(dòng)窗范圍過(guò)小,無(wú)人機(jī)需要在靠近障礙物時(shí)才能察覺(jué),容易引起路徑的不平滑,有時(shí)還容易陷入“陷講區(qū)域”。
文獻(xiàn)[3]在基于3DVFH算法上實(shí)現(xiàn)了系統(tǒng)中的局部避障飛行模塊,并基于本系統(tǒng)中無(wú)人機(jī)通過(guò)激光雷達(dá)可以360°感知障礙物信息。文獻(xiàn)[4]克服人工勢(shì)場(chǎng)法規(guī)劃的航跡容易陷入局部最優(yōu)陷阱和使用常規(guī)智能避障算法會(huì)使時(shí)間成本增加的問(wèn)題,利用鴿群優(yōu)化算法尋找最優(yōu)預(yù)測(cè)距離。在文獻(xiàn)[5]中,提出了一種基于改進(jìn)速度障礙法的局部路徑避障規(guī)劃算法。將傳統(tǒng)速度障礙法拓展到三維空間中,建立三維空間速度障礙模型。
本文在VFH算法基礎(chǔ)上,提出了一種基于多視野多分辨率的無(wú)人機(jī)局部路徑規(guī)劃算法。該算法的特點(diǎn)是無(wú)人機(jī)的視野范圍(活動(dòng)窗)不再固定不變,而是根據(jù)無(wú)人機(jī)周圍的環(huán)境狀況實(shí)時(shí)調(diào)整,以增強(qiáng)算法的環(huán)境適應(yīng)性。路徑規(guī)劃算法的輸入數(shù)據(jù)是當(dāng)前工作的環(huán)境模型,此模型需要構(gòu)建并實(shí)時(shí)更新環(huán)境地圖信息。通過(guò)多視野多分辨率的思想,算法可以更準(zhǔn)確地識(shí)別環(huán)境中的障礙物和缺口,更好地進(jìn)行避障規(guī)劃。
算法基于固定活動(dòng)窗的VFH算法[6-9],通過(guò)動(dòng)態(tài)確定視野層次并對(duì)其進(jìn)行分辨率處理,從而增強(qiáng)算法的環(huán)境適應(yīng)性。然后,算法找出各層的候選通行方向,并計(jì)算它們的通行代價(jià)[10-12]。最終,算法根據(jù)通行代價(jià)確定本輪算法的目標(biāo)方向,以實(shí)現(xiàn)更準(zhǔn)確的避障規(guī)劃。通過(guò)這種方式,算法能夠更好地適應(yīng)不同的環(huán)境,并更加準(zhǔn)確地識(shí)別障礙物和缺口,從而更好地進(jìn)行無(wú)人機(jī)的局部路徑規(guī)劃。
第一步是根據(jù)區(qū)域障礙物密度來(lái)劃分視野層次。區(qū)域內(nèi)障礙物密度d(destiny)是區(qū)域中所有障礙物數(shù)量和其矩形窗格的函數(shù),定義如下:
式(1)中,l是障礙物矩形窗格的平均邊長(zhǎng),M是障礙物的數(shù)量,s所選區(qū)域的面積。
視野層次的劃分流程如下,設(shè)定一個(gè)障礙物密度閾值θ。然后以無(wú)人機(jī)中心為圓心,隨機(jī)生成視野半徑r,不超過(guò)傳感器半徑。在視野圓內(nèi),計(jì)算區(qū)域障礙物密度d。如果d大于閾值θ,則當(dāng)前視野為最大視野層,逐漸減小r,當(dāng)密度降至閾值以下時(shí),確定為最小視野層。若d小于θ,則相反。類似于人類視野,遠(yuǎn)處視野模糊,近處視野清晰。所以各層分辨率的設(shè)置有兩個(gè)參考因素,一個(gè)是與無(wú)人機(jī)的距離,另一個(gè)是環(huán)境復(fù)雜度。分辨率Res計(jì)算公式如下:
式(2)中,Resi表示第i層視野的分辨率。λ是分辨率校正參數(shù)。當(dāng)該層障礙物密度d低于閾值θ時(shí),意味著當(dāng)前層障礙物密度低,環(huán)境簡(jiǎn)單,分辨率不要求很高,可以用上一次密度數(shù)據(jù)簡(jiǎn)單處理。而高于閾值時(shí),意味著環(huán)境復(fù)雜,需要傳感器仔細(xì)探索,參數(shù)采取指數(shù)形式的校正參數(shù),而不是簡(jiǎn)單線性關(guān)系處理。低分辨視野層通過(guò)形態(tài)學(xué)閉操作將障礙物進(jìn)行合并[13-15]。
軌跡搜索進(jìn)行如下步驟:首先,將無(wú)人機(jī)的視野劃分為多個(gè)扇形區(qū)域,如圖1所示。圖1表示在傳感器距離范圍內(nèi)分成四層視野,中心是無(wú)人機(jī)質(zhì)點(diǎn),S1,S2表示視野層內(nèi)繼續(xù)劃分的各級(jí)扇區(qū)。然后,統(tǒng)計(jì)每個(gè)扇區(qū)的環(huán)境復(fù)雜度。接著,根據(jù)預(yù)定的條件來(lái)判斷該扇區(qū)是否可以自由通行。以扇區(qū)中線為可通行方向,完成整個(gè)軌跡搜索過(guò)程。這種算法的優(yōu)點(diǎn)在于引入了多視野多分辨率層次,使得每個(gè)視野層次都有對(duì)應(yīng)的數(shù)據(jù),最終輸出的自由扇區(qū)也更加準(zhǔn)確。
圖1 視野和扇區(qū)劃分示意圖
扇區(qū)劃分后計(jì)算各扇區(qū)的環(huán)境復(fù)雜度,計(jì)算公式如下:
Hv表示第v個(gè)扇區(qū)的環(huán)境復(fù)雜度。λi表示第i層視野的調(diào)整因子。k∈Si,v表示v個(gè)扇區(qū)中第i層視野中的第k個(gè)障礙物。ai,bi是i層視野常數(shù),dk表示障礙物與無(wú)人機(jī)的距離,ri表示i層視野半徑。
由扇區(qū)的環(huán)境復(fù)雜度確定通行扇區(qū),只要低于閾值就認(rèn)為該扇區(qū)為可通行。連續(xù)的可通行扇區(qū)可以進(jìn)行合并,以減少后續(xù)通行代價(jià)的計(jì)算量。
最后是通行代價(jià)的計(jì)算,通過(guò)代價(jià)函數(shù)計(jì)算來(lái)判定評(píng)價(jià)候選方向的優(yōu)劣。但因?yàn)橐肓硕鄬右曇?,所以需要分層考慮。除了通用代價(jià)函數(shù)[16-18],加入了候選方向附近區(qū)域障礙物復(fù)雜度的正則式,以更準(zhǔn)確地標(biāo)識(shí)各候選方向的通行代價(jià),代價(jià)函數(shù)計(jì)算如式(4)所示:
其中,θ表示候選方向的角度,θt表示與全局目標(biāo)點(diǎn)的夾角,θn是無(wú)人機(jī)正對(duì)方向的角度,θn-1是上一輪前進(jìn)方向的角度,dsl,dsr,dsc分別是候選扇區(qū)左側(cè),右側(cè)和當(dāng)前扇區(qū)的障礙物密度。λ1,λ2,λ3,λ4是四項(xiàng)權(quán)重系數(shù),值越大意味著越傾向目標(biāo)角度、不改變當(dāng)前朝向、與上一輪規(guī)劃一致和保證附近扇區(qū)障礙物最小的原則。算法1偽代碼整體流程如下:
根據(jù)以上對(duì)無(wú)人局部路徑的算法求解,無(wú)人機(jī)獲取自身和障礙物的信息后,會(huì)建立一個(gè)多層多分辨率的視野信息,由以上信息來(lái)進(jìn)行避障機(jī)動(dòng)。為驗(yàn)證結(jié)果的有效性,本文采用Matlab進(jìn)行仿真試驗(yàn),基本參數(shù)設(shè)置為無(wú)人機(jī)速度v=20 m/s,傳感器最大量程d=500 m,障礙物安全距離為8 m。障礙物為機(jī)動(dòng)障礙物,初始信息如下:
假設(shè)機(jī)動(dòng)性障礙物的速度方向角的最大不確定度ε=100,根據(jù)表1所示的障礙物場(chǎng)景,考慮到三種障礙物的機(jī)動(dòng)性,為保證無(wú)人機(jī)路徑安全,通過(guò)算法計(jì)算規(guī)避路線,用matlab軟件記錄了考慮障礙物機(jī)動(dòng)性與無(wú)人機(jī)避障過(guò)程的相關(guān)參數(shù)。
為了評(píng)估自適應(yīng)威脅距離對(duì)無(wú)人機(jī)避障的影響,在考慮動(dòng)態(tài)障礙物機(jī)動(dòng)性和保證障礙物安全距離的前提下,表2中的相關(guān)參數(shù)顯示,無(wú)人機(jī)仍然能夠成功躲避機(jī)動(dòng)性障礙物。通過(guò)表中的UAV飛行狀態(tài)改變點(diǎn)的坐標(biāo)值,可以發(fā)現(xiàn)自適應(yīng)威脅距離的引入使無(wú)人機(jī)延遲了飛行狀態(tài)的調(diào)整,從而提高了原飛行路徑的利用率。圖2記錄了無(wú)人機(jī)在當(dāng)前情景下的偏航角與俯仰角的變化過(guò)程,均滿足無(wú)人機(jī)的約束條件。從圖2可以看出無(wú)人機(jī)的偏航角和俯仰角在3種障礙物下呈現(xiàn)出不同程度的變化,時(shí)間軸可以看出變化位置是符合預(yù)估的。
表2 無(wú)人機(jī)局部路徑規(guī)劃相關(guān)參數(shù)
圖2 無(wú)人機(jī)方向角變化
實(shí)驗(yàn)結(jié)果表明,雖然引入威脅距離前后的多層次視野算法在設(shè)定場(chǎng)景下的求解耗時(shí)都在0.008 s左右,但在引入威脅距離后的局部?jī)?yōu)化障礙中,由于無(wú)人機(jī)在保證安全距離的前提下延遲了飛行狀態(tài)的調(diào)整,所以在距離機(jī)動(dòng)障礙物更近的情況下才開(kāi)始規(guī)避,因此在飛行沖突解脫過(guò)程中耗時(shí)更短,提高了機(jī)動(dòng)障礙規(guī)避的時(shí)效性。
本文提出了一種基于多層視野多分辨率的無(wú)人機(jī)局部路徑規(guī)劃算法,無(wú)人機(jī)避障視野被分為三個(gè)層次,算法根據(jù)環(huán)境復(fù)雜度情況動(dòng)態(tài)確定使用的視野層次和各層的分辨率。然后進(jìn)行二值化處理,得到視野各區(qū)域的通行性。接著,從候選方向中根據(jù)代價(jià)值確定備選方向,最終綜合考慮各備選方向的累計(jì)選擇代價(jià)來(lái)確定算法的目標(biāo)行駛方向。仿真實(shí)驗(yàn)結(jié)果表明,本文方法有效地解決了固定活動(dòng)窗方法導(dǎo)致的問(wèn)題,具有更強(qiáng)的環(huán)境適應(yīng)性,特別是在陷阱區(qū)域和狹窄必行通道并存的較復(fù)雜環(huán)境中相比原方法具有更明顯的優(yōu)勢(shì)。