符小衛(wèi)，吳迪，支辰元

西北工業(yè)大學(xué)，陜西 西安 710072

隨著戰(zhàn)場環(huán)境日益復(fù)雜多變,微型旋翼無人機(jī)憑借其造價低廉、機(jī)動性好、難以攔截等優(yōu)點(diǎn),在偵察監(jiān)視、電子戰(zhàn)攻擊等領(lǐng)域能夠極大提高任務(wù)完成效率[1]。

為了讓無人機(jī)能夠在復(fù)雜未知環(huán)境下,自主高效完成任務(wù),國內(nèi)外專家學(xué)者對無人機(jī)路徑規(guī)劃算法進(jìn)行了深入研究,主要涉及全局規(guī)劃算法和局部規(guī)劃算法兩類。人工勢場法于20世紀(jì)80年代由Khatib提出, 將移動機(jī)器人的運(yùn)動視作質(zhì)點(diǎn)在一種抽象的人造勢力場中的運(yùn)動:目標(biāo)點(diǎn)對質(zhì)點(diǎn)產(chǎn)生引力, 而障礙物對質(zhì)點(diǎn)產(chǎn)生斥力, 依據(jù)合力確定移動機(jī)器人的運(yùn)動方向和路徑[2-3]。尼爾森等在1980 年提出了A*算法, 是一種應(yīng)用很廣的啟發(fā)式搜索算法。利用空間啟發(fā)式信息, 通過對比選擇恰當(dāng)?shù)脑u估函數(shù), 通過動態(tài)搜索策略, 求出移動機(jī)器人的最優(yōu)規(guī)劃路徑。劉斌和許震洪等[4-5]提出了一種基于A*算法的動態(tài)多路徑規(guī)劃方法,結(jié)合A*算法與矩形限制搜索區(qū)域算法, 給出了一種求解單一優(yōu)化路徑的動態(tài)路徑規(guī)劃算法,同時提出了一種重復(fù)路徑懲罰因子, 利用一次搜索得出多條優(yōu)化路徑。彭錦城等[6]提出了基于改進(jìn)Hybrid A*的無人機(jī)路徑規(guī)劃算法,通過改進(jìn)Hybrid A*前端三維路徑搜索的啟發(fā)式函數(shù)和后端非線性優(yōu)化的平滑函數(shù),解決了無人機(jī)自主避障飛行中快速旋轉(zhuǎn)翻轉(zhuǎn)等問題,提高了無人機(jī)目標(biāo)識別、偵察監(jiān)視的準(zhǔn)確度。

針對傳統(tǒng)VFH算法[7-13]的無記憶性而易于陷入局部陷阱且閾值選擇影響避障性能優(yōu)劣,本文提出一種實(shí)時陷阱檢測機(jī)制,在激光雷達(dá)[10]探測信息的基礎(chǔ)上動態(tài)添加歷史柵格信息以避免陷入局部陷阱,并設(shè)計了一種動態(tài)閾值更新策略,克服了傳統(tǒng)VFH 算法的閾值敏感性問題,增強(qiáng)了算法在不同場景的泛化性。由于在障礙物信息不確定環(huán)境下單無人機(jī)避障規(guī)劃問題中,傳統(tǒng)的全局規(guī)劃算法（如A*算法）需要完全了解全局環(huán)境且無法滿足實(shí)時性要求,本文基于局部規(guī)劃和全局規(guī)劃的特點(diǎn),將A*算法與改進(jìn)的VFH算法相結(jié)合[11],設(shè)計了一種復(fù)雜環(huán)境中的單無人機(jī)路徑規(guī)劃與避障策略,在靜態(tài)已知障礙物的全局路徑規(guī)劃基礎(chǔ)上,根據(jù)激光雷達(dá)采集的數(shù)據(jù)輔助無人機(jī)局部規(guī)劃處理,實(shí)現(xiàn)有效避障。

1 環(huán)境建模

