CN113721637B - 智能车动态避障路径连续规划方法、系统及存储介质 - Google Patents

Abstract

本发明公开一种智能车动态避障路径连续规划方法、系统及存储介质。方法包括：利用Frenet坐标转换将车辆沿复杂弯曲道路行驶时的局部路径规划问题转化为直线道路下的双移线路径规划问题，大大降低了规划的复杂度；并将双移线路径分为避障路段和回归路段两部分，基于贝塞尔曲线分别进行路径规划；当周围环境发生动态变化时，特别是在车辆跟踪局部路径行驶时，障碍物突然加速、减速或发生侧向位移，规划路径的终点就会进行相应的更新，同时对局部路径进行再规划，并保证路径连接点处的曲率连续性。本发明可提高智能车规划路径的实时性，有助于增强智能车对复杂交通环境的适应性。

Description

智能车动态避障路径连续规划方法、系统及存储介质

技术领域

本发明涉及自动驾驶决策规划技术领域，尤其涉及基于Frenet坐标系和贝塞尔曲线的智能车动态避障路径连续再规划方法，具体为一种智能车动态避障路径连续规划方法、系统及存储介质。

背景技术

自动驾驶技术能够有效保障交通安全、提升通行效率并改善出行方式，自动驾驶的底层架构和95％的常规技术问题已经得以解决，剩下5％的长尾问题成为制约自动驾驶落地应用的关键。这些长尾问题涉及各种复杂场景下的可靠感知、快速预测、最优化决策规划等关键技术。

针对车辆运动规划问题，目前的解决方案是当检测到前方存在障碍物时，决策规划层根据当前环境信息输出局部避障路径，然后跟踪行驶。这种方案多为静态避障路径规划，这里的静态指只利用避障前的障碍物运动状态信息(位置、姿态、速度等)进行避障路径规划，在避障过程中不再对避障路径进行实时更新和优化。然而对于复杂动态的交通环境，当自车沿避障路径行驶时，一旦障碍物运动状态突然发生变化，避障前规划的路径就不再可靠，极易出现避障路径与障碍物路径干涉的情况，存在极大的碰撞风险，进而影响自动驾驶的行驶安全。

发明内容

为克服上述现有技术的不足，本发明提供一种智能车动态避障路径连续规划方法、系统及存储介质，其能够根据障碍物运动状态的变化进行实时路径再规划，并保证新规划的路径曲率与原路径曲率连续。

根据本发明说明书的一方面，提供一种智能车动态避障路径连续规划方法，包括：

构建基于Frenet坐标系的局部路径规划架构；

基于贝塞尔曲线进行分路段的局部路径规划；

依据规划好的局部路径进行车辆跟踪行驶，以预定时间步长获取障碍物的运动状态信息，并在障碍物的运动状态发生变化时基于贝塞尔曲线进行路径再规划，且再规划的路径曲率与原路径曲率连续。

上述技术方案首先构建基于Frenet坐标系的局部路径规划架构，实现Frenet坐标与笛卡尔坐标的相互转换，将车辆沿复杂弯曲道路行驶时的局部路径规划问题转化为直线道路下的双移线路径规划问题，并在Frenet坐标系下基于贝塞尔曲线进行分路段的路径规划，极大降低了路径规划复杂度；该技术方案在跟踪行驶时，考虑障碍物的实时运动状态，并在障碍物的实时运动状态发生变化时重新进行路径规划，实时更新车辆的局部路径，在复杂动态的交通环境下实现了避障路径连续动态更新，且保证了前后规划路径的曲率连续性和整体路径的平滑性。

作为进一步的技术方案，分路段的局部路径规划包括：避障路段路径规划和回归路段路径规划，其中避障路段的终点与回归路段的起点一致。在利用Frenet坐标转换将车辆沿复杂弯曲道路行驶时的局部路径规划问题转化为直线道路下的双移线路径规划问题后，将双移线路径分为避障路段和回归路段两部分，以便于分别进行路径规划，降低规划的复杂度。

