李小毛,張鑫,王文濤,瞿棟,祝川

(上海大學機電工程與自動化學院,上海 200072)

基于3D激光雷達的無人水面艇海上目標檢測

李小毛,張鑫,王文濤,瞿棟,祝川

(上海大學機電工程與自動化學院,上海 200072)

無人水面艇(unmanned surface vehicle,USV)實現(xiàn)自主導航避障需要實時感知周圍的環(huán)境信息,檢測出威脅無人水面艇航行的障礙物,而3D激光雷達在無人系統(tǒng)障礙檢測中起重要作用.提出了一種基于3D激光雷達的障礙檢測算法,即將一個周期內的3D激光點云投影到2.5D柵格地圖中,對障礙物進行聚類分割；對柵格中的原始點云數(shù)據(jù)進行特征提取,表示為橢圓障礙物.而多幀激光數(shù)據(jù)采用最近鄰數(shù)據(jù)關聯(lián)識別出其中的動態(tài)障礙物,并用卡爾曼濾波實時跟蹤.基于電子海圖的仿真激光數(shù)據(jù)驗證了該方法在無人水面艇障礙檢測中的有效性.

無人水面艇;激光雷達;障礙檢測;柵格地圖;橢圓表示

無人水面艇(unmanned surface vehicle,USV)是一種智能化的小型水面平臺,可以代替有人水面艇在特定水域完成一些繁瑣的、復雜的甚至危險的任務.如果要在多變的、復雜的海況下安全自主地完成各項任務,則無人水面艇必須具備良好的環(huán)境感知能力,其中準確穩(wěn)定的障礙物檢測能力是無人水面艇實現(xiàn)自主導航避障的前提和關鍵技術.

無人水面艇障礙物檢測法按所用傳感器的不同可以分為被動檢測法和主動檢測法[1].被動檢測法是指利用單目視覺或立體視覺傳感器感知環(huán)境信息的方法,這種方法是文獻[2-5]中最常見的障礙物檢測方法,其主要特點是可以獲得豐富的顏色特征,通常是利用邊緣檢測算法提取出海天交界線,然后在海天交界線下方搜索感興趣的目標特征.但是,單目視覺檢測法往往難以獲得目標精確的深度信息,且圖像極易受光照變化的影響;而立體視覺檢測法數(shù)據(jù)量較大,較難實現(xiàn)實時處理的需求.主動檢測法是利用激光雷達、海事雷達等距離傳感器檢測無人水面艇周圍的障礙物.海事雷達主要用于遠距離的海上目標檢測與跟蹤,但是通常只能提供目標的位置和速度信息,無法識別出目標的形狀特征,且雷達在近距離范圍內存在一定盲區(qū).Almeida等[6]、Blaich等[7]和Hermann等[8]分別提出了基于雷達的障礙檢測方法,較好地實現(xiàn)了無人水面艇遠距離的目標檢測.

激光雷達根據(jù)其掃描激光束的數(shù)量分為2D激光雷達和3D激光雷達.由于測量精度高,抗干擾能力強,故激光雷達非常適用于無人水面艇中遠距離的障礙檢測.Peng等[9]提出了一種基于2D激光雷達的障礙檢測算法(障礙物表示為圓形),由于傳感器的限制,其檢測距離只有50 m,且在無人水面艇劇烈晃動時會出現(xiàn)丟失目標的情況.

使用3D激光雷達可以較好地解決上述問題.3D激光雷達檢測距離遠,水平、垂直視場大,為此被廣泛應用于無人水面艇、無人車等無人平臺的目標檢測和環(huán)境感知中.3D激光雷達的數(shù)據(jù)表示方法有點云表示法、基于特征的表示法和基于柵格的表示法[10],其中基于柵格的表示法將空間離散成較小的柵格單元(cell),每個柵格中存儲了相應位置的傳感器數(shù)據(jù).該方法計算效率高,易于實施且不依賴于預先定義的特征,故非常適用于對實時性要求較高的無人系統(tǒng).基于柵格的表示法按照柵格劃分的維度又可以分為2D柵格地圖、2.5D柵格地圖和3D體素柵格,其中2.5D柵格地圖是2D柵格地圖在第3維度上的擴展,將激光點云數(shù)據(jù)投影到柵格中后,只保留其最大最小高度值表示柵格的高度,這樣相當于對3D點云數(shù)據(jù)作了降維處理,這種柵格地圖可以較好地處理懸掛結構.

