基于卡尔曼滤波的自动驾驶多传感器融合定位技术详解

自动驾驶闭环最常用的算法—卡尔曼滤波

俗话说“基础不牢，地动山摇”。

那作为自动驾驶工程师，各式各样的常用算法就是构建起整个系统的基础，而卡尔曼滤波算法作为贯穿感知、定位、决策和控制整个自动驾驶系统闭环中最常用的算法之一，自然是学习的重中之重。

目标追踪需要用到卡尔曼滤波，多传感器融合需要用到卡尔曼滤波，定位建图需要用到卡尔曼滤波，轨迹跟踪与控制还是会用到卡尔曼滤波。

这一伟大的算法自上世纪六十年代诞生以来已经应用于无数的工程学术项目中，无论你是学控制的、电子的、通信的、计算机的还是车辆、机械的，一定曾经或多或少听说过、学习过这一算法。但是相信你也会和我有一样的感觉，就是老师讲的总是感觉差了那么点意思，总感觉没有深刻理解其精髓。

两年前因为课程项目需要研究过卡尔曼滤波及其变体，当时看了很多经典文献以及知乎上通俗易懂讲解卡尔曼滤波的文章，收获颇丰。

但是时间久了，就容易遗忘。恰好最近项目中又与卡尔曼滤波相逢，索性再次寻找资料学习一番。“书常读常新”，每一次的学习总能带来不一样的体会，这次的卡尔曼滤波学习是跟随B站上控制理论的大佬DR_CAN的视频结合自己的理解整理出来的。

大佬对于控制理论的把握和理解真的是深入精髓，除了卡尔曼滤波还出了很多其他控制理论相关的视频，对于抽象难懂的控制理论学习真的是受益良多，推荐大家学习。

下面开始正式的卡尔曼滤波学习~

卡尔曼滤波器是一种最优化递归数据处理算法。开篇标明其实际含义。卡尔曼是发明者的名字，滤波是一种方法，二者结合对于初学者而言并不知道这是一种什么算法，甚至“滤波”二字还会产生干扰和误解，让人误以为这是一种和低通滤波器类似的滤除某一类信号的东西。而在之前的学习中，“滤波”的含义已经被解释得非常清楚，即滤除的并非某一类信号，而是不确定性。

当我们去描述一个系统的时候，不确定性主要体现在三个方面：

1、不存在完美的数学模型。世界是复杂多变的，人们可以找到数学模型去描述一个系统，但是总是需要各种各样的理想条件，因此没有一个系统能用数学模型完美描述，只是近似程度高与低的差别；

2、系统的扰动不可控也很难建模。例如车辆行驶在路上突遇暴雨，这种扰动如何建模、如何表示？很难；

3、测量传感器存在误差。这也很好理解，传感器就像量物体长度的尺子，用不同的尺子测总会存在不一样的结果。

接下来就通过上面举的尺子的例子来看一下递归算法的实现。我认为这一例子举得是非常巧妙的，通过简单的数学推导让人领悟到卡尔曼滤波思想的精髓。

我们假设用一把尺子量硬币的长度，记第次的测量结果为，现在需要你去估计一下这个硬币的真实长度（估计值我们记为）。很自然的你会想到把这次的结果求平均，它还有一个高大上的名字叫做均值滤波，因为从另一个角度看求平均值代表着你为每一次测量提供了相同的权重。

很简单的求取平均值的表达式，接下来我们对它做一些变形

这个变形用到一点小技巧，和高中学的数学归纳法类似。最终得到一个递归的等式，分析这个等式，可以发现，当增加，趋近于0，就趋近于。其背后的逻辑也很好理解，随着测量次数的增加，测量的结果也就不再重要，预测已经能够很好地估计出真实值；同样的，当很小的时候，例如时， ，因为此时测量次数很少，预测还很不确定，测量值起到的作用比较大。

到这里其实就和卡尔曼滤波的核心思想很接近了。卡尔曼滤波分两步，预测和更新，最终的结果实际上就是预测值和观测值之间的加权和，而每次决定两者权重的就是卡尔曼的核心——卡尔曼增益kalman Gain。

因此我们把上式改写一下成为

它的实际意义就是当前的估计值等于上一次的估计值+系数x（当前的测量值-上一次的估计值）。

所以卡尔曼滤波只与上一时刻的信息有关，而不需要在全过程记录以往的数据，这就使得其在内存及计算能力有限的嵌入式设备中广为应用。

基于卡尔曼滤波的自动驾驶多传感器融合定位技术

接下来，我们聊聊基于卡尔曼滤波的自动驾驶多传感器融合定位技术，谈到定位，实际上每个人都很熟悉。无论是你手机里的GPS还是交流沟通时“我在xxx”，都是一个定位的概念的表达。

而对于无人驾驶车辆而言，它也需要表达自己的位置。那这时候它不能仅仅简单地表达“我在某某路上”，这太粗糙了，我们需要知道每个时刻它处于一个什么点位上，这时候就需要引入坐标系了，因为在坐标系下你就可以用XYZ来表示你的具体位置了，并且该位置是唯一的。

