未知環境中無人機自適應邊界快速檢測算法

唐嘉寧,謝翠娟,趙一帆,李孟霜,彭志祥

(1.云南民族大學 電氣信息工程學院, 昆明 650000; 2.云南民族大學 無人自主系統研究院, 昆明 650000)

0 引言

由于無人機具有獨特的優勢,近年來被廣泛應用到救援[1]、測繪[2]、軍事[3]等領域。然而,大多數環境在此之前都是部分未知的或完全未知的。自主探索允許機器人在沒有任何先驗信息的情況下探索和繪制環境,這是全自動機器人的必要能力。

自主探索指執行以前部分未知或完全未知環境建圖任務,當所有可訪問的空間都被分類為占用或空閑時,探索任務完成。自主探索一般分為3個流程：邊界感知檢測、路徑規劃、即時定位與建圖(simultaneous localization and mapping,SLAM),3個模塊迭代工作,相輔相成,完成整個環境的探索與建圖。邊界感知檢測作為探索的前端,形成邊界點以及選出合適的邊界點對探索過程來講尤為重要。邊界感知檢測又通常分為兩步：邊界獲取與最佳邊界點選擇。國內外學者提出了多種方法,大致將邊界檢測分為3類：基于邊界的探索、基于采樣的探索、基于采樣與邊界相結合的探索。基于邊界的方法主要是通過檢測地圖中已知區域和未知區域的邊界(frontier point,FP),并將FP作為候選目標,利用效用函數對FP進行綜合評價,選擇最佳FP作為探索目標。Yamauchi等[4]首次提出基于邊界探索的重要探索工作,并將此項工作擴展到多機器人[5]。為了有效檢測邊界,波前邊界檢測[6]將搜索空間從整個地圖減少到局部邊界的搜索空間。也有研究致力于減少邊界的搜索空間[7-8]。為提高邊界處理速度,Batinovic等[9]利用八叉樹地圖的結構特點,用不同的分辨率表示邊界點。Zhou等[10]將全局邊界聚類成多個簇,降低計算量,該方法雖然能快速檢測到FP,但大量的FP聚類的計算代價無疑是昂貴的。相比之下,Bircher等[11]提出基于采樣(next-bext-view-point,NBVP)的方法是在已知空間中采樣,并基于一些路徑規劃算法移動到探索增益最高的姿態。Umari等[12]利用該方法生成全局樹和局部樹來提高邊界檢測的效率。Witting等[13]通過維護歷史節點提高采樣效率。Dharmadhikari等[14]結合無人機飛行動力學和續航能力,提出使用運動原語識別配置空間的路徑(GBPlanner)。有學者使用更新快速探索隨機樹(rapidly-exploring random trees,RRT)節點[15]、前沿濾波[16]、陰影投射算法[17]、動態更新與重定位結合[18]的方法提高探索效率。為了適應地下隧道環境,Dang等[19]提出識別地下環境的關鍵拓撲特征,提出基于分岔的局部和全局規劃結構,實現了周期滑動窗口的快速邊界檢測,限制了從所有探測區域的自由空間到激光雷達傳感范圍內的機器人周圍局部采樣空間,并將該策略擴展到大型洞穴探索[20]和腿式機器人與空中機器人協同的工作中[21]。該方法可以有效地利用采樣為無人機提供探索目標,并能減小邊界聚類的成本代價,擴展到三維環境較容易。結合八叉樹體素結構[22-23],將基于邊界和基于采樣的前沿點方法結合,提高探索效率。

信息增益被定義為機器人變換一個姿勢將獲得多少關于環境的新信息,針對不同的目標,提出了不同的信息增益公式,例如快速探索或重建,在最初的前沿探索中,僅考慮距離無人機當前位置最近的邊界。NBVP 利用累計未知體素數量選擇最佳邊界點,盡管取得了進展,但大多數此類工作的一個限制因素與優化的單一目標有關。而且由于環境的復雜性、傳感器傳回的數據質量、缺乏未探索區域的實體等相關信息,導致整體探索的效率低下,并且增益函數的呈現形式也會變得困難棘手。基于上述問題,將動力學約束[19]和續航能力考慮到增益函數中,對探索的實際應用尤為重要。

本文中提出一種的邊界檢測器(adaptive detector planner,ADPlanner)。解決在未知環境下,怎樣快速完成未知環境的探索,并形成無人機(unmanned aerial vehicle,UAV)可識別的地圖。同時,在探索中必須考慮有限續航能力,以及無人機動力學約束的軌跡和姿態估計規劃。邊界感知檢測是UAV自主探索未知環境的關鍵模塊。邊界是靠近未知區域的已知區域的集合。因此,ADPlanner利用部署在空中機器人上的激光雷達感知環境和自適應邊界框,從而在局部空間采樣,通過重要性采樣限制重復區域的重采樣率,提升采樣速率。同時,根據UAV在移動過程中實時建立地圖來檢測邊界,并將評估的最佳邊界點發送給路徑規劃模塊,完成大規模復雜地形環境下的高效和穩健的自主探索。

