多光學雷達外參⾃動化標定演算法及程式碼實例

光學雷達是⽬前⾃動駕駛系統中的核⼼感測器之⼀，但是由於其資訊密度低、存在垂直盲區等問題，⼚商⼤多在其L4級⾃動駕駛系統中搭配多組光學雷達，下圖為通⽤（Cruise）的⾃動駕駛汽⻋，采⽤了多光學雷達以彌補lidar+camera的不⾜，使⽤多光學雷達進⾏環境感知的前提是對各雷達的外參進⾏精準的標定，本⽂介紹⼀種基於NDT演算法的⾃動多光學雷達標定技術，並且給出了程式碼實例以及測試數據（rosbag）供讀者實踐。

图片
Cruise的多光學雷達感測器配置

NDT演算法詳解

感測器外參標定本質上是獲得兩個感測器的位移量（x，y，z ）和旋轉量（roll，pitch，yaw ），三維空間中可以⽤⼀個⻬次變換矩陣（Homogeneous transformation matrix）來描述這樣的變換關係，在三維數據處理領域，點雲配准（Point Cloud Registration）就是⽤於處理兩個點雲間位姿匹配問題的⼀類x, y, z roll, pitch, yaw⽅法，其中NDT(Normal Distribution Transform，正態分布變換)和ICP(Iterative Closest Point, 迭代最近點演算法)是其中的代表。本⽂主要基於NDT演算法介紹多光學雷達外參標定的⽅法和實踐。

⻬次變換矩陣：描述兩個坐標系間的平移和旋轉變換關係的4×4矩陣，給定兩個坐標系的平移量(xt, yt , zt )和歐拉⻆R(α, β, γ) ，⼀個3D的⻬次變換矩陣寫作：
图片

其中，α 為yaw ，β 為pitch ， γ為roll 。

正態分布變換
正態分布變換是⼀種⽤正態分布函數來描述⼀個體素⽹格（voxel）內的點的⽅法，令P = {pi∣i =0, 1, ..., t − 1} = {(xi, yi , zi )∣i = 0, 1, ..., t − 1}表示⼀個包含 個點的點雲，NDT演算法⾸先使⽤三維的⽹格（也稱為體素）將點雲進⾏劃分，如下圖所⽰，我們稱這類體素為ND體素：
图片

假定ND體素k中包含有m個點，那麼這個ND體素中所有點的均值μk和協⽅差矩陣Σk計算公式為:

图片

其中pki為ND體素k中的點i，即图片，那麼該ND體素內點的概率密度函數f(k)可以表示為：

图片
按照此⽅法，可以將整個⽬標點雲使⽤ND體素進⾏劃分，並且計算每⼀個ND體素內的正態分布參數。

NDT匹配的準確度和ND體素格的劃分尺⼨相關，采⽤的ND體素尺⼨越⼩，相應的NDT匹配精度也會越⾼，所以在利⽤NDT演算法進⾏掃描匹配定位時需要對匹配精度和演算法實時性間進⾏取捨，但是在本例中，我們使⽤NDT進⾏多光學雷達標定為離線任務，所以可以將ND Voxel設置相對⼩⼀些以提⾼最終的精度。

題外話：在利⽤NDT繼續光學雷達掃描匹配定位中，考慮到城市道路場景下地圖的尺⼨通常較⼤，采⽤⾼密度的地圖尺⼨進⾏劃分將造成記憶體占⽤偏⾼的問題，並且使得匹配的計算量增⼤，⽆法滿⾜定位的實時性要求。所以在⾃動駕駛定位的場景中，通常使⽤較⼤尺⼨的ND體素格配合⾼密度的點雲地圖來平衡NDT匹配定位的實時性和準確性。
NDT匹配參數
NDT匹配定位演算法定義了⼀些匹配參數，通過這些匹配參數可以將輸⼊點雲進⾏三維坐標變換，得到輸⼊點雲到⽬標點雲中的變換關係，注意由於本⽂討論的是光學雷達的外參標定，所以⽬標點雲實際上就是⽬標光學雷達（ROS TF中的parent frame），輸⼊點雲就是要標定到⽬標光學雷達坐標系下的光學雷達的點雲（ROS TF中的child frame）。NDT演算法中⼀共有兩組配置參數，分別是旋轉參數向量(α, θ, γ)T和平移參數向量图片,旋轉參數向量表⽰變換後的點雲相對當前輸⼊點雲在姿態⻆上的旋轉量，平移參數向量表⽰在(x, y, z)三個⽅向的平移量。那麼對於輸⼊點雲⽽⾔，每個點進⾏三維坐標轉換的計算公式如下：
图片
其中R為旋轉矩陣，其計算公式為：
图片
所以對於NDT匹配演算法⽽⾔，配準的⽬標就是找到⼀組三維坐標系變換參數向量，使得輸⼊點雲在經過這組參數指定的旋轉和平移之後，能夠和⽬標點雲擬合。我們稱參數向量為NDT匹配參數，接下來，將介紹如何搜索找到這⼀組參數。

