基于改進(jìn)動(dòng)態(tài)窗口法的無(wú)人機(jī)避障航跡規(guī)劃

戴凱龍，賈子彥，潘玲佼

（江蘇理工學(xué)院 電氣信息工程學(xué)院，江蘇 常州 213000）

0 引 言

避障與航跡規(guī)劃一直是無(wú)人機(jī)系統(tǒng)的研究重點(diǎn)，這就對(duì)如何控制無(wú)人機(jī)進(jìn)行自主導(dǎo)航避障提出了要求。大多數(shù)的航跡規(guī)劃算法中，忽略了無(wú)人機(jī)的狀態(tài)和運(yùn)動(dòng)學(xué)約束。如A*算法、蟻群算法、人工勢(shì)場(chǎng)法等，規(guī)劃出的航跡轉(zhuǎn)折較大，無(wú)法直接使用，往往需要進(jìn)行軌跡優(yōu)化等操作，其代價(jià)隨搜索空間的維度和復(fù)雜度的增大而明顯增加。在航跡規(guī)劃時(shí)，應(yīng)當(dāng)充分考慮無(wú)人機(jī)自身的狀態(tài)和性能變化，使仿真更貼近于實(shí)際，增強(qiáng)實(shí)際控制的魯棒性。

動(dòng)態(tài) 窗 口 法（Dynamic Windows Approach，DWA）是一種直接在控制指令空間內(nèi)搜索最佳控制指令的自主避障算法。根據(jù)智能體的運(yùn)動(dòng)學(xué)特性，得到一系列離散的速度控制組合，并模擬在各控制輸入下的軌跡，最后通過(guò)最大化目標(biāo)函數(shù)來(lái)選擇下一刻的最優(yōu)控制輸入。

針對(duì)動(dòng)態(tài)窗口法存在易陷入局部最優(yōu)的問(wèn)題，很多研究人員考慮將全局路徑規(guī)劃算法與局部路徑規(guī)劃算法相結(jié)合。首先通過(guò)全局規(guī)劃算法尋找出一條最優(yōu)的路徑，然后提取路徑中的關(guān)鍵節(jié)點(diǎn)，最后以局部路徑規(guī)劃算法逐一跟蹤這些關(guān)鍵節(jié)點(diǎn)來(lái)不斷朝目標(biāo)點(diǎn)逼近。但是，該方法存在一些局限性：一是很多情況下的探索任務(wù)需要實(shí)時(shí)進(jìn)行，無(wú)法取得完整的環(huán)境信息；二是路徑的好壞與全局規(guī)劃出的關(guān)鍵節(jié)點(diǎn)有很大關(guān)系，在受到動(dòng)態(tài)障礙物的影響偏離航線后，不能保證不會(huì)陷入局部最優(yōu)中。

基于以上分析，本文針對(duì)飛行環(huán)境中存在諸多障礙物的情況，搭建了無(wú)人機(jī)的運(yùn)動(dòng)模型，采用動(dòng)態(tài)窗口法來(lái)預(yù)測(cè)避障軌跡，并選擇最優(yōu)的速度作為控制輸入。同時(shí)針對(duì)局部最優(yōu)問(wèn)題，給出了一種簡(jiǎn)單的失效環(huán)境判別方法，并通過(guò)設(shè)置虛擬目標(biāo)點(diǎn)的方法實(shí)現(xiàn)了規(guī)避。

1 無(wú)人機(jī)運(yùn)動(dòng)學(xué)模型

對(duì)無(wú)人機(jī)進(jìn)行分析前，需要對(duì)模型進(jìn)行一定的簡(jiǎn)化，只考慮無(wú)人機(jī)的質(zhì)心運(yùn)動(dòng)學(xué)方程。當(dāng)無(wú)人機(jī)存在全向運(yùn)動(dòng)時(shí)，則在二維笛卡爾坐標(biāo)系下的運(yùn)動(dòng)學(xué)方程為：

其中，x，y，θ、分別為無(wú)人機(jī)當(dāng)前的位置和偏轉(zhuǎn)角度；v、v為無(wú)人機(jī)在機(jī)體坐標(biāo)系下，軸和軸兩個(gè)方向的飛行速度；為偏轉(zhuǎn)速度；為時(shí)間間隔；x，y，θ為無(wú)人機(jī)下一時(shí)刻的位置和偏轉(zhuǎn)角度。