本工作提出了一種基于3D激光雷達和2.5D柵格地圖的障礙檢測與跟蹤方法,本方法不同于常規(guī)的柵格表示法,其主要表現(xiàn)為在障礙物的特征提取和動態(tài)障礙物判定時,不直接對柵格進行處理,而是提取出每個柵格中保存的原始激光點云,根據(jù)海上目標的形狀特點表示為橢圓障礙物,并對橢圓特征進行最近鄰關聯(lián)和卡爾曼濾波跟蹤.本方法在基于電子海圖的仿真界面中取得了較好的效果.

1 傳感器配置

本工作使用的激光雷達是Velodyne 16線激光雷達(見圖1).該激光雷達不同于同種類型的32線或64線激光雷達,是一款混合固態(tài)激光雷達(其旋轉部件在雷達內部),受外界環(huán)境干擾小,非常適用于惡劣的海上環(huán)境.

圖1 Velodyne 16線激光雷達Fig.1 Velodyne 16-layers Lidar

Velodyne 16線激光雷達是一個360°全方位激光雷達,其有效探測距離為100 m,垂直視場為30°,在垂直方向共有16條掃描線,每秒可以獲取30萬個激光點云數(shù)據(jù).除了能獲得目標的距離方位信息外,該雷達還可以獲得目標的反射強度信息,這些信息對于雜波的濾除和目標的分類具有重要作用.Velodyne 16線激光雷達的詳細技術參數(shù)如表1所示.

表1 Velodyne 16線激光雷達技術參數(shù)Table 1 Specifcation of Velodyne 16-layers Lidar

值得一提的是,為了獲得360°全方位的目標信息,最好將Velodyne 16線激光雷達安裝在無人水面艇的最高處,且在使用激光點云數(shù)據(jù)時首先將其從激光坐標系轉換到船體坐標系,具體的轉換關系根據(jù)激光雷達的實際安裝位置而定.

2 激光雷達數(shù)據(jù)預處理

2.1 坐標轉換和無效點云濾除

3D激光雷達的點云數(shù)據(jù)大多是以球坐標形式(ρ,α,ω)存儲的.為了方便起見,首先將點云轉換為3D笛卡爾坐標系形式(x,y,z).坐標轉換所用的幾何關系如下:

式中,ρ,α,ω分別為球坐標中的球徑、方位角和高度角.

平靜的水面幾乎不會產(chǎn)生激光回波[11],但激光點云中仍然有很多無效數(shù)據(jù),這些無效數(shù)據(jù)主要有無人水面艇本身反射的激光點、有效范圍外的點和環(huán)境中返回的雜波點,如海浪的激光回波等.對于第一類無效點云,可以指定一個激光點云的有效范圍,將該范圍外的點云濾除.如距離激光雷達較近的點很可能是無人水面艇本身的反射回波,而激光有效檢測范圍外的點往往不可靠,故可以事先濾除掉;對于第二類無效點云,其反射強度通常較低,根據(jù)這一條件也即可濾除.

2.2 點云畸變校準

無人水面艇的運動和激光雷達的掃描延時會導致產(chǎn)生的激光點云發(fā)生一定程度的畸變[12],利用慣導數(shù)據(jù)可以較好地解決激光點云的畸變問題.

假設激光雷達的旋轉頻率為f,則可以計算出激光雷達旋轉一定角度?所用的時間為

然后從慣導數(shù)據(jù)中得到當前無人水面艇的橫滾角α、俯仰角β和偏航角γ.為了校準每個激光點的位置,需要得到t時刻無人水面艇的平移矩陣和旋轉矩陣.由于激光掃描頻率很快,因此t

時刻無人水面艇的橫滾角α(t)、俯仰角β(t)和偏航角γ(t)可以通過線性插值估計出來:

