robots_estimation and learning之occupancy建图

本次博文是cousera的第三次课程，主要涉及slam建图，没有特别复杂的算法，几乎都是概念理解。

我们知道地图，是对机器人所在环境的空间建模(spatial model)。那么，机器人是怎么理解地图得？地图该包含哪些信息？使用什么样的坐标系？如何合理的解析传感器数据？SLAM建图的难点在哪里？

• 地图的种类

1. Metric Map

这类地图的位置用坐标表示

1. Topological Map

这类地图的位置由节点和它的连接线表达。并且节点之间的相对位置关系，只受连接关系制约。该类地图通常用于路径规划。

1. Semantic Map

语义地图，其位置由文字标签和有意义的建筑物图形代替，而不再是坐标。

对于Matric Map建图的难点在于：

1. 传感器测量值带有噪声
2. 需要将传感器的局部坐标系转换为全局坐标系
3. 机器人是一边运动，一边建图的，建图效果受机器人运动规划和导航好坏影响
4. 现实世界的物体，可能随着时间一直在变化，理论上地图需要每隔一段时间更新一次

本次作业就是基于Matric Map建图。

• 雷达建图

1. 变量及概念梳理

首先，定义occupancy随机变量m，它的取值是0和1二值变量。

随机变量是指，该变量的取值服从某个概率分布，即通过某个概率函数将样本空间投影到实数空间。说白了就是下图，根据每个(x,y)处cell的概率大小取0或1。

由于机器人永远无法确切的感受世界，因此用往往采用概率而非具体0或1状态来表达occupancy地图。该地图每个单元格由贝叶斯滤波器迭代更新。

其次，我们定义传感器观测变量z，它也是二值变量。

1. 利用贝叶斯规则更新occupancy地图

接下来，我们就可以根据前一时刻的地图状态，以及当前的观测变量，预测下一时刻的地图状态。

然而直接跟踪每个单元格的概率很难实现，因此我们引入odd变量，简化我们的计算。

那么给定观测变量z时，单元格(x,y)处为非空的概率为：

将贝叶斯规则公式代入上式，得到：

两边取对数，得到：

我们可以用如下符号简单概括上述公式，odd+代表当前时刻地图的状态，odd-表示上一个时刻地图的状态

Occupancy地图的局部更新流程，也就是如下图所示：

Log odd形式下的观测变量，将有两种形式：

更新规则由两点需要注意：

1. 仅对观测到的格子进行更新，传感器没扫描到的，不涉及的，不更新
2. 当接受到新的观测量后，已经被更新的单元格将变成先验

接下来举例说明occupancy地图如何随着机器人的运动而更新。

我们定义：

Log odd_occ = 0.9

Log odd_free = 0.7

对于下图的t0时刻，每个格子的概率都是0

对于下图的t1时刻，对于测量是free的格子，概率减去0.7，对于是非空格子，概率加上0.9

对于下图的t2时刻，

1. 全局坐标转换

假设我们的传感器只能扫描二维平面，并且该传感器由多条射线，每条射线的夹角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