基于激光雷達(dá)的無人駕駛障礙物檢測和跟蹤

王 濤 ，曾文浩 ，于 琪

（西南交通大學(xué)電氣工程學(xué)院，四川 成都 610031）

近些年，隨著人工智能和機(jī)器人技術(shù)的不斷發(fā)展，無人駕駛汽車作為人工智能領(lǐng)域的一個(gè)重要的分支已經(jīng)成為國內(nèi)外的研究熱點(diǎn). 無人駕駛汽車主要通過激光雷達(dá)、相機(jī)、毫米波雷達(dá)、IMU (inertial measurement unit) 等傳感器實(shí)現(xiàn)對(duì)周圍環(huán)境的感知.障礙物檢測和跟蹤是環(huán)境感知的重要環(huán)節(jié)，檢測和跟蹤的效果好壞直接影響無人車的安全行駛. 近些年基于相機(jī)圖像的檢測技術(shù)獲得了很大發(fā)展，但是其檢測效果易受光線等環(huán)境影響. 激光雷達(dá)則因其能夠獲得障礙物的基本形貌、距離和位置等信息，同時(shí)具有精度高等優(yōu)點(diǎn)，被廣泛運(yùn)用于無人駕駛的障礙物檢測和跟蹤中[1].

國內(nèi)外學(xué)者對(duì)障礙物檢測和跟蹤的做了大量研究. 文獻(xiàn)[2]基于八叉樹網(wǎng)格法檢測運(yùn)動(dòng)物體，然后用DBSCAN (density-based spatial clustering of applications with noise）算法聚類. 然而其網(wǎng)格繪制過程復(fù)雜、難度大；文獻(xiàn)[3]用選取4 個(gè)代表點(diǎn)取代對(duì)所有點(diǎn)的搜索去改進(jìn)DBSCAN，實(shí)現(xiàn)激光雷達(dá)目標(biāo)物的快速、準(zhǔn)確檢測，但是其代表點(diǎn)的選取方法可能選入之前訪問過的點(diǎn)從而降低效率. 文獻(xiàn)[4]通過引入?yún)^(qū)域步長參數(shù)的方式，將激光點(diǎn)云數(shù)據(jù)距離測量值與搜索半徑建立了關(guān)聯(lián). 這樣在距離增大時(shí)，搜索半徑也隨之增大，適應(yīng)了激光雷達(dá)數(shù)據(jù)在距離增大時(shí)測量點(diǎn)密度降低的變化趨勢，使得算法對(duì)于區(qū)域密度分布不均勻的數(shù)據(jù)具備更好的普適性，在單線激光雷達(dá)中效果優(yōu)于傳統(tǒng)DBSCAN. 文獻(xiàn)[5]和文獻(xiàn)[6]采用自適應(yīng)的最小聚類數(shù)量MinPts 適應(yīng)遠(yuǎn)近聚類的物體點(diǎn)云，水平和垂直方向采用不同搜索半徑適應(yīng)激光雷達(dá)點(diǎn)云分布，但是該方法的搜索半徑都沒有隨距離調(diào)整，當(dāng)物體足夠遠(yuǎn)時(shí)候選點(diǎn)的鄰域點(diǎn)可能不存在，導(dǎo)致聚類失敗. 文獻(xiàn)[7]對(duì)聚類后的障礙物采用最近鄰數(shù)據(jù)關(guān)聯(lián)算法對(duì)距離特征進(jìn)行前后幀關(guān)聯(lián)，再使用卡爾曼濾波器進(jìn)行運(yùn)動(dòng)狀態(tài)估計(jì)和速度估計(jì)，但是用單一特征對(duì)障礙物進(jìn)行關(guān)聯(lián)，容易造成漏跟和錯(cuò)跟；文獻(xiàn)[8]認(rèn)為如果前后幀兩個(gè)對(duì)象的邊界框重疊，則被認(rèn)為是同一個(gè)對(duì)象，然而該方法只適用于緩慢移動(dòng)的物體，一旦速度過快就會(huì)關(guān)聯(lián)失敗. 汪世財(cái)?shù)萚9]考慮了障礙物的點(diǎn)云位置特征和幾何特征，利用多特征提高了障礙物關(guān)聯(lián)的可靠性，但是特征權(quán)重值通過經(jīng)驗(yàn)獲得的固定值，無法隨著位置不斷調(diào)整.

