my_gmapping_slam来龙去脉（一）

最新推荐文章于 2024-09-27 18:31:22 发布

置顶

家门Jm

最新推荐文章于 2024-09-27 18:31:22 发布

阅读量284

点赞数

分类专栏： gmapping 文章标签：自动驾驶

本文链接：https://blog.csdn.net/qq_38167930/article/details/119087665

版权

本文介绍了激光SLAM算法中的gmapping，包括其基础知识、算法流程主线、激光雷达运动畸变去除等。gmapping是基于2D激光雷达的SLAM算法，使用RBPF算法实时构建室内环境地图，但不适合大场景。文章详细阐述了FastSLAM的提出、SLAM问题分解，以及gmapping如何通过降低粒子数量和缓解粒子耗散来解决实际问题。还讨论了gmapping的匹配机制、地图数据结构和存储方式，以及算法流程中的关键函数解析。

摘要由CSDN通过智能技术生成

0、激光SLAM前言

1、gmapping基础知识

1.1 什么是gmapping？

1.2 gmapping怎么来的？解决了哪些问题？

1.2.1 FastSLAM的提出

1.2.2 机器人轨迹增量估计分解

1.2.3 SLAM问题形象化描述

1.2.4 gmapping解决的两个问题

2.3.3 normalize()归一化函数解析

6、part_ros（map to odom的变换）

0、激光SLAM前言

最近准备面试，跟随做了一个项目，现在将知识点进行总结，算是项目回顾，也为面试准备。

这里需要说明的是，这里的gmapping并不是原版的gmapping，而是进行了删减，添加激光雷达运动畸变去除模块，代码量仅有4000余行。

1、gmapping基础知识

1.1 什么是gmapping？

Gmapping是一个基于2D激光雷达使用RBPF(Rao-Blackwellized Particle Filters)算法完成二维栅格地图构建的SLAM算法。

优点：gmapping可以实时构建室内环境地图，在小场景中计算量小，且地图精度较高，对激光雷达扫描频率要求较低。

缺点：随着环境的增大，构建地图所需的内存和计算量就会变得巨大，所以gmapping不适合大场景构图。一个直观的感受是，对于200×200米的范围，如果栅格分辨率是5cm，每个栅格占用一个字节内存，那么每个粒子携带的地图都要16M（400/0.0025=160000）的内存，如果是100个粒子就是1.6G内存。

1.2 gmapping怎么来的？解决了哪些问题？

一个完整的SLAM问题是在给定传感器数据的情况下，同时进行机器人位姿和地图的估计问题。然而，二者是相互依存的，如果需要的得到一个精确的位姿需要与地图进行匹配，如果要得到一个好的地图需要精确的位姿。

1.2.1 FastSLAM的提出

FastSLAM采用基于RBPF的粒子滤波算法。FastSLAM算法是Montemerlo等人在2002年首次提出，将Rao-Blackwellised粒子滤波器应用到机器人SLAM中。这句话又引出两个概念：

1. 什么是粒子滤波？通过寻找一组在状态空间中传播的随机样本来近似地表示概率密度函数，用样本均值来替代积分运算，进而获得系统状态的最小方差估计的过程。这些样本被形象地称为“粒子”，故而叫做粒子滤波。其核心思想是通过从后验概率中抽取的随机状态粒子来表达其分布。

通俗理解：先在地图上均匀地撒一把沙子，再根据传感器值来给每粒沙子赋予不同的权重，高权重的沙子会被保留，低权重的沙子会被删除。

2. 什么是RBPF？RBPF算法将SLAM问题分解成机器人定位问题和基于位姿估计的环境特征位置估计问题，用粒子滤波算法做整个路径的位置估计，用EKF估计环境特征的位置，每一个EKF对应一个环境特征。

说回FastSLAM，采用RBPF的思路，将SLAM算法分解成两个问题：一个是机器人定位问题，另一个是已知机器人位姿进行地图构建的问题。SLAM问题是一个条件联合概率分布问题，描述如下：

（啰嗦一句，就是用在移动机器人从开机到t时刻一系列传感器测量数据z1:t（这里当然是/scan）以及一系列控制数据u1:t(这里认为是/odom)的条件下，同时对地图m、机器人轨迹状态x1:t进行的估计来描述SLAM问题。）分解为两个问题之后，公式变为如下：

其中p(x1:t | u1:t, z1:t)表示估计机器人的轨迹，p(m|x1:t, z1:t) 表示在已知机器人轨迹（/odom）和传感器观测数据（/scan）情况下，进行地图构建的闭式计算。这样 SLAM 问题就分解成两个问题。其中①已知机器人位姿的地图构建是个简单问题，所以②机器人位姿的估计是一个重点问题。

为什么先定位后建图？

SLAM的过程是一个条件联合概率分布，由概率论可知联合概率可以转换成条件概率即：P(x,y) = p(y|x)p(x)。 通俗点解释就是我们在同时求两个变量的联合分布不好求时，可以先求其中一个变量，再将这个变量当做条件求解另一个变量。这就是解释了Gmapping为什么要先定位再建图：同时定位和建图是比较困难的，因此我们可以先求解位姿。这也造成了Gmapping很依赖里程计的精度。

1.2.2 机器人轨迹增量估计分解

FastSLAM算法采用粒子滤波来估计机器人的位姿，并且为每一个粒子构建一个地图。所以，每一个粒子都包含了机器人的轨迹和对应的环境地图。现在我们着重研究一下 p(x1:t | u1:t, z1:t) 估计机器人的轨迹。通过使用贝叶斯准则对 p(x1:t | u1:t, z1:t) 进行公式推导如式（3.3）所示：

