无人驾驶车辆分段拟合算法

针对路网文件提供的参考路径通常是一系列不包含曲率信息的离散路点序列这一情况，前期的研究已经证明忽略道路曲率或将其视为定值的跟踪方法会产生较大的跟踪误差，并且导致系统振荡，甚至影响车辆操控的稳定性。因此，多项式曲线、样条曲线、螺旋线和贝塞尔曲线等参数化的路径拟合方法被广泛地用于参考道路曲率信息的提取。但这些参数化的曲线拟合方法面临着如何确定曲线阶次的难题。一方面，选择较高的曲线阶次虽然能够实现较好的拟合精度，但是增加了求解的难度和计算量，甚至会造成过拟合现象；另一方面，选择较低的曲线阶次虽然能够降低计算量，但是在拟合复杂路径时容易产生较大的拟合误差。在这种情况下，为了降低拟合误差，通常将参考路径分成多段进行拟合，但分段过多也会增加拟合的计算量。因此，需要研究一种自适应分段的参考路径拟合方法。

由于贝塞尔曲线具备控制简便、描述能力强和易于生成复杂的平滑曲线等优势，常被用来进行参考路径的平滑。贝塞尔曲线可以表示为

其中，m为贝塞尔曲线阶次，q(τi)为参数τi处的插值点，Pk为第k个控制点。通过对参数τi在[0，1]内取值，可以在第一个控制点P0和最后一个控制点Pm之间生成任意个插值点。常用的是三次贝塞尔曲线，此时m=3，其表达式为

为保证两段曲线在连接处平滑(在连接点处切矢量方向相同、大小不等，即G1连续)，需要使拟合出来的相邻两段曲线在接点处的3个控制点共线，如图4.5所示。图4.5以三次贝塞尔曲线为例，Qk、Pk(k=0，1，2，3)分别为前、后两段曲线的控制点，其中，P0、P3为端点，P1、P2为中间点。曲线在连接点处G1连续要求：

其中，Gb为方向矢量，sb为待定的标量参数。

图4.5　两段三次贝塞尔曲线的G1连续

由于参考路径的曲线拟合不要求拟合曲线严格地通过各个离散路点，因此选择最小二乘法拟合各段参考路径的中间点。设pi为参考路径给定的离散路点，则拟合残差的平方和可以表示为(https://www.xing528.com)

其中，n为一段参考路径所包含的离散路点的个数。

通过求解∂S/∂P1=0和∂S/∂P2=0，可以得到三次贝塞尔曲线的中间控制点P1和P2，表示为

基于三次贝塞尔曲线的自适应参考路径拟合流程如图4.6所示。

图4.6　自适应的参考路径曲线拟合流程

在路径拟合的初始化阶段，将参考路径的起止点分别作为三次贝塞尔曲线的第一个和最后一个控制点。在每次曲线拟合的迭代过程中，首先根据式(4.29)和式(4.30)求解各段贝塞尔曲线中间控制点的位置。然后根据式(4.25)可以得到与原路径点对应的插值点，并计算插值点与原路径点之间的残差Er。若贝塞尔曲线中间插值点处的残差大于人为设定的阈值εr，即Er≥εr，则选择在残差最大的中间点对应的路径点处将参考路径分段。这个过程不断重复，直到拟合得到的各段曲线中间点处的残差满足拟合精度的要求。