本文可分為障礙物檢測和跟蹤兩個(gè)部分，其中障礙物檢測分為點(diǎn)云預(yù)處理和聚類兩個(gè)部分. 在點(diǎn)云預(yù)處理部分主要是道路邊緣檢測和地面點(diǎn)去除等預(yù)處理工作；在聚類部分，本文對(duì)傳統(tǒng)的DBSCAN進(jìn)行改進(jìn)，采用自適應(yīng)搜索參數(shù)和代表候選點(diǎn)法，分別提高聚類適用性和實(shí)時(shí)性. 在障礙物跟蹤部分，本文利用加權(quán)的最小二乘法進(jìn)行速度估計(jì)，然后采用一種自適應(yīng)權(quán)重的多特征關(guān)聯(lián)算法進(jìn)行多目標(biāo)關(guān)聯(lián).

最后在實(shí)驗(yàn)中，采用16 線激光雷達(dá)進(jìn)行障礙物檢測和跟蹤，驗(yàn)證了本文障礙物聚類和關(guān)聯(lián)算法的有效性. 通過實(shí)驗(yàn)表明整個(gè)檢測流程能夠?qū)?0 幀/s的激光雷達(dá)數(shù)據(jù)實(shí)現(xiàn)準(zhǔn)確、穩(wěn)定地檢測和跟蹤.

1 點(diǎn)云預(yù)處理

本文使用的激光雷達(dá)點(diǎn)云數(shù)據(jù)來源于兩臺(tái)型號(hào)為Velodyne VLP-16 的激光雷達(dá)，這兩臺(tái)激光雷達(dá)被安裝在“智軌”列車的左右兩側(cè)，“智軌”列車為中車株洲電力機(jī)車研究所有限公司自主研發(fā)的智能軌道快運(yùn)列車. 本文數(shù)據(jù)處理硬件平臺(tái)為個(gè)人電腦，硬件配置為Intel 8 核2.3 GHz 處理器，內(nèi)存8 GB，軟件采用Ubuntu16.04 下的ROS 機(jī)器人操作系統(tǒng). 圖1 所示為“智軌”列車平臺(tái)和數(shù)據(jù)采集場所——某市的株洲大道.

圖1 “智軌”列車平臺(tái)以及株洲大道Fig. 1 ART Train Platform and Zhuzhou Avenue

1.1 路沿檢測和感興趣區(qū)域提取

VLP-16 的豎直視場角為30°，豎直方向有16 線激光束，水平視場角為360°. 工作頻率為10 Hz 時(shí)，一臺(tái)激光雷達(dá)每一幀大約有3 萬個(gè)點(diǎn). 兩臺(tái)激光雷達(dá)的數(shù)據(jù)量是單臺(tái)的兩倍，因此有必要減少無關(guān)點(diǎn)云對(duì)數(shù)據(jù)處理速度的影響.

激光雷達(dá)回傳的點(diǎn)云數(shù)據(jù)中包含許多與行駛區(qū)域無關(guān)的點(diǎn)，提取出可行駛區(qū)域，可以減少數(shù)據(jù)量，提高計(jì)算速度. 首先進(jìn)行道路邊沿檢測[10]. 根據(jù)激光雷達(dá)點(diǎn)云在路緣石處存在高程突變的特點(diǎn)，提取出每一線的激光點(diǎn)云；然后按坐標(biāo)y的絕對(duì)值對(duì)左右雷達(dá)點(diǎn)云分別排序（激光雷達(dá)坐標(biāo)系符合空間坐標(biāo)系的右手法則）；利用“滑動(dòng)窗口”對(duì)坐標(biāo)z的值突變較大的“窗口”進(jìn)行標(biāo)記，將其“窗口”內(nèi)突變最大的點(diǎn)作為路沿點(diǎn). 提取效果如圖2 所示，圖中紅色大點(diǎn)即為路沿點(diǎn). 然后選取路沿中間的區(qū)域和路沿附近的區(qū)域作為感興趣區(qū)域，其余區(qū)域不參與之后的處理，從而減少計(jì)算量.

圖2 路沿點(diǎn)提取效果Fig. 2 Effect of curb point extraction

