CN112327830B - 车辆自动驾驶变道轨迹的规划方法及电子设备 - Google Patents

Abstract

本发明公开了一种车辆自动驾驶变道轨迹的规划方法，包括：将上一周期规划的第一车体坐标系下的历史规划轨迹变换至当前周期的第二车体坐标系，获得第一变换轨迹；确定第二车体坐标系下的第一参考线；确定当前周期终止时刻的预测位置点；将预测位置点和第一变换轨迹拼接并变换至第三车体坐标系获得第二变换轨迹；将第一参考线变换至第三车体坐标系获得第二参考线；将第二参考线的始末位置点变换至Frenet坐标系并进行多项式拟合，获得横向规划轨迹和纵向规划轨迹；将横向规划轨迹和纵向规划轨迹变换至第三车体坐标系，获得第三变换轨迹；将第二变换轨迹和第三变换轨迹拼接，获得目标规划轨迹；上述方法能够减小相邻周期规划轨迹的误差。

Description

车辆自动驾驶变道轨迹的规划方法及电子设备

技术领域

本申请涉及车辆自动驾驶技术领域，尤其涉及一种车辆自动驾驶变道轨迹的规划方法及电子设备。

背景技术

汽车的车体坐标系是用来描述汽车运动的特殊动坐标系；其原点与质心重合，当车辆在水平路面上处于静止状态，X轴平行于地面指向车辆前方，Z轴通过汽车质心指向上方，Y轴指向驾驶员的左侧。

目前在车辆自动驾驶中的变道轨迹规划研究中，绝大多数是基于已有高精地图的基础上，采用全局坐标系进行轨迹规划。而为了节约成本，在没有地图的情况下，目前的规划方法是通过摄像头和毫米波雷达给出车道线和目标物在车体坐标下的位置，速度等信息，再通过本车初始状态和变道完成时刻的设定状态，通过拟合得到车体坐标系下每个规划周期的变道轨迹，然后输出到车辆控制模块进行自动控制；其中，规模模块在每个规划周期中输出当前周期的规划路径，在当前周期的规划轨迹还没输出之前，控制模块使用的是上一个周期的规划轨迹。但这种规划方法存在的问题是：上一个周期规划的轨迹在规划模块将结果输出至控制模块时，已经经过了一段时间，因此当前周期、当前时刻时车辆状态、位置已经发生了改变，产生了较大的误差；若控制模块直接使用上一个周期变道轨迹，由于上一个周期变道轨迹的预测位置与当前周期车辆的实际位置之间的较大误差，致使当前周期与上一周期的两次规划轨迹之间出现明显的差异，会导致车辆出现方向盘猛然抖动的情况，造成乘客不适及安全隐患。

发明内容

本发明提供了一种车辆自动驾驶变道轨迹的规划方法及电子设备，以解决或者部分解决目前自动驾驶在相邻两个规划周期之间输出的规划轨迹不重合或存在较大的差异，造成车辆控制突变，产生安全隐患的技术问题。

为解决上述技术问题，本发明提供了一种车辆自动驾驶变道轨迹的规划方法，包括：

根据变道轨迹的规划周期T，在当前周期的起始时刻，获取上一周期规划的在第一车体坐标系下的历史规划轨迹；

将历史规划轨迹变换至当前周期起始时刻下的第二车体坐标系，获得第一变换轨迹；

确定第二车体坐标系下的第一参考线；

确定在当前周期终止时刻、第二车体坐标系下车辆的预测位置点；

将预测位置点和第一变换轨迹进行拼接，获得在第二车体坐标系下的待处理轨迹；其中，待处理轨迹中的第一个位置点为预测位置点；

将待处理轨迹变换至第三车体坐标系，获得第二变换轨迹；其中，第三车体坐标系为预测位置点的车体坐标系；

获取指定位置点；根据预测位置点和指定位置点，将第一参考线变换至第三车体坐标系，获得第二参考线；其中，第二参考线的起始位置点对应于预测位置点在第一参考线上的投影点，末尾位置点对应于指定位置点在第一参考线上的投影点；

将起始位置点和末尾位置点变换至Frenet坐标系，获得起始位置点和末尾位置点在Frenet坐标系下的坐标信息；

根据起始位置点和末尾位置点在Frenet坐标系下的坐标信息，确定在Frenet坐标系下的横向规划轨迹和纵向规划轨迹；

将横向规划轨迹和纵向规划轨迹变换至第三车体坐标系，获得第三变换轨迹；

按照第二变换轨迹在前，第三变换轨迹在后的顺序，将第二变换轨迹和第三变换轨迹进行拼接，获得当前周期的目标规划轨迹。

可选的，确定第二车体坐标系下的第一参考线，具体包括：

获取车道线性质、前车历史行驶轨迹和道路信息；

根据车道线类型、前车历史行驶轨迹、道路信息中的至少一种，确定第二车体坐标系下的第一参考线。

可选的，确定在当前周期终止时刻、第二车体坐标系下车辆的预测位置点，具体包括：

根据车辆在当前周期起始时刻的行驶速度，当前周期起始时刻的横摆角速度和当前周期内的预测横摆角，使用自行车模型确定当前周期终止时刻、第二车体坐标系下车辆的预测位置点。

可选的，根据预测位置点和指定位置点，将第一参考线变换至第三车体坐标系，获得第二参考线，具体包括：

根据设定距离步长，将第一参考线进行采样，获得第二序列点；第二序列点包括Q个采样点，Q≥2；

根据预测位置点，从第二序列点中确定出与预测位置点对应的预测位置投影点；

根据指定位置点，从第二序列点中确定出与指定位置点对应的指定位置投影点；

将预测位置投影点与指定位置投影点之间的第二序列点变换至第三车体坐标系，获得第二参考线。

可选的，将起始位置点和末尾位置点变换至Frenet坐标系，获得起始位置点和末尾位置点在Frenet坐标系下的坐标信息，具体包括：

确定起始位置点在第三车体坐标系下的几何信息，起始位置点的几何信息包括：起始位置点的坐标，起始位置点与x轴的方位角θ_x_start，起始位置点处的曲率kx_start；

确定起始位置点在第二参考线上的第一投影点的几何信息，第一投影点的几何信息包括：第一投影点的坐标，第一投影点与x轴的方位角θ_r_start，第一投影点与第二参考线上的起始投影点之间的位移l_start；第一投影点处的曲率kr_start，以及曲率kr_start对s的一阶导数kr′_start；

获取当前时刻的车辆速度v_x_start和加速度a_x_start；

根据起始位置点的几何信息和第一投影点的几何信息，确定起始位置点在Frenet坐标系下的坐标信息，包括：s,d_s,dd_s,d,d_d和dd_d；

其中，s是在Frenet坐标系下，车辆在第二参考线上的投影点在经过时间t后的位移，d_s是s对时间t的一阶导数，dd_s是s对时间t的二阶导数；d是在Frenet坐标系下，车辆在t时刻的位置与在第二参考线上的对应投影点之间的距离，d_d是d对s的一阶导数，dd_d是d对s的二阶导数。

进一步的，根据起始位置点和末尾位置点在Frenet坐标系下的坐标信息，确定在Frenet坐标系下的横向规划轨迹和纵向规划轨迹，具体包括：

在Frenet坐标系的纵向上进行s与t的5次多项式拟合，获得纵向拟合方程s＝s₀+s₁t+s₂t²+s₃t³+s₄t⁴+s₅t⁵；

根据起始位置点和末尾位置点的坐标信息，求解纵向拟合方程得到多项式系数s₀、s₁、s₂、s₃、s₄、s₅，获得纵向规划轨迹；

在Frenet坐标系的横向上进行d与s的5次多项式拟合，获得横向拟合方程d＝d₀+d₁s+d₂s²+d₃s³+d₄s⁴+d₅s⁵；

根据起始位置点和末尾位置点的坐标信息，求解横向拟合方程得到多项式系数d₀、d₁、d₂、d₃、d₄、d₅，获得横向规划轨迹。

