在自动驾驶中和移动机器人的应用中，感知是至关重要的一环，周边环境障碍物的确定对于之后的路径规划和移动有着中要的影响。在实践中，我们往往用激光雷达（lidar）来满足感知的要求，主要原理是通过雷达接收到反射的光线进行距离的测量（也称为测距仪）。

举个例子，在下图所示矩形地图中，lidar检测了边界和三角形障碍物，并的到了距离障碍物的距离

， 在这个情况下，机器人通过地图数据得知真实的边界距离在

,所以这时候，机器人会得出三角形是障碍物的概率，来判断到底是不是障碍物。

从上面的例子我们可以知道，感知的目的就是根据测量值得置信度来确定环境。所以测量模型由一个条件概率p(zt|xt,m)表示， 其中xt是机器人的位姿，zt是在时刻t的测量值，而m则是环境的地图。

在slam构图过程中，就是根据感知的概率来判断是否有障碍物，然后绘制黑色的标识边界信息。

一般来说，传感器模型越精确结果越好。 实际上，因为物理世界过于复杂，我们基本不可能完全准确地对传感器建模。

一般，传感器的响应特性依赖于我们不希望在一个概率机器人算法中显式表述的变量，比如说，在机器人建图的过程中，我们就不会考虑墙面的材料。 从随机过程的角度看来，概率机器人学对传感器的不精确性具有一定的容忍度。因为它用条件概率

对测量过程建模，而不是确定性方程

， 所以模型具有一定的模糊特性可以兼容传感器测量的不确定性。这就是概率学方法相比于传统机器人技术的优势，在实际应用中我们可以使用极为粗糙的模型。 但是，在设计概率学模型的时候，必须考虑不同类型的不确定性可能对传感器测量值带来什么样的影响。

当我们请求传感器数据的时候，很多传感器不止提供一个测量数值。比如说，摄像头生成的就是一个亮度、饱和度、颜色的阵列，类似的，距离扫描器通常生成的是整个区域的扫描结果。 我们用符号K来表示测量数据的纬度，对于一次测量zt，可以写作：

其中zkt对应着一个测量值(比如,一个距离)。我们用各个测量值的概率之积来近似p(zt|xt,m)

我们假设上式是独立性的，认为每个测量束的噪声都是独立的， 这个假设只在理想情况下才成立。通过独立性假设方便计算对结果影响也不大。

距离扫描的波束模型(beam models of range finders)

测距仪（lidar）是使用最多的传感器之一，因此本章第一个要介绍的就是测距扫描仪的近似物理模型。测距扫描仪测量通过一个波束beam或者测量锥cone来测量附近物体的距离。 激光雷达是一个比较精确的距离传感器，常用波束模型建模。超声传感器通常使用测量锥模型。

如下图所示，扫描的范围是个锥形。

这类模型有4中误差我们在实际应用中需要进行建模，以用来计算最终的置信度概率。

1.传感器吱声的比较小的测量噪声

一般传感器的测量值都是比较准确的，传感器自身的硬件干扰我们一般用一个正太高斯分布来建模。

其中

是根据当前状态

和地图信息m通过波束投射求得的。

表示均值为

标准差为σhit的正态分布：

归一化算子η通过下式计算:

（

这里介绍下为什么要归一化：很简单就是保证最后的概率在0-1的范围内）

2.非预期的物体所导致的的误差

在机器人运行过程中，工作环境是动态的，而地图m却是静态的。因此，没有在地图中描述的物体将使得距离扫描仪产生意外的超短距离， 仅从地图数据上看它是一个不合理的值。比如说，人类就是一种典型的频繁出现在机器人的工作空间中的物体。处理这样物体的一种方法就是，将它们看作是状态向量的一部分并估计它们的位置。 还有一种更简单的方法，直接将它们看作是传感器的噪声。未建模的物体将总是使得传感器的测量值小于或者等于根据地图的估计值

。就如上述图中三角形障碍物一样。