1.2 地面分割

感興趣區(qū)域內(nèi)的點(diǎn)云數(shù)據(jù)中不僅包含障礙物的點(diǎn)，還包含大量的地面點(diǎn)，障礙物檢測關(guān)心的是地面以上的點(diǎn)，因此需要去除地面點(diǎn). 由于現(xiàn)實(shí)的地面并不是一個(gè)完美的平面，而且當(dāng)距離較大時(shí)激光雷達(dá)會(huì)存在一定的測量噪聲，單一的平面模型并不足以描述現(xiàn)實(shí)的地面. 本文使用基于掃描線束的分割算法[11]對(duì)地面進(jìn)行分割，該算法的核心是以射線的形式來組織點(diǎn)云. 將點(diǎn)云的 (x，y，z) 三維空間降到 (x，y) 平面來看，計(jì)算每一個(gè)點(diǎn)的水平夾角θ，對(duì)同一水平夾角的點(diǎn)按照水平距離r進(jìn)行排序，計(jì)算前后兩點(diǎn)的坡度Δp是否大于事先設(shè)定的坡度閾值，從而判斷點(diǎn)是否為地面點(diǎn)，計(jì)算如式（1）.

式中：m為測量點(diǎn)序號(hào)；zm和rm分別為點(diǎn)m的z方向距離和水平距離.

分割效果如圖3 所示，圖3（a）為原始點(diǎn)云，包括地面和障礙物點(diǎn)云，圖3（b）為地面分割算法得到的障礙物點(diǎn)云. 由圖3 可見，通過以上方法，成功達(dá)到了減少點(diǎn)云數(shù)據(jù)數(shù)量并保留障礙物點(diǎn)云的效果.

圖3 地面分割效果Fig. 3 Effect of ground segmentation

2 優(yōu)化的DBSCAN 聚類檢測算法

本文采用自適應(yīng)的聚類算法進(jìn)行障礙物檢測，并提取障礙物的幾何特征特征. DBSCAN 是經(jīng)典的密度聚類算法，該方法將具有足夠高密度的區(qū)域劃分為簇，可發(fā)現(xiàn)任意形狀的聚類. DBSCAN 算法需要兩個(gè)參數(shù)：搜索半徑ε和最小包含點(diǎn)數(shù)MinPts.DBSCAN 算法的思想是在數(shù)據(jù)集中任選一點(diǎn)q作為起始點(diǎn)，如果q的ε鄰域內(nèi)的點(diǎn)數(shù)量不小于MinPts，則q的鄰域點(diǎn)作為即候選點(diǎn)，通過不斷查找從候選點(diǎn)的鄰域點(diǎn)來擴(kuò)展該簇，直至找到1 個(gè)完整的簇；如果q的ε鄰域內(nèi)的點(diǎn)數(shù)量小于MinPts，則q被暫時(shí)標(biāo)注為噪聲點(diǎn). 對(duì)數(shù)據(jù)集中未被處理的點(diǎn)重復(fù)上述過程，進(jìn)行其他簇的擴(kuò)展，直至所有點(diǎn)均被訪問.

2.1 自適應(yīng)參數(shù)的改進(jìn)

全局固定ε閾值會(huì)導(dǎo)致分割程度的不確定性，閾值設(shè)置過大會(huì)導(dǎo)致距離較近的兩類物體被分為一類，閾值過小會(huì)導(dǎo)致本應(yīng)該屬于同一個(gè)物體的點(diǎn)云被分開，這就造成了存在分割不足或者分割過當(dāng)?shù)那闆r[12]. 因此ε的取值應(yīng)該隨點(diǎn)云分布而變化，本文采用隨距離增大而線性增大的ε值.

由于激光雷達(dá)在水平方向的分辨率與豎直方向的分辨率一般不同，所以某些情況下兩個(gè)距離很近的物體，無論用較大的ε還是較小的ε都無法有效分割，因此ε的選擇不僅需要隨著離激光雷達(dá)原點(diǎn)距離變化，還要在不同方向上取不同的值，最終形成一個(gè)空間中大小不斷變化的“搜索橢球”，如圖4所示.

圖4 中：L和H分別為兩個(gè)相鄰點(diǎn)之間的水平和豎直距離；εh和εv分別為水平和豎直搜索半徑，即“搜索橢球”的半短軸和半長軸.