從而可得到旋轉矩陣?R(t).進一步地,為獲得平移矩陣,假設無人水面艇沿全局坐標系的x軸方向運動,則x方向的基向量為

無人水面艇沿x方向的速度可以表示為

對速度進行積分,得到無人水面艇的航跡矢量x(t):

將初始條件代入式(6)可以求得平移矩陣T(t).最終根據(jù)點云的平移矩陣和旋轉矩陣求得修正后的每個激光點的位置,即式中,pi表示第i個激光點的位置,p′i表示校正后激光點的位置.

3 2.5D柵格表示法

3.1 柵格投影

2.5D柵格地圖又稱為高度圖[13],是無人車領域比較成熟的3D激光傳感器柵格表示法[14].本工作的柵格表示基于該方法進行了一些改進.

首先,根據(jù)激光雷達的檢測范圍建立2D柵格地圖,無人水面艇位于柵格地圖的原點(見圖2),其中R為激光的檢測范圍.柵格的尺寸選取不同于常用的柵格表示法,而是將無人水面艇可通行寬度w表示為柵格的邊長,這樣可以保證障礙物不會被過度分割.

接著,對一個周期內的激光點云進行遍歷,根據(jù)每個點的坐標將其投影到相對應的柵格中,柵格中的點云計數(shù)器加1.3D激光點存于柵格中的雙向鏈式數(shù)據(jù)結構中,該結構記錄所有點云的最小高度.

可以將柵格按其點云數(shù)據(jù)的屬性分為障礙柵格和非障礙柵格,判斷為障礙柵格的條件有:①柵格中的激光點數(shù)不小于2;②所有點云的最小高度小于無人水面艇的可通行高度.前者是為了濾除點云中的孤立點,后者則是為了避免懸掛結構(如橋梁等)對無人水面艇行駛的影響.

圖2 2D柵格地圖Fig.2 2D grid map

3.2 障礙柵格聚類

對于障礙柵格聚類即障礙物點云的聚類,本工作采用的是圖像處理領域的二值連通域區(qū)域標記算法,其本質是一種基于距離的聚類算法.

首先遍歷柵格地圖,找到第一個未標記的障礙柵格,將其標記為O1,接著在該障礙柵格的8鄰域內尋找其他的障礙柵格,同樣標記為O1,反復標記障礙柵格的鄰域,直到該連通域內所有障礙柵格都被成功標記為止,記錄該連通域內所有障礙柵格的位置.接著重復上述操作找到柵格地圖中其余連通域,并標記.最終得到若干個連通區(qū)域即柵格地圖中的若干個障礙物,這時可以將同一連通域中每個障礙柵格存儲的激光點云合并,得到同一障礙物的所有激光點云.

為了加快連通域標記算法的速度,本工作采用了文獻[15]中提出的一種快速二值圖像連通域區(qū)域標記算法,該算法綜合了線標記法和區(qū)域增長法的優(yōu)點,即只需對二值圖像掃描就可以依次標記出所有的連通區(qū)域,能大幅提高算法的計算效率.圖3為障礙柵格聚類的結果,其中黑色柵格為障礙柵格,白色柵格為非障礙柵格,柵格中的數(shù)字為障礙柵格所屬的連通域的標號,即柵格地圖中障礙物的序號.

圖3 障礙柵格聚類結果Fig.3 Clustering results of obstacle grid

4 橢圓特征提取

通常,無人車領域激光目標用一個封閉盒子[16]表示,考慮到海上的船只等障礙物的形狀類似于橢圓形,故本工作考慮將激光檢測到的障礙物用橢圓表示,并提取橢圓的特征參數(shù).

為了得到橢圓障礙物的方向,本工作采用了一種主成分分析(principal component analysis,PCA)法,其本質是一種降維思路,該方法可以將m個變量轉化為n個主成分(m≥n),這些主成分能夠最大程度地反映出原始變量的信息,且每個主成分都是原始變量的線性組合.對于2D平面上的激光點云來說,其幾何解釋就是計算這些點云的主要和次要特征方向向量,即橢圓的長軸和短軸方向向量.

