CN115140096A - 一种基于样条曲线与多项式曲线的自动驾驶轨迹规划方法 - Google Patents

Abstract

本发明公开了一种基于样条曲线与多项式曲线的自动驾驶轨迹规划方法，涉及自动驾驶技术领域，包括全局路径的生成与处理、环境感知与数据融合处理、局部参考路径的提取与处理、局部轨迹规划、最优轨迹采样与轨迹输出等。本发明能够在多车辆、多动静态障碍物的复杂场景下，实现车辆平稳、最佳的行驶效果。

Description

一种基于样条曲线与多项式曲线的自动驾驶轨迹规划方法

技术领域

本发明属于无人驾驶汽车技术领域，具体涉及一种基于样条曲线与多项式曲线的自动驾驶轨迹规划方法。

背景技术

随着科技的不断发展，传感器技术越来越先进，芯片技术取得巨大进步，通信技术日新月异，车辆智能驾驶技术也逐渐变得成熟。智能驾驶技术的发展最大的作用在于：减轻驾驶员的行车负担、减少交通事故的发生。目前国内外很多知名公司都投入了大量的高精尖人员和技术来发展高水平的智能驾驶技术。

驾驶决策与轨迹规划模块是自动驾驶最核心的部分之一，尤其是轨迹规划算法越来越受到研究者的关注。良好的轨迹规划算法，能够使车辆更加平稳、更加安全地行驶。通常车辆的轨迹规划算法，能够通过传感器等感知部件获取车辆及其周围信息，如：动静态障碍物位置、速度等，自车位置、速度等，规划出一条安全、可行、合理的行驶轨迹，并将轨迹信息实时发送给控制模块，由控制模块对车辆做出合理的控制，如加速、减速、避障、变道等控制，实现车辆自动驾驶功能。目前，尽管现有的自动驾驶车辆，能够检测动静态障碍物，但由于障碍物千变万化，车辆对于障碍物的响应不够灵活稳定，存在车辆与障碍物碰撞的风险，具有较大的安全隐患。

此外，现有的车辆对于停止点的处理，通常是在参考路径附近设定以停止点为圆心、半径为R的停止圈，只要车辆处于停止区域内，车辆则停止。但由于车辆行驶误差、车辆避障、车辆难以立即减速等原因，车辆会冲出停止圈甚至无法到达停止圈，导致停车失败。

发明内容

本发明的目的在于提供一种基于样条曲线与多项式曲线的自动驾驶轨迹规划方法，以解决现有技术中导致的上述缺陷。

本发明公开了一种在复杂多变的环境下，基于样条曲线与多项式曲线的轨迹规划方法，包括全局路径的生成与处理、局部参考路径的提取与处理、环境感知与数据融合处理、局部轨迹规划、最优轨迹采样与轨迹输出等。其中：

S1.全局路径的生成与处理，全局路径从高精地图或事先采样的经纬度坐标获取。

进一步地，若通过高精地图获取全局路径，其原始数据由横纵相交的道路、红绿灯等元素组成，根据起点、终点，通过A*算法按照一定规则，规划出合理的全局路径，获取全局路径的离散点或特征点，作为轨迹规划的一次参考路径。

进一步地，若通过事先采样的经纬度坐标获取全局路径，其原始数据是按照一定时间频率采样的位置数据，数据点密度不均匀，需要对采样位置数据预处理。按照等间距的方式，提取经纬度坐标并将其转化为世界坐标(x,y)，作为轨迹规划的一次参考路径。

S2.局部参考路径的提取与处理

局部路径是全局一次路径的一段区间。由于全局路径是离散点，若直接将离散点做密集插值处理，其路径曲率、航向角等曲线属性变化较大，车辆以此为参考路径做局部轨迹规划，候选轨迹变化剧烈，不利于车辆平稳行驶。因此，需要对局部参考路径做样条拟合处理，使局部路径更加平滑。

S3.环境感知与数据融合

感知与融合模块，其融合后的数据是基于车辆坐标系，而局部路径或全局路径是基于世界坐标系，由于坐标系的不同，感知数据与局部路径或全局路径无法映射，需要将基于车辆坐标系的融合数据，转化为基于世界坐标系的融合数据，使二者数据坐标系统一，实现感知与路径的映射。

