cartographer CSM理解

格网地图怎么建立？

参考《概率机器人》

M(i,j)为二维地图像素点取值大小,(i,j)为坐标，如M(17,867)=255，即在地图上坐标
(17,867)那点像素点的值为255.M(17,867)具体含义就是占据概率大小，假如P代表这点是障碍物的概率，那么1-P为空的概率，但是概率取值为0到1的数，为了方便计算，我们取它们的比值$\frac{p}{1-p}$,但是为了避免出现无穷大数值计算不稳定，再取$M=\ln \frac{p}{1-p}$，则可以反过来得到$P=1-\frac{1}{1+e^M}$，再继续限制M取值为-128到127，当M=127时，P基本近似为1，当M=-128时P基本近似为0，实际操作中M再加上128偏移量（计算概率时减去偏移量）这样刚好取值为0到255，即为图像灰度值。这样做的好处之一是确保了数值计算的稳定性。

好处之二为，当占据概率增加时，例如$M=\frac{0.6}{0.4}$到$M=\frac{0.8}{0.2}$时，M其实增加了一个正数，反之概率减小时M加上一个负数，概率问题又一次转化为简单的加加减减运。实际计算时我们统一每次增加一个相同正数或者负数，比如取负数-9，正数9（这个可以自由设置，看实际效果，如果某个点值加9超过了127，就直接取值127，反之某个值减9小于-128了就直接取值-128）。实际上最后基本不会用P值，而用M值代替。比如刚开始时地图的每个像素值$M=\ln \frac{0.5}{0.5}=0$，最后根据激光雷达测量数据在每个像素点值上加加减减即可，可能最后有些点的值为负了，有些正了，等等。

另外一个问题是激光雷达数据怎么转化为占据值（实际测量值转化为地图表示）：假如激光雷达在原点(X,Y,Theta)=(0,0,0),X,Y为为平面坐标，Theta为角度值（即方向，相对于X轴正方向，顺时针负，逆时针正，取值$(-\pi ,\pi ]$。扫描环境产生了数据帧。激光雷达数据扫描角为270°，从-135°到135°（也可认为0到270°），间隔为建分辨率（根据扫描仪类型定义，例如：0.25°），一帧数据有固定点数（根据扫描仪类型定义，例如：1000个点），一秒钟最多产生固定帧（根据扫描仪类型定义，例如20帧）。每帧数据都是按角度顺序排列（-135°，-134.75°，-134.5°，…，134.75°，135°）产生的测量值。假如激光测量值得0°对应实际我们定义的坐标系Theta=0,那么激光雷达数据对应-135°的测量值就是实际坐标系中与X轴正方向偏离-135°的射线长度，假定激光测量值在有效范围内，假如取值为11米。得到直线$y=x,x\in [-\frac{11}{\sqrt{2}},0]$,端点坐标为$[-\frac{11}{\sqrt{2}},\frac{11}{\sqrt{2}}]$，近似$[-7.77817,7.77817]$.假设地图中一个像素代表0.05m，那么实际坐标值对应地图上的$[-\frac{-7.77817}{0.05}，\frac{-7.77817}{0.05}]=[-155.6,155.6]$，但是像素点坐标一般为正的，需要加偏移值让它为正就行了，像素点坐标是整数,因此需要取整，可以取$[-155,-155]$或$[-156,-156]$这样单个像素点，也可以取四个点$[-155,-155]$,$[-155,-156]$,$[-156,-155]$,$[-156,-156]$,选中这些点值都加9，因为显然遇到障碍物了。端点值计算好以后计算直线上的点，显然计算时会出现坐标不是整数的情况，可以用一个简单的办法，Bresenham算法或者类似算法实现.线条上的像素值减9，因为肯定没被挡住才有这条线。剩下的就是把一帧中另外1000多个点数据按部就班的计算，对应像素值加加减减。实际中由于不停地扫描，同一个像素点被多次加或减，最后就可以得到灰度值不一样的地图了。

栅格地图是如何构建的（以二维激光雷达为例）

二维激光雷达传感器会扫描出空间中某一高度的平面，相当于给整个环境来一刀，得到一个切面（内侧）。栅格图中的线条对应环境中障碍物的内侧边界。

