发布于 由quickembed

占据栅格地图

激光传感器会向固定的方向发射激光束，发射出的激光遇到障碍物会被反射，这样就能得到激光从发射到收到的时间差，乘以速度除以二就得到了传感器到该方向上最近障碍物的距离。

这样看来，似乎利用激光传感器，机器人能够很好地完成Mapping这一任务。但是我们前面提到了，传感器数据是有噪音的。例如，假如我们在此时检测到距离障碍物4米，下一时刻检测到距离障碍物4.1米，我们是不是应该把4米和4.1米的地方都标记为障碍物？又或者怎么办呢？

为了解决这一问题，我们引入占据栅格地图（Occupancy Grid Map）的概念。

我们首先来解释这里的占据率（Occupancy）指的是什么。在通常的尺度地图中，对于一个点，它要么有（Occupied状态，下面用1来表示）障碍物，要么没有（Free状态，下面用0来表示）障碍物（旁白：那么问题来了，薛定谔状态呢？）。在占据栅格地图中，对于一个点，我们用[公式]来表示它是Free状态的概率，用[公式]来表示它是Occupied状态的概率，当然两者的和为[公式]。两个值太多了，我们引入两者的比值来作为点的状态：[公式]。

 

对于一个点，新来了一个测量值（Measurement，[公式]）之后我们需要更新它的状态。假设测量值来之前，该点的状态为[公式]，我们要更新它为：[公式]。这种表达方式类似于条件概率，表示在[公式]发生的条件下[公式]的状态。

根据贝叶斯公式，我们有：

[公式]

带入之后，我们得

[公式]

我们对两边取对数得：

[公式]

这样，含有测量值的项就只剩下了[公式]。我们称这个比值为测量值的模型（Measurement Model），标记为[公式]。测量值的模型只有两种：[公式]和[公式]，而且都是定值。

这样，如果我们用[公式]来表示位置[公式]的状态[公式]的话，我们的更新规则就进一步简化成了：[公式]。其中[公式]和[公式]分别表示测量值之后和之前[公式]的状态。

另外，在没有任何测量值的初始状态下，一个点的初始状态[公式]。

 

经过这样的建模，更新一个点的状态就只需要做简单的加减法了。这，就是数学的魅力。

例如，假设我们设定[公式]，[公式]。那么， 一个点状态的数值越大，就表示越肯定它是Occupied状态，相反数值越小，就表示越肯定它是Free状态。

上图就展示了用两个激光传感器的数据更新地图的过程。在结果中，一个点颜色越深表示越肯定它是Free的，颜色越浅表示越肯定它是Occupied的。

用激光传感器构建占据栅格地图

前面讲到通常用激光传感器数据来构占据栅格地图，这一节我们将详细介绍其中的实现细节。具体来说，我们需要编写函数：

function myMap = occGridMapping(ranges, scanAngles, pose, param)

其中，scanAngles是一个[公式]的数组，表示激光传感器的[公式]个激光发射方向（与机器人朝向的夹角，定值）；ranges是一个[公式]的数组，表示[公式]个时间采样点激光传感器的读数（距离障碍物的距离）；pose是一个[公式]的数组，表示[公式]个时间采样点机器人的位置和朝向信息（前两维为位置，第三维为朝向角度）；param是一些传入的参数，param.origin是机器人的起点，param.lo_occ和param.lo_free分别是第二节中的[公式]和[公式]，param.max和param.min表示位置状态的阈值（超过则置为阈值边界），param.resol表示地图的分辨率，即实际地图中一米所表示的格点数目，param.size表示地图的大小。

首先，我们解决如何将真实世界中的坐标转化为栅格地图中的坐标。

考虑一维的情况：

图中[公式]是真实世界中的坐标，[公式]为离散化了的地图（栅格地图）中的坐标，[公式]为一格的长度，[公式]表示分辨率，显然我们有：[公式]。

同理，二维情况下：[公式]。

其次，我们来计算每一条激光所检测出的障碍物和非障碍物在栅格地图中的位置。

假设机器人的状态为[公式]，激光与机器人朝向的夹角为[公式]，测量的障碍物的距离为[公式]（途中未标明[公式]，不好意思）：

计算障碍物所在点的实际位置：[公式]
[公式]

再计算障碍物在栅格地图中的位置[公式]，以及机器人在栅格地图中的位置[公式]。根据这两个坐标可以使用Bresenham算法来计算非障碍物格点的集合。

最后，利用第二节中的结论，我们使用简单的加减法不断更新格点的状态即可。

完整的Matlab代码如下：

function myMap = occGridMapping(ranges, scanAngles, pose, param)

resol = param.resol; % the number of grids for 1 meter.
myMap = zeros(param.size); % the initial map size in pixels
origin = param.origin; % the origin of the map in pixels

% Log-odd parameters 
lo_occ = param.lo_occ;
lo_free = param.lo_free;
lo_max = param.lo_max;
lo_min = param.lo_min;

lidarn = size(scanAngles,1); % number of rays per timestamp
N = size(ranges,2); % number of timestamp

for i = 1:N % for each timestamp
    theta = pose(3,i); % orientation of robot
    % coordinate of robot in real world
    x = pose(1,i);
    y = pose(2,i);

    % local coordinates of occupied points in real world
    local_occs = [ranges(:,i).*cos(scanAngles+theta), -ranges(:,i).*sin(scanAngles+theta)];

    % coordinate of robot in metric map
    grid_rob = ceil(resol * [x; y]);

    % calc coordinates of occupied and free points in metric map
    for j=1:lidarn
        real_occ = local_occs(j,:) + [x, y]; % global coordinate of occ in real world
        grid_occ = ceil(resol * real_occ); % coordinate of occ in metric map

        % coordinates of free in metric map (by breshnham's algorithm)
        [freex, freey] = bresenham(grid_rob(1),grid_rob(2),grid_occ(1),grid_occ(2));

        % convert coordinate to offset to array
        free = sub2ind(size(myMap),freey+origin(2),freex+origin(1));
        occ = sub2ind(size(myMap), grid_occ(2)+origin(2), grid_occ(1)+origin(1));

        % update metric map
        myMap(free) = myMap(free) - lo_free;
        myMap(occ) = myMap(occ) + lo_occ;
    end
end

% reassign value if out of range
myMap(myMap < lo_min) = lo_min;
myMap(myMap > lo_max) = lo_max;

使用课程给的数据，我们最终画出下面这样的占据栅格地图（用灰度图显示出来　的）：

发表回复