1 問題陳述

本文主要目標是生成自適應采樣邊界框的邊界采樣器,解決環境探索運動規劃問題。ADPlanner系統流程如圖1所示。

定義2(同心松弛)：令Ξ表示無碰撞的連通集合,Nnode?Ξ 表示RRT樹擴展的節點,κ表示不同節點對邊界點選擇的影響系數。距離越遠,影響系數越小。

問題1(邊界檢測與探索問題)：給定一個有界未探索空間V, ① 基于激光雷達等傳感器得到的占用體素和空閑體素,② 執行時使用{Si}執行完整的環境覆蓋,直到不存在任何無碰撞的配置V{Vfree,Vocc}或者Vocc外部可觀測表面Socc的任何子集,即Vfree∪Vocc=VVres和Sfree∪Socc=VSres。

2 自適應邊界檢測

想要實現UAV在大型復雜環境下快速探索的任務,得到安全有效的采樣點尤為重要。地下環境復雜多樣,隧道寬度有寬有窄,在GBPlanner[19]中采用固定采樣框采樣并不能很好地適應地下隧道變化起伏的地形,很難確定一個采樣框尺寸去適應地下環境,采樣框的尺寸太大,在該框中采樣點多數為無效點(不能與RRT樹連接成邊的點均為無效點),采樣框的尺寸太小,采樣點數量過少,無法形成邊界點,直接造成無人機探索任務的失敗。因此,在本節中詳細介紹自適應采樣的邊界檢測器(oriented bounding box,OBB)[24],所提方法的重點是系統迭代執行探索任務并同時提供高效的建圖能力,該過程需要實時決定最佳信息采樣行為和相關路徑。

2.1 自適應滑動窗口

在得到激光雷達掃描的點云數據local_cloud后,與地圖坐標對齊,遍歷激光雷達局部點云數據點,利用主成分分析法(principal components analysis,PCA)對數據進行降維分析,得到點云主數據的3個主方向。以無人機當前位置為質心,計算協方差矩陣,求取協方差矩陣的特征值和特征向量,得到采樣框的主軸。滑動窗口自適應采樣如圖2所示,具體算法如下。

算法：自適應采樣邊界框生成

輸入：激光雷達掃描局部點云數據local_cloud

無人機的位姿pose

邊界框的最大尺寸boundboxsize_max

輸出：自適應采樣框的邊界值(min_val,max_val)

1.Pi←Local point cloud(pose,local_cloud)

2.Pfilter←Filter(Pi)

3.Fv←get Eigen Vectors(Pfilter)

4.Variance←get Eigen Values(Pfilter)

5.Pcentroid←compute 3D Centroid(Pfilter)

6.δoffset←compute Offset(Pcentroid,pose)

7.δdeviation←cwiseSqrt(Variance)

8.(min_val,max_val)←

compute(δoffset,δdeviation,boundboxsize_max)

9. return (min_val,max_val)

在算法的第1、2行,無人機得到局部(當前時刻激光雷達的掃射范圍)激光雷達輸入點云數據Pi后,先對點云數據進行過濾,減少點云不均勻密度造成的誤差,去除由于視角不同看到的非常接近的點。過濾后得到一系列局部點云的坐標Pfilter=[xi,yi,zi]T。利用協方差矩陣Cov(x,y,z)表示不同坐標之間的相關性：

(1)

E=[E(x),E(y),E(z)]T

(2)

式中：E(x)、E(y)、E(z)是點云坐標x、y、z軸的方差。為了進一步得到主軸,降低3個主軸方向的數據投影到主軸的相關性,將協方差矩陣對角化。利用算法第3—5行對Cov(x,y,z)進行特征向量Fv和特征值Variance的計算,將協方差用特征值組成的對角陣Λ和特征向量組成的矩陣P表示：

Cov(x,y,z)=PΛP-1

(3)

特征向量方向就代表3個主軸的方向,計算出無人機當前自適應采樣框的質心Pcentroid：

(4)

利用計算出來的質心Pcentroid和無人機的位姿得到在質心處的偏移量δoffset和標準差δdeviation。由此得到對地下隧道自適應的邊界采樣框。

2.2 重要性采樣

