1. 몬테 카를로 현지화
위치 : 자신의 위치 (현지화)을 결정하기 위해 센서 정보의지도 정보를 사용하는 방법을 아는 로봇.
일부는 그 위치가지도 정보가 필요하지 않습니다 말할 것입니다. 바퀴의 속도에 대해 알고 알고 로봇의 초기 위치, 그것은 많은 라운드가 거리를 간 후 위치 정보를 업데이트하기 위해, 로봇의 각도 변위를 계산 한 방법에 대한 일정 기간에 걸쳐 계산 될 수있다. 그러나 분명히,이 방법에 큰 문제가있는 것입니다. 또, 로봇과 같은 기계적인 문제와 같은 슬립있을] 우선, 속도 센서를 얻을 수 있지만, 센서의 정확도는 심지어 더 큰 에러는 거리를 찾기 위해 시간에 걸쳐 적분되어, 오류가 있음을 의미하는 제한된다. 로봇의 위치를지도를 바인딩 효과적으로 오류를 줄일 수 있습니다.
센서 측면. 우리는 레이저 센서를 사용하여, 상기 로봇의 모든 방향에서 가장 가까운 장해물과의 거리를 측정 할 수있다. 각 시점에서, 로봇은 레이저 센서의 측정치를 획득한다. 이하, 로봇이 녹색 삼각형, 빨간색 선은 레이저 빔, 레이저 가까운 장애물의 방향에서 검출 노란색 그리드 로봇이다.
지도는 래스터지도 (점유 격자지도)를 차지한다. 예를 들어, 다음지도, 밝은 색 (흰색) 상자 장애물이 어두운 (검은 색) 상자가 비어있는 위치를 나타냅니다.
음,이 시점에서, 우리는 (아래 그림 참조)지도 정보에 맞춰 가능한 한 많은 레이저 센서 판독을 위해 만드는지도에 로봇을 넣어하는 것입니다 필요가있다.
따라서, 시점의 위치 결정을 위해 최적의 문제를 해결하는 기능이된다. 그러나,이 최적화 기능 해결 너무 어렵다 (좌표와 각도를 연속적으로 변화하고 값 맵은 그리드이다).
우리는 두 가지를주의해야합니다. 먼저, 소정의 로봇의 위치 정보에 대해, 우리는 쉽게 착용감지도의 정도를 계산할 수 있으며, 둘째, 인접하는 2 개 개의 시점이 로봇의 위치에서 크게 변하지 않을 것이다. 이 두 점을 바탕으로, 우리는 몬테 카를로 방법을 찾아 리드. 핵심 아이디어는 로봇의 가능한 위치를 설명하는 입자의 수가 많은 가우시안 잡음 로봇 위치 정보의 배포를 설명한다.
具体来说，假如估测的机器人位置信息为（
和
表示坐标，
表示机器人朝向），我们会记录机器人的位置信息符合
（
视具体情况而定）的多元高斯分布。在算法实现中，我们用高斯分布采样出的
个粒子来表示机器人的位置。如下左图所示的单元高斯模型，下面蓝色的点是采样的粒子，上面是对应的高斯分布的模型；如下右图所示是二元高斯模型采样的情况。
蒙特卡罗定位法分为四步：
下面，我们将结合具体例子来说明算法的步骤。
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,1)*size(map,2), 1)); N = size(ranges, 2); % number of poses to calculate myPose = zeros(3, N); % initialize return value resol = param.resol; % map resolution origin = param.origin; % origin in pixel sig = [0.08, 0, 0; 0, 0.08, 0; 0, 0, 0.08]; % noise for particle movement myPose(:,1) = param.init_pose; % init position M = 200; % number of particles P = repmat(myPose(:,1), [1, M]); thr = ceil(3.5/5*size(angles,1)); % set the score threshold as 70% for j = 2:N maxscore = 0; while maxscore < thr Q=P+(randn(size(P,2),3)*sig)'; % particles movement score = zeros(size(Q,2), 1); % scores for k = 1:size(Q,2) % calculate score for each particle occ_x = ceil( (ranges(:,j) .* cos(angles+Q(3,k)) + Q(1,k) ) * resol + origin(1) ); occ_y = ceil( (-ranges(:,j) .* sin(angles+Q(3,k)) + Q(2,k) ) * resol + origin(2) ); ids = occ_x > 0 & occ_x <= size(map,2) & occ_y > 0 & occ_y <= size(map,1); score(k) = size( map( map (sub2ind(size(map), occ_y(ids), occ_x(ids)) ) > unknown ), 1); 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, 1, ceil(M/size(Q,2)) ); % regenerate particles end end
于是，我们就得到了测试数据的定位图了：
总结
经典的定位算法——蒙特卡罗定位法，最早的论文可见"Monte Carlo localization for mobile robots"，这种方法十分实用。当然，我们只讲了地图已知的情况下如何定位。在地图未知的时候，Mapping和Localization需要同时进行，也就是SLAM，这四个步骤就会有很多变化，但是算法的框架并不会变。
原文链接：https://zhuanlan.zhihu.com/p/21974439