2 動(dòng)態(tài)窗口法

根據(jù)無(wú)人機(jī)速度和位置的對(duì)應(yīng)關(guān)系，將無(wú)人機(jī)的位置控制轉(zhuǎn)換為對(duì)無(wú)人機(jī)的速度控制，將避障問(wèn)題轉(zhuǎn)換成速度空間帶約束的優(yōu)化問(wèn)題。

2.1 無(wú)人機(jī)速度空間

同時(shí)，無(wú)人機(jī)在水平飛行存在最大飛行速度和最大偏轉(zhuǎn)速度以及最小偏轉(zhuǎn)速度的限制，速度空間里的速度還應(yīng)滿足式（3）的條件：

2.2 軌跡預(yù)測(cè)

在預(yù)測(cè)軌跡前，需要按照一定的速度分辨率和偏轉(zhuǎn)速度分辨率，將速度空間離散成有限的速度組合。在預(yù)測(cè)軌跡時(shí)，保持控制輸入不變，即加速度和偏轉(zhuǎn)加速度為0，然后根據(jù)式（1）對(duì)無(wú)人機(jī)在每一組速度下飛行一段時(shí)間后的軌跡進(jìn)行預(yù)測(cè)，同時(shí)將其中會(huì)與障礙物發(fā)生碰撞的軌跡剔除。預(yù)測(cè)軌跡如圖1所示。

圖1預(yù)測(cè)軌跡仿真Fig.1 Simulation of predicted trajectory

2.3 目標(biāo)函數(shù)

為了使無(wú)人機(jī)能夠更好、更安全地到達(dá)目標(biāo)點(diǎn)，通過(guò)目標(biāo)函數(shù)對(duì)預(yù)測(cè)軌跡進(jìn)行評(píng)價(jià)，其形式如下：

其中，航向得分項(xiàng)（v，v，）度量該條預(yù)測(cè)軌跡速度下軌跡末端的飛行方向與目標(biāo)方向之間的偏離程度，其公式為180（為軌跡末端的飛行方向與目標(biāo)方向之間的角度差）；距離得分項(xiàng)（v，v，）表示該條軌跡到最近障礙物的距離，用來(lái)剔除會(huì)發(fā)生碰撞的軌跡以保證飛行安全，當(dāng)無(wú)障礙物時(shí)，將函數(shù)值設(shè)置為一個(gè)常數(shù)；速度得分項(xiàng)（v，v，）用于從能夠安全避障的軌跡中選擇出飛行速度較快的軌跡，其值為合速度的模；，，分別為3個(gè)得分項(xiàng)的權(quán)重。

在計(jì)算完所有軌跡的得分后，需要對(duì)每條軌跡的各個(gè)得分項(xiàng)進(jìn)行歸一化（σ）處理，然后再相加。歸一化的準(zhǔn)則是每一項(xiàng)除以每一項(xiàng)的總和。以航向得分項(xiàng)為例，其計(jì)算公式如式（5）：

其中，為待評(píng)價(jià)的當(dāng)前軌跡，為所有預(yù)測(cè)軌跡的采樣數(shù)量。

最后，選擇使目標(biāo)函數(shù)值最大的速度，作為下一時(shí)刻的控制輸入。重復(fù)上述過(guò)程，直至到達(dá)目標(biāo)點(diǎn)。

3 算法失效環(huán)境規(guī)避

當(dāng)目標(biāo)點(diǎn)出現(xiàn)在“U”型障礙物后面時(shí)，容易陷入局部最優(yōu)，使算法失效。但是，在這種情況發(fā)生時(shí)很容易被檢測(cè)到。

3.1 算法失效環(huán)境判斷

