基于扩展卡尔曼滤波的无人车避障算法研究

江楠　徐健

关键词：扩展卡尔曼；避障算法；A*；自动驾驶

中图分类号：TP391 文献标识码：A

文章编号：1009-3044（2023）08-0016-03

0 引言

随着科学技术的快速进步，出现了越来越多的智能化设备方便人们的生活，其中自动驾驶技术便是当下的热点研究问题之一，所有主流车企都在最新的车型上尝试加入更加智能的自动驾驶算法，并成为品牌的主要卖点之一。自动驾驶技术包含了大量技术，包括对于复杂环境的全方位感知、车辆定位与导航、线路规划等，其中避障算法是自动驾驶的核心技术之一。优秀的避障算法不仅能够躲避行驶路线上的静态障碍物，还能够对动态的障碍物进行提前预判，根据预判结果选择合适的规避方法，从而防止碰撞的发生。

本文提出了一种基于拓展卡尔曼滤波的无人车避障算法，当无人车的相机检测到前方出现动态障碍物时，通过拓展卡尔曼滤波算法对障碍物的运动轨迹进行预测，从而指导无人车规避障碍。

1 卡尔曼滤波及其变种

1.1 相关工作

卡尔曼滤波（Kalman Filter） 也被称为线性最小均方差估计（Linear Least-Mean-Squares Estimator） ，最初由斯坦利施密特和卡尔曼提出[1]，该算法能够在线性高斯随机系统中，将系统的输入输出使用线性状态方程进行运算，并测量在随机噪声干扰下的输出，其中随机噪声一般假设满足高斯分布，并根据输出对系统进行最优估計。

由于卡尔曼滤波器计算量小、鲁棒性强等特点，科研学者针对卡尔曼滤波只能处理线性方程等限制，研究了许多衍生算法。田亚蕾等[2]针对一般目标跟踪算法在遮挡场景下的退化甚至失效的问题，将核相关滤波算法与卡尔曼滤波预测相结合实现模型的自适应抗遮挡目标跟踪；周发华[3]利用拍摄道路图像的连续帧中，车道线发展的连续性提出了一种改进的卡尔曼滤波车道识别方法；易鑫等[4]针对车辆无法确定状态方程中噪声分布的问题，将遗传算法与扩展卡尔曼滤波相结合构建车辆的状态观测器。卡尔曼滤波的变种算法还能够应用于物体的位姿估计与状态修正等场景[5-7]。

1.2 卡尔曼滤波

假设需要预测的系统为高斯线性系统，在线性高斯系统中，运动方程f （x，u）和观测方程h（x）都为线性方程，并且运动噪声和观测噪声都满足高斯分布。建立对应的运动方程和观测方程：

EKF的缺陷主要在于，在一阶泰勒展开时可能无法很好地对运动方程和观测方程进行很好的近似，并且由于马尔可夫性，EKF只是对前一时刻的状态对当前进行最优无偏估计，所以容易出现累积误差，并且算法本身没有异常值检测机制，导致估计失效。

2 基于卡尔曼滤波的无人车避障

在检测到视野中的动态障碍物后，在无人车接近动态障碍物的过程中，可以通过统计一段时间内该障碍物在图像中的状态变化，并建立合适的运动方程与状态观测方程，之后通过扩展卡尔曼滤波算法结合动态障碍物当前的状态进行状态预测，其中预测时间随着障碍物距离无人车的距离缩短而逐渐减小，动态障碍物的预测位置为无人车需要避让的位置。

由于无人车和障碍物都在动态地运动，所以无人车需要提前做出动作对障碍物进行躲避，所以需要通过局部路径规划算法实现无人车的避障。最早的局部路径规划算法是由Khatib在1986年提出的人工势场法[8]，类似于磁场，相对于车辆障碍物会产生排斥力，而目标点会产生吸引力，在引力场的作用下沿着势场梯度下降方向行驶实现障碍物躲避。当存在较多障碍物时，由于障碍物排斥力叠加，抵消目标点的吸引力从而导致无解，Fan等[9]在人工势场法中加入距离修正因子和正则六边形引导法，从而获得局部最优解。除此以外，其他学者[10-11]也对人工势场法避障针对不同应用做出了改进。