经过上面的公式推导，这里将机器人轨迹估计转化成一个增量估计的问题，用p(x1:t-1 | u1:t-1, z1:t-1) 表示上一时刻的机器人轨迹，用上一时刻的粒子群表示。每一个粒子都用运动学模型p(xt | xt-1, ut)进行状态传播，这样就得到每个粒子对应的预测轨迹。对于每一个传播之后的粒子，用观测模型p(zt | xt)进行权重计算归一化处理，这样就得到该时刻的机器人轨迹。之后根据估计的轨迹以及观测数据进行地图构建。

1.2.3 SLAM问题形象化描述

根据上面的公式推导，我们必须整理出一个流程来，只有这样我们才能更加清晰地理解它们。

以一个粒子为例，每个粒子携带着上一时刻的位姿、权重、地图。

通过公式（3.3）可以看出，根据上一时刻机器人轨迹通过里程计的状态传播之后，我们得到了该粒子的预测位姿。（干讲原理码字，可能看得云里雾里，下面附上这句话对应的代码）

for (int i=0; i<tmp_size; i ++)
{
    // 对于每一个粒子，将从运动模型传播的激光雷达位姿存放到m_particles[i].pose （最新的地图坐标系下的激光雷达位姿）
    OrientedPoint& pose(m_particles_[i].pose);
    pose = m_motionModel_.drawFromMotion(m_particles_[i].pose, relPose, m_odoPose_);    // 这里一定要区分三个位姿
    // m_particles[i].pose:最新的地图坐标系下的激光雷达最优位姿  relPose:当前激光雷达里程计位姿   m_odoPose:表示上一次的激光雷达里程计位姿
}

所谓的里程计状态传播就是计算当前雷达的里程计位姿m_odoPose_（注意是里程计位姿，不是map上雷达位姿）与上一帧雷达的里程计位姿relPose在里程计坐标系下的差值，m_particles[i].pose是引用传递，传入上一帧雷达在map坐标系下的位姿，将里程计坐标系下的位姿差值，嫁接（学术点讲叫转换）到map坐标系下，根据两帧的里程计差值，加上地图坐标系下的上一帧雷达位姿起点，就可以得到当前时刻的雷达在map坐标系下的预测位姿。

根据预测位姿在观测模型的作用下，我们对粒子进行位姿优化(addscan中的optimize爬山算法)得到了该粒子代表的当前机器人轨迹，也就是完成了该粒子的机器人位姿估计。

通过公式（3.2）根据机器人轨迹结合观测数据，即可闭式得到该粒子代表的地图。

这样每一个粒子都存储了一个机器人轨迹，以及一张环境地图。

1.2.4 gmapping解决的两个问题

我们已经知道SLAM问题被分解成了两个问题，机器人轨迹估计问题和已知机器人轨迹的地图构建问题。FastSLAM算法中的机器人轨迹估计问题使用的粒子滤波方法。由于使用的是粒子滤波，将不可避免的带来两个问题。

第一个问题，当环境大或者机器人里程计误差大时，需要更多的粒子才能得到较好的估计，这时将造成内存爆炸；
第二个问题，粒子滤波避免不了使用重采样，以确保当前粒子的有效性，然而重采样带来的问题就是粒子耗散，粒子多样性的丢失。由于这两个问题出现，导致FastSLAM算法理论上可行实际上却不能实现。针对以上问题gmapping提出了两种针对性的解决方法。

也就是说gmapping是基于FastSLAM算法将RBPF方法变成了现实。

（1）降低粒子数量

问题由来：每一个粒子都包含自己的栅格地图，对于一个稍微大一点的环境来说，每一个粒子都会占用比较大的内存。如果机器人里程计的误差比较大，即proposal分布（提议分布）跟实际分布相差较大，则需要较多的粒子才能比较好的表示机器人位姿的估计，这样将会造成内存爆炸。

目的：通过降低粒子数量的方法大幅度缓解内存爆炸。

分析：里程计的概率模型比较平滑，是一个比较大的范围，如果对整个范围采样将需要很多的粒子，如果能找到一个位姿优值，在其周围进行小范围采样，这样不就可以降低粒子数量了嘛。

方法一：直接采用极大似然估计的方式，根据粒子的位姿的预测分布和与地图的匹配程度，通过扫描匹配找到粒子的最优位姿参数，就用该位姿参数，直接当做新粒子的位姿。如下图所示，从普通的proposal分布采样替换为用极大似然估计提升采样的质量。这也就是gmapping算法中的做法。

方法二：gmapping算法通过最近一帧的观测（scan），把proposal分布限制在一个狭小的有效区域。然后再正常的对proposal分布进行采样。该方法是gmapping论文中详细描述的方法。

如果proposal分布用激光匹配来表示，则可以把采样范围限制在一个比较小的区域，因此可以用更少的粒子即覆盖了机器人位姿的概率分布。也就是说将proposal分布从里程计的观测模型变换到了激光雷达观测模型。具体公式推导如下所示：

从上图可以看到激光雷达观测模型方差较小，假设其服从高斯分布，使用多元正态分布公式即可进行计算，于是求解高斯分布是下面需要做的。

同方法一，首先通过极大似然估计找到局部极值x_t，认为该局部极值距离高斯分布的均值比较近，于是在位姿x_t附近取了k个位姿；对k个位姿进行打分（位姿匹配），这样我们就具有k个位姿，以及它们与地图匹配的得分；我们又假设它们服从高斯分布，计算出它们的均值和方差之后，即可使用多元正态分布，对机器人位姿的估计进行概率计算。矢量均值和协方差的计算。具体如下式(3.4) (3.5)：