多激光雷达外参⾃动化标定算法及代码实例

2年前浏览4447

激光雷达是⽬前⾃动驾驶系统中的核⼼传感器之⼀，但是由于其信息密度低、存在垂直盲区等问题，⼚商⼤多在其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

结果如下：