計算激光點云主成分的步驟如下.

(1)從3.2節(jié)所描述的連通域中取出所有的激光點云,記Xi和Yi分別為第i個激光點的x,y坐標.

(2)計算Xi和Yi的協(xié)方差矩陣.

(3)計算協(xié)方差矩陣的特征向量和特征值.

計算協(xié)方差矩陣的特征值λ1,λ2和對應的特征向量ζ1,ζ2,其中較大的特征值對應的特征向量為橢圓長軸方向向量,較小的特征值對應的特征向量為橢圓短軸方向向量(見圖4).

圖4 點云數(shù)據(jù)主成分Fig.4 Principal components of point cloud

得到點云的主成分方向向量后,將所有數(shù)據(jù)在這兩個方向投影,得到邊界點,可確定橢圓長軸的長度和短軸的長度,即能確定橢圓障礙物的唯一位置和形狀.橢圓障礙物的特征參數(shù)主要有中心點位置、短半軸長度、長半軸長度和長軸方向等.對所有連通域內的激光點云進行上述相同的操作,便可以將柵格地圖上所有的障礙物表示成橢圓,并得到其特征參數(shù).

5 動態(tài)障礙物跟蹤

針對單幀激光點云數(shù)據(jù)的處理,只能得到靜態(tài)場景下某一時刻無人水面艇周圍的障礙物.為了滿足海上動態(tài)環(huán)境需求,需要關聯(lián)多幀激光數(shù)據(jù),提取出周圍的動態(tài)障礙物信息,并對這些動態(tài)障礙物的位置和速度進行預測和跟蹤.

考慮到海上目標較少,且分布比較分散,故本工作采用最近鄰數(shù)據(jù)關聯(lián)方法關聯(lián)多幀激光數(shù)據(jù)中的障礙物,并用經(jīng)典卡爾曼濾波預測動態(tài)目標的位置和速度.

最近鄰數(shù)據(jù)關聯(lián)算法是最簡單且易實現(xiàn)的數(shù)據(jù)關聯(lián)方法,其基本思想[17]是首先以量測值的預測位置為中心建立相關波門,如果落入相關波門內的量測值只有1個,則該值即為匹配值;如果有多個量測值落入相關波門內,此時則需要統(tǒng)計距離最小的量測值并將其作為匹配值:

式中,z為(k+1)時刻的量測值,︿z(k+1|k)為(k+1)時刻量測的預測值,S?1(k+1)為信息協(xié)方差.

利用最近鄰關聯(lián)和卡爾曼濾波跟蹤動態(tài)障礙物的步驟如下.建立一個動態(tài)障礙物列表,將k時刻的所有障礙物加入該列表,每個障礙物有一個存在置信度.利用最近鄰數(shù)據(jù)關聯(lián)方法關聯(lián)第(k+1)時刻的障礙物,關聯(lián)結果有3種情況[18]:①障礙物在動態(tài)障礙物列表中,但(k+1)時刻沒有找到匹配目標的障礙物,將其存在置信度減1;②在(k+1)時刻新出現(xiàn)而在動態(tài)障礙物列表中不存在的障礙物,將其加入到動態(tài)障礙物列表,存在置信度賦初值;③(k+1)時刻出現(xiàn)的障礙物與動態(tài)障礙物列表中的障礙物成功匹配,將其存在置信度加1,并利用卡爾曼濾波預測其位置和速度.另外,將速度小于1 m/s的障礙物視為靜態(tài)障礙物,移出動態(tài)障礙物列表;當動態(tài)障礙物列表中某一障礙物的存在置信度小于特定值時,判定該障礙物已經(jīng)消失,將其從動態(tài)障礙物列表中移除.

6 實驗結果

6.1 實驗設置

本算法的實驗驗證是在基于電子海圖的仿真程序中進行的.本仿真程序可以模擬海上場景,為無人水面艇規(guī)劃路徑,并在無人水面艇周圍設置若干個模擬障礙物,其中障礙物以球形表示.

圖5 仿真程序中障礙物的設置Fig.5 Setup of obstacles in simulation program