进一步的，将横向规划轨迹和纵向规划轨迹变换至第三车体坐标系，获得第三变换轨迹，具体包括：

从横向规划轨迹和纵向规划轨迹中确定一组待转规划点；

确定待转规划点在Frenet坐标系下的坐标信息

将第二参考线变换至Frenet坐标系，获得第三参考线；

根据待转规划点在Frenet坐标系下的坐标信息，确定待转规划点在第三参考线上的第二投影点；

确定第二投影点在第三车体坐标系中的几何信息，包括：第二投影点的坐标，第二投影点与第三车体坐标系的x轴的方位角θ_r，第二投影点的曲率kr，第二投影点的曲率kr对s的一阶导数kr′；

根据待转规划点在Frenet坐标系下的坐标信息和第二投影点在第三车体坐标系中的几何信息，确定第三变换轨迹和目标变换策略；其中，第三变换轨迹包括待转规划点在第三车体坐标系下的坐标；目标变换策略包括待转规划点在第三车体坐标系下的曲率kx，与x轴的方位角θ_x，速度v_x和加速度a_x。

如上述的技术方案，历史变换轨迹包括基于上一周期起始时刻的第一车体坐标系输出的第一序列点，第一序列点中包括M个位置点，M≥3；

将预测位置点和第一变换轨迹进行拼接，获得在第二车体坐标系下的待处理轨迹，具体包括：

将预测位置点确定为待处理轨迹中的第一个位置点；

从第一变换轨迹中提取出包括连续P个位置点的子序列点，将子序列点拼接在预测位置点之后，获得待处理轨迹；其中，子序列点中的第一个位置点是从第一序列点中的第3个位置点开始，根据x坐标大于预测位置点的x坐标的规则，确定出的第一个位置点；P≥2。

可选的，在将子序列点拼接在预测位置点之后，获得待处理轨迹的过程中，还包括：

获取第一变换轨迹中的第3个点至子序列中的第P个点之间的间隔数k；

将第二变换轨迹和第三变换轨迹进行拼接，获得当前周期的目标规划轨迹，具体包括：

从第三变换轨迹中的第k+1个位置点开始，依次判断位置点的x坐标是否大于第二变换轨迹中的第P+1个位置点的x坐标；

若是，将第三变换轨迹中的对应位置点和对应位置点之后的位置点标记为待选位置点；

将所有待选位置点拼接至第二变换轨迹之后，获得目标规划轨迹。

基于前述技术方案相同的发明构思，本发明还提供了一种电子设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，处理器执行程序时实现前述技术方案中任一项规划方法的步骤。

通过本发明的一个或者多个技术方案，本发明具有以下有益效果或者优点：

本发明提供了一种车辆自动驾驶变道轨迹的规划方法：通过将上一周期的基于第一车体坐标系的历史规划轨迹通过坐标变换转换至当前周期当前时刻的第二车体坐标系，然后将变换后的历史规划轨迹与本周期的预测位置点进行轨迹拼接，得到待处理轨迹，再将待处理轨迹变换至基于本周期预测位置点的第三车体坐标系，作为目标规划轨迹的前半部分；然后根据当前周期下确定的自动驾驶参考线，在参考线上确定起始位置点，指定末尾位置点，接着将起始位置点和末尾位置点之间的第一参考线变换至第三车体坐标系获得第二参考线，将第二参考线的起始位置点和末尾位置点变换至Frenet坐标系；接着根据变换后的起始位置点和末尾位置点的坐标信息，确定出Frenet坐标系下的横纵向规划；在将横纵向规划变换至第三车体坐标系，作为目标规划轨迹的后半部分；最后将前半部分和后半部分拼接，获得完整的目标规划轨迹；

上述方案通过将上一周期基于第一车体坐标系的历史规划轨迹变换成基于本周期预测位置点的第三车体坐标系后输出，并作为目标规划轨迹的前半部分，是利用预测位置点减小相邻规划周期之间因车辆运动产生的预测位置与实际位置间的差异，从而在本周期开始时使上一周期输出的轨迹坐标与本周期输出的轨迹坐标更加接近，以减少上一周期的规划轨迹与当前周期的规划轨迹之间的误差；接着，再基于第二参考线始末位置信息，在Frenet坐标系下规划并变换至第三车体坐标系，可获得更加准确的预测轨迹；然后，再将预测轨迹作为后半部分，通过同处于第三车体坐标系的前半部分和后半部分的拼接，获得最终的目标规划轨迹，使车辆在上一个周期规划的运动趋势下能平滑的过渡到当前周期的规划轨迹，从而使车辆在两个周期之间的变化更加平滑或平顺，减小规划轨迹的误差；

之所以将当前周期当前时刻下的第一参考线变换到当前周期预测位置点的第三车体坐标系后再进行始末位置点的Frenet变换和轨迹规划，是因为第一参考线是通过车辆传感器在实时车体坐标系下采集的位置信息得到的，若不更新参考线至预测位置点的第三车体坐标系，则将出现不同车体坐标系的信息混合使用，导致数据维度不统一，产生较大误差；

故而，上述方案能够显著减小相邻规划周期输出的规划轨迹之间的差异，从而消除了车辆的控制突变以及相应的安全隐患。

上述说明仅是本发明技术方案的概述，为了能够更清楚了解本发明的技术手段，而可依照说明书的内容予以实施，并且为了让本发明的上述和其它目的、特征和优点能够更明显易懂，以下特举本发明的具体实施方式。

附图说明

通过阅读下文优选实施方式的详细描述，各种其他的优点和益处对于本领域普通技术人员将变得清楚明了。附图仅用于示出优选实施方式的目的，而并不认为是对本发明的限制。而且在整个附图中，用相同的参考符号表示相同的部件。在附图中：

图1示出了根据本发明一个实施例的车辆自动驾驶变道轨迹的规划方法的流程示意图；

图2示出了根据本发明一个实施例的Frenet坐标系的示意图。

具体实施方式

为了使本申请所属技术领域中的技术人员更清楚地理解本申请，下面结合附图，通过具体实施例对本申请技术方案作详细描述。在整个说明书中，除非另有特别说明，本文使用的术语应理解为如本领域中通常所使用的含义。因此，除非另有定义，本文使用的所有技术和科学术语具有与本发明所属领域技术人员的一般理解相同的含义。若存在矛盾，本说明书优先。除非另有特别说明，本发明中用到的各种设备等，均可通过市场购买得到或者可通过现有方法制备得到。

为了解决车辆在自动驾驶模式下的相邻两个周期下输出的规划路径存在较大差距的问题，本发明提供了一种车辆自动驾驶变道轨迹的规划方法，如图1所示，其整体思路如下：

S1：根据变道轨迹的规划周期T，在当前周期的起始时刻，获取上一周期规划的在第一车体坐标系下的历史规划轨迹；

S2：将历史规划轨迹变换至当前周期起始时刻下的第二车体坐标系，获得第一变换轨迹；

S3：确定第二车体坐标系下的第一参考线；

S4：确定在当前周期终止时刻、第二车体坐标系下车辆的预测位置点；

S5：将预测位置点和第一变换轨迹进行拼接，获得在第二车体坐标系下的待处理轨迹；其中，待处理轨迹中的第一个位置点为预测位置点；

S6：将待处理轨迹变换至第三车体坐标系，获得第二变换轨迹；其中，第三车体坐标系为预测位置点的车体坐标系；

S7：获取指定位置点；根据预测位置点和指定位置点，将第一参考线变换至第三车体坐标系，获得第二参考线；其中，第二参考线的起始位置点对应于预测位置点在第一参考线上的投影点，末尾位置点对应于指定位置点在第一参考线上的投影点；

S8：将起始位置点和末尾位置点变换至Frenet坐标系，获得起始位置点和末尾位置点在Frenet坐标系下的坐标信息；