假設(shè)無(wú)人機(jī)可獲得周?chē)械然蜉^低精度的障礙物距離和方位，選取前進(jìn)方向上一定角度扇形探測(cè)區(qū)域，作為算法失效環(huán)境的判斷依據(jù)，如圖2所示。當(dāng)障礙物與無(wú)人機(jī)之間的距離小于閾值R時(shí)，分析扇形區(qū)域中角度內(nèi)探測(cè)到的距離信息。若角度中沒(méi)有可通行區(qū)域，且距離信息整體呈現(xiàn)“短長(zhǎng)短”的形式，則判斷前方為“U”型失效環(huán)境，需要進(jìn)行規(guī)避，否則判斷為一般障礙物，不做處理。

圖2 失效環(huán)境判斷Fig.2 Judgment of failure environment

3.2 基于虛擬逃逸目標(biāo)點(diǎn)的障礙規(guī)避

由于原目標(biāo)點(diǎn)無(wú)法幫助無(wú)人機(jī)規(guī)避失效環(huán)境，因此需要人為設(shè)置一個(gè)虛擬的目標(biāo)點(diǎn)進(jìn)行逃逸。另外，這個(gè)虛擬目標(biāo)點(diǎn)應(yīng)使無(wú)人機(jī)可達(dá)且安全。在一些特殊情況下，可能會(huì)檢測(cè)出沒(méi)有安全的區(qū)域。此時(shí)可以對(duì)探測(cè)信息進(jìn)行二次處理，從無(wú)人機(jī)就近的安全區(qū)域中尋找逃逸目標(biāo)點(diǎn)。

將傳感器的扇形探測(cè)區(qū)域按照一定的角度（）均等分割，虛擬目標(biāo)點(diǎn)從沒(méi)有障礙物的分塊區(qū)域中選擇，應(yīng)滿足式（6）的要求：

其中，為扇形分塊區(qū)域與最近障礙物之間的夾角；為扇形分塊區(qū)域與目標(biāo)點(diǎn)之間的夾角；，分別為相應(yīng)系數(shù)；為歸一化處理。

通過(guò)得分選擇一個(gè)比較安全，且使無(wú)人機(jī)偏離目標(biāo)點(diǎn)越小的分塊區(qū)域的中點(diǎn)位置，作為逃逸時(shí)的虛擬目標(biāo)點(diǎn)。同時(shí)增加（v，v，）得分項(xiàng)，使無(wú)人機(jī)朝虛擬目標(biāo)點(diǎn)逃逸，其形式如式（7）：

其中，φ為預(yù)測(cè)軌跡末端的前進(jìn)方向與到虛擬目標(biāo)點(diǎn)方向之間的夾角；為失效環(huán)境標(biāo)志量，當(dāng)判定前方障礙物為“U”型失效環(huán)境時(shí)，為0，否則為1。

為避免航向得分項(xiàng)在逃逸期間影響無(wú)人機(jī)，對(duì)航向得分項(xiàng)進(jìn)行修改，其形式如式（8）：

當(dāng)沒(méi)有探測(cè)到失效環(huán)境時(shí)，將目標(biāo)點(diǎn)作為虛擬目標(biāo)點(diǎn)，同時(shí)對(duì)目標(biāo)函數(shù)進(jìn)行改進(jìn)。

3.3 算法流程

本文算法的具體實(shí)現(xiàn)步驟如下：

獲得無(wú)人機(jī)當(dāng)前的飛行速度和方向，并根據(jù)運(yùn)動(dòng)學(xué)特性，分別計(jì)算無(wú)人機(jī)下一刻所能達(dá)到的軸、軸方向的速度，以及偏轉(zhuǎn)速度的范圍；

根據(jù)速度范圍生成速度空間，并在速度空間中，對(duì)各個(gè)速度進(jìn)行離散采樣，得到一系列速度（v，v，）；

根據(jù)無(wú)人機(jī)的運(yùn)動(dòng)學(xué)方程，對(duì)采樣得到的速度進(jìn)行模擬，進(jìn)而得到對(duì)應(yīng)速度下的預(yù)測(cè)軌跡；

根據(jù)傳感器信息，判斷前方飛行區(qū)域是否存在“U”型失效環(huán)境，若存在則從探測(cè)區(qū)域中尋找虛擬目標(biāo)點(diǎn)進(jìn)行逃逸；否則將目標(biāo)點(diǎn)作為虛擬目標(biāo)點(diǎn)進(jìn)行正常飛行；

