三种建图算法的原理（覆盖栅格建图算法、计数建图算法、TSDF建图算法）

机器人地图的分类

尺度地图（Metric Map）

机器人领域中常见的尺度地图有栅格地图和点云地图。它的好处是可以用一个唯一的坐标描述一个具体的位置。

拓扑地图（Topological Map）

拓扑地图用节点表示每一个位置，位置与位置连接的线就是可以通行的路。拓扑地图一般用于大地图，比如国家公路网和城市地图路线图等等。

语义地图（Semantic Map）

语义地图会用标签来描述地点。如手机里的地图APP。

覆盖栅格建图算法

特点

• 把环境分解成一个一个的小栅格。常用0.05m的正方形栅格
• 每个栅格有两种状态：占用(Occupied)或者空闲(free)
• 非参模型
• 随着地图的增大，内存需求急剧增加
• 天然区分可通行区域，适合进行轨迹规划

数学描述

给定机器人的位姿和传感器的观测数据(主要指激光雷达)。

dat a = { x 1 , z 1 , x 2 , z 2 , ⋯ x n , z n } a=\left\{x_{1}, z_{1}, x_{2}, z_{2}, \cdots x_{n}, z_{n}\right\} a={x1​,z1​,x2​,z2​,⋯xn​,zn​}

估计出最可能的地图

$m^{\ast }=\arg {\max }_{m}P(m|data)$

$m^{\ast }=\arg {\max }_{m}P\left(m|x_{1:t},z_{1:t}\right)$

假设

• 栅格地图中的栅格是一个二元随机变量，只能取两个值：占用(Occupied)或者空闲(Free)
• 𝑝 𝑚𝑖 = 1表示被占用， 𝑝 𝑚𝑖 = 0表示空闲， 𝑝 𝑚𝑖 = 0.5表示不知道(Unknown)
• 在建图的过程中，环境不会发生改变
• 地图中的每一个栅格都是独立的，因此数学表达式可以表示为：

$\mathrm{p}(m)=\prod p\left(m_i\right)$

• 地图估计问题表示为：

$\mathrm{p}\left(m|x_{1:t},z_{1:t}\right)=\prod p\left(m_i|x_{1:t},z_{1:t}\right)$

由上可知，估计环境的地图只需要对每一个独立的栅格进行估计即可。

公式推导

$m_i$是一个二元随机变量，因此：

$\begin{array}{rl}\mathrm{p}\left(m_i|x_{1:t},z_{1:t}\right)& =\frac{p\left(z_t|m_i,z_{1:t-1},x_{1:t}\right)p\left(m_i|z_{1:t-1},x_{1:t}\right)}{p\left(z_t|z_{1:t-1},x_{1:t}\right)}\\ & =\frac{p\left(z_t|m_i,x_t\right)p\left(m_i|z_{1:t-1},x_{1:t-1}\right)}{p\left(z_t|z_{1:t-1},x_{1:t}\right)}\end{array}$

$m_i$表示第$i$个栅格的状态
$z_{1:t}$表示$1:t$时刻的激光数据
$x_{1:t}$表示$1:t$时刻的机器人位姿

上面的公式其实就是贝叶斯公式。将$z_{1:t}$分成了$z_{1:t-1}$和$z_t$。因为每帧激光数据都是独立的，所以$p(z_{1:t-1})$和$p(z_t)$是互不影响的。根据Markov，$z_t$只与当前机器人位姿有关，跟之前的位姿无光。则上面可以有$p(z_t|m_i,z_{1:t-1},x_{1:t})=p(z_t|m_i,x_t)$。同理可得，$p(m_i|z_{1:t-1},x_{1:t})=p(m_i|z_{1:t-1},x_{t-1})$。

其中

$\mathrm{p}\left(z_t|m_i,x_t\right)=\frac{p\left(m_i|z_t,x_t\right)p\left(z_t|x_t\right)}{p\left(m_i|x_t\right)}$

$m_i$是一个二元随机变量，因此：

