基于已知点云地图的NDT的激光SLAM定位

概述

对于L4级自动驾驶系统而言，定位模块通常会融合GNSS、IMU、轮速计（车辆底盘）、摄像头以及激光雷达odometry多种测量，使用滤波算法（EKF、UKF等）以获得平滑、厘米级别的绝对定位，其中基于高精度点云地图和激光雷达的配准定位（Lidar Odometry）因其精度高、可靠性好，在整个融合定位中通常占很大的权重，是自动驾驶定位系统中相对可靠的“绝对定位”数据来源。

依据网上已开源的算法框架以及各位大佬们的经验分享，本文demo为ROS系统下，基于建图算法（SC-LEGO-LOAM）+点云匹配算法（NDT）进行定位功能实现。

点云地图

自动驾驶汽车的激光雷达定位通常依赖于提前离线构建好的高精度点云地图，之所以这么做原因有以下几个方面：

L4级别以上自动驾驶系统对定位精度和稳定性要求很高，绝对误差需要控制在20cm以内；
纯SLAM目前来说无法达到自动驾驶对于定位精度、可靠性的要求，即我们现在的研究很难实现自动驾驶车的在线制图和定位（问题包括闭环优化，全局优化，误差累计修正等等）
高精度地图制造商的完整生产流程需要较大的算力和人工，他们能够生产非常理想的点云地图和语义地图，但是需要离线生产（时间和人力）；
利用高精度地图可以相对简单地实现激光雷达定位，在融合了IMU和轮速计以后这类定位方法的精度和可靠性基本满足自动驾驶汽车定位的需求。

所以综合以上的客观原因，目前的L4和大部分L3自动驾驶系统定位模块仍然是以事先构建的高精度地图为基础进行的配准定位，这个配准使用的传感器，少数厂商使用的是camera(如mobileeye)，绝大多数厂商目前仍然采用的是激光雷达配准思路。

点云地图就是激光雷达配准所需事先构建的“用来定位的地图”。

我们使用了LEGO-LOAM和SC-LEGO-LOAM的算法分别进行了点云地图构建，对于面积较大的点云地图，明显采用Scan Context方法对点云地图进行闭环检测和姿态图优化后闭环效果更好，所以在这里采用SC-LEGO-LOAM构建的点云地图进行下一步的点云匹配定位。

将建好的地图放入map文件夹下项目中的map_loader节点主要用于载入地图：

MapLoader::MapLoader(ros::NodeHandle &nh){std::string pcd_file_path, map_topic;nh.param<std::string>("pcd_path", pcd_file_path, "");nh.param<std::string>("map_topic", map_topic, "point_map");init_tf_params(nh);pc_map_pub_ = nh.advertise<sensor_msgs::PointCloud2>(map_topic, 10, true);file_list_.push_back(pcd_file_path);auto pc_msg = CreatePcd();auto out_msg = TransformMap(pc_msg);if (out_msg.width != 0) {out_msg.header.frame_id = "map";pc_map_pub_.publish(out_msg);}}

构造函数中读取pcd文件的路径和map topic，并且初始化map的变换参数（如果不需要对map进行变换，则数值都设置为０）

void MapLoader::init_tf_params(ros::NodeHandle &nh){nh.param<float>("x", tf_x_, 0.0);nh.param<float>("y", tf_y_, 0.0);nh.param<float>("z", tf_z_, 0.0);nh.param<float>("roll", tf_roll_, 0.0);nh.param<float>("pitch", tf_pitch_, 0.0);nh.param<float>("yaw", tf_yaw_, 0.0);ROS_INFO_STREAM("x" << tf_x_ <<"y: "<<tf_y_<<"z: "<<tf_z_<<"roll: "<<tf_roll_<<" pitch: "<< tf_pitch_<<"yaw: "<<tf_yaw_);
}

函数CreatePcd()用于加载pcd，TransformMap()用于平移和旋转地图，我们使用Eigen和pcl::transformPointCloud()实现点云的变换：

