罗 勇，陈 慧

(同济大学 汽车学院，上海 201804)

随着我国汽车保有量逐年上升，路边停车位、车库等资源变得越来越紧张，增加了泊车的难度。因此，可提高安全性并能减轻驾驶员负担的自动泊车系统受到了广泛关注，并成为研究的热点。

自动泊车系统大都是根据检测出的库位信息，规划一条泊车轨迹，然后控制车辆跟踪轨迹进行泊车[1-4]。因此，自动泊车中实时准确地获得车辆当前的定位信息十分必要，可增强系统的安全性和可靠性。文献[5]利用四轮小型机器人在室内进行试验，基于运动学模型开发了泊车控制算法，但由于四轮机器人的机械特性和乘用车有所差别，并且室内环境相比于室外环境要理想很多，所以结果参考性不强；文献[6]在乘用车上开发了泊车系统，其位姿估计算法用了4个ABS轮速传感器和1个方向盘转角传感器，试验中利用定位精度1 cm的差分GPS来验证算法精度，轨迹终点误差达到了30 cm；文献[7]用4个轮速传感器和1个方向盘转角传感器设计了位姿估计算法，并在乘用车上进行试验，同样针对轨迹终点的位置偏差进行讨论，但并未提及参考位置的获取方式，对结果的论证缺乏严谨性。

对于位姿估计算法的研究，难点在于解决误差累积的问题。常见的精度影响因素有车辆实际参数与理论值不同(如车轮半径、轮距等)，参数的实时变化，传感器信号噪声等。常采用设计参数标定实验、滤除信号噪声、多传感器信号融合等来减少误差累积。

本文基于扩展卡尔曼滤波(EKF)技术，利用2个后轮轮速信号和方向盘转角信号设计了车辆位姿估计算法。通过离线仿真及实车试验，验证了该算法的可行性和估计精度。

1 位姿估计算法设计

1.1 坐标系定义和运动学模型

如图1所示，在全局坐标系xOy下，k时刻车辆的位姿信息表述为：车辆后轴中点的坐标(xk,yk)，以及车辆中轴线指向车头的方向与x轴的夹角θk。

泊车工况下，以运动学模型来近似描述车辆状态，将系统离散化后，k时刻的位姿信息满足

Δxk=vkT0cos θk，

(1)

Δyk=vkT0sin θk，

(2)

图1 坐标系定义

(3)

式中 Δxk，Δyk为k时刻车辆位姿信息的增量；T0为采样时间步长;vk为车辆后轴中点纵向速度;ωk为车辆横摆角速度。

初始时刻起，对式(1) ～ (3)积分，可得k时刻车辆的位置(xk,yk)和航向角θk。然而，利用式(1) ～ (3)来计算车辆位姿，信号的噪声可能会在积分运算中累积，进而影响算法的估计精度。所以，需要滤除算法中所用信号的噪声。

1.2 卡尔曼滤波器

图2 卡尔曼滤波器结构

卡尔曼滤波器只适用于线性系统，当状态方程或测量方程为非线性方程时，常使用扩展卡尔曼滤波算法。其基本思想是求非线性方程的一阶Taylor展开式，忽略其余高阶项，从而将非线性转为线性，进而可将卡尔曼滤波理论应用于非线性系统中。本文基于式(1) ～ (3)设计位姿估计算法，由于方程是非线性的，需要使用EKF算法。

1.3 算法实现

卡尔曼滤波器原理设计算法，需要计算k+1时刻的系统矩阵Φk+1,k和k+1时刻的测量矩阵Hk+1。

设系统状态向量X为

X=[x,y,θ,kl,kr]T，

式中 x、y和θ为车辆位姿信息;kl和kr为两个后轮半径变动系数，用来描述实际运行过程中车轮半径的变化量。

设系统观测向量为Z，有

Z=[v,ω]T，

式中 v为车辆后轴中点的速度;ω为车辆的横摆角速度。

图3 算法结构

算法的总体结构如图3所示，输入量为系统观测向量Z，输出量为车辆的位姿信息。实际应用中，观测量v和ω的获取方式为：取两后轮轮速的平均值算得后轴中点的速度v；提前标定好方向盘转角和后轴中点转弯半径的关系，通过查表插值的方式获得某方向盘转角对应的车辆后轴中点转弯半径R0，通过v和R0计算得到车辆横摆角速度ω。

1)时间更新

式中f(Xk,k)为k时刻系统状态的增量，f(Xk,k)=[Δxk,Δyk,Δθk,0,0]T。

利用k时刻左右2个后轮轮速(v3k,v4k,)和各自车轮半径变动系数(klk,krk)来计算vk和ωk，有

vk=(klkv3k+krkv4k)/2，

(4)

ωk=(-klkv3k+krkv4k)/D，

(5)

式中 D为后轮轮距。

综合公式(1)～(3)及(4) ～ (5)，求得f(Xk,k)为

(6)

使式(6)线性化，得k+1时刻的系统矩阵Φk+1,k为

2)测量更新

k时刻，系统观测向量Zk为

