用了几天的时间看了下lidar_align项目的源码,现在把整个项目的思路及个人理解整理成一篇博客,供大家参考,如有错误之处,还希望不吝赐教。
移动机器人实现智能化,需要配备多种传感器,比如定位用的GPS,测量姿态用的IMU,测距传感器激光雷达等。多传感器的配合使用往往会涉及到两个较为棘手的问题:一个多传感器数据的时间同步;另一个是多传感器数据的数据融合。而在多传感器数据融合时,首先解决的问题就是坐标系对齐,即将不同坐标系下的测量数据,转换到相同的坐标系下,然后再进行后序数据的处理。
为了获得较好的视野,测距传感器一般放置在机器人位置较高的地方,基于不同的感兴趣区域,还会调整相应的传感器姿态。根据传感器的测距原理,每个测距传感器上都会固连着一个传感器坐标系S,测距传感器基于周围环境测量得到的点云信息,均以此坐标系为参考,而坐标系的具体情况,可以参照厂家给出的说明文档。
为了测量机器人的整体姿态,IMU则放置在机器人的几何中心位置,它会实时测量载体的姿态变化。与测距传感器一样,IMU上也固连着一个虚坐标系,我们一般将此坐标系称为载体坐标系V,IMU测量得到的加速度、角速度、角度等信息都是基于此坐标系的,此坐标系的具体情况可以参考厂家给出的说明文档。
为了完成位姿与视觉信息的融合,需要将点云信息转换到载体坐标系V下,那就需要测量载体坐标系V到传感器坐标系S的转换矩阵T。lidar_align项目给我们提供了一种测量转换矩阵T的解决方案,现将实现思路介绍如下。
为了更好的说明问题,我会做一些理想化或着不太合理的假设。比如,假设测距传感器基于周围环境而获取到的点云信息整体上成矩形,如下图所示,图片是我用word画的一个矩形,然后填充的纹理,看起来像是点云,见谅。
如我们准确的获得了由载体坐标系V到传感器坐标系S的转换矩阵T,那么,当机器人沿着直线向前移动时,将点云转换到载体坐标系下后,我们看到某时刻的一系列点云将如下图所示,可以发现前后几帧的点云有很大的区域相互覆盖,那么,对于某一帧点云中的某一个点,与前后几帧点云肯定会有大量重复的点,于是对于这个点,在整体点云中求其K邻近时,K邻近的误差必接近零。由于前后几帧点云中还存在较小区域不会相互覆盖,所以K邻近的误差不会是零。为了进一步的减小K邻近的误差,lidar_align的实现源码中采用了随机采样的方法(均匀分布随机数),即对原始点云进行随机采样,因为前后几帧点云中没有相互覆盖的区域较小,这样我们可以认为采样出来的点,大部分会与重复点,而没有重复点的点将进一步减少,这样对每个点求其K邻近是,误差会进一步减少。
假如我们获取的转换矩阵存在一定的误差,那么,当机器人沿着直线向前移动时,将点云转换到载体坐标系下后,我们看到某时刻的一系列点云将如下图所示。由于误差,导致前后几帧点云中重复区域减少,这样在求每个点的K邻近时,其误差必将增大。
整个项目将点云中每个点的的K邻近误差作为优化目标,采用非线性优化库nlopt进行优化,最终将求取一个转换矩阵T。