ADAS/AD控制器模块开发10 - 卡尔曼滤波器与智能驾驶

前言

本来打算把基于CAN通信的五部曲（CAN通信、UDS诊断、Bootloader、Alignment、PCAN）先更完再讲其他Building blocks，但是一连几天的CAN相关内容写起来确实让人烦躁，今天又恰巧碰到一篇非常棒的卡尔曼滤波文章，临时改了注意，打算先留着Bootloader的坑，明天有时间再填。要分享的这篇文章来源于BZARG大神的文章《How a Kalman filter works， in pictures》，由微信公众号汽车ECU设计翻译整理，本文权当做自习笔记，同大家分享。无论如何，手打一遍总比读一遍印象深刻~

PS：再次重申，版权归汽车ECU设计和BZARG所有。侵权立删。

 

一、什么是卡尔曼滤波？

• 利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。

• 由于观测数据中包含噪声和干扰，所以最优估计也可以看做是滤波过程。

• 数据滤波是去除噪声、还原真实数据的一种数据处理技术。Kalman滤波在测量方差已知的情况下，能够从一系列处在测量噪声的数据中，估计动态系统的状态。

• 它具有占用内存空间小（除了前一个状态量外，无需保留其他历史数据）的特点，并且速度很快，适合应用于实时问题和嵌入式系统。

 

二、卡尔曼滤波器的应用范围

卡尔曼滤波器是一种从ADAS到AD领域都被滥用的基础滤波技术。不论是视觉上的车道线跟踪、目标跟踪，还是毫米波雷达或激光雷达的目标跟踪，概括的讲，只要用“跟踪”这个词来形容的算法，即tracking、Tracker等这种词描述的算法，基本都要用kalman滤波处理下数据。

 

三、卡尔曼滤波的原理

假设一个问题模型

一个在树林中到处跑的小机器人，这个小机器人有GPS定位系统，但是准确度不够（精度10米），仅依赖GPS会导致小机器人撞到树上或者掉到沟壑中。

小机器人知道它给执行器发送的控制量，可以大致估计自己应该走的轨迹，但是悲催的是，它不知道它自己的执行器的执行效果到底如何。比如，它知道它当前的环境，计算出了需要以5kph的速度、转向6°的角度来绕过一个障碍物，但是执行器执行时可能因为路面颠簸不平，导致车速快了一点、轮子因风吹影响角度偏了一点（但是小机器人自己不知道这些情况），导致发生事故。

 

四、状态表征描述

假设小机器人状态表征只有两个参数，位置和速度。

我们并不知道实际的位置和速度，他们之间有很多种可能正确的组合，但其中一些组合的可能性要大于其他组合。如下图（图1）.

图1 白色模糊区域表征所有可能的状态空间坐标位置

 

卡尔曼滤波器假设两个变量（位置、速度）都是随机的，并且服从高斯分布，即，每个变量都有一个均值μ，表征随机分布的中心（代表最最可能的状态），方差σ^2表征圆的半径，表示不确定性。假如有多个参数，那么中心只有一个，即状态均值μ，但是方差则对应不同参数，有多个方差存在，那么这个分布也不再是圆形，而是按照坐标系不同的纬度有不同的方差（不确定度）。如图2所示。