柵格地圖的范圍設置為X(?100,100),Y(?100,100),其中無人水面艇的位置設置在柵格地圖的中心點.柵格大小設置為5 m×5 m,激光數(shù)據(jù)的采集頻率為5 Hz.

為了驗證障礙物檢測法在不同環(huán)境下的檢測和跟蹤效果,本工作建立了兩個不同的場景:單個運動障礙物沿直線運動的場景和多個障礙物的場景(其中一部分是靜態(tài)障礙物,其余是動態(tài)障礙物,動態(tài)障礙物沿任意曲線運動).

6.2 實驗結果

圖6為設置單個動態(tài)障礙物的檢測和跟蹤結果,圖中綠色圓形表示激光的檢測范圍,其中障礙物的半徑設置為6 m,速度為2 m/s,沿水平方向向右運動.由圖6可以看出,障礙物檢測法可以有效地檢測出障礙物的位置,不會出現(xiàn)丟幀現(xiàn)象.障礙物中心的檢測位置和實際位置始終有一個固定偏差(見圖7),該偏差大約在3 m左右,這是由于激光雷達只能檢測到物體局部表面,該誤差可能根據(jù)障礙物的形狀、方位的不同而發(fā)生改變,但是對無人水面艇的導航避障結果影響很小.

圖6 單個障礙物檢測和跟蹤結果Fig.6 Detection and tracking result of single obstalce

圖7 動態(tài)障礙物檢測距離誤差Fig.7 Detection distance errors of dynamic obstacles

圖8為設置多個障礙物的檢測和跟蹤結果,圖中設置兩個靜態(tài)障礙物和一個動態(tài)障礙物.兩個靜態(tài)障礙物的半徑分別為10和6 m,動態(tài)障礙物的半徑為7 m,沿任意曲線運動,速度為5 m/s.由圖8可以看出,本算法可以有效地檢測和跟蹤隨機動態(tài)目標.由于激光雷達檢測的局部性,靜止的障礙物在檢測結果中也有運動的趨勢,但是其位置變化很小,且速度通常會小于

設定的閾值,故可根據(jù)上述方法將其從動態(tài)障礙物列表中有效剔除.

圖8 多個障礙物檢測和跟蹤結果Fig.8Detection and tracking results of multi-obstalces

7 結束語

本工作以無人水面艇的環(huán)境感知為背景,提出了一種基于多線激光雷達和2.5D柵格地圖的海上目標檢測算法.本算法利用3D激光點云數(shù)據(jù)可以較大地提高目標的檢測效率,同時將點云數(shù)據(jù)的預處理投影到2D空間進行,可以有效地減少計算時間,提高算法的實時性.此外,對障礙物實施數(shù)據(jù)關聯(lián)和卡爾曼濾波可以準確地獲得障礙物的位置、速度大小和運動方向,為無人水面艇的自主導航、自主避障提供了數(shù)據(jù)支持.本算法在基于電子海圖的仿真程序中顯示出較好的效果.同時,本算法擬應用在上海大學“精海5號”無人水面艇上.

[1]LIu Z,ZHANG Y,Yu X,et al.Unmanned surface vehicles:an overview of developments and challenges[J].Annual Reviews in Control,2016,41:71-93.

[2]WANG H,WEI Z,WANG S,et al.Real-time obstacle detection for unmanned surface vehicle[C]//IEEE Defense Science Research Conference and Expo.2011:1-4.

[3]WANG H,MOu X,MOu W,et al.Vision based long range object detection and tracking for unmanned surface vehicle[C]//IEEE International Conference on Cybernetics and Intelligent Systems.2015:101-105.

[4]LI C,CAO Z,XIAO Y,et al.Fast object detection from unmanned surface vehicles via objectness and saliency[C]//IEEE Chinese Automation Congress.2015:500-505.

[5]KRIsTAN M,PERˇs J,STLIˇC V,et al.A graphical model for rapid obstacle image-map estimation from unmanned surface vehicles[C]//ACCV.2014:391-406.

