HectorSLAM论文解析・代码重写（2）

Posted on 2017-06-25 00:50 scomup 阅读(2646) 评论(5) 编辑收藏举报

这篇文章为HectorSLAM系列的以下部分

HectorSLAM的整体逻辑

激光匹配

地图构造

地图更新

500行代码重写一个LidarSLAM

测试数据的准备，和测试数据读取模块的编写

GUI编写

地图模块的编写

核心模块的编写

主循环

匹配算法

参考以下论文

A flexible and scalable SLAM system with full 3D motion estimation
https://www.researchgate.net/publication/228852006_A_flexible_and_scalable_SLAM_system_with_full_3D_motion_estimation

首先绘制一张以扫描匹配为核心的SLAM（例如Hector,Karto_slam,Cartographer..）大概的流程图：

基本上以这一类SLAM流程分以下几个部分：

motion prediction（运动预测）
scan matching（扫描匹配）
map update（地图更新）
pose update（姿态更新）

因为原版的Hector并没有使用车轮odometry。所以运动预测部分仅为根据前一个状态的位置和速度的预测。所以我们直接从跳过motion prediction，下面从２.scan matching（扫描匹配）开始谈起。

１.scan matching（扫描匹配）

所谓匹配就是把从传感器得到的数据和已经知道的地图进行匹配。我们首先假定周围环境是静态的，在静态的环境中，传感器数据应该和已知的地图情报应该高度一致才对。所以在没有噪音的情况下以下方程应该成立：

$\sum _{i=0}^{n}\left[ 1-M\left( S_{i}\left( \xi \right) \right) \right] = 0$

其中$\xi=\left(p_x,p_y,\psi\right)$代表机器人的姿态(x坐标，y坐标，角度)

Si代表，i号激光束，在姿态ξ下照射到的障碍物的坐标。

M代表某坐标为障碍物的概率。１为100%为障碍物，０为100%空闲区域

在传感器没有噪音，地图已知且没有误差的的情况下，可以求解以上方程，得到未知量ξ（姿态）。直观上来说就是调整姿态使得所有的激光都正确的测量到障碍物。但现实的情况却没有怎么完美，传感器有噪音，地图有误差，且部分未知。所以我们转而求解以下最小二乘：

$argmin_{\xi}\sum _{i=0}^{n}\left[ 1-M\left( S_{i}\left( \xi \right) \right) \right]^2$

这个式子的求解方法，在论文的公式（９）～（１３）有推导过程。这里虽然不推导（论文已经足够详细），但是仍然建议自己推导一次加深理解。

根据下面的方程我们似乎已经可以通过扫描匹配的方法解出机器人姿态了。

原版的Hector中，为了避免对姿态的求解陷入局部最优解，用了几种不同的分辨率表示地图。首先在分辨率较低的地图中求解姿态，然后逐渐带入较高分辨率的地图中。

２.地图构造

但为什么只是似乎呢？因为求解以上（12）（13）是需要求M的偏导的，我们知道求解偏导的是需要连续空间的，但Hector使用的格子地图（grid map）存在于离散的空间中。所以，首先我们需要一些方法把我们的离散的格子地图，转换为可以求导的连续地图。论文的【IV. 2D SLAM】介绍了一种用Bilinear filtering的解决办法：

这个方法直观的理解：比如下图中我们要求点P_m的值时，由于格子地图是离散的，所以并没有办法直接取得。我们转而根据距离P_m最近的4个点P₀₀,P₁₀,P₁₀P₁₁，估计出P_m的取值。