S9：根据起始位置点和末尾位置点在Frenet坐标系下的坐标信息，确定在Frenet坐标系下的横向规划轨迹和纵向规划轨迹；

S10：将横向规划轨迹和纵向规划轨迹变换至第三车体坐标系，获得第三变换轨迹；

S11：按照第二变换轨迹在前，第三变换轨迹在后的顺序，将第二变换轨迹和第三变换轨迹进行拼接，获得当前周期的目标规划轨迹；

根据上述得到的目标规划轨迹，可对车辆的自动驾驶进行规划。

上述方案中的车体坐标系是用来描述汽车运动的特殊动坐标系；其原点与质心重合，当车辆在水平路面上处于静止状态，X轴平行于地面指向车辆前方，Z轴通过汽车质心指向上方，Y轴指向驾驶员的左侧。

Frenet坐标系的示意图如图2所示，是基于给定的参考线(记为Tref)，使用参考线的切线向量和法线向量建立的坐标系。Tref可以为任意曲线，一般情况下定义为车道的中心线。假设自动驾驶车辆在全局坐标系下的坐标为(x，y)，可以用(s，d)描述Frenet坐标系下自动驾驶车辆的坐标值。纵向位移s为从参考线的起始点到投影点F的曲线距离，横向位移d为F点与车辆位置(x，y)的距离，F点为从车辆的位置(x，y)向参考线Tref做投影，得到的投影点。

上述方案中涉及的名词解释：

第一车体坐标系：基于在上一个周期确定的预测位置点的实际车体坐标系；

第二车体坐标系：基于当前周期当前时刻的实际车体坐标系；

第三车体坐标系：基于当前周期确定的预测位置点处的车体坐标系；

第一参考线：基于当前周期当前时刻、在第二车体坐标系下的参考线；

第二参考线：将第一参考线变换至第三车体坐标系后的参考线。

需要注意的是，上述预测位置点特指车辆在一个周期后的预测位置点。

在上述方案中，规划轨迹是自动驾驶规划的行驶路径，又称之为变道轨迹。在将变道轨迹中的位置点从一个坐标系变换到另一个坐标系时，位置点的实际空间位置并没有发生变化，而是将位置点映射到不同的坐标系，相当于坐标轴旋转。例如，将某个位置点从第二车体坐标系变换至第三车体坐标系，实际上是将该位置点从以当前周期起始时刻车体位置为原点的车体坐标系，变换至以一个周期后的车体预测点位置为原点的车体坐标系；

总的来说，本方案可应用在无全局地图的情况下，通过将上一周期的基于第一车体坐标系的历史规划轨迹通过坐标变换转换至当前周期当前时刻的第二车体坐标系，然后将变换后的历史规划轨迹与本周期的预测位置点进行轨迹拼接，得到待处理轨迹，再将待处理轨迹变换至基于本周期预测位置点的第三车体坐标系，作为目标规划轨迹的前半部分；然后根据当前周期下确定的自动驾驶参考线，在参考线上确定起始位置点，指定末尾位置点，接着将起始位置点和末尾位置点之间的第一参考线变换至第三车体坐标系获得第二参考线，将第二参考线的起始位置点和末尾位置点变换至Frenet坐标系；接着根据变换后的起始位置点和末尾位置点的坐标信息，确定出Frenet坐标系下的横纵向规划；在将横纵向规划变换至第三车体坐标系，作为目标规划轨迹的后半部分；最后将前半部分和后半部分拼接，获得完整的目标规划轨迹；

之所以结合上一个周期的历史规划轨迹、当前周期的预测位置点、当前周期的预测轨迹进行变换、拼接，是因为在本周期规划结果未出的期间，控制系统使用的是上一周期的结果，由于车辆的实际运动与预测运动的差别，上一周期的规划轨迹与本周期的规划轨迹存在较多的差异部分；而将上一周期基于第一车体坐标系的历史规划轨迹变换成基于本周期预测位置点的第三车体坐标系后输出，并作为目标规划轨迹的前半部分，是利用预测位置点减小相邻规划周期之间因车辆运动产生的预测位置与实际位置间的差异，从而在本周期开始时使上一周期输出的轨迹坐标与本周期输出的轨迹坐标更加接近，以减少上一周期的规划轨迹与当前周期的规划轨迹之间的误差；

之所以将上一周期的历史规划轨迹变换到第三车体坐标系作为前半部分，再基于第二参考线始末位置信息，在Frenet坐标系下规划并变换至第三车体坐标系，可获得更加准确的预测轨迹，然后将预测轨迹作为后半部分，通过同处于第三车体坐标系的历史轨迹和预测轨迹的拼接，获得最终的目标规划轨迹，使车辆在上一个周期规划的运动趋势下能平滑的过渡到当前周期的规划轨迹，从而使车辆在两个周期之间的变化更加平滑或平顺，减小规划轨迹的误差；

之所以将当前周期当前时刻下的第一参考线变换到当前周期预测位置点的第三车体坐标系后再进行始末位置点的Frenet变换和轨迹规划，是因为第一参考线是通过车辆传感器在实时车体坐标系下采集的位置信息得到的，若不更新参考线至预测位置点的第三车体坐标系，则将出现不同车体坐标系的信息混合使用，导致数据维度不统一，产生较大误差。

在接下来在一个可选的实施例中，对上述规划方法进行详细的说明：

S1：根据变道轨迹的规划周期T，在当前周期的起始时刻，获取上一周期规划的在第一车体坐标系下的历史规划轨迹；

自动驾驶的规划周期T(或数据采样周期)的时间范围可以根据需要进行设置，如可设置一个规划周期的时间T为0.1秒。规划轨迹中包括一组不同时刻下的序列点，以当前周期即将输出的规划轨迹为例，第i序列点代表从当前周期起计，在第i周期结束时刻车辆所在的预测位置点。

S2：将历史规划轨迹变换至当前周期起始时刻下的第二车体坐标系，获得第一变换轨迹；

在本实施例中，上一周期输出的参考线和变道轨迹是在上一周期预测的本周期起始时刻车体的坐标系(第一车体坐标系)建立的，但车辆的实际位置与预测值有一定的偏差，故将上一周期输出的历史轨迹通过坐标变换，转换为当前时刻的车体坐标系(第二车体坐标系)。此处的车辆的实际位置是指上一周期的结束时刻(也就是当前周期的起始时刻)对应的实际位置。预测位置点就是车从一个周期的开始时刻经过一个周期后的位置，预测位置点的确定方法详见S4的描述。

转换方法：因为上一周期输出的变道轨迹的序列点经过坐标变换，其空间位置并没有发生变化，只是映射到了不同的坐标系，所以上一周期输出的坐标序列转换为当前时刻实际车体坐标系下的坐标可等效为将上一周期输出的序列点在上一周期起始时刻车体的实际坐标系下的坐标转换到当前时刻实际车体坐标系，因此可采用上一个周期起始和结束时刻车体的速度和横摆角速度，进行坐标变换，其公式如下：

在上式中，

分别表示在上一周期输出序列点中的第i个位置点在上一周期实际车体坐标系下的横坐标和纵坐标，

为将

转换为当前周期第二车体坐标系下的横坐标，

为将

转换为当前周期第二车体坐标系下的纵坐标。为了方便起见，在下述内容中，使用(x_n，y_n)表示规划轨迹中的序列点在各个车体坐标系下的坐标信息。

α为经过一个周期后车体的横摆角，计算方式如下：

其中，ω_n-1为上一周期的起始时刻的瞬时横摆角速度；

ω_n为本周期起始时刻的瞬时横摆角速度；

横摆角速度由车辆传感器检测得到，T为规划周期。

Δx为在上一个周期实际车体坐标系下经过时间T后车体的纵向位移或纵向坐标，具体如下：

其中，v_n-1为上一周期起始时刻的瞬时速度，v_n为本周期起始时刻的瞬时速度；v_n-1和v_n也是通过车辆传感器获得；

Δy为在上一个周期实际车体坐标系下经过时间T后车体的横向位移或横向坐标。