[6]ALMEIDA C,FRANCO T,FERREIRA H,et al.Radar based collision detection developments on USV ROAZⅡ[C]//Oceans.2009:1-6.

[7]BLAICH M,SCHusTER M,REuTER J.Collision avoidance for vessels using a low-cost radar sensor[C]//Proc of the Ifac World Congress.2014:9673-9678.

[8]HERMANN D,GALEAZZI R,ANDERsEN J C,et al.Smart sensor based obstacle detection for highspeed unmanned surface vehicle[C]//Ifac Conference on Manoeuvring and Control of Marine Craft.2015:190-197.

[9]PENG Y,Qu D,ZHONG Y,et al.The obstacle detection and obstacle avoidance algorithm based on 2-D lidar[C]//IEEE International Conference on Information and Automation.2015:1648-1653.

[10]AsVADI A,PREMEBIDA C,PEIXOTO P,et al.3D lidar-based static and moving obstacle detection in driving environments[J].Robotics&Autonomous Systems,2016,83(S1):299-311.

[11]HALTERMAN R,BRuCH M.Velodyne hdl-64e lidar for unmanned surface vehicle obstacle detection[C]//SPIE Defense,Security,and Sensing.International Society for Optics and Photonics, 2010:76920D.

[12]HIMMELsBACH M,M¨uLLER A,LuETTEL T,et al.LIDAR-based 3D object perception[C]// Proceedings of 1st International Workshop on Cognition for Technical Systems.2008:1.

[13]DOuILLARD B,UNDERWOOD J,MELKuMYAN N,et al.Hybrid elevation maps:3D surface models for segmentation[C]//2010 IEEE/RSJ International Conference on Intelligent Robots and Systems(IROS).2010:1532-1538.

[14]AsVADI A,PEIXOTO P,NuNEs U.Detection and tracking of moving objects using 2.5D motion grids[C]//IEEE International Conference on Intelligent Transportation Systems.2015:788-793.

[15]高紅波,王衛(wèi)星.一種二值圖像連通區(qū)域標記的新算法[J].計算機應用,2007,27(11):2776-2777.

[16]CHO H,SEO Y W,KuMAR B V K V,et al.A multi-sensor fusion system for moving object detection and tracking in urban driving environments[C]//IEEE International Conference on Robotics and Automation.2014:1836-1843.

[17]何友,修建娟,關欣.雷達數(shù)據(jù)處理及應用[M].2版.北京:電子工業(yè)出版社,2009:120-121.

[18]辛煜,梁華為,梅濤,等.基于激光傳感器的無人駕駛汽車動態(tài)障礙物檢測及表示方法[J].機器人, 2014,36(6):654-661.

3D Lidar-based marine object detection for USV

LI Xiaomao,ZHANG Xin,WANG Wentao,QU Dong,ZHU Chuan
(School of Mechatronic Engineering and Automation,Shanghai University,Shanghai 200072,China)

To realize autonomous navigation and avoidance,it is necessary for an unmanned surface vehicle(USV)to perceive the surrounding environment in real time,and detect obstacles threatening its sailing.Three dimensional Lidar plays an important role in unmanned system obstacle detection.In this paper,an obstacle detection algorithm based on 3D Lidar is proposed.The 3D laser point cloud is projected onto a 2.5D grid map in one period.The obstacles are clustered and segmented.The elliptical feature of obstacles is extracted from the raw laser point.Dynamic obstacles are found and tracked using the nearest neighbor data association and a Kalman flter.The simulation data based on the electronic chart verifes efectiveness of the method in USV obstacle detection.

unmanned surface vehicle(USV);Lidar;obstacle detection;grid map;elliptical representation

TP 242.6

A

1007-2861(2017)01-0027-10

10.3969/j.issn.1007-2861.2016.07.022

2017-01-11

國家自然科學基金資助項目(61673254);上海市自然科學基金資助項目(13ZR1454300);上海市科委能力建設資助項目(14500500400)

李小毛(1981—),男,研究員,研究方向為圖像處理、雷達數(shù)據(jù)處理、無人艇環(huán)境感知、導航和控制及其總體技術.E-mail:lixiaomao@shu.edu.cn