不可错过的gmapping算法使用与详细解释

138
0
2020年10月15日 12时25分

您真的懂了移动机器人是怎么构建地图的吗?

1、前言

假如,您现在已经拥有一辆ROS小车了,您已经完成了通过ROS节点发布的/cmd_vel话题控制小车的基本运动了。当然,这些都是底层部分,您也许还需要完成移动机器人的建图和导航工作。本篇文章以激光SLAM算法gmapping为例,向大家介绍移动机器人构建环境地图的必备条件、算法流程、算法原理。

其实,使用gmapping算法实现建图的步骤十分简单,一般分为了解算法、安装算法、更改参数、执行算法、保存地图。各大博客,ros_wiki都有详细的流程,基本一路上不会遇到什么问题,而且还会有良好的建图效果。在您完成SLAM建图的欣喜时,您是否想过,算法的原理是什么?算法的流程是什么?如果没有或者是想了又没想清楚,那你一定要向下读,仔细读,不懂的地方查资料也要搞清楚。如有表达错误或者表达不清楚,还请大佬指教。

2、gmapping功能包的使用(懂的直接跳过本节就好)

声明:为什么选择gmapping当做例子介绍,因为经典,能说明问题。

2.1、了解算法

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

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

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

2.2、安装算法

ROS提供了gmapping算法功能包,这大大减少了大家使用该算法的难度,通过如下命令即可完成算法的二进制安装,前提是你已经安装了ROS。

二进制方式安装gmapping算法功能包

sudo apt-get install ros-kinetic-gmapping       #根据您ROS的版本,自行更改即可

关于gmapping 算法功能包中的话题和服务如下图所示:

image-20200927095254706

从上图可以看出,gmapping需要订阅机器人关节变换话题/tf激光雷达扫描数据话题/scan,将会发布栅格地图话题/map

关于/tf又分为两个部分,gmapping功能包中的TF变换如下图所示:

image-20200927095435992

所以,从上面的介绍我们就知道,gmapping算法的必备条件,/tf、/odom、/scan,就可以通过算法发布/map了。如果您的机器人还不能满足该算法的必备条件,或者不懂/tf、/odom如何获得,可以看一下我之前写的文章。关于/scan根据各自的激光雷达而定。

https://blog.csdn.net/zhao_ke_xue/article/details/108396430

https://blog.csdn.net/zhao_ke_xue/article/details/108425922

在启动gmapping算法之前,启动ROS小车以及激光雷达传感器之后,你的TF树应该大致如下图所示,这样才能满足启动gmapping算法的必备条件,也就是上面所说的内容。

使用下面命令查看tf树。

rosrun rqt_tf_tree rqt_tf_tree

frames

2.3、更改参数

启动算法的gmapping.launch文件如下,这里需要根据各自的情况,对下面文件中注释的部分进行更改,更改内容根据自己的情况。

该启动的launch文件放在任意一个功能包的launch的文件夹中都可以,也可以新建一个功能包,因为按照二进制方式安装gmapping算法是没有这个文件的。

<launch>
    <arg name="scan_topic" default="scan" />                  <!-- 根据自己发布scan名称进行修改 -->
    <node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen" clear_params="true">
        <param name="base_frame" value="base_footprint"/>     <!-- 根据自己的基座标系名称进行修改 -->
        <param name="odom_frame" value="odom"/>               <!-- 根据自己的里程计坐标系名称进行修改 -->
        <param name="map_update_interval" value="4.0"/>
        <!-- Set maxUrange < actual maximum range of the Laser -->
        <param name="maxRange" value="5.0"/>
        <param name="maxUrange" value="4.5"/>
        <param name="sigma" value="0.05"/>
        <param name="kernelSize" value="1"/>
        <param name="lstep" value="0.05"/>
        <param name="astep" value="0.05"/>
        <param name="iterations" value="5"/>
        <param name="lsigma" value="0.075"/>
        <param name="ogain" value="3.0"/>
        <param name="lskip" value="0"/>
        <param name="srr" value="0.01"/>
        <param name="srt" value="0.02"/>
        <param name="str" value="0.01"/>
        <param name="stt" value="0.02"/>
        <param name="linearUpdate" value="0.5"/>
        <param name="angularUpdate" value="0.436"/>
        <param name="temporalUpdate" value="-1.0"/>
        <param name="resampleThreshold" value="0.5"/>
        <param name="particles" value="80"/>
        <param name="xmin" value="-1.0"/>
        <param name="ymin" value="-1.0"/>
        <param name="xmax" value="1.0"/>
        <param name="ymax" value="1.0"/>
        <param name="delta" value="0.05"/>
        <param name="llsamplerange" value="0.01"/>
        <param name="llsamplestep" value="0.01"/>
        <param name="lasamplerange" value="0.005"/>
        <param name="lasamplestep" value="0.005"/>
        <remap from="scan" to="$(arg scan_topic)"/>
    </node>
