激光雷達(dá)輔助駕駛道路參數(shù)計(jì)算方法研究

游安清，潘旭東，趙 平，潘文武

（1.中國工程物理研究院 高能激光科學(xué)與技術(shù)重點(diǎn)實(shí)驗(yàn)室，四川 綿陽 621900；2.中國工程物理研究院 應(yīng)用電子學(xué)研究所，四川 綿陽 621900）

引言

在一些特定場合，比如特定的軍事轉(zhuǎn)移過程中，需要用一輛小型導(dǎo)引車對轉(zhuǎn)移路線上的道路路況進(jìn)行實(shí)時勘測，為后面大部隊(duì)大型車輛的可通過性提供路線決策和輔助駕駛依據(jù)，包括道路寬度、限高、拐彎半徑等因素是否支持大型車輛通過。為此，需要在前端導(dǎo)引車上配備激光雷達(dá)輔助駕駛系統(tǒng)[1-4]，實(shí)時掃描路況并快速計(jì)算道路參數(shù)。與二維圖像不同，三維激光點(diǎn)云屬于無序離散圖像，其處理方法遠(yuǎn)不如二維圖像成熟。本文設(shè)計(jì)一種基于點(diǎn)云垂直切片的路寬、限高計(jì)算方法，可以有效解決城區(qū)結(jié)構(gòu)化道路的參數(shù)提取[5-11]問題。并設(shè)計(jì)一種基于載車位姿數(shù)據(jù)最小二乘擬合的拐彎半徑全自動、快速計(jì)算方法，通過與Terrasolid 等經(jīng)典激光點(diǎn)云處理軟件相比，不僅效率上有大幅提高，而且精度也更高。

1 系統(tǒng)組成

本系統(tǒng)在硬件上采用“VLP 多線激光掃描頭+慣性導(dǎo)航單元IMU + 數(shù)據(jù)處理單元DPU”的構(gòu)架如圖1 所示。激光掃描頭完成對載車周圍景物及道路路面的激光掃描；慣性導(dǎo)航單元實(shí)時測量系統(tǒng)所處的地理位置和在三維空間所處的姿態(tài)[12-13]；數(shù)據(jù)處理單元運(yùn)行數(shù)據(jù)解算軟件，完成激光點(diǎn)云數(shù)據(jù)預(yù)處理、三維重構(gòu)、點(diǎn)云分割、道路提取、路面參數(shù)計(jì)算等所有算法功能[14-15]。

圖1 車載輔助駕駛系統(tǒng)的系統(tǒng)Fig.1 Vehicle-mounted auxiliary driving system

2 點(diǎn)云重構(gòu)

跟傳統(tǒng)激光雷達(dá)一樣，點(diǎn)云解算方法及其應(yīng)用軟件開發(fā)也非常重要。為了在車載平臺上對道路進(jìn)行三維掃描、并構(gòu)建三維點(diǎn)云圖，必須定義3 個坐標(biāo)系。

1)掃描儀局部坐標(biāo)系：如圖1，y軸平行于VLP16 掃描儀的軸線、指向天頂；x軸在掃描儀的橫截面內(nèi)、垂直y軸、指向車右側(cè)方向；z軸經(jīng)過掃描儀的0°光束方向的反方向、指向車尾。

2)載車固連坐標(biāo)系：取為車載慣導(dǎo)系統(tǒng)(IMU)的軸系。

3)世界坐標(biāo)系：一個相對靜止的全局坐標(biāo)系，可以任意自定義，比如載車出發(fā)地的地平坐標(biāo)系。

車載三維掃描時，掃描儀局部坐標(biāo)系置于載車固連坐標(biāo)系之上。由于存在安裝關(guān)系，所以這兩個坐標(biāo)系并不平行、原點(diǎn)也不重合，設(shè)掃描儀局部坐標(biāo)系相對載車固連坐標(biāo)系有(α1，β1，γ1)的姿態(tài)角(這3 個角的繞轉(zhuǎn)軸分別是y、x、z，可分別稱為航向角、俯仰角、滾轉(zhuǎn)角)，掃描儀局部標(biāo)系相對載車固連坐標(biāo)系的原點(diǎn)偏移為(u1，v1，w1)；同理，載車固連坐標(biāo)系相對世界坐標(biāo)系也有姿態(tài)角和原點(diǎn)偏移，設(shè)為(α2，β2，γ2)和(u2，v2，w2)。

有了這些姿態(tài)和位移量，就可以將掃描儀的測點(diǎn)坐標(biāo)(r，θ)轉(zhuǎn)換成全局點(diǎn)云坐標(biāo)P(x，y，z)：

式中：Raxis(·)為3×3 的右手螺旋坐標(biāo)旋轉(zhuǎn)矩陣。