NDT掃描匹配演算法

NDT演算法⾸先對⽬標點雲進⾏正態分布變換，得到⽬標點雲的所有ND體素，接著需要輸⼊當前輸⼊點雲的初始位姿（x, y, z, roll, pitch, yaw），將此位姿作為初始搜索位置。在感測器標定中，這個初始變換位姿也就是我們對於多個光學雷達平移量和旋轉量的粗略估計，可以通過簡單的測量得到（精度不需要太⾼，我們最終將通過NDT演算法得到⾼精度的結果）。

初始位姿是對初始點雲在⽬標點雲中的位姿的估計，這個估計值可以幫助NDT演算法中的參數優化⽅法迅速收斂。得到初始估計後，開始搜索⼀組最優的NDT匹配參數，使得輸⼊點雲在通過這組參數變換後和⽬標點雲擬合度最⾼。使⽤如下公式來描述兩個點雲的擬合度：
图片

其中，图片是輸⼊點雲在經過三維坐標變換參數θ轉換後的點，μi是與該輸⼊點相對應的⽬標點雲ND體素格的均值，Σi是相應的ND體素內的協⽅差矩陣。擬合度F itness(P , θ)數值越⼤說明輸⼊點雲和⽬標點雲在該位置越匹配，通過搜索參數向量 得到⼤的擬合度，這是⼀個典型的⾮線性最優化問題，NDT演算法使⽤⽜頓法來求解最優參數。⽜頓法是⼀種最⼩化⽬標函數的⽅法，所以⽬標函數f (θ)為：

图片

其中θ = (α, θ, γ, dx, dy , dz )T，是輸⼊點雲到⽬標點雲的三維坐標轉換參數。通過迭代⽜頓法，不斷調整θ向量，使得f (θ)⼩於⼀個閾值，則稱NDT參數優化已經收斂，根據此時的變換參數向量θ即可確定輸⼊點雲在⽬標點雲中的姿態，也就是lidar A到lidar B的TF關係。

多光學雷達標定程式碼實例

在理解NDT演算法後，我們知道這類基於點雲配準的多光學雷達標定⽅法需要給⼀個初始姿態估計，這個初始估計要求的精度不⾼，可以簡單的測量甚⾄估計得到，下載後台提供的rosbag和程式碼倉庫，通過rosbag info我們發現該bag包含以下數據：

图片

這個bag包含了6個光學雷達的數據，我們選取其中⼀個Lidar作為主要的坐標系（TF中的parentframe），建⽴起其他5個lidar到它的TF變換關係，就完成了6光學雷達的外參標定。選取 /hesai/front_high 作為parent frame，我簡單的⽬測估計了其他5顆光學雷達到該雷達的姿態變換關係並且寫到了項⽬的 cfg/child_topic_list ⽂件中：

图片

第⼀⾏是child frame的個數（即其他光學雷達的個數）。之後依次是每個lidar的點雲數據topic名稱以及到主雷達變換的初始估計（x, y, z, yaw, pitch, roll），為了讓讀者感受到NDT演算法收斂的過程，我有意將初始估計做的⾮常差（平移量存在1⽶左右的誤差，旋轉量普遍存在20-30度的誤差）。接著是launch⽂件中的參數：

图片