S3：确定第二车体坐标系下的第一参考线；

参考线是自动驾驶规划轨迹使用的路线，以向左侧变道为例，参考线可选择本车道的左侧车道线。第一参考线表示本周期开始时刻的参考线。

可选的，确定第二车体坐标系下的第一参考线，具体包括：

获取车道线性质、前车历史行驶轨迹和道路信息；

根据车道线类型、前车历史行驶轨迹、道路信息中的至少一种，确定第二车体坐标系下的第一参考线。

具体的，在摄像头检测到车道线的情况下，以车道线为参考线

每个周期起始时刻，摄像头都会监测到一段车道线，并通过摄像头的采样周期(与规划周期T不同)，采集一系列基于当前车体坐标系下的坐标点，x为摄像头采集坐标点的纵向坐标，y为横向坐标，可通过五次多项式拟合y＝a₀+a₁x+a₂x²+a₃x³+a₄x⁴+a₅x⁵输出参考线系数a₀,a₁,a₂,a₃,a₄,a₅；参考线系数可以通过代入六个采样点的坐标求出。

车道线性质包括实线、虚线、车道线完整程度等，不同性质的车道线对应不同的规划逻辑，具体如下：

若车道线为实线，可作为参考线，但表示不允许跨越，则在规划轨迹时的条件应包括车体不越过参考线即横向距离符号保持不变；

若车道线为虚线，且相邻车道与本车道行驶方向一致，可作为参考线，表示允许变道；

若车道线不完整，通过检测到的点进行5次多项式拟合后进行延长，得到参考线；

若摄像头内几乎检测不到车道线：通过记录稳定无变道动作的前车行驶历史轨迹点，转换到当前车体坐标系下进行5次多项式拟合后作为本周期参考线；

若摄像头未检测到车道线，且无前车，但能检测到道路两侧的护栏或者路沿，则采样护栏或路沿上的一系列点，进行五次多项式拟合后减去0.5m的安全距离防止离车道边缘太近，并作为参考线。

S4：确定在当前周期终止时刻、第二车体坐标系下车辆的预测位置点；

一种可选的确定预测位置点的方案如下：

根据车辆在当前周期起始时刻的行驶速度，当前周期起始时刻的横摆角速度和当前周期内的预测横摆角，使用自行车模型确定当前周期终止时刻、第二车体坐标系下车辆的预测位置点。

自行车模型是指把车体简化成一个前后只均有一个轮的模型来进行分析，主要使用横摆角和速度来进行相关的运动分析，使用Δx_和Δy_两个参数进行表征，指的是本周期实际车体坐标系下，车辆按照当前的速度v_n和横摆角速度ω_n运动一个周期T时的位移，Δx_和Δy_的计算方式如下：

Δx_为本周期在实际车体坐标系下，车辆按照当前的速度v_n和横摆角速度ω_n运动一个周期T后预测的车体的纵向位移或纵向坐标，v_n可使用本周期起始时刻瞬时速度，由车辆传感器获得；

Δy_为本周期在实际车体坐标系下，车辆按照当前的速度v_n和横摆角速度ω_n运动一个周期T后预测的车体的横向位移或横向坐标；

其中，pre_α＝ω_nT，pre_α为一个周期内预测车体的横摆角，ω_n为本周期起始时刻瞬时横摆角速度，由车辆传感器获得。

S5：将预测位置点和第一变换轨迹进行拼接，获得在第二车体坐标系下的待处理轨迹；

待处理轨迹以预测位置点作为第一个点，然后从第一变换轨迹中选择一定数量的点拼接在预测位置点之后，形成待处理轨迹。待处理轨迹中的所有点在经过S6的坐标变换后，作为S11中的目标规划轨迹的前段部分。

因此，可选的，历史变换轨迹包括基于上一周期起始时刻的第一车体坐标系输出的第一序列点，第一序列点中包括M个位置点，M≥3；

将预测位置点和第一变换轨迹进行拼接，获得在第二车体坐标系下的待处理轨迹，具体包括：

将预测位置点确定为待处理轨迹中的第一个位置点；

从第一变换轨迹中提取出包括连续P个位置点的子序列点，将子序列点拼接在预测位置点之后，获得待处理轨迹；其中，子序列点中的第一个位置点是从第一序列点中的第3个位置点开始，根据x坐标大于预测位置点的x坐标的规则，确定出的第一个位置点；P≥2。

第一序列点是历史变换轨迹中的位置点，包括按照一定的采样周期输出的一系列位置点。在将预测位置点与第一变换轨迹拼接时，是从第一序列点中提取连续P个点，并将其拼接在预测位置点之后。

以P＝3为例，裁剪拼接的原理是按照预测的车体坐标点作为最终输出的规划轨迹的第一个点；从变道轨迹(第一变换轨迹)上寻找连续的3个点作为一个过渡并作为最终输出的目标规划轨迹的第2、3、4个点；因为变道轨迹上第2个点和本周期的预测位置点在时间上对应，又为了保证取的连续的三个点是在预测点之后的，因此使用x坐标进行了比较，以变道轨迹的第3个点开始的第一个x坐标大于预测点的点作为最终输出轨迹的第2个点，然后取3个连续的点，所以就有了下面具体的选取第2、3、4个点的过程。

因为最终输出的规划轨迹是轨迹序列点，所以剪裁拼接指的是对点的选取。

S2中的(x_n，y_n)序列中的第1点与当前车体坐标系中原点对应，序列中的2个点与S4中的一周期后的预测坐标点(Δx_，Δy_)对应，并选择点(Δx_，Δy_)，记作(cut_x₀，cut_y₀)；比较S2中(x_n，y_n)序列的第3个点(x₃，y₃)与预测位置点(Δx_，Δy_)，若x₃>Δx_，则记(x₃，y₃)为第2个点，(x₄，y₄)为第3个点，(x₅，y₅)为第4个点；若x₃<Δx_，再比较x₄与Δx_，若x₄>Δx_，则记(x₄，y₄)为(cut_x₁，cut_y₁)，(x₅，y₅)为(cut_x₂，cut_y₂)，(x₆，y₆)为(cut_x₃，cut_y₃)，以此类推得到(cut_x_n，cut_y_n),n＝0，1，2，3。

可选的，可记下从点(x₃，y₃)到(cut_x₃，cut_y₃)的个数k，用于第S11作为裁剪拼接的依据。

S6：将待处理轨迹变换至第三车体坐标系，获得第二变换轨迹；其中，第三车体坐标系为预测位置点的车体坐标系；

将S5中待处理轨迹中的序列点(cut_x_n，cut_y_n),n＝0，1，2，3从第二车体坐标系下的坐标转换为S4中确定的预测位置点的第三车体坐标系下的坐标(pre_x_n，pre_y_n)，n＝0,1,2,3，作为当前周期规划轨迹的前半部分，将与S10输出的第三变换轨迹进行拼接，变换公式如下：

pre_x_n＝(cut_x_n-Δx_)cos(pre_α)+(cut_y_n-Δy_)sin(pre_α)

pre_y_n＝(cut_x_n-Δx_)sin(pre_α)+(cut_y_n-Δy_)cos(pre_α)

其中，(cut_x_n，cut_y_n)为S5中标记的第1、2、3、4个点在当前车体坐标系下的坐标，Δx_和Δy_、pre_α为S4中得到的数据,(pre_x_n，pre_y_n)为序列点(cut_x_n，cut_y_n)转换为第三车体坐标系下的对应坐标；

S7：获取指定位置点；根据预测位置点和指定位置点，将第一参考线变换至第三车体坐标系，获得第二参考线；其中，第二参考线的起始位置点对应于预测位置点在第一参考线上的投影点，末尾位置点对应于指定位置点在第一参考线上的投影点；

一种可选的根据预测位置点和指定位置点，将第一参考线变换至第三车体坐标系，获得第二参考线的方案如下：

根据设定距离步长，将第一参考线进行采样，获得第二序列点；第二序列点包括Q个采样点，Q≥2；