作为进一步的技术方案，所述方法进一步包括：当自车发现前方障碍物并沿全局路径行驶至局部路径避障路段的起点时，根据自车及前方障碍物的初始运动状态确定避障路段的终点，然后基于贝塞尔曲线进行路径规划；当自车行驶至避障路段的终点时，根据自车及前方障碍物的当前运动状态确定回归路段的终点，然后基于贝塞尔曲线进行路径规划。该技术方案中，自车在发现前方障碍物并进行了分段路径规划，在各分路段分别进行跟踪行驶时，前方障碍物的运动状态未发生变化，即在一次路径规划基础上完成整个避障过程。

作为进一步的技术方案，所述方法进一步包括：依据规划好的路径进行车辆跟踪行驶时，若获知前方障碍物的运动状态发生变化，则重新计算当前路段的路径终点，并根据车辆当前位置及重新计算出的路径终点基于贝塞尔曲线进行路径再规划。该技术方案中，自车在进行跟踪行驶时，以预定的时间步长获取前方障碍物的运动状态，若前方障碍物的运动状态发生了变化，导致已规划好的路径不再适用，则进行路径再规划。该技术方案通过重新确定当前路段的路径终点并依据新的终点进行路径再规划，使再规划后的避障路径能够适应障碍物运动状态变化后的避障操作，实现复杂动态交通环境下避障路径的连续动态更新。

作为进一步的技术方案，当车辆依据规划路径进行车辆跟踪行驶时，若障碍物突然加速，则增大路径终点；若障碍物突然减速，则减小路径终点的_s坐标；若障碍物产生一定侧向移动，则改变路径终点的l坐标。

作为进一步的技术方案，所述方法进一步包括：

获取全局路径；

在笛卡尔坐标系下获取地图、自车运动状态及障碍物运动状态的信息；

当自车前方出现障碍物时，以全局路径为参考性，将笛卡尔坐标系下实时获取的地图、自车运动状态及障碍物运动状态转换到Frenet坐标系下，进行局部路径规划；

将Frenet坐标系下规划的局部路径转化为笛卡尔坐标系下的局部路径，并跟踪行驶。

该技术方案中，当车辆沿全局路径行驶时，车辆前方出现障碍物，进行局部路径规划以避开障碍物，并且避障完成后回到全局路径继续行驶。其中，结构化道路上的全局路径通常为车道中心线，开放环境下的全局路径为全局规划层计算的起点至终点的静态避障路径。

作为进一步的技术方案，所述方法基于三阶贝塞尔曲线进行路径再规划，所述三阶贝塞尔曲线由四个控制点确定，其中，第一控制点和第四控制点为路径的起点和终点，第二控制点和第三控制点用于控制路径的曲率变化。

作为进一步的技术方案，所述第二控制点按如下公式求解：

其中，s₀，l_o为第一控制点的坐标，s₁，l₁为第二控制点的坐标，m为第一控制点、第二控制点的间距，为贝塞尔曲线的第一个待优化变量，ψ₀为Frenet坐标下车辆在第一个控制点时的航向角；

所述第三控制点按如下公式求解：

其中，s₂，l₂为第三控制点的坐标，s₃，l₃为第四控制点的坐标，m为第三控制点、第四控制点的间距，为贝塞尔曲线的第二个待优化变量，ψ₃为Frenet坐标下避障路段终点的航向角或回归路段终点的航向角。

根据本发明说明书的一方面，提供一种智能车动态避障路径连续规划系统，采用所述的方法实现，所述系统包括：

构建模块，用于构建基于Frenet坐标系的局部路径规划架构；

路径规划模块，用于基于贝塞尔曲线进行局部路径规划和再规划，且再规划的路径曲率与原规划的路径曲率连续；

跟踪行驶模块，用于依据规划好的局部路径进行车辆跟踪行驶，以预定时间步长获取障碍物的运动状态信息，并在障碍物的运动状态发生变化时触发路径规划模块进行路径再规划。

