车辆运动状态的准确跟踪是汽车辅助驾驶系统研究领域的关键技术之一[1-2].目前，利用GPS、车载惯导、雷达、红外和机器视觉等路侧传感设备来感知道路车辆的运动状态信息已形成了较为成熟的技术与方法[3].刘进等[4]借助非线性三自由度单轨模型，并结合扩展卡尔曼滤波，对车辆的横向运动状态进行了估计；盘朝奉等[5]采用统计模型作为机动目标模型，引入卡尔曼滤波算法对车辆的运动状态进行估计，取得了良好的效果.然而，在实际道路交通环境下，由于传感器局部感知信息不完整、目标遮挡以及传感器受环境噪声影响等问题，采用基于单一传感器信息源对汽车运动状态进行估计存在较大误差，而且车辆在行驶的过程中具有匀速、加速和转弯等不同的运动状态，采用单一的运动模型必然会降低跟踪的精度.因此，本文提出了基于多传感器的信息融合方法，对车辆运动状态进行跟踪，大大提高了汽车运动状态的跟踪性能和精度，为汽车辅助驾驶提供了强有力的技术支持.
1 车辆运动状态描述
1.1 车辆运动状态测量
由于激光雷达在测距与方位感知上具有较好的效果，本文选用激光雷达作为路侧传感器.但是单一传感器的测量会受到环境和噪声的影响，使其感知能力受到限制，故采用在路侧分布式部署多个传感器形成网络的方法进行数据融合处理，使不同位置的传感器测量方位和角度不同，从而降低环境噪声对车辆状态估计的影响，达到优势互补的效果[6].路侧传感器需要输出距离和方位角两个基本测量的变量，其是实现车辆目标状态检测和跟踪的基本要素，路侧传感器测量模型如图1所示.
图1 路侧传感器测量模型
Fig.1 Measurement model for roadside sensor
图1中，大地坐标系xoy构成参考坐标系，以路侧监控系统为原点构成感知坐标系xroryr，其原点位置在大地坐标系中的坐标为(sx，sy)，目标车辆在大地坐标系中的位置坐标为
被跟踪的目标车辆到传感器之间的欧氏距离为
目标车辆与传感器坐标横轴之间的夹角为
根据图1，每个路侧传感器的测量方程为
(1)
式中：I为传感器编号，在路侧传感网中部署N个传感器，则有
为第I个传感器的零均值高斯测量误差.
1.2 车辆运动模型
车辆运动模型是用来描述运动状态信息随时间的演化过程，如位置、速度、加速度和横摆角等.要实现对目标汽车的精确跟踪和定位，关键在于如何有效地从观测数据中提取目标车辆运动状态的信息，并估计目标运动的状态轨迹[7].由于道路上行驶的车辆具有机动性，其运动模型在不同道路条件下是不同的，在进行车辆状态估计和跟踪时必须考虑其运动模型的多样性和动态特性.假设车辆在理想的二维平面内运动，通常可以由匀速直线运动和匀速转向运动来描述.设采样间隔为T，在笛卡尔坐标系下车辆运动状态的离散模型可以表示为车辆匀速直线运动模型(CV model)和车辆匀速转向运动模型(CT model)，其表达式分别为
(2)
(3)
式中：
和
为车辆状态向量；xk-1和yk-1分别为车辆运动横纵向位置坐标；
和
为相应两个方向上的速度分量；qk-1为零均值方差矩阵为Q的高斯白噪声；υk-1为零均值方差为
的高斯白噪声；
为车辆转向速率，即车辆横摆角速度.
1.3 多模型融合
多模型融合的基本思路是：假设车辆运动模型是由有限个运动模型构成的模型集，如CV、CT等，并记为M=[M1，M2，…，Mn]，在某一个时刻的运动特性可由模型Mi来描述，其先验概率为
不同时刻模型之间可以发生转移，转移可以看作一阶马尔科夫过程[8]，转移概率为
每个模型有各自对应的滤波器，每次状态融合时再用滤波器计算自己的估计值，最后根据不同模型的概率值进行综合，得到最终的融合结果.
2 多模交互UKF滤波融合估计
2.1 无迹卡尔曼滤波
考虑到汽车运动模型和测量模型的非线性特性，选择无迹卡尔曼滤波作为IMM算法中的基本滤波器[9-10].非线性系统的一般形式为
(4)
式中：xk和zk分别为n维状态向量和m维测量向量；f(·)为系统的非线性状态转移函数；h(·)为系统的非线性测量函数；系统过程噪声qk～N(0，Qk-1)和系统量测噪声rk～N(0，Rk)都是互补相关的高斯白噪声.给定t0时刻的初始状态
及其方差p0，UKF的计算过程由预测和更新两个部分构成.
1) 预测.选择Sigma采样策略[11]，首先由
和
计算一组Sigma点ξi，k-1(i=0，1，…，L)，通过非线性状态转移函数f(·)传播为γi，k|k-1，由γi，k|k-1可得状态预测
及预测协方差矩阵
计算公式为
γi，k|k-1=fk-1(ξi，k-1) (i=0，1，…，L)
(5)
(6)
(7)
式中，
分别为Sigma点一阶和二阶权值.
2) 更新.根据选定的Sigma采样策略利用和
计算Sigma点ξi，k|k-1(i=0，1，…，L)，通过非线性测量函数h(·)传播为χi，k|k-1，由χi，k|k-1可得输出预测
及自协方差矩阵
和互协方差矩阵
计算公式为
χ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)
式中：
为模型j在上一时刻的模型概率；πji为模型的转移概率，代表车辆运动模型从模型j迁移到模型i的概率；i或j取值为1时代表CV模型，取值为2时代表CT模型.
的计算公式为
(16)
重初始化的状态与协方差按混合估计分别为
(17)
(18)
2) 滤波.滤波计算步骤和通常情况下的卡尔曼滤波过程一致，本文采用UKF滤波器，包括预测UKFp和更新UKFu，其表达式为
(19)
(20)
3) 模型概率更新.计算每个模型的测量似然函数[14]，即
(21)
式中，
和
分别为模型i在UKF滤波更新中的测量残差和协方差.模型i在k时刻的概率计算表达式为
(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)
其中，CV模型的过程噪声方差q=0.01 m/s，CT模型的过程噪声方差
利用均方根误差RMSE对估计结果进行评价，即
(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%，为汽车的安全和辅助驾驶提供了强有力的技术支持.
[1]韩飞龙，应捷，朱丹丹.一种新的车辆辅助驾驶动态障碍物检测与分类方法 [J].计算机应用研究，2017，34(6)：1909-1912.
(HAN Fei-long，YING Jie，ZHU Dan-dan.New me-thod of moving obstacles detection and classification for driver assistance system [J].Application Research of Computers，2017，34(6)：1909-1912.)
[2]李进，刘洋洋，胡金芳.人机协同下辅助驾驶系统的车道保持控制 [J].机械工程学报，2018，54(2)：169-175.
(LI Jin，LIU Yang-yang，HU Jin-fang.Lane tracking of driver assistance system with man-machine-coordination [J].Journal of Mechanical Engineering，2018，54(2)：169-175.)
[3]姚琛，李成兵.激光测距技术在交通安全辅助驾驶中的应用 [J].激光杂志，2016，37(12)：25-28.
(YAO Chen，LI Cheng-bing.Application of laser ranging technology in traffic safety assistant driving [J].Laser Journal，2016，37(12)：25-28.)
[4]刘进，赵熙俊.一种自主车辆横向运动状态估计的方法 [J].车辆与动力技术，2017(3)：1-7.
(LIU Jin，ZHAO Xi-jun.State estimation of lateral dynamics for autonomous vehicles [J].Vehicle & Power Technology，2017(3)：1-7.)
[5]盘朝奉，丁亚强，江浩斌.基于车辆前方目标运动模型的主动避撞系统的研究 [J].重庆理工大学学报(自然科学)，2018，32(7)：25-31.
(PAN Chao-feng，DING Ya-qiang，JIANG Hao-bin.Research on active collision avoidance system based on vehicle forward target movement model [J].Journal of Chongqing University of Technology (Natural Science)，2018，32(7)：25-31.)
[6]袁大龙，纪庆革.协同运动状态估计的多目标跟踪算法 [J].计算机科学，2017，44(11)：154-159.
(YUAN Da-long，JI Qing-ge.Multiple object tracking algorithm via collaborative motion status estimation [J].Computer Science，2017，44(11)：154-159.)
[7]范云锋，郑益凯.IMM-UKF机动目标跟踪方法在地空导弹武器指控系统中的应用研究 [J].航天控制，2018，36(4)：28-33.
(FAN Yun-feng，ZHENG Yi-kai.Research on maneuvering target tracking based on IMM-UKF in air defense command and control system [J].Aerospace Control，2018，36(4)：28-33.)
[8]范岩，马立平.利用马尔可夫链的高校教师教学质量模糊综合评价方法 [J].数学的实践与认识，2017，47(4)：77-82.
(FAN Yan，MA Li-ping.Research on the fuzzy comprehensive evaluation model of college teachers’ teaching quality evaluation based on Markov chain [J].Mathematics in Practice and Theory，2017，47(4)：77-82.)
[9]赵亚妮.基于强跟踪卡尔曼滤波的电池SOC估计 [J].沈阳工业大学学报，2018，40(2)：192-197.
(ZHAO Ya-ni.Battery SOC estimation based on strong tracking Kalman filtering [J].Journal of Shenyang University of Technology，2018，40(2)：192-197.)
[10]汤启，何腊梅.带非线性等式约束无迹卡尔曼滤波方法 [J].计算机应用，2018，38(5)：1481-1487.
(TANG Qi，HE La-mei.Unscented Kalman filtering method with nonlinear equality constraint [J].Journal of Computer Applications，2018，38(5)：1481-1487.)
[11]刘毅，谭国俊，何晓群.优化电池模型的自适应Sigma卡尔曼荷电状态估算 [J].电工技术学报，2017，32(2)：108-118.
(LIU Yi，TAN Guo-jun，HE Xiao-qun.Optimized battery model based adaptive Sigma Kalman filter for state of charge estimation [J].Transactions of China Electrotechnical Society，2017，32(2)：108-118.)
[12]许红，袁华东，谢文冲，等.角闪烁下基于变分贝叶斯
交互式多模型的目标跟踪 [J].电子与信息学报，2018，40(7)：1583-1590.
(XU Hong，YUAN Hua-dong，XIE Wen-chong，et al.Variational bayesian-interacting multiple model tracking filter with angle glint noise [J].Journal of Electronics & Information Technology，2018，40(7)：1583-1590.)
[13]吴清怡，吴中红.炮弹发射弹道目标精度校正估计仿真 [J].沈阳工业大学学报，2018，40(5)：571-576.
(WU Qing-yi，WU Zhong-hong.Simulation for correction estimation of accuracy of trajectory target with projectile launching [J].Journal of Shenyang University of Technology，2018，40(5)：571-576.)
[14]杨啟明，徐建城，田海宝，等.基于IMM的无人机在线路径规划决策建模 [J].西北工业大学学报，2018，36(2)：323-331.
(YANG Qi-ming，XU Jian-cheng，TIAN Hai-bao，et al.Decision modeling of UAV on-line path planning based on IMM [J].Journal of Northwestern Polytechnical University，2018，36(2)：323-331.)