圖4 球型搜索和橢球型搜索示意Fig. 4 Illustration of spherical search and ellipsoid search

可以使用式（2）計(jì)算兩個(gè)相鄰點(diǎn)之間的L和H.

式中：d為測量點(diǎn)到激光雷達(dá)原點(diǎn)的距離；θh和θv分別為激光雷達(dá)的水平和垂直角分辨率. 查閱VLP-16 用戶手冊(cè)可知，激光雷達(dá)在10 Hz 的工作頻率下其水平和垂直角分辨率分別為0.2° 和2.0°.

為了確保可以搜索到同一物體表面的相鄰點(diǎn)，搜索半徑應(yīng)大于或等于該位置的兩個(gè)相鄰點(diǎn)之間的距離. 所以水平和豎直搜索半徑εh和εv可表示為

式中：a為搜索系數(shù)，代表著搜索半徑跨越的激光束的數(shù)量，a≥ 1.

MinPts 值是根據(jù)從搜索橢球內(nèi)收集的最大點(diǎn)數(shù)以及障礙物傾斜角度來估算的. 當(dāng)激光束垂直照射到障礙物時(shí)，障礙物點(diǎn)數(shù)達(dá)到最大值，此時(shí)可以將橢球體視為橢圓形. 直接計(jì)算橢圓內(nèi)的點(diǎn)數(shù)很困難，因此可以利用蒙特卡洛隨機(jī)模擬法的思想求解橢圓內(nèi)點(diǎn)數(shù). 即首先計(jì)算長為εv、寬為εh的矩形內(nèi)的點(diǎn)數(shù)量Nr，再根據(jù)矩形和橢圓的面積比，計(jì)算出橢圓內(nèi)點(diǎn)的數(shù)量為

考慮到實(shí)際情況下激光束與障礙物平面之間的角度可能不為90°，障礙物的總點(diǎn)數(shù)等于或小于計(jì)算的N值. 假設(shè)原本激光束與障礙物平面夾角為90°，而現(xiàn)在發(fā)生水平方向偏轉(zhuǎn)角度φh，如圖5 所示.O為激光雷達(dá)原點(diǎn)，E為相鄰點(diǎn)連線的與相鄰激光束角平分線交點(diǎn)，原來相鄰點(diǎn)A、B之間的水平距離為L（AB長度），旋轉(zhuǎn)φh度后為點(diǎn)C、D，距離變成L1（CE長度）+L2（DE長度）.

圖5 物體發(fā)生傾斜時(shí)的示意Fig. 5 Schematic diagram in the case of tilted object

此時(shí)兩個(gè)相鄰點(diǎn)之間的豎直距離不會(huì)發(fā)生變化，而水平距離會(huì)發(fā)生變化. 計(jì)算方法如式（5）.

經(jīng)化簡可計(jì)算出偏轉(zhuǎn)后相鄰點(diǎn)之間的水平距離Lr（CD長度） 為

定義水平偏轉(zhuǎn)系數(shù)kh，當(dāng)水平偏轉(zhuǎn)φh時(shí)相鄰點(diǎn)之間的水平距離增加kh倍，則kh可以表示為

實(shí)際過程中水平角分辨率θh很小，可當(dāng)作趨近于0；而若φh趨近于90° 時(shí)，該面將無法接收點(diǎn)云.因此：

同理可以得到豎直偏轉(zhuǎn)系數(shù)kv，kv≈1/cosφv，φv為垂直偏轉(zhuǎn)角. 因此，結(jié)合式（4）、（5）、（7）和式（8）可得，物體與激光數(shù)發(fā)生偏轉(zhuǎn)后橢圓內(nèi)的點(diǎn)數(shù)量為

考慮實(shí)際情況，水平方向偏轉(zhuǎn)角度φh最大取60°且豎直方向偏轉(zhuǎn)角度φv最大取45°. 另外由于空氣中雨霧、塵埃和鏡面反射等原因會(huì)造成的點(diǎn)云空缺和偏移，所以引入損失系數(shù)η，取值0.8. 則最后MinPts 取值為