上述技术方案中，通过构建模块构建基于Frenet坐标系的局部路径规划架构，实现Frenet坐标到笛卡尔坐标的转换，将笛卡尔坐标系下的实时状态信息转换到Frenet坐标系下并经由路径规划模块进行局部路径规划，然后通过跟踪行驶模块进行车辆的跟踪行驶，当作为前一次路径规划基础的前方障碍物的运动状态发生变化时，启动路径规划模块进行路径再规划，再经由跟踪行驶模块依据再规划后的路径进行跟踪行驶，直至完成避障过程。该技术方案充分考虑作为避障路径规划基础的障碍物的实时运动状态对避障过程的影响，依据障碍物的实时运动状态动态更新避障路径，保证避障过程的顺利完成及避障路径的平滑和曲率的连续。

根据本发明说明书的一方面，提供一种计算机可读存储介质，所述计算机可读存储介质上存储有计算机程序，所述计算机程序被处理器执行时实现所述智能车动态避障路径连续规划方法的步骤。

与现有技术相比，本发明的有益效果在于：

(1)本发明提供一种方法，该方法利用Frenet坐标转换将车辆沿复杂弯曲道路行驶时的局部路径规划问题转化为直线道路下的双移线路径规划问题，大大降低了规划的复杂度；并将双移线路径分为避障路段和回归路段两部分，基于贝塞尔曲线分别进行路径规划；当周围环境发生动态变化时，特别是在车辆跟踪局部路径行驶时，障碍物突然加速、减速或发生侧向位移，规划路径的终点就会进行相应的更新，同时对局部路径进行再规划，并保证路径连接点处的曲率连续性。本发明可提高智能车规划路径的实时性，有助于增强智能车对复杂交通环境的适应性。

(2)本发明提供一种系统，该系统通过构建模块构建基于Frenet坐标系的局部路径规划架构，实现Frenet坐标到笛卡尔坐标的转换，将笛卡尔坐标系下的实时状态信息转换到Frenet坐标系下并经由路径规划模块进行局部路径规划，然后通过跟踪行驶模块进行车辆的跟踪行驶，当作为前一次路径规划基础的前方障碍物的运动状态发生变化时，启动路径规划模块进行路径再规划，再经由跟踪行驶模块依据再规划后的路径进行跟踪行驶，直至完成避障过程。该系统充分考虑作为避障路径规划基础的障碍物的实时运动状态对避障过程的影响，依据障碍物的实时运动状态动态更新避障路径，保证避障过程的顺利完成及避障路径的平滑和曲率的连续。

附图说明

图1为根据本发明实施例的智能车动态避障路径连续规划方法示意图。

图2为根据本发明实施例的Frenet坐标系下的局部路径规划示意图。

图3为根据本发明实施例的避障路段和回归路段的再规划示意图。

图4为根据本发明实施例的不同控制点分布下的贝塞尔曲线示意图。

图5为根据本发明实施例的自动驾驶实时避障路径更新示意图。

具体实施方式

以下将结合附图对本发明各实施例的技术方案进行清楚、完整的描述，显然，所描述发实施例仅仅是本发明的一部分实施例，而不是全部的实施例。基于本发明的实施例，本领域普通技术人员在没有做出创造性劳动的前提下所得到的所有其它实施例，都属于本发明所保护的范围。

本发明提供一种智能车动态避障路径连续规划方法及系统，该方法及系统基于Frenet坐标系和贝塞尔曲线实现智能车动态避障路径的连续再规划，能够在复杂动态的交通环境下实现避障路径连续动态更新，并保证整体路径的平滑和曲率连续。

实施例1

如图1所示，本实施例提供一种智能车动态避障路径连续规划方法。本实施例在路径规划时，考虑障碍物的实时运动状态，以一定时间步长，实时更新车辆的局部路径，并保证曲率连续。

本实施例的方法包括：构建基于Frenet坐标系的局部路径规划架构；基于贝塞尔曲线的曲率连续性路径规划；以及，复杂动态环境下的实时避障路径更新。

