激光定位精度简单测试

目的

　　利用LEGO-LOAM这种SLAM方法建立环境地图，然后用NDT（OpenMP版）方法进行定位，通过与GPS数据对比计算激光定位的精度。

1　准备工作

　　测试在工厂环境中进行，其中既有开阔地带，也有树荫遮挡的狭窄走廊，既有结构化特征好的区域，也有树木较多的非结构化区域。选择百度Apollo无人车作为试验平台，其上搭载了卫星定位传感器和16线激光雷达。用差分卫星定位的结果作为基准测试激光雷达定位精度，为此提前用激光跟踪仪对卫星定位精度进行测试，其最大位置误差是4cm。

2　激光建图

　　用Velodyne 16线激光雷达以10Hz频率采集环境点云数据，同时采集卫星定位数据，卫星输出频率也为10Hz，保存为rosbag，随后利用LEGO-LOAM建立地图。打开LEGO-LOAM的闭环检测设置项（extern const bool loopClosureEnableFlag = true）。mapOptmization.cpp文件中的滤波器分辨率都设置为0.1，关键帧间隔设置为0.2（在saveKeyFramesAndFactor函数中）。LEGO-LOAM建立的地图坐标系原点就是开始时刻激光雷达的位置，但是LEGO-LOAM建立的地图坐标系不是Z轴向上，所以要转换一下。
　　我没有办法测量建图的精度，所以只能看最终的定位精度。

3　卫星数据坐标转换

　　卫星定位传感器的输出是WGS84坐标系中的数据，包括经纬度、高度和偏航角等等，为了对比，要与激光地图统一到一个坐标系。方法是：先将数据转化为地心地固坐标系：WGS84$\to$ECEF，然后再将地心地固坐标系转化为局部切平面中的坐标系：ECEF$\to$Ground。ECEF和Ground坐标系都是直角坐标系。最后这一步将卫星坐标系的原点设置为开始时刻的位置，角度则为0°。

4　定位结果

　　定位效果如下图所示，显示在Rviz中，其中有三个坐标系：全局地图坐标系map、激光定位的坐标系velodyne、卫星定位的坐标系M2。开始时刻三个坐标系是重合的。

　　无人车开始移动，velodyne和M2离开了map，如下图所示。velodyne和M2这两个坐标系重合度还比较高。

　　一段距离后，velodyne和M2逐渐分离，如下图所示。图中网格的边长是1米。

　　最后控制无人车回到起点（地图的原点）附近，形成一个环形路径。其目的是利于SLAM实现闭环，消除累积误差。
　　全程计算两个坐标系velodyne和M2在水平面投影的欧式距离，如下图所示。无人车总行驶距离500米左右，激光定位与卫星定位的最大距离误差接近1.3米，平均误差0.5米。角度误差没有计算，但是目测较小。340秒（0.1秒一个数据）以后的一段时间，误差变化剧烈，此时无人车进入树荫中，结构化特征不明显。计算误差时激光雷达和卫星的时间戳没有严格对齐，但是差别不大（最大10ms）。
　　这次试验初步的结论是，NDT激光定位的角度精度较高，对于位置精度来说，环境结构化程度越好（直线平面等特征多）则精度越高，在树木较多的地方精度不好。