sensor_msgs::PointCloud2 MapLoader::TransformMap(sensor_msgs::PointCloud2 & in){pcl::PointCloud<pcl::PointXYZ>::Ptr in_pc(new pcl::PointCloud<pcl::PointXYZ>);pcl::fromROSMsg(in, *in_pc);pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_pc_ptr(new pcl::PointCloud<pcl::PointXYZ>);Eigen::Translation3f tl_m2w(tf_x_, tf_y_, tf_z_);                 // tl: translationEigen::AngleAxisf rot_x_m2w(tf_roll_, Eigen::Vector3f::UnitX());  // rot: rotationEigen::AngleAxisf rot_y_m2w(tf_pitch_, Eigen::Vector3f::UnitY());Eigen::AngleAxisf rot_z_m2w(tf_yaw_, Eigen::Vector3f::UnitZ());Eigen::Matrix4f tf_m2w = (tl_m2w * rot_z_m2w * rot_y_m2w * rot_x_m2w).matrix();pcl::transformPointCloud(*in_pc, *transformed_pc_ptr, tf_m2w);SaveMap(transformed_pc_ptr);sensor_msgs::PointCloud2 output_msg;pcl::toROSMsg(*transformed_pc_ptr, output_msg);return output_msg;
}

输入点云降采样

NDT算法优化的目标函数主要是输入点云和目标点云概率分布的相似性，这种配准算法的计算复杂度和两个要素正相关：

输入点云的点的密度
初始姿态估计的偏差

输入点云点越密集，NDT配准所需的计算复杂度就越大；
初始姿态估计越差（越偏离真实的姿态），相应的计算复杂度也越大，初始姿态过差的话NDT甚至无法收敛。
自动驾驶激光雷达定位对实时性有较高的要求，点云配准所用的时间显然越少越好，所以我们可以通过降采样输入点云以提高NDT配准的速度，本文中我们采用VoxelGrid降采样方法降低输入点云的密度，代码在项目的 voxel_grid_filter.cpp中，主要代码如下：

