概率机器人总结——占用栅格地图先实践再推导

概率机器人总结——占用栅格地图构建先实践再推导

当我将概率机器人看到这里的时候，越发觉将数学理论转到实际应用是一件非常有意思的事情，像我的话很早之前就用过gmapping和amcl这些ros自带的功能包了，但是知其然不知其所以然，看起来很炫酷的操作却不明白其背后的原理是什么，通过这一段时间的学习总结，对此有了一个更加深入的认识，很爽的。

实践过程

伪代码分析

这里我先放上来《概率机器人》中的伪代码

其中$m_i$就是我们见到的栅格，$l_{t,i}$这个东西叫对数占用概率，由后验概率表示如下：$$l_{t,i}=\log \frac{p\left(m_i|z_{1,t},x_{1,t}\right)}{1-p\left(m_i|z_{1,t},x_{1,t}\right)}$$当然反过来，当我们更新对数占用概率后可以用其恢复后验概率，如下： p ( m i ∣ z 1 , t , x 1 , t ) = 1 − 1 1 + exp ⁡ { l t , i } p\left(m_{i} | z_{1, t}, x_{1, t}\right)=1-\frac{1}{1+\exp \left\{l_{t, i}\right\}} p(mi​∣z1,t​,x1,t​)=1−1+exp{lt,i​}1​这个让我联想到在视觉SLAM里面用到的李群和李代数的关系，我们回到伪代码，第3-7行表示只对机器人感知范围内的栅格进行更新对数占用概率的更新，不在范围内的不进行跟新，在更新的过程中有一个inverse_sensor_model函数，这个是什么呢？看下面伪代码：

看起来会稍微复杂一些，但其实很简单，结合下面这张图解释：

首先我们通过观测获得的数据的坐标系肯定是基于我们传感器的，第2-5行可以理解为将其转到世界坐标系下，给定一个传感器观测的范围，也就是上图中的白色区域，第6-12行表示的是，如果我们给定的一个栅格$m_i$位于白色区域就返回一个固定值$l_{free}$，如果位于黑色区域就返回一个固定值$l_{occu}$，如果位于灰色区域就返回一个值$l_0$，再下面实际的程序里氛围设置为了0.4，0.6和0.5。

真代码分析

PythonRobotics里面关于mapping的Demo太过简单，因此我在知乎上搜到了一个北航小哥写的文章和代码，推导清晰，代码也很简单，一看就懂，先附上链接Grid Mapping 占用栅格地图构建实现，然后粘几段代码过来看一下：

void GridMapper::updateMap ( const sensor_msgs::LaserScanConstPtr& scan,  Pose2d& robot_pose )
{
    /* 获取激光的信息 */
    const double& ang_min = scan->angle_min;
    const double& ang_max = scan->angle_max;
    const double& ang_inc = scan->angle_increment;
    const double& range_max = scan->range_max;
    const double& range_min = scan->range_min;
    
    /* 设置遍历的步长，沿着一条激光线遍历 */
    const double& cell_size = map_->getCellSize();
    const double inc_step = 1.0 * cell_size;

    /* for every laser beam */
    for(size_t i = 0; i < scan->ranges.size(); i ++)
    {
        /* 获取当前beam的距离 */
        double R = scan->ranges.at(i); 
        if(R > range_max || R < range_min)
            continue;
        
        /* 沿着激光射线以inc_step步进，更新地图*/
        double angle = ang_inc * i + ang_min;
        double cangle = cos(angle);
        double sangle = sin(angle);
        Eigen::Vector2d last_grid(Eigen::Infinity, Eigen::Infinity); //上一步更新的grid位置，防止重复更新
        for(double r = 0; r < R + cell_size; r += inc_step)
        {
            Eigen::Vector2d p_l(
                r * cangle,
                r * sangle
            ); //在激光雷达坐标系下的坐标
            
            /* 转换到世界坐标系下 */
            Pose2d laser_pose = robot_pose * T_r_l_;
            Eigen::Vector2d p_w = laser_pose * p_l;

            /* 更新这个grid */
            if(p_w == last_grid) //避免重复更新
                continue;
            
            updateGrid(p_w, laserInvModel(r, R, cell_size));
            	    
            last_grid = p_w;
        }//for each step
    }// for each beam
}

这个是最主要的代码段，这个函数就是从激光的数据结构中遍历每一条激光线，然后以栅格长度的步长去遍历这条激光线上的栅格，然后将各个栅格的坐标从雷达坐标系下转到世界坐标系下，然后逐个更新栅格被占用的后验概率，跟新概率这两个函数如下：

void GridMapper::updateGrid ( const Eigen::Vector2d& grid, const double& pmzx )
{
    /* TODO 这个过程写的太低效了 */
    double log_bel;
    if(  ! map_->getGridLogBel( grid(0), grid(1), log_bel )  ) //获取log的bel
        return;
    log_bel += log( pmzx / (1.0 - pmzx) ); //更新
    map_->setGridLogBel( grid(0), grid(1), log_bel  ); //设置回地图
}

double GridMapper::laserInvModel ( const double& r, const double& R, const double& cell_size )
{
    if(r < ( R - 0.5*cell_size) )
        return P_free_;
    
    if(r > ( R + 0.5*cell_size) )
        return P_prior_;
    
    return P_occ_;
}

