本次课程讲解，假设在给定地图的情况下，如何根据机器人测量进行定位，如何使用机器人定位算法估计每个时刻机器人的位姿。作业是利用粒子滤波求解机器人定位或跟踪问题。
一、里程模型Odometry Model
1.各种定位传感器概述
在汽车全球导航应用里，用于定位的信息源有gps，通信运营商基站，wifi热点。这几类信息的精度各不相同，gps是3.5米，基站是600米，wifi是40米。但是对于无人驾驶的车辆，定位精度要求更高，远远小于3.5米，比如要区分车行道与人行道。上述设备不能满足要求。
里程计就是要通过局部坐标找到机器人在世界坐标里的位置。里程计的局部测量精度更加精确，有如下几种设备：
*陀螺仪[没接触过，据说是0.1度，指标比较多]
*RGB-D深度摄像头，1-10厘米
*激光雷达，3-5厘米
*编码轮Encoder，200,000 pulse/m [没接触过，不知道单位这个啥概念]
但是这些设备测量的都是局部信息，由于长时间的累计误差，最后全局的定位结果会出现漂移drift现象。可以通过使用高精度地图定位，解决这种累计误差。
2.基于编码轮的定位
接下来以编码轮为例，介绍下如何进行全局定位。编码轮可以记录小车四个轮子各自的转速，我们可以通过内外轮子的转速差，计算小车的位置以及运动的朝向。
我们分别定义内径ei和外径eo的弧长公式如下，
ri为内轮运动半径，ro为外轮运动半径，则两个轮子之间的距离w为，
这样，我们就能计算小车转向的角度theta，
有了小车的运动角度，咱就能计算小车的全局坐标了，
由于编码论计算车轮转速时有噪声，小车运动角度的误差会传播到全局坐标的计算。可以用陀螺仪作为更精确的瞬时角度计量工具。
假设陀螺仪可以给定某时刻的角度theta’，结合编码轮的弧长计算，新的全局坐标为，
上述两种传感器都是基于局部信息来计算世界坐标的，随着时间推移都是不可靠的。编码轮有slippage和missing counts问题，即使融合了陀螺仪后，仍然有drift问题。因此，需要高精度世界地图来纠正定位误差。
二、地图配准
接下来，以雷达测量和occupancy地图为例，介绍下地图配准map registration问题。
雷达深度传感器，基于极坐标，可以持续的读取各个离散角度方向的深度。
关于地图继续沿用上一节的occupancy地图，白色区域为有障碍物obstacle的地方，灰色区域为空闲区域free
所谓地图配准问题，就是根据机器人的传感器测量数值来计算机器人的位姿，以此最好的适配地图。
目标函数如下，寻找最佳位姿p，
函数m(x,y)代表地图空闲或不空闲的，位姿p包含位置(x,y)及朝向p_theta，r_theta代表雷达不同角度的射线，函数delta表示从地图中获取当前位姿p下的地图数值。
直接求偏导什么的，计算量太大，不切实际。接下来我们将用粒子滤波算法，求解上述目标函数。
三、粒子滤波进行地图配准
关于粒子滤波，我之前一篇博文详细介绍过。
应用背景不同，一个是基于roi区域的颜色的直方图距离计算用于目标跟踪，一个是基于occupancy地图距离计算用于定位。
粒子滤波是一种基于采样的随机状态估计方法，它用一组采样粒子代表概率密度分布，每一个粒子代表一种状态(不同粒子代表不同位姿及其权重)，因此这里不像混合高斯模型一样有均值，有方差。如下图所示，用粒子分布的密集程度来反映概率分布。
粒子滤波算法包含步骤
a.初始化粒子
每个粒子包含位姿和权重，两种数值。这些粒子代表潜在的小车定位。粒子颜色越深，权重越大。初始时刻，每个粒子的权重相同。
b.更新里程计
基于雷达的测量信息更新粒子，同时加入高斯噪声，拟合雷达测量时的噪声，Pi为位姿。
注意任何时候，粒子的数量都保持不变。
c.更新粒子权重
粒子的权重基于不同位姿下的雷达测量与occupancy地图的距离，也就是下图中各个散射的线段端点是否都与occupancy的obstacle契合。
d.粒子重新采样
根据N个粒子的权重，计算归一化累计权重数组(一定是递增的)。然后生成N个0-1的随机小数，在累计权重数组中从小到大依次寻找第一个比这个随机数大的累计权重，用该累计权重索引代表的粒子作为新的粒子集的粒子。说白了，就是这些粒子按照一定的规则重新排序。至于有效粒子数量这个准则，我没用使用。我采取每帧粒子更新后都要重采样。
这次cousera课程外国小哥的讲课体验很差，所以我这次博文写的也很难理解，建议回顾我之前的博文。
本次作业，给定雷达3701帧的测量矩阵和occupancy地图，计算每帧小车在occupancy地图中的坐标。Matlab代码如下：
% Robotics: Estimation and Learning
% WEEK 4
%
% Complete this function following the instruction.
function myPose = particleLocalization(ranges, scanAngles, map, param)
% Number of poses to calculate
N = size(ranges, 2);
% Output format is [x1 x2, ...; y1, y2, ...; z1, z2, ...]
myPose = zeros(3, N);
[h,w]=size(map);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%5
% Map Parameters
%
% % the number of grids for 1 meter.
myResolution = param.resol;
% % the origin of the map in pixels
myOrigin = param.origin;
%myMap = zeros(size(map));
% The initial pose is given
myPose(:,1) = param.init_pose;
% You should put the given initial pose into myPose for j=1, ignoring the j=1 ranges.
% The pose(:,1) should be the pose when ranges(:,j) were measured.
% thresh = graythresh(map); %自动确定二值化阈值
% I2 = im2bw(map,thresh); %对图像二值化
auto_thresh = mode(reshape(map, size(map,1)*size(map,2), 1));
% Decide the number of particles, M.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
M = 800; % Please decide a reasonable number of M,
% based on your experiment using the practice data.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Create M number of particles
P = repmat(myPose(:,1), [1, M]);
weights = zeros(1,M);
acc_weights = zeros(1,M);
for k=1:1:M
weights(k) = 1/M;
end
% num_lines = size(ranges,1);
% i_occ=zeros(size(ranges,1),2,M);
for j = 2:N % You will start estimating myPose from j=2 using ranges(:,2).
% 1) Propagate the particles
noise_sigma = diag([0.1 0.1 0.02]);
rand_matrix = noise_sigma*randn(3,M);
P = P + rand_matrix;
% 2) Measurement Update
% 2-1) Find grid cells hit by the rays (in the grid map coordinate frame)
for m = 1:M
d_occ = 0;
x_o = ranges(:,j) .* cos(scanAngles + P(3,m)) + P(1,m);
y_o = -ranges(:,j) .* sin(scanAngles + P(3,m)) + P(2,m);
occ_x = ceil(x_o*myResolution)+myOrigin(1);
occ_y = ceil(y_o*myResolution)+myOrigin(2);
occ_x_ = occ_x';
occ_y_ = occ_y';
del_occ = occ_x_<1 | occ_y_<1 | occ_x_ > size(map,2) | occ_y_ > size(map,1);
occ_x_(del_occ) = [];
occ_y_(del_occ) = [];
occupied = sub2ind(size(map),occ_y_',occ_x_');
d_occ = d_occ - sum(sum(map(occupied) <= auto_thresh));
d_occ = d_occ + sum(sum(map(occupied) > auto_thresh))*10;
weights(m) = max(0,d_occ);
end
% for k = 1:num_lines
% Loc_x = ranges(k,j).*cos(P(3,:)+scanAngles(k))+P(1,:);
% Loc_y = -ranges(k,j).*sin(P(3,:)+scanAngles(k))+P(2,:);
% temp_x = ceil(Loc_x*myResolution) + myOrigin(1);
% temp_y = ceil(Loc_y*myResolution) + myOrigin(2);
% for mm=1:1:M
% if temp_x(mm) >=1 && temp_x(mm) < size(map,2)
% i_occ(k,1,mm) = temp_x(mm);
% %flag(k,mm) = flag(k,mm)+1;
% end
% if temp_y(mm) >=1 && temp_y(mm) < size(map,1)
% i_occ(k,2,mm) = temp_y(mm);
% %flag(k,mm) = flag(k,mm)+1;
% end
% end
% end
% for q=1:1:M
% d_occ = 0;
% occupied = sub2ind(size(map),i_occ(:,2,q),i_occ(:,1,q));
% d_occ = d_occ - sum(sum(map(occupied) <= auto_thresh))*1;
% d_occ = d_occ + sum(sum(map(occupied) > auto_thresh))*20;
% weights(q) = d_occ;
% end
weights = weights / sum(weights);
%[Max_,Ind_] = max(weights);
%disp(Max_)
%myPose(:,j) = P(:,Ind_);
myPose(:,j) = P*weights';
% P = repmat(myPose(:,j), [1, M]);
acc_weights(1) = weights(1);
for k=2:1:M
acc_weights(k) = acc_weights(k-1) + weights(k);
end
acc_weights = acc_weights / sum(weights);
qq = P;
for k=1:1:M
temp_index = M;
temp = rand();
for q=1:1:M
if temp <= acc_weights(q)
temp_index = q;
break;
end
end
P(:,k) = qq(:,temp_index);
end
% 2-2) For each particle, calculate the correlation scores of the particles
% 2-3) Update the particle weights
% 2-4) Choose the best particle to update the pose
% 3) Resample if the effective number of particles is smaller than a threshold
% 4) Visualize the pose on the map as needed
end
end实验截图如下：
参数调试到怀疑人生，如果每次运行结果不一样的话，就添加更多的粒子吧。。。
这位朋友的代码也蛮不错的，但是跟课程介绍的偏差比较多了。
https://zhuanlan.zhihu.com/p/21974439
本次博文代码地址：
https://github.com/haopo2005/robots_estimation-and-learning/tree/master/week_four