采用目標(biāo)函數(shù)對(duì)預(yù)測(cè)軌跡進(jìn)行評(píng)價(jià)，然后選擇目標(biāo)函數(shù)值最大的軌跡對(duì)應(yīng)下的速度，作為下一時(shí)刻的控制輸入；

執(zhí)行最優(yōu)速度，基于當(dāng)前位置判斷是否到達(dá)目標(biāo)點(diǎn)，若已到達(dá)則結(jié)束運(yùn)行；否則返回第一步，進(jìn)入下一循環(huán)。

4 仿真驗(yàn)證及結(jié)果分析

4.1 基本參數(shù)

為了驗(yàn)證本文算法的有效性，對(duì)算法進(jìn)行仿真實(shí)驗(yàn)。仿真環(huán)境為MATLAB R2021b，算法涉及無(wú)人機(jī)自身參數(shù)與算法參數(shù)。根據(jù)實(shí)際情況和常規(guī)硬件配置，本文選取參數(shù)見(jiàn)表1。

表1 算法相關(guān)參數(shù)Tab.1 Relevant parameters of the algorithm

仿真時(shí)將無(wú)人機(jī)當(dāng)作質(zhì)點(diǎn)處理（實(shí)際無(wú)人機(jī)具有一定的體積），因此設(shè)置了無(wú)人機(jī)與障礙物的碰撞距離閾值（仿真閾值為0.5 m），同時(shí)當(dāng)無(wú)人機(jī)到達(dá)目標(biāo)點(diǎn)半徑0.2 m范圍內(nèi)，便視為到達(dá)目標(biāo)點(diǎn)。為了避免逃逸目標(biāo)點(diǎn)不可達(dá)的情況，無(wú)人機(jī)在逃逸目標(biāo)點(diǎn)附近1.5 m范圍內(nèi)視為到達(dá)。

4.2 避障實(shí)驗(yàn)

建立如圖3所示的24×24的環(huán)境地圖，為了避免環(huán)境的偶然性，分別設(shè)計(jì)了兩條路線。分別為起點(diǎn)（4，9）到目標(biāo)點(diǎn)（13，21），以及起點(diǎn)（3，3）到目標(biāo)點(diǎn)（22，17），黑色方格為障礙物。由仿真結(jié)果可見(jiàn)，當(dāng)目標(biāo)點(diǎn)出現(xiàn)在“U”型障礙物后面時(shí)，改進(jìn)前的算法面對(duì)“U”型障礙物時(shí)直接陷入了局部最優(yōu)，而改進(jìn)后的算法通過(guò)虛擬目標(biāo)點(diǎn)，實(shí)現(xiàn)了逃逸，并最終到達(dá)目標(biāo)點(diǎn)。

圖3 算法仿真圖Fig.3 Simulation diagram of algorithm

4.3 動(dòng)態(tài)環(huán)境實(shí)驗(yàn)

為了驗(yàn)證本文算法在動(dòng)態(tài)環(huán)境中的性能，仿真結(jié)果如圖4所示。在環(huán)境中加入一些動(dòng)態(tài)障礙物（圖中的紅色圓）后，無(wú)人機(jī)飛行時(shí)不但能夠躲避環(huán)境中的動(dòng)態(tài)障礙物，同時(shí)還能夠避免因躲避障礙物而陷入局部最優(yōu)的情況。

圖4 動(dòng)態(tài)環(huán)境實(shí)驗(yàn)Fig.4 Experiment in dynamic environment

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

本文考慮了無(wú)人機(jī)帶有運(yùn)動(dòng)約束情況的避障問(wèn)題，使用動(dòng)態(tài)窗口法來(lái)尋找最優(yōu)的避障路徑，同時(shí)提出了一種依靠局部環(huán)境信息來(lái)規(guī)避“U”型失效環(huán)境的方法。仿真結(jié)果表明：本文算法能夠使無(wú)人機(jī)在避障規(guī)劃時(shí)避免陷入局部最優(yōu)的情況，實(shí)現(xiàn)了在動(dòng)態(tài)復(fù)雜環(huán)境中的自主飛行。