根据预测位置点，从第二序列点中确定出与预测位置点对应的预测位置投影点；

根据指定位置点，从第二序列点中确定出与指定位置点对应的指定位置投影点；

将预测位置投影点与指定位置投影点之间的第二序列点变换至第三车体坐标系，获得第二参考线。

具体的，车与参考线的相对位置可通过寻找车辆在车体坐标系下的参考线上的投影点，根据投影点进行描述。例如：车辆起始位置对应第一参考线的投影点为a，经过时间t后车辆的位置对应第一参考线上的投影点为b，则车辆在时间t之内的行驶距离用第一参考线a点到b点的弧长来描述，车到第一参考线的距离就用车的每一时刻到其投影点的距离来表示。

在本步骤使用到的指定位置点是通过人为设定的，在当前周期的第二车体坐标系下指定将要规划的目标轨迹的终点位置，可参照第一参考线，确定一个终点坐标。

在变换第一参考线前，需要确定预测位置点的坐标(Δx_，Δy_)在第一参考线上的投影点(r_x_start，r_y_start)，以及指定位置点在第一参考线上的投影点(r_x_over,r_y_over)。

确定上述投影点的方法为：根据Frenet坐标系对车体与其投影点的几何关系，逐个搜索得到车体在第一参考线上的投影点。几何关系是指参考线上投影点的法向量经过车体质心，采用互相垂直向量的数量级为0这一性质进行确定。以预测位置点的坐标(Δx_，Δy_)在第一参考线上的投影点(r_x_start，r_y_start)为例，一种实现方法如下：

根据S3中求解出的第一参考线的拟合方程：y＝a₀+a₁x+a₂x²+a₃x³+a₄x⁴+a₅x⁵进行x＝0.1米的采样，得到第二序列点为(r_x_n，r_y_n),n＝0,1,2,···；

求第一参考线上采样序列点的切向量A_n，可用(r_x_n+1-r_x_n，r_y_n+1-r_y_n)来近似表示点(r_x_n，r_y_n)的切向量；

构建模拟法向量B_n，用点(r_x_n，r_y_n)到预测位置点(Δx_，Δy_)的向量(Δx_-r_x_n,Δy-r_y_n)来表示；

从n＝0开始遍历整个参考线的采样序列点，选择|(A_n·B_n)/(|A_n||B_n|)|最小时的n，则此时的(r_x_n，r_y_n)即为预测位置点(Δx_，Δy_)在第一参考线上的投影点，记作(r_x_start，r_y_start)，并作为后续步骤中的起始位置点(或起始状态点)；

同理可确定指定位置点在第一参考线上的投影点(r_x_over,r_y_over)，并作为后续步骤中的末尾位置点(或末尾状态点)；

注意，上述的起始/末尾位置点(或状态点)是指路面上的某个位置点，无论坐标系怎么选取，并不改变其在空间中的位置，只是使用不同坐标系下的语言来描述此点，所以状态点是固定的，但描述其状态的坐标信息，在不同坐标系下具有不同描述。

在确定了始末位置点/状态点之后，开始将第一参考线中的第二序列点(r_x_n，r_y_n)变换至第三车体坐标系，获得对应的序列点：(pre_r_x_n，pre_r_y_n)，具体如下：

pre_r_x_n＝(r_x_n-Δx_)cos(pre_α)+(r_y_n-Δy_)sin(pre_α)

pre_r_y_n＝(r_x_n-Δx_)sin(pre_α)+(r_y_n-Δy_)cos(pre_α)

其中，将(r_x_start，r_y_start)变换后的坐标记为(pre_r_x_start，pre_r_y_start)，将(r_x_over，r_y_over)变换后的坐标记为(pre_r_x_over，pre_r_y_over)

在变换后的序列点：(pre_r_x_n，pre_r_y_n)中从保留(pre_r_x_start,pre_r_y_start)到(pre_r_x_over,pre_r_y_over)之间的序列点，其他的点删除，并从(pre_r_x_start,pre_r_y_start)开始以n＝0开始编号，得到剪裁后的在第三车体坐标系下的第二参考线的序列点(pre_r_x_n，pre_r_y_n)，n＝0,1,2,…

S8：将起始位置点和末尾位置点变换至Frenet坐标系，获得起始位置点和末尾位置点在Frenet坐标系下的坐标信息；

可选的，将起始位置点和末尾位置点变换至Frenet坐标系，获得起始位置点和末尾位置点在Frenet坐标系下的坐标信息，具体包括：

确定起始位置点在第三车体坐标系下的几何信息，起始位置点的几何信息包括：起始位置点的坐标(x_start,y_start)，起始位置点与x轴的方位角θ_x_start，起始位置点处的曲率kx_start；

确定起始位置点在第二参考线上的第一投影点的几何信息，第一投影点的几何信息包括：第一投影点的坐标(r_x_start，r_y_start)，第一投影点与x轴的方位角θ_r_start，第一投影点与第二参考线上的起始投影点的位移l_start；第一投影点处的曲率kr_start，以及曲率kr_start对s的一阶导数kr′_start；

获取当前时刻的车辆速度v_x_start和加速度a_x_start；

根据(x_start，y_start)、(r_x_start，r_y_start)、θ_x_start、θ_r_start、l_start、kx_start、kr_start、kr′_start、v_x_start和a_x_start，确定起始位置点在Frenet坐标系下的坐标信息[s_start,d_s_start,dd_s_start,d_start,d_d_start,dd_d_start]；

其中，s是在Frenet坐标系下，车辆在第二参考线上的投影点在经过时间t后的位移，d_s是s对时间t的一阶导数，dd_s是s对时间t的二阶导数；d是在Frenet坐标系下，车辆在t时刻的位置与在第二参考线上的对应投影点之间的距离，d_d是d对s的一阶导数，dd_d是d对s的二阶导数。

具体的，Frenet坐标系是用s，d_s，dd_s，d，d_d,dd_d来描述车辆在t时刻的状态。转换公式是通过车Frenet坐标系下的车的坐标与投影点的几何关系式，通过求导、替代等数学处理得出，具体算法如下：

(1)求d：

上式中的(x，y)为待转点(起始位置点或末尾位置点)的坐标，(r_x，r_y)为待转点在Frenet坐标系下的的第二参考线上的投影点，θ_r为投影点与x轴的方位角。

(2)求d_d：

d_d＝(1-kr*d)tan(θ_x-θ_r)

kr为第二参考线上的投影点的曲率；θ_x为待转点与x轴的方位角；

(3)求dd_d：

kr′为投影点曲率对s的一阶导数；kx为待转点的曲率。

(4)求s：s＝s；

(5)求d_s：

(6)求dd_s：

基于上述算法，将起始位置点(x_start,y_start)和末尾位置点(x_over,y_over)的几何信息代入算法，即可得到起始位置点和末尾位置点在Frenet坐标系下的坐标信息[s_start,d_s_start,dd_s_start,d_start,d_d_start,dd_d_start]和[s_over,d_s_over,dd_s_over,d_over,d_d_over,dd_d_over]。

S9：根据起始位置点和末尾位置点在Frenet坐标系下的坐标信息，确定在Frenet坐标系下的横向规划轨迹和纵向规划轨迹；

在Frenet坐标系的纵向上进行s与t的5次多项式拟合，获得纵向拟合方程s＝s₀+s₁t+s₂t²+s₃t³+s₄t⁴+s₅t⁵；

根据起始位置点和末尾位置点的坐标信息，求解纵向拟合方程得到多项式系数s₀、s₁、s₂、s₃、s₄、s₅，获得纵向规划轨迹；

在Frenet坐标系的横向上进行d与s的5次多项式拟合，获得横向拟合方程d＝d₀+d₁s+d₂s²+d₃s³+d₄s⁴+d₅s⁵；

根据起始位置点和末尾位置点的坐标信息，求解横向拟合方程得到多项式系数d₀、d₁、d₂、d₃、d₄、d₅，获得横向规划轨迹。