$\begin{array}{rl}\mathrm{p}\left(m_i|x_{1:t},z_{1:t}\right)& =\frac{p\left(m_i|z_t,x_t\right)p\left(z_t|x_t\right)}{p\left(m_i|x_t\right)}\frac{p\left(m_i|z_{1:t-1},x_{1:t-1}\right)}{p\left(z_t|z_{1:t-1},x_{1:t}\right)}\\ & =\frac{p\left(m_i|z_t,x_t\right)p\left(z_t|x_t\right)}{p\left(m_i\right)}\frac{p\left(m_i|z_{1:t-1},x_{1:t-1}\right)}{p\left(z_t|z_{1:t-1},x_{1:t}\right)}\end{array}$

上式中$m_i$和$x_t$是相互独立的。所以有$p(m_i|x_t)=p(m_i)$。

同理，对于$\neg m_i$：

$\begin{array}{rl}\mathrm{p}\left(\neg m_i|x_{1:t},z_{1:t}\right)& =\frac{p\left(\neg m_i|z_t,x_t\right)p\left(z_t|x_t\right)}{p\left(\neg m_i|x_t\right)}\frac{p\left(\neg m_i|z_{1:t-1},x_{1:t-1}\right)}{p\left(z_t|z_{1:t-1},x_{1:t}\right)}\\ & =\frac{p\left(\neg m_i|z_t,x_t\right)p\left(z_t|x_t\right)}{p\left(\neg m_i\right)}\frac{p\left(\neg m_i|z_{1:t-1},x_{1:t-1}\right)}{p\left(z_t|z_{1:t-1},x_{1:t}\right)}\end{array}$

两者之比

$\begin{array}{rl}\frac{\mathrm{p}\left(m_i|x_{1:t},z_{1:t}\right)}{\mathrm{p}\left(\neg m_i|x_{1:t},z_{1:t}\right)}& =\frac{p\left(m_i|z_t,x_t\right)}{p\left(m_i\right)}\frac{p\left(m_i|z_{1:t-1},x_{1:t-1}\right)p\left(\neg m_i\right)}{p\left(\neg m_i|z_t,x_t\right)p\left(\neg m_i|z_{1:t-1},x_{1:t-1}\right)}\\ & =\frac{p\left(m_i|z_t,x_t\right)}{p\left(\neg m_i|z_t,x_t\right)}\frac{p\left(m_i|z_{1:t-1},x_{1:t-1}\right)}{p\left(\neg m_i|z_{1:t-1},x_{1:t-1}\right)}\frac{p\left(\neg m_i\right)}{p\left(m_i\right)}\end{array}$

对于二元随机变量：

$\frac{\mathrm{p}\left(m_i|x_{1:t},z_{1:t}\right)}{\mathrm{p}\left(\neg m_i|x_{1:t},z_{1:t}\right)}=\frac{p\left(m_i|z_t,x_t\right)}{1-p\left(m_i|z_t,x_t\right)}\frac{p\left(m_i|z_{1:t-1},x_{1:t-1}\right)}{1-p\left(m_i|z_{1:t-1},x_{1:t-1}\right)}\frac{1-p\left(m_i\right)}{p\left(m_i\right)}$

对于p(x)，定义对应的Log-Odd项：

$l(x)=\log \frac{p(x)}{1-p(x)}$

则：

$\mathrm{p}(x)=1-\frac{1}{1+\exp (l(x))}$

等式两边取$log$,则变成：

$l\left(m_i|x_{1:t},z_{1:t}\right)=l\left(m_i|x_t,z_t\right)+l\left(m_i|x_{1:t-1},z_{1:t-1}\right)-l\left(m_i\right)$

其中：

$l\left(m_i|x_t,z_t\right)$表示激光雷达的逆观测模型(inverse measurement Model)

$l\left(m_i|x_{1:t-1},z_{1:t-1}\right)$表示栅格𝑚𝑖在t-1时刻的状态

$l(m_i)$表示栅格$m_i$的先验值，该值对所有栅格都相同

算法流程

• 该算法对某一个栅格进行操作的时候，只有加法操作，因此具有非常高的更新速度。
• 更新的时候，需要知道传感器的逆测量模型。

示例代码
在hector-slam包中包含了覆盖栅格地图更新栅格概率的代码。文件为GridMapLogOdds.h。

//=================================================================================================
// Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt
// All rights reserved.

// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//     * Redistributions in binary form must reproduce the above copyright
//       notice, this list of conditions and the following disclaimer in the
//       documentation and/or other materials provided with the distribution.
//     * Neither the name of the Simulation, Systems Optimization and Robotics
//       group, TU Darmstadt nor the names of its contributors may be used to
//       endorse or promote products derived from this software without
//       specific prior written permission.

// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//=================================================================================================

#ifndef __GridMapLogOdds_h_
#define __GridMapLogOdds_h_

#include <cmath>

/**
 * Provides a log odds of occupancy probability representation for cells in a occupancy grid map.
 */
class LogOddsCell
{
public:

  /*
  void setOccupied()
  {
    logOddsVal += 0.7f;
  };

  void setFree()
  {
    logOddsVal -= 0.4f;
  };
  */


  /**
   * Sets the cell value to val.
   * @param val The log odds value.
   */
  void set(float val)
  {
    logOddsVal = val;
  }

  /**
   * Returns the value of the cell.
   * @return The log odds value.
   */
  float getValue() const
  {
    return logOddsVal;
  }

  /**
   * Returns wether the cell is occupied.
   * @return Cell is occupied
   */
  bool isOccupied() const
  {
    return logOddsVal > 0.0f;
  }

  bool isFree() const
  {
    return logOddsVal < 0.0f;
  }

  /**
   * Reset Cell to prior probability.
   */
  void resetGridCell()
  {
    logOddsVal = 0.0f;
    updateIndex = -1;
  }

  //protected:

public:

  float logOddsVal; ///< The log odds representation of occupancy probability.
  int updateIndex;


};

/**
 * Provides functions related to a log odds of occupancy probability respresentation for cells in a occupancy grid map.
 */
class GridMapLogOddsFunctions
{
public:

  /**
   * Constructor, sets parameters like free and occupied log odds ratios.
   */
  GridMapLogOddsFunctions()
  {
    this->setUpdateFreeFactor(0.4f);
    this->setUpdateOccupiedFactor(0.6f);
    /*
    //float probOccupied = 0.6f;
    float probOccupied = 0.9f;
    float oddsOccupied = probOccupied / (1.0f - probOccupied);
    logOddsOccupied = log(oddsOccupied);

    float probFree = 0.4f;
    float oddsFree = probFree / (1.0f - probFree);
    logOddsFree = log(oddsFree);
    */
  }

  /**
   * Update cell as occupied
   * @param cell The cell.
   */
  void updateSetOccupied(LogOddsCell& cell) const
  {
    if (cell.logOddsVal < 50.0f){
      cell.logOddsVal += logOddsOccupied;
    }
  }

  /**
   * Update cell as free
   * @param cell The cell.
   */
  void updateSetFree(LogOddsCell& cell) const
  {

    cell.logOddsVal += logOddsFree;

  }

  void updateUnsetFree(LogOddsCell& cell) const
  {
    cell.logOddsVal -= logOddsFree;
  }

  /**
   * Get the probability value represented by the grid cell.
   * @param cell The cell.
   * @return The probability
   */
  float getGridProbability(const LogOddsCell& cell) const
  {
    float odds = exp(cell.logOddsVal);
    return odds / (odds + 1.0f);

    /*
    float val = cell.logOddsVal;

    //prevent #IND when doing exp(large number).
    if (val > 50.0f) {
      return 1.0f;
    } else {
      float odds = exp(val);
      return odds / (odds + 1.0f);
    }
    */
    //return 0.5f;
  }

  //void getGridValueMap( const LogOddsCell& cell) const{};
  //void isOccupied(LogOddsCell& cell) {};

  //void resetGridCell() {};

  void setUpdateFreeFactor(float factor)
  {
    logOddsFree = probToLogOdds(factor);
  }

  void setUpdateOccupiedFactor(float factor)
  {
    logOddsOccupied = probToLogOdds(factor);
  }

protected:

  float probToLogOdds(float prob)
  {
    float odds = prob / (1.0f - prob);
    return log(odds);
  }

  float logOddsOccupied; /// < The log odds representation of probability used for updating cells as occupied
  float logOddsFree;     /// < The log odds representation of probability used for updating cells as free


};


#endif

参考网址：
占据栅格地图（Occupancy Grid Map）
占用栅格地图构建

计数建图算法

概念

