本次博文是cousera的第三次课程，主要涉及slam建图，没有特别复杂的算法，几乎都是概念理解。
我们知道地图，是对机器人所在环境的空间建模(spatial model)。那么，机器人是怎么理解地图得？地图该包含哪些信息？使用什么样的坐标系？如何合理的解析传感器数据？SLAM建图的难点在哪里？
- 地图的种类
- Metric Map
这类地图的位置用坐标表示
- Topological Map
这类地图的位置由节点和它的连接线表达。并且节点之间的相对位置关系，只受连接关系制约。该类地图通常用于路径规划。
- Semantic Map
语义地图，其位置由文字标签和有意义的建筑物图形代替，而不再是坐标。
对于Matric Map建图的难点在于：
- 传感器测量值带有噪声
- 需要将传感器的局部坐标系转换为全局坐标系
- 机器人是一边运动，一边建图的，建图效果受机器人运动规划和导航好坏影响
- 现实世界的物体，可能随着时间一直在变化，理论上地图需要每隔一段时间更新一次
本次作业就是基于Matric Map建图。
- 雷达建图
- 变量及概念梳理
首先，定义occupancy随机变量m，它的取值是0和1二值变量。
随机变量是指，该变量的取值服从某个概率分布，即通过某个概率函数将样本空间投影到实数空间。说白了就是下图，根据每个(x,y)处cell的概率大小取0或1。
由于机器人永远无法确切的感受世界，因此用往往采用概率而非具体0或1状态来表达occupancy地图。该地图每个单元格由贝叶斯滤波器迭代更新。
其次，我们定义传感器观测变量z，它也是二值变量。
- 利用贝叶斯规则更新occupancy地图
接下来，我们就可以根据前一时刻的地图状态，以及当前的观测变量，预测下一时刻的地图状态。
然而直接跟踪每个单元格的概率很难实现，因此我们引入odd变量，简化我们的计算。
那么给定观测变量z时，单元格(x,y)处为非空的概率为：
将贝叶斯规则公式代入上式，得到：
两边取对数，得到：
我们可以用如下符号简单概括上述公式，odd+代表当前时刻地图的状态，odd-表示上一个时刻地图的状态
Occupancy地图的局部更新流程，也就是如下图所示：
Log odd形式下的观测变量，将有两种形式：
更新规则由两点需要注意：
- 仅对观测到的格子进行更新，传感器没扫描到的，不涉及的，不更新
- 当接受到新的观测量后，已经被更新的单元格将变成先验
接下来举例说明occupancy地图如何随着机器人的运动而更新。
我们定义：
Log odd_occ = 0.9
Log odd_free = 0.7
对于下图的t0时刻，每个格子的概率都是0
对于下图的t1时刻，对于测量是free的格子，概率减去0.7，对于是非空格子，概率加上0.9
对于下图的t2时刻，
- 全局坐标转换
假设我们的传感器只能扫描二维平面，并且该传感器由多条射线，每条射线的夹角a已知，每条射线的测距d也已知。另外，已知机器人的pose，即位置(x,y)和朝向theta已知。
由于传感器测量的数据来自机器人自己的坐标系，我们需要把局部坐标系里的传感器读数转化为全局坐标系。此外，我们还要将连续的坐标数值，进行离散化表达，即每个格子是有物理尺寸resol含义的。具体转换公式如下图所示：
这次作业给定了机器人的初始位置，所以不必担心扫描会出界，程序不会出错的。
此外，为了标记和分区free和occupied状态，我们使用bresenham扫描线算法绘图。本次课程不必理解算法细节，已经给定函数了。
Matlab代码如下：
% Robotics: Estimation and Learning
% WEEK 3
%
% Complete this function following the instruction.
function myMap = occGridMapping(ranges, scanAngles, pose, param)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%5
% Parameters
% the number of grids for 1 meter.
myResol = param.resol;
% the initial map size in pixels
myMap = zeros(param.size);
% the origin of the map in pixels
myorigin = param.origin;
% 4. Log-odd parameters
lo_occ = param.lo_occ;
lo_free = param.lo_free;
lo_max = param.lo_max;
lo_min = param.lo_min;
N = size(pose,2);
num_lines = size(ranges,1);
i_occ=zeros(size(ranges,1),2);
for j = 1:N % for each time,
start_x = ceil(pose(1,j)*myResol)+myorigin(1);
start_y = ceil(pose(2,j)*myResol)+myorigin(2);
for k = 1:num_lines
Loc_x = ranges(k,j).*cos(pose(3,j)+scanAngles(k))+pose(1,j);
Loc_y = -ranges(k,j).*sin(pose(3,j)+scanAngles(k))+pose(2,j);
i_occ(k,1) = ceil(Loc_x*myResol) + myorigin(1);
i_occ(k,2) = ceil(Loc_y*myResol) + myorigin(2);
end
for k = 1:size(i_occ,1)
[freex, freey] = bresenham(start_x,start_y,i_occ(k,1),i_occ(k,2));
free = sub2ind(size(myMap),freey,freex);
occupied = sub2ind(size(myMap),i_occ(k,2),i_occ(k,1));
myMap(occupied) = min(myMap(occupied) + lo_occ,lo_max);
myMap(free) = max(myMap(free) - lo_free,lo_min);
end
% figure(2),
% imagesc(myMap); hold on;
% plot(myorigin(1),myorigin(2),'rx','LineWidth',3); % indicate start point
% axis equal;
end
end
效果图如下：
完整的代码地址：
https://github.com/haopo2005/robots_estimation-and-learning/tree/master/hw3