连载文章,长期更新,欢迎关注:
下面将从原理分析、源码解读和安装与运行这3个方面展开讲解Gmapping 算法。
8.1.1 Gmapping原理分析
首先要知道,Gmapping是一种基于粒子滤波的算法。在7.7.2节中已经提到过用RBPF(Rao-Blackwellization Particle Filter)这种粒子滤波器来求解SLAM问题,Fast-SLAM算法就是其典型实现之一。其中也有人基于RBPF来研究构建栅格地图(Grid Map)的SLAM算法,它就是ROS中大名鼎鼎的Gmapping算法。不过在Gmapping算法中,对RBPF的建议分布(proposal distribution)和重采样(resampling)进行了改进。下面就首先介绍RBPF的滤波过程,然后介绍对RBPF建议分布和重采样的改进,最后介绍使用改进RBPF滤波的过程,这些内容主要参考Gmapping算法的论文[1]。
1.RBPF的滤波过程
其实RBPF的思想就是将SLAM中的定位和建图问题分开来处理,如式(8-1)所示。也就是利用首先估计出机器人的轨迹
,然后在轨迹
已知的情况下很容易估计出地图
。
在给定机器人位姿的情况下,利用进行建图很简单,可以参考文献[2]。所以,RBPF讨论的重点其实就是
定位问题的具体求解过程,一种流行的粒子滤波算法是SIR(sampling importance resampling)滤波器。那么,下面就来介绍基于SIR的RBPF滤波过程。
(1)采样
新的粒子点集由上个时刻粒子点集
在建议分布
里采样得到。通常把机器人的概率运动模型做为建议分布
,这样新的粒子点集
的生成过程就可以表示成
。
(2)重要性权重
上面只是介绍了生成当前时刻粒子点集的过程,考虑整个运动过程,机器人每条可能的轨迹都可以用一个粒子点
表示,那么每条轨迹对应粒子点
的重要性权重可以定义成式(8-2)所示的形式。其中分子是目标分布,分母是建议分布,重要性权重反映了建议分布与目标分布的差异性。
(3)重采样
新生成的粒子点需要利用重要性权重进行替换,这就是重采样。由于粒子点总量保持不变,当权重比较小的粒子点被删除后,权重大的粒子点需要进行复制以保持粒子点总量不变。经过重采样后粒子点的权重都变成一样,接着进行下一轮的采样和重采样。
(4)地图估计
在每条轨迹对应粒子点条件下,都可以用
计算出一幅地图
,然后将每个轨迹计算出的地图整合就得到最终的地图
。
从式(8-2)中可以发现一个明显的问题,不管当前获取到的观测是否有效,都要计算一遍整个轨迹对应的权重。随着时间的推移,轨迹将变得很长,这样每次还是计算一遍整个轨迹对应的权重,计算量将越来越大。可以将式(8-2)进行适当变形,推导出权重的递归计算方法,如式(8-3)所示。其实就是用贝叶斯准则和全概率公式将分子展开,用全概率公式将分母展开,然后利用贝叶斯网络中的条件独立性进一步化简,最后就得到了权重的递归计算形式。
值得注意的是,式(8-2)中的建议分布以及利用权重重采样的策略还是一个开放性话题。其实,Gmapping算法主要就是对该RBPF的建议分布和重采样策略进行了改进,下面就来具体讨论这两个改进。
2.RBPF的建议分布改进
式(8-3)中建议分布最直观的形式就是采用运动模型来计算,那么当前时刻粒子点集
的生成及对应权重的计算方式就变为式(8-4)所示。
不过直接采用运动模型做为建议分布,显然有问题。如图8-1所示,当观测数据可靠性比较低时(即观测分布的区间比较大),利用运动模型
采样生成的新粒子落在区间
内的数量比较多;而当观测数据可靠性比较高时(即观测分布的区间