モンテカルロ局在（粒子フィルタローカライズ）

1.モンテカルロ局在

ポジショニング：ロボットが自分の位置（ローカライズ）を決定するためにセンサ情報の地図情報を使用する方法を知っています。

いくつかは、その位置が地図情報を必要とされていないと言うでしょう。ロボットの初期位置は、車輪の速度について知っている知っている、それは距離を行って、その後、ロボットの角度変位を計算し、位置情報を更新するためにされたどのように多くのラウンド約期間にわたって計算することができます。しかし、明らかに、このアプローチには大きな問題があります。また、ロボットはまた、機械的な問題は次のように滑る可能性があります。まず、速度センサが得られる、しかし、センサーの精度はさらに大きくなり、エラーが距離を見つけるために時間をかけて統合され、エラーがあることを意味し、限定されています。ロボットの位置を特定するためにマップをバインドすると効果的にエラーを減らすことができます。

センサーの様相。私たちは、レーザーセンサーを使用し、ロボットとすべての方向に最も近い障害物との距離を測定することが可能です。各時点で、ロボットは、レーザセンサの測定値を得るであろう。以下、ロボットは緑色の三角形で、赤い線は、レーザビーム、レーザに最も近い障害物の方向で検出黄色グリッドロボットです。

地図は、ラスター地図（占有グリッドマップ）占めます。例えば、次のマップ、淡色（白色）ボックスは、障害物を示し、暗い（黒い）ボックスは空の位置を示します。

さて、この時点で、私たちは何をする必要があります（下図のように）マップ情報に合わせて可能な限りのレーザーセンサーの読み取りのために作り、マップにロボットを置くことです。

このように、ある時点の位置決めのためには、最適な問題解決の関数となります。しかしながら、これは、（座標及び角度が連続的に変化しており、値マップはグリッドである）最適化関数を解決するには余りにも困難です。

我々は2つの事柄に注意する必要があります。まず、与えられたロボットの位置情報のため、我々は簡単にフィット感とマップの度合いを計算することができ、第二、隣接する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