基于Kalman滤波的车位侧方距离修正方法

杨述斌,2，蒋宗霖，刘 寒

(1.武汉工程大学 电气信息学院， 武汉 430205; 2.智能机器人湖北省重点实验室，武汉 430205)

0 引言

传统国外汽车厂商所采用的车位识别方案都基于超声波传感器或者毫米波雷达获取车位信息识别，如丰田、奥迪、奔驰、宝马等公司，利用安装于车身保险杠两端的超声波传感器进行车位的检测，但由于存在传感器本身的固有特性以及在测量过程中因波束角的跳变等问题带来的随机干扰，导致传感器不能直接得到所需要的测量真实值。

针对超声波传感器测量数据准确度不高、实时性较差的特性问题，学者们提出了不同的解决办法。如陈乔松等提出采用双探头平均值法和双探头融合数据法来降低测量误差，以此来提高超声波传感器的探测精度，虽计算简单，但因瞬时测量值绝对误差大，导致实时性差[1]。又如毕清磊通过大量实验数据得到障碍物距离与误差值之间的关系式，利用传感器误差补偿公式来进行修正误差，但公式运算复杂，大幅度降低单片机的运算速度，且传感器测量误差会随着测量距离的增大而增大，导致该方法准确性低[2]。因此，如何提高超声波测距的准确性和实时性，并有效降低由传感器带来的随机噪声干扰是自动泊车系统中车位识别技术的一个新研究趋势。

Kalman滤波就是这种能够有效降低随机噪声影响的工具。近年来，随着科学技术的日益发展，Kalman滤波与相关改进算法都能够在计算机的辅助下得以快速实现，无论是在机器人的轨迹规划还是决策领域或者是在航天航空领域等重大军工业领域都有广泛地应用[3]。总之，Kalman滤波在信号处理与控制技术方面得到了均可有效地运用，所以文章将以Kalman滤波算法为重点研究对象，阐述其在自动泊车系统中车位识别技术的应用与实现。

1 线性离散Kalman滤波原理

Kalman滤波由系统状态方程和系统观测方程两组基本方程组成。

系统状态方程是被估计状态Xk与受噪声驱动Wk-1的关系式：

Xk=Ak,k-1Xk+BUk-1+Wk-1

(1)

对Xk的观测满足线性关系，观测方程为：

Zk=HkXk+Vk

(2)

Xk是k时刻的系统状态，Uk是k时刻对系统的控制量。Ak,k-1是从k-1时刻到k时刻的状态转矩阵。Hk是观测矩阵。Wk是系统的激励噪声，Vk是观测噪声。在Kalman滤波中，要求Wk和Vk是正态分布的白色噪声。因此有：

(3)

其中，Qk和Rk分别是系统噪声Wk和观测噪声Vk的方差矩阵。

Kalman滤波信息更新由时间更新和观测更新这两个过程组成，其中时间更新过程也称为预测过程，观测更新过程也称为修正过程。

预测过程：

1)根据上一状态预测的当前系统状态：

(4)

2)对误差协方差的预测：

Pk-=Ak,k-1Pk-1Ak,k-1T+Qk

(5)

上述两个方程是系统的预测过程，式(4)根据k-1时刻的状态最优估计预测k时刻的状态估计，式(5)描述了这种预测的均方差，对预测的好坏程度做定量描述，其值也是由上一时刻的误差协方差来做预测。从时间角度来分析，即这两个式子将时间从k-1时刻推进到k时刻，于是这个过程称为Kalman滤波的预测过程。

修正阶段：

1)计算 Kalman增益：

Kk=Pk-HkT(HkPk-HkT+Rk)-1

(6)

Kalman增益的校正状态值得误差协方差的重要参数，用来权衡系统预测的传感器量测。其中Hk是观测矩阵，Rk是观测噪声序列的方差阵。

2)对系统状态预测值的修正：

(7)

3)对误差协方差的修正：

Pk= (I-KkHk)Pk-

(8)

式(6)和式(8)利用状态预测的质量优劣(Pk-)、观测信息的质量优劣(Rk)、观测与状态的关系(Hk)以及观测信息Zk修正时间上的预测，构造改进后的最优估计，描述了Kalman滤波的修正过程。得到的最优估计又会作为先验数据供下次预测使用，由此行成递归推算。其中，Kalman滤波也就是由式(4)～式(8)这个五项基本方程组成[4-7]。

