何晨陽，王耀力，常 青，孫永明

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

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

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

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

1 避障規(guī)劃算法

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

1.1 環(huán)境建圖

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

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

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

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

反變換為：

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

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

1.2 3DVHF+算法

1.2.1 算法原理

3DVFH+算法一共分為四個階段，用于計算無人機下一時刻的航路點。

1）八叉樹地圖探索

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

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

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

如圖2所示，在笛卡爾坐標系下對體素進行建模。

圖2 體素在笛卡爾坐標系下的表示

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

其中，L可表示為:

3）二值化極直方圖

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

4）路徑檢測與選擇

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

3DVFH+算法同時將候選方向與目標方向、當前方向、前一時刻方向的差值考慮進來。采用式(9)所示代價函數(shù)得到最佳的前進方向：

式中，dk為第k個候選方向，Δ 為計算兩項的絕對值，λ1、λ2、λ3分別為各個代價項的權(quán)重因子。第一項表示dk與目標方向kt的絕對差值，第二項表示dk與無人機航向角的絕對差值，第三項表示dk與無人機上一時刻運動方向kd,n-1的絕對差值。

1.2.2 3DVFH+算法仿真

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

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

圖3 原生算法

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

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

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

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

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

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

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

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

其中，參數(shù)kt為導(dǎo)航方向代價的權(quán)重因子，參數(shù)ks為路徑平滑代價的權(quán)重因子?？赏ㄟ^配置上述各個代價項的權(quán)重因子得到側(cè)重性能不同的算法效果。

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

式中，t[k] 為當前時刻，d[k] 為當前時刻無人機與目標之間的距離。當s[k] 為負值且絕對值較大時，說明無人機飛行效果良好，在不斷靠近目標點；當s[k] 在零附近時，說明無人機在沿著障礙物做長時間的繞行；當s[k] 為正值且較大時，說明無人機在不斷地遠離目標點。

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

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

圖4 改進算法

2.2 預(yù)探索機制

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

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

圖5 預(yù)探索

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

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

3 3D可視化仿真平臺算法驗證分析

3.1 仿真平臺搭建

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

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

3.2 仿真結(jié)果

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

圖8 改進算法與原生算法對比

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

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

表1 算法效果對比表

3.3 航跡規(guī)劃

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

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

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

圖10 CPU和RAM使用情況

4 結(jié)束語

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