代码解析之range_sensor_layer中costmap更新的方法和模型（二）

         隔了一段时间回头再看costmap的更新，结合自己使用gridmap的经历，发现costmap中那么多代码无非是做2件事情，一是限定传感器探测扫描过的区域大小和边界（即后续要更新的区域），二是根据传感器模型和贝叶斯方法计算上述区域中所有栅格的更新后的被占据概率。

         costmap中使用的是扇形传感器模型，认为传感器的探测区域是一个以sensor为中心的一定视场角的扇形，然后评估扇形中每个栅格到sensor的距离和与sensor中轴线的夹角，以此计算最新一次观测该栅格的概率。我在实际应用中，遇到了以下几个问题：

• 障碍物面向sensor的表面与sensor的距离是r，障碍物理应位于比r更远的位置，因此，扇形的半径R应略大于测距返回值r，否则，位于r附近的栅格（被占据概率较大）会从扇形中排除而不被更新到，导致错误。
• 如下代码所示，计算概率时依据栅格到sensor的距离，分为4种情况。0---r-2dr，这个范围内的概率很小，并且所有栅格概率相同；r-2dr---r-dr，这个范围内的概率较小，方程2的最大值是0.5；r-dr---r+dr，这个范围内的概率较大，方程3的最小值是0.5；其他的给赋予值0.5。因为传感器测距值是有抖动的，导致距离分界值也会抖动，对于接近r的某一个栅格A，有可能会使用方程3计算，则B可能使用方程2计算，下一次观测时，也可能A、B都使用方程3或2计算，导致A、B的概率值也在忽上忽下的抖动，始终不会平稳的更新。这个问题是传感器数据引起的，我想到的避免方法就是对距离平滑滤波，但是会降低更新速度，影响使用效果。
• 使用gridmap时，可以在sensor坐标系下求取扇形所在多边形的5个顶点（sensor, plm, plt, prt, prm, 注意要按顺序组成闭合空间），然后转换到全局坐标系，依据对角度、距离的限制，划定扇形的范围，使用多边形迭代器处理其中的栅格。

double Costmap2D::SensorModel(double r, double phi, double theta)
{
    double lbda = Delta(phi)*Gamma(theta);
    double delta = (resolution_ > 1 ? resolution_/100.0 : resolution_);
    double res = -1.0;

    if(phi >= 0.0 and phi < r - 2 * delta * r)
        res = (1- lbda) * (0.5);        //方程1
    else if(phi < r - delta * r)
        res = lbda* 0.5 * pow((phi - (r - 2*delta*r))/(delta*r), 2)+(1-lbda)*.5;        //方程2
    else if(phi < r + delta * r){
        double J = (r-phi)/(delta*r);
        res = lbda * ((1-(0.5)*pow(J,2)) -0.5) + 0.5;        //方程3
    }
    else
        res = 0.5;        //方程4
    return res;
}