无人驾驶Autoware代码中GNSS和激光雷达定位ndt_matching

基本结构

autoware.ai是一个日本开发的自动驾驶开源平台,是个较为全面的开放资源,是学习和二次开发的不错选择。
autoware的定位模块ndt_matching,其中融合了gnss卫星信息,IMU惯导信息,odom里程计,lidar激光雷达点云数据,进行了综合的定位判断。在我们的实验中,只使用GNSS和激光雷达。主要有以下两个topic的回调控制。

topic

一、Lidar定位

  • points_callback:这里主要做的是各种数据的融合定位,首先会结合IMU和odom数据进行一个基本位置预测,然后就是根据,filtered_points消息传过来的点云数据进行矩阵变化,加入到ndt的算法内进行计算得到一个ndt的预测位置。得到了点云匹配算法加入后的位置,与之前利用IMU和里程计计算的位置进行比较
predict_pose_error = sqrt((ndt_pose.x - predict_pose_for_ndt.x) * (ndt_pose.x - predict_pose_for_ndt.x) +
(ndt_pose.y - predict_pose_for_ndt.y) * (ndt_pose.y - predict_pose_for_ndt.y) +
(ndt_pose.z - predict_pose_for_ndt.z) * (ndt_pose.z - predict_pose_for_ndt.z));
if (predict_pose_error <= PREDICT_POSE_THRESHOLD)//默认值0.5

小于0.5说明和计算匹配值相近,则说明定位没有跳跃,设当前位置为点云计算匹配值,否则使用惯导和IMU物理计算综合值

点云过滤voxel_grid_filter

上边的lidar定位中filtered_points这个topic就是voxel_grid_filter这个节点将接收到的points_raw原始点云信息,进行体素化处理得到的。

使用体素化网格方法实现下采样,即减少点的数量,减少点云数据,并同时保持点云的形状特征,在提高配准、曲面重建、形状识别等算法速度中非常实用。
PCL实现的VoxelGrid类通过输入的点云数据创建一个三维体素栅格(可把体素栅格想象为微小的空间三维立方体的集合),然后在每个体素(即,三维立方体)内,用体素中所有点的重心来近似显示体素中其他点,这样该体素就内所有点就用一个重心点最终表示,对于所有体素处理后得到过滤后的点云。这种方法比用体素中心来逼近的方法更慢,但它对于采样点对应曲面的表示更为准确。
其中可配置的voxel_leaf_size即为设置滤波器处理时采用的体素大小。

二、GNSS定位

  • gnss_callback:接收到gnss_pose消息,进行判断rosparam参数是否设置使用_use_gnss,init_pos_set是否已经有初始化位置,fitness_score为points_callback中ndt算法点云匹配的结果值,
    _gnss_reinit_fitness为静态值500.
(_use_gnss == 1 && init_pos_set == 0) || fitness_score >= _gnss_reinit_fitness
当满足以上条件时,则对当前位置进行重新设置,判断ndt可能存在失准,改用GNSS位置为当前正确位置。这也是对激光雷达定位的一个辅助,这里将会重新设置点云的起始位置,本身点云匹配就会有累计误差出现的,加上gnss后使点云匹配恢复准确度,接下来继续使用以激光雷达定位为主的定位方式。

处理GNSS消息

autoware中对gnss进行处理的包在gnss和gnss_localizer,订阅的topic为/nmea_sentence。
nmea为国际标准的卫星定位传输信号,可以通过华测等定位设备直接读取出来。如一个标准的信息为:
$GPGGA,063201.60,3016.3898531,N,12004.0198533,E,4,19,0.7,6.795,M,7.038,M,1.6,1792*78 $GPRMC,063201.60,A,3016.3898531,N,12004.0198533,E,0.04,190.24,311219,5.6,W,D,C*46
这里就发送了两个标准信息GPGGA和GPRMC,其中会有经纬度,高度,卫星质量等数据,我们使用其中的经纬度即可。

在autoware中默认是为日本的坐标系统,demo中也是默认使用了plane为7的初始经纬度,经过测试我们可以在geo_pos_conv.cpp这个类中。使用我们当前位置的经纬度即可。如
lon_deg = 30;
lon_min = 16;
lat_deg = 120;
lat_min = 4;

三、坐标转换

我们知道每个物体都有基于自己的坐标系,如雷达必须是以车身为基准的,但是车身位置又要在map中展示,而map的坐标和gps的坐标因为初始点的不同又可能存在偏差,那么坐标转换就是为了将这多个物体统一显示在一张图中而做的。


tf
  1. base_link->velodyne,也就是激光雷达距离车身后轴的相对位置如:
  2. map->base_link,这个是ndt算法内实时计算的,车在map坐标系中的位置。
  3. map->gps,gps为上边gnss计算出来的实际位置,gps在map坐标系中的位置。
  4. world->map。为什么加一个world到map呢,主要是因为,底下有一个地图pcd就是白色的点云,它的位置要和gps位置配准,将两个显示在中间,这样点云计算的值才能和gps基本一致。而这里设置的world到map的值为gnss起始点计算出来的值的相反,因为我们在制作底图pcd的时候已经将其从原本的(0,0)点移到了gps计算出来的初始点。这样在world坐标系下底图,点云,gps将会有同一个起始点。

四、实际效果

cloud

从数据发出,到RVIZ中显示,首先由GNSS信息确定一个初始点,然后使用点云匹配得到精确位置,随着车辆前进,不断计算gnss的值和点云匹配的值,若出现点云不精确的情况,立即使用gnss重新定位初始点,一直重复。

你可能感兴趣的:(无人驾驶Autoware代码中GNSS和激光雷达定位ndt_matching)