本实施例中，构建基于Frenet坐标系的局部路径规划架构，包括：

(1)需要已知全局路径，结构化道路上的全局路径通常为车道中心线，开放环境下的全局路径为全局规划层计算的起点至终点的静态避障路径。局部路径规划的目的是当车辆沿全局路径行驶时，车辆前方出现障碍物，进行局部路径规划以避开障碍物，并且能回到全局路径继续行驶。

(2)实时获取高精度地图信息、自车运动状态信息以及障碍物运动状态信息，障碍物主要指自主车辆周围的其他交通参与者(车辆、行人、骑行者等)，运动状态信息主要包括位置(x，y)、姿态ψ、速度v、加速度a等，前述的所有信息均为各传感器基于全局坐标系(笛卡尔坐标系)采集获得。

(3)根据Frenet坐标转换规则，以全局路径为参考线，将上述实时获取的高精地图信息、自车及障碍物的运动状态信息，转化到Frenet坐标系下。

(4)在Frenet坐标系中，所有沿全局路径的局部避障路径规划问题都被转化为直线道路下的双移线路径规划问题，如图2所示，双移线局部路径包括：为避开障碍物进行一次单移线路径规划(避障路段AB)，并在跟踪过程中实时更新，避开障碍物之后需要回到全局路径，再进行一次单移线路径规划(回归路段BC)，并在跟踪过程中实时更新。

(5)将Frenet坐标下的规划的局部路径转化为笛卡尔坐标系下的局部路径，用以跟踪行驶。

更进一步，上述Frenet坐标转换的原理描述如下：全局路径作为参考线，以全局路径上各点的切线方向为横轴S(车辆行驶方向为正)，以全局路径上各点法线方向为纵轴L(车辆行驶方向左边为正)，坐标转换的主要目的是实现笛卡尔坐标系下的坐标(x，y)与Frenet坐标系下的坐标(s，l)之间的相互转换。

本实施例中，Frenet坐标转换是基于全局路径离散点实现的，转换方法包括：

(1)笛卡尔坐标系(x，y)转Frenet坐标(s，l)

在自动驾驶系统中，全局路径通常以离散点的形式发布，已知全局路径为某一固定间距的离散点集，并且以k*3的表格形式进行存储，每一行代表一个点，一至三列分别为笛卡尔坐标下每个点的路径长度s、横向位置x、纵向位置y，其中路径长度s可以用各点间的直线距离的累加近似代替。以全局路径作为参考线，以全局路径上各点的切线方向为横轴S(车辆行驶方向为正)，以全局路径上各点法线方向为纵轴L(车辆行驶方向左边为正)，建立Frenet坐标系。

已知(x，y)，求对应Frenet坐标下的(s，l)。

首先在笛卡尔坐标系下找到点(x，y)距离全局路径最近的点(x_n，y_n)，即(x-x_n)²+(y-y_n)²最小，则在全局路径的表格中，(x_n，y_n)对应的S_n即为点(x，y)在Frenet坐标系下的s，而[(x-x_n)²+(y-y_n)²]^1/2即为Frenet坐标系下l的绝对值。

然后利用两个向量的叉积判断l的正负(右手定则)。记全局路径上点(x_n，y_n)的下一个点为点(x_nn，y_nn)，令空间向量a＝(x_nn-x_n，y_nn-y_n，0)、b＝(x-x_n，y-y_n，0)，c＝a×b，则空间向量c的第三个元素的正负即为l的正负，正代表点(x，y)在全局路径的左边，负代表右边。

(2)Frenet坐标(s，l)转笛卡尔坐标系(x，y)

已知Frenet坐标下的(s，l)求对应的(x，y)。