按照以上方法選擇自適應(yīng)搜索半徑ε和最小包含點(diǎn)數(shù)MMinPts，可以對(duì)不同距離和傾角的障礙物3 維點(diǎn)云進(jìn)行準(zhǔn)確聚類.

2.2 實(shí)時(shí)性優(yōu)化

原始的聚類算法中，核心點(diǎn)的鄰域內(nèi)所有的點(diǎn)都會(huì)作為候選點(diǎn)，而實(shí)際起作用的點(diǎn)主要是鄰域邊緣的數(shù)據(jù)點(diǎn)，選取全部鄰域點(diǎn)必定會(huì)浪費(fèi)較多的搜索時(shí)間. 為了加速DBSCAN 算法，文本選取核心點(diǎn)鄰域內(nèi)某些具有代表性的點(diǎn)作為候選種子點(diǎn)，簡稱代表點(diǎn). 如圖6 所示，P為核心點(diǎn)，三角形和圓形點(diǎn)是P的鄰域點(diǎn). 在未被訪問過的領(lǐng)域核心點(diǎn)中選出離6 個(gè)邊緣點(diǎn)（εh，0，0）、（?εh，0，0）、（0，εh，0）、（0，?εh，0）、（0，0，εv）和（0，0，?εv）距離最近的點(diǎn)作為代表點(diǎn)，添加入該種子簇，如圖6 中三角形. 若某個(gè)代表點(diǎn)被選擇兩次，則它只被添加一次. 采用這種方法，在激光雷達(dá)點(diǎn)云數(shù)據(jù)中，任意核心點(diǎn)最多6 個(gè)代表點(diǎn). 因此可以有效減少計(jì)算量，提高聚類速度.

圖6 橢球域及其代表對(duì)象的三維鄰域Fig. 6 Three-dimensional neighborhood of ellipsoidal domain and its representative objects

本文采用的激光雷達(dá)是低線束激光雷達(dá)，如果使用64 線、128 線等激光雷達(dá)時(shí)，因?yàn)閿?shù)據(jù)量過于大，該算法內(nèi)存消耗較大，會(huì)出現(xiàn)延遲. 此時(shí)可以考慮對(duì)點(diǎn)云進(jìn)行柵格化處理，然后對(duì)有序的柵格進(jìn)行聚類，以提高算法實(shí)時(shí)性.

3 障礙物跟蹤

為了獲取動(dòng)態(tài)障礙物的動(dòng)態(tài)特性，需要將當(dāng)前幀信息與歷史信息進(jìn)行匹配[13]. 本文利用加權(quán)的多特征數(shù)據(jù)關(guān)聯(lián)算法結(jié)合濾波算法實(shí)現(xiàn)對(duì)動(dòng)態(tài)障礙物的跟蹤. 激光雷達(dá)點(diǎn)云前后幀間隔0.1 s，每幀之間可以認(rèn)為是線性運(yùn)動(dòng)，而卡爾曼濾波器是一個(gè)線性遞歸濾波器，相比于粒子濾波也具有更高的實(shí)時(shí)性.因此本文先采用卡爾曼濾波器對(duì)目標(biāo)障礙物的位置進(jìn)行預(yù)測后，再用多特征數(shù)據(jù)關(guān)聯(lián)算法在前后相鄰兩幀之間進(jìn)行關(guān)聯(lián)，最后采用卡爾曼濾波器對(duì)目標(biāo)障礙物的位置進(jìn)行濾波，經(jīng)過以上步驟即可完成對(duì)目標(biāo)跟蹤. 算法流程如圖7 所示.

圖7 障礙物跟蹤算法流程Fig. 7 Flowchart of obstacle tracking algorithm

3.1 濾波器預(yù)測

受測量噪聲和自主駕駛車輛車體自身運(yùn)動(dòng)的影響，直接對(duì)前后幀的障礙物進(jìn)行關(guān)聯(lián)，有可能導(dǎo)致關(guān)聯(lián)錯(cuò)誤. 為了解決目標(biāo)跟蹤中測量噪聲引起的目標(biāo)運(yùn)動(dòng)軌跡的大幅變動(dòng)對(duì)預(yù)測目標(biāo)運(yùn)動(dòng)軌跡造成的影響，以及在障礙物關(guān)聯(lián)中對(duì)于預(yù)測目標(biāo)下一幀位置的需要，有必要采用卡爾曼濾波器對(duì)目標(biāo)的位置進(jìn)行預(yù)測.

