无人驾驶车辆目标检测与运动跟踪的卡尔曼滤波方法

在无人驾驶目标跟踪任务中，对目标进行运动状态估计是非常重要的，因为只有当无人驾驶车辆能够将传感器数据从环境测量信息转换成目标运动学信息时，才能给规划决策模块提供有效的信息输入。而在多目标跟踪问题中，需要对每个目标进行运动状态更新。假设已经获得了每个目标的观测量，接下来要做的是选择合适的状态变量，构建观测模型和运动模型，利用观测量来更新目标状态。而实际观测量是带有不确定性的，即含有噪声，如何从噪声中恢复或估计最优状态，这也正是卡尔曼滤波的基本思想。

卡尔曼滤波器（Kalman Filter，KF）［G. Welch，1995］是一种时域上的最优线性滤波器，由卡尔曼在20 世纪60 年代初期对离散数据线性滤波问题进行迭代求解从而演变成卡尔曼滤波思想。之后随着数字计算领域的飞速发展，卡尔曼滤波在信号处理、最优控制及导航等领域得到广泛应用。卡尔曼滤波的本质是基于贝叶斯概率原理，利用一组强而合理的假设，结合最新的系统测量值，建立最大化系统状态变量的后验概率的模型。(www.xing528.com)

卡尔曼滤波器对系统的表征分为两部分，即状态方程和观测方程，前者是时间更新方程，后者是测量更新方程。状态方程通过建立系统运动模型，利用当前最优估计状态，向前推算当前状态变量和误差协方差估计的值，即所谓先验估计，观测方程则通过建立系统观测模型，将先验估计的结果和新产生的测量量结合起来，得到最优后验估计。最早提出的卡尔曼滤波都是针对线性系统而言的，实际应用中系统经常会引入非线性部分，非线性部分的引入主要包括两个方面，一个是测量方程引入非线性因素——由于测量工具和测量原理的不同，从而导致从测量量中推导得到状态变量会引入非线性部分；另一个是过程模型，会引入非线性因素——如在建立系统状态转移方程时，系统状态变量并不一定完全遵循线性关系。由于非线性部分的引入，导致传统线性卡尔曼滤波器无法很好地估计系统状态变化，为此提出了针对非线性系统的扩展卡尔曼滤波（Extended Kalman Filter，EKF）［S. Bolognani，1999］、粒子滤波（Particle Filter，PF）［J. Carpenter，1999］和无迹卡尔曼滤波（Unscented Kalman Filter，UKF）［E. Wan，2000］等。EKF 使用雅克比矩阵将非线性系统转换成线性系统进行求解，PF 则借助蒙特卡罗思想，对系统状态进行密集采样，通过估计采样粒子的状态变换来估计系统状态，相较于EKF 计算雅克比矩阵，PF 采样也需要复杂计算量。UKF则通过构建Sigma 点进行无迹变换的思想来解决非线性系统问题，其收敛速度快、效果良好。本节主要介绍线性卡尔曼滤波和无迹卡尔曼滤波。