基于卡尔曼滤波的超声波数据CANOE在线仿真

严达兴 罗覃月 周翔

上汽通用五菱汽车股份有限公司 广西柳州 545000

摘要：随着辅助驾驶技术的迅速普及，越来越多的汽车安装超声波传感器，用于探测周围环境。本文基于卡尔曼滤波算法，提出了一种考虑超声波传感器数据和轮速脉冲计数的滤波算法，设计了统计观测量量精度的方案，并利用CANOE编程，在某一车型上进行了在线仿真，取得了较好的结果。

关键词：超声波数据 卡尔曼滤波 CANOE仿真

0引言

车载超声波测距在障碍物距离显示和辅助泊车控制等方面具有重要的应用。受限于超声波传感器的精度、环境温度、车速以及车辆机械系统的影响，超声波数据的变化一般滞后于障碍物实际距离的变化，且存在较为明显的数据跳变。一般的解决办法是换装精度更高的超声波模块，但不利于车辆的成本控制，特别是针对低端车型。卡尔曼滤波算法的引入有助于推动驾驶辅助系统在前装市场的普及，实现企业地降本增效。

1卡尔曼滤波和CANOE软件简介

卡尔曼滤波算法非常适合于障碍物距离的动态估计。利用运动方程和前一时刻估计出的障碍物距离预测出当前时刻的障碍物距离，再综合传感器探测到的当前时刻的障碍物距离，估计出当前时刻的障碍物距离^[1]。其基本思想是贝叶斯公式和传感器测量误差符合高斯分布。

CANOE是一款强大的车载通讯总线分析软件，可以实时收发CAN总线报文，描画出信号随时间变化的曲线，其编程模块支持采用类C语言进行二次开发，实时监控、处理、发送CAN总线报文。适合初步验证嵌入式算法。

2估计障碍物距离的卡尔曼滤波算法

卡尔曼滤波的基本原理是：求出障碍物距离的预测值和测量值，假定两者符合高斯分布。两个高斯分布的乘积（也是高斯分布）即是估计出的障碍物距离满足的分布。

原理图如图2-1所示：

图2-1

超声波传感器向周围环境发射超声波束，利用发射到接收反射波的时间差和超声波传播速度，计算障碍物的距离。车轮每转过固定角度，轮速脉冲计数增加1。根据轮胎转动半径，即可计算出1个轮速脉冲代表的车辆位移量。

卡尔曼滤波的预测方程(公式2-1)和测量方程(公式2-2)^[2]，如下所示：

其中，x(t)是t时刻预测的障碍物距离，x(t-1)是t-1时刻估计的障碍物距离，△n是t-1时刻到t时刻之间轮速脉冲计数变化值，s是1个轮速脉冲代表的车辆位移量。s受到胎压，路面附着系数等因素的影响，假定其满足高斯分布，z(t)是t时刻超声波探测到的障碍物距离，假定z(t)也满足高斯分布。设：

为了以统一的标准衡量预测方程精度和测量方程精度对障碍物距离估计的影响，采用变异系数替代标准差。即：

取超声波探测到的障碍物初始距离为初始时刻估计的障碍物距离，则x(t)也满足高斯分布。

卡尔曼增益为：

t时刻的最佳障碍物估计距离为：

t时刻的障碍物距离方差为：

3先验知识获取

从公式(2-7)和(2-9)可知，超声波传感器精度 和单个轮速脉冲对应位移量精度 需要通过实验获取。

单个轮速脉冲对应位移量精度的测量采用如下方法。

图3-1

利用激光测距仪测量的数据作为障碍物与车辆之间距离的真值，通过CAN总线读取轮速脉冲计数。车辆每改变一次位置，读出激光测距仪读数变化量 和轮速脉冲计数改变值 。则单个轮速脉冲对应位移量实际值为：

由如下公式计算，其中n应该不小于50：

超声波传感器精度的测量采用如下方法。

图3-2

固定车辆与障碍物之间的距离，持续记录超声波读数一段时间，产生一个距离读数序列[ ]。利用matlab计算 、 。

由于超声波的测距精度与距离有关，需要在超声波的测距范围内不断改变车辆与障碍物之间的距离，计算出不同位置的 、 。假定 与距离(使用 表示)呈线性关系，利用最小二乘法拟合出方程：

4 CANOE编程

利用CANOE编程模块，可以实时读取CAN总线上的轮速脉冲计数和超声波距离数据，并输出卡尔曼滤波后的超声波距离数据，与原始超声波数据进行对比。算法原理如下：

1. 采用数组循环记录最近几次的轮速脉冲计数，数组大小需要考虑轮速脉冲计数和超声波距离数据的周期。

2. 每产生一个接收到轮速脉冲计数报文的事件，按照“先进先出”的原则，在数组中循环记录轮速脉冲计数。

3. 每产生一个接收到超声波距离数据报文的事件，输出原始距离数据。对原始距离数据进行卡尔曼滤波后，再次输出。

5试验过程及结果

在车辆前方放置障碍物，CANOE软件通过vector1640A连接到车辆的CAN网络，读取超声波数据和轮速脉冲计数。障碍物处固定一个激光测距仪，用于探测车辆与障碍物的距离，激光测距仪的读数作为车辆与障碍物距离的真值。如图3-1所示。

驾驶车辆持续靠近障碍物。利用CANOE的Graphics模块，描画出原始超声波数据和处理后超声波数据曲线，进行比对。激光测距仪读数曲线用python描画出来。试验结果如下。

图5-1 图5-2

序号	时间	超声波数据	滤波后的数据	激光测距仪读数	△1	△2
1	7.38366s	154	154.278402	154.3	0.3	0.02
2	8.06367	124	123.338254	122.9	1.1	0.44
3	8.26367	112	115.757633	116.0	4	0.42
4	8.94368	88	87.510042	86.8	1.2	-0.71
5	9.74368	54	53.111909	53.5	0.5	0.39

图5-3

从图5-1，5-2的对比中可以明显看出，超声波原始数据呈现阶梯状，且阶梯之间的跳跃幅度较大，达到了十几厘米，没能很好地反映出车辆与障碍物距离持续变化的现实。经过卡尔曼滤波后的数据变化较为光滑，“阶梯”基本被抹平.

图5-3选取不同时间点的数据进行对比，可以看出经过卡尔曼滤波后的数据更接近激光测距仪读数。

6结束语

卡尔曼滤波算法的计算量少，很适合嵌入式算法，保证了数据处理的实时性。一方面可以弥补单个传感器测量精度的不足，另一方面可以提高控制的精度和平顺性。

【参考文件】

[1] 乔少杰, et al. 基于卡尔曼滤波的动态轨迹预测算法. 电子学报, 2018, 46.2: 418-423.

[2] 彭丁聪. (2009). 卡尔曼滤波的基本原理及应用. 软件导刊, 8(11), 32-34.