卡爾曼濾波的預(yù)測方程如式（11）.

式中：Xt為第t幀（即時(shí)刻t）障礙物中心的狀態(tài)向量，

本文激光雷達(dá)工作頻率為10 Hz，所以每幀時(shí)間間隔T取0.1 s；

ωt為狀態(tài)噪聲，滿足ωt～N（0，Q）的高斯分布，本文取Q= 0.01E4，E4為4 階單位矩陣.

則障礙物中心的狀態(tài)向量預(yù)測值和預(yù)測協(xié)方差方程分別為

式中：Xt|t和Pt|t分別為時(shí)刻t的預(yù)測值向量和預(yù)測協(xié)方差向量.

令S(a0，a1，a2)取最小值，即可得到系數(shù)a0、a1、a2，擬合后可求出時(shí)刻t的速度值.

當(dāng)障礙物出現(xiàn)次數(shù)為1，即新出現(xiàn)時(shí)，認(rèn)為速度為0；當(dāng)障礙物出現(xiàn)次數(shù)為2，即前面只有1 幀時(shí)，速度即為前一幀的速度；當(dāng)障礙物出現(xiàn)次數(shù)為3、4、5 時(shí)，進(jìn)行1 次函數(shù)擬合；當(dāng)障礙物出現(xiàn)次數(shù)大于5，即前面的5 幀以上時(shí)，則利用前面的5 幀數(shù)據(jù)進(jìn)行上述二次曲線擬合計(jì)算當(dāng)前時(shí)刻的速度.

3.2 多特征數(shù)據(jù)關(guān)聯(lián)

逐幀動(dòng)態(tài)跟蹤目標(biāo)屬于動(dòng)態(tài)規(guī)劃領(lǐng)域中的“配對(duì)問題”[14]，兩組點(diǎn)可基于距離進(jìn)行配對(duì). 本文采用最近鄰算法進(jìn)行數(shù)據(jù)關(guān)聯(lián)，最近鄰算法計(jì)算所有觀測數(shù)據(jù)和目標(biāo)預(yù)測值之間的差異度，然后再取差異度最小的一個(gè)觀測數(shù)據(jù)作為該目標(biāo)的觀測值.

設(shè)第t幀內(nèi)檢測到的第i個(gè)障礙物與第t+1 幀內(nèi)檢測到的第j個(gè)障礙物之間的差異度函數(shù)通常采用歐式距離計(jì)算, 如式 (16).

式中：n為障礙物數(shù)；Δck為第t幀檢測到的第i個(gè)障礙物與第t+1 幀檢測到的第j個(gè)障礙物之間第k個(gè)特征的差值.

由于16 線激光雷達(dá)垂直方向分辨率低，運(yùn)動(dòng)的障礙物的點(diǎn)云在垂直方向會(huì)有很大突變，所以本文不使用垂直方向特征. 根據(jù)激光雷達(dá)的掃描特性，物體位于激光雷達(dá)不同位置會(huì)被掃描到不同形態(tài)的點(diǎn)云，例如物體位于激光雷達(dá)正前后方，則點(diǎn)云主要是yOz面點(diǎn)云，如圖8 的物體A，其長度特性消失；物體位于激光雷達(dá)側(cè)方，則點(diǎn)云主要是xOz面點(diǎn)云，如圖8 的物體B，其寬度特性消失；物體位于激光雷達(dá)其他位置，則點(diǎn)云由xOz和yOz面按組成點(diǎn)云，圖8的物體C，同時(shí)擁有寬度和長度特性.

圖8 障礙物與激光雷達(dá)的相對(duì)位置Fig. 8 Relative position of obstacles and LiDAR

假設(shè)圖中x方向?yàn)殚L度方向，y方向?yàn)閷挾确较? 越靠近x軸障礙物的點(diǎn)云的寬度特征越明顯；越靠近y軸障礙物的點(diǎn)云的長度特征越明顯. 因此障礙物長和寬特征的權(quán)值可以根據(jù)障礙物相對(duì)激光雷達(dá)的水平位置而調(diào)節(jié).