與大多數(shù)傳統(tǒng)建模方法中的假定障礙物分布情況全局已知不同,本文研究不確定的障礙物分布情況,即無人機(jī)在飛行前對地形信息并非完全了解,需要在飛行過程中實(shí)時探測周圍的地形情況[14-15]。本文的研究場景如圖1所示,在l×l的二維空間區(qū)域內(nèi)包括靜態(tài)已知障礙物與未知障礙物:圖1 中深藍(lán)色幾何體為靜態(tài)已知障礙物,代表通過情報、地圖或其他手段獲取到的地形信息,無人機(jī)在起飛前就對這些障礙物完全了解,因此可以通過這些信息進(jìn)行初步的路徑規(guī)劃；圖1中淺藍(lán)色幾何體為未知障礙物,其坐標(biāo)位置、大小、形狀都是未知的,無人機(jī)需要運(yùn)動到其四周才能探測到其信息。

本文假設(shè)一架微型四旋翼無人機(jī)始終在同一高度平面內(nèi)運(yùn)動,并建立二維空間內(nèi)的無人機(jī)二階運(yùn)動模型。

式中,在t時刻時,無人機(jī)的位置矢量為pu(t) =(pux,puy),速度矢量為v(t) =(vx,vy),加速度矢量為u(t) =(ux,uy),加速度矢量為無人機(jī)的控制輸入量。四旋翼無人機(jī)具有最大速度限制,|v(t)| ≤vmax。四旋翼無人機(jī)的任務(wù)是穿越障礙物區(qū)域到達(dá)目標(biāo)位置pg=(pgx,pgy)。

圖1 無人機(jī)避障場景圖Fig.1 UAV obstacle avoidance scene

在使用A*算法進(jìn)行全局路徑規(guī)劃之前需要對地形進(jìn)行合理構(gòu)建,本文采用柵格法對地圖進(jìn)行建模,將無人機(jī)所在二維場景劃分為大小相同的矩形柵格點(diǎn),柵格精度為dg。因此,可用一個二維矩陣M=(mij)n×n表示該二維地圖信息,其中,矩陣中每個元素表示一個dg×dg的矩形區(qū)域內(nèi)的地形信息。當(dāng)柵格點(diǎn)屬于靜態(tài)障礙物點(diǎn)集時mij=1,否則mij= 0。

本文采用激光雷達(dá)作為傳感器來進(jìn)行地形探測,設(shè)其水平分辨律為ar,最大探測距離為dr,即在以激光雷達(dá)掛載點(diǎn)為圓心、dr為半徑的圓形區(qū)域內(nèi)每隔ar角度收集該方向上的地形信息,其示意圖如圖2 所示。激光雷達(dá)每一幀返回的數(shù)據(jù)以點(diǎn)云的形式呈現(xiàn),表示該未知障礙物的輪廓表面的點(diǎn)數(shù)據(jù)集合。以無人機(jī)的時變?nèi)肿鴺?biāo)pu(t) =(pux,puy)為中心建立局部時變坐標(biāo)系Γ 用來表示每個點(diǎn)的坐標(biāo)。點(diǎn)云中第i個點(diǎn)的時變?nèi)肿鴺?biāo)為pi=(pix,piy),即Γ中對應(yīng)的坐標(biāo)為piu=(pix-pux,piy-puy)。

圖2 激光雷達(dá)點(diǎn)云示意圖Fig.2 Lidar point cloud map

2 傳統(tǒng)向量場直方圖算法原理

向量場直方圖（VFH）算法將無人機(jī)四周的障礙物映射到了以無人機(jī)為中心的極坐標(biāo)系上,并采用三個層次的數(shù)據(jù)信息表示方法來對傳感器反饋的無人機(jī)周邊環(huán)境信息進(jìn)行逐級數(shù)據(jù)縮減,以用于最終的基于閾值的方向控制,具體算法實(shí)現(xiàn)如下。

（1） VFH算法的第一步同樣需要構(gòu)建柵格地圖。如圖3所示,將運(yùn)行環(huán)境以柵格形式進(jìn)行劃分,以無人機(jī)為中心構(gòu)建活動窗口,并通過cij表示柵格內(nèi)存在障礙物的置信度,該值隨著激光雷達(dá)的不斷掃描而遞增更新。