static void scan_callback(const sensor_msgs::PointCloud2::ConstPtr& input)
{pcl::PointCloud<pcl::PointXYZ> scan;pcl::fromROSMsg(*input, scan);if(measurement_range != MAX_MEASUREMENT_RANGE){scan = removePointsByRange(scan, 0, measurement_range);}pcl::PointCloud<pcl::PointXYZ>::Ptr scan_ptr(new pcl::PointCloud<pcl::PointXYZ>(scan));pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_scan_ptr(new pcl::PointCloud<pcl::PointXYZ>());sensor_msgs::PointCloud2 filtered_msg;// if voxel_leaf_size < 0.1 voxel_grid_filter cannot down sample (It is specification in PCL)if (voxel_leaf_size >= 0.1){// Downsampling the velodyne scan using VoxelGrid filterpcl::VoxelGrid<pcl::PointXYZ> voxel_grid_filter;voxel_grid_filter.setLeafSize(voxel_leaf_size, voxel_leaf_size, voxel_leaf_size);voxel_grid_filter.setInputCloud(scan_ptr);voxel_grid_filter.filter(*filtered_scan_ptr);pcl::toROSMsg(*filtered_scan_ptr, filtered_msg);}else{pcl::toROSMsg(*scan_ptr, filtered_msg);}filtered_msg.header = input->header;std::cout <<"frame.id"<<filtered_msg.header.frame_id << std::endl ;filtered_points_pub.publish(filtered_msg);}

在得到点云以后，首先对点云进行截取，我们只保留 MAX_MEASUREMENT_RANGE 距离以内的点用于定位（本文中MAX_MEASUREMENT_RANGE = 120 米），VoxelGrid降采样的主要参数就是voxel_leaf_size，该参数设定了降采样选取的立方体的边长（单位为米），在一个这样的立方体内只保留１个点，可以在 points_downsample.launch文件中配置该参数：

<arg name="leaf_size" default="3.0" />

如上所示，本文采用了３米的leaf size，这个参数可以根据你实际使用的激光雷达点的密度决定，虽然我们追求配准的实时性，但同时我们也不希望牺牲太多定位的精度，所以对输入点云降采样的度需要平衡实时性和定位精度，根据经验，如果你猜用的是16线的激光雷达，那么降采样的leaf size控制在１-２m较为合适，当采用的激光雷达为32线及以上,可以将leaf size设置为2-3m。
降采样后的点云将被输出至 /filtered_points 话题，以供后续的NDT配准定位使用。

使用NDT为自动驾驶车提供高精度定位

这部分代码主要实现在ndt.cpp中：

初始姿态获取
一切使用预先构建的地图进行配准定位的方法都需要提供初始姿态，在工业界的实践中，这一初始姿态通常是通过gnss获得，本文中我们简化这一步，在Rviz中手动指定初始姿态，Rviz中设定的初始姿态通常会被默认发送至/initialpose topic上，在NdtLocalizer构造函数中，写一个subscriber监听该topic：

initial_pose_sub_ = nh_.subscribe("initialpose", 100, &NdtLocalizer::callback_init_pose, this);

当有初始姿态（geometry_msgs::PoseWithCovarianceStamped）传来的时候，执行的是以下回调callback_init_pose：

void NdtLocalizer::callback_init_pose(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr & initial_pose_msg_ptr)
{if (initial_pose_msg_ptr->header.frame_id == map_frame_) {initial_pose_cov_msg_ = *initial_pose_msg_ptr;} else {// get TF from pose_frame to map_framegeometry_msgs::TransformStamped::Ptr TF_pose_to_map_ptr(new geometry_msgs::TransformStamped);get_transform(map_frame_, initial_pose_msg_ptr->header.frame_id, TF_pose_to_map_ptr);// transform pose_frame to map_framegeometry_msgs::PoseWithCovarianceStamped::Ptr mapTF_initial_pose_msg_ptr(new geometry_msgs::PoseWithCovarianceStamped);tf2::doTransform(*initial_pose_msg_ptr, *mapTF_initial_pose_msg_ptr, *TF_pose_to_map_ptr);// mapTF_initial_pose_msg_ptr->header.stamp = initial_pose_msg_ptr->header.stamp;initial_pose_cov_msg_ = *mapTF_initial_pose_msg_ptr;}// if click the initpose again, re init！init_pose = false;
}

在NDT配准中，我们主要关注四个坐标系间的变化，分别是：

世界坐标系（frame_id = world）
地图坐标系（frame_id = map）
车辆基础坐标系（frame_id = base_link）
激光雷达坐标系（本文中frame_id = ouster，根据你使用的激光雷达不同，frame id也会不一样）

在本项目中，我们使用static_tf.launch发布world 到 map以及ouster到base_link这两个固定变换：

<node pkg="tf2_ros" type="static_transform_publisher" name="localizer_to_base_link" args="0 0 1.753999 0.03 0.01 0.01 base_link rslidar"/>
<node pkg="tf2_ros" type="static_transform_publisher" name="world_to_map" args="0 0 0 0 0 0 map world" />

注：在这里我们假定map和world为同一坐标系以简化问题，在具体的自动驾驶系统研发中，你需要根据WGS84坐标系下的经纬度配合通用横轴墨卡托投影（Universal Transverse Mercator，UTM）以获得当前Map到世界坐标系的平移关系以及东北天（East North Up, ENU）坐标系下的旋转量。
localizer_to_base_link即激光雷达到base link的变换关系，是激光雷达的外参之一，也是一个静态变换。

回到初始姿态获取，得到Rviz上手动指定的初始姿态后，首先对坐标系进行统一，如果该pose是在地图坐标系，那么保存用于后续使用，如果是其他坐标系，则先将该pose转换至地图坐标系，通过函数 get_transform 获取变换关系，该函数定义如下：

bool NdtLocalizer::get_transform(const std::string & target_frame, const std::string & source_frame,const geometry_msgs::TransformStamped::Ptr & transform_stamped_ptr)
{if (target_frame == source_frame) {transform_stamped_ptr->header.stamp = ros::Time::now();transform_stamped_ptr->header.frame_id = target_frame;transform_stamped_ptr->child_frame_id = source_frame;transform_stamped_ptr->transform.translation.x = 0.0;transform_stamped_ptr->transform.translation.y = 0.0;transform_stamped_ptr->transform.translation.z = 0.0;transform_stamped_ptr->transform.rotation.x = 0.0;transform_stamped_ptr->transform.rotation.y = 0.0;transform_stamped_ptr->transform.rotation.z = 0.0;transform_stamped_ptr->transform.rotation.w = 1.0;return true;}try {*transform_stamped_ptr =tf2_buffer_.lookupTransform(target_frame, source_frame, ros::Time(0), ros::Duration(1.0));} catch (tf2::TransformException & ex) {ROS_WARN("%s", ex.what());ROS_ERROR("Please publish TF %s to %s", target_frame.c_str(), source_frame.c_str());transform_stamped_ptr->header.stamp = ros::Time::now();transform_stamped_ptr->header.frame_id = target_frame;transform_stamped_ptr->child_frame_id = source_frame;transform_stamped_ptr->transform.translation.x = 0.0;transform_stamped_ptr->transform.translation.y = 0.0;transform_stamped_ptr->transform.translation.z = 0.0;transform_stamped_ptr->transform.rotation.x = 0.0;transform_stamped_ptr->transform.rotation.y = 0.0;transform_stamped_ptr->transform.rotation.z = 0.0;transform_stamped_ptr->transform.rotation.w = 1.0;return false;}return true;
}

在获得到map的tf以后，直接通过tf2::doTransform将初始姿态转换到地图坐标系下。(这部分demo中待补充）

初始化地图

NDT配准中的目标点云就是我们事先使用SC-LEGO-LOAM构建的点云地图了，编写Subscriber监听mapLoader节点发来的点云地图message，执行如下回调：

map_points_sub_ = nh_.subscribe("points_map", 1, &NdtLocalizer::callback_pointsmap, this);

void NdtLocalizer::callback_pointsmap(const sensor_msgs::PointCloud2::ConstPtr & map_points_msg_ptr)
{const auto trans_epsilon = ndt_.getTransformationEpsilon();const auto step_size = ndt_.getStepSize();const auto resolution = ndt_.getResolution();const auto max_iterations = ndt_.getMaximumIterations();pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> ndt_new;ndt_new.setTransformationEpsilon(trans_epsilon);ndt_new.setStepSize(step_size);ndt_new.setResolution(resolution);ndt_new.setMaximumIterations(max_iterations);pcl::PointCloud<pcl::PointXYZ>::Ptr map_points_ptr(new pcl::PointCloud<pcl::PointXYZ>);pcl::fromROSMsg(*map_points_msg_ptr, *map_points_ptr);ndt_new.setInputTarget(map_points_ptr);// create Thread// detachpcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud(new pcl::PointCloud<pcl::PointXYZ>);ndt_new.align(*output_cloud, Eigen::Matrix4f::Identity());// swapndt_map_mtx_.lock();ndt_ = ndt_new;ndt_map_mtx_.unlock();
}

其中最关键的一步就是ndt_new.setInputTarget(map_points_ptr);在获取点云地图以后，设置ndt的目标点云为该点云地图，同时也是这里NDT算法的基本参数：

ndt_new.setTransformationEpsilon(trans_epsilon);搜索的最小变化量；
ndt_new.setStepSize(step_size);搜索的步长
ndt_new.setResolution(resolution);目标点云的ND体素的尺寸，单位为米
ndt_new.setMaximumIterations(max_iterations);使用牛顿法优化的迭代次数

NDT配准定位

点云配准定位主要实现于以下回调中：

void NdtLocalizer::callback_pointcloud(const sensor_msgs::PointCloud2::ConstPtr & sensor_points_sensorTF_msg_ptr)

该回调监听降采样后的点云，首先解析PointCloud2消息为pcl的PointCloud结构：

const std::string sensor_frame = sensor_points_sensorTF_msg_ptr->header.frame_id;
const auto sensor_ros_time = sensor_points_sensorTF_msg_ptr->header.stamp;boost::shared_ptr<pcl::PointCloud<pcl::PointXYZ>> sensor_points_sensorTF_ptr(
new pcl::PointCloud<pcl::PointXYZ>);

该点云是在激光雷达坐标系下，所以接着将数据投射到base_link坐标系下：

geometry_msgs::TransformStamped::Ptr TF_base_to_sensor_ptr(new geometry_msgs::TransformStamped);
get_transform(base_frame_, sensor_frame, TF_base_to_sensor_ptr);//const auto exe_start_time = std::chrono::system_clock::now();
const Eigen::Affine3d base_to_sensor_affine = tf2::transformToEigen(*TF_base_to_sensor_ptr);
const Eigen::Matrix4f base_to_sensor_matrix = base_to_sensor_affine.matrix().cast<float>();boost::shared_ptr<pcl::PointCloud<pcl::PointXYZ>> sensor_points_baselinkTF_ptr(
new pcl::PointCloud<pcl::PointXYZ>);
pcl::transformPointCloud(
*sensor_points_sensorTF_ptr, *sensor_points_baselinkTF_ptr, base_to_sensor_matrix);

设置为NDT的输入点云：

 ndt_.setInputSource(sensor_points_baselinkTF_ptr);if (ndt_.getInputTarget() == nullptr) {ROS_WARN_STREAM_THROTTLE(1, "No MAP!");return;}

最后我们还需要设定这次配准的初始姿态估计，这里需要分为以下两种情况：

Eigen::Matrix4f initial_pose_matrix;if (!init_pose){Eigen::Affine3d initial_pose_affine;tf2::fromMsg(initial_pose_cov_msg_.pose.pose, initial_pose_affine);initial_pose_matrix = initial_pose_affine.matrix().cast<float>();// for the first time, we don't know the pre_trans, so just use the init_trans, // which means, the delta trans for the second time is 0pre_trans = initial_pose_matrix;init_pose = true;}else{// use predicted pose as init guess (currently we only impl linear model)initial_pose_matrix = pre_trans * delta_trans;}

如果是第一次配准，则使用我们在Rviz中手工指定的初始姿态，否则使用线性模型（匀速匀角速度）预测的初始估计。pcl实现的NDT要求初始姿态估计使用Eigen::Matrix4f表示(也就是标准的齐次变换矩阵)，所以上面的代码中，如果是初次配准，需要将Pose转换为Eigen::Matrix4f，使用tf2::fromMsg()函数完成，对于非初次配准，我们的思路是用上一次NDT的定位结果（变换矩阵pre_trans） + 线性变换量（变换矩阵delta_trans）。在线性代数中，如果用向量AB 描述上一次定位的变换（即上一次定位base_link到地图原点的变换，即pre_trans）， BC表示当前一次定位到上一次定位的变换（即delta_trans）,那么当前的定位AC就可以表示为：
AC=AB * BC
所以对当前位置的初始姿态估计就可以用 pre_trans * delta_trans表示。

接着我们设置该初始估计，并且使用ndt进行配准：

ndt_.align(*output_cloud, initial_pose_matrix);

获得了定位的变换矩阵（base_link 到map的变换）result_pose_matrix

const Eigen::Matrix4f result_pose_matrix = ndt_.getFinalTransformation();

将之转换为Pose msg以及TF发布出去，完成本次定位：

// publishgeometry_msgs::PoseStamped result_pose_stamped_msg;result_pose_stamped_msg.header.stamp = sensor_ros_time;result_pose_stamped_msg.header.frame_id = map_frame_;result_pose_stamped_msg.pose = result_pose_msg;if (is_converged) {ndt_pose_pub_.publish(result_pose_stamped_msg);}// publish tf(map frame to base frame)publish_tf(map_frame_, base_frame_, result_pose_stamped_msg);

此外，我们还需要计算下一次用于初始姿态估计的delta_trans:

// calculate the delta tf from pre_trans to current_transdelta_trans = pre_trans.inverse() * result_pose_matrix;pre_trans = result_pose_matrix;

delta_trans即当前变换和上一次变换的差值（平移量和旋转量的差值）。最后将当前的变换保存为pre_trans供下一次初始姿态估计使用。至此NDT配准的流程结束。

实践

打开终端

source devel/setup.bash
roslaunch ndt_localizer ndt_localizer.launch

实时输出匹配时间，基本在8ms左右，满足自动驾驶配准定位实时性的要求,并会实时输出匹配迭代次数、线性变换矩阵.

通过rostopic echo /ndt_pose，可查看该topic实时输出的定位及角度信息