基于預(yù)探索的動(dòng)態(tài)自適應(yīng)3DVFH+無(wú)人機(jī)避障算法

何晨陽(yáng)，王耀力，常 青，孫永明

（1.太原理工大學(xué)信息與計(jì)算機(jī)學(xué)院，山西太原 030024；2.山西省林業(yè)與草原科學(xué)研究院，山西 太原 030024）

旋翼無(wú)人機(jī)廣泛應(yīng)用在農(nóng)業(yè)[1]、消防[2]、電力檢測(cè)[3]等諸多領(lǐng)域。因此，對(duì)于無(wú)人機(jī)來(lái)說(shuō)，能夠在未知且復(fù)雜的環(huán)境下實(shí)現(xiàn)高效感知復(fù)雜環(huán)境下自主規(guī)劃航跡并實(shí)時(shí)規(guī)避障礙物至關(guān)重要[4]。

無(wú)人機(jī)的避障規(guī)劃算法可分為全局避障規(guī)劃算法和局部規(guī)劃算法，全局規(guī)劃算法需要對(duì)環(huán)境進(jìn)行建圖并將環(huán)境信息存儲(chǔ)起來(lái)，在已知環(huán)境地圖的前提下進(jìn)行規(guī)劃[5-6]。全局規(guī)劃算法包括Dijkstra 算法[7]、A*算法[8]、Hybird A*算法[9]、Lazy Theat*算法[10]、D*算法[11]以及RRT（快速拓展隨機(jī)樹）算法[12]等，該類算法不會(huì)陷入局部最優(yōu)的問(wèn)題，但需要消耗大量?jī)?nèi)存來(lái)存儲(chǔ)環(huán)境信息；局部規(guī)劃算法對(duì)環(huán)境信息不做存儲(chǔ)，實(shí)時(shí)生成避障指令，比如人工勢(shì)場(chǎng)法[13]、BUG 算法[14]、DWA（動(dòng)態(tài)窗口）算法[15]、VFH（靜態(tài)矢量場(chǎng)）算法[16]以及3DVFH+算法[17]等，該類算法不需要存儲(chǔ)環(huán)境信息，所需內(nèi)存資源非常小，但會(huì)陷入局部最優(yōu)解。綜合對(duì)比以上各類算法，結(jié)合旋翼無(wú)人機(jī)平臺(tái)硬件資源稀缺且作業(yè)區(qū)域是三維環(huán)境，決定采用3DVFH+算法作為研究對(duì)象，針對(duì)該算法存在的問(wèn)題作出改進(jìn)。

文中將3DVFH+作為一個(gè)純局部算法，不需要構(gòu)建全局地圖。全局地圖被3D 點(diǎn)云所取代，開發(fā)一種成本較低的3D 環(huán)境避障算法。

1 避障規(guī)劃算法

3DVFH+算法利用Octomap 框架來(lái)表征環(huán)境信息，以octotree（八叉樹）的形式構(gòu)建全局地圖，從全局地圖中提取環(huán)境信息，生成障礙物直方圖進(jìn)行避障。

1.1 環(huán)境建圖

Octomap 數(shù)據(jù)結(jié)構(gòu)是一種表示3D 環(huán)境的有效方法[18]。八叉樹中的每個(gè)節(jié)點(diǎn)表示包含在立方體的空間，通常稱為體素。如圖1 所示，白色葉子節(jié)點(diǎn)為空閑狀態(tài)，灰色葉子節(jié)點(diǎn)狀態(tài)為占據(jù)狀態(tài)。通過(guò)這種方式，將整個(gè)環(huán)境都由體素進(jìn)行表示。

圖1 八叉樹結(jié)構(gòu)

在對(duì)環(huán)境的觀測(cè)過(guò)程中，往往會(huì)受到噪聲或者是環(huán)境本身動(dòng)態(tài)特性的影響，導(dǎo)致節(jié)點(diǎn)是否被占據(jù)屬于概率事件。假設(shè)t=1,2,…,T時(shí)刻，觀測(cè)到的數(shù)據(jù)為z1,z2,…,zT，則第n個(gè)節(jié)點(diǎn)被占據(jù)的概率為：

