基于模糊卡尔曼滤波车速估算的泊车运动学模型

王 龙，李红娟

(西安航空学院 a.车辆工程学院；b.汽车检测工程技术中心， 西安 710077)

未来汽车的主要发展方向之一是智能化，自动泊车系统作为智能汽车的重要组成部分，越来越多地受到来自科研机构和汽车厂家的关注[1]。

目前，科研机构对自动泊车技术的研究主要是在车位智能快捷识别、自动泊车路径规划、泊车运动跟踪及控制等。在车位智能快捷识别的研究中，文献[1-3]主要通过距离和视觉信息融合对停车位进行智能识别。这些泊车车位智能快捷识别对泊车的运动学因素均有考虑，但并未对泊车运动学模型的建立做深入的研究。

在自动泊车的路径规划与运动控制研究中，文献[4]解决多车泊车路径规划问题，文献[5]提出了一种基于优化狭窄环境下的停车场路径规划方法。此外，还有其他文献的泊车路径规划[6-8]，而目前缺乏基于自动泊车自身特点的专用泊车运动学模型。文献[9]提出了一种基于坐标补偿的自动泊车系统无模型自适应控制。文献[10]提出了一种基于机器视觉和模糊控制的自动泊车系统。其他的文献还有利用卡尔曼滤波控制、自适应控制以及神经网络算法和智能算法[11-14]等控制方法来解决泊车运动跟踪及控制问题。自动泊车的路径规划与运动控制研究的前提就是建立泊车的运动学模型，但这些泊车运动学模型主要基于1989年J.P.Laumond and T.Simeon中提出的简化汽车运动学模型，缺乏基于自动泊车自身特点的专用泊车运动学模型理论及验证实验。

自动泊车的运动过程是一种非完整链式系统[15]，具有非线性、时变、多变量等特点，很难建立起准确的非完整链式自动泊车模型，但可以通过对模型相关因素的分析建立简化的汽车运动学模型。此外，文献[16]通过分析没有主观知识的最优控制问题，实现了基于时空分解的平行泊车运动优化策略，提供精确、客观的车辆运动学模型，但其理论深奥，算法复杂，缺乏实车验证。文献[17]利用自动泊车的起始坐标位置或起始区域、车速、车身转角、前轮转角的实时数据建立简化的汽车运动学模型，通过对速度误差和车身转角误差的考虑建立模糊逻辑控制器，实现了较好的泊车效果，但对于狭小空间内的多次泊车超低速运动缺乏深入研究和实车验证。

总之，泊车运动过程中的控制复杂，建立基于自动泊车自身特点的专用运动学模型十分重要。本文通过对相关文献和实际泊车过程的分析，提出了一种基于模糊卡尔曼滤波车速估算的泊车运动学模型，符合泊车运动过程的要求，同时可以降低泊车运动过程中位移和航向角的计算误差。

1 汽车简化的泊车运动学模型

1989年，J.P.Laumond and T.Simeon提出了简化的汽车运动学模型如图1所示[18]。

在图1中，建立全局坐标系xf，选取后轴中心M作为泊车车辆运动学模型的基点，M点坐标为(xf)，以后轴中心M为基准点，车辆低速运动学方程为：

图1 简化的车辆运动学模型示意图

(1)

其中，xf为前轴中心横坐标；yf为前轴中心纵坐标；xr为后轴中心横坐标；yr为后轴中心纵坐标；vr为后轴中心速度；vf为前轴中心速度；θ为航向角；φ为前轴中心转角；hm为前后轴轴距；Rmin为最小转弯半径。

以后轴车速vr作为整车车速v，建立车辆运动学数学模型，如图2所示。

图2 车辆运动学数学模型示意图

2 泊车车速分析

通过对自动泊车运动过程的分析，车辆在超低速情况下对车速的测量存在局限性，高精度测量车速存在困难，建立的泊车运动学模型也不能准确反映泊车的过程。在泊车起步阶段、中间阶段、最后泊车结束阶段都存在泊车车速测试偏差现象。在泊车中间阶段，车速变化不大，这一过程主要是泊车的方向盘变化较大，引起前轮中心转角变化增大，最终影响车辆的航向角和位置改变。针对这种情况，准确估算泊车起步阶段和泊车结束阶段的车速就显得尤为重要。泊车过程测试的车速变化曲线，如图3所示。

图3 泊车过程测试的车速变化曲线

在泊车起步阶段，车速从0到稳定车速，这一过程，前期是瞬时阶段，中期是加速阶段，后期车速趋于稳定。车速变化剧烈，时间在2～5 s内。对于具体车速要充分考虑前期的瞬时速度，中期加速阶段变化，以及后期车位内调整车速。在泊车结束阶段，车速从高速变为0，这一阶段车速变化剧烈，时间在4～12 s内，且有一部分车速无法测试出来。对于具体车速的估算要充分考虑泊车过程中超声波传感器的工作过程，车速传感器的精度，以及驾驶员的反应时间等因素。整体上而言，泊车过程考虑因素较多，比较复杂，因此通过车轮转动速度的测量估算车速不能准确反映泊车运动过程中车速的变化。

