无人驾驶汽车系统入门（一）——卡尔曼滤波与目标追踪

转载：https://blog.csdn.net/AdamShan/article/details/78248421

前言：随着深度学习近几年来的突破性进展，无人驾驶汽车也在这些年开始不断向商用化推进。很显然，无人驾驶汽车已经不是遥不可及的“未来技术”了，未来10年必将成为一个巨大的市场。本系列博客将围绕当前使用的最先进的无人驾驶汽车相关技术，一步一步地带领大家学习并且掌握无人驾驶系统的每一个模块的理论基础和实现细节。由于无人驾驶汽车系统构成及其复杂，本系列博客仅讨论软件部分的内容，关于汽车，传感器和底层的硬件，不是我们关注的重点。至于连载的顺序和技术路线，本人其实是同时开始的，先完成哪一节就先发那一节。

我一直反感国内的一些中文水书，这些书的产生的根源就是国内浮躁的技术氛围，我希望能够系统详细的以中文总结无人驾驶汽车的整个技术基础，本系列博客将详细展开无人汽车的基础理论和和这些理论的实现的小例子，编写不易，转载请附来源：http://blog.csdn.net/AdamShan/article/details/78248421

首先我将带大家了解无人驾驶汽车系统感知模块的重要技术——卡尔曼滤波，卡尔曼滤波部分我打算分三节（三次博客的内容）：

• 卡尔曼滤波与行人状态估计
• 扩展卡尔曼滤波（EKF）与传感器融合
• 处理模型，无损卡尔曼滤波（UKF）与车辆状态轨迹

本节为卡尔曼滤波，主要讲解卡尔曼滤波的具体推导，卡尔曼滤波在行人状态估计中的一个小例子。

为什么要学卡尔曼滤波？

卡尔曼滤波以及其扩展算法能够应用于目标状态估计，如果这个目标是行人，那么就是行人状态估计（或者说行人追踪），如果这个目标是自身，那么就是车辆自身的追踪（结合一些地图的先验，GPS等数据的话就是自身的定位）。在很多的无人驾驶汽车项目中，都能找到卡尔曼滤波的扩展算法的身影（比如说EKF，UKF等等）。本节我们从最简单的卡尔曼滤波出发，完整的理解一遍卡尔曼滤波的推导过程，并实现一个简单的状态估计Python程序。

卡尔曼滤波是什么？

我们通常要对一些事物的状态去做估计，为什么要做估计呢？因为我们通常无法精确的知道物体当前的状态。为了估计一个事物的状态，我们往往会去测量它，但是我们不能完全相信我们的测量，因为我们的测量是不精准的，它往往会存在一定的噪声，这个时候我们就要去估计我们的状态。卡尔曼滤波就是一种结合预测（先验分布）和测量更新（似然）的状态估计算法。

一些概率论的知识基础

下面是一些概率论的基础知识，如果之前有这方面的知识储备那当然是最好的，很有利于我们理解整个博客内容，如果没有这方面的基础而且也看不懂下面的内容也没关系，我会以一个相对直观的方式来展现整个理论部分。

• 先验概率 P(X)P(X)：已知结果区推测固有性质的可能性

贝叶斯公式：

P(A|B)=P(A)×P(B|A)P(B)P(A|B)=P(A)×P(B|A)P(B)

后验分布正比于先验分布乘以似然。

卡尔曼滤波完整推导

一个简单的例子

若干年后，我们的可回收火箭要降落到地球，我们比较关心的状态就是我们的飞行器的高度了，飞行器的高度就是我们想要估计的状态，我们会通过一些传感器去测量我们当前的高度信息，比如说使用气压计。假如我们每一次测量，当前高度都变成上一次测量的 95%95%
我们可以使用递归来表示这样的公式——为了计算我们当前的高度，我们必须知道我们上一个测量的高度，以此递推，我们就会推到我们的飞行器的初始高度。

但是，由于我们的测量往往来自于一些传感器（比如说GPS，气压计），所以测量的结果总是带有噪声的，这个噪声是有传感器本身引起的，那么我们的表达式就变成了：

Measurement(t)=Height(t)+Noise(t)Measurement(t)=Height(t)+Noise(t)

。

思考一下，在这里我们用一个简单的比例（常数）来描述“可回收火箭”的运动规律，会存在什么问题？

很显然，现实中的运动不会像我们这个简单的处理模型一样高度按比例缩小。在这里为了简化，我们先假设我们的模型挺好的，能够大致描述出这颗“神奇火箭”的运动规律，只是偶尔会存在一定的偏差（比如所空气湍流的影响），那么我们在计算xkxk 为了简化，后面的分析我们会先忽略处理噪声，但是我们在传感器融合的部分会将处理噪声重新考虑进来。