横纵向规划是根据起始位置点和末尾位置点在Frenet坐标系下的坐标信息，在Frenet坐标系下规划出的两条轨迹；其中，用s-t的多项式描述纵向规划结果，用d-s的多项式来描述横向规划结果。

(1)首先对纵向进行s-t的5次多项式拟合得到s＝s₀+s₁t+s₂t²+s₃t³+s₄t⁴+s₅t⁵，过程如下：

根据定义可知，d_s＝s₁+2s₂t+3s₃t²+4s₄t³+5s₅t⁴；dd_s＝2s₂+6s₃t+12s₄t²+20s₅t³

起始状态对应t＝0时刻，对应第6步中[s_start，d_s_start，dd_s_start，d_start，d_d_start,dd_d_start],终点时刻t＝t₀,对应第6步中[s_over，d_s_over，dd_s_over，d_over，d_d_over,dd_d_over]，故可构建如下六个方程，并解出s₀,s₁,s₂,s₃,s₄,s₅：

s₀＝s_start；

s₁＝d_s_start；

2s₂＝dd_s_start；

s₀+s₁t₀+s₂t₀ ²+s₃t₀ ³+s₄t₀ ⁴+s₅t₀ ⁵＝s_over；

s₁+2s₂t₀+3s₃t₀ ²+4s₄t₀ ³+5s₅t₀ ⁴＝d_s_over；

2s₂+6s₃t₀+12s₄t₀ ²+20s₅t₀ ³＝dd_s_over；

(2)再对横向进行d-s的五次多项式拟合得到：

d＝d₀+d₁s+d₂s²+d₃s³+d₄s⁴+d₅s⁵,方法如下：

根据定义可知：

d_s＝d₁+2d₂s+3d₃s²+4d₄s³+5d₅s⁴；dd_s＝2d₂+6d₃s+12d₄s²+20d₅s³

起始的s_start，对应[d_start，d_d_start,dd_d_start],终点的s_over对应[d_over，d_d_over,dd_d_over]，故可构建如下六个方程，解出d₀,d₁,d₂,d₃,d₄,d₅：

d₀+d₁(s_start)²+d₂(s_start)²+d₃(s_start)³+d₄(s_start)⁴+d₅(s_start)⁵＝d_start；

d₁+2d₂(s_start)+3d₃(s_start)²+4d₄(s_start)³+5d₅(s_start)⁴＝d_d_start；

2d₂+6d₃(s_start)+12d₄(s_start)²+20d₅(s_start)³＝dd_d_start；

d₀+d₁(s_over)+d₂(s_over)²+d₃(s_over)³+d₄(s_over)⁴+d₅(s_over)⁵＝d_over；

d₁+2d₂(s_over)+3d₃(s_over)²+4d₄(s_over)³+5d₅(s_over)⁴＝d_d_over；

2d₂+6d₃(s_over)+12d₄(s_over)²+20d₅(s_over)³＝dd_d_over；

S10：将横向规划轨迹和纵向规划轨迹变换至第三车体坐标系，获得第三变换轨迹；

可选的，将横向规划轨迹和纵向规划轨迹变换至第三车体坐标系，获得第三变换轨迹，具体包括：

从横向规划轨迹和纵向规划轨迹中确定一组待转规划点；

确定待转规划点在Frenet坐标系下的坐标信息；

将第二参考线变换至Frenet坐标系，获得第三参考线；

根据待转规划点在Frenet坐标系下的坐标信息，确定待转规划点在第三参考线上的第二投影点；

确定第二投影点在第三车体坐标系中的几何信息，包括：第二投影点的坐标(r_x,r_y)，第二投影点与第三车体坐标系的x轴的方位角θ_r，第二投影点的曲率kr，第二投影点的曲率对s的一阶导数kr′；

根据待转规划点在Frenet坐标系下的坐标信息、(r_x,r_y)，θ_r，kr，kr′，确定第三变换轨迹和目标变换策略；其中，第三变换轨迹包括待转规划点在第三车体坐标系下的坐标(x,y)；目标变换策略包括待转规划点在第三车体坐标系下的曲率kx，与x轴的方位角θ_x，速度v_x和加速度a_x。

由Frenet坐标系转换为预测点车体坐标系需使用待转规划点的坐标信息[s，d_s，dd_s，d，d_d,dd_d]。其中，待转规划点是根据预设采样频率，对S9中拟合得到的横向规划轨迹和纵向规划轨迹进行采样得到的序列点，例如，可以按t＝0.1秒的频率，对横向规划轨迹和纵向规划轨迹进行取样，并得到取样点在Frenet坐标系下的坐标信息[s，d_s，dd_s，d，d_d,dd_d]；在将待转规划点从Frenet坐标系变换回第三车体坐标系时，还需要使用待转规划点在Frenet坐标系下的第三参考线上的投影点，投影点在第三车体坐标系下的坐标(r_x，r_y)，投影点与x轴方位角θ_r，投影点的曲率kr，投影点曲率对s的一阶导kr’，从而求出待转规划点在第三车体坐标系下的坐标(x，y)，与x轴的方位角θ_x，曲率kx，速度v_x，加速度a_x，得到[x_n，y_n，θ_x_n，kx_n，v_x_n，a_x_n]，n＝0，1，2，3，¨。上述过程的算法具体如下：

(1)求待转规划点在第三参考线上的投影点，需在结合待转规划点在Frenet坐标系下的坐标信息和第二参考线上采样点的坐标信息进行搜寻，具体如下：

①根据S7中确定的第三车体坐标系下的第二参考线的起始位置点和末尾位置点；按x＝0.1m对第二参考线进行采样，得到参考线序列：(r_x_n，r_y_n),n＝0,1,2,……；其中x＝0m对应s₀＝0；

相邻两点的位移为：

因此：

s₁＝Δs₁；

s₂＝Δs₁+Δs₂；

,……,

s_n＝Δs₁+Δs₂+¨+Δs_n；

②将待转规划点的Frenet坐标[s，d_s，dd_s，d，d_d,dd_d]中的s与s₀进行比较，s<＝s₀，则s投影点为s₀；若s>s₀，则比较s和s₁，若s<＝s₁,则s的投影点选择s₀和s₁中更接近s的一方，若s>s₁，类比上一轮比较方式，往下逐一搜索，直到确定s_n的投影点(r_x_n，r_y_n)为止；

(2)确定投影点s_n与第三车体坐标系的x轴方位角θ_r_n，用Δs_n与x轴的夹角近似表示；投影点的曲率kr_n＝(θ_r_n-θ_r_n-1)/Δs_n；投影点曲率对s的一阶导(kr_n’＝kr_n-kr_n-1)/Δs_n；

(3)将Frenet横纵向规划转化为预测点车体坐标系下的规划轨迹，转换公式如下：

①求(x，y)：

x＝r_x-dsin(θ_r)；y＝r_y+dcos(θ_r)

②求θ_x：

d_d＝(1-kr*d)tan(θ_x-θ_r)

③求kx：

④求v_x：

⑤求a_x：

S11：按照第二变换轨迹在前，第三变换轨迹在后的顺序，将第二变换轨迹和第三变换轨迹进行拼接，获得当前周期的目标规划轨迹；

在拼接时，结合S5中确定的k：从点(x₃，y₃)到(cut_x₃，cut_y₃)的数量，给出了一种具体的拼接方法，具体如下：

在将子序列点拼接在预测位置点之后，获得待处理轨迹的过程中，还包括：

获取第一变换轨迹中的第3个点至子序列中的第P个点之间的间隔数k；

将第二变换轨迹和第三变换轨迹进行拼接，获得当前周期的目标规划轨迹，具体包括：

从第三变换轨迹中的第k+1个位置点开始，依次判断位置点的x坐标是否大于第二变换轨迹中的第P+1个位置点的x坐标；

若是，将第三变换轨迹中的对应位置点和对应位置点之后的位置点标记为待选位置点；