圖3 柵格地圖構(gòu)建Fig.3 Grid map construction

（2） 第二層VFH算法將柵格置信度矩陣和相對無人機(jī)的距離映射到極坐標(biāo)直方圖上。以無人機(jī)坐標(biāo)pu=(pux,puy)為中心建立極坐標(biāo)系,每個柵格點(diǎn)(i,j)都可以看作一個由無人機(jī)指向柵格點(diǎn)的矢量,βij為該柵格在極坐標(biāo)系下的角度,mij為障礙物確定度,用于計算極障礙物密度（POD）

式中,點(diǎn)(i,j)的坐標(biāo)為(xi,yj)；cij為該柵格點(diǎn)的障礙物置信度；dij為該柵格點(diǎn)距離無人機(jī)的歐氏距離；a、b為正常數(shù),且取值滿足a-bdmax=0,其中dmax是無人機(jī)當(dāng)前位置到活動窗口中最遠(yuǎn)柵格的距離。

在VFH 算法中,無人機(jī)采用激光雷達(dá)等傳感器,具有360°全向視野。水平角度分辨率為ar,表示將360°視野分為n個角度為ar的扇區(qū),即n=。因此,每個柵格點(diǎn)即位于對應(yīng)的扇區(qū)中,其扇區(qū)編號k如式（4）所示

式中,k= 0, 1, …,n- 1。每一個扇區(qū),都有一個極障礙物密度值,記作hk,由該扇區(qū)角度范圍內(nèi)障礙物確定度計算可得。

（3） 第三層是算法輸出,經(jīng)過第一層及第二層的障礙物信息處理,得到極坐標(biāo)直方圖H,以表示無人機(jī)周圍各個扇區(qū)內(nèi)的障礙物分布情況。

直方圖中存在多個波峰波谷,設(shè)置一個閾值T來篩選出可能的前進(jìn)方向。圖4 所示是一個典型的極坐標(biāo)直方圖,其中每一個藍(lán)色柱形即為扇區(qū)對應(yīng)的平滑處理后的極障礙物密度值。圖4中紅線即閾值T,根據(jù)極障礙物密度值小于閾值的扇區(qū)為波谷,從直方圖中篩選出三個備選波谷。

圖4 極坐標(biāo)直方圖Fig.4 Polar coordinate histogram

設(shè)定角度閾值at,根據(jù)每個波谷跨越的連續(xù)扇區(qū)角度值大小將波谷劃分為寬波谷和窄波谷,若波谷寬度大于at則是寬波谷,否則是窄波谷。定義第m個波谷的起始和終止角度邊界值為αm_begin和αm_end,針對每一個波谷計算無人機(jī)運(yùn)動的備選方向。窄波谷的角度中線即為備選方向,即；而寬波谷會在貼近邊界處產(chǎn)生兩個備選方向,即。

得到所有備選方向后根據(jù)目標(biāo)在局部時變坐標(biāo)系Γ下角度αg,選擇進(jìn)行前進(jìn)方向αtarget。若第m個波谷有αm_begin<αg<αm_end,即包含角度αg對應(yīng)方向,則直接向目標(biāo)運(yùn)動即可；否則找出與αg最接近的備選方向作為新的αtarget。如圖5 所示,目標(biāo)方向被波峰遮擋,因此無人機(jī)從波谷1 中選擇與目標(biāo)方向最接近的備選方向作為前進(jìn)方向。

最后根據(jù)本文研究場景,通過VFH算法設(shè)計無人機(jī)控制律,即u=(ηcos(αtarget),ηsin(αtarget)),其中η為正常數(shù)。

3 改進(jìn)的VFH算法

VFH算法作為一種局部路徑規(guī)劃算法,存在以下缺陷:（1）傳統(tǒng)VFH算法的無記憶性導(dǎo)致在一些特殊場景中易陷入局部陷阱；（2）對于閾值的選擇影響到避障性能優(yōu)劣。針對以上問題,本節(jié)針對具體場景進(jìn)行建模分析,提出基于陷阱檢測機(jī)制與動態(tài)閾值的VFH算法,仿真驗(yàn)證所提出改進(jìn)策略的有效性。