在地下隧道窄道飛行時,采樣框的寬度一般大于窄道的寬度,如果此時采樣是均勻的,無人機在飛行過程中容易出現重采樣,在探索的多次迭代中,累計重采樣會降低無人機整體探索效率,增加飛行時間。為了解決這個問題,利用重要性采樣提高前后時刻采樣邊界框非重疊區域的采樣率,盡可能引導無人機朝著未知區域探索,提高探索速率。

重要性采樣如圖3所示,為了限制在前后時刻采樣邊界框的重疊區域內重復采樣率,實現增量前沿檢測,根據上一時刻的采樣窗口8個角的坐標和下一時刻采樣窗口8個角的坐標,分別得到重復采樣區域體積Sol、上一時刻采樣框體積Ssam1、下一時刻采樣框體積Ssam2,進而得到重疊區域的采樣點數nol。設ntotal1為上一時刻采樣框內的總采樣點數,ntotal2為下一時刻采樣框內的總采樣點數,令前后時刻采樣率η相同,從而降低重疊區域的重采樣率ηre。

圖3 重要性采樣

(5)

一般來說,Ssam1和Ssam2是不一樣的,因此ntotal1和ntotal2也是不一樣的,為了使重復區域的采樣率與前后時刻采樣框的采樣率相同,只有減少重復區域新增采樣點個數,增加新增區域采樣點數,降低重采樣,引導無人機朝新增區域飛行。

2.3 未知環境探索

自適應采樣框為未知環境提供了局部視野,是整個探索框架的前端與基礎。將整個未知環境地圖體素分為3種狀態：占用、空閑、未知。在探索過程中,未知體素逐漸被傳感器感知,未知體素逐漸被空閑體素和占用體素替代,若探索空間不存在位置體素,代表探索過程結束,即環境建圖完成。在這個過程中,并不是每次都能順利地將預想的未知環境探索完成,總是會出現因采樣點不合理造成無人機來回折返,甚至無法探索直接回到起點的情況,但可以利用增益調節系數解決這個問題。

在每次迭代中,路徑增益是路徑上途徑的每個節點增益之和,表示為沿路徑上覆蓋的體積之和：

(6)

式中：L(nk,nk-1)表示當前點到前沿邊界點的歐氏距離;λ表示正常數,衡量了無人機運動距離成本的重要性,λ→0時,邊界點選擇不受距離影響,λ→∞意味著運動代價太大,λ的值通過實驗確定。

在每次迭代中,目標是找到讓信息增益最大的路徑μbp：

(7)

為了避免在地下隧道等狹窄環境中無人機頻繁轉向,將路徑邊界點集合中的偏航角考慮到信息增益中：

(8)

式中：n為路徑上的路徑點總數,對于n=1的情況,使用UAV當前方向。

在局部規劃中考慮無人機當前運動方向和邊界點的距離。計算節點的累計增益為：

G=e-(γφk+κD)I(nk)

(9)

式中：D為前沿點到當前點累計根節點的距離,好處在于解決隔著障礙墻的近處點與不隔障礙墻的遠處點的選擇問題;γ和κ代表方向和距離的影響程度,γ>0,松弛因子κ的大小取決于所選節點在重疊區域內還是重疊區域外,待選節點處于重疊區域外,κ取大于0的常數,在重疊區域內時,κ與Dn成正比,Dn為待選節點到當前節點的歐氏距離。

2.4 復雜度分析

為了進一步理解所提算法的計算復雜度,對邊界框進行了詳細分析,總的來說,自適應邊界框的計算復雜度主要來源于3個方面,即①邊界點的選擇及自適應邊界框的形成,②體素增益的計算,③形成當前點到最佳邊界點的路徑。這3步最重要的就是前沿邊界點的形成。

Ttotal=Tsample+Tgain+Tpath

(10)

在GBPlanner的基礎上,采用八叉樹(Octomap)地圖作為地圖框架進行建圖,利用自適應滑動窗口進行樣本采樣,在自適應滑動窗口區域總采樣點數為Κ個點,成功采樣了N個點,假設在滑動窗口成功采樣的平均次數為p,嘗試的總次數為p*N,故采樣的時間復雜度為：

Tsample=Trand(p*K)+Tnearest(p*K)+

Tstear(p*K)+Tcheckobstacle(p*K)+Tnew(p*N)

(11)

式中：Tsample表示RRG樹生長的采樣總復雜度時間;Trand、Tnearest、Tstear、Tcheckobstacle、Tnew表示RRG單個樹枝生長過程;N為采樣成功的點,點的時間復雜度為O。Trand、Tstear、Tcheckobstacle需要執行p*K次,故計算復雜度分別為O(p*K)。Tnew只需要執行p*N次,其余K-N次采樣在執行Tcheckobstacle時,不能加入到RRG樹中而被丟棄,它的計算復雜度為O(p*N)。Tnearest、Tnew涉及K-D樹的搜索與添加,Tnew的計算復雜度為O(N*log2N),在D維K-D樹搜索和添加過程的復雜度為O(K1-1/D)和O(KlogK),當D=3,Tnearest的復雜度為O((p*K)2/3),Tnew的復雜度為O(NlogN)。此外,假設采樣點數的多少不影響激光雷達處理數據的時間,對于前沿邊界點增益的計算和路徑形成的復雜度分析,同GBPlanner的一樣。激光雷達處理數據的時間可忽略。因此,總體的采樣復雜度為：

