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

基本结构

autoware.ai是一个日本开发的自动驾驶开源平台，是个较为全面的开放资源,是学习和二次开发的不错选择。
autoware的定位模块ndt_matching,其中融合了gnss卫星信息，IMU惯导信息，odom里程计，lidar激光雷达点云数据，进行了综合的定位判断。在我们的实验中，只使用GNSS和激光雷达。主要有以下两个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的坐标因为初始点的不同又可能存在偏差，那么坐标转换就是为了将这多个物体统一显示在一张图中而做的。

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将会有同一个起始点。
   

四、实际效果

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