基于三次B样条曲线拟合的主车轨迹预测算法的研究

邓琬云、曲延羽、杨子钰、林智桂、廖尉华

（上汽通用五菱汽车股份有限公司，柳州 545007）

0 引言

自适应巡航控制功能（Adaptive Cruise Control，简称ACC）是利用摄像头、雷达等传感器识别周围环境，从前方众多目标中选择一个有效目标，作为本车需要跟随的目标。然后系统通过控制驱动和制动系统调整车速，使本车和有效目标之间保持一定的安全距离，从而达到提升车辆主动安全能力和舒适性的目的[1]。

ACC 功能的系统框架如图1所示，其中,选择有效目标模块、速度控制模块及距离控制模块皆由选择有效目标模块决定。当选择有效目标模块检测到前方探测范围内不存在有效目标时，则按照驾驶员设定的巡航速度进行定速控制；当选择有效目标模块检测到前方存在有效目标时，则按照驾驶员设定的安全距离进行跟随控制。通常选择有效目标的策略是预测主车行驶轨迹，在预测轨迹上选择较近目标作为前方有效目标。所以，主车轨迹预测算法是ACC 功能中识别前方有效目标的重要环节。

图1 ACC 功能的系统框架

1 主车轨迹预测算法介绍

当前常用的主车轨迹预测算法包括定曲率识别算法[2]、基于前方目标拟合车道线算法[3-4]和基于前方目标和主车轨迹分两段预测主车行驶轨迹算法[5]等。定曲率识别算法计算简单，它将当前车头所指方向作为主车预期行驶轨迹，此方法导致主车行驶在弯道时会存在严重的误差。基于前方目标拟合车道线算法是将前方不同目标的运动轨迹进行融合得到主车预期行驶轨迹，但在交叉路口、车辆换道等复杂工况下，不能体现出很好的适应性。用两段回旋线预测主车行驶轨迹的算法是将主车预期轨迹分为两段，近端由主车行驶轨迹的历史状态拟合得出，远端由前方车辆行驶轨迹的历史状态拟合得出，该方法的预测准确性较高，但由于要充分保证分段点处的曲率一致性，分段点较难选取。

上述算法普遍使用主车行驶轨迹和目标轨迹对主车预期行驶轨迹进行拟合，但忽略了在行驶过程中车道信息对预期轨迹的影响。因此，本文基于分两段预测主车行驶轨迹的算法，提出了采用三次B 样条曲线对主车预期行驶轨迹进行拟合的方法，以达到提高运算效率、提升ACC 性能的目的。

2 基于曲线拟合的主车轨迹预测算法

本文提出的主车轨迹预测算法模型架构如图2所示。将摄像头、雷达和底盘读取的信号分别用于计算车道中心线、识别前方目标运动轨迹及拟合主车行驶轨迹，在轨迹离散模块中将计算出的路径和轨迹进行离散化处理，在轨迹融合模块中引入距离权重的概念对3 条轨迹线进行融合处理，最后用三次B 样条曲线拟合主车预期行驶轨迹。

图2 主车轨迹预测算法模型架构

图3描述了轨迹融合模块中进行融合操作的3 种情况，即根据轨迹和路径的有效性，选择有效的轨迹或路径进行融合。为了保证主车行驶轨迹和本车道或者前方目标运动轨迹的方向一致，需先判断所选择的有效轨迹或路径是否皆同号，如果同号则对轨迹进行融合，否则不进行融合。为了保证预期行驶轨迹可以包含所有潜在有效目标，预期行驶轨迹的长度应为主车到有效目标的纵向距离。但在有效目标不存在的情况下，选择用车道中心线的长度作为预期行驶轨迹的长度，保证预期行驶轨迹参数的完整性。

在融合过程中，将同一时间戳上依据图3描述的3 种融合情况，选取轨迹线的离散点作为融合对象。使用2 条有效轨迹进行融合的公式如下：

图3 轨迹融合的三种情况

式中 k——步长

W——权重，权重的大小为预期轨迹长度的倒数

（x1，y1）、（x2，y2）——同一时间戳上离散点的坐标

（x，y）——融合点的坐标

当使用主车行驶轨迹、前方目标运动轨迹和车道中心线三者进行融合时，应优先保证主车在本车道上行驶，故使用车道中心线和主车行驶轨迹的离散点代入式（1）进行计算。然后将融合之后的点（x，y）作为（x1，y1），将前方目标运动轨迹的点（x2，y2）代入式（2）进行计算，即可得到最终规划轨迹上的点（x，y）。