除了人工势场法，图搜索算法也是局部路径规划的主流算法之一。图搜索算法需要先将地图进行栅格化生成栅格地图，并将障碍物通过占用栅格表示，每一个可通行栅格都有通行代价，通过不断迭代搜索代价最小的栅格路径进行障碍物躲避。常见的图搜索算法包括Dijkstra 算法和A* 算法及其衍生算法[12-13]。

上述算法主要解决的是静态障碍物，针对动态障碍物，我们提出了如图2所示的一种多层栅格地图A*算法。扩展卡尔曼滤波根据动态障碍物的当前状态对未来运动轨迹做出预测，但由于障碍物可能会出现突然加速或减速情况，导致最终的预测发生一定偏差。多层栅格地图A*算法分为障碍物层、预测轨迹层和膨胀层，其中障碍物层会在栅格地图中对当前障碍物的位置进行标注，通过拓展卡尔曼滤波得到的障碍物预测轨迹输入至预测轨迹层，最后根据无人车的尺寸计算膨胀层的膨胀因子，对障碍物层和预测轨迹层进行膨胀，通行代价随着与障碍物距离增加而减小。最终使用A*算法求解多层代价栅格地图，获得最优路径。

基于扩展卡尔曼滤波的无人车避障方法的整体流程图如图3所示。无人车在感知到存在动态障碍物时，通过建立状态方程和观测方程，结合扩展卡尔曼滤波算法对目标位置进行预测，在更新卡尔曼增益的过程中不断对预测进行修正，将获得的预测轨迹输入至多层栅格地图的预测轨迹层，并添加障碍物层和膨胀层后使用A*算法进行求解最优避障路径。

3 实验与分析

为了验证算法的有效性，本文针对不同运动模型的障碍物以及不同数量障碍物进行仿真实验，并与其他算法进行比较。仿真环境的栅格地图大小为长200栅格、宽30栅格的矩形，沿途放置静态障碍物来约束行驶路线，动态障碍物大小为长度不超过3栅格的随机形状。

常见的障碍物运动模型分为恒定速度模型（Con⁃stant Velocity， CV） 和恒定加速度模型（Constant Acce⁃leration， CA） ，其中恒定速度模型假设障碍物在单位内以恒定速度移动，恒定加速度模型在单位时间内以恒定加速度运行。本文首先对单个动态障碍物的避障进行了实验，实验结果如表1所示。由于A*算法和人工势场算法不存在轨迹预测功能，只能根据动态障碍物的当前位置进行实时更新躲避路线，但是由于躲避路线可能与障碍物的运动轨迹重合，最终导致发生碰撞，而加入卡尔曼滤波预测的算法都能够很好地在单一障碍物的情况下进行躲避。

为了进一步验证本文算法的优异性能，在仿真中随机添加五个动态障碍物，并对比不同算法下的避障性能。实验结果如表2所示，在多个移动障碍物的干扰下，A*算法和人工势场法由于没有预测功能所以总会碰撞某个障碍物，人工势场法在加入卡尔曼滤波后，由于恒定速度模型中由于速度恒定，线性卡尔曼滤波也能够进行很好的预测，所以能够很好地躲避多个动态障碍物。但是由于恒定加速度模型中速度会发生变化，需要对预测模块进行非线性处理，同时人工势场法无法获得局部最优解，导致了碰撞发生。而本文提出的算法能够很好地解决多个运动模型并实现多个动态障碍物的避障。

4 结束语

性能优秀避障算法对自动驾驶的安全至关重要，本文提出了一种基于扩展卡尔曼滤波的无人车避障算法，针对多种运动模型的动态障碍物，以及多个动态障碍物干扰的复杂环境中都能够很好的运行。基于扩展卡尔曼滤波算法构建非线性运动方程与观测方程，对动态障碍物的运动轨迹进行预测，并输入到基于A*算法的多层栅格代价地图，在减少计算量的同时，额外的预测轨迹层能够很好的处理多障碍物情况下产生的干扰问题。