S4.局部轨迹规划

以样条拟合的局部路径为参考线，建立Frenet坐标系，并在该坐标系下，分别从横向与纵向两个维度，以时间为变量，通过多项式曲线插值的方式，完成轨迹规划，并将该轨迹作为候选轨迹。

S5.对候选轨迹进行运动学校核、曲率校核、碰撞检测校核、边界条件校核等，筛选可行轨迹。

S6.对可行轨迹做代价计算，代价因素包括：舒适性、参考轨迹偏离度、轨迹长度、碰撞风险、交通规则等。

S7.根据代价最小原则，选择最优轨迹。

S8.最优轨迹采样与轨迹输出

最优轨迹的初步离散采样，是以等时间间隔进行，相邻采样点距离间隔不均匀，不利于车辆控制。因此对等时间采样的离散点，做样条二次等间距插值采样，并计算等间距采样点的位置、曲率、航向角、弧长等信息，输出到车辆控制模块，由车辆控制模块根据离散采样点，根据实际情况选择合适的预瞄点，执行车辆控制。

本发明还公开了一种车辆停止方法，在参考路径上，计算车辆当前位置与停止点在参考路径上的投影，比较二者投影点的相对位置。若车辆投影点在停止点的投影点停止区间内，则车辆应执行停车动作。

本发明的优点在于：

1.对全局路径采样点做等距离间隔间隔提取处理，在保证全局路径准确有效的前提下，降低了全局路径采样点的数量，进而降低了轨迹规划系统的计算量，提升系统的稳定性与鲁棒性。

2.对局部路径做样条拟合处理，在保证拟合路径与期望路径误差较小的前提下，使局部路径更加平滑，弥补了因传感器等不可消除的误差导致的参考路径异常。

3.考虑更加符合驾驶习惯的代价因素，如障碍物碰撞风险、交通规则等，规划出的最优轨迹将更加适应环境，如左边避障/超车、贴近障碍物减速通过等。

4、以投影的方式，比较车辆与停止点在参考线上的投影点相对位置关系，可以适应更多的停车场景，如行驶中途停车、因避障而无法到达停车点的停车等。

附图说明

图1为轨迹规划算法的总流程。

图2为局部路径样条拟合处理示意图。

图3为障碍物转换示意图。

图4为车辆局部路径轨迹规划示意图。

图5为最优轨迹确认流程图。

图6为车辆停止示意图。

具体实施方式

为使本发明实现的技术手段、创作特征、达成目的与功效易于明白了解，下面结合具体实施方式，进一步阐述本发明。

参照图1所示，本发明的一种基于样条曲线与多项式曲线的自动驾驶轨迹规划方法，其特征包括：全局路径的生成与处理、局部参考路径的提取与处理、环境感知与数据融合处理、局部轨迹规划、最优轨迹采样与轨迹输出等。

在轨迹规划任务开始之前，需要获取由环境感知与融合模块输出的障碍物信息。感知融合模块包括的硬件传感器有相机、毫米波雷达、超声波雷达、激光雷达等，他们各自检测的障碍物信息，由感知融合模块统一转换到同一坐标系，通常是车辆坐标系，并输出稳定的障碍物信息。

轨迹规划任务开始之前，还需要获取车辆定位信息，包括车辆的位置、姿态等，这些定位信息信息通常由GPS或SLAM等定位方式获得。此外，还需要获取车辆需要行驶的全局路径，全局路径的来源主要有高精地图、SLAM地图等。

S1.全局路径的生成与处理

从高精地图中的路径是分片段行驶保存的，车辆无法知道如何选择行驶路径。此时的做法是，通过A*算法，根据车辆位置及终点位置，规划距离最短且合理的路径，并对全局路径进行采样。此外，对于采样地图，由于采样时根据等时间间隔采样，数据量巨大，为了降低轨迹规划的计算量，根据等距离间隔选取采样点作为全局路径。

S2.局部参考路径的提取与处理

如图2所示，对于从采样地图获取的全局路径，由于传感器存在噪音干扰，采集的数据点与车辆行驶的位置存在较大误差，与实际道路中心线的误差更大。若直接对采样路径进行样条插值，曲线曲率变化较大，不利于车辆行驶。此时，需要对采样的全局路径，根据车辆位置选取区间片段，并对区间采样点做样条拟合，拟合的局部路径更接近车辆行驶的路径，曲线更加平滑。同时，样条拟合后，对参考线等间隔离散，并计算离散点的位置(x,y,s),航向角