對(duì)式(1)進(jìn)行l(wèi)ogit變換，將概率變換至全實(shí)數(shù)空間：

反變換為：

式中，α稱為log-odds，用L() 表示節(jié)點(diǎn)的logodds，則上式變?yōu)椋?/p>

由此將空間點(diǎn)狀態(tài)的概率問(wèn)題轉(zhuǎn)化成對(duì)實(shí)數(shù)空間的加和運(yùn)算。

1.2 3DVHF+算法

1.2.1 算法原理

3DVFH+算法一共分為四個(gè)階段，用于計(jì)算無(wú)人機(jī)下一時(shí)刻的航路點(diǎn)。

1）八叉樹地圖探索

當(dāng)無(wú)人機(jī)在大型環(huán)境中移動(dòng)時(shí)，由于計(jì)算能力的限制，不可能探索所有的體素，因此，八叉樹探索階段只研究無(wú)人機(jī)周圍包圍框內(nèi)的體素。包圍框以無(wú)人機(jī)中心點(diǎn)為中心，大小為ws·ws·ws。

2）構(gòu)建2D 主極直方圖

將三維體素映射到二維主直方圖中，二維主極直方圖的橫坐標(biāo)為方位角，縱坐標(biāo)為俯仰角。對(duì)于三維點(diǎn)云中的每個(gè)點(diǎn)云數(shù)據(jù)，計(jì)算無(wú)人機(jī)位置相對(duì)于該點(diǎn)云的方位角θz和俯仰角θe，然后將該點(diǎn)映射到二維主極直方圖中。

如圖2所示，在笛卡爾坐標(biāo)系下對(duì)體素進(jìn)行建模。

圖2 體素在笛卡爾坐標(biāo)系下的表示

圖2 中，(xo,yo,zo)為無(wú)人機(jī)的質(zhì)心坐標(biāo)，(xi,yi,zi)為障礙物體素的坐標(biāo)，則有：

其中，L可表示為:

3）二值化極直方圖

將2D 主極直方圖中的每個(gè)格點(diǎn)的值與兩個(gè)閾值τlow和τhigh進(jìn)行比較，大于τhigh時(shí)，將該格點(diǎn)置為1；小于τlow時(shí)，將該格點(diǎn)置為0。如果格點(diǎn)的值位于兩個(gè)閾值之間，則用該格點(diǎn)旁邊的值來(lái)替代，如式（8）所示，這兩個(gè)閾值允許算法區(qū)分真實(shí)障礙物和測(cè)量誤差。

4）路徑檢測(cè)與選擇

為了確定可用路徑，使用滑動(dòng)窗口在2D 主極直方圖中進(jìn)行檢測(cè)，如果窗口中的所有元素都等于0，則將該窗口標(biāo)記為可通過(guò)，這樣可以得到所有的候選方向，然后計(jì)算代價(jià)函數(shù)得到最優(yōu)方向。

3DVFH+算法同時(shí)將候選方向與目標(biāo)方向、當(dāng)前方向、前一時(shí)刻方向的差值考慮進(jìn)來(lái)。采用式(9)所示代價(jià)函數(shù)得到最佳的前進(jìn)方向：

式中，dk為第k個(gè)候選方向，Δ 為計(jì)算兩項(xiàng)的絕對(duì)值，λ1、λ2、λ3分別為各個(gè)代價(jià)項(xiàng)的權(quán)重因子。第一項(xiàng)表示dk與目標(biāo)方向kt的絕對(duì)差值，第二項(xiàng)表示dk與無(wú)人機(jī)航向角的絕對(duì)差值，第三項(xiàng)表示dk與無(wú)人機(jī)上一時(shí)刻運(yùn)動(dòng)方向kd,n-1的絕對(duì)差值。

1.2.2 3DVFH+算法仿真

使用四軸旋翼飛行器作為實(shí)驗(yàn)平臺(tái)，通過(guò)設(shè)置“倒L”型以及長(zhǎng)墻障礙物來(lái)說(shuō)明算法的局部死區(qū)以及最優(yōu)解問(wèn)題。