对Zk线性化后得Hk+1为

对Φk+1,k和Hk+1的计算完成之后，再根据图2所示的算法结构进行计算，便完成了基于EKF技术的位姿估计算法的设计。

2 仿真试验

在veDYNA中进行仿真，它是基于Matlab/Simulink平台开发的车辆动力学模型，被广泛应用于车辆动力学仿真等领域。

按照所用实验车辆的参数对veDYNA车辆模型进行相应的设置：整车质量1 184 kg，轴距2.407 m，后轮距1.45 m，车轮半径0.288 m，转向传动比16.3。

仿真时间为40 s，最高车速为2.5 m/s。在轮速信号中添加随机扰动来模拟实际轮速信号的噪声。所加的随机扰动满足高斯分布，其均值和标准差分别为0，0.7 r/min，运行时左后轮轮速信号如图4所示，车辆行驶轨迹如图5所示。

(1)地质灾害高易发区(Ⅰ)。地质灾害高易发区主要分布县境内巴水河、尕干曲下游段两岸河谷平原区以及赛欠曲两岸低山丘陵区、黄河沿岸一带中高山区，总面积760.51 km2，占总面积的15.18%，该区地貌单元主要为河谷冲积带状平原、低山丘陵区和中高山区。区内共发育地质灾害点199个，泥石流灾害最为发育，共计116条，其次为滑坡65处，不稳定斜坡18段。

图4 左后轮轮速信号波形 图5 车辆行驶轨迹

车辆行驶了26.26 m，位置误差结果如图6所示。本文所设计的算法在x和y方向上的最大估计偏差分别3.76 cm和2.03 cm，此精度的估计结果对于自动泊车系统的开发具有实际应用价值。

图6 位置误差

3 实车试验

3.1 试验方案介绍

图7 春晖2号电动汽车

图7所示为同济大学汽车学院“春晖2号”分布式四轮驱动电动汽车，以它为平台进行实车试验。

使用MicroAutoBox快速原型控制器采集传感器数据以及运行估计算法。用RT3002型GPS/INS导航仪采集车辆位置和航向角信息作为参考值，并与算法估计的结果对比，以验证算法的精度。导航仪的位置精度为：2 cm(1σ)，航向角精度为0.1(°)(1σ)。

3.2 直线行驶工况

试验中，车辆沿直线行驶，如图8所示，对比GPS所测行驶轨迹和算法估计的行驶轨迹。x和y方向上的偏差分别如图9所示。车辆行驶12.22 m，位置估计的最大误差在x方向为6.31 cm，y方向为2.74 cm。

图8 行驶轨迹 图9 位置误差

3.3 圆弧轨迹行驶工况

在行驶过程中，保持方向盘转角为-178°。对比GPS所测车辆行驶轨迹和算法估计得到的行驶轨迹，如图10所示。x和y方向的偏差分别如图11所示。车辆行驶距离13.53 m，位置最大误差x方向为5.02 cm，y方向为4.77 cm。

图10 行驶轨迹 图11 位置误差

3.4 泊车工况

3.5 试验小结

算法估计得到的车辆直线、圆弧和泊车轨迹均能与GPS所测车辆实际轨迹保持较高的一致性，尤其是沿直线和圆弧轨迹行驶的时候。算法估计得到的车辆位姿信息，可以为自动泊车系统提供车辆的轨迹信息，为增强泊车安全性奠定了基础。

图12 行驶轨迹 图13 位置误差

4 结论

基于扩展卡尔曼滤波技术设计了一套自动泊车系统位姿估计算法，算法用到了两个后轮轮速传感器及方向盘转角传感器。仿真和实车试验的结果表明，算法具有可行性，且位置估计的最大误差控制在3%的范围内，换言之，车辆行驶10 m，x和y方向上最大位置估计误差在30 cm之内。本文提出的算法可以为自动泊车系统提供车辆轨迹信息的支持，为增强泊车安全性奠定了基础。

参考文献：

[1]吴锴.智能自动泊车系统研究[D].南京: 南京理工大学, 2008.

[2]久保田有一, 岩切英之, 岩崎克彦，等.用于车辆的停车辅助装置及停车辅助方法:中国, CN100535598C[P].2009-09-09.

[3]Sven Chlosta, Michael Dr Rohlfs. Method and Device for Parking a Motor Vehicle：Germany, EP1561673A2[P].2005-08-10.

[5]Kooktae Lee, Dalhyung Kim, Woojin Chung, et al. Car Parking Control Using a Trajectory Tracking Controller[C]//International Joint Conference.Busan： SICE-ICASE,2006.

[6]Kochem M, Wagner N, Hamann, et al. Data Fusion for Precise Dead Reckoning of Passenger Cars[C].Spain: IFAC, 2002.

[7]张诚, 罗勇, 陈慧. 基于扩展卡尔曼滤波算法的智能泊车航位推算研究[J].上海汽车, 2012(6)：56-58，62.

[8]付梦印, 邓志红, 闫莉萍. Kalman滤波理论及其在导航系统中的应用[M].2版.北京：科学出版社，2010.