3.1 基于陷阱檢測機(jī)制的VFH算法

圖5 VFH算法示意圖Fig.5 VFH schematic diagram

本文將歷史時刻柵格信息作為記憶棧,并根據(jù)無人機(jī)是否陷入陷阱來動態(tài)選取記憶棧中的信息,以擴(kuò)展VFH算法的記憶性。

首先設(shè)計了一種陷阱檢測機(jī)制,以實(shí)時判斷是否陷入了局部陷阱,示意圖如圖6所示。

圖6 實(shí)時陷阱檢測機(jī)制示意圖Fig.6 Real time trap detection mechanism

設(shè)置陷阱檢測精度αtrap,將每一個全局地形柵格(i,j)劃分為n個扇區(qū),即n=,每個扇區(qū)用一個對應(yīng)值trap_numi,j,k描述無人機(jī)曾經(jīng)踏足柵格(i,j)且速度矢量角度φ位于扇區(qū)k的時刻信息。

（1） 初始階段,統(tǒng)計值trap_numi,j,k初始化為-1。

（2）t0時刻當(dāng)無人機(jī)第一次到達(dá)某個柵格點(diǎn)(i,j)時（發(fā)現(xiàn)對應(yīng)trap_numi,j,k=- 1）,設(shè)置此時無人機(jī)速度矢量角度φ對應(yīng)扇區(qū)k的trap_numi,j,k為t0,其中。對于同一柵格點(diǎn),無人機(jī)以不同角度φ′對應(yīng)的扇區(qū)k′到達(dá)該柵格點(diǎn),具有各自的發(fā)現(xiàn)時刻trap_numi,j,k和trap_numi,j,k′。

（3）設(shè)置陷阱檢測時間差ttrap,當(dāng)t1時刻無人機(jī)再次到達(dá)柵格點(diǎn)(i,j) 時對應(yīng)的trap_numi,j,k> =0,當(dāng)t1-trap_numi,j,k>ttrap時判斷無人機(jī)已陷入陷阱,此時更新trap_numi,j,k為t1。若trap_numi,j,k>= 0 且t1-trap_numi,j,k≤ttrap,則說明有可能是由于無人機(jī)速度較慢導(dǎo)致在同一柵格內(nèi)停留較長時間,此時不需要更新trap_numi,j,k,以避免無人機(jī)在較小范圍內(nèi)震蕩運(yùn)動時無法檢測出是否陷入陷阱。

針對任意時刻獲取到的障礙物信息,將其歸入記憶棧中,以用于基于部分歷史信息的局部避障。其示意圖如圖7 所示,記憶棧擁有先進(jìn)后出的特性,棧中每一個元素以時間為單位,表示該時刻t新發(fā)現(xiàn)的障礙物柵格集合,即集合中的每一項都代表t時刻新發(fā)現(xiàn)的存在障礙的柵格(i,j),其對應(yīng)置信度隨著傳感器掃描過程不斷更新并用于后續(xù)VFH極坐標(biāo)直方圖的構(gòu)建。對于圖7的場景,t=1 時刻無人機(jī)第一次探測到(3, 2), (3, 3), (3, 4), (3, 5),將其作為一個集合壓入記憶棧中；t= 2 時刻忽略已探測的(3, 5),將新探測到的(3, 6), (3, 7), (3, 8)壓入記憶棧中。t=3 時 則 將(3, 9), (3, 10), (3, 11), (4, 11), (5, 11),(5,1 2) 壓 棧。同時,對于壓入棧中的集合賦予對應(yīng)的下標(biāo)index,該索引值從0開始依次遞增,以表示障礙柵格集合壓棧順序。隨著無人機(jī)運(yùn)動,逐漸構(gòu)建起保存所有歷史障礙信息的記憶棧結(jié)構(gòu)。

