十大网站黄页,网站导航下拉菜单代码,单页面网站 seo,网站网站做维护概述
在SLAM中#xff0c;机器人位姿和地图都是状态变量#xff0c;我们需要同时对这两个状态变量进行估计#xff0c;即机器人获得一张环境地图的同时确定自己相对于该地图的位置。我们用x表示机器人状态#xff0c;m表示环境地图#xff0c;z表示传感器观测情况#xf…概述
在SLAM中机器人位姿和地图都是状态变量我们需要同时对这两个状态变量进行估计即机器人获得一张环境地图的同时确定自己相对于该地图的位置。我们用x表示机器人状态m表示环境地图z表示传感器观测情况u表示输入控制下标表示时刻则对 进行估计。而由条件贝叶斯法则可以得到 这一分解相当于把SLAM分离为定位和构建地图两步大大降低的SLAM问题的复杂度。基于此Gmaping算法的大致过程为用上一时刻的地图和运动模型预测当前时刻的位姿然后根据传感器观测值计算权重重采样更新粒子的地图如此往复。ROS中实现的Gmapping算法框架大致如下后面讲述原理时将说明对应的代码模块 定位
Gmapping算法基于粒子滤波因此定位部分和粒子滤波大致相同粒子状态预测测量更新重采样。接下来分别说明
1、状态预测draw from motion 当前时刻粒子的状态首先由运动模型进行更新在初始值上增加高斯采样的噪声进行一个粗略状态估计。 在Gmapping算法中则采用以下算法对运动进行采样 2、测量scan match 这一步是在粗略估计的基础上做一次扫描匹配找到一个使当前观测最贴合地图的位姿以改进基于里程计模型的提议分布。基本思路是在基于运动模型预测的位姿向负x正x负y正y左旋转右旋转一共六个状态移动预测位姿计算每个状态下的匹配得分选择最高得分对应的位姿为最优位姿。
扫描匹配的重点就在于如何计算匹配得分。所谓匹配是将当前采集的激光数据与环境地图进行对准1激光点的坐标转换至网格地图坐标2分别处理六个状态当确定激光点网格坐标的地图值为障碍物时进行打分原理与NDT类似距离越小分数越大3得分最高的位姿为最优位姿。 获得最优粒子位姿后可以把粒子采样范围从又扁又宽的区域更改到激光雷达观测模型所代表的尖峰区域L新的粒子分布就可以更贴近于真实分布。 扫描匹配之后我们就找到了L所代表的尖峰区域接下来的任务是确定
该尖峰区域所代表的高斯分布的均值和方差。作者的方法是在L中随机采样K个点根据这K个点的里程计和观测模型计算均值和方差如下式所示。 3、计算权重 然后对于每个粒子我们需要计算它的权重以供后续的重采样步骤使用。由于在前面我们利用激光数据对提议分布进行了优化 那么粒子的权重公式变成了 这里还有一个问题就是权重计算权重描述的是目标分布和提议分布之间的差别。因此我们在计算权重时就是计算我们模拟出的提议分布和目标分布的不同。而这种不同体现在我们是由有限的采样模拟出目标分布因此权重的计算公式为 4、重采样update Tree Weights 在执行重采样之前计算了每个粒子的权重有时会因为环境相似度高或是由于测量噪声的影响会使接近正确状态的粒子数权重较小而错误状态的粒子的权重反而会大。重采样是依据粒子权重来重新采粒子的这样正确的粒子就很有可能会被丢弃频繁的重采样更加剧了正确但权重较小粒子被丢弃的可能性。
Gmapping算法中作者采用权重值离差的量度进行重采样的判定。 Neff越大粒子权重差距越小。想象极端情况当所有粒子权重都一样的时候比如重采样之后这些粒子恰好可以表示真实分布类似于按照某个分布随机采样的结果。当Neff降低到某个阈值以下说明粒子的分布与真实分布差距很大在粒子层面表现为某些粒子离真实值很近而很多粒子离真实值较远这时候恰好进行重采样。
建图
Gmapping算法会构建一个栅格地图对二维环境进行了栅格尺度划分而假设每一个栅格的状态是独立的。
对于环境中的一个点我们用 来表示它是Free状态的概率用 来表示它是Occupied状态的概率当然两者的和为1。为了更方便的表示我们用 作为该点的状态比值越大说明该点约可能是障碍物。
对于一个点对于一个点新来了一个测量值z之后我们需要更新它的状态。假设测量值来之前该点的状态为 我们要更新它为 。
由贝叶斯公式计算可得 为了方便计算我们对两边取对数 在没有任何测量值的初始状态下一个点的初始状态为0而这一部分关键的地方在于 的计算我们称这个比值为测量值的模型标记为lomeas。实际上测量值的模型只有两种 和 而且都是定值。这样每获得一次测量值我们都能用加减法对点状态进行更新。从而完成更新地图的工作。以下图为例 x是真实世界中的坐标 为栅格地图中的坐标r为一格的长度1/r表示分辨率则 。则二维情况下 。假设图中机器人的位姿为xy 我们可以很容易计算障碍物的位置 其中d为测量得到的距离 为激光线与机器人位姿角的夹角。我们得到两个坐标后能计算出两点在栅格地图的位置ij 与 。
然后我们利用bresenham算法(compute active area)来计算非障碍物格点的集合。然后利用上文所述结论更新栅格地图即可。 Bresenham算法基本思想是采用递推步进的办法令每次最大变化方向的坐标步进一个像素同时另一个方向的坐标依据误差判别式的符号来决定是否也要步进一个像素。举例说明 由于显示直线的象素点只能取整数值坐标可以假设直线上第i个像素点坐标为(xiyi)它是直线上点(xiyi)的最佳近似并且xixi(假设直线斜率小于1。那么直线上下一个像素点的可能位置是(xi1yi)或(xi1yi1)。由图中可以知道在xxi1处直线上点的y值是ym(xi1)b该点离像素点(xi1yi)和像素点(xi1yi1)的距离分别是d1和d2 这两个距离差是 分析d1-d2有以下三种情况
当此值为正时d1d2说明直线上理论点离(xi1yi1) 像素较近下一个像素点应取(xi1yi1)。当此值为负时d1d2说明直线上理论点离(xi1yi) 像素较近则下一个像素点应取(xi1yi)。当此值为零时说明直线上理论点离上、下两个像素点的距离相等取哪个点都行算法规定这种情况下取(xi1yi1)作为下一个像素点。
因此只要利用(d1-d2)的符号就可以决定下一个像素点的选择。
参考文献
https://blog.csdn.net/qq_36355662/article/details/90301219https://blog.csdn.net/shixiaolu63/article/details/93739379https://www.jianshu.com/p/f044da681454https://blog.csdn.net/liuyanpeng12333/article/details/81946841https://www.cnblogs.com/yhlx125/p/5634128.html概率机器人粒子滤波从推导到应用Improved Techniques for Grid Mapping with Rao-Blackwellized Particle Filters