3 泊车车速估算的算法原理

3.1 泊车车速测量方法

泊车车速可以通过车轮转动圈数(车轮滚动距离)及转动时间的测量，通过简单计算得到，易于实现，当车轮打滑或轮胎不断磨损时，会造成测量精度有较大误差。

泊车车速也可以通过10轴加速度传感器JY-901B(加速度计，如图4所示)测量加速度值积分后得到车速。将4个相同的10轴加速度传感器JY-901B分别安装在车辆4个车轮中心，通过对相关数据的解析，得到泊车车速，即加速度积分可得到车速。

图4 加速度传感器JY-901B实物图

综上，车轮转动速度测得的车速易受车轮侧偏和车身惯性的影响(甚至在某些情况下传感器通讯传输丢码)。车身加速度积分所得车速对于安装位置较敏感且存在误差累积。由于单一的算法会造成较大的误差，本文通过对相关文献的分析，建立了一种将测量车轮转动速度与车身加速度积分所得车速进行模糊卡尔曼滤波融合得到泊车不同阶段的车速估算值[19]。

3.2 泊车车速估算结构

首先，分别用车轮转动(取后轮两车速均值)和加速度的方法(四轮主要取后轮两加速度均值，也可适量考虑前后轴动力分配来调整四轮加速度加权值)测量车速和加速度值；然后将得到的数据利用卡尔曼滤波算法进行融合，去掉各传感器带来的测量噪声，同时考虑累积误差[20]；再根据泊车的不同状况，利用模糊算法确定速度和加速度的权重充分进行滤波处理。图5所示为泊车车速估算结构。

图5 泊车车速估算结构框图

3.3 泊车车速估算的算法设计

泊车车轮在启动和停止过程中的惯性力量、车轮的挤压形变甚至磨损均会对泊车车速产生影响，那么泊车车速的真实值、测量值、估算值之间会存在一定的函数关系。加速度也有类似关系。

νt=νg(1+α)

(2)

νc=νt+nν

(3)

ac=at+na

(4)

式中：vt为速度的真实值；vg为速度的估算值；vc为速度的测量值；nv为速度测量时的测量噪声；at为加速度的真实值；ac为加速度的测量值；na为加速度测量时的测量噪声。

在此假设nv与na为均值为0的高斯白噪声。同时，定义加速度的变化率wa和速度的变化率wv。

(5)

(6)

偏移量的变化率为

(7)

将其代入卡尔曼滤波的离散化方程为：

X(k+1)=A(k)X(k)+B(K)ω(k)

(8)

Z(k)=CX(k)+N(k)

(9)

式中：参数分别为

(10)

(11)

协方差的上一个状态方程：

(12)

卡尔曼滤波增益方程：

(13)

卡尔曼滤波估计方程：

(14)

卡尔曼滤波协方差更新方程：

(15)

泊车运动过程中车速的变化幅度较大，固定参数的卡尔曼滤波器可以过滤掉一部分测量误差，但是估算的车速离真实值有较大偏差，不能很好地反映泊车过程中的真实车速。尤其是泊车起始和结束阶段，速度变化值较快，测量的速度值与真实值偏离较大，此时需要根据加速度值的算法估算真实车速。在泊车过程中，速度测量值与真实车速比较接近，所以应加大速度值的权重进行泊车车速估算。

模糊控制器的输入量分别为估计值与测量值的绝对差值，即：

(16)

模糊控制的输出量协方差矩阵R=(Rν，Ra)，即噪声协方差矩阵R由速度和加速度测量的协方差组成。可以通过改变噪声协方差矩阵R的值改变测量速度与加速度值的权重，从而得到准确的泊车车速估算值。泊车车速v的取值区域为[0，0.5]m/s，泊车加速度a的取值区域为[0，0.6]m/s2，车速协方差Rν的取值区域为[0，0.2]，加速度协方差Ra的取值区域为[0，0.4]，输入输出隶属度函数如图6～9所示。根据泊车实际实验建立模糊规则表，如表1、2所示。

图6 输入变量泊车车速v的隶属度函数曲线

图7 输入变量泊车加速度a的隶属度函数曲线

图8 输出变量车速协方差Rν的隶属度函数曲线

图9 输出变量泊车加速度协方差Ra的隶属度函数曲线

表1 Rν的模糊规则

表2 Ra的模糊规则

表中S、M、L分别代表模糊子集的小、中、大，逻辑关系主要代表泊车过程中的运动情况。

1) 在泊车的起始过程中，车辆速度很低，加速度较大，加速度测量误差较小。由于车身的重量具有一定的惯性，车轮的车速值较小，测量误差较大。即a= L，v= S，此时应提高加速度测量值的权重，减小速度测量值的权重。

