散文網(wǎng) » 生活 »日常 » 多激光雷達(dá)外參?動化標(biāo)定算法及代碼實例

多激光雷達(dá)外參?動化標(biāo)定算法及代碼實例

2021-04-10 16:20 作者:深藍(lán)學(xué)院 0人讀過 | 我要投稿

激光雷達(dá)是?前?動駕駛系統(tǒng)中的核?傳感器之?，但是由于其信息密度低、存在垂直盲區(qū)等問題，?商?多在其L4級?動駕駛系統(tǒng)中搭配多組激光雷達(dá)，下圖為通?（Cruise）的?動駕駛汽?，采?了多激光雷達(dá)以彌補(bǔ)lidar+camera的不?，使?多激光雷達(dá)進(jìn)?環(huán)境感知的前提是對各雷達(dá)的外參進(jìn)?精準(zhǔn)的標(biāo)定，本?介紹?種基于NDT算法的?動多激光雷達(dá)標(biāo)定技術(shù)，并且給出了代碼實例以及測試數(shù)據(jù)（rosbag）供讀者實踐。

代碼和測試數(shù)據(jù)（rosbag）??末鏈接，考慮到?家下載的困難，本例的bag做了處理只有300多MB.

NDT算法詳解

我的??駕駛汽?系統(tǒng)??系列博客的第?三篇有簡要介紹過NDT算法，本?給出更加詳細(xì)全?的介紹。

傳感器外參標(biāo)定本質(zhì)上是獲得兩個傳感器的位移量（ $x%2Cy%2Cz$ ）和旋轉(zhuǎn)量（ $roll%2Cpitch%2Cyaw$ ），三維空間中可以??個?次變換矩陣（Homogeneous transformation matrix）來描述這樣的變換關(guān)系，在三維數(shù)據(jù)處理領(lǐng)域，點云配準(zhǔn)（Point Cloud Registration）就是?于處理兩個點云間位姿匹配問題的?類x,y,zroll,pitch,yaw?法，其中NDT(Normal Distribution Transform，正態(tài)分布變換)和ICP(Iterative Closest Point,迭代最近點算法)是其中的代表。本?主要基于NDT算法介紹多激光雷達(dá)外參標(biāo)定的?法和實踐。

?次變換矩陣：描述兩個坐標(biāo)系間的平移和旋轉(zhuǎn)變換關(guān)系的 $4%5Ctimes%204$ 矩陣，給定兩個坐標(biāo)系的平移量 $(x_%7Bt%7D%20%2Cy_%7Bt%7D%20%2Cz_%7Bt%7D)$ 和歐拉? $R(%5Calpha%20%2C%5Cbeta%2C%20%5Cgamma%20)$ ，?個3D的?次變換矩陣寫作：

其中， $%5Calpha%20$ 為 $yaw$ ， $%5Cbeta%20$ 為 $pitch$ ， $%5Cgamma%20$ 為 $roll$ 。

正態(tài)分布變換