圖7 記憶棧示意圖Fig.7 Memory stack schematic diagram

設(shè)置索引值mt,即無人機(jī)選擇從下標(biāo)index為mt直到棧頂?shù)恼系K柵格集合,距離現(xiàn)在最近時刻發(fā)現(xiàn)的柵格點(diǎn)將會被優(yōu)先取出。將集合中柵格(i,j)對應(yīng)的置信度cij用于VFH算法的第二層數(shù)據(jù)縮減過程,與活動窗口中的柵格點(diǎn)共同構(gòu)建極坐標(biāo)直方圖H。區(qū)別于第2節(jié),本節(jié)在引入歷史障礙信息后,定義mij即柵格的障礙物確定度為mij=(a'-b'dij),其中,b'=b,其中,dmax是無人機(jī)當(dāng)前位置到活動窗口中最遠(yuǎn)柵格的距離,d′max是無人機(jī)當(dāng)前位置到記憶棧選擇出的最遠(yuǎn)柵格距離,通過以上方式可以避免柵格位于活動窗口之外導(dǎo)致的mij< 0。

歷史信息的選擇閾值mt并非一個定值,其隨運(yùn)動過程中是否陷入陷阱而動態(tài)變換,以適應(yīng)不同場景中的選取策略:當(dāng)未陷入陷阱時,mt增大以避免使用冗余的歷史信息；當(dāng)頻繁陷入陷阱時,mt減小以加強(qiáng)VFH 的記憶性幫助脫困,動態(tài)閾值mt的更新策略為:當(dāng)陷阱檢測機(jī)制判斷無人機(jī)陷入陷阱時,mt=mt-gt,其中g(shù)t設(shè)計為大于1 的正整數(shù),以避免無人機(jī)脫困后mt難以追上記憶棧棧頂?shù)那闆r；當(dāng)無人機(jī)踏足新柵格點(diǎn),即柵格點(diǎn)(i,j) 的n個扇區(qū)都有trap_numi,j,k=-1,其中k= 0, 1,…,n-1時,mt=mt+gt。

根據(jù)本節(jié)提出的記憶信息選取策略,無人機(jī)在正常運(yùn)動或陷入陷阱時的局部規(guī)劃策略是相同的,歷史信息選取數(shù)量會隨著無人機(jī)狀態(tài)的改變而發(fā)生動態(tài)變化,以適應(yīng)不同場景下的VFH記憶性需求。

3.2 基于動態(tài)閾值的VFH算法

VFH 算法中如何選擇合適閾值是一個重要問題,無人機(jī)需要在避障與盡可能到達(dá)目標(biāo)這兩者之間均衡。在原VFH算法中,閾值T大多數(shù)時候是基于經(jīng)驗(yàn)的,這種方式雖然較為簡便,但是不利于未知復(fù)雜環(huán)境的有效避障。本文設(shè)計基于動態(tài)閾值的VFH算法,根據(jù)周圍環(huán)境情況自適應(yīng)更新閾值T,以幫助無人機(jī)更好地在不同場景條件下進(jìn)行局部規(guī)劃。

如圖8所示,希望在復(fù)雜場景中提高T以獲得更多可選方向,避免傳統(tǒng)VFH算法由于固定閾值可能出現(xiàn)的無解情況,并在脫離復(fù)雜環(huán)境后減小T以增強(qiáng)避障能力。

閾值T的自適應(yīng)更新可能會增加部分非安全的波谷對應(yīng)的無人機(jī)備選方向,此時需要考慮無人機(jī)運(yùn)動過程中的安全性問題,即無人機(jī)應(yīng)該與障礙物邊界保持一定的安全距離ds。因此,對傳統(tǒng)VFH中的部分窄波谷進(jìn)行修正,對第m個波谷的起始和終止角度邊界值αm_begin和αm_end,定義修正量am_begin及am_end。

圖8 動態(tài)閾值示意圖Fig.8 Dynamic threshold schematic diagram

