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

这篇文章为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流程分如下几个部分：

1. motion prediction（运动预测）
2. scan matching（扫描匹配）
3. map update（地图更新）
4. 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的取值。

 

越近的点权重越大，具体公式为：

至此咱们已经得到了一个能够求偏导的地图，具体的求偏导方法为如下：

 

3.map update（地图更新）

在求解出机器人姿态之后，咱们已经可以根据如今姿态更新地图了，

更新地图具体方法为，对于地图的网格的原始数据（raw data）：

• 激光束的顶点（hit），存在障碍物的可能性变高，因此增长一个权重
• 激光束上的其余点（miss），存在障碍物的可能性变小，因此减小一个权重
• 激光束的始点（original point），存在障碍物的可能性变小，因此减小一个权重

根据如下公式能够求出障碍物存在的几率：

$M=\dfrac {e^{x}} {1+e^{x}}$

其中x表明网格的原始数据的值，M表明障碍物存在的几率，这个函数是形以下图：

咱们能够看网格的原始数据的值越大，越接近1，反之越接近0，很完美的实现的上文中对M的定义

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

 

 最后更新姿态（并无须要细说的地方），至此基本的SLAM的框架已经创建好了。

 

既然理论咱们已经清楚，那么从下一篇文章开始，咱们就用python，手把手把上面的内容用代码实现。