正態(tài)分布變換是?種?正態(tài)分布函數(shù)來描述?個體素?格（voxel）內(nèi)的點的?法，令 $P%3D%5Cleft%5C%7B%20p_%7Bi%7D%20%5Cvert%20i%3D0%2C1%2C...%2Ct-1%20%5Cright%5C%7D%20%3D%5Cleft%5C%7B%20(x_%7Bi%7D%20%2Cy_%7Bi%7D%20%2Cz_%7Bi%7D%20%5Cvert%20i%3D0%2C1%2C...%2Ct-1%20%20%5Cright%5C%7D%20$ 表??個包含個點的點云，NDT算法?先使?三維的?格（也稱為體素）將點云進(jìn)?劃分，如下圖所?，我們稱這類體素為ND體素：

假定ND體素 $k$ 中包含有 $m$ 個點，那么這個ND體素中所有點的均值 $%5Cmu%20_%7Bk%7D%20$ 和協(xié)?差矩陣 $%5CSigma%20_%7Bk%7D%20$ 計算公式為:

其中 $p_%7Bki%7D%20$ 為ND體素 $k$ 中的點 $i$ ，即 $p_%7Bki%7D%3D(x_%7Bki%7D%2Cy_%7Bki%7D%2Cz_%7Bki%7D)%20$ ，那么該ND體素內(nèi)點的概率密度函數(shù) $f(k)$ 可以表?為：

按照此?法，可以將整個?標(biāo)點云使?ND體素進(jìn)?劃分，并且計算每?個ND體素內(nèi)的正態(tài)分布參數(shù)。

NDT匹配的準(zhǔn)確度和ND體素格的劃分尺?相關(guān)，采?的ND體素尺?越?，相應(yīng)的NDT匹配精度也會越?，所以在利?NDT算法進(jìn)?掃描匹配定位時需要對匹配精度和算法實時性間進(jìn)?取舍，但是在本例中，我們使?NDT進(jìn)?多激光雷達(dá)標(biāo)定為離線任務(wù)，所以可以將ND Voxel設(shè)置相對??些以提?最終的精度。

題外話：在利?NDT繼續(xù)激光雷達(dá)掃描匹配定位中，考慮到城市道路場景下地圖的尺?通常較?，采??密度的地圖尺?進(jìn)?劃分將造成內(nèi)存占?偏?的問題，并且使得匹配的計算量增?，?法滿?定位的實時性要求。所以在?動駕駛定位的場景中，通常使?較?尺?的ND體素格配合?密度的點云地圖來平衡NDT匹配定位的實時性和準(zhǔn)確性。

NDT匹配參數(shù)

NDT匹配定位算法定義了?些匹配參數(shù)，通過這些匹配參數(shù)可以將輸?點云進(jìn)?三維坐標(biāo)變換，得到輸?點云到?標(biāo)點云中的變換關(guān)系，注意由于本?討論的是激光雷達(dá)的外參標(biāo)定，所以?標(biāo)點云實際上就是?標(biāo)激光雷達(dá)（ROS TF中的parent frame），輸?點云就是要標(biāo)定到?標(biāo)激光雷達(dá)坐標(biāo)系下的激光雷達(dá)的點云（ROS TF中的child frame）。NDT算法中?共有兩組配置參數(shù)，分別是旋轉(zhuǎn)參數(shù)向量 $(%5Calpha%2C%5Ctheta%20%20%20%2C%5Cgamma%20)%5ET%20$ 和平移參數(shù)向量 $(d_%7Bx%7D%20%2Cd_%7By%7D%2Cd_%7Bz%7D)%5ET%20$ ,旋轉(zhuǎn)參數(shù)向量表?變換后的點云相對當(dāng)前輸?點云在姿態(tài)?上的旋轉(zhuǎn)量，平移參數(shù)向量表?在(x,y,z)三個?向的平移量。那么對于輸?點云??，每個點進(jìn)?三維坐標(biāo)轉(zhuǎn)換的計算公式如下：

? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?? $p'_%7Bi%7D%20%3DR%20p_%7Bi%7D%20%2Bd$

其中為旋轉(zhuǎn)矩陣，其計算公式為：

所以對于NDT匹配算法??，配準(zhǔn)的?標(biāo)就是找到?組三維坐標(biāo)系變換參數(shù)向量，使得輸?點云在經(jīng)過這組參數(shù)指定的旋轉(zhuǎn)和平移之后，能夠和?標(biāo)點云擬合。我們稱參數(shù)向量 $%5Ctheta%20$ 為NDT匹配參數(shù)，接下來，將介紹如何搜索找到這?組參數(shù)。

NDT掃描匹配算法

NDT算法?先對?標(biāo)點云進(jìn)?正態(tài)分布變換，得到?標(biāo)點云的所有ND體素，接著需要輸?當(dāng)前輸?點云的初始位姿（x,y,z,roll,pitch,yaw），將此位姿作為初始搜索位置。在傳感器標(biāo)定中，這個初始變換位姿也就是我們對于多個激光雷達(dá)平移量和旋轉(zhuǎn)量的粗略估計，可以通過簡單的測量得到（精度不需要太?，我們最終將通過NDT算法得到?精度的結(jié)果）。

初始位姿是對初始點云在?標(biāo)點云中的位姿的估計，這個估計值可以幫助NDT算法中的參數(shù)優(yōu)化?法迅速收斂。得到初始估計后，開始搜索?組最優(yōu)的NDT匹配參數(shù)，使得輸?點云在通過這組參數(shù)變換后和?標(biāo)點云擬合度最?。使?如下公式來描述兩個點云的擬合度：

其中， $p'_%7Bi%7D%20$ 是輸?點云在經(jīng)過三維坐標(biāo)變換參數(shù) $%5Cvartheta%20$ 轉(zhuǎn)換后的點， $%5Cmu%20_%7Bi%7D%20$ 是與該輸?點相對應(yīng)的?標(biāo)點云ND體素格的均值， $%5CSigma%20_%7Bi%7D%20$ 是相應(yīng)的ND體素內(nèi)的協(xié)?差矩陣。擬合度 $Fitness(P%2C%5Ctheta%20)$ 數(shù)值越?說明輸?點云和?標(biāo)點云在該位置越匹配，通過搜索參數(shù)向量 $%5Ctheta%20$ 得到?的擬合度，這是?個典型的?線性最優(yōu)化問題，NDT算法使??頓法來求解最優(yōu)參數(shù)。?頓法是?種最?化?標(biāo)函數(shù)的?法，所以?標(biāo)函數(shù) $f(%5Ctheta%20)$ 為：

? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?? $f(%5Ctheta)%3D%20-Fitness(P%2C%5Ctheta%20)$

其中 $%5Ctheta%20%3D(%5Calpha%20%2C%5Ctheta%20%2C%5Cgamma%2Cd_%7Bx%7D%20%2Cd_%7By%7D%20%2Cd_%7Bz%7D)%5ET%20$ ，是輸?點云到?標(biāo)點云的三維坐標(biāo)轉(zhuǎn)換參數(shù)。通過迭代?頓法，不斷調(diào)整 $%5Ctheta%20$ 向量，使得 $f(%5Ctheta%20)$ ?于?個閾值，則稱NDT參數(shù)優(yōu)化已經(jīng)收斂，根據(jù)此時的變換參數(shù)向量 $%5Ctheta%20$ 即可確定輸?點云在?標(biāo)點云中的姿態(tài)，也就是lidar A到lidar B的TF關(guān)系。

多激光雷達(dá)標(biāo)定代碼實例

在理解NDT算法后，我們知道這類基于點云配準(zhǔn)的多激光雷達(dá)標(biāo)定?法需要給?個初始姿態(tài)估計，這個初始估計要求的精度不?，可以簡單的測量甚?估計得到，下載?末提供的rosbag和代碼倉庫，通過rosbag info我們發(fā)現(xiàn)該bag包含以下數(shù)據(jù)：

這個bag包含了6個激光雷達(dá)的數(shù)據(jù)，我們選取其中?個Lidar作為主要的坐標(biāo)系（TF中的parent frame），建?起其他5個lidar到它的TF變換關(guān)系，就完成了6激光雷達(dá)的外參標(biāo)定。選取/hesai/front_high作為parent frame，我簡單的?測估計了其他5顆激光雷達(dá)到該雷達(dá)的姿態(tài)變換關(guān)系并且寫到了項?的cfg/child_topic_list?件中：

第??是child frame的個數(shù)（即其他激光雷達(dá)的個數(shù)）。之后依次是每個lidar的點云數(shù)據(jù)topic名稱以及到主雷達(dá)變換的初始估計（x,y,z,yaw,pitch,roll），為了讓讀者感受到NDT算法收斂的過程，我有意將初始估計做的?常差（平移量存在1?左右的誤差，旋轉(zhuǎn)量普遍存在20-30度的誤差）。接著是launch?件中的參數(shù)：

這?的points_parent_src即主雷達(dá)的topic，points_child_src是這?次標(biāo)定的雷達(dá)的topic，每執(zhí)??次程序可以標(biāo)定兩顆雷達(dá)的外參，對于本例??，執(zhí)?5次程序即可?動完成所有雷達(dá)的標(biāo)定，接著是NDT算法的配置參數(shù)：

voxel_size：輸?點云的降采樣尺度，在執(zhí)?NDT算法之前我們通常會對輸?點云進(jìn)?預(yù)處理，如體素降采樣，以加速計算;
ndt_epsilon：該參數(shù)定義了搜索的最?變化量，該參數(shù)尺度過?會造成最終的收斂不穩(wěn)定，過?容易造成收斂速度過慢的問題;
ndt_resolution：?標(biāo)點云的ND體素的尺?，單位為?;
ndt_iterations：使??頓法優(yōu)化的迭代次數(shù)，迭代次數(shù)越多計算量越?。

理解好參數(shù)以后簡要的過?下代碼，本?實例主要基于PCL庫，在主函數(shù)中，我?了?個頻率為10Hz的loop：

我們沒有通過callback來調(diào)?代碼邏輯，?是采?了固定的循環(huán)，這樣我們的rosbag就不需要很?（哪怕每個topic只包含?條消息也能收斂），在InitializeROSIo函數(shù)中主要初始化初始估計以及NDT參數(shù)，并且使?message_filters同步兩個topic的消息，代碼如下：

對應(yīng)的回調(diào)PointsCallback中我們并沒有寫代碼的業(yè)務(wù)邏輯，僅僅只是獲取點云數(shù)據(jù)并保存于內(nèi)部成員變量中，并對輸?點云做了Voxel Grid降采樣：

點云的配準(zhǔn)我們寫在了循環(huán)中的PerformNdtOptimize()函數(shù)中,?先判定成員變量是否已經(jīng)接受到點云消息,接著初始化NDT的參數(shù)、輸?點云和?標(biāo)定義：

如果沒有初始估計，則從配置?件中讀取位移量和歐拉?換算成?次變換矩陣：

執(zhí)?NDT點云配準(zhǔn)，得到新的變換矩陣，基于新的變換矩陣對輸?點云進(jìn)?位姿調(diào)整，并且發(fā)布該debug點云以在rviz中可視化：

根據(jù)求得的?次變換矩陣換算TF，使?TF Broadcaster發(fā)布該TF：

使?測試數(shù)據(jù)實踐6激光雷達(dá)標(biāo)定

下載?末的數(shù)據(jù)和代碼，將代碼cp到你的ros workspace的src?錄下，編譯：

catkinbuild

提?：你需要安裝catkin_tools才能夠使?catkinbuild指令進(jìn)?編譯，你可以選擇：

安裝catkin_tools：

sudoapt-getinstallpython-catkin-tools

或者使?catkin_make進(jìn)?編譯。

編譯完成后啟動roscore:

roscore

在另?個終端中啟動rviz（使?我提供的rviz配置?件）：

cd src/multi_lidar_calibration

rviz-drviz/multi_lidar_calibration.rviz

運(yùn)? source devel/setup.bash?

roslaunch multi_lidar_calibrator multi_lidar_calibrator.launch

rosbag play multi_lidar.bag

通過Rviz可以看到?概?時不到5秒中左右，兩顆激光雷達(dá)即標(biāo)定完成，如下圖：

并且可以得到相應(yīng)的TF關(guān)系，如下圖：

程序的輸出：

rostopic echo /tf的結(jié)果：

將收斂后程序的輸出結(jié)果中的1.00938 -0.478343 -0.442721 1.36447 0.0686235 -0.080712/fh/lf10拷?到?件launch/tf.launch中的args中，我們可以通過rostf中的static_transform_publisher將6顆激光雷達(dá)的數(shù)據(jù)融合在?個坐標(biāo)系下。

修改multi_lidar_calibrator.launch中的points_child_src名稱為其他data的topic，執(zhí)?程序5次即可完成所有l(wèi)idar的標(biāo)定，得到5組外參（?次矩陣、TF、平移量和四元數(shù)均能得到），將結(jié)果添加到tf.launch?件中后，我們就可以通過Rviz看到6顆激光雷達(dá)的標(biāo)定結(jié)果了，執(zhí)?tf.launch并運(yùn)?bag：

roslaunch multi_lidar_calibrator tf.launch

rosbag play multi_lidar.bag-l--clock

結(jié)果如下：

作者簡介：

申澤邦，奔馳高級自動駕駛工程師，Google Developer Expert（機(jī)器學(xué)習(xí)方向），蘭州大學(xué)自動駕駛團(tuán)隊創(chuàng)始人，CSDN博客專家，《無人駕駛原理與實踐》書籍作者。

代碼倉庫：https://github.com/AbangLZU/multi_lidar_calibration

rosbag下載鏈接：https://pan.baidu.com/s/1Ny3Ji9aF1ZTjgklhxPvDbg提取碼:pemc

標(biāo)簽：