這⾥的 points_parent_src 即主雷達的topic， points_child_src 是這⼀次標定的雷達的topic，每執⾏⼀次程式可以標定兩顆雷達的外參，對於本例⽽⾔，執⾏5次程式即可⾃動完成所有雷達的標定，接著是NDT演算法的配置參數：

  • voxel_size ：輸⼊點雲的降取樣尺度，在執⾏NDT演算法之前我們通常會對輸⼊點雲進⾏預處理，如體素降取樣，以加速計算;

  • ndt_epsilon ：該參數定義了搜索的最⼩變化量，該參數尺度過⼤會造成最終的收斂不穩定，過⼩容易造成收斂速度過慢的問題;

  • ndt_resolution ：⽬標點雲的ND體素的尺⼨，單位為⽶;

  • ndt_iterations ：使⽤⽜頓法優化的迭代次數，迭代次數越多計算量越⼤。

理解好參數以後簡要的過⼀下程式碼，本⽂實例主要基於PCL庫，在主函數中，我⽤了⼀個頻率為10Hz的loop：

图片

我們沒有通過callback來調⽤程式碼邏輯，⽽是采⽤了固定的循環，這樣我們的rosbag就不需要很⼤（哪怕每個topic只包含⼀條消息也能收斂），在 InitializeROSIo 函數中主要初始化初始估計以及NDT參數，並且使⽤message_filters同步兩個topic的消息，程式碼如下：

图片

對應的回調 PointsCallback 中我們並沒有寫程式碼的業務邏輯，僅僅只是獲取點雲數據並保存於內部成員變數中，並對輸⼊點雲做了Voxel Grid降取樣：

图片

點雲的配准我們寫在了循環中的 PerformNdtOptimize() 函數中,⾸先判定成員變數是否已經接受到點雲消息,接著初始化NDT的參數、輸⼊點雲和⽬標定義：

图片

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

图片

執⾏NDT點雲配准，得到新的變換矩陣，基於新的變換矩陣對輸⼊點雲進⾏位姿調整，並且發布該debug點雲以在rviz中可視化：

图片

根據求得的⻬次變換矩陣換算TF，使⽤TF Broadcaster發布該TF：

图片

使⽤測試數據實踐6光學雷達標定
下載後台的數據和程式碼，將程式碼cp到你的ros workspace的src⽬錄下，編譯：
catkin build

提⽰：你需要安裝catkin_tools才能夠使⽤ catkin build 指令進⾏編譯，你可以選擇：

1. 安裝catkin_tools：

sudo apt-get install Python-catkin-tools

2. 或者使⽤ catkin_make 進⾏編譯。

編譯完成後啟動roscore:

roscore

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

cd src/multi_lidar_calibration

rviz -d rviz/multi_lidar_calibration.rviz

運⾏

source devel/setup.bash
roslaunch multi_lidar_calibrator multi_lidar_calibrator.launch
rosbag play multi_lidar.bag

通過Rviz可以看到⼤概⽤時不到5秒中左右，兩顆光學雷達即標定完成，如下圖：

图片

並且可以得到相應的TF關係，如下圖：

图片

程式的輸出：

图片
rostopic echo /tf 的結果：

图片

將收斂後程式的輸出結果中的1.00938 -0.478343 -0.442721 1.36447 0.0686235 -0.080712 /fh /lf 10 拷⻉到 ⽂件launch/tf.launch中的args中，我們可以通過ros tf中的static_transform_publisher將6顆光學雷達的 數據融合在⼀個坐標系下。

修改multi_lidar_calibrator.launch中的points_child_src名稱為其他data的topic，執⾏程式5次即可完成所有 lidar的標定，得到5組外參（⻬次矩陣、TF、平移量和四元數均能得到），將結果添加到tf.launch⽂件 中後，我們就可以通過Rviz看到6顆光學雷達的標定結果了，執⾏tf.launch並運⾏bag：

roslaunch multi_lidar_calibrator tf.launch

rosbag play multi_lidar.bag -l --clock 

結果如下：
图片
图片
图片
作者簡介：

申澤邦，Benz高級自動駕駛工程師，Google Developer Expert（機器學習方向），蘭州大學自動駕駛團隊創始人，CSDN部落格專家，《無人駕駛原理與實踐》書籍作者。

Tags:

VirMach 便宜 VPS