小豆包的学习之旅：机器人定位

　　《概率机器人》一书用两章介绍了几种定位方法，一种是基于马尔科夫决策，另外一章是栅格和蒙特卡罗方法。

1.马尔科夫定位

2.EKF定位

3.栅格定位

4.MCL蒙特卡罗定位

　　这里主要学习一下蒙特卡罗定位。机器人定位问题可以描述为：如何确定机器人在关联的已知环境地图中的位姿pose。抛开SLAM，定位问题可以单独讨论，目前的室内定位技术研究也很火热。

　　地图表达的是全局坐标系，地图独立于机器人的位姿而存在。定位问题也可以描述为确定机器人局部坐标系统在全局坐标系中对应关系的过程，即确定机器人的位姿$x_{t}=(x,y, heta)^T$。

机器人定位的必要性

　　有人会说，定位是不需要地图信息的。机器人知道初始位置，知道左右轮的速度，就可以算出在一段时间内左右轮分别走了多少距离，进而算出机器人的转角和位移，以便更新位置信息。但是显然，这种方法存在很大的问题。首先，速度是传感器获得的，然而传感器是有精度限制的，这就意味着误差的存在，对时间积分求距离误差就更大了；另外，机器人也可能存在打滑之类的机械问题。（文献1是一篇很好的文章，介绍的很详细，主要介绍的内容来源于开放课程：机器人学：估计和学习）

　　结合地图来对机器人进行定位能有效减小误差。

　　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

　　

参考文献：

（1）蒙特卡罗定位（Particle Filter Localization）https://zhuanlan.zhihu.com/p/21974439

（2）Robotics-Estimation-and-Learning https://www.coursera.org/learn/robotics-learning/home/welcome

（3）作业 https://github.com/codetaobeibei/Coursera-Robotics-Estimation-and-Learning