2 Kalman滤波的距离修正原理

2.1 测距系统建模

超声波测距模块采样时间为25 ms，在3.75 s内采样到150组距离数据信息，对这150组数据滤波及修正，便可以稳定输出一个比较接近真实值的数值。经过多次测量，并取平均值得到小车离侧方障碍物大约为5 cm，这个距离可能会受到传感器固有特性等问题带来一些外部因素的干扰，于是将这个扰动则为过程噪声Wk，其方差矩阵为Q，大小假定为Q=0.000 1(如果不考虑过程噪声的影响，此时Q=0)。因为考虑系统中的Xk是在第k时刻采样时的距离信息是一维的，而且无控制量。由此对照式子(1)，可以得出该系统的状态方程是:

Xk=Xk-1+Wk-1

(9)

小车侧方传感器与障碍物的实时测量部分数据如表1所示。

取前20组数据作为实验输入，经过如下式(10)计算得测量误差方差为：R=0.7959。

(10)

由此得超声波第k次测的的数据不一定是准确的，因含有测量噪声Vk，所以得到系统的观测方程为：

Z(k)=X(k)+V(k)

(11)

综上所述，该系统的状态方程和观测方程为：

Xk=Ak,k-1Xk-1+Wk-1

Zk=HkXk+Vk

(12)

表1 小车与障碍物距离信息表

式中，Xk是距离信息为一维变量，则Ak,k-1=1，Hk=1，Wk-1和Vk的方差矩阵分别为Q和R[8-10]。

建好系统之后,即可用Kalman滤波来处理过程噪声Wk和测量噪声Vk。根据Kalman滤波的实质，可以知道如果要估算k时刻的实际值，就要根据k-1时刻的值来对其进行估算。

2.2 Kalman滤波的距离修正

1)假定前20组值测得第一组距离值为k-1时刻的值Lk-1，距离的真实值为S，那么该测量值的偏差是S-Lk-1，即该时刻的协方差Pk-1=(S-Lk-1)2=M。

2)在k时刻，超声波测距传感器的测量值由于传感器的固有特性，测得第二组值Lk，偏差为S-Lk。现在用于估算第kk时刻的测量值由两个测量值，分别是k-1时刻的Lk和k时刻的S-Lk，将以上两组测量值进行融合使其逼近真实值，Kalman滤波也非常适合循环迭代运算，因此也适合采用计算机程序实现，实现流程如图1所示。

图1 Kalman滤波的数据修正过程

因为Ak,k-1及Hk均为1，所以具体操作步骤如下。根据Kalman滤波的五项基本方程(4～8)，这里分5步来分析Kalman滤波的数据修正流程。

1)根据k-1时刻的值，对状态预测

2)计算协方差预测，

Pk-=Pk-1+Q(S-Lk-1)2+Q=M+QPk-

3)计算Kalman增益，

Kk=Pk-/(Pk-+R) = (M+Q)/[(M+Q) +R]

4)状态更新，由

得：

5)协方差更新，

Pk= (I-KkHk)Pk-=

{I-[(M+Q)/(M+Q+R)]*Hk}*(M+Q)

其中，Matlab开发平台下的部分核心代码如下：

% State initialization

S=zeros(1,T);S(1,1)=5.00;

% observation initialization

L=zeros(1,T);L(1,1)=3.97;

%State initialization by Kalman filtering

Skf=zeros(1,T);Skf(1,1)=L(1,1);

% covariance initialization

P=zeros(1,T);P(1,1)=1.0609;

% The measured target is one-dimensional information

for k=2:T

% X is the true distance value, which consists of the real value and the disturbance caused by the disturbance.

% Equation of state S(1,k)=F*S(1,k-1)+G*W(1,k);

% The range finder can only be measured by the sensor, the measurement information is Z, and the filter starts according to the measurement information.

% observation equation

L(1,k)=H*X(1,k)+V(1,k);

% Step 1: Status Forecast

Spre=H*Skf(1,k-1);

% Step 2: Covariance prediction

Ppre=F*P(k-1)*F'+Q;

% Step 3: Calculate the Kalman gain

