基于超声波与IMU融合的倒车测距算法

马强，王云，陈文淼，杨新达，武玉臣，刘晓林

(1.内燃机可靠性国家重点实验室，山东潍坊 261061；2.潍柴动力股份有限公司发动机研究院，山东潍坊 261040)

0 引言

随着经济的发展和人们对物质文化需求的增长，汽车倒车安全的要求越来越高，采用精确倒车测距技术已成为车辆发展的必由之路[1-2]。

目前倒车预警系统主要应用超声波测距报警技术，一般为区段报警，无法提供精确距离信息，需要结合驾驶员的判断力，这在一定程度上增加了倒车的难度，影响车辆的控制精度、安全性及驾驶体验[3-6]。

倒车过程中驾驶员最关心的是车辆与障碍物之间的距离，目前直接用于探测距离的传感器主要有红外线、激光雷达、毫米波雷达及超声波传感器等。红外线受环境影响较大，而激光雷达价格昂贵，2种传感器目前尚不适于大规模应用和商业化开发；毫米波雷达不适于短距测量，在应用方面也受到限制；超声波传感器具有环境适用性强、结构简单、成本低廉、短距测量可靠性高等特点，越来越受到市场的欢迎。本研究中采用超声波传感器作为距离感知的主要部件[7-11]。

任何类型的测距传感器都有内在的缺陷和不足，一个传感器无法满足系统鲁棒性的要求。目前国内外倒车精确测距应用中一般采用多个或多种类型的传感器，通过对各种类型传感器的信息进行融合实现对环境较准确的感知[12-15]。目前倒车精确测距系统多采用超声波和摄像头的融合方案[16]，但摄像头受环境影响较大，易受光线等因素制约。惯性测量单元(inertial measurement unit，IMU)不受光线影响，提供的车辆加速度、车速等信息同样可参与到超声波的测距融合中，本文中提出一种基于超声波和IMU融合的测距方案，并结合方案对具体的融合算法进行设计。

1 感知融合算法

超声波与IMU融合测距算法主要结合超声波和IMU各自的优点，使得探测到的距离信息更加可靠，精确度更高。首先，超声波传感器探测到的距离信息要经过“同类型多传感器融合算法”进行数据预处理，然后，结合IMU感知的车辆加速度及当前车速信息，利用卡尔曼滤波对障碍物的距离进行最优估计，从而实现IMU与超声波传感器数据的最终融合[17-20]。

1.1 同类型多传感器融合算法

同类型传感器信息的融合采用基于最大似然法的多传感器数据融合方法，该方法可以改善单个传感器可靠性对最终测量结果的影响[21]。设xi和xj分别表示在一次测量中第i个和第j个传感器的输出数据，有：

式中：dij为xi对xj的置信距离；dji为xj对xi的置信距离；Si和Sj的几何意义分别为概率密度曲线pi(x│xi)和pj(x│xj) 与区间(xi，xj)或(xj，xi)围成的曲边形面积，pi(x│xi)和pj(x│xj)分别为当xi和xj为均值时xi和xj的概率密度函数

式中σi、σj为均值方差。

置信距离反映了传感器输出数据之间的相互支持关系，如dij反映了传感器输出数据对传感器j输出数据的支持程度。置信距离越小，2个传感器的观测值越相近，否则2个传感器偏差就大。

由此方法可以得到n(本系统n=4)个传感器中任意2个传感器输出数据之间的置信距离，将这些数据用矩阵形式表示，即为n个传感器输出数据的置信距离矩阵

得到置信距离矩阵后需要选择一个临界值βij对置信距离进行划分，用以判断2个传感器输出数据是否互相支持。βij值根据实际情况选取，本系统选取βij=0.5， 当dij≤βij时，则认为第i个传感器的输出数据与第j个传感器的输出数据互相支持，互相支持度rij为1，否则为0,即

由此可得到一个关系矩阵

关系矩阵表示任意2个传感器输出数据之间是否支持，由此可以判断每一个传感器输出数据是否有效。这样需要临界值m，即对于一个传感器输出数据，当它跟多于或等于m个(包括本身传感器，本系统m=3)传感器的输出数据互相支持时则认为其输出数据有效。由此方法依据关系矩阵对n个传感器的输出结果进行选择，得到若干个有效数据参与融合计算，这些有效数据为最佳融合。

基于最大似然法的似然函数求导，最终得到融合结果：

式中:μ为均值;σi为均值方差。

1.2 基于卡尔曼滤波的IMU与超声波传感器数据融合算法

卡尔曼滤波器是一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法，是多传感器信息融合系统的主要手段之一，也是进行距离估计的有效方法。本系统采用距离(同类型传感器融合算法处理后的距离)、速度融合的标准卡尔曼滤波方式，这是现代最优估计——卡尔曼滤波技术最简单融合方式[22]。直接使用标准卡尔曼滤波算法对距离信息做最优估计，滤波器的状态方程和观测方程都是线性的。对于本系统，挡墙距离的状态方程可表示为：

定义pt为系统在t时刻状态预测距离，vt为t时刻的车辆速度，at为IMU计算的t时刻的车辆加速度，可得到：

本系统的状态方程推导如下：

设zt为观测的信息，本文中为超声波传感器探测的距离信息，则t时刻的系统状态到观测状态之间的关系可表示为：

式中H为观测矩阵。

测量余量协方差

最优卡尔曼增益

更新系统的估计状态为：

更新估计误差协方差

如此，通过卡尔曼滤波不断的迭代，从而估算出最优的障碍物距离值。

2 试验验证及分析

试验验证如图1所示，采用4个超声波传感器作为主要测距部件， IMU作为车辆加速度的感知部件。等间隔(4 cm)选取15个测量点，每个测量点重复测量10次取均值，由人工分别实时采集4个超声波传感器原始测量数据及融合后的距离，对比分析融合前后的数据。

图1 试验验证示意图

根据试验数据得到各超声波传感器原始测量距离及经融合算法计算后的距离与实际距离的关系，如图2所示。计算单个传感器及经融合算法后数据的随机测量方差，以数据的最大误差及方差作为评判指标，测量结果表明单传感器的测量数据较为发散(方差为7.58～10.26)且误差较大，与实际值的最大偏差为±5 cm，经融合算法处理后的数据较为集中(方差为4.66)且误差较小，最大偏差为±3 cm，经融合算法处理后的数据明显优于单个传感器的测量结果，单传感器与数据融合后测量方差对比如图3所示。

图2 单传感器与数据融合后的实际距离与测量距离 图3 单传感器与数据融合后测量方差对比

3 结论

设计的测距算法以基于最大似然法的同类传感器融合为前提，在此基础上通过卡尔曼滤波算法进一步完成了2种传感器之间的信息互补，综合利用了2种传感器的优势，最终实现了超声波与IMU的信息融合，使得融合后的距离信息具有比单一传感器更高的可靠性及精确度，结果表明该融合算法优于任意单一数据的评判方法，对于精确倒车测距技术的开发具有实际的应用价值。