上式模型的12 個位姿參數(shù)中，(α1，β1，γ1，u1，v1，w1)與掃描儀在載車上的安裝角和安裝位置有關(guān)，提前標(biāo)定獲得；(α2，β2，γ2，u2，v2，w2)可由載車上的慣導(dǎo)系統(tǒng)實(shí)時給出。依此模型，就可在載車前進(jìn)的同時，實(shí)現(xiàn)點(diǎn)云重構(gòu)和整體三維場景的“逐漸生長”，圖2 為示例。

圖2 激光雷達(dá)道路場景掃描示例Fig.2 Scanning road with LIDAR

3 道路參數(shù)提取

3.1 道寬、限高計(jì)算與實(shí)驗(yàn)

本文設(shè)計(jì)一種通過點(diǎn)云法截面網(wǎng)格投影計(jì)算道路寬度、限高的方法，步驟如下：

1) 設(shè)定算法參數(shù)，包括網(wǎng)格尺寸s(等于所期望的精度水平比，如5 cm)、網(wǎng)格內(nèi)非噪點(diǎn)的最小點(diǎn)數(shù)閾值Nmin(比如5 個點(diǎn)/格)、點(diǎn)云切片層厚度d(比如5 m)、單邊路寬最大值a(比如20m)、上方限高物最大高度b(比如15 m)。這些參數(shù)的取值是通過事后離線數(shù)據(jù)分析、參數(shù)嘗試和算法效果測試確定出來的，它們與激光雷達(dá)點(diǎn)頻率、載車行進(jìn)速度等作業(yè)配置有關(guān)。

2) 根據(jù)慣導(dǎo)給出的載車位姿數(shù)據(jù)序列，計(jì)算生成載車的軌跡曲線，如圖3 所示；根據(jù)軌跡曲線，計(jì)算軌跡上每個位置點(diǎn)處載車行進(jìn)的方向，以及該方向的法截面方程。

圖3 道路點(diǎn)云與載車軌跡Fig.3 Road point cloud and truck track

3) 在每個軌跡點(diǎn)處，垂直于行進(jìn)方向，切出厚度為d的點(diǎn)云層，并按尺度s作二維網(wǎng)格劃分，如圖4 所示。

圖4 道路點(diǎn)云切片及網(wǎng)格化Fig.4 Point cloud slice and mesh

4) 從載車上掃描儀中心位置(圖4 中粉色實(shí)心圓點(diǎn))所在網(wǎng)格起，往上取若干格(比如至1.5 m 處為止)、往下取若干格(比如至-1 m 處為止)，組成一個豎直網(wǎng)格條。將該網(wǎng)格條逐漸向左平移，直至碰到內(nèi)含點(diǎn)數(shù)大于Nmin的網(wǎng)格阻擋為止，該位置定為路的左邊界；再將豎直網(wǎng)格條向右平移，直至碰到內(nèi)含點(diǎn)數(shù)大于Nmin的網(wǎng)格阻擋為止，該位置定為路的右邊界；計(jì)算左右邊界間的距離，得路的寬度W。

5) 以中心網(wǎng)格及其左右邊界之間的網(wǎng)格一起組成一個水平網(wǎng)格條，將該網(wǎng)格條自中心位置向上平移，直至碰到內(nèi)含點(diǎn)數(shù)大于Nmin的網(wǎng)格阻擋為止，該位置定為路的上方凈空高度H。路寬W和上方凈高H，共同構(gòu)成該路段的可通過空間(一個矩形范圍)。

6) 幾種常見情形：

a) 如果往左/右平移時始終未碰到有阻擋的網(wǎng)格，則以設(shè)定的最大單邊路寬a作為該側(cè)路寬。

b) 如果往上平移始終未碰到有阻擋的網(wǎng)格，則認(rèn)為該段路上方是開闊空域、沒有限高。

c)當(dāng)載車經(jīng)過上方有廣告牌、交通燈橫桿等物體時，會測量得到有效限高。

d)當(dāng)載車從架空電線或橋梁下面經(jīng)過時，會測量得到上方電線或橋梁的相對高度。

e)當(dāng)載車從隧道內(nèi)經(jīng)過時，會同時掃描測到隧道的左右寬度和頂面高度。

在戶外場景開展了初步的驗(yàn)證實(shí)驗(yàn)，在城區(qū)結(jié)構(gòu)化道路上效果良好，如圖5 所示，圖中上方有線纜限高物，黃色門形框?yàn)檐囕v可通過的矩形范圍。

相對文獻(xiàn)中的方法，此方法的優(yōu)點(diǎn)是不假設(shè)路邊沿是直線或參數(shù)化曲線、不作直(曲)線擬合，完全借助當(dāng)前截面的點(diǎn)云分布分析確定路寬與限高，所以計(jì)算量小、速度快，缺點(diǎn)是對沿路方向的關(guān)聯(lián)信息利用不足、對路的結(jié)構(gòu)化(即路沿的凸起性)依賴較大。