状态估计

因为我们是要估计飞行器的状态（高度），所以我们把测量的公式变换成：

xk=zk−vkxk=zk−vk

，则表示对两者的权重分配。

计算卡尔曼增益

那么如何计算卡尔曼增益呢？我们使用一种间接的方法，我们虽然不知道测量噪声 vkvk 呢？

那么假设前一次的预测误差 pk−1=0pk−1=0 是个很小的数值，所以增益为1，所以完全接受这一次的测量作为我们的估计（因为上一次的的预测误差太大了，为1，所以一旦拿到了新的测量，如获至宝，就干脆把不准确的上次的估计舍弃掉了）

对于下面的公式的分析是一样的，我们考虑极端的例子，当增益为 00。

预测和更新

那么现在我们有关于当前的火箭高度状态的两个公式了，它们分别是：

xk=axk−1xk=axk−1

。 结合这写我们就可以得到卡尔曼滤波预测和更新过程了：

预测

x^k=ax^k−1+bukx^k=ax^k−1+buk

使用线性代数的方法来表示预测和更新

预测

x^k=Ax^k−1+Bukx^k=Ax^k−1+Buk

至此，卡尔曼滤波的完整推导就结束了。下面，我们来看看卡尔曼滤波在无人汽车的感知模块的应用。

卡尔曼滤波算法为什么会叫滤波算法？
以一维卡尔曼滤波为例，如果我们单纯的相信测量的信号，那么这个信号是包含噪声的，是很毛糙的，但是当我们运行卡尔曼滤波算法去做估计，我们估计的信号会很光滑，看起来似乎滤掉了噪声的影响，所以称之为滤波算法。实际上，卡尔曼滤波不仅仅过滤掉了测量信号的噪声，它同时也结合了以往的估计，卡尔曼滤波在线性问题中被证明是最优估计。

卡尔曼滤波在无人驾驶汽车感知模块的应用

无人车感知模块的传感器

无人驾驶汽车要安全的在道路上行驶，需要“耳听六路，眼观八方”。那么无人车的耳朵和眼睛是什么呢？那就是安装在无人车上的各种各样的传感器了。无人车上的传感器能够多达几十个，而且是不同种类的，比如：

• 立体摄像机
• 交通标志摄像机
• 雷达（RADAR）
• 激光雷达（LIDAR）

立体摄像机往往用于获取图像和距离信息；交通标志摄像机用于交通标志的识别；雷达一般安装在车辆的前保险杆里面，用于测量相对于车辆坐标系下的物体，可以用来定位，测距，测速等等，容易受强反射物体的干扰，通常不用于静止物体的检测；激光雷达往往安装在车顶，使用红外激光束来获得物体的距离和位置，但是空间分辨率高，但是笨重，容易被天气影响。

由此可知，各种传感器都有其优点和缺点，在实际的无人驾驶汽车里，我们往往结合多种传感器的数据去感知我们的车辆周边的环境。这种结合各种传感器的测量数据的过程我们称之为传感器融合（Sensor Fusion）。本系列博客后面的章节我将详细给大家介绍基于扩展卡尔曼滤波和无损卡尔曼滤波在传感器融合中的应用。在本节中，我们主要考虑卡尔曼滤波基于单一的传感器数据来估算行人位置的方法。

基于卡尔曼滤波的行人位置估算

卡尔曼滤波虽然简单，确实无人驾驶汽车的技术树中非常重要的一部分，当然，在真实的无人驾驶汽车项目中使用到的技术是相对更加复杂的，但是其基本原理仍然是本博客介绍的这些内容。在无人驾驶中，卡尔曼滤波主要可以用于一些状态的估计，主要应用于行人，自行车以及其他汽车的状态估计。下面，我们以行人的状态估计为例展开。

当我们要估计一个行人的状态时，首先就是要建立起腰估计的状态的表示。这里，行人的状态大致可以表示为x=(p,v)x=(p,v) 则是行人此时的速度。我们用一个向量来表示一个状态：

x=(px,py,vx,vy)Tx=(px,py,vx,vy)T

在确定了我们要估计的状态以后，我们需要确定一个处理模型，如前文所说，处理模型用于预测阶段来产生一个对当前估计的先验。在本文中，我们先以一个最简单的处理模型来描述行人的运动——恒定速度模型：

xk+1=Axk+νxk+1=Axk+ν

即：

之所以称之为恒定速度模型，是因为我们将上面这个行列式展开可以得到：

pk+1