随着测量距离的增加，传感器扫描到非预期物体的可能性逐渐降低。假设有两个人以固定的概率出现在传感器的感知范围内，其中一个人到传感器的距离为z1， 而另一个人的距离为z2。不失一般性的，令z1<z2。传感器更可能返回z1而不是z2。因为只要第一个人存在，传感器的测量值就是z1。 要使测量值为z2，除了要保证第二个人存在外，还需要第一个人不出现在工作空间中。如下图所示：

数学上，一般使用指数分布exponential distribution来描述这种情况出现的概率。该分布也有一个参数记为λshort，这也是测量模型的固有参数。根据指数分布的定义， 有如下关于

的表达式：

， 同样获取归一化算子。

从最终的分布图上看，

Z1 或 Z2离真实的边界距离远近，是非预期障碍物的概率就越低，反之越大。

3.没有检测到物体的噪声

很多时候，传感器会漏检障碍物。比如发射的光线被黑色的介质吸收了，这样lidar就不能根据返回值就行距离测量，这样就以

（传感器所能返回的最大测距数值）来表示。这一现象很常见，所以在设计传感器的测量模型时需要尤其注意。

这种情况下，我们很简单就用一个二值分布来表示就可以了

4.随机噪声

距离扫描器偶尔会产生完全不可解释的测量值unexplainable measurements。 比如说，经过多次反射的超声，或者不同传感器之间的串扰，都会使得声纳设备产生鬼一样的读数。为了简化模型，通常使用一个遍及整个传感器测量区间[0,zmax]的均匀分布来对其建模:

以上我们详细建模了4中噪声，那么现在我们就要来融合他们以获取最终的检测概率。

我们可以通过加权平均的方式融合这四种不同的分布，令zhit,zshort,zmax,zrand分别为四种参数，并且zhit+zshort+zmax+zrand=1。

算法实现伪代码：

实现很简单这里就不详细说明了，就是所有的扫描点密度函数积。

调参

我们注意到，在计算以上概率时候我们一共有6个参数，这些参数包括混合权重zhit, zshort, zmax, zrand，以及σhit和λshort。如何调试这些个参数对于模型最终的准确度很重要。

上图是激光雷达扫描到的数据，目前是在300的位置，但是我们能够看到有很多的噪声在别的地方。还有很多在500附近，这些点大概率是无法加测的误差。我们通过调节这6个参数，来使扫描的误差集中在300附近，已达到一定的准确度。

手动设定这些内在参数Θ往往能够得到一个完美的结果。我们所要做的的工作仅仅是盯着密度结果调整参数，直到满意为止。另外一种更原则化的方法就是，从实际的数据中学习这些参数。 这是通过最大化数据集合Z=zi在相关位置X=xi和地图m下的似然度来实现的，其中zi是一个实际的测量值，xi是测量时所在的位置，m则是地图。 数据Z的似然度表示如下：

我们的目标是找到能够最大化似然度的内参Θ。这种最大化数据似然度的算法称为极大似然估计maximum likelihood estimator，简记为ML估计。

算法如下：

经过多次的迭代收敛，最终可以得到不错的参数值。

看到收敛后，误差基本都是很窄的高斯分布，集中在

。

总结：

从以上的介绍中，不难感受到，测距传感器模型的计算量不小，激光传感器在每次扫描都会返回几百个数据，每秒钟还要扫描很多次。 而且对每个扫描束都必须处理一个ray-tracing操作，考虑每一个可能的位置，对整个扫描过程进行积分从而得到目前的置信度，所以这个过程通常不能实时完成。 解决这一问题的经典方法就是只用所有测量值中的一小部分数据(比如说，使用激光传感器中8个方向上的数据来代替360°的数据)。这种方法有一个很好的特点， 剔除一部分测量值整个状态估计过程对于相邻测量的噪声就不那么敏感了，因为相邻的扫描束通常不是独立的。

参考：

PROBABILISTIC ROBOTICS