圖5 上方有線纜和橋梁限高的路段Fig.5 Limited above by cables and bridge

3.2 拐彎半徑計(jì)算

道路的拐彎半徑可以通過對路道軌跡曲線作圓擬合、求取圓半徑的方法得到。算法步驟如下：

1) 設(shè)定道路曲線擬合的每段路段長度為L(比如每25 m 擬合一個結(jié)果)。

2) 從慣導(dǎo)的位姿數(shù)據(jù)中取出載車行進(jìn)過程中各個時刻的位置，得到離散的載車軌跡序列。

3) 對載車原始軌跡進(jìn)行濾波，使軌跡變得平滑，抑制必然存在的位置噪聲對后續(xù)曲線擬合的影響。

4)對離散的軌跡點(diǎn)序列進(jìn)行等間距樣條插值，得到均勻分布的位置點(diǎn)序列。

5)用路段上的位置點(diǎn)序列(x，y)擬合一個圓，得圓半徑r和圓心位置(xc，yc)。為此，記圓的方程為：

展開后得：

記a=-2xc、b=-2yc、c=xc2+yc2-r2，則上式變成：

上式中(a，b，c)為待擬合參數(shù)，軌跡序列(x，y)已知，故上式是關(guān)于(a，b，c)的線性超定方程，可用最小二乘法求解，并進(jìn)而得圓心位置(xc，yc)和圓半徑r：

圖6 是某段道路進(jìn)行曲線擬合和拐彎半徑分析的結(jié)果，其中綠色粗實(shí)線路段是拐彎半徑小于80 m 的路段，以醒目方式標(biāo)示，可以提醒駕駛?cè)藛T小心駕駛，這就是輔助駕駛的意義所在。表1 給出了該路段上部分位置處拐彎半徑的定量值，可以看出，自然路段的拐彎半徑一般都在幾十米量級。

圖6 多彎路段的曲線擬合Fig.6 Curve fitting on winding road

表1 曲線路段拐彎半徑r 的定量計(jì)算結(jié)果Table 1 Calculation of turnaround radius along curve road

3.3 對比實(shí)驗(yàn)

為了進(jìn)行對比實(shí)驗(yàn)，本文選取了一段有4 個拐彎點(diǎn)的軌跡進(jìn)行拐彎半徑擬合，如圖7(a)所示。載車從①點(diǎn)處出發(fā)，依次經(jīng)過并在②、③、④號點(diǎn)處拐彎。表2 給出了各個拐彎點(diǎn)處用本算法算得的拐彎半徑。部分點(diǎn)處其實(shí)不是一個單一的拐彎圓，比如，圖7(b)對4 號點(diǎn)進(jìn)行了放大，該路段明顯呈現(xiàn)兩個拐彎圓，算法很有效地?cái)M合出了這兩個結(jié)果，如表2 所示。

圖7 四拐彎軌跡的拐彎半徑擬合Fig.7 Turn radius fit along a 4-turn road

表2 專業(yè)軟件方法與本方法擬合拐彎半徑的結(jié)果對比Table 2 Comparison of radius fit between professional software and this method

作為對比，用專業(yè)的激光點(diǎn)云數(shù)據(jù)處理軟件Terrasolid 也進(jìn)行了拐彎半徑擬合，結(jié)果也列于表2中。由于Terrasolid 是一種半自動、交互式的點(diǎn)云處理軟件，其結(jié)果依賴于人工框選的軌跡弧段，所以效率并不高、結(jié)果也不穩(wěn)定。從表2 可以看出，使用Terrasolid 需嘗試多次量測，才能得到與本算法相近的結(jié)果。圖8(a)也展示了這種多次測量的不穩(wěn)定性(同一個點(diǎn)處，一次擬合得白色圓[小圓]，另一次擬合得綠色圓[大圓]，差別明顯)，而本文算法不僅過程自動，而且結(jié)果也符合直觀感受，如圖8(b)所示，這說明本文方法確有一定優(yōu)勢或好處。

圖8 3 號點(diǎn)處的拐彎半徑的擬合Fig.8 radius fit at 3rd turn

4 結(jié)論

本文從特定場景輔助駕駛的需求出發(fā)，設(shè)計(jì)一套基于三維激光雷達(dá)點(diǎn)云的道路場景參數(shù)計(jì)算方法，包括道路寬度、限高、拐彎半徑等，為判斷特定車輛的可通過性提供依據(jù)。經(jīng)測試，并與Terrasolid專業(yè)軟件對比，本方法表現(xiàn)良好，精度很高。

進(jìn)一步的改進(jìn)包括：對路面障礙物的自動檢測與識別、對鄉(xiāng)村非結(jié)構(gòu)化道路的參數(shù)提取，以進(jìn)一步提高此系統(tǒng)和方法的適用范圍和實(shí)用性。