首先已知s，根据上述全局路径表格可以知道笛卡尔坐标下(x，y)在全局路径上的最近点(x_n，y_n)，然后根据根据点(x_n，y_n)和点(x_nn，y_nn)计算最近点的单位方向向量e₁＝(e_x1，e_y1)；l为正则将e₁顺时针旋转θ＝3*π/2，l为负则将e₁顺时针旋转θ＝π/2，得到点(x_n，y_n)与点(x，y)连线的单位向量e₂＝(e_x2，e_y2)，其中e_x2＝e_y1sinθ+e_x1cosθ，e_y2＝e_y1cosθ-e_x1sinθ；最后得到点(x，y)，其中x＝x_n+|l|e_x2，y＝y_n+|l|e_y2。

根据上述坐标转换方法，将笛卡尔坐标系下的高精度地图信息、障碍物信息及自车信息转到为Frenet坐标系下，即可实现将弯曲道路下的局部路径规划问题转化为直线道路下的规划问题，然后基于贝塞尔曲线进行双移线路径规划，并根据障碍物运动状态的变化，实时更新局部路径，保证曲率连续。

本实施例中，基于构建的Frenet坐标系进行基于贝塞尔曲线的路径规划及再规划。

如图2所示，将局部路径分为避障路段AB和回归路段BC，局部路径规划前需要确定A点坐标，并根据当前障碍物的运动状态确定避障路段的终点B，然后基于贝塞尔曲线进行避障路段的路径规划。

在车辆跟踪避障路段行驶过程中，如果障碍物运动状态发生变化，会导致避障路段的终点发生变化，则需要从车辆当前位置开始再规划一条路径连接新的终点，并且保证各连接点处曲率连续。如图3所示，图中空心原点即代表障碍物运动状态发生变化，需要重新规划路径时的自车位置，星标点为不同障碍物运动状态下的路段终点。

当车辆行驶至B点，用同样的方法完成回归路段的规划和动态更新，需要注意的是，回归路段的终点不管如何变化都必须在全局路径上，即l_C＝0。

如图3所示，根据定时距纵向跟驰模型，当发现全局路径前方存在静止障碍物或速度过低的障碍物时，自车需要进行局部路径规划，规划的起点A的坐标(s_A，l_A)由定时距纵向跟驰模型确定，

其中，s_ob，A为自车在A点时障碍物的S坐标，v_ego为自车速度，t_f和d_f分别为跟车时距和停车距离，为常数。

确定局部路径规划起点后，采用三阶贝塞尔曲线进行局部路径规划，如图2所示，局部路径包括两个部分，首先实现一定距离的横向位移避开障碍物，图2中的AB段(避障路段)，然后回到全局路径上，图2中的BC段(回归路段)。

如图5所示，当自车沿全局路径行驶至局部路径规划起点A点时，根据前方障碍物运动状态，按以下方式计算避障路段终点B的坐标(s_B，l_B)：

其中，s_ob，A、v_ob，A、l_0b，A分别为自车在A点时，障碍物的S坐标、S方向的运动速度和L坐标，Δt₁为避障路段预计耗时，预计耗时等于路段终点与当前点的s差值除以当前车速，w_ob、w_ego、w分别为障碍物宽度、自车宽度及自车与障碍物的期望侧向间距。

由公式2可知，路段终点与障碍物的实时位置、速度以及尺寸有关，自车以一定的时间步长Δt_s更新障碍物的运动状态信息，当障碍物的运动状态发生变化(如突然加速、减速或产生一定侧向移动)，即更新时障碍物的实时位置Sob、l_ob及速度v_ob的数值发生变化，如l_ob变化超过±0.1m或v_ob变化超过±0.1m/s，则对路段终点进行相应更新，并依据更新后的终点及车辆当前位置对局部路径进行再规划，并保证连接点处曲率连续。

如图5所示，当自车运动至A′点，障碍物运动状态发生变化，避障路段终点更新为B′点。

本实施例基于三阶贝塞尔曲线构建避障路段局部路径，其中，三阶贝塞尔曲线原理及曲率连续性再规划方法具体描述如下：

三阶贝塞尔曲线由四个控制点P₀、P₁、P₂、P₃确定，如图4所示，为不同控制点分布下的贝塞尔曲线示意图，P₀、P₃即为路径的起点和终点，P₁、P₂用于控制路径的曲率变化。