O(p*K)+O((p*K)2/3)+O(p*K)+

O(p*K)+O(N*logN)≈3p*O(K)+

p2/3*O(K2/3)+O(N*logN)

(12)

根據上述分析,在GBPlanner采用固定尺寸的采樣框,每次采樣的點數N大于本文方法中的P值,越狹窄的隧道體現的越明顯。本文方法在探索地下隧道環境時大大提高了采樣時間,減少了迭代次數。環境越空曠,本文算法越等效于GBPlanner,對于計算復雜度,ADPlanner省去了重疊區域的采樣點計算。

3 實驗

3.1 系統框架

使用自主探索開發環境,無人機飛行參數與GBPlanner的默認設置一致,該算法的運行環境是基于2.5 GHz的i7-11700,運行系統為Ubuntu 20.04,ROS noetic。

為驗證本文算法的有效性,將算法應用于模擬地下不規則礦洞以及規則長廊中。UAV飛行參數如表1所示,將本文算法與GBPlanner進行對比,利用探索覆蓋率Cov、整個探索過程中無人機飛行的路徑長度L、每個滑動窗口的平均采樣時間Tsample及探索完未知環境全程算法迭代運行時間T4個方面評估該算法的有效性與快速性。

表1 無人機飛行參數

探索覆蓋率表示為：

Cov=Vocc/V

(13)

式中：Vocc表示占用體素個數;V表示待探索環境的總體積。

3.2 地下隧道校驗

為了驗證提出的ADPlanner的有效性和合理性,將該算法應用于地下隧道不規則環境中,探索效果如圖4所示。圖4(a)為待探索的環境,圖4(b)為探索完成時UAV的建圖效果,在圖4(c)和圖4(d)中,使用GBPlanner出現局部最小值,導致UAV重復探索,降低探索速度。分別用航跡長度L、采樣時間Tsample、算法覆蓋率、算法總迭代時間T衡量ADPlanner的快速性和魯棒性,結果如圖5所示,GBPlanner和ADPlanner都能將環境探索完成。ADPlanner出現重復探索的幾率小,且探索時間縮短。

圖4 地下隧道建圖

圖5 地下隧道環境探索指標曲線

如圖6所示,相較GBPlanner,在探索總體性能上,ADPlanner探索路徑總長度L縮短18.86%,采樣時間Tsample(自適應采樣框形成時間與采樣點形成時間之和)減少20.2%,完成環境探索任務的總時間T提升了38.38%,極大提高了無人機探索未知環境的效率。

3.3 長廊環境校驗

為了驗證提出的ADPlanner的有效性和合理性,將該算法應用于長廊環境中,探索效果如圖7所示,圖7(a)為待探索的環境,圖7(b)為探索完成時UAV的建圖效果,在圖7(c)—(f)中,使用GBPplanner探索出現探索變慢、重復探索等問題,從而降低探索速度。

在長廊環境中,如圖8(b)所示,GBPlanner和ADPlanner都能將環境探索完成,但從圖8(a)和圖8(c)看出,ADPlanner的航跡長度、采樣時間明顯小于GBPlanner,從而提高整體探索效率,縮短探索時間。

圖8 長廊環境探索指標曲線

在探索總體性能上,如圖9所示,ADPlanner探索路徑總長度L縮短11.24%,采樣時間Tsample(自適應采樣框形成時間與采樣點形成時間之和)減少38.33%,完成環境探索任務的總時間T減少27.38%。

圖9 長廊環境環境算法性能

4 結論

針對地下隧道和長廊等狹長多變的復雜未知環境,提出自適應采樣的快速邊界檢測方法。通過自適應滑動采樣窗口提高采樣的成功率,選擇性地在非重疊區域采樣,解決重疊區域的重采樣問題。利用距離和偏航角評價邊界點,在此基礎上,通過松弛系數引導無人機向未知環境探索。在2個模擬場景中驗證了自適應采樣的快速性與適應性。實驗結果表明,與GBPlanner相比,自適應邊界檢測的邊界采樣時間分別縮短了20.2%～38.33%,路徑長度縮短了11.24%～18.86%,算法迭代運行時間縮短了27.38%～38.38%。