式中,dm_begin是波谷起始邊界障礙物與無人機(jī)間距,dm_end是波谷終止邊界障礙物與無人機(jī)間距。將起始和終止角度邊界 值αm_begin和αm_end修 正 為α'm_begin=αm_begin+am_begin和α'm_end=αm_end-am_end。修正后的第m個波谷的跨越扇區(qū)角度為(α'm_begin,α'm_end)。當(dāng)α'm_begin≤α'm_end時,波谷m滿足無人機(jī)安全距離,可產(chǎn)生備選方向；否則跨越扇區(qū)范圍過窄,需要忽略掉該扇區(qū)以首先保證無人機(jī)安全性。

如圖9所示,設(shè)置最大閾值Tmax為極坐標(biāo)直方圖H中的最大值,即動態(tài)閾值T不能大于Tmax,避免對障礙地形的過度忽略。閾值T動態(tài)更新策略為首先計算各個波谷中滿足安全邊界距離的POD最大值h'i_max,從各個波谷的h'i_max中選擇最小值h'min,根據(jù)該最小值h'min和最大閾值Tmax計算動態(tài)閾值T,具體如下:

圖9 閾值選擇示意圖Fig.9 Threshold selection schematic diagram

（1）由極坐標(biāo)直方圖H中n個波谷扇區(qū)的POD 極小值h′k,k= 0, 1, …,n- 1,構(gòu)成集合M,其中第i個POD極小值h′i對應(yīng)角度屬于第i個極小值扇區(qū),h′i∈M。

（2）針對第i個極小值扇區(qū),其跨越角度范圍為(βi_begin,βi_end)且滿足βi_end-βi_begin=ar。根據(jù)無人機(jī)障礙邊界安全距離ds,計算到極小值扇區(qū)中心的角度修正量,從而得到安全波谷范圍為(pi,qi)

式中,βpi_begin為扇區(qū)pi的角度左邊界,βqi_end為扇區(qū)qi的角度右邊界；dpi為扇區(qū)pi中的障礙物與無人機(jī)最近距離,dqi為扇區(qū)qi中的障礙物與無人機(jī)最近距離。

（3）根據(jù)獲得的包含扇區(qū)i的安全波谷范圍(pi,qi),通過極坐標(biāo)直方圖H,選擇該安全波谷范圍中各角度對應(yīng)POD值的最大值h'i_max,其中pi≤i_max ≤qi。

（4）從多個波谷得到多個h'i_max,選擇其中最小值h′min=min(h'i_max),i= 0, 1, …,n-1為基準(zhǔn)計算閾值T

式中,λt為動態(tài)閾值增益系數(shù),是滿足0 <λt< 1的常數(shù)。

針對圖8 場景中固定閾值可能導(dǎo)致的無解問題,通過上述動態(tài)閾值策略基于Airsim仿真驗(yàn)證方法的有效性。設(shè)置40 × 40m的二維平面區(qū)域,無人機(jī)起始坐標(biāo)為(0,0)m,目標(biāo)坐標(biāo)為(12,12)m,約束無人機(jī)最大速度為vmax=2m/s。設(shè)置激光雷達(dá)最大探測半徑為dr= 12m、探測精度為ar=5°。對于VFH 算法中的參數(shù),設(shè)置柵格大小為0.5m × 0.5m,活動窗口大小為25m × 25m,障礙物確定度常數(shù)a=12,b由活動窗口大小和a的值確定,實(shí)時陷阱檢測精度αtrap=,記憶信息增量gt=2,安全距離ds=1.5m,動態(tài)閾值增益系數(shù)λt=0.5,無人機(jī)控制律增益系數(shù)η= 0.5。在距離無人機(jī)初始位置較近區(qū)域存在較密集障礙物,且對目標(biāo)造成遮擋。仿真步長設(shè)置為0.1s,仿真結(jié)果如圖10所示。

圖10 無人機(jī)運(yùn)動過程Fig.10 UAV motion process

