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

合集 - SLAM(55)

1.个人资源索引2018-03-03 2.SLAM学习笔记（1）基本概念2015-08-06 3.SLAM学习笔记（2）SLAM算法2015-08-21 4.[ROS]2 尝试编译OrbSLAM2016-01-18 5.[ROS]1 小乌龟2016-01-18 6.SLAM最近的工作2015-11-05 7.SLAM学习笔记（3）相关概念2015-09-16 8.[SLAM]2D激光线特征提取2016-07-16 9.[SLAM] GMapping SLAM源码阅读（草稿）2016-07-01 10.[QGLViewer]First Demo2016-06-29 11.SLAM数据集2016-06-23 12.[SLAM]2D激光扫描匹配方法2016-06-15 13.[g2o]一个备忘2016-04-21 14.[ROS] slam_gmapping2016-01-24 15.[ROS]3 Linux编程练习2016-01-20 16.Linux学习和ROS安装（1）2017-12-19

17.小豆包的学习之旅：机器人定位2017-05-02

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

1.马尔科夫定位

2.EKF定位

3.栅格定位

4.MCL蒙特卡罗定位

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

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