當(dāng)收到第t+1 幀障礙物觀測點(diǎn)云時(shí)，計(jì)算其與第t幀障礙物預(yù)測狀態(tài)的加權(quán)歐式距離，然后將差異度最小的點(diǎn)進(jìn)行關(guān)聯(lián). 則第t幀的第i個(gè)障礙物和第t+1 幀的第j個(gè)障礙物差異度計(jì)算如式（17）.

最近鄰數(shù)據(jù)關(guān)聯(lián)算法計(jì)算量小，適用于目標(biāo)數(shù)量較少時(shí)的前后匹配. 但在目標(biāo)數(shù)目較多、運(yùn)動(dòng)情況復(fù)雜時(shí)，可以考慮歷史信息或者概率信息，比如采用JPDA （joint probabilistic data association） 或者M(jìn)HT （multiple hypothesis tracking） 算法，提高對(duì)多目標(biāo)復(fù)雜場景的抗干擾能力.

3.3 濾波器更新

當(dāng)前一幀預(yù)測后的狀態(tài)與下一幀觀測狀態(tài)關(guān)聯(lián)上之后，需要利用下一幀的觀測值對(duì)上一幀的預(yù)測值進(jìn)行修正，得到下一幀狀態(tài)作為最優(yōu)估計(jì)，即卡爾曼濾波的更新過程.

測量狀態(tài)轉(zhuǎn)移方程如下：

νt為狀態(tài)噪聲，滿足νt～N(0，R) 的高斯分布.其中，R為測量噪聲，本文取R=0.001E2，E2為2 階單位矩陣.

則時(shí)刻t+1 的誤差增益、更新值和濾波誤差的協(xié)方差矩陣分別為

4 實(shí)驗(yàn)結(jié)果

為了驗(yàn)證本文算法的有效性，在Ubuntu 的ROS系統(tǒng)下，采用C++編程處理激光雷達(dá)采集的數(shù)據(jù).

由于城市道路環(huán)境相對(duì)單調(diào)，在聚類算法實(shí)驗(yàn)中，采用激光雷達(dá)固定，3 人在雷達(dá)面前自由活動(dòng)，然后對(duì)人員進(jìn)行聚類，最后將聚類結(jié)果用三維邊框進(jìn)行框選，方便觀察效果.

為了驗(yàn)證自適應(yīng)搜索半徑的作用，對(duì)比加入和未加入自適應(yīng)搜索半徑的DBSCAN 算法對(duì)同一數(shù)據(jù)的處理效果. 其中自適應(yīng)算法的搜索系數(shù)a取10，傳統(tǒng)算法的搜索半徑ε取0.2 m 和0.8 m，最小聚類數(shù)量均取MinPts 值取22. 其中第260 幀數(shù)據(jù)聚類的俯視圖效果如圖9 所示. 由圖9 可見：傳統(tǒng)算法搜索半徑取0.2 m 時(shí)漏檢了左側(cè)人員，如圖9（a）；搜索半徑為0.8 m 時(shí)又出現(xiàn)了分割不足的現(xiàn)象，將中間和右側(cè)人員歸入一類，如圖9（b）；而優(yōu)化后的算法可以檢測出所有人員，如圖9（c）.

圖9 第260 幀的聚類結(jié)果Fig. 9 Clustering results at the frame 260th

為了驗(yàn)證“橢球搜索”的作用，對(duì)比加入和未加入“橢球搜索”的算法對(duì)同一數(shù)據(jù)的處理效果.截取第280 幀結(jié)果的正視圖效果，如圖10 所示. 由圖10 可見：未加入“橢球搜索”存在分割不完全的現(xiàn)象，如圖10（a）；加入“橢球搜索”后可以聚類出完整的障礙物點(diǎn)云，如圖10（b）.

圖10 第280 幀的聚類結(jié)果Fig. 10 Clustering results at the frame 280th

城市道路的障礙物大部分為運(yùn)動(dòng)車輛，對(duì)障礙物檢測的實(shí)時(shí)性要求比較高. 在聚類算法實(shí)時(shí)性驗(yàn)證和跟蹤算法實(shí)驗(yàn)中，采集城市道路上的激光雷達(dá)數(shù)據(jù)，該數(shù)據(jù)由2 臺(tái)安裝在“智軌”兩側(cè)的VLP-16 雷達(dá)在某市的城市道路采集.