由仿真結(jié)果可知,由于無人機(jī)的探測半徑較大且障礙物對無人機(jī)呈包圍分布,因此傳統(tǒng)VFH算法的固定閾值策略使所有扇區(qū)均處于波峰,沒有無人機(jī)的可選運(yùn)動方向。根據(jù)本節(jié)提出的策略,通過極坐標(biāo)直方圖中具體數(shù)據(jù)信息動態(tài)更新閾值,從而擴(kuò)展波谷扇區(qū)范圍,增加無人機(jī)備選方向,幫助無人機(jī)脫困。改進(jìn)的VFH算法流程如圖11所示。

3.3 無人機(jī)整體避障策略

本文采用A*算法作為全局規(guī)劃部分,且A*中的柵格與靜態(tài)已知障礙物的柵格劃分規(guī)則保持一致（此處用于全局規(guī)劃的柵格矩陣M與局部規(guī)劃部分的障礙物置信度矩陣C的柵格精度并非一致）。經(jīng)過A*算法,無人機(jī)得到一條避過靜態(tài)已知障礙物的具體路徑,且該路徑以柵格點(diǎn)集的形式表示。無人機(jī)跟隨該路徑開始運(yùn)動,按照順序設(shè)置全局規(guī)劃結(jié)果點(diǎn)集中的柵格點(diǎn)為臨時目標(biāo)點(diǎn),并通過激光雷達(dá)傳感器進(jìn)行實(shí)時地形探測。當(dāng)感知到周邊地形覆蓋了臨時目標(biāo)點(diǎn)時,使用上文中基于改進(jìn)VFH 算法的局部規(guī)劃策略,并更新臨時目標(biāo)點(diǎn)為全局規(guī)劃結(jié)果點(diǎn)集中的下一個柵格點(diǎn),重復(fù)循環(huán)以上過程直到到達(dá)臨時目標(biāo)后從局部規(guī)劃策略切換回全局規(guī)劃策略。單無人機(jī)整體避障策略流程如圖12所示。

4 仿真驗(yàn)證

圖11 改進(jìn)VFH算法流程Fig.11 Improved VFH flow chart

圖12 無人機(jī)整體避障策略流程Fig.12 UAV overall obstacle avoidance strategy flow chart

本節(jié)對上文提出的未知復(fù)雜環(huán)境下單無人機(jī)避障策略進(jìn)行仿真驗(yàn)證。在全局規(guī)劃部分采用A*算法,在局部規(guī)劃策略部分,分別采用基于傳統(tǒng)VFH算法及本文提出的基于陷阱檢測機(jī)制與動態(tài)閾值的VFH算法進(jìn)行了仿真,并對比兩種局部規(guī)劃策略下單無人機(jī)避障效果,以驗(yàn)證本文提出策略的有效性。

首先基于Airsim 仿真平臺進(jìn)行環(huán)境搭建,如圖13 所示,構(gòu)建40m×40m的矩形區(qū)域,無人機(jī)起始坐標(biāo)為(2,7)m,目標(biāo)坐標(biāo)為(38, 32)m,約束無人機(jī)最大速度為vmax=1.5m/s,地圖柵格精度dg=0.5m,激光雷達(dá)最大探測范圍dr= 4m、水平探測精度ar=5°。

圖13 基于Airsim的仿真場景搭建Fig.13 Simulation scenes based on Airsim

以柵格點(diǎn)集的形式設(shè)置靜態(tài)已知障礙物,圖14中的黑色矩形即為無人機(jī)起飛前的已知地形。根據(jù)基于A*算法的全局規(guī)劃策略進(jìn)行路徑規(guī)劃,圖14中的藍(lán)色點(diǎn)集即為規(guī)劃結(jié)果。無人機(jī)需要以全局規(guī)劃結(jié)果作為飛行路徑,并通過激光雷達(dá)傳感器實(shí)時感知地形信息判斷是否需要切換到局部規(guī)劃策略。

圖14 全局路徑規(guī)劃結(jié)果Fig.14 Global path planning

（1） 基于傳統(tǒng)VFH算法的局部規(guī)劃策略

