基于IMM-UKF的协同式车辆运动状态跟踪算法*

崔雅博， 王晓婷

(开封大学 信息工程学院， 河南 开封 475004)

车辆运动状态的准确跟踪是汽车辅助驾驶系统研究领域的关键技术之一[1-2].目前，利用GPS、车载惯导、雷达、红外和机器视觉等路侧传感设备来感知道路车辆的运动状态信息已形成了较为成熟的技术与方法[3].刘进等[4]借助非线性三自由度单轨模型，并结合扩展卡尔曼滤波，对车辆的横向运动状态进行了估计；盘朝奉等[5]采用统计模型作为机动目标模型，引入卡尔曼滤波算法对车辆的运动状态进行估计，取得了良好的效果.然而，在实际道路交通环境下，由于传感器局部感知信息不完整、目标遮挡以及传感器受环境噪声影响等问题，采用基于单一传感器信息源对汽车运动状态进行估计存在较大误差，而且车辆在行驶的过程中具有匀速、加速和转弯等不同的运动状态，采用单一的运动模型必然会降低跟踪的精度.因此，本文提出了基于多传感器的信息融合方法，对车辆运动状态进行跟踪，大大提高了汽车运动状态的跟踪性能和精度，为汽车辅助驾驶提供了强有力的技术支持.

1 车辆运动状态描述

1.1 车辆运动状态测量

由于激光雷达在测距与方位感知上具有较好的效果，本文选用激光雷达作为路侧传感器.但是单一传感器的测量会受到环境和噪声的影响，使其感知能力受到限制，故采用在路侧分布式部署多个传感器形成网络的方法进行数据融合处理，使不同位置的传感器测量方位和角度不同，从而降低环境噪声对车辆状态估计的影响，达到优势互补的效果[6].路侧传感器需要输出距离和方位角两个基本测量的变量，其是实现车辆目标状态检测和跟踪的基本要素，路侧传感器测量模型如图1所示.

图1 路侧传感器测量模型Fig.1 Measurement model for roadside sensor

(1)

1.2 车辆运动模型

车辆运动模型是用来描述运动状态信息随时间的演化过程，如位置、速度、加速度和横摆角等.要实现对目标汽车的精确跟踪和定位，关键在于如何有效地从观测数据中提取目标车辆运动状态的信息，并估计目标运动的状态轨迹[7].由于道路上行驶的车辆具有机动性，其运动模型在不同道路条件下是不同的，在进行车辆状态估计和跟踪时必须考虑其运动模型的多样性和动态特性.假设车辆在理想的二维平面内运动，通常可以由匀速直线运动和匀速转向运动来描述.设采样间隔为T，在笛卡尔坐标系下车辆运动状态的离散模型可以表示为车辆匀速直线运动模型(CV model)和车辆匀速转向运动模型(CT model)，其表达式分别为

(2)

(3)

1.3 多模型融合

2 多模交互UKF滤波融合估计

2.1 无迹卡尔曼滤波

考虑到汽车运动模型和测量模型的非线性特性，选择无迹卡尔曼滤波作为IMM算法中的基本滤波器[9-10].非线性系统的一般形式为

(4)

γi，k|k-1=fk-1(ξi，k-1) (i=0，1，…，L)

(5)

(6)

(7)

χi，k|k-1=hk(ξi，k|k-1)

(8)

(9)

(10)

(11)

在获得新的测量值zk后，进行滤波量测更新，即

(12)

(13)

(14)

2.2 融合估计算法流程

综合车辆运动的CV和CT模型，设计基于IMM的车辆运动状态融合估计算法，其中每个模型的基本滤波器采用UKF滤波器，算法过程分为4个步骤：交互、滤波、模型概率更新和融合估计[12-13].

1) 交互.设第i个模型在有效的情况下，将由上一时刻各滤波器估计混合的结果作为该模型滤波器的输入.设k-1和k时的匹配模型分别为j和i，则混合概率的表达式为

(15)

(16)

重初始化的状态与协方差按混合估计分别为

(17)

(18)

2) 滤波.滤波计算步骤和通常情况下的卡尔曼滤波过程一致，本文采用UKF滤波器，包括预测UKFp和更新UKFu，其表达式为

(19)

(20)

3) 模型概率更新.计算每个模型的测量似然函数[14]，即

(21)

(22)

4) 融合估计.计算k时刻联合估计与联合估计误差协方差矩阵，其表达式为

(23)

(24)

3 仿真实验与分析

3.1 实验设计

为了验证算法的性能，选择交通仿真软件CARSIM进行仿真实验，该仿真平台软件可以模拟各种无线通信环境和通信协议，提供了多种传感器模型.采用软件提供的仿真功能模块构建路侧传感网，模拟车路通信功能，预先设定目标车辆的行驶路线和运动参数，图2为仿真场景图.

图2 仿真场景Fig.2 Simulation scenarios

仿真场景设定为FHWA公路的某一路段，包括一段直线行驶道路和弯道行驶道路.设置路侧传感网由3个激光雷达构成，分别位于交叉不同方位以覆盖整个行驶区域.雷达扫描角度范围和分辨率分别设定为70°和0.2°，探测最大距离为70 m，采样率为20 Hz(即计算步长为0.05 s)，测距和方位角测量噪声的分辨率分别为0.05 m和0.05 rad.假定实验中仅采用一辆目标车辆作为状态跟踪的对象，车辆从图2中A点出发，途经一段弯道并转向到达B点，算法中模型转移概率矩阵设为

(25)

(26)

式中，lt和rt分别为采样点t(t=1，2，…，n)上的估计值和真实值.

3.2 单模型跟踪结果

单一模型情况下车辆运动只考虑CV模型，滤波器仍然采用UKF.图3a为轨迹的跟踪结果，其中，实线为车辆经过的真实轨迹，虚线为估计值.图3b为速度绝对值的跟踪结果，其中，实线为真实速度值曲线，虚线为估计值.

图3 单一CV模型跟踪结果Fig.3 Tracking results by single CV model

由图3可知，采用单一CV模型进行估计时，位置误差为RMSE-Positionmodel-1=11.62 m；速度误差为RMSE-Speedmodel-1=14.87 m/s.位置和速度估计出现偏差较大的时段正是车辆行驶至交叉口并开始转向后的一段时间，表明CV模型并不能很好地刻画车辆转向这一运动趋势.

3.3 多模型跟踪结果

联合CV和CT两个模型，利用本文提出的多模型融合估计算法对车辆运动状态进行跟踪，结果如图4所示.图4a为轨迹的跟踪结果，其中，实线为车辆经过的真实轨迹，虚线为估计值.图4b为速度绝对值跟踪结果，其中，实线为真实速度值，虚线为估计值.

图4 多模型数据融合算法跟踪结果Fig.4 Tracking results by multi-model data fusion algorithm

由图4可知，采用多模型算法可得，位置误差为RMSE-PositionIMM=1.53 m；速度误差为RMSE-SpeedIMM=3.18 m/s.对比图3、4的结果可以看到，采用多模型的状态跟踪方法与使用单模型的跟踪方法相比，位置和速度值的估计精度均有大幅提高，跟踪误差分别降低了86.8%和78.6%.

4 结 论

采用交互多模型设计的车辆运动状态估计方法能够根据运动状态的变化实现模型之间的自动切换，通过UKF滤波的融合估计大大提高了车辆行驶过程中运动状态参数的精度.仿真结果表明：本文算法具有良好的适应性，与单一模型的跟踪方法相比，位置和速度值的跟踪误差分别降低了86.8%和78.6%，为汽车的安全和辅助驾驶提供了强有力的技术支持.