如圖3 所示，對(duì)于長(zhǎng)墻障礙物，無(wú)人機(jī)在障礙物面前往返飛行，猶豫不定。對(duì)于“倒L”型障礙物，無(wú)人機(jī)在拐角處往返飛行，陷入局部死區(qū)。同時(shí)可以看出，針對(duì)“倒L”型障礙物，由于視野正前方?jīng)]有障礙物，故無(wú)人機(jī)選擇了一條更長(zhǎng)的路徑，暴露了算法局部最優(yōu)問(wèn)題。

圖3 原生算法

2 基于預(yù)探索的動(dòng)態(tài)自適應(yīng)3DVFH+算法

3DVFH+算法無(wú)法根據(jù)環(huán)境特性實(shí)時(shí)動(dòng)態(tài)地做出調(diào)整，導(dǎo)致在面對(duì)特殊障礙物時(shí)會(huì)陷入局部死區(qū)。同時(shí)3DVFH+作為一個(gè)純局部算法，存在局部最優(yōu)解的固有問(wèn)題。下面將對(duì)這兩個(gè)問(wèn)題進(jìn)行改進(jìn)。

2.1 自適應(yīng)代價(jià)函數(shù)

在原生代價(jià)函數(shù)中，當(dāng)候選航路點(diǎn)與目標(biāo)點(diǎn)之間的距離到達(dá)某一閾值時(shí)，代價(jià)由候選航路點(diǎn)與目標(biāo)之間的距離主導(dǎo)，此時(shí)無(wú)人機(jī)便會(huì)做出轉(zhuǎn)向的決策，從而造成了無(wú)人機(jī)往返飛行猶豫不定的現(xiàn)象。

對(duì)于3DVFH+算法的局部死區(qū)問(wèn)題，根據(jù)飛行進(jìn)度對(duì)代價(jià)函數(shù)的目標(biāo)距離權(quán)重因子進(jìn)行動(dòng)態(tài)調(diào)整，設(shè)置自適應(yīng)代價(jià)函數(shù)來(lái)消除算法的局部死區(qū)問(wèn)題。

首先將原始代價(jià)函數(shù)分為兩部分，分別是導(dǎo)航方向代價(jià)和路徑平滑代價(jià)，如下所示：

將代價(jià)函數(shù)式（10）細(xì)化為偏航代價(jià)與俯仰代價(jià)兩部分，如下所示：

f(dk)t為導(dǎo)航方向代價(jià)，分為偏航代價(jià)和俯仰代價(jià)兩部分，對(duì)應(yīng)的權(quán)重因子分別為λt_yaw和λt_pitch值。總的代價(jià)函數(shù)如式（13）所示：

其中，參數(shù)kt為導(dǎo)航方向代價(jià)的權(quán)重因子，參數(shù)ks為路徑平滑代價(jià)的權(quán)重因子。可通過(guò)配置上述各個(gè)代價(jià)項(xiàng)的權(quán)重因子得到側(cè)重性能不同的算法效果。

為實(shí)現(xiàn)對(duì)飛行進(jìn)展進(jìn)行監(jiān)控，動(dòng)態(tài)調(diào)整無(wú)人機(jī)距離目標(biāo)的成本，需要監(jiān)控?zé)o人機(jī)與目標(biāo)距離的趨勢(shì)。文中采用滑動(dòng)平均濾波器來(lái)表征無(wú)人機(jī)行為，如式（14）所示：

式中，t[k] 為當(dāng)前時(shí)刻，d[k] 為當(dāng)前時(shí)刻無(wú)人機(jī)與目標(biāo)之間的距離。當(dāng)s[k] 為負(fù)值且絕對(duì)值較大時(shí)，說(shuō)明無(wú)人機(jī)飛行效果良好，在不斷靠近目標(biāo)點(diǎn)；當(dāng)s[k] 在零附近時(shí)，說(shuō)明無(wú)人機(jī)在沿著障礙物做長(zhǎng)時(shí)間的繞行；當(dāng)s[k] 為正值且較大時(shí)，說(shuō)明無(wú)人機(jī)在不斷地遠(yuǎn)離目標(biāo)點(diǎn)。