将所有待选位置点拼接至第二变换轨迹之后，获得目标规划轨迹。

具体的，k值表示最后想要输出的前半部分规划轨迹对应的时长为k倍的采样周期k*time_step，则从S10的输出轨迹中开始截取的点应从k点之后开始选取，为保证方向性，从S10输出轨迹中选取的第1个点应在点(pre_x₃，pre_y₃)之后，即与点(x_k+1，y_k+1)进行比较，若x_k+1>pre_x₃，则确认拼接点为(x_k+1，y_k+1)；若x_k+1<＝pre_x₃，判断x_k+2>pre_x₃，若成立则确认拼接点为(x_k+2，y_k+2)，依次进行判断，直到找到拼接点，并获取拼接点的下标：j；

将S6中获得的第二变换轨迹的序列点(pre_x_n，pre_y_n)，n＝0,1,2,3，与S10中得到的第三变换轨迹中的序列点(x_n，y_n)，n＝j，j+1，j+2，¨，进行拼接，得到目标规划轨迹的序列：(out_xn，out_yn)n＝0，1，2，3，4，¨，并满足：

(out_xn，out_yn)＝(pre_x_n，pre_y_n)，n＝0,1,2,3；

(out_x₄，out_y₄)＝(x_j，y_j)；

(out_x₅，out_y₅)＝(x_j+1，y_j+1)；

……

在车辆自动驾驶的过程中，规划模块持续实施S1～S11，不断输出每个周期下的目标规划轨迹和目标变换策略至控制模块进行自动驾驶控制。

本实施例提供的详细技术方案的有益效果为：

(1)利用轨迹预测点减小了规划周期内车辆运动造成的误差；通过在上一周期规划时将输出的坐标转换成预测点车体坐标系下的坐标输出，那么在本周期开始时，上周期输出的坐标就与本周期实际车体坐标系下的坐标更接近，从而能减少误差；

(2)自车辆在上一周期的历史规划轨迹与当前周期的预测轨迹的拼接，使得车辆变化更加平滑，误差更小；通过选取上一周期的部分规划轨迹，使得车辆在原来运动趋势下能平滑的过渡到当前规划轨迹

(3)利用坐标旋转实时更新参考线和规划的轨迹为当前坐标系状态：在没有全局地图的情况下，车辆传感器提供的是实时车体坐标系下的信息，若不更新参考线和变道轨迹到第三车体坐标系下，将其与传感器提供信息混合使用时，就出现了数据维度不统一的情况，从而造成较大误差；

(4)利用车体坐标系转Frenet坐标系，采用相对于不同性质的车道线，实现在没有地图情况下车辆的定位；此处的定位是指局部定位，而通常的定位是基于一个全局坐标下的一个坐标(x，y)对应一个位置；在Frenet坐标下，描述一个车的位置是用车到车道线的距离，车沿车道线行驶的位移来表示，实现的是局部定位；通过确定当前的车道线，可知道车辆相对与车道线的位置，从而在遇到障碍时，只需改变与车道线的横纵向关系，即可到达另一个位置；

(5)通过五次多项式拟合，使得车辆满足更多的边界条件，更加兼容；边界条件指限定次规划轨迹的数学条件，包含车辆运动学，限定的速度、横摆角速度安全时距等方面；纵向五次多项式拟合采用的是限定始末状态的纵向位移，纵向速度，纵向加速度；横向五次多项式拟合采用的是横向距离，相对纵向的斜率(横摆角进行正切变换得到)，斜率的变化率(近似横摆角速度)；传统的拟合通常使用三次多项式，三次多项式引入的曲率二阶导数不连续对下一周期的反馈控制有不利影响，而五次多项式具有三阶连续可导和曲率连续不突变的优点，能够更兼容下一周期的反馈控制。

基于前述实施例相同的发明构思，在又一个可选的实施例中，提供了一种电子设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，处理器执行程序时实现前述实施例中的规划方法的步骤。

通过本发明的一个或者多个实施例，本发明具有以下有益效果或者优点：

本发明提供了一种车辆自动驾驶变道轨迹的规划方法：通过将上一周期的基于第一车体坐标系的历史规划轨迹通过坐标变换转换至当前周期当前时刻的第二车体坐标系，然后将变换后的历史规划轨迹与本周期的预测位置点进行轨迹拼接，得到待处理轨迹，再将待处理轨迹变换至基于本周期预测位置点的第三车体坐标系，作为目标规划轨迹的前半部分；然后根据当前周期下确定的自动驾驶参考线，在参考线上确定起始位置点，指定末尾位置点，接着将起始位置点和末尾位置点之间的第一参考线变换至第三车体坐标系获得第二参考线，将第二参考线的起始位置点和末尾位置点变换至Frenet坐标系；接着根据变换后的起始位置点和末尾位置点的坐标信息进行多项式拟合，确定出Frenet坐标系下的横纵向规划；在将横纵向规划变换至第三车体坐标系，作为目标规划轨迹的后半部分；最后将前半部分和后半部分拼接，获得完整的目标规划轨迹；

上述方案通过将上一周期基于第一车体坐标系的历史规划轨迹变换成基于本周期预测位置点的第三车体坐标系后输出，并作为目标规划轨迹的前半部分，是利用预测位置点减小相邻规划周期之间因车辆运动产生的预测位置与实际位置间的差异，从而在本周期开始时使上一周期输出的轨迹坐标与本周期输出的轨迹坐标更加接近，以减少上一周期的规划轨迹与当前周期的规划轨迹之间的误差；接着，再基于第二参考线始末位置信息，在Frenet坐标系下规划并变换至第三车体坐标系，可获得更加准确的预测轨迹；然后，再将预测轨迹作为后半部分，通过同处于第三车体坐标系的前半部分和后半部分的拼接，获得最终的目标规划轨迹，使车辆在上一个周期规划的运动趋势下能平滑的过渡到当前周期的规划轨迹，从而使车辆在两个周期之间的变化更加平滑或平顺，减小规划轨迹的误差；

之所以将当前周期当前时刻下的第一参考线变换到当前周期预测位置点的第三车体坐标系后再进行始末位置点的Frenet变换和轨迹规划，是因为第一参考线是通过车辆传感器在实时车体坐标系下采集的位置信息得到的，若不更新参考线至预测位置点的第三车体坐标系，则将出现不同车体坐标系的信息混合使用，导致数据维度不统一，产生较大误差；

故而，上述方案能够显著减小相邻规划周期输出的规划轨迹之间的差异，从而消除了车辆的控制突变以及相应的安全隐患。

尽管已描述了本申请的优选实施例，但本领域内的普通技术人员一旦得知了基本创造性概念，则可对这些实施例作出另外的变更和修改。所以，所附权利要求意欲解释为包括优选实施例以及落入本申请范围的所有变更和修改。

显然，本领域的技术人员可以对本申请进行各种改动和变型而不脱离本申请的精神和范围。这样，倘若本申请的这些修改和变型属于本申请权利要求及其等同技术的范围之内，则本申请也意图包含这些改动和变型在内。

Claims (10)

1.一种车辆自动驾驶变道轨迹的规划方法，其特征在于，所述规划方法包括：

根据变道轨迹的规划周期T，在当前周期的起始时刻，获取上一周期规划的在第一车体坐标系下的历史规划轨迹；

将所述历史规划轨迹变换至当前周期起始时刻下的第二车体坐标系，获得第一变换轨迹；

确定所述第二车体坐标系下的第一参考线；

确定在当前周期终止时刻、所述第二车体坐标系下车辆的预测位置点；

将所述预测位置点和所述第一变换轨迹进行拼接，获得在所述第二车体坐标系下的待处理轨迹；其中，所述待处理轨迹中的第一个位置点为所述预测位置点；

将所述待处理轨迹变换至第三车体坐标系，获得第二变换轨迹；其中，所述第三车体坐标系为所述预测位置点的车体坐标系；

