基于多層視野的無(wú)人機(jī)局部路徑規(guī)劃算法

尚嵩，劉明剛，丁士強(qiáng)

（海軍航空大學(xué)，山東煙臺(tái) 264001）

1 引言

在基于無(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ī)劃。

2 方法介紹

算法基于固定活動(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偽代碼整體流程如下：

3 仿真實(shí)驗(yàn)結(jié)果與分析

3.1 參數(shù)設(shè)置

根據(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ù)。

3.2 仿真結(jié)果

為了評(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í)效性。

4 結(jié)論

本文提出了一種基于多層視野多分辨率的無(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ì)。