蒙特卡罗定位(Particle Filter Localization)
1. 蒙特卡罗定位
定位:机器人知道地图信息的情况下如何利用传感器信息确定自己的位置(Localization)。
有人会说,定位是不需要地图信息的。机器人知道初始位置,知道左右轮的速度,就可以算出在一段时间内左右轮分别走了多少距离,进而算出机器人的转角和位移,以便更新位置信息。但是显然,这种方法存在很大的问题。首先,速度是传感器获得的,然而传感器是有精度限制的,这就意味着误差的存在,对时间积分求距离误差就更大了;另外,机器人也可能存在打滑之类的机械问题。结合地图来对机器人进行定位能有效减小误差。
传感器方面。我们使用激光传感器,它能够测量机器人各个方向和最近障碍物之间的距离。在每一个时间点,机器人都会获得激光传感器的测量值。如下图,绿色三角形是机器人,红色的线是激光束,黄色的格子是机器人在该激光方向上检测到的最近的障碍物。
地图是占据栅格地图(Occupancy Grid Map)。比如,下面的地图中,浅色(白色)的格子表示障碍物,深色(黑色)的格子表示空白位置。
那么,在这个时间点,我们要做的就是把机器人放到地图中去,使得激光传感器的读数尽可能符合地图信息(如下图所示)。
这样,对于一个时间点的定位问题就变成了求解最优函数的问题了。然而这个最优化函数太难求解了(坐标和角度都是连续变化的,而地图是一个一个格子的数值)。
我们需要注意到两点。第一,对于给定的机器人位置信息,我们可以很轻松地计算出和地图的吻合程度;第二,相邻两个时间点机器人位置的变化不会太大。基于这两点,我们引出蒙特卡罗定位法。这个算法的核心思想是用高斯分布描述机器人位置信息的噪音,用大量的粒子来描述机器人可能的位置。
具体来说,假如估测的机器人位置信息为(和表示坐标,表示机器人朝向),我们会记录机器人的位置信息符合(视具体情况而定)的多元高斯分布。在算法实现中,我们用高斯分布采样出的个粒子来表示机器人的位置。如下左图所示的单元高斯模型,下面蓝色的点是采样的粒子,上面是对应的高斯分布的模型;如下右图所示是二元高斯模型采样的情况。
蒙特卡罗定位法分为四步:
下面,我们将结合具体例子来说明算法的步骤。
2. 实践:利用激光传感器定位
前面我们介绍了为什么使用蒙特卡洛法进行机器人定位以及大体步骤,在这一节我们将介绍算法实现的具体细节。我们需要编写matlab函数:
function [ myPose ] = particleLocalization(ranges, scanAngles, map, param)
scanAngles是的数组,表示同一时间发射的束激光与机器人前进方向的夹角。ranges是的矩阵,表示个时间点的激光传感器数据。map是的矩阵,表示已知的占据栅格地图。param是一个结构体,包含一些必要的输入,其中param.resol表示分辨率,即一米的方格数量,param.origin是在占据栅格地图中的起始位置。我们需要计算出一个的矩阵myPose,机器人在个时间点的坐标和朝向。
第一步,初始化粒子群。由于初始位置已经给出了准确值(param.origin),我们只需要设定种群大小之后利用repmat方法复制个初始位置作为初始种群。在实际应用(比如SLAM)中,如果初始位置不确定,可以使用高斯分布随机采样的方法(randn函数)初始化粒子群。
第二,模拟粒子运动。卡尔曼滤波为机器人运动建模,这是实际应用中一种十分有效的方法。在这里,我们采用一种简单粗暴的方法,不对机器人运动进行建模,直接假定机器人在两个采样的时间点间隔内运动的范围有限,然后利用randn函数随机生成可能运动到的位置。
第三,计算粒子评分。对于每一条激光射线,传感器读数与地图的匹配有四种情况如下表。
我实践的时候发现,雷达定位出的空白区域太多了,计算这些栅格十分耗时,所以只计算传感器定位的障碍物与地图中障碍物的符合个数(即上表的评分改为0,0,0,+1)。对每一个粒子评分结束后,我们选择得分最高的粒子作为该时间点机器人的位置。
第四,粒子群重采样。在评分结束后,我们会发现有的粒子评分很低,即严重偏离可能位置,对于这些粒子我们需要舍弃。而有一些粒子的评分很高且很接近(比如传感器读数与地图吻合度80%以上的粒子),我们需要把它们都保留下来。这就是粒子群重采样。在实际操作中,我们直接将评分过低的粒子舍弃,对评分高的粒子进行复制,重采样之后保持粒子群数量基本不变。
完整的matlab代码为:
function myPose = particleLocalization(ranges, angles, map, param) % occupancy value of unexplored pixels
unknown = mode(reshape(map, size(map,)*size(map,), )); N = size(ranges, ); % number of poses to calculate
myPose = zeros(, N); % initialize return value resol = param.resol; % map resolution
origin = param.origin; % origin in pixel sig = [0.08, , ; , 0.08, ; , , 0.08]; % noise for particle movement myPose(:,) = param.init_pose; % init position M = ; % number of particles P = repmat(myPose(:,), [, M]); thr = ceil(3.5/*size(angles,)); % set the score threshold as % for j = :N
maxscore = ;
while maxscore < thr
Q=P+(randn(size(P,),)*sig)'; % particles movement
score = zeros(size(Q,), ); % scores
for k = :size(Q,) % calculate score for each particle
occ_x = ceil( (ranges(:,j) .* cos(angles+Q(,k)) + Q(,k) ) * resol + origin() );
occ_y = ceil( (-ranges(:,j) .* sin(angles+Q(,k)) + Q(,k) ) * resol + origin() );
ids = occ_x > & occ_x <= size(map,) & occ_y > & occ_y <= size(map,);
score(k) = size( map( map (sub2ind(size(map), occ_y(ids), occ_x(ids)) ) > unknown ), );
end
[maxscore, index] = max(score); % select particle with maximum score
end
myPose(:,j) = Q(:,index); % set pose(j) as the optimal particle Q = Q(:,score >= thr); % select particles with high score
P = repmat(Q, , ceil(M/size(Q,)) ); % regenerate particles
end end
于是,我们就得到了测试数据的定位图了:
总结
经典的定位算法——蒙特卡罗定位法,最早的论文可见"Monte Carlo localization for mobile robots",这种方法十分实用。当然,我们只讲了地图已知的情况下如何定位。在地图未知的时候,Mapping和Localization需要同时进行,也就是SLAM,这四个步骤就会有很多变化,但是算法的框架并不会变。
原文链接:https://zhuanlan.zhihu.com/p/21974439
蒙特卡罗定位(Particle Filter Localization)的更多相关文章
- 蒙特卡罗定位(Particle Filter Localization)笔记
善始善终,这篇文章是Coursera课程Robotics: Estimation and Learning最后一周的课程总结.里面的小哥讲得不是很清晰,留下的作业很花功夫(第二周课程也是酱紫). 这周 ...
- 基于粒子滤波的物体跟踪 Particle Filter Object Tracking
Video来源地址 一直都觉得粒子滤波是个挺牛的东西,每次试图看文献都被复杂的数学符号搞得看不下去.一个偶然的机会发现了Rob Hess(http://web.engr.oregonstate.edu ...
- 粒子滤波(PF:Particle Filter)
先介绍概念:来自百科 粒子滤波指:通过寻找一组在状态空间中传播的随机样本来近似的表示概率密度函数,再用样本均值代替积分运算,进而获得系统状态的最小方差估计的过程,波动最小,这些样本被形象的称为&quo ...
- Particle filter for visual tracking
Kalman Filter Cons: Kalman filtering is inadequate because it is based on the unimodal Gaussian dist ...
- 机器学习理论基础学习14.2---线性动态系统-粒子滤波 particle filter
一.背景 与卡曼滤波不同的是,粒子滤波假设隐变量之间(隐变量与观测变量之间)是非线性的,并且不满足高斯分布,可以是任意的关系. 求解的还是和卡曼滤波一样,但由于分布不明确,所以需要用采样的方法求解. ...
- ROS导航之参数配置和自适应蒙特卡罗定位
我们的机器人使用两种导航算法在地图中移动:全局导航(global)和局部导航(local).这些导航算法通过代价地图来处理地图中的各种信息,导航stack使用两种costmaps http://www ...
- Channel Estimation for High Speed Wireless Systems using Gaussian Particle Filter and Auxiliary Particle Filter
目录 论文来源 摘要 基本概念 1.时变信道 2.粒子滤波 3.高斯粒子滤波 4.辅助粒子滤波 比较 借鉴之处 论文来源 International Conference on Communicati ...
- 理解粒子滤波(particle filter)
1)初始化阶段-提取跟踪目标特征 该阶段要人工指定跟踪目标,程序计算跟踪目标的特征,比如可以采用目标的颜色特征.具体到Rob Hess的代码,开始时需要人工用鼠标拖动出一个跟踪区域,然后程序自动计算该 ...
- Multi-task Correlation Particle Filter for Robust Object Tracking--论文随笔
摘要:在这篇论文中,作者提出一种鲁棒视觉跟踪的多任务相关粒子滤波琪跟踪算法(MCPF).作者首先向我们展示了多任务相关滤波器,该滤波器在训练滤波器模板的时候可以学习不同特征之间的联系.本文提出的MCP ...
随机推荐
- D3.js 弦生成器(V3版本)
弦生成器(Chord Generator) 弦生成器(Chord Generator)根据两段弧来绘制弦,共有五个访问器,分别为source().target().radius().startAn ...
- mac 如何卸载node和npm采坑之旅
因为本地npm一直报错,所以决定直接卸载node和npm,重新装.第一次卸载,具体咱也不会呀!能咋整呢,百度呗 茫茫百度中各种找呀,找到一个转载最多的方法 sudo npm uninstall npm ...
- 22-7map
<!DOCTYPE html> <html lang="en"> <head> <meta charset="UTF-8&quo ...
- Python 文件名
- 开启SSH 使用SSH登录工具连接虚拟机
修改sshd_config文件,命令为:vi /etc/ssh/sshd_config将#PermitRootLogin without-password注释去掉修改为PermitRootLogin ...
- python_django_在views模块中操作状态保持(session)
什么叫状态保持? 就比如说我们登陆一个网站,登陆之后的当前页面显示的是登陆状态,但是我们要再跳转同一网站的其他页面,则显示的未登录状态,状态保持就是:我们在当前页面登陆后,再访问其他页面时也显示为登陆 ...
- gitj基础2
回滚版本 git reset --hard HEAD^ 回滚上一个版本 git reset --hard 版本号(或者版本号前6位) 回滚到指定版本 如果修改版本了,也关 ...
- openwrt redis
2071 make V=s 2072 cd build_dir/target-x86_64_uClibc-0.9.33.2/root-x86/ cd package/network/services/ ...
- 41. wait notify 方法
wait() 等待,如果一个线程执行了wait方法,那么该线程就会进去一个以锁对象为标识符的线程池中等待 notity() 唤醒,如果一个线程执行了notity方法,那么就会唤醒以锁对象为标识符的线 ...
- [转]Netty入门(最简单的Netty客户端/服务器程序)
Java中的NIO是一种解决阻塞式IO问题的基本技术,但是NIO的编写对java程序员是有比较高的要求的.那么Netty就是一种简化操作的一个成熟的网络IO编程框架.这里简单介绍一个程序,代码是< ...