針對局部規(guī)劃下傳統(tǒng)VFH算法對無人機(jī)的運(yùn)動控制,對激光雷達(dá)點(diǎn)云進(jìn)行三層數(shù)據(jù)減縮處理,設(shè)置障礙物置信度柵格精度為0.25m × 0.25m,活動窗口大小為10m × 10m,障礙物確定度常數(shù)a= 4,b由活動窗口大小和a的值確定,無人機(jī)控制律增益系數(shù)η= 0.5。無人機(jī)運(yùn)動過程如圖15所示。

無人機(jī)在t= 15s 左右處于局部路徑規(guī)劃策略,無人機(jī)與目標(biāo)間有連續(xù)密集障礙物,由于傳統(tǒng)VFH算法的無記憶性與貪心的規(guī)劃策略導(dǎo)致無人機(jī)會在一定空間區(qū)域內(nèi)來回震蕩運(yùn)動,即陷入了陷阱。這也是許多局部路徑規(guī)劃算法存在的問題,即短視的規(guī)劃策略導(dǎo)致僅僅能夠避過附近障礙,在復(fù)雜場景下不具有有效性。

圖15 基于傳統(tǒng)VFH算法的無人機(jī)運(yùn)動過程Fig.15 UAV motion process based on traditional VFH

（2） 基于改進(jìn)VFH算法的局部規(guī)劃策略

針對局部規(guī)劃下改進(jìn)的VFH 算法對無人機(jī)的運(yùn)動控制,在傳統(tǒng)VFH 的基礎(chǔ)上引入陷阱檢測與動態(tài)閾值機(jī)制,對其中涉及參數(shù)設(shè)置實(shí)時陷阱檢測精度αtrap=,記憶信息增量gt= 3,安全距離ds= 1.5m,動態(tài)閾值增益系數(shù)λt= 0.7,其他傳統(tǒng)VFH 算法中涉及參數(shù)與改進(jìn)前VFH 算法仿真參數(shù)保持一致。無人機(jī)運(yùn)動過程如圖16 所示。無人機(jī)運(yùn)動狀態(tài)、t= 15s 時,無人機(jī)同樣處于局部規(guī)劃策略,但是實(shí)時陷阱監(jiān)測機(jī)制根據(jù)無人機(jī)的重復(fù)震蕩運(yùn)動判斷出無人機(jī)已陷入陷阱,動態(tài)調(diào)整VFH 算法的記憶性,通過記憶棧擴(kuò)展極坐標(biāo)直方圖中的扇區(qū)障礙物信息以幫助無人機(jī)脫困。與此同時,動態(tài)閾值策略保證在復(fù)雜或稀疏的不同場景中都能有合理的波谷篩選閾值,讓無人機(jī)有更多的備選方向。

對比改進(jìn)前后VFH算法的仿真結(jié)果,本文提出的基于陷阱檢測機(jī)制與動態(tài)閾值的VFH 算法能夠解決傳統(tǒng)局部規(guī)劃算法中的易陷入局部陷阱的問題,更加符合未知環(huán)境下無人機(jī)路徑規(guī)劃的要求。

圖16 基于改進(jìn)VFH算法的無人機(jī)運(yùn)動過程Fig.16 UAV motion process based on improved VFH

5 結(jié)論

本文針對未知復(fù)雜環(huán)境下的無人機(jī)路徑規(guī)劃問題,提出了一種全局規(guī)劃基于A*算法與局部規(guī)劃基于改進(jìn)VFH算法的避障策略。首先進(jìn)行無人機(jī)避障環(huán)境建模,并對傳統(tǒng)VFH 算法進(jìn)行詳細(xì)介紹并闡述其局限性。針對VFH 算法的無記憶性問題,設(shè)計了一種實(shí)時陷阱檢測機(jī)制,根據(jù)無人機(jī)運(yùn)動狀態(tài)動態(tài)選擇歷史柵格信息以避免陷入局部陷阱；針對VFH 算法的閾值敏感性問題,設(shè)計了一種動態(tài)閾值更新策略,增強(qiáng)算法在不同場景中的有效性。最后通過Airsim對比仿真驗(yàn)證了本文提出算法的有效性。