唐嘉寧, 劉雨晴, 周思達(dá), 李丁奎

(云南民族大學(xué) 電氣信息工程學(xué)院, 云南 昆明 650504)

近年來,無人機的應(yīng)用領(lǐng)域越來越廣泛,特別是在軍事海事、運輸投送、應(yīng)急救援等方面。在這些任務(wù)中,理想結(jié)果是無人機能快速而又充分覆蓋給定的未知區(qū)域,但當(dāng)無人機處于室內(nèi)無全球定位系統(tǒng)(GPS)環(huán)境時,其定位導(dǎo)航困難,無法進行有效自主探索,故本文通過RGBD傳感器對無人機進行定位以完成探索任務(wù)。在過去的幾十年里,研究者們已經(jīng)提出了許多方法,包括基于邊界的方法和基于采樣的方法。

基于邊界的方法通過探索地圖已知區(qū)域和未知區(qū)域之間的邊界來完成探索未知環(huán)境的任務(wù)。最早是由Yamuchi[1]于1997年提出的,這是目前大多數(shù)探索算法的基礎(chǔ),邊界定義為開放空間和未開放空間之間邊界上的區(qū)域,機器人移動到邊界可以看到未開放的空間,并將新信息添加到地圖上,但該方法存在無人機探索時間長且效率低的問題。Zhou等[2]為提高探索效率,提出一個支持快速探索的分層框架FUEL,通過前沿信息結(jié)構(gòu)維護空間所需信息，可找到有效的全局覆蓋路徑。Gomez等[3]將基于前沿的概念與基于行為的策略相結(jié)合以構(gòu)建環(huán)境的拓?fù)浔硎?根據(jù)成本效用函數(shù)進行語義邊界分類和邊界選擇。Faria等[4]為解決同時執(zhí)行自主探索和路徑規(guī)劃,將基于邊界的探索與Lazy Theta*路徑規(guī)劃算法結(jié)合在八叉樹地圖上,有效減少了尋找探索前沿單元所需的迭代次數(shù)。

基于采樣的方法則隨機產(chǎn)生機器人狀態(tài)并搜索路徑。Umari等[5]提出用快速拓展隨機樹(RRT)檢測邊界點,其局部RRT進行了一步樹的重置工作,每次重置樹都會從一個新的初始點開始生長,對計算資源要求高。Qiao等[6]為減少內(nèi)存消耗以及在不利環(huán)境中探索,提出用分塊結(jié)構(gòu)判斷樹節(jié)點是否在前沿檢測中起決定性作用,從而刪除大量冗余的樹節(jié)點。Bircher等[7]采用了滾動水平的次最佳視圖(next-best-view)方案,在在線計算的隨機樹中找到最佳分支,每個規(guī)劃步驟只執(zhí)行此分支的第一條邊,但在視場中選擇次最佳可能導(dǎo)致次優(yōu)解,且重復(fù)擴展RRT導(dǎo)的前提下,分層規(guī)劃器分步規(guī)劃探索以導(dǎo)致計算量大。Dornhege等[8]為解決選擇次優(yōu)視圖的問題,通過引入空隙和邊界,將基于二維前沿的勘探方法擴展到三維環(huán)境。

如何選取自主探索的目標(biāo)點在無人系統(tǒng)自主探索環(huán)境中起到?jīng)Q策作用。Yamuchi利用廣度優(yōu)先搜索方法尋找離當(dāng)前位置最近的點,導(dǎo)致無人機會反復(fù)探索同一個區(qū)域,增加探索路徑長度。Ibrahim等[9]為提高基于邊界的方法的搜索速度,提出了一種基于遺傳算法的全局搜索目標(biāo)分配方法, 通過遺傳算法啟發(fā)式地生成可能訪問的所有邊界的路線,迭代幾次后選擇最短路線的第一個邊界點作為下一個目標(biāo)位置。Liang等[10]使用聚類算法過濾從八叉圖中提取的邊界,并使用基于信息增益的代價函數(shù)來選擇最優(yōu)邊界,可節(jié)約探索時間但是算法復(fù)雜度高。Batinovic等[11]通過改變八叉樹圖中點的分辨率過濾邊界點,使用均值漂移聚類減少邊界點數(shù)量并選擇要探索的最佳邊界點。Fang等[12]通過優(yōu)化RRT算法生成的隨機前沿點,提出隨機前沿點優(yōu)化算法,將該算法與前沿點評價函數(shù)相結(jié)合,選擇有高估值的前沿點作為目標(biāo)前沿點。