和曲率

S3.环境感知与数据融合处理

如图3所示，从感知融合模块获取的障碍物信息，其位置姿态是基于车辆坐标系的描述，而轨迹规划是基于世界坐标系的描述。因此轨迹规划中的碰撞检测要求将二者坐标系统一起来。已知障碍物在车辆坐标系的位置(^vx_ob,^vy_ob,^vθ_ob)，车辆在世界坐标系的位置(^wx_car,^wy_car,^wθ_car)，那么，他们的关系如下：

S4.局部轨迹规划

如图4所示，以局部参考路径的弧长方向为纵向、以局部参考路径的左垂直方向为横向建立局部参考路径的Frenet坐标系，并计算车辆所在Frenet坐标系的位置(s_car,d_car)；对车辆行驶的目标位置做横向与纵向采样，其中，横向采样等间距间隔序列为(d_n d_{n- 1}...d₀...d_-n+1d_-n),n＝1,2,3...,该序列是以局部参考线为对称中心的序列，参考线左边为正，右边为负。纵向采样弧长与预测时间相关，对时间等间距采样，则时间序列为(T,T-ΔT,T-2ΔT,T-3ΔT,...,T-nΔT),n＝0,1,2,3...。据此，分别从Frenet坐标系横向d与纵向s两个维度进行五次多项式轨迹规划，形成候选选轨迹，其编号按照横向与纵向采样特点编号(lat_index,lon_index)，标号规则为：lat_index为横向编号，以参考线为对称中心，左边为正，右边为负，如：(-3，-2，-1,0,1,2,3)；lon_index为纵向编号，由远及近依次标号为0,1,2,3...。纵向距离s、横向距离d，时间t之间的关系为:

并对候选轨迹按照等时间间隔采样离散形成候选轨迹的离散点。

为了求解系数a0～a5与系数b0～b5,需要确定初始条件。