2) 在泊车的中间阶段、转动角度变化不大的过程中，车辆保持低速运动，此时车轮的加速度和车速测量误差都较小。即a=S，v=S，此时应提高泊车速度测量值的权重，减小加速度测量值的权重。

3) 在泊车的中间阶段、转动角度变化较大的过程中，车辆保持低速运动，此时车轮的加速度和车速测量误差均较大。即a=L，v=L，此时应提高泊车加速度测量值的权重。

4) 在泊车的中间阶段、转动角度变化适度的过程中，车辆保持低速运动，此时车轮的加速度和车速测量误差一般大。即a=M，v=M，此时应提高泊车速度测量值的权重。

5) 在泊车的中间阶段、换前后挡的过程中，车辆保持低速运动，此时车轮的加速度误差较大，泊车车速测量误差一般。即a=L，v=M，此时应提高泊车速度测量值的权重。

6) 在泊车停止阶段的过程中，车辆保持超低速运动，泊车车速测量误差较大。此时a=M，v=L，应增加泊车加速度测量值的权重。

最后，对模糊算法进行反模糊化求解，确定测量噪声的协方差矩阵R。

3.4 泊车车速估算的仿真

将图3中的泊车过程数据进行车速估算仿真，根据速度估算值代入式(1)中进行泊车过程的仿真。泊车车速估算的仿真过程如图10所示。泊车过程车速估算结果如图11所示。泊车实验测得前轮转角如图12所示。

图10 泊车车速估算仿真过程框图

图11 泊车过程车速估算结果

图12 泊车过程前轮转角

3.5 泊车运动simulink仿真模型

通过建模式(1)，得到泊车运动仿真时的姿态信息[21]。图13所示即为Simulink中搭建的泊车车辆运动学模型。

图13 Simulink搭建泊车车辆运动学模型框图

3.6 泊车实际坐标与仿真模型的比较分析

通过Matlab的绘图功能，得到估算车速的泊车模型、原有泊车模型以及实际泊车航向角，如图14所示。横坐标X对比，如图15所示。纵坐标Y对比，如图16所示。

图14 3种泊车运动学模型航向角

图15 3种泊车运动学模型横坐标X

图16 3种泊车运动学模型纵坐标Y

由图14可知：在泊车中期和末期，仿真曲线中原有泊车模型的航向角与实际泊车差距较大，主要由于车速测量不准确造成。原有泊车模型航向角的偏差为[-4.2°，6.5°]，最大误差率为0.39°。车速估算后的车辆航向角偏差为[-3°，0.5°]，最大误差率为0.09。车速估算使得泊车末期的航向角整体趋势更接近实际测量值，误差率降低了30%。

由图15可知：原有泊车模型与实际泊车位移横坐标偏差为[-450 mm，50 mm]，最大误差率为0.09。估算车速的泊车模型与实际泊车横坐标的最大位移偏差为153 mm，最大误差率为0.03。误差率下降了6%。

由图16可知：泊车前期和泊车中期，估算车速的泊车模型、原有泊车模型与实际泊车纵坐标吻合度较高。通过计算，前期和中期的最大Y位移偏差为[0 mm，490 mm]，最大误差率为0.05。而后期的误差率大是由于车速测量不准确引起的，原有泊车模型与实际泊车纵坐标后期的最大位移偏差为490 mm，最大误差率为0.14。估算车速的泊车模型与实际泊车纵坐标最大位移偏差为48 mm。最大误差率为0.05，降低了9%。验证了合理的车速估算可以极大地降低因车速测量不准确导致的仿真误差。

本文中所设计的模糊卡尔曼滤波能更好地处理实际泊车工况下的车速估计问题，所采用的模糊卡尔曼滤波相较普通卡尔曼滤波在估计车速上优势明显，更接近实际泊车情况。模糊卡尔曼估算车速的泊车模型、普通卡尔曼泊车模型以及实际泊车航向角如图17所示。横坐标X如图18所示。纵坐标Y如图19所示。

图17 3种泊车运动学模型航向角

图18 3种泊车运动学模型横坐标X

图19 3种泊车运动学模型纵坐标Y

经Matlab/simulink仿真和泊车实验的数据对比分析结果表明：建立的基于模糊卡尔曼滤波车速估算的泊车运动学模型与原泊车运动学模型相比在精度上提高了15%，与实际泊车情况偏差3%。

4 结论

针对自动泊车运动过程中的自身特点，提出了一种基于模糊卡尔曼滤波车速估算的泊车运动学模型，符合泊车运动过程的要求。从3种泊车运动学模型实验分析对比可得模糊卡尔曼滤波估算车速的泊车模型与实际泊车结果基本吻合，误差值在可接受范围之内。该估算车速的泊车模型符合车辆低速泊车时的运动规律要求，降低了泊车整个过程的位移和航向角计算误差，对于进一步研究泊车路径规划与控制具有重要意义。