用P_b表示贝塞尔曲线上的点，则

P_b＝(1-τ)P_2，1+τP_2，2 公式3；

其中，

τ从0到1变化，间隔越小，贝塞尔曲线上点越密集。

三阶贝塞尔曲线的确定需要找到Frenet坐标系下的四个控制点的坐标P₀(s₀，l₀)、P₁(s₁，l₁)、P₂(s₂，l₂)、P₃(s₃，l₃)。

贝塞尔曲线的第一个控制点P₀(s₀，l₀)即为车辆当前位置，所述车辆当前位置可以是局部路径中避障路段的起点A或回归路段的起点B，也可以是自车沿局部路径行驶过程中，任一时刻障碍物运动状态发生变化时自车的位置。

贝塞尔曲线的第二个控制点P₁(s₁，l₁)，按如下方式求解：

其中，m为控制点P₀、P₁的间距，为贝塞尔曲线的第一个待优化变量，ψ₀为Frenet坐标下车辆在第一个控制点时的航向角，这样处理可以保证路径起点处的曲率连续。

贝塞尔曲线的第四个控制点P₃(s₃，l₃)为局部路径中AB避障路段或BC回归路段的终点。

贝塞尔曲线的第三个控制点P₂(s₂，l₂)，按如下方式求解：

其中，n为控制点P₂、P₃的间距，为贝塞尔曲线的第二个待优化变量，ψ₃为Frenet坐标下B点的航向角或者C点的航向角，这样处理可以保证路径终点处的曲率连续。

如图5所示，避障路段的起点(图5点A或点A′)和终点(图5点B或点B′)即为三阶贝塞尔曲线的第一个控制点P₀(s₀，l₀)和第四控制点P₃(s₃，l₃)，第二控制点P₁(s₁，l₁)和第三控制点P₂(s₂，l₂)按如下方式计算：

其中，m为控制点P₀、P₁的间距，为贝塞尔曲线的第一个待优化变量，ψ₀为Frenet坐标下车辆在第一个控制点时的航向角，n为控制点P₂、P₃的间距，为贝塞尔曲线的第二个待优化变量，ψ₃为路段终点的航向角，这样处理可以保证路径终点处的曲率连续，因为三阶贝塞尔曲线的第一条控制边和第三条控制边即为第一控制点和第四控制点的切线。

上述贝塞尔曲线有两个待优化变量m、n，本实施例以相邻路径点曲率增量的累计和最小为目标对m、n进行优化，

公式8中，ρ_i和ρ_i-1分别表示第i点和第i-1点的曲率，J表示优化函数，k为路径点个数。

综上，当车辆行驶至局部路径规划起点A点时，计算出避障路段终点B，并在行驶过程中实时更新路段终点；连接车辆当前点与路段终点的路径由贝塞尔曲线计算，而贝塞尔曲线存在两个待优化变量，通过确定的目标函数进行求解即可得到避障路段的动态局部路径，且保证各连接点处曲率连续。

本实施例中，当车辆行驶至避障路段终点时，开始进行回归路段的局部路径规划，具体步骤同避障路段的规划与更新过程。首先确定回归路段的终点，需要注意的是回归路段终点只能在全局路径上，即l_C＝0，然后利用贝塞尔曲线规划连接当前点与路段终点，在行驶过程中，障碍物运动状态发生变化，则更新回归路段终点，重新规划当前点与路段终点的路径。

本实施例给出了复杂动态环境下的实时避障路径更新策略，包括：

(1)当自车发现前方障碍物，并沿全局路径行驶至局部路径规划的起点A时，首先根据自车及前方障碍物的初始运动状态确定局部路径避障路段的终点B(s_B，l_B)，然后利用三阶贝塞尔曲线进行路径规划。

(2)自车沿规划的路径行驶，并以一定的时间步长Δt_s更新障碍物的运动状态信息，当障碍物运动状态发生变化(如突然加速、减速或产生一定侧向移动)时，路径终点B将进行相应调整(增大、减小s_B或改变l_B)。