进一步地，求解a0～a5时，确定初始条件：车辆当前位置条件(s₀,s'₀,s″₀)，目标点位置条件(s₁,s′₁,s″₁)。其中，s＝f(t)表示t时刻的纵向距离；

表示t时刻的纵向速度；

表示t时刻的纵向加速度。

进一步地，求解b0～b5时，确定初始条件：车辆当前位置条件(d₀,d'₀,d″₀)，目标点位置条件(d₁,d′₁,d″₁)。其中，d＝f(s)，表示局部参考线s处车辆的横向偏移；

表示局部参考线s处的航向角θ；

表示局部参考线s处的曲率k。

S5.对候选轨迹做曲线校核

包括曲率curvature校核、速度v校核、加速度a校核、边界d校核、碰撞检测校核等，将符合条件的候选轨迹保存到可行轨迹列表中。

其中，i,j表示第i候选轨迹第j离散点。

S6.计算可行轨迹的代价cost

考虑的因素包括：舒适度C_comfort，行驶效率C_efficiency，中心偏离度，C_offset安全性C_safty，交通规则代价C_left,因此可行轨迹的总代价为：

cost＝k₁C_comfort+k₂C_efficiency+k₃C_offset+k₄C_safty+k₅C_left

其中k₁为舒适度系数，k₂为行驶效率，k₃为中心偏离度系数，k₄为安全系数，k₅为交通规则代价系数；

进一步地，舒适度代价，主要表现在车辆的加加速度，反应了汽车加速度的变化率。因此，从Frenet坐标系考虑横向与纵向舒适度，其描述为车辆从起点t0到终点t1,加加速度平方的积分：

C_comfort＝K_dC_d+K_sC_s

进一步地，行驶效率代价，主要表现在车辆速度与目标速度的误差积分，即：

进一步地，中心偏离度代价，主要描述为车辆行驶的目标点与中心参考线的偏离程度。

进一步地，安全代价，主要描述为可行轨迹与障碍物垂直最小距离的倒数，距离越小，碰撞风险越大，距离远远，碰撞风险越小。

进一步地，交通规则代价，主要描述为：当车辆避障或变道时，优先选择参考线左边轨迹。

S7.选择选择可行轨迹时，按照代价最小原则选取轨迹。

S8.最优轨迹采样与轨迹输出

如图5所示，由于障碍物检测稳定性等原因，最优轨迹可能产生频繁跳动，这会使车辆发生剧烈摆动，甚至冲出道路，存在巨大安全隐患。为了避免因障碍物检测不稳定等因素带来的负面影响，需要对最优轨迹做确认处理。其方法是：存储连续M个周期的临时最优轨迹，统计M个周期内最优轨迹相同序号出现的次数。当最大次数N大于设定阈值时，则认为轨迹稳定，需要更新最优轨迹；否则认为轨迹不稳定，则继续保持上次的最优轨迹行驶。

本发明还公开了一种车辆到达终点停止方法，如图6所示，当停止点处于局部参考线区间内，计算车辆在参考线上的投影弧长为s_车,停止点在参考线上的投影弧长为s_停，当满足条件

则认为，车辆达到了停止点区域内，此时车辆应该执行预定动作，如停车。

由技术常识可知，本发明可以通过其它的不脱离其精神实质或必要特征的实施方案来实现。因此，上述公开的实施方案，就各方面而言，都只是举例说明，并不是仅有的。所有在本发明范围内或在等同于本发明的范围内的改变均被本发明包含。

Claims (10)

1.一种基于样条曲线与多项式曲线的自动驾驶轨迹规划方法，其特征在于，包括如下步骤：

S1.全局路径的生成与处理

通过A*算法，根据车辆位置及终点位置，规划距离最短且合理的路径，并对全局路径进行采样，根据等距离间隔选取采样点作为全局路径；

S2.局部参考路径的提取与处理

对采样的全局路径，根据车辆位置选取区间片段，并对区间采样点做样条拟合，拟合的局部路径更接近车辆行驶的路径，曲线更加平滑；

样条拟合后，对参考线等间隔离散，并计算离散点的位置(x,y,s),航向角

和曲率

S3.环境感知与数据融合处理

从感知融合模块获取的障碍物信息，其位置姿态是基于车辆坐标系的描述，而轨迹规划是基于世界坐标系的描述，因此轨迹规划中的碰撞检测要求将二者坐标系统一起来，已知障碍物在车辆坐标系的位置(^vx_ob,^vy_ob,^vθ_ob)，车辆在世界坐标系的位置(^wx_car,^wy_car,^wθ_car)，那么，他们的关系如下：

S4.局部轨迹规划

以局部参考路径的弧长方向为纵向、以局部参考路径的左垂直方向为横向建立局部参考路径的Frenet坐标系，并计算车辆所在Frenet坐标系的位置(s_car,d_car)；

对车辆行驶的目标位置做横向与纵向采样：

横向采样等间距间隔序列为(d_n d_n-1...d₀...d_-n+1d_-n),n＝1,2,3...,该序列是以局部参考线为对称中心的序列，参考线左边为正，右边为负；

纵向采样弧长与预测时间相关，对时间等间距采样，则时间序列为(T,T-ΔT,T-2ΔT,T-3ΔT,...,T-nΔT),n＝0,1,2,3...；

据此，分别从Frenet坐标系横向d与纵向s两个维度进行五次多项式轨迹规划，形成候选选轨迹，其编号按照横向与纵向采样特点编号，标号规则为：lat_index为横向编号，以参考线为对称中心，左边为正，右边为负；lon_index为纵向编号，由远及近依次编号为0,1,2,3...；

纵向距离s、横向距离d，时间t之间的关系为:

并对候选轨迹按照等时间间隔采样离散形成候选轨迹的离散点，为了求解系数a0～a5与系数b0～b5，需要确定初始条件，具体如下：

求解a0～a5时，确定初始条件：车辆当前位置条件(s₀,s'₀,s”₀)，目标点位置条件(s₁,s₁',s₁”)，其中，s＝f(t)表示t时刻的纵向距离；

表示t时刻的纵向速度；

表示t时刻的纵向加速度；

求解b0～b5时，确定初始条件：车辆当前位置条件(d₀,d'₀,d'₀')，目标点位置条件(d₁,d₁',d₁”)，其中，d＝f(s)，表示局部参考线s处车辆的横向偏移；

表示局部参考线s处的航向角θ；

表示局部参考线s处的曲率k；

并对候选轨迹按照等时间间隔采样离散形成候选轨迹的离散点；

S5.候选轨迹校核

对候选轨迹做曲线校核，包括曲率cur校核、速度v校核、加速度a校核、边界d校核、碰撞检测校核，将符合条件的候选轨迹保存到可行轨迹列表中：

其中，i,j表示第i候选轨迹第j离散点；

S6.可行轨迹的代价计算

计算可行轨迹的代价cost，考虑的因素包括：舒适度C_comfort、行驶效率C_efficiency、中心偏离度C_offset、安全性C_safty以及交通规则代价C_left,因此可行轨迹的总代价为：

cost＝k₁C_comfort+k₂C_efficiency+k₃C_offset+k₄C_safty+k₅C_left

其中k₁为舒适度系数，k₂为行驶效率，k₃为中心偏离度系数，k₄为安全系数，k₅为交通规则代价系数；

S7.最优轨迹选择

选择可行轨迹时，按照代价最小原则选取最优轨迹；

S8.最优轨迹采样与轨迹输出

存储连续M个周期的临时最优轨迹，统计M个周期内最优轨迹相同序号出现的次数，当最大次数N大于设定阈值时，则认为轨迹稳定，需要更新最优轨迹；否则认为轨迹不稳定，则继续保持上次的最优轨迹行驶。

2.根据权利要求1所述的一种基于样条曲线与多项式曲线的自动驾驶轨迹规划方法，其特征在于，在轨迹规划任务开始之前，需要获取由环境感知与融合模块输出的障碍物信息，感知融合模块包括的硬件传感器包括但不限于相机、毫米波雷达、超声波雷达、激光雷达，上述硬件传感器检测的障碍物信息，由感知融合模块统一转换到同一坐标系，并输出稳定的障碍物信息。

3.根据权利要求2所述的一种基于样条曲线与多项式曲线的自动驾驶轨迹规划方法，其特征在于，所述坐标系为车辆坐标系。

4.根据权利要求1所述的一种基于样条曲线与多项式曲线的自动驾驶轨迹规划方法，其特征在于，在轨迹规划任务开始之前，还需要获取车辆定位信息，包括但不限于车辆的位置、姿态，上述车辆定位信息信息由GPS或SLAM定位方式获得，此外，还需要获取车辆需要行驶的全局路径，全局路径的来源包括但不限于高精地图、SLAM地图。

5.根据权利要求1所述的一种基于样条曲线与多项式曲线的自动驾驶轨迹规划方法，其特征在于，所述舒适度代价，主要表现在车辆的加加速度，反应了汽车加速度的变化率，因此，从Frenet坐标系考虑横向与纵向舒适度，其描述为车辆从起点t₀到终点t₁的加加速度平方的积分：

6.根据权利要求1所述的一种基于样条曲线与多项式曲线的自动驾驶轨迹规划方法，其特征在于，所述行驶效率代价，主要表现在车辆速度与目标速度的误差积分，即：

7.根据权利要求1所述的一种基于样条曲线与多项式曲线的自动驾驶轨迹规划方法，其特征在于，所述中心偏离度代价，主要描述为车辆行驶的目标点与中心参考线的偏离程度：

8.根据权利要求1所述的一种基于样条曲线与多项式曲线的自动驾驶轨迹规划方法，其特征在于，所述安全代价，主要描述为可行轨迹与障碍物垂直最小距离的倒数，距离越小，碰撞风险越大，距离远远，碰撞风险越小：

9.根据权利要求1所述的一种基于样条曲线与多项式曲线的自动驾驶轨迹规划方法，其特征在于，所述交通规则代价，主要描述为当车辆避障或变道时，优先选择参考线左边轨迹：

10.一种采用权利要求1所述的自动驾驶轨迹规划方法判断车辆是否到达终点并停止的方法，其特征在于，当停止点处于局部参考线区间内，计算车辆在参考线上的投影弧长为s_车，停止点在参考线上的投影弧长为s_停，当满足条件：

则认为，车辆达到了停止点区域内，此时车辆应该执行预定停车动作。

Images