綜上所述,探索是由一個或一組機器人在合理的時間內(nèi)獲得盡可能多的關(guān)于周圍的信息,達(dá)到覆蓋未知環(huán)境的過程。然而,大多數(shù)方法的探索效率都不高,而且為了保證獲取盡可能高的信息增益以及提高在未知環(huán)境下無人機飛行的安全性,大多數(shù)方法都是在低速條件下進行,這在解決短時間內(nèi)獲取更多的信息方面具有局限性。受文獻(xiàn)[13]啟發(fā),在高速飛行時,選擇并保持一個方向,若存在未探索邊界再返回繼續(xù)搜尋,結(jié)果會比以廣度優(yōu)先搜索的方式來回搜索更快,故本文基于八叉樹建圖,針對邊界驅(qū)動方法探索效率低以及探索不充分的問題,提出復(fù)合邊界點方法,再從眾多邊界點中利用評估函數(shù)選擇信息增益最大與偏航角度最小的點作為邊界導(dǎo)引點,使無人機在快速飛行的同時能最大限度地降低地圖熵,實驗結(jié)果表明,本文方法具有一定的可行性。

1 構(gòu)建八叉樹地圖

目前使用較多的地圖主要是點云地圖,但點云地圖有難以用于導(dǎo)航、地圖占用空間大、不方便處理重疊地圖等缺陷。八叉樹[14]是描述三維空間坐標(biāo)場景中常用的一種數(shù)據(jù)結(jié)構(gòu),其優(yōu)點是可建模任意環(huán)境、隨時更新、可動態(tài)擴展地圖。經(jīng)過優(yōu)缺點比較,本文使用八叉樹生成的3D占用柵格地圖描述環(huán)境V,并用從RGBD相機收集的數(shù)據(jù)來提供點云以更新地圖。在八叉樹中,3D占用柵格地圖中每個體素由其質(zhì)心表示,先驗概率為x=0.5,體素的狀態(tài)分為空閑Vfree<0.5,占用Vocc>0.5,未知Vunk=0.5

V=Vfree∪Vocc∪Vunk

(1)

無人機飛行時,通常會把地圖信息轉(zhuǎn)換成占用概率以供無人機導(dǎo)航,本文將占用概率值小于0.5的體素視為空閑,將占用概率值等于0.5的體素視為未知,將占用概率值大于0.5的體素視為障礙。一個柵格的狀態(tài)數(shù)值越大表示它越可能是占據(jù)狀態(tài),越小表示它越可能是空閑狀態(tài),狀態(tài)值為初始值則表示它是未知狀態(tài),由這3種體素狀態(tài)定義了整個空間,使無人機能夠準(zhǔn)確感知環(huán)境信息。

2 基于局部環(huán)境類型的復(fù)合型邊界點提取方法

2.1 室內(nèi)局部環(huán)境分類與識別

為提高無人機面對復(fù)雜未知環(huán)境的瞬時反應(yīng)速度以及降低計算復(fù)雜性,本文針對室內(nèi)局部環(huán)境固有特點,把環(huán)境和無人機進行了相對位置模式分類,將當(dāng)前視角環(huán)境分為無障礙飛行模式、無人機朝左邊墻飛行模式,無人機朝右邊墻飛行模式、右邊沒有障礙物飛行模式、左邊沒有障礙物飛行模式、多障礙物飛行模式,如圖1所示,無人機根據(jù)識別的室內(nèi)局部環(huán)境,規(guī)劃下一個位置要達(dá)到的位置點,再發(fā)布命令控制無人機飛往該點。

圖1 室內(nèi)局部環(huán)境類型圖

如圖1所示,模式1為無障礙,在機器人的視野內(nèi)無障礙物時,無人機將朝扇形視野中心點飛行,飛行角度保持不變,設(shè)t為當(dāng)前時刻時間戳,t+1為無人機下一時刻時間戳,則此時

θt+1=θt

(2)

t∈0,1,2,…,n-1,n

當(dāng)無人機處于模式2和模式3時,無人機有一部分視野被連續(xù)障礙物擋住了,此時選擇將這一部分視野丟棄,以模式2為例,如圖2所示,此時無人機的視野有一部分被墻面擋住(虛線部分),為了防止無人機撞墻,選擇將這一份視野丟棄,找尋剩余的視野的中點(綠色長虛線扇形的中點)。

圖2 模式2中無人機下一步飛行示意圖

求此時無人機偏航角度如下:假定無人機定高飛行zA=zB,通過八叉樹中的光線投射方法(CastRay)訪問體素占用狀態(tài)獲取其正前向扇形視野與墻面交點A(xA,yA,zA),采用仿真系統(tǒng)無人機模塊獲取無人機當(dāng)前坐標(biāo)B(xB,yB,zB),無人機原始飛行方向與向量AB之間的夾角為φ,可求得其正切角