(3)路径终点发生变化，基于贝塞尔曲线进行路径再规划，以此保证更新后的路径与原路径在连接点处曲率连续。

(4)当自车行驶至局部路径避障路段的终点B时，根据当前状态确定局部路径回归路段的终点C(s_B，l_B)，然后用相同的方式进行路径动态更新直到完成整个局部路径规划和跟踪行驶。

实施例2

本实施例提供一种智能车动态避障路径连续规划系统，包括：构建模块，用于构建基于Frenet坐标系的局部路径规划架构；路径规划模块，用于基于贝塞尔曲线进行局部路径规划和再规划，且再规划的路径曲率与原规划的路径曲率连续；跟踪行驶模块，用于依据规划好的局部路径进行车辆跟踪行驶，以预定时间步长获取障碍物的运动状态信息，并在障碍物的运动状态发生变化时触发路径规划模块进行路径再规划。

本实施例通过构建模块构建基于Frenet坐标系的局部路径规划架构，实现Frenet坐标到笛卡尔坐标的转换，将笛卡尔坐标系下的实时状态信息转换到Frenet坐标系下并经由路径规划模块进行局部路径规划，然后通过跟踪行驶模块进行车辆的跟踪行驶，当作为前一次路径规划基础的前方障碍物的运动状态发生变化时，启动路径规划模块进行路径再规划，再经由跟踪行驶模块依据再规划后的路径进行跟踪行驶，直至完成避障过程。

本实施例充分考虑作为避障路径规划基础的障碍物的实时运动状态对避障过程的影响，依据障碍物的实时运动状态动态更新避障路径，保证避障过程的顺利完成及避障路径的平滑和曲率的连续。

根据本发明说明书的一方面，提供一种计算机可读存储介质，所述计算机可读存储介质上存储有计算机程序，所述计算机程序被处理器执行时实现所述智能车动态避障路径连续规划方法的步骤。

最后应说明的是：以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述实施例所记载的技术方案进行修改，或者对其中部分或者全部技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明实施例技术方案。

Claims (9)

1.智能车动态避障路径连续规划方法，其特征在于，包括：

构建基于Frenet坐标系的局部路径规划架构；

基于贝塞尔曲线进行分路段的局部路径规划；在Frenet坐标系中，所有沿全局路径的局部避障路径规划问题都被转化为直线道路下的双移线路径规划问题，双移线局部路径包括：为避开障碍物进行一次单移线路径规划，并在跟踪过程中实时更新，避开障碍物之后需要回到全局路径，再进行一次单移线路径规划，并在跟踪过程中实时更新；

依据规划好的局部路径进行车辆跟踪行驶，以预定时间步长获取障碍物的运动状态信息，并在障碍物的运动状态发生变化时基于贝塞尔曲线进行路径再规划，且再规划的路径曲率与原路径曲率连续；

当自车沿全局路径行驶至局部路径规划起点A点，当前方障碍物的运动状态更新时，障碍物的实时位置s_ob、l_ob及速度v_ob的数值发生变化，则对路段终点进行相应更新，按以下方式计算避障路段终点B的坐标(s_B，l_B)：

其中，s_ob，A、v_ob，A、l_ob，A分别为自车在A点时，障碍物的S坐标、S方向的运动速度和L坐标，Δt₁为避障路段预计耗时，预计耗时等于路段终点与当前点的s差值除以当前车速，w_ob、w_ego、w分别为障碍物宽度、自车宽度及自车与障碍物的期望侧向间距；

对路段终点进行相应更新进一步包括：当障碍物突然加速，则增大路径终点；当障碍物突然减速，则减小路径终点的s坐标；当障碍物产生侧向移动，则改变路径终点的l坐标；

避障路段的起点和终点即为三阶贝塞尔曲线的第一个控制点P₀(s₀，l₀)和第四控制点P₃(s₃，l₃)，第二控制点P₁(s₁，l₁)和第三控制点P₂(s₂，l₂)按如下方式计算：