有了對(duì)飛行行為的表征后，先為權(quán)重因子λt_yaw設(shè)定一個(gè)最大值λt_yaw_max和最小值λt_yaw_min以保障飛行安全。引入斜率閾值st且為負(fù)值，當(dāng)s[k] 小于st時(shí)，說(shuō)明此時(shí)無(wú)人機(jī)正在接近目標(biāo)，不用調(diào)整；當(dāng)s[k] 大于st且在零值附近時(shí)，引入?yún)^(qū)間Δsec來(lái)衡量s[k] 與零的絕對(duì)差值，說(shuō)明無(wú)人機(jī)正在做無(wú)用的繞行，需要對(duì)繞行動(dòng)作加以抑制，故以一定的速率rdec減小λtarget_yaw；當(dāng)s[k] 大于st且為正值時(shí)，說(shuō)明無(wú)人機(jī)正在遠(yuǎn)離目標(biāo)，若在無(wú)人機(jī)視野范圍FOV 內(nèi)檢測(cè)到障礙物，則以一定的速率rdec減小λt_yaw，允許無(wú)人機(jī)為了避開障礙物而做出適度遠(yuǎn)離目標(biāo)的行為，若在FOV 內(nèi)沒(méi)有檢測(cè)到障礙物，則以一定的速率rinc增大λt_yaw，即在沒(méi)有障礙物情況下增大無(wú)人機(jī)遠(yuǎn)離目標(biāo)的代價(jià)，使無(wú)人機(jī)更快到達(dá)目標(biāo)，如式（15）所示：

對(duì)采用自適應(yīng)代價(jià)函數(shù)的3DVFH+算法在同樣的障礙物環(huán)境下進(jìn)行仿真，結(jié)果如圖4 所示，可以看出改進(jìn)之后的算法消除了原生算法的局部死區(qū)問(wèn)題。

圖4 改進(jìn)算法

2.2 預(yù)探索機(jī)制

通過(guò)圖4(b)可以看出，自適應(yīng)代價(jià)函數(shù)解決了算法的局部死區(qū)問(wèn)題，但無(wú)人機(jī)當(dāng)前規(guī)劃的航跡并不是最優(yōu)軌跡。文中提出了預(yù)探索機(jī)制，通過(guò)構(gòu)建搜索樹消除局部最優(yōu)的缺陷。

如圖5 所示，虛線為無(wú)人機(jī)所有的探索節(jié)點(diǎn)，實(shí)線為代價(jià)函數(shù)值最小的節(jié)點(diǎn)方向。O點(diǎn)為無(wú)人機(jī)當(dāng)前位置，Ai、Bi、Ci、Di、Ei為無(wú)人機(jī)未來(lái)可能到達(dá)的位置。從所有的候選方向中計(jì)算出代價(jià)最小的方向，在此方向上前進(jìn)ds繼續(xù)進(jìn)行探索，經(jīng)過(guò)N次直方圖的迭代并逐步構(gòu)建搜索樹，樹的根節(jié)點(diǎn)為O，深度為Dtree。當(dāng)樹深度為1時(shí)該算法便退化為原生3DVHF+算法。

圖5 預(yù)探索

如圖6 所示，建立預(yù)探索后的算法對(duì)整個(gè)傳感器范圍進(jìn)行探索，消除了原生算法局部最優(yōu)的問(wèn)題。

圖6 消除局部最優(yōu)

3 3D可視化仿真平臺(tái)算法驗(yàn)證分析

3.1 仿真平臺(tái)搭建

如圖7 所示，PX4 飛行控制器是無(wú)人機(jī)的控制核心，該模型上搭載了一個(gè)深度相機(jī)，提供點(diǎn)云數(shù)據(jù)。仿真引擎Gazebo模擬無(wú)人機(jī)及外部環(huán)境，它允許在無(wú)人機(jī)模型上搭載傳感器，以提供數(shù)據(jù)輸出流，模擬真實(shí)環(huán)境。機(jī)載計(jì)算機(jī)提供ROS操作系統(tǒng)運(yùn)行環(huán)境。

圖7 仿真系統(tǒng)架構(gòu)

3.2 仿真結(jié)果