• 对于每一个栅格统计两个量：𝑚𝑖𝑠𝑠𝑒𝑠(𝑖)和ℎ𝑖𝑡𝑠(𝑖)
• 𝑚𝑖𝑠𝑠𝑒𝑠(𝑖)表示栅格𝑖被激光束通过的次数，即被标为free的次数
• ℎ𝑖𝑡𝑠(𝑖)表示栅格𝑖被激光束击中的次数，即被标为occupied的次数
• 当ℎ𝑖𝑡𝑠(𝑖) / (𝑚𝑖𝑠𝑠𝑒𝑑(𝑖) + ℎ𝑖𝑡𝑠(𝑖))超过阈值则认为该栅格为Occupied，否则认为栅格是Free的。
• 𝐻𝑖𝑡𝑠(𝑖)/(𝑚𝑖𝑠𝑠𝑒𝑠(𝑖) + ℎ𝑖𝑡𝑠(𝑖))表示栅格𝑖的极大似然估计

观测模型

其中：

• $t$时刻的机器人位姿为$x_t$
• $t$时刻的激光雷达数据为$z_t$，第$n$个激光束为$z_t{,}_n$
• $c_t{,}_n$表示t时刻的第$n$个激光束是否为击中障碍物。$c_t{,}_n=1$表示最大值，即没有击中任何障碍物，$c_t{,}_n=0$表示正常值。
• $f(x_t,n,z_t,n)$表示$t$时刻第$n$个激光束击中的栅格的下标，$m_f(x_t,n,z_t,n)$表示对应的栅格的占用概率。

地图的极大似然估计为：

等价于：


则函数可简化为：

显然是关于$m_j$的函数，其极值可直接求其对于$m_j$的导数，令其等于0即可：

化解可得：

最终得到的就是上面提到的结论。

TSDF建图算法

基本思路

• 充分考虑传感器测量的不确定性，利用多次测量数据来实现更精确的表面重构，从而得到更精确、更细、更薄的地图。
  用多次激光测量数据更新同一个栅格的TSDF值。线性插值得到TSDF值为0的栅格就是被占用的栅格。理论上可以只用一个栅格的厚度表示环境曲面。
• 为障碍物表面建立Signed Distance Function。
  根据该方程更新每个栅格的TSDF值。
• 距离表面较远的点忽略不计，因为不会影响到表面重构。
  由于截止距离的设置，大于截止距离的点不会用于曲面重建。

数学描述
$sd{f}_i(x)=lase{r}_i(x)-dis{t}_i(x)$

$lase{r}_i(x)$表示激光测量距离

$dis{t}_i(x)$表示栅格离传感器原点的距离

$tsdf(x)$的定义

$tsdf(x)=max(-1,min(1,sd{f}_i(x)/t))$

$t$为设定的截止距离。

多次观测的融合更新方程：

通常$w_i$设置为1。每个栅格初始状态时，$TSD{F}_0(x)=0$,$W_0=0$。每次有观测数据（即激光击中或扫过）都按照上面的方程更新栅格的TSDF值。该方程实际上就是更新每个栅格的TSDF平均值。而环境曲面的重建就是寻找TSDF值正负交替的栅格。该栅格就是被占用的栅格。

上图展示了使用上述方程得到的TSDF场。在TSDF场中，符号进行变化的栅格就是曲面的所在。通过在两个栅格之间进行插值可以得到TSDF值为0的栅格坐标，而该坐标就是曲面的精确位置。由于可以得到精确的栅格坐标，其构建的地图通常只有一个栅格的厚度。

论文：Truncated Signed Distance Function-Experiments on Voxel Size

总结

针对Occupancy Grid和Count Model

优势：这两种算法计算量比较小，实现也较为简单。Occupancy Grid只需对Grid Log Likelihood进行加法更新；Count Model只需对Grid Hit/Miss计数进行加法更新。之后便可通过公式获取每个栅格的占用概率，进而构建整张地图。
劣势：这两个算法对测量噪声较为敏感，得到的障碍物可能占用多个栅格，无法获取环境曲面的精确位置。

针对TSDF

优势：TSDF方法通过加权线性最小二乘，使用多帧观测值进行平均计算来融合，能够有效减小测量噪声的影响。同时可以通过线性插值得到TSDF的零点位置来获取环境曲面的精确位置。最终得到障碍物仅会占用一个栅格。
劣势：计算复杂，实时性较差。需要更多的工程优化（如并行计算）来保证建图效率。

关注公众号《首飞》回复“机器人”获取精心推荐的C/C++，Python，Docker，Qt，ROS1/2，机器人学等机器人行业常用技术资料。