其中，m为控制点P₀、P₁的间距，为贝塞尔曲线的第一个待优化变量，ψ₀为Frenet坐标下车辆在第一个控制点时的航向角，n为控制点P₂、P₃的间距，为贝塞尔曲线的第二个待优化变量，ψ₃为路段终点的航向角；

以相邻路径点曲率增量的累计和最小为目标对m、n进行优化，

公式8中，ρ_i和ρ_i-1分别表示第i点和第i-1点的曲率，J表示优化函数，k表示路径点个数。

2.根据权利要求1所述智能车动态避障路径连续规划方法，其特征在于，分路段的局部路径规划包括：避障路段路径规划和回归路段路径规划，其中避障路段的终点与回归路段的起点一致。

3.根据权利要求2所述智能车动态避障路径连续规划方法，其特征在于，所述方法进一步包括：当自车发现前方障碍物并沿全局路径行驶至局部路径避障路段的起点时，根据自车及前方障碍物的初始运动状态确定避障路段的终点，然后基于贝塞尔曲线进行路径规划；当自车行驶至避障路段的终点时，根据自车及前方障碍物的当前运动状态确定回归路段的终点，然后基于贝塞尔曲线进行路径规划。

4.根据权利要求3所述智能车动态避障路径连续规划方法，其特征在于，所述方法进一步包括：依据规划好的路径进行车辆跟踪行驶时，当获知前方障碍物的运动状态发生变化，则重新计算当前路段的路径终点，并根据车辆当前位置及重新计算出的路径终点基于贝塞尔曲线进行路径再规划。

5.根据权利要求1所述智能车动态避障路径连续规划方法，其特征在于，构建基于Frenet坐标系的局部路径规划架构进一步包括：

获取全局路径；

在笛卡尔坐标系下获取地图、自车运动状态及障碍物运动状态的信息；

当自车前方出现障碍物时，以全局路径为参考性，将笛卡尔坐标系下实时获取的地图、自车运动状态及障碍物运动状态转换到Frenet坐标系下，进行局部路径规划；

将Frenet坐标系下规划的局部路径转化为笛卡尔坐标系下的局部路径，并跟踪行驶。

6.根据权利要求1所述智能车动态避障路径连续规划方法，其特征在于，所述方法基于三阶贝塞尔曲线进行路径再规划，所述三阶贝塞尔曲线由四个控制点确定，其中，第一控制点和第四控制点为路径的起点和终点，第二控制点和第三控制点用于控制路径的曲率变化。

7.根据权利要求6所述智能车动态避障路径连续规划方法，其特征在于，所述第二控制点按如下公式求解：

其中，s₀，l₀为第一控制点的坐标，s₁，l₁为第二控制点的坐标，m为第一控制点、第二控制点的间距，为贝塞尔曲线的第一个待优化变量，ψ₀为Frenet坐标下车辆在第一个控制点时的航向角；

所述第三控制点按如下公式求解：

其中，s₂，l₂为第三控制点的坐标，s₃，l₃为第四控制点的坐标，m为第三控制点、第四控制点的间距，为贝塞尔曲线的第二个待优化变量，ψ₃为Frenet坐标下避障路段终点的航向角或回归路段终点的航向角。

8.智能车动态避障路径连续规划系统，采用权利要求1-7中任一项所述的方法实现，其特征在于，所述系统包括：

构建模块，用于构建基于Frenet坐标系的局部路径规划架构；

路径规划模块，用于基于贝塞尔曲线进行局部路径规划和再规划，且再规划的路径曲率与原规划的路径曲率连续；

跟踪行驶模块，用于依据规划好的局部路径进行车辆跟踪行驶，以预定时间步长获取障碍物的运动状态信息，并在障碍物的运动状态发生变化时触发路径规划模块进行路径再规划。

9.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质上存储有计算机程序，所述计算机程序被处理器执行时实现如权利要求1至7任一项所述智能车动态避障路径连续规划方法的步骤。

Images