本次博文是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