K=Ppre*inv(H*Ppre*H'+R);

% calculation information

e=L(k)-H*Xpre;

% Step 4: Status Update

Skf(1,k)=Spre+K*e;

% Step 5: Covariance update

P(1,k)=(I-K*H)*Ppre;

end

其中，状态转移矩阵F=1；噪声驱动矩阵G=1；观测矩阵H=1；过程噪声方差Q=0.0001；观测噪声方差R=0.7959；仿真总步数T=150；过程噪声为W=sqrt(Q)*randn(1,T)；观测噪声为V=sqrt(R)*randn(1,T)；单位矩阵I=eye(1)；

从Kalman滤波的本质来看，Step 1和Step 2将时间从k-1时刻推进至k时刻，描述了Kalman滤波的预测过程，具体的距离修正量由时间更新的质量优劣(Pk-)、观测信息的质量优劣(R)、观测与状态的关系(H)及具体的观测信息L(k)得以确定，这些步骤都是以如何正确合理的利用观测L(k)为目的的，同时这5步也恰恰描述了Kalman滤波的修正过程。

3 距离修正仿真结果与分析

应用以上的系统的状态方程和测量方程，在Inter(R) Core(TM) CPU主频为2.3 GHz，安装内存4 GB的计算机上运行Matlab R2018a进行仿真实验，分析小车与侧方障碍物距离信息的真实值、测量值和滤波修正值的差异，同时也计算了测量偏差与滤波偏差，部分核心代码已在上文给出。

图2中，实线代表的是真实值，圆圈代表的是在随机噪声干扰下的超声波传感器测量值，而星号线则代表的是滤波修正值。由图2可见，超声波传感器的测量值准确度低，绝对误差的波动非常大，但滤波修正值逐渐地逼近真实值，且趋于稳定。标记出6组样本值，记录如表2所示。

图2 真实值、测量值与滤波值的差异图

表2 kalman滤波算法样本值实验结果

图3中，将各个采样时刻、滤波结果和真实值去做差，计算其绝对值，这个值就是偏差值，定义为：

Xdev(k)=|Xn(k)-X(k)|

(13)

同样地，将其用于Matlab的“Figure”中自带的数据标尺工具在图3中将第1次和第159次的偏差标出，结果发现其测量偏差值由1逐渐减小到0.4779，且其偏差逐渐趋于稳定值。

图3 误差分析图

表2中的6组样本值分别代表第25、 50、75、100、125、150次的滤波结果，由表2可知经过150次迭代计算后的测量值已经达4.68 cm，相对误差由0.98 cm减少到0.32 cm，可见反复迭代计算值的绝对误差逐渐降低，并将继续减小，使其测量值逼近真实值，且趋于稳定。

为了验证Kalman滤波在自动泊车系统中车位识别技术的普适性，在真实距离S为15 cm，25 cm，35 cm，45 cm，55 cm，65 cm的不同情况下，利用上述原理进行测量数据的滤波修正，并记录其结果，如表3所示。

表3 不同真实距离下的测量数据滤波修正结果

由表3数据发现，经过150次迭代计算后的测量值，其绝对误差不会随着距离增加而增加，且绝对误差可以保证在2 cm以内，实验的平均绝对误差仅1.575 cm，准确度高。

同上，运行在真实距离S为15 cm，25 cm，35 cm，45 cm，55 cm，65 cm的不同情况下，并用MATLAB的“Etime”函数获取各数据修正所需的时间，并记录如表4所示。

表4 不同真实距离下的测量数据滤波修正所需时间

由表4可计算得到这6次实验的数据修正所需平均时间为0.028 s，而且Kalman滤波计算过程为一个不断“预测—修正”的过程，在数据修正时不需要存储大量的数据，且一旦观测到新的数据，随时可以计算得滤波修正值，有非常好的实时处理性。

综上所述，该方法使用的kalman滤波算法可有效降低了随机噪声干扰，使车位识别技术中侧方障碍物距离数据得以修正，准确性高且实时性好，同时也完全符合安全泊车距离±5 cm的标准，进而使车辆泊车可达到理想效果。

4 结语

本文通过合理地设计状态方程及观测方程，采用Kalman滤波的车位侧方距离修正方法得到的距离平均修正误差小，运行时间更短，表明该算法具有更好的准确性和实时性。逐步迭代的计算使滤波修正值继续逼近真实值，可以有效应用于实际的车位识别系统中。