基于Carsim的车辆运动轨迹跟踪算法_张亚岐

u v wr ax v u wr a y
京 汽 车
坐标变换和速度积分求解车辆运动轨迹 在此以线性二自由度的车辆模型为基础，忽 略车辆沿 z 轴的垂直运动，在车辆坐标系下分析 车辆的运动，车辆在固定坐标系下的坐标值用下 式可以求得[3]
（11）
X VX dt u cos v sin dt Y VY dt u sin v cos dt
关键字：轨迹；质心；Kalman 滤波；Simulink；Carsim
北 京 汽 车 0
中图分类号：U462.3
文献标识码：A
引பைடு நூலகம்
言
靠的数据支持， 也为后续的车辆目标跟踪研究提 供现实基础。
汽车质心的运动轨迹作为车辆最直观的运 动结果，对于驾驶员——汽车闭环系统运动性能 的分析和评价是十分重要的。车辆行驶轨迹的在 线计算一直是车辆研究人员关注的焦点。车载辅 助驾驶系统基本都是靠对车辆状态的在线估计来 支撑的，典型的如全球定位系统（GPS） 、地理信 息系统（GIS） 、侧面碰撞修正系统（BSI）以及 压线报警系统都是以车辆当前的参数为基础，通 过条件判断、预测来控制系统的执行操作，这些 系统都是针对豪华轿车设计的。因此，寻求低成 本、高精度且能实时计算车辆运动轨迹的方法已 成为车辆控制研究的重点，而经典 Kalman 技术 能够有效去除输入变量中的噪声，从而为仿真的 可靠性和精确性提供数据基础。文中以二自由度 的车辆模型[1]为基础， 利用 Simulink 和 Carsim 进 行联合仿真。Simulink 能够实时在线计算车辆的 行驶轨迹，进而能够对车辆进行跟踪。为了保 证试验真实可靠，将一段实际道路曲线作为 Carsim 的道路输入，这就为试验验证提供了可 · 22 ·
图6
车辆侧向加速度
[4]高越，杨得军，宗长富，等. 汽车轨迹测量的速度积分方法 及其实施技术[J]. 汽车技术，2002， （9） .
收稿日期： 201111 09
从图中可以看出，通过 Kalman 滤波平滑处理后， 各个参数的波动性明显减小，曲线变得更为平滑。 · 24 ·
《北京汽车》 2012.No.2
・基于 Carsim 的车辆运动轨迹跟踪算法・ 文章编号： 1002-4581(2012)02-0022-03
基于 Carsim 的车辆运动轨迹跟踪算法
张亚岐 Zhang Yaqi
（长安大学汽车学院，陕西西安，710064） 摘 要：车辆质心运动轨迹作为车辆最为直观的运动结果，对车辆行驶安全性和操纵稳定性的研究十分重要，
图 2 汽车运动参数在固定坐标系和车辆坐标系中的表达
2.1.1
计算汽车横摆角 俯仰角、 俯仰角速度 根据车身侧倾角 、


以及欧拉角速度和动坐标系角速度的转换关系计 算车辆的横摆角[4]
wr sin q cos
q sin wr cos sec
d t
2.1.2


（7） （8）