这个laserInvModel函数相当于上面伪代码里面的inverse_sensor_model，但是要简单很多，就是判定该栅格距离雷达中心所在位置，而updateGrid函数运行的就是第一段伪代码所示的步骤，对应着看就好了，代码在我电脑上运行效果如下（看上去和gmapping差不多了，gmapping也是不带回环检测的）：

推导过程

上面的效果看到了，接下来需要通过推导来看看为什么这么做是好使的，占用栅格地图的构建是基于静态二值贝叶斯滤波实现的。

静态二值贝叶斯滤波

这里基本的贝叶斯滤波方程入手：$$\begin{array}{rl}p\left(x|z_{1,t}\right)& =\frac{p\left(z_t|x,z_{1,t-1}\right)p\left(x|z_{1,t-1}\right)}{p\left(z_t|z_{1,t-1}\right)}\\ & =\frac{p\left(z_t|x\right)p\left(x|z_{1,t-1}\right)}{p\left(z_t|z_{1,t-1}\right)}\end{array}$$这里$p\left(z_t|x\right)$可以称作观测方程，也可以成为似然函数， 而$p\left(x|z_{1,t-1}\right)$可以成为运动方程，也可以成为先验值，因为是静态的所以我们省去了$u_t$，$p\left(z_t|z_{1,t-1}\right)$我们一般视为一个常数。现在我们对测量方程再运用一次贝叶斯法则有：$$p\left(x|z_{1,t}\right)=\frac{p\left(x|z_t\right)p\left(z_t\right)p\left(x|z_{1,t-1}\right)}{p(x)p\left(z_t|z_{1,t-1}\right)}$$这里出现了一个关键的分布$p\left(x|z_t\right)$，称为反向观测模型，注意和我们的后验$p\left(x|z_{1,t}\right)$是不同的，那么前向观测模型当然指的是$p\left(z_t|x\right)$，那么反向观测模型和前向观测模型有什么区别呢？为什么要这样做？反向观测模型可以理解为设计一个函数根据相机图像来计算门为关着的概率，而前向观测模型指的是描述所有相机图像中显示门为关着的分布更容易些。前者当然要简单些，那为什么普通的贝叶斯滤波不这么做呢? 因为$p\left(z_t\right)$我们不知道呀～
然后同理求其对立事件$\neg x$的分布$$p\left(\neg x|z_{1,t}\right)=\frac{p\left(\neg x|z_t\right)p\left(z_t\right)p\left(\neg x|z_{1,t-1}\right)}{p(\neg x)p\left(z_t|z_{1,t-1}\right)}$$相除得$$\begin{array}{rl}\frac{p\left(x|z_{1,t}\right)}{p\left(\neg x|z_{1;t}\right)}& =\frac{p\left(x|z_t\right)}{p\left(\neg x|z_t\right)}\frac{p\left(x|z_{1,t-1}\right)}{p\left(\neg x|z_{1;t-1}\right)}\frac{p(\neg x)}{p(x)}\\ & =\frac{p\left(x|z_t\right)}{1-p\left(x|z_t\right)}\frac{p\left(x|z_{1,t-1}\right)}{1-p\left(x|z_{1,t-1}\right)}\frac{1-p(x)}{p(x)}\end{array}$$因为是二值所以上式才成立，然后我们取对数得$$\begin{array}{rl}{l}_t(x)& =\log \frac{p\left(x|z_t\right)}{1-p\left(x|z_t\right)}+\log \frac{p\left(x|z_{1;t-1}\right)}{1-p\left(x|z_{1,t-1}\right)}+\log \frac{1-p(x)}{p(x)}\\ & =\log \frac{p\left(x|z_t\right)}{1-p\left(x|z_t\right)}-\log \frac{p(x)}{1-p(x)}+l_{t-1}(x)\end{array}$$其中$l_t(x)$就是我们定义的概率对数表达式，是我们递归二值后验的一个中间量：$$l(x):=\log \frac{p(x)}{1-p(x)}$$然后我们的结论是，二值静态贝叶斯滤波按照如下方式递归：$$l_t=l_{t-1}+\log \frac{p\left(x|z_t\right)}{1-p\left(x|z_t\right)}-\log \frac{p(x)}{1-p(x)}$$
接下来我们将静态二值贝叶斯滤波应用到占用栅格地图的构建中来：所谓地图构建问题就是根据位置和观测建立地图：$$p\left(m|z_{1:t},x_{1:t}\right)$$标准的占用栅格方法是按照栅格将其分为一些独立问题：$$p\left(m|z_{1,t},x_{1,t}\right)=\prod _{i}p\left(m_i|z_{1,t},x_{1,t}\right)$$然后我们在栅格上建立对数占用概率表达方式：$$l_{t,i}=\log \frac{p\left(m_i|z_{1,t},x_{1,t}\right)}{1-p\left(m_i|z_{1,t},x_{1,t}\right)}$$然后就利用上述的静态二值贝叶斯滤波进行递归就好了，带有反向观测模型的部分就变成了：$$inverse-sensor-model\left(m_i,x_t,z_t\right)=\log \frac{p\left(m_i|z_t,x_t\right)}{1-p\left(m_i|z_t,x_t\right)}$$其实就是根据当前的位置和所观测到的情况给栅格赋值，对应了上述的操作，最后我们每迭代一次恢复成将对数占用概率恢复成后验就知道当前栅格是否被占用的概率。

总结完毕，欢迎交流～

此外，对SLAM算法感兴趣的同学可以看考我的博客SLAM算法总结——经典SLAM算法框架总结