</launch>

2.4、启动算法

使用下面的命令,即可启动gmapping算法,前提是已经启动了小车及传感器,发布了tf、odom、scan。

roslaunch robot_test gmapping.launch      #这里的gmapping.launch放在robot_test功能包下

在启动gmapping算法功能包之后,你的TF树应该大致如下图所示:

frames2

此时,在运行的ROS节点关系如下图所示,其中/mbot_bringup为手写的功能包节点主要用来发布TF和odom以及与下位机通信。

rosgraph2

2.5、保存地图

保存地图命令

rosrun map_server map_saver -f map

生成map.pgm map.yaml两个文件,其中map.yaml文件内容如下:

image: map.pgm                               #图像文件名字
resolution: 0.050000                         #图像分辨率
origin: [-13.800000, -1.000000, 0.000000]    #机器人在开始建图时在地图图片上的姿态
negate: 0                                    #对立面
occupied_thresh: 0.65                        #被占据的临界值
free_thresh: 0.196                           #自由状态的临界值

其实,我感觉几乎所有的同学都可以顺利的完成以上的所有内容,不然你也不会看到这篇文章。是不是感觉很空,准备条件都懂,算法安装使用都清楚、地图质量还算可以,总感觉缺点什么。对,就是算法本身,我们对算法本身还没有一点了解。所以,继续看。

3、gmapping的前世今生

声明:为了不影响描述的连贯性,对文中一些理论内容没有进行解释,这些内容将在附录里进行详细解释,不懂的地方可以查看附录。

3.1、SLAM问题的概率描述—前世

首先,一个完整的SLAM问题是在给定传感器数据的情况下,同时进行机器人位姿和地图的估计的问题。然而,现实的情况是这样的,如果需要得到一个精确的位姿需要与地图进行匹配,如果需要得到一个好的地图需要有精确的位姿才能做到,显然这是一个相互矛盾的问题。

3.1.1、SLAM问题概率描述

概率描述如下式(3.1):显然是一个条件联合概率分布

image-20200928102949971

在移动机器人从开机到t时刻一系列传感器测量数据z1:t(这里当然是/scan)以及一系列控制数据u1:t(这里认为是/odom)的条件下,同时对地图m、机器人轨迹状态x1:t进行的估计描述SLAM问题

3.1.2、SLAM问题分解

FastSLAM算法独辟蹊径,采用RBPF方法,将SLAM算法分解成两个问题。一个是机器人定位问题,另一个是已知机器人位姿进行地图构建的问题。分解过程的公式推导如下式(3.2)

image-20200928211810956

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

3.1.3、机器人轨迹增量估计分解

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

image-20200929095351956

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

image-20200928222101727

3.1.4、SLAM问题形象化描述

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

这里以一个粒子为例,每个粒子都携带这上一时刻的位姿、权重、地图。

通过公式(3.3)我们可以看出根据上一时刻机器人轨迹通过里程计的状态传播之后,我们得到了该粒子的预测位姿。

根据预测位姿在观测模型的作用下,我们得到了该粒子代表的当前机器人轨迹,也就是完成了该粒子的机器人位姿估计。

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

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

仔细理一理,是不是有点概念了。

3.1.5、总结

到这里,我感觉对SLAM的问题的概率描述已经十分明了了,大家应该明确了SLAM的关键问题是机器人轨迹的估计问题,对运动模型(里程计)、观测模型(激光雷达)的角色也应该也有了更加深入的把握。然而,这不是问题的终结,而是问题的开端。继续看。

3.2、从论文看gmapping算法—今生

从上一节的描述上面我们已经知道SLAM问题被分解成了两个问题,机器人轨迹估计问题和已知机器人轨迹的地图构建问题。FastSLAM算法中的机器人轨迹估计问题使用的粒子滤波方法。由于使用的是粒子滤波,将不可避免的带来两个问题。第一个问题,当环境大或者机器人里程计误差大时,需要更多的粒子才能得到较好的估计,这时将造成内存爆炸;第二个问题,粒子滤波避免不了使用重采样,以确保当前粒子的有效性,然而重采样带来的问题就是粒子耗散,粒子多样性的丢失。由于这两个问题出现,导致FastSLAM算法理论上可行实际上却不能实现。针对以上问题gmapping提出了两种针对性的解决方法。也就是说gmapping是基于FastSLAM算法将RBPF方法变成了现实。