北
（9）
图1
Kalman 滤波器的工作原理图
2
2.1
运动方程的建立
计算车辆坐标系下的纵向速度和横向速度 根据坐标变换可以得出车辆坐标系下的纵 向、横向加速度与固定坐标系下的纵向、横向加 速度之间的关系 ax cos sin a X （10） a y sin cos aY 车辆坐标系下的车辆纵向、横向加速度为
且准确计算车辆的行驶轨迹对驾驶员准确操控车辆很有帮助。但是由于技术水平的制约，当前车载定位系统的精度和 稳定性较差， 且价格昂贵。 文中以二自由度车辆模型为基础， 通过坐标系变换， 建立质心坐标的微分方程， 运用 Kalman 滤波技术对输入变量进行滤波，通过 MATLAB/SIMULINK 和 Carsim 进行联合仿真，试验结果与真实车辆行驶轨迹相 吻合。
（5） （6）
其中，X,Y 为固定坐标系下汽车质心纵向和 VX,VY 为固定坐标系下车辆质心的纵向 侧向位移； 速度和横向速度； u,v 为车辆坐标下的纵向速度和 横向速度； 为车辆的横摆角。汽车运动参数在 固定坐标系和车辆坐标系中的表达： 其中 OXY 为 固定坐标系，oxy 为车辆坐标系。
《北京汽车》 2012.No.2
仿真结果显示，仿真计算的车辆行驶轨迹基 本与道路曲线相吻合。总的趋势与实际道路曲线 保持一致，误差为 5.9%，该仿真算法能够对车辆 行驶轨迹进行较好跟踪。从图 7 中也可以看出， 仿真结果有些滞后，这可能是由于转向系的响应 延迟导致车辆横摆角速度、侧向加速度响应滞后 而产生的， 因此只需对仿真结果向右平移 0.05 个 单位就能跟实际道路曲线很好地吻合，进而能够 对车辆行驶轨迹进行准确的跟踪。
1
Kalman 预测算法
卡尔曼滤波器用于估计离散时间过程的状态
n
变量 x R 。离散线性定常系统的状态方程如下： xk Axk 1 Buk 1 wk 1 （1） 定义观测变量 z R ，得到量测方程
m
zk Hxk vk
（2）
其中 wk 和 vk 分别表示过程激励噪声和观测 噪声。假设它们为相互独立、正态分布的白色噪 声，Kalman 滤波器主要是通过式（1）对下一个 状态进行估计，根据 zk 对估计量进行修正
4
结
论
图5
车辆纵向加速度
文中利用 MATLAB/Simulink 和 Carsim 进行 联合仿真，以实际道路曲线作为仿真的道路曲线 输入，且对车辆横摆角速度、车辆纵向加速度以 及侧向加速度进行了 Kalman 滤波，剔除输入参 数中随机噪声，仿真结果与实际车辆运动曲线相 吻合。运用 Simulink 和 Carsim 联合仿真能够有 效地对车辆行驶轨迹进行跟踪，但是仅用二自由 度的车辆模型建立的车辆运动微分方程不能充分 反映车辆质心侧偏和轮胎侧偏对仿真结果的影 响，对于精度要求较高的跟踪系统还需建立更为 复杂的车辆模型。 参考文献
利用（9）式、 （11）式即可得到车辆坐标系 下的汽车的纵向速度、横向速度以及横摆角 u v wr ax d t （12） v u w a d t r y 再结合（5 ）式、 （6 ）式就能求出车辆的质 心运动轨迹。
3
试验验证
试验采用 Simulink 和 Carsim 联合仿真，考 虑到直线行驶时车辆运动轨迹为简单直线， 因此， 选取较为复杂的实际道路曲线作为 Carsim 的道 路曲线输入，来验证该跟踪算法的可靠性和真实 · 23 ·
x k x k K zk H x k （3） Pk H T K k Pk H T HPk H T R （4） HPk H T R
其中， x k R n （-代表先验，^代表估计） 为在已知第 k 步以前状态情况下第 k 步的先验状
・基于 Carsim 的车辆运动轨迹跟踪算法・
性。将车辆起始位置设置为（0，0） ，起始时间为 0。在 Carsim 中输入的道路曲线如图 3 所示。
图7 图3 试验道路曲线
实际试验道路曲线与车辆行驶轨迹对比
图 4～图 6 为试验中采集到的车辆横摆角速 度、纵向加速度和侧向加速度。
北 京 汽 车
图4 车辆横摆角速度
[1]余志生. 汽车理论（第 4 版）[M]. 北京：机械工业出版社， 2006. [2]付梦印，邓志红，阎莉萍. Kalman 滤波理论及其在导航系统 中的应用（第 2 版）[M]. 北京：科学技术出版社，2010. [3]M.米奇克. 汽车动力学（第 4 版）[M]. 陈荫三，余强，译. 北京：清华大学出版社，2009.
《北京汽车》 2012.No.2

・基于 Carsim 的车辆运动轨迹跟踪算法・
态估计； x k R 为已知测量变量 zk 时第 k 步的 后验状态估计；Pk 为先验估计误差协方差；Pk 为
n

后验估计协方差；R 为观测噪声的方差。 卡尔曼滤波器用反馈控制的方法估计过程 状态：滤波器估计过程某一时刻的状态，然后以 （含噪声的）测量变量的方式获得反馈。最终的 Kalman 估计算法成为一种具有数值解的预估－ 校正算法[2]，Kalman 滤波器的工作原理如图 1。