設(shè)置無(wú)人機(jī)與障礙物之間安全距離為2 m。改進(jìn)算法與原生算法對(duì)比如圖8 所示。實(shí)線為預(yù)規(guī)劃軌跡，虛線為預(yù)規(guī)劃軌跡經(jīng)過(guò)飛行控制器結(jié)合無(wú)人機(jī)動(dòng)力學(xué)特性后的實(shí)際飛行軌跡。

圖8 改進(jìn)算法與原生算法對(duì)比

如圖8(a)、(b)所示，針對(duì)原生算法的局部死區(qū)問(wèn)題，設(shè)置長(zhǎng)墻障礙物，目標(biāo)點(diǎn)坐標(biāo)為(12,0,3)，無(wú)人機(jī)起點(diǎn)坐標(biāo)為(0,0,0)，長(zhǎng)墻障礙物中心點(diǎn)坐標(biāo)為(8,0,6)，尺寸為(1,30,10)。原生算法陷入局部死區(qū)，而改進(jìn)算法順利到達(dá)目標(biāo)點(diǎn)。如圖8(c)、(d)所示，針對(duì)原生算法局部最優(yōu)問(wèn)題，同樣設(shè)置“倒L”型障礙物。目標(biāo)坐標(biāo)為(20,0,3)，起點(diǎn)坐標(biāo)為(0,0,0)，水平方向障礙物中心坐標(biāo)為(7,-2,5)，尺寸為(1,5,10)。垂直方向障礙物中心坐標(biāo)為(5,1,5)，尺寸為(5,2,10)。在算法構(gòu)建搜索樹的決策中，對(duì)“倒L”型障礙物下方空間。也進(jìn)行了探索，當(dāng)探索到障礙物時(shí)便停止了探索，并通過(guò)對(duì)上方空間的探索找到了更優(yōu)路徑。

表1 是算法在面對(duì)不同類型障礙物的規(guī)劃路徑長(zhǎng)度和時(shí)間對(duì)比。對(duì)于長(zhǎng)墻障礙物，原生算法陷入死區(qū)，而改進(jìn)算法消除了死區(qū)現(xiàn)象，成功到達(dá)目標(biāo)點(diǎn)；對(duì)于“倒L”型障礙物，改進(jìn)算法解決了局部最優(yōu)解問(wèn)題，相較于原生算法規(guī)劃航線縮短了37.15%，時(shí)間減少了25.87%。

表1 算法效果對(duì)比表

3.3 航跡規(guī)劃

森林相對(duì)來(lái)說(shuō)環(huán)境復(fù)雜度較高，點(diǎn)云稀疏且不規(guī)則，無(wú)人機(jī)在森林環(huán)境避障的難度較大。如圖9所示，搭建森林模型，利用地面站規(guī)劃航跡上傳至飛控，共設(shè)置了七個(gè)航路點(diǎn)，可以直觀地看出無(wú)人機(jī)進(jìn)行自主避障生成的平滑且無(wú)碰撞的飛行軌跡，進(jìn)一步驗(yàn)證了改進(jìn)算法的有效性。

圖9 森林障礙物軌跡規(guī)劃

圖10 所示是所提算法與使用八叉樹框架進(jìn)行建圖的3DVFH+算法在執(zhí)行相同飛行任務(wù)時(shí)的所需資源對(duì)比，橫坐標(biāo)為飛行時(shí)間，縱坐標(biāo)為所占資源比例。可以看出，改進(jìn)算法由于不用進(jìn)行建圖和存儲(chǔ)環(huán)境信息，RAM 節(jié)省了約25.26%。

圖10 CPU和RAM使用情況

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

文中提出了一種基于預(yù)探索機(jī)制的動(dòng)態(tài)自適應(yīng)3DVFH+避障規(guī)劃算法，該算法作為一個(gè)純反應(yīng)式的避障算法，不需要對(duì)環(huán)境建圖且不存儲(chǔ)環(huán)境信息，減小了對(duì)存儲(chǔ)資源的消耗。實(shí)驗(yàn)表明該文提出的避障規(guī)劃算法在面對(duì)特殊復(fù)雜障礙物時(shí)仍能生成一條平滑無(wú)碰撞的飛行軌跡，生成的路徑長(zhǎng)度更短，所需時(shí)間更少，且不會(huì)陷入死區(qū)和局部最優(yōu)解問(wèn)題。