3.2.1、降低粒子数量

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

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

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

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

image-20200930113951769

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

image-20200930141451949

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

image-20200930151553779

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

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

image-20200929205006904

多元正态分布公式如下,可以根据均值和协方差,直接计算得出后验概率。

image-20200929205546484

当然,这里不仅要计算每一个粒子的轨迹的后验概率,还要为每一个粒子计算权重,这里对权重具体的计算方式就不做描述了。论文里有。

3.2.2、缓解粒子耗散

问题由来:gmapping算法采用粒子滤波算法对移动机器人轨迹进行估计,必然少不了粒子重采样的过程。随着采样次数的增多,会出现所有粒子都从一个粒子复制而来,这样粒子的多样性就完全丧失了。

目的:缓解粒子耗散

分析:gmapping算法从算法原理上使用粒子滤波,不可避免的要进行粒子重采样,所以,问题实实在在的存在,不能解决,只能从减少重采样的思路走。

方法:gmapping提出选择性重采样的方法,根据所有粒子自身权重的离散程度(也就是权重方差)来决定是否进行粒子重采样的操作。于是就出现了式(3.7),当Neff小于某个阈值,说明粒子差异性过大,进行重采样,否则,不进行。

image-20200929215241599

3.2.3、gmapping算法流程

论文中采用以上两个针对性方法,一定程度上解决了FastSLAM算法的不现实性。下面,我们从gmapping算法的伪代码来认识它。

在这里插入图片描述

3.3、从源码看gmapping算法

关于gampping算法的源码,不是一篇文章就能介绍清楚的,这里仅仅给出一个算法函数的调用流程,以后有机会,会按照模块分析源码。

1

4、总结

根据以上的全部内容,我们得到了什么,如果仔细读的话,你会发现gmapping算法进行地图构建的流程还是十分清晰的。不管是SLAM问题的概率描述、SLAM问题的分解、机器人位姿估计的迭代式的转变还是gmapping的算法流程,我们都可以从头到尾的分析清楚,清楚每一步是为什么,是做什么;这也就是本文的目的所在。关于gmapping源码解读,已知位姿构建地图算法,或者贝叶斯滤波、粒子滤波等等相关题材的文章,将在不就的将来陆续出现。

关于gmapping论文,带中文注释的源码,以及相关课件,在公众号发送:gmapping,即可获得。

附录:粒子滤波

从知乎上看到的一个有趣的解释

简单来说,在机器人定位问题中,我们想要估计机器人的位置和姿态。最初,我们完全不知道机器人在哪,那就索性假设机器人以同等的概率出现在地图上的任意一个位置。比如,假如地图是整个中国,那么机器人就等可能地出现在北京、上海、广州、哈尔滨等地。于是我们就可以用一个粒子代表一个机器人可能出现的位置。现在,机器人说话了,它说它感觉特别冷,雪落在了它洁白的脖子上。于是,我当即排除了长江以南的城市,南方怎么可能下雪呢!机器人系上围脖,继续向前走。没走几步,它又抱怨道,“今天空气质量可真不怎么样,我的双目都要失明了。”显然,它是遇到了雾霾天,这种天气在北方某帝都倒是挺常见的。它点亮了IR主动光探测,雾霾天上路多加小心总没有错。转眼间,机器人来到一个庞大的建筑物面前,这里人声鼎沸,还有遍地的商贩在叫卖着不知什么东西。它借助自身廉价的激光雷达小心翼翼地在人群中穿梭。突然,它若有所思地停了下来,似乎发现了一个美丽的秘密。虽然外面寒冬凌冽,这里却如春天般富有生机,到处洋溢着绿色的海洋。人声此起彼伏,它依稀听出了五个字,“国安是冠军”…

在上面的例子中,“我”作为机器人的大脑,根据机器人的感受,可以得出如下推理。机器人发现下雪了,那么可以确定机器人应该在北方的某个城市。接着,机器人遇到了雾霾天,那么说明该城市的空气质量很差,这就进一步把搜索范围缩小到了某几个重点空气污染城市。最后,机器人听到的五个字“国安是冠军”,彻底让我锁定了它所在的城市——“北京”。

这就是一个形象的粒子滤波案例。机器人不断地通过运动、观测的方式,获取周围环境信息,逐步降低自身位置的不确定度,最终得到准确的定位结果。

从理论上看粒子滤波。

2

3
==以往链接,点击访问。==

系列文章从零搭建ROS机器人

 

如果你感觉,我的文章比较适合你,关注我,点个赞,给你不一样的惊喜。

稿定设计导出-20200810-205521
在这里插入图片描述

发表评论

后才能评论