為驗(yàn)證代表點(diǎn)方法對(duì)DBSCAN 算法實(shí)時(shí)性的提升，對(duì)比采用和不采用代表點(diǎn)加速的算法對(duì)同一數(shù)據(jù)的處理效果. 統(tǒng)計(jì)第t1?2 幀到第t1+92 幀數(shù)據(jù)的聚類處理時(shí)間（t1∈t），每隔10 幀采樣一次，每次采樣取該幀前、后共5 幀的平均值，最后繪制曲線如圖11 所示. 由圖11 可見：采用代表點(diǎn)法的DBSCAN 算法處理同一幀數(shù)據(jù)的時(shí)間明顯小于不采用代表點(diǎn)法DBSCAN. 可求出傳統(tǒng)算法平均耗時(shí)0.015 1 s，代表點(diǎn)法加速算法平均耗時(shí)0.011 4 s，時(shí)間上縮短32%.

圖11 聚類所消耗的時(shí)間對(duì)比Fig. 11 Comparison of time consumed by clustering

為驗(yàn)證跟蹤算法的可靠性，在城市道路數(shù)據(jù)中，對(duì)路上行駛車輛進(jìn)行檢測并對(duì)車輛進(jìn)行框選，然后對(duì)框選的車輛進(jìn)行跟蹤和標(biāo)號(hào)（ID），對(duì)比同一車輛前后幀的標(biāo)號(hào)是否發(fā)生變化和不同車輛標(biāo)號(hào)是否不同，以驗(yàn)證數(shù)據(jù)關(guān)聯(lián)和跟蹤算法的可靠性. 實(shí)驗(yàn)結(jié)果如圖12 所示. 圖12（a）為第t2幀車輛目標(biāo)（t2∈t）標(biāo)號(hào)為13、16、20 和21，圖12（b）為第t2+1 幀跟蹤后對(duì)應(yīng)的位置；圖12（c）為第t3幀車輛目標(biāo)（t3∈t）標(biāo)號(hào)為33、50 和51 號(hào)的車輛，圖12（d）為第t3+1 幀跟蹤后對(duì)應(yīng)的位置. 由圖12 可見：本文對(duì)動(dòng)態(tài)障礙物進(jìn)行了穩(wěn)定檢測和跟蹤.

圖12 動(dòng)態(tài)障礙物跟蹤效果Fig. 12 Effect of dynamic obstacle tracking

5 結(jié) 論

本文主要針對(duì)基于三維激光雷達(dá)的動(dòng)態(tài)障礙物檢測和跟蹤問題進(jìn)行研究. 在障礙物聚類過程中，對(duì)傳統(tǒng)的DBSCAN 聚類方法進(jìn)行改進(jìn)，提出“橢球搜索”和自適應(yīng)的搜索半徑提升了激光雷達(dá)點(diǎn)云的聚類效果，使用代表對(duì)象法改進(jìn)搜索速度，同時(shí)提高了障礙物聚類的適應(yīng)性和實(shí)時(shí)性；在數(shù)據(jù)關(guān)聯(lián)過程中，考慮了障礙物的位置特征和幾何特征，利用多特征提高了障礙物關(guān)聯(lián)的可靠性；然后采用加權(quán)的最小二乘法對(duì)障礙物的速度進(jìn)行預(yù)測和采用卡爾曼濾波算法對(duì)障礙物位置進(jìn)行預(yù)測和跟新，從而實(shí)現(xiàn)多目標(biāo)跟蹤. 最后分別就聚類效果和跟蹤效果進(jìn)行實(shí)驗(yàn)分析，結(jié)果表明：采用本文方法在障礙物檢測時(shí)比傳統(tǒng)聚類方法在處理效果和時(shí)間上都得到了提升，并且能夠?qū)?dòng)態(tài)的障礙物進(jìn)行穩(wěn)定地關(guān)聯(lián)和跟蹤. 實(shí)驗(yàn)過程中還發(fā)現(xiàn)，路旁的樹木等障礙物會(huì)對(duì)跟蹤算法準(zhǔn)確性造成影響. 因此，后續(xù)研究考慮加入點(diǎn)云的類別信息，提高復(fù)雜場景的跟蹤準(zhǔn)確率.