欢迎关注「智驾最前沿」微信视频号

关于坐标系既可以使用全局坐标系，例如GPS获得的全球定位坐标；也可以使用一个局部坐标系，例如你今天就只运行在某段封闭道路或园区内，就可以以起点作为坐标系原点。

除了这些外在的不会随意变化的坐标系，无人驾驶汽车本身还有一个自车坐标系，也就是常说的位姿。位姿就是位置+姿态，位置即X、Y、Z三维，姿态即横摆yaw、俯仰pitch、侧倾roll。对于车辆而言，一般都行驶在路面上，产生Z这个维度的数值比较少见。同样地，俯仰和侧倾也相对较少。如果发生大范围侧倾，那可能自动驾驶功能是没本事纠正回来了，这时候可能需要叫个拖车。

定位功能为后续规划、控制模块提供了重要的输入信息，也因此对定位模块提出了很高的需求。具体可以看看下表。

首先是精度要求，我们常说定位要精准，那怎么精准法，就是要求误差不能超过10cm。想想，如果你的定位和实际位置差得太大，那在遇到复杂场景时想要变道避让就会和别的目标发生碰撞。鲁棒性说的是定位系统的容错能力，保证最差不会差过30cm。全天候场景也好理解，谁也不希望车在雨雪大雾天或者隧道里就不能使了。

那么，要实现这么高的要求需要怎样的技术呢？

常用的有三种：

（1）基于电子信号的定位；

（2）航迹推算；

（3）环境特征匹配。

先来说说第一种。基于电子信号的定位用通俗点的话来说就是GPS定位。通过接受卫星实时的电子信号，解算出目标当前时刻的位置。不过GPS定位算法也有好有差，例如我们手机里用的定位误差就能达到三四米，这也是为什么有的时候你在桥上走，定位却显示你在水里游的原因。

那汽车显然不能用误差这么大的方法，于是就有了实时动态载波相位差分技术。说人话就是通过车辆到天上多个卫星之间的回波周期数，通过比较解除周期模糊，确定一个数值，这个值就是精确的定位位置。

这个方法的优势就是定位精度是真的很高，能达到5cm，但是缺点也很明显，他得需要在车上安装一套设备，既和卫星通讯也和基站交互，同时还不能去高楼大厦、隧道、立交桥这种对信号有遮挡的地方。显然，这一方法满足了精度，缺忽视了鲁棒性和场景。

航迹推算说的是根据上一时刻的位置姿态，结合当前测量到的信息来推测现在的位姿。做航迹推算必不可少的传感器便是IMU惯性测量单元。有印象的同学会记得我们在之前的系列中讲过这一传感器，不记得的同学请点击下方传送门，这里就不赘述这个传感器了。

直接来说说这一方法的优点，由于IMU是装在自己车上的，装上就能测，也不需要接收啥外部信号，因此对于场景就完全不在乎。同时这个小东西输出频率非常高，5ms就输出一次位姿信息，并且短时精度也很高，看着相当生猛。它的缺点就是会累计误差，用着用着就越来越不准了，因此需要不停的校准，常常与其他定位方法结合在一起消除零漂。

显然，这一方法满足了精度和场景，但是鲁棒性不够。

最后我们再来看看环境特征匹配。所谓的环境特征匹配其实就是SLAM技术，及时定位与建图。那SLAM有激光SLAM和视觉SLAM，所以根据传感器不同分为激光环境特征匹配和视觉环境特征匹配。

激光定位就是利用激光雷达事先生成一个地图，这个地图可以是3D的也可以是2D的。

随后我们把感知得到的实时点云和预先建立的地图进行匹配，最容易想到的就是搜索方式。任选地图中的某一块区域，拿现有点云和它去做比较，看颜色信息是否接近、高度信息是否一致，计算出一个匹配概率，概率最高的当然就是匹配成功的点。

通过这种方式我们就完成了一个现实世界到地图的转换，可以很精确计算出我们处在地图中的某个位置，类似在景区地图上找自己所在位置的感觉。

视觉定位与上述激光定位原理一致，也是需要一幅预制好的高精地图，通过摄像头感知到的实时数据与地图上的信息进行匹配，确认自己的位置。但是需要注意的一点是，视觉定位在自动驾驶中受光照干扰很大，因为室外场景光线会不断变化，因此之前提前到的特征点可能换个时间就提取不到了。

这时候对视觉感知提出了更高要求，需要从场景中提取语义信息，例如交通标志牌、红绿灯等等参考目标，他们相对于光照要稳定的多。除非道路大改造，否则一块路牌、一个红绿灯立在那很多年是没问题的。

这种基于环境特征的定位由于有高精地图buff在手，因而定位再差也不会差到哪儿去，同时实时的测量值输入进来也不断较准系统的预测。这样看来，这个方法的鲁棒性能够得到保证。

介绍完这三种定位方式，你会发现“小孩子才做选择，成年人全都要”。多传感器融合定位才能取长补短，让整个系统发挥出最优效能，实现自动驾驶汽车的精准定位。

以卡尔曼滤波器串连起GPS定位、IMU定位与激光点云定位