之前一直说总结一下定位模块的,拖到现在才开始,因为最近面试了下,发现这块做的内容已经忘记了不少。主要是基于NDT+UKF的定位方法,主要方法来源于一个开源的lidar localization project。在这里我主要对其代码进行讲解,并适配我们自己的园区场景,后续我将在此基础上进行一些工程上的改进,使之更加鲁棒和易用。本篇主对原项目进行了重写,目前修改的部分主要有以下两点:
- 新增了定位初始化模块。
- 改进map_loader节点,结合之前第四篇的动态记载。
由于数据是采用目前实习公司的,不会放出来,大家感兴趣的可以结合我的博客内容,对自己的传感器数据进行适配。起始博客中已经给出了关键的代码,如果看懂的话完全可以自己实现出来,这比直接跑别人的工程要好得多,也更有意义。

2020.6.07开始写..
定位概述
目前基于激光雷达的定位由于其全天时、稳定等特点,是自动驾驶领域的主流方案,主要运用的是SLAM技术。这里不同于传统的机器人SLAM,自动驾驶领域的L和M是解耦的。基于激光雷达定位,需要预先制作点云地图,然后基于点云地图通过点云匹配算法完成实时定位。目前一些大型的开源项目中,采用的定位方案通常是多传感器融合,非本篇要叙述的内容,但其核心还是基于点云的定位。主要分为两种,一类是以Apollo为代表的基于反射强度的2.5D地图定位方法,另外一类是基于ICP/NDT的3D点云定位方法。
第一种由于国产激光雷达反射强度普遍不行,这里不太适用我的雷达,暂不考虑。本篇主要是基于NDT的3D点云定位方法。纵观整个点云定位流程,我们可以发现一些潜在的问题,也是影响定位鲁棒性的关键问题。
为什么采用NDT算法?
这个问题在第一篇博客中已经做出说明,相比与ICP算法暴力求解,我认为NDT算法对环境进行概率建模更符合实际情况。其对匹配初值要求低,能容忍一定程度的环境变化,最后就是匹配速度快。但由于点云稀疏的特性,点云配准存在着一个通病。
点云配准问题是一个非凸优化问题,NDT和ICP算法的本质是局部搜索,通过对目标函数的迭代求解求得一个初值附近的局部最优解,而正确的空间转换可以理解成对目标函数的全局最优解。但目标函数是非凸的,在整个参数空间上有许多局部最优解,如果初值不在全局最优解附近,就可能收敛于其他次优解,导致找不到正确的转换结果。而通常我们用到的pcl ndt icp库都是采用迭代法来求解的,所以有时候会出现,明明匹配的score很小,但是明显匹配失败的场景。
此问题其实也就是点云对于局部特征的描述较好,但对于全局特征的描述能力较弱的表现。目前基于深度学习提取的点云特征在一定程度上缓解了这个问题,由于我还没过此领域基于深度学习方法的调研,这里不做评价。由这个原因导致的点云定位的第一步——定位初始化,光靠激光雷达一个传感器,不给初值,通过当前点云帧和全局地图匹配,进而得到当前位置,是不可能做到的,包括一些基于点云的传统重定位方法。所以目前这部分一般采用GPS来给定大致的位置和姿态,完成初始化。在后续的实时定位过程中,点云匹配的初值就是上一帧的定位结果。至于定位精度,这和迭代法设定的步长有关系,一般0.1m级别是能够保证的。
制图
参考以下博客
自动驾驶实战系列(一)——利用NDT算法构建点云地图 http://xchu.net/2019/10/11/31ndt-map/
LOAM/LEGO_LOAM/ALOM/LIOM等开源算法
初始化定位
初始化定位需要在点云地图上找到当前车辆的位置,这里通过GPS+NDT算法完成。GPS主要是提供初值,初值包括位置和姿态。双天线的GPS能够给出位置和yaw角。一般的GPS只能给出位置,需要通过车辆的运动在计算yaw角。
这里我们直接采用GPS实时的一个odometry
实时定位
实时定位的点云配准初值主要来源于上一帧的定位结果。
定位地图加载
动态加载网格地图
定位地图加载的方式多样,如果是大地图场景,我们采用预先划分网格、动态记载的方法,参考博客
自动驾驶实战系列(六)——网格地图的动态加载:http://xchu.net/2020/01/20/39dynamic_map_loader/
自动驾驶实战系列(二)——点云地图划分网格并可视化:http://xchu.net/2019/10/30/32pcd-divider/
其实就是拿IO换内存,其主要目的还是减小点云地图的内存占用,以及点云匹配的target。
GPS动态更新局部地图
如果在地图比较小的情况下,频繁的IO操作会拖慢整个定位系统的效率,这里我们根据GPS位置动态更新当前的局部地图。
首先初始化参数,包括地图的原点、更新频率等。
1 2 3 4 5 6 7 8 9 10 11 12 13
| radius = private_nh.param<float>("radius", 100.0f); trim_low = private_nh.param<float>("trim_low", 0.0f); lidar_height = private_nh.param<float>("lidar_height", 2.4f); trim_high = private_nh.param<float>("trim_high", 4.0f);
int mapUpdateTime = private_nh.param<int>("mapUpdateTime", 5); auto downsample_resolution = private_nh.param<float>("downsample_resolution", 0.1f); std::string globalmap_pcd = private_nh.param<std::string>("global_map_pcd_path", "data/map.pcd"); origin_latitude = private_nh.param<double>("origin_latitude", 22.663029715); origin_longitude = private_nh.param<double>("origin_longitude", 114.045642255); origin_altitude = private_nh.param<double>("origin_altitude", 59.620000000000005);
std::string gps_topic = private_nh.param<std::string>("gps_topic", "/novatel718d/pos");
|
然后初始化当前pose以及加载地图
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23
| curr_pose.reset(new geometry_msgs::PoseStamped()); curr_pose->pose.position.x = private_nh.param<float>("init_x", 0.0f); curr_pose->pose.position.y = private_nh.param<float>("init_y", 0.0f); curr_pose->pose.position.z = 0.0f;
full_map.reset(new pcl::PointCloud<pcl::PointXYZI>()); pcl::io::loadPCDFile(globalmap_pcd, *full_map);
boost::shared_ptr <pcl::VoxelGrid<pcl::PointXYZI>> voxelgrid(new pcl::VoxelGrid<pcl::PointXYZI>()); voxelgrid->setLeafSize(downsample_resolution, downsample_resolution, downsample_resolution); voxelgrid->setInputCloud(full_map); pcl::PointCloud<pcl::PointXYZI>::Ptr filtered(new pcl::PointCloud<pcl::PointXYZI>()); voxelgrid->filter(*filtered); full_map = filtered;
full_map->header.frame_id = "map"; globalmap_pub = nh.advertise<sensor_msgs::PointCloud2>("/globalmap", 5); globalmap_pub.publish(full_map); kdtree.setInputCloud(full_map);
|
局部地图的划分与更新
1 2 3 4 5 6 7 8
| localmap_pub = nh.advertise<sensor_msgs::PointCloud2>("/localmap", 5, true); timer = nh.createTimer(ros::Duration(mapUpdateTime), &GlobalmapProviderNodelet::localmap_callback, this); gps_sub = mt_nh.subscribe(gps_topic, 10, &GlobalmapProviderNodelet::gnss_callback, this);
ros::TimerEvent init_event; localmap_callback(init_event); NODELET_INFO("globalmap_provider_nodelet initial completed");
|
下面是详细的callback,根据GPS更新当前pose
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27
| void gnss_callback(const sensor_msgs::NavSatFixPtr &gps_msg) {
if (std::isnan(gps_msg->latitude + gps_msg->longitude + gps_msg->altitude)) { ROS_INFO("GPS LLA NAN..."); return; }
gpsTools gpsTools; gpsTools.lla_origin_ << origin_latitude, origin_longitude, origin_altitude;
if (gps_msg->status.status == 4 || gps_msg->status.status == 5 || gps_msg->status.status == 1 || gps_msg->status.status == 2) {
Eigen::Vector3d lla = gpsTools.GpsMsg2Eigen(*gps_msg); Eigen::Vector3d ecef = gpsTools.LLA2ECEF(lla); Eigen::Vector3d enu = gpsTools.ECEF2ENU(ecef); gpsTools.gps_pos_ = enu;
gps_pos_ = enu; has_pos_ = true; curr_pose->pose.position.x = gps_pos_(0); curr_pose->pose.position.y = gps_pos_(1); curr_pose->pose.position.z = gps_pos_(2); } }
|
最后是局部地图的划分,kd tree近邻搜索在当前定位半径100米区域内的局部点云
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43
| void localmap_callback(const ros::TimerEvent &event) { clock_t start = clock();
std::vector<int> pointIdxRadiusSearch; std::vector<float> pointRadiusSquaredDistance; pcl::PointXYZI searchPoint; searchPoint.x = curr_pose->pose.position.x; searchPoint.y = curr_pose->pose.position.y; searchPoint.z = curr_pose->pose.position.z;
pcl::PointCloud<pcl::PointXYZI>::Ptr trimmed_cloud(new pcl::PointCloud <pcl::PointXYZI>); float z_min_threshold = lidar_height + trim_low; float z_max_threshold = lidar_height + trim_high;
if (kdtree.radiusSearch(searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0) {
trimmed_cloud->points.reserve(60000); for (int i : pointIdxRadiusSearch) { if (full_map->points[i].z > z_min_threshold && full_map->points[i].z < z_max_threshold) { pcl::PointXYZI cpt; cpt.x = full_map->points[i].x; cpt.y = full_map->points[i].y; cpt.z = full_map->points[i].z; cpt.intensity = full_map->points[i].intensity; trimmed_cloud->points.push_back(cpt); } } trimmed_cloud->width = trimmed_cloud->points.size(); trimmed_cloud->height = 1;
cout << "full_map.size()\t:" << full_map->size() << endl << "trimmed_cloud->width:\t" << trimmed_cloud->width << endl; }
trimmed_cloud->header.frame_id = "map"; pcl_conversions::toPCL(curr_pose->header.stamp, trimmed_cloud->header.stamp); localmap_pub.publish(trimmed_cloud);
NODELET_INFO(" local map updated on x:%f, y:%f, z:%f", searchPoint.x, searchPoint.y, searchPoint.z); clock_t end = clock(); NODELET_INFO("trim time = %f seconds", (double) (end - start) / CLOCKS_PER_SEC); }
|
整体下来代码还是比较容易实现的。
致谢
hdl_localization:https://github.com/koide3/hdl_localization