基于概率的SLAM算法详解

REF：基于激光雷达的 SLAM 和路径规划算法研究与实现

1. 格栅地图构建算法

移动机器人的激光雷达及栅格地图坐标系的转换，可以得到激光雷达的每条激光点所测量到的障碍物在栅格地图中的坐标，障碍物在真实环境中的坐标如下，其中： (x,y,θ)(x,y,\theta)(x,y,θ) ：表示机器人的当前位姿 aaa：激光雷达任意采样点与机器人朝向角的夹角 ρ\rhoρ：激光雷达采样点返回得到障碍物与自身距离

625e833041333adabb84b5d9dea27b44.png

已知障碍物和移动机器人在栅格地图中的坐标的情况下，Bresenham 算法将线特征转换为网格特征，获得激光点通过的空闲栅格集合，即激光线穿过的非占用栅格地图区域，进行地图更新

f40c6b751572046b6a86803543b03e38.png

2. 扩展卡尔曼滤波SLAM

扩展卡尔曼滤波(Extended kalman filter，EKF)是传统非线性估计的代表，是围绕状态估值对非线性模型进行一阶 Taylor 展开（用泰勒展开式中的一次项来对非线性系统中状态量和观测量行线性化处理, 即计算状态和观测函数的雅克比矩阵），然后应用线性系统的卡尔曼滤波公式（在标准卡尔曼滤波框架下进行递归滤波），基本过程可分为以下四个部分： (1) 地图初始化， (2) 使用里程计数据和运动模型更新当前状态， (3) 通过路标矫正当前状态， (4) 在当前地图状态下更新新的路标信息。

• 扩展卡尔曼滤波过程

0bc414d1f41aabc13e48b27bbb42ae4d.png

• EKF 和 EKF_SLAM 对应过程

d2bb7dbcacc5f9f928e3e8d9e3d72042.png

• Matlab 仿真 EKF_SLAM

61cbd246edac2554dd0ffe1eea56afb9.png

3. 基于粒子滤波的SLAM

假设状态转移模型符合一阶马尔科夫过程， k 时刻的状态只由k-1 时刻决定，粒子滤波算法的核心思想是利用一系列随机样本的加权和，表示后验概率密度，通过求和来近似积分操作。每次得到一个新的观测值都需要通过递归滤波器对系统进行一次估计，递归滤波器包含两个步骤：

• 预测：由前一个状态预测下一个状态的概率密度函数

d97c9443153800a9a4214f5d23a3b0bc.png

• 更新：利用最新的观测值，修改预测出的概率密度函数

8615782fe13d3d655c21eb27628fee28.png

在序列重要性采样(Sequential Importance Sampling，SIS)中，将后验概率用 N 个随机采样的样本(即粒子)与各自权重表示，其中w表示每个粒子的权重，权重和必须等于1

421947ccccaaa8936696f95790d91da2.png

权重的递归式可表示为：

9ba1c204160e13c5dc31d302805299c4.png

SIS 在经历过几次递归后，很多小权重粒子可以忽略不计，只剩下权重比较大的粒子，在递归过程中，权重的变换只会越来越大，因此退化问题不可避免，为了评估退化问题，若有效粒子数小于某一个阀值，对粒子进行重采样，定义有效粒子数为

1aef35325e2253926e039a495f1bb752.png

4. 基于高斯-牛顿梯度法的SLAM

Hector SLAM 算法依赖于高分辨率、高扫描频率的激光传感器，三维运动估计是基于二维平面的定位信息，所以很容易扩展为普通的平面 SLAM 算法

0444c06f28c4275ca5b24897c2ef8602.png

f99e6372b2f55149435199833e360423.png

f9af299f44e93562671191cc54494fd6.png