(3)

此時φ滿足

(4)

設(shè)無人機下一個飛行方向與當(dāng)前飛行方向偏航角為θ,無人機的扇形視角大小為φ,可求得θ

(5)

加入偏航角可以使無人機有效避免被墻面擋住視野。

當(dāng)無人機處于模式4,5時,表示無人機有部分視野被非連續(xù)障礙物擋住,當(dāng)無人機處于模式6時,表示無人機有部分視野被連續(xù)障礙物擋住。

圖3 右無障礙模式中無人機下一步飛行示意圖

以模式4為例,如圖3所示。由于此時無人機左邊有障礙物,為了避開障礙物安全飛行,選擇朝空閑視角更大的右邊飛行,設(shè)此時無人機右邊扇形視角邊緣點為O(xO,yO,zO),障礙物與扇形視角右側(cè)交點為Q(xQ,yQ,zQ),無人機自身坐標(biāo)為B(xB,yB,zB),則在向量BQ的基礎(chǔ)上,偏轉(zhuǎn)一定角度α,得到向量BP,再根據(jù)點P與點O坐標(biāo)可求得無人機下一步邊界導(dǎo)引點R

(10)

此處加入偏航角α是為了避免無人機與障礙物過于接近導(dǎo)致摩擦碰撞,主動避免可預(yù)見的風(fēng)險。

隨著無人機偏離障礙物一段距離,無人機的視野會逐漸增加到原始視野大小,該方法可以有效提高無人機對環(huán)境的能見度,實現(xiàn)快速飛行條件下的安全飛行。

2.2 復(fù)合邊界點

邊界點F定義為具有以下特性的點

F={vF∈Vfree:?neighbour(vF∈Vunk)}

(11)

針對傳統(tǒng)邊界點選取規(guī)則單一的問題,本文邊界點分為2類:局部邊界點和全局邊界點。

理論上的局部邊界點往往分布于無人機視野附近的局部區(qū)域,其優(yōu)點是靠近前沿點能獲得較大的信息增益,數(shù)量少,便于計算最優(yōu)候選點,缺點是在探索過程中可能出現(xiàn)候選點數(shù)量為零的情況。全局邊界點基于可通行空間所有的點,所以候選點的數(shù)量較大,分布在全局可通行區(qū)域內(nèi),全局邊界點幾乎覆蓋整個可通行區(qū)域,其優(yōu)點是能為機器人下一步探索的邊界導(dǎo)引點提供更多的選擇,相比局部型候選點,魯棒性更強,缺點是數(shù)量巨大,在評估最優(yōu)點時計算量很大,故相較局部型候選點,全局型候選點需要的計算時間更多。

本文定義的局部邊界點分布在無人機視野周邊,定義的全局邊界點是在視野以外且體素狀態(tài)介于Vunk和Vfree之間的點。由于單一的邊界點提取方法無法同時滿足自主探索中高效與全面的要求,本文在探索過程中擬結(jié)合2種提取方法對邊界點進行復(fù)合提取。方法流程如圖4所示。

圖4 復(fù)合邊界點選擇流程圖

無人機主動探索可以認(rèn)為是一種最小化地圖熵的行為,而信息增益表示某點對降低地圖不確定性貢獻(xiàn)的大小,即能夠獲取環(huán)境信息量的大小,定義地圖的香農(nóng)熵為

(12)

式中，i表示地圖的各個柵格單元,j表示柵格的占用情況,可以是占據(jù)、空閑、未知3種情況,未知柵格p(mi,j)=0.5,熵值越高,地圖不確定性越高,兩者呈正比例關(guān)系,使用互信息I(m,xi)評估對邊界點xi的預(yù)期信息增益

I(m,xi)=H(m)-H(m|xi)

(13)

式中：H(m)表示當(dāng)前地圖的熵；H(m|xi)表示在邊界點xi處預(yù)期的熵。

對于本文提出的復(fù)合邊界點提取方法,本文采用信息增益及旋轉(zhuǎn)角度非線性加權(quán)的效益函數(shù)對邊界點進行評估,評估函數(shù)表達(dá)式如下

(14)

式中：λ為加權(quán)系數(shù)；θq為無人機偏航角；F表示邊界點；I(m,xi)為邊界導(dǎo)引點處的信息增益估計值,使V(q)值最大的點q為邊界導(dǎo)引點。

圖5 無人機探索邊界導(dǎo)引點示意圖