雷达扫描过程中，光线撞到（hit）的地方所处坐标位置就对应一种灰度值，没有撞到（miss）的地方所处坐标位置对应另外的灰度值。栅格图中，一个格子某一时刻只有两种状态，要么被光线撞到了要么没有；但是该格子可能多次被撞到或多次没被撞到。如果某个格子多次被撞到了，说明它很有可能是被占用了（有障碍物的意思）。因此，咱们可以用概率的方式来表示某个格子有多大的可能被占用了。

那么问题来了，一方面，格子的概率如何表示？另一方面，格子多次被撞这种事情的概率要怎么算？

第一个问题，咱们可以利用图像的灰度值来表达概率，只要有一张“灰度 – 概率”对应表。如下图所示，纵坐标可以换算为八位的灰度值，横坐标为概率值。图中曲线为单调函数，这样就能从灰度图中直观的看出概率大小（颜色越深代表可能性越大/小）。一般情况下，纯黑色（灰度值为0）概率为0，代表占用；灰色（灰度值为127）概率为0.5，代表未知；纯白色（灰度值为255）概率为1，代表空闲。

怎么计算激光的概率以及似然值？

占据栅格地图（Occupancy Grid Map）的概念的提出

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

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

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

Scans

一个 scans即激光点云图，包含一个起点和许多的终点。起点称为origin，终点称为scan points，用 H 表示点云集，其表达形式如下。

H = { h k } k = 1 , . . . , K H=\{h_k\}_k=1,...,K H={hk​}k​=1,...,K

当获得一个新的scans，并且要插入到submap中时，scans中点集在submap中的位置被表示成 ，其转换公式如下：
$$T_{\xi }p=\begin{array}{c}\underbrace{\left\{\begin{array}{cc}cos{\xi }_{\theta }& -sin{\xi }_{\theta }\\ sin{\xi }_{\theta }& cos{\xi }_{\theta }\end{array}\right\}}\\ R_{\xi }\end{array}P+\begin{array}{c}\underbrace{\left\{\begin{array}{c}{\xi }_x\\ {\xi }_x\end{array}\right\}}\\ t_{\xi }\end{array}$$

每一帧的scans都会生成一组称为hits的栅格点集和一组称为misses的栅格点集 ,如图所示

其中阴影带叉的表示hits,阴影不带叉的表示misses. 每个hits中的栅格点被赋予初值 $p_{hit}$（这里理解为固定值）每个misses中的栅格点被称为 $p_{miss}$（这里理解为固定值，且$p_{miss}$=1- $p_{hit}$） ,如果栅格点 $x$ 在先前已经有 $p$ 值，则用下述公式对该栅格点的值进行更新（此处为$p_{hit}$ ，$p_{miss}$类似）。

$$odds(p)=\frac{p}{1-p}$$

$$M_{new}(x)=clamp(odd{s}^{-1}(odds(M_{old}(x))\ast odds(p_{hit})))$$
等价于
$$M_{new}(x)=clamp(\frac{1-odds(M_{old}(x))\ast odds(p_{hit})}{odds(M_{old}(x))\ast odds(p_{hit})})$$

其中：clamp是区间限定函数；

p表示针对格网点$x$的被占据的值（$1-p$表示未被占据的值）；

$odds(p)$表示该点的状态（该值大于1表示被占据，小于1表示未被占据，由两个值（$p_{hit}$，$p_{miss}$）表示状态变为一个值 表示状态）；

M表示格网概率。

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

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

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

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

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

考虑一维的情况：

图中$x$是真实世界中的坐标，$i$为离散化了的地图（栅格地图）中的坐标，$r$为一格的长度，$1/r$表示分辨率，显然我们有：$i=ceil(x/r)$。
同理，二维情况下：$(i,j)=(ceil(x/r),ceil(y/r))$。

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

假设机器人的状态为$(x,y,\theta )$，激光与机器人朝向的夹角为$$，测量的障碍物的距离为 $\alpha$（图中未标明）：

计算障碍物所在点的实际位置：
$x_0=dcos(\theta +\alpha )+x$
$y_0=dsin(\theta +\alpha )+y$

再计算障碍物在栅格地图中的位置$(i_0,j_0)$，以及机器人在栅格地图中的位置$(i,j)$。根据这两个坐标可以使用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;

参考网址:

https://blog.csdn.net/haishaoli/article/details/79647594