获取指定位置点；根据所述预测位置点和所述指定位置点，将所述第一参考线变换至第三车体坐标系，获得第二参考线；其中，所述第二参考线的起始位置点对应于所述预测位置点在所述第一参考线上的投影点，末尾位置点对应于所述指定位置点在所述第一参考线上的投影点；

将所述起始位置点和所述末尾位置点变换至Frenet坐标系，获得所述起始位置点和所述末尾位置点在所述Frenet坐标系下的坐标信息；

根据所述起始位置点和所述末尾位置点在所述Frenet坐标系下的坐标信息，确定在所述Frenet坐标系下的横向规划轨迹和纵向规划轨迹；

将所述横向规划轨迹和所述纵向规划轨迹变换至所述第三车体坐标系，获得第三变换轨迹；

按照所述第二变换轨迹在前，第三变换轨迹在后的顺序，将所述第二变换轨迹和所述第三变换轨迹进行拼接，获得当前周期的目标规划轨迹。

2.如权利要求1所述的规划方法，其特征在于，所述确定所述第二车体坐标系下的第一参考线，具体包括：

获取车道线性质、前车历史行驶轨迹和道路信息；所述车道线性质包括实线和虚线；

根据所述车道线性质、所述前车历史行驶轨迹、所述道路信息中的至少一种，确定所述第二车体坐标系下的第一参考线。

3.如权利要求1所述的规划方法，其特征在于，所述确定在当前周期终止时刻、所述第二车体坐标系下车辆的预测位置点，具体包括：

根据车辆在当前周期起始时刻的行驶速度，当前周期起始时刻的横摆角速度和当前周期内的预测横摆角，使用自行车模型确定所述当前周期终止时刻、所述第二车体坐标系下车辆的预测位置点。

4.如权利要求1所述的规划方法，其特征在于，所述根据所述预测位置点和所述指定位置点，将所述第一参考线变换至第三车体坐标系，获得第二参考线，具体包括：

根据设定距离步长，将所述第一参考线进行采样，获得第二序列点；所述第二序列点包括Q个采样点，Q≥2；

根据所述预测位置点，从所述第二序列点中确定出与所述预测位置点对应的预测位置投影点；

根据所述指定位置点，从所述第二序列点中确定出与所述指定位置点对应的指定位置投影点；

将所述预测位置投影点与所述指定位置投影点之间的第二序列点变换至所述第三车体坐标系，获得所述第二参考线。

5.如权利要求1所述的规划方法，其特征在于，所述将所述起始位置点和所述末尾位置点变换至Frenet坐标系，获得所述起始位置点和所述末尾位置点在所述Frenet坐标系下的坐标信息，具体包括：

确定所述起始位置点在所述第三车体坐标系下的几何信息，所述起始位置点的几何信息包括：所述起始位置点的坐标，所述起始位置点与x轴的方位角θ_x_start，所述起始位置点处的曲率kx_start；

确定所述起始位置点在所述第二参考线上的第一投影点的几何信息，所述第一投影点的几何信息包括：所述第一投影点的坐标，所述第一投影点与x轴的方位角θ_r_start，所述第一投影点与所述第二参考线上的起始投影点之间的位移l_start；所述第一投影点处的曲率kr_start，以及所述曲率kr_start对s的一阶导数kr′_start；

获取当前时刻的车辆速度v_x_start和加速度a_x_start；

根据所述起始位置点的几何信息和所述第一投影点的几何信息，确定所述起始位置点在所述Frenet坐标系下的坐标信息，包括：s,d_s,dd_s,d,d_d和dd_d；

其中，s是在所述Frenet坐标系下，车辆在所述第二参考线上的投影点在经过时间t后的位移，d_s是所述s对所述时间t的一阶导数，dd_s是所述s对所述时间t的二阶导数；d是在所述Frenet坐标系下，车辆在t时刻的位置与在所述第二参考线上的对应投影点之间的距离，d_d是所述d对所述s的一阶导数，dd_d是所述d对所述s的二阶导数。

6.如权利要求5所述的规划方法，其特征在于，所述根据所述起始位置点和所述末尾位置点在所述Frenet坐标系下的坐标信息，确定在所述Frenet坐标系下的横向规划轨迹和纵向规划轨迹，具体包括：

在所述Frenet坐标系的纵向上进行所述s与t的5次多项式拟合，获得纵向拟合方程s＝s₀+s₁t+s₂t²+s₃t³+s₄t⁴+s₅t⁵；

根据所述起始位置点和所述末尾位置点的坐标信息，求解所述纵向拟合方程得到多项式系数s₀、s₁、s₂、s₃、s₄、s₅，获得所述纵向规划轨迹；

在所述Frenet坐标系的横向上进行所述d与s的5次多项式拟合，获得横向拟合方程d＝d₀+d₁s+d₂s²+d₃s³+d₄s⁴+d₅s⁵；

根据所述起始位置点和所述末尾位置点的坐标信息，求解所述横向拟合方程得到多项式系数d₀、d₁、d₂、d₃、d₄、d₅，获得所述横向规划轨迹。

7.如权利要求6所述的规划方法，其特征在于，所述将所述横向规划轨迹和所述纵向规划轨迹变换至所述第三车体坐标系，获得第三变换轨迹，具体包括：

从所述横向规划轨迹和所述纵向规划轨迹中确定一组待转规划点；

确定所述待转规划点在Frenet坐标系下的坐标信息

将所述第二参考线变换至所述Frenet坐标系，获得第三参考线；

根据所述待转规划点在所述Frenet坐标系下的坐标信息，确定所述待转规划点在所述第三参考线上的第二投影点；

确定所述第二投影点在所述第三车体坐标系中的几何信息，包括：所述第二投影点的坐标，所述第二投影点与所述第三车体坐标系的x轴的方位角θ_r，所述第二投影点的曲率kr，所述第二投影点的曲率kr对s的一阶导数kr′；

根据所述待转规划点在Frenet坐标系下的坐标信息和所述第二投影点在所述第三车体坐标系中的几何信息，确定所述第三变换轨迹和目标变换策略；其中，所述第三变换轨迹包括所述待转规划点在所述第三车体坐标系下的坐标；所述目标变换策略包括所述待转规划点在所述第三车体坐标系下的曲率kx，与x轴的方位角θ_x，速度v_x和加速度a_x。

8.如权利要求1所述的规划方法，其特征在于，所述历史变换轨迹包括基于上一周期起始时刻的第一车体坐标系输出的第一序列点，所述第一序列点中包括M个位置点，M≥3；

所述将所述预测位置点和所述第一变换轨迹进行拼接，获得在所述第二车体坐标系下的待处理轨迹，具体包括：

将所述预测位置点确定为所述待处理轨迹中的第一个位置点；

从所述第一变换轨迹中提取出包括连续P个位置点的子序列点，将所述子序列点拼接在所述预测位置点之后，获得所述待处理轨迹；其中，所述子序列点中的第一个位置点是从所述第一序列点中的第3个位置点开始，根据x坐标大于所述预测位置点的x坐标的规则，确定出的第一个位置点；P≥2。

9.如权利要求8所述的规划方法，其特征在于，在所述将所述子序列点拼接在所述预测位置点之后，获得所述待处理轨迹的过程中，还包括：

获取所述第一变换轨迹中的第3个点至所述子序列中的第P个点之间的间隔数k；

所述将所述第二变换轨迹和所述第三变换轨迹进行拼接，获得当前周期的目标规划轨迹，具体包括：

从所述第三变换轨迹中的第k+1个位置点开始，依次判断位置点的x坐标是否大于所述第二变换轨迹中的第P+1个位置点的x坐标；

若是，将所述第三变换轨迹中的对应位置点和对应位置点之后的位置点标记为待选位置点；

将所有待选位置点拼接至所述第二变换轨迹之后，获得所述目标规划轨迹。

10.一种电子设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，其特征在于，所述处理器执行所述程序时实现权利要求1-9任一项所述规划方法的步骤。

Images