如圖5所示,假定無人機當(dāng)前坐標(biāo)為(xcurr,ycurr,zcurr),且局部點已探索完畢,而右上角區(qū)域未被探索,設(shè)紅色點為邊界導(dǎo)引點,邊界導(dǎo)引點的坐標(biāo)為(xatt,yatt,zatt),可求得兩點在x方向的距離xL,y方向上的距離yL,從而求出從當(dāng)前坐標(biāo)點到邊界導(dǎo)引點的正切角

(15)

再根據(jù)反正切函數(shù)可得出偏航角度

(16)

在選定邊界導(dǎo)引點后,可確定一個吸引力勢場

Uatt=γd2(xcurr,xatt)]

(17)

式中：γ是引力勢場尺度因子；d(xcurr,xatt)為當(dāng)前位置與邊界導(dǎo)引點之間的距離,使全局點對無人機產(chǎn)生一個吸引力,再給定無人機每一步飛行步長,把這些信息發(fā)布給無人機,無人機即可飛往邊界導(dǎo)引點實現(xiàn)自主探索。

3 仿真實驗與結(jié)果分析

為了驗證所提出方法的可行性,本文在ubuntu16.04環(huán)境下的Gazebo7.0中使用RotorS[15]模型進行了仿真實驗,使用RViz來查看即時構(gòu)建地圖的過程和生成的地圖、探索路徑(綠色線),設(shè)計了4種常見的室內(nèi)局部環(huán)境,并將提出的方法與文獻(xiàn)7方法進行了比較。為了驗證方法的有效性,本文提出的方法在文獻(xiàn)[7]方法的公寓環(huán)境中進行了實驗,對探索時間,相同時間內(nèi)地圖覆蓋率進行了對比,如圖6～7所示。

圖6 公寓環(huán)境探索實驗結(jié)果

圖7 公寓環(huán)境類型下探索時間與覆蓋率關(guān)系

地圖覆蓋率:用八叉樹中的被占據(jù)的體素Vocc與總體素V之比表示探索覆蓋率

(18)

仿真參數(shù)如表1所示,因為文獻(xiàn)[7]中的模型較小,如果速度過快無人機會無法轉(zhuǎn)彎,出現(xiàn)這種情況是由于沒有足夠大的空間支持轉(zhuǎn)彎,導(dǎo)致無人機在墻角靜止不動或試圖轉(zhuǎn)彎導(dǎo)致撞墻。

表1 仿真參數(shù)表1

表2 文獻(xiàn)[7]中公寓環(huán)境類型下本文方法與

通過對公寓環(huán)境類型的探索平均時間進行加權(quán)計算可得,本文方法比文獻(xiàn)[7]方法探索時間降低約68.7%。

為了驗證本文方法在不同環(huán)境、不同參數(shù)下的有效性與普適性,本文模擬了4種環(huán)境類型來進行驗證,在所有的環(huán)境類型下,2種方法無人機放置的起始位置相同,仿真參數(shù)如表3所示。

表3 仿真參數(shù)表2

本文模擬的環(huán)境類型1是單房間,該地圖大小為15 m×15 m×3 m,本文方法和文獻(xiàn)[7]方法探索效果如圖8所示。

圖8 環(huán)境類型1探索實驗結(jié)果

圖9 單房間環(huán)境類型下探索時間與覆蓋率關(guān)系

在單房間環(huán)境類型下,本文方法最短探索用時38 s,文獻(xiàn)[7]方法最短探索用時2 161 s,這是由于文獻(xiàn)[7]方法需要計算RRT樹,不斷尋求最佳分支,而本文計算量較小,計算時間幾乎可以忽略。

圖10 環(huán)境類型2探索實驗結(jié)果

本文模擬的環(huán)境類型2是單房間加4個柱狀障礙物,該地圖大小為15 m×15 m×3 m,本文方法和文獻(xiàn)[7]方法探索效果圖如圖10所示。

圖11 單房間+障礙物環(huán)境類型下探索時間與覆蓋率關(guān)系

設(shè)計此環(huán)境類型的目的是驗證2種方法是否能避障,結(jié)果表明2種方法都可以較好地避開障礙物,由于加入障礙物,導(dǎo)致2種方法探索時間略有增加,本文方法最短探索用時54 s,文獻(xiàn)[7]方法最短探索用時2 435 s。

本文模擬的環(huán)境類型3是模仿教室設(shè)計,分別有前門和后門,由4個房間加上中間的通道組成,該地圖大小為32 m×20 m×3 m,4個房間大小均為11 m×10 m×3 m,中間通道為10 m×20 m×3 m,本文方法和文獻(xiàn)[7]方法探索效果如圖12所示。

圖12 環(huán)境類型3探索實驗結(jié)果