对于规划轨迹上的点（x，y）需要使用曲线进行拟合。三次B 样条曲线是一种参数曲线，它由多段贝塞尔曲线组成，并在交点处具有连续性，拥有精度高、数值稳定性好等优点，常常作为数据平滑拟合的方法应用于路径规划中。故本文选择使用三次B样条曲线对主车预期行驶轨迹进行拟合，可得到曲率连续、满足车辆行驶要求的主车预期行驶轨迹。

3 MATLAB 算法仿真分析

在Matlab 软件的Simulink 环境下，基于三次B 样条曲线对主车预期行驶轨迹进行仿真研究。主车的预瞄距离应与车速和参考轨迹的形状特征有关，所以要依据驾驶员驾驶行为统计数据进行分析归纳，形成驾驶行为预瞄距离统计表。在该方法的实际应用中，使用插值法对预瞄距离进行计算。

3.1 场景一：主车前方无目标车辆且存在车道线时

场景一模拟的是蓝车行驶在车道线清晰且前方没有目标车辆的情况（图4a）。从图4b 中可以看到，拟合的轨迹从主车当前位置出发，远端靠近车道中心线，满足一般驾驶员将远方车道中心作为预瞄点的驾驶习惯。图4c 得到主车预期行驶轨迹的曲率与车道中心线的曲率偏差较小，故拟合的轨迹形状特征与车道中心线一致。从图4d 可知，主车与车道中心线的横向偏差逐渐减小，说明主车逐渐靠近车道中心。

图4 使用车道中心线和主车历史轨迹预测主车预期行驶轨迹

3.2 场景二：主车前方有目标车辆且不存在车道线时

场景二模拟的是蓝车行驶在不存在车道线，且前方绿车为目标车辆的情况（图5a）。从图5b 中可以看出，拟合的轨迹从主车当前位置出发，远端靠近前方目标，满足一般驾驶员的驾驶习惯。图5c 得到主车预期行驶轨迹的曲率与前方目标运动轨迹的曲率偏差较小，故拟合的轨迹形状特征与前方目标运动轨迹一致。从图5d 可知，主车与前方目标运动轨迹的横向偏差逐渐减小，说明主车逐渐靠近前方目标车辆。

图5 使用前方目标运动轨迹和主车历史轨迹预测主车预期行驶轨迹

3.3 仿真场景三：主车前方有目标车辆且存在车道线时

场景三模拟的是蓝车行驶在车道线清晰，且前方绿车为目标车辆的情况（图6a）。从图6b 中可以看出拟合的轨迹从主车当前位置出发，将前方目标运动轨迹作为参考轨迹，远端靠近车道中心线，满足一般驾驶员的驾驶习惯。图6c 得到主车预期行驶轨迹的曲率与车道中心线及前方目标运动轨迹曲率的变化趋势相近，故拟合的轨迹形状特征与已知轨迹一致。图6d 可知，主车与参考轨迹横向偏差的最大值小于国家标准车道宽度3 m，故主车仍在本车道上行驶，满足行驶轨迹的安全性的要求。

图6 使用车道中心线、前方目标运动轨迹和主车历史轨迹预测主车预期行驶轨迹

3.4 总结与分析

上述仿真实验结果表明，本文提出的方法对于仅存在车道线或前方目标的场景，仍可使用三次B 样条曲线对主车预期的行驶轨迹进行预测。预测的行驶轨迹曲率变化平滑，且其最大值小于车道中心线或前方目标运动轨迹的最大曲率值，主车预期行驶轨迹与参考轨迹的横向偏差值逐渐减小。预测轨迹的近端以主车历史轨迹为基准，远端以车道中心线或前方目标运动轨迹为基准，符合一般驾驶员的驾驶习惯。

4 结束语

针对分两段预测主车行驶轨迹算法的局限性，本文提出了一种基于三次B 样条曲线对主车预期行驶轨迹进行拟合的方法，并将其在MATLAB 软件的Simulink 中予以实现。通过仿真实验证明此方法具有可行性，应用场景广泛，在不同情况下皆可对主车预期轨迹进行拟合。并且，拟合出的轨迹具有良好的连续性和可控性，计算过程简单，满足车辆对实时性的要求。