圖13 多房間+多通道環(huán)境類型下探索時間與覆蓋率關(guān)系

設(shè)計此環(huán)境類型的目的是驗證2種方法能否在多個房間中探索,單純在一個房間探索比較簡單,體現(xiàn)不出無人機的智能性。實驗結(jié)果表明,本文方法無人機可以探索得比較全面,而文獻(xiàn)[7]方法探索的效果不太理想,總是在同一個地方徘徊,探索覆蓋率較低且探索時間很長,其最短探索用時為3 035 s,本文方法最短探索用時僅148 s。

本文模擬的環(huán)境類型4設(shè)計成1扇墻連通2個房間,在墻中間有一處空隙,該地圖大小為20 m×20 m×3 m,中間空隙為3 m,空隙左右兩邊的墻為8.5 m,本文方法和文獻(xiàn)[7]方法探索效果如圖14所示。

圖14 環(huán)境類型4探索實驗結(jié)果

圖15 多房間+單通道環(huán)境類型下探索時間與覆蓋率關(guān)系

設(shè)計這個環(huán)境類型的目的是考證復(fù)合邊界點的有效性,通過驗證無人機能否有效從空隙穿過探索另一個房間,如果不能從空隙穿過,證明無人機只是被局部點引導(dǎo)飛行,反之,若能從空隙穿過,證明確實有全局點吸引無人機飛行。

實驗結(jié)果表明,本文方法可以有效探索整個空間,且重復(fù)探索區(qū)域較少,而文獻(xiàn)[7]方法無法探索完整個空間,總是在已探索的地方循環(huán)探索,且其最短探索用時2 040 s,本文方法最短探索用時僅107 s。

在這4種情況下,每種方法都以相同的初始配置運行10次,當(dāng)文獻(xiàn)[7]方法出現(xiàn)“planner not reachable”時認(rèn)為探索結(jié)束停止計時。4種方法的探索時間對比如表4所示,其中探索平均時間根據(jù)對10次探索時間進行加權(quán)平均得到。

表4 不同環(huán)境類型下本文方法與文獻(xiàn)[7]方法探索時間對比

從探索時間和覆蓋率的關(guān)系可以看出,采用本文的方法無論在小型環(huán)境或大型環(huán)境中都基本能夠探索全部地圖,而文獻(xiàn)[7]方法只能在小型環(huán)境中探索,一旦進入大型環(huán)境則會出現(xiàn)在同一區(qū)域重復(fù)徘徊的情況。此外,由探索效果圖可以看出,本文的方法走重復(fù)區(qū)域不多,探索環(huán)境的順序比較合理,雖然由于環(huán)境類型的復(fù)雜性,有時會出現(xiàn)重復(fù)探索,這是因為該地方有遺留的全局點待探索,但是總體而言,本文方法極大地減少了探索時間,對4種環(huán)境類型的探索平均時間分別進行加權(quán)計算可得,對于環(huán)境類型1,本文方法比文獻(xiàn)[7]方法探索時間約降低98.6%;對于環(huán)境類型2,本文方法比文獻(xiàn)[7]方法探索時間約降低97.9%;對于環(huán)境類型3,本文方法比文獻(xiàn)[7]方法探索時間約降低95.7%;對于環(huán)境類型4,本文方法比文獻(xiàn)[7]方法探索時間約降低96.3%,為了保證方法的有效性及合理性,對計算得到的4種環(huán)境類型的探索時間下降率,進行加權(quán)計算,得到本文方法比文獻(xiàn)[7]方法在4種環(huán)境類型下的平均探索時間約降低97.1%。文獻(xiàn)[7]方法存在同一區(qū)域重復(fù)探索的現(xiàn)象,故其探索時間大大增加,且由于文獻(xiàn)[7]方法需要在線計算RRT樹以找最佳分支,故其計算時間也較長,導(dǎo)致其探索總時間與本文方法相比高出不少。與文獻(xiàn)[7]方法相比,本文提出的方法在同樣的時間探索覆蓋率更高,探索時間更短,故本文提出的方法具有一定的優(yōu)勢。

4 結(jié) 論

本文結(jié)合了邊界驅(qū)動方法和復(fù)合邊界點提取方法對未知環(huán)境進行探索,在此基礎(chǔ)上用八叉樹建立環(huán)境地圖,使無人機在未知環(huán)境下具有自主探索和構(gòu)建室內(nèi)環(huán)境的能力,與文獻(xiàn)[7]方法相比探索同樣面積所需時間更少,探索范圍更全,路徑更為平滑,并遷移在不同參數(shù)、不同環(huán)境類型下進行了驗證,結(jié)果表明本文方法具有一定可行性與普適性。