CN108519773A - 一种结构化环境下无人驾驶车辆的路径规划方法 - Google Patents

Abstract

一种结构化环境下无人驾驶车辆的路径规划方法该方法，包括以下步骤：S1、定位；S2、当前车道路况分析；其中判断是否需要完整重规划，是，转步骤S3；否，转步骤S4；S3、完整重规划；S4、更新局部规划结果。本发明旨在设计一个集路径规划和换道决策于一体的结构化环境下无人驾驶车辆的规划方法，保证无人车行驶的安全性和舒适性。

Description

一种结构化环境下无人驾驶车辆的路径规划方法

技术领域

本发明属于无人驾驶技术领域，具体涉及一种结构化环境下无人驾驶车辆的路径规划方法。

背景技术

近年来，随着科技的发展，无人驾驶汽车已从概念逐渐变为现实。无人驾驶汽车是一种智能汽车，也可以称之为轮式移动机器人，主要依靠车内以计算机***为主的智能驾驶仪来实现无人驾驶。无人驾驶技术主要包括环境信息的感知，驾驶行为的智能决策，无碰撞路径的规划以及车辆的运动控制四个部分。其中，规划模块是无人驾驶技术中十分重要的组成部分，为环境感知和运动控制起着承上启下的作用。

路径规划是规划模块的一个重要组成部分，目前主流的路径生成算法主要有：基于图搜索的规划算法、基于采样的规划算法和曲线插值的规划算法。基于图搜索的规划算法主要包括：Dijkstra算法、A*算法和State lattices算法，其中Dijkstra 算法是一种典型的最短路径算法，它是以起始点为中心向外层层扩展，直到扩展到终点为止，适合于结构化和非结构化环境的全局规划，但在点的数量巨大的情况下效率不高，所以不适用于需要实时规划的场景；A*算法需要设定合适的启发函数，全面估计各扩展搜索节点的代价值，通过比较各扩展节点的代价值大小，选择最有希望的点加以扩展，直到找到目标节点为止，A*算法扩展节点少，鲁棒性好，对环境信息反映快，但在实际应用中要注意运动体自身体积带来的限制；State lattices能够处理多维状态(位置、速度、加速度、时间)，适合局部规划和动态环境，但计算代价太大。基于采样的方法主要是RRT及其衍生算法，它能够在多维的***中提供一种快速的解决方案，适合全局规划和局部规划，但它产生的轨迹不能持续，因此是不稳定的。基于曲线插值的方法主要有：B样条曲线，贝塞尔曲线，多项式曲线，它们的优点是计算代价低，但生成轨迹的时候需要满足一定的生成条件，不够灵活，难以胜任复杂的交通场景。

中国专利CN105549597A公开了一种基于环境不确定性的无人车动态路径规划方法，其考虑了车辆所具有的解构特性和运动学特性，使用车辆自身多维状态作为条件求解六次多项式的系数，生成从起点到终点的候选路径。上述专利没有考虑变道时后方车辆的行为，也没有考虑无人车在行驶过程中遇到的交通标志，容易造成自动驾驶过程的交通事故。

发明内容

本发明的目的在于克服上述现有技术的缺点，提出了一种结构化环境下无人驾驶车辆的路径规划方法，在满足无人驾驶安全性条件下，同时保证了程序运行效率，并且能够合理的应对较为复杂的交通场景。

本发明的技术方案具体包括以下步骤：

步骤S1、定位；

首先加载全局地图数据，然后读取本车状态信息，最后找出离本车距离最短的全局路径点作为全局定位点；

步骤S2、当前车道路况分析；

第一、由传感器获取交通场景信息即障碍物信息，交通标志信息，车道线信息；第二、判断所获取的交通场景信息是否需要完整重规划，如果是，转到步骤S3完整重规划，否则，转到步骤S4，更新局部规划结果；

步骤S3、完整重规划；

首先，换道分析获取无人车与前后车辆的纵向距离，速度和加速度，计算得出无人车在当前交通场景下是否可以换道；其次，模板生成中采取五次样条插值算法，结合车辆运动学模型，生成31条备选路径，这31条备选路径的集合，称为模板；最后，计算每条备选路径的代价，采用代价最小的备选路径更新局部规划结果；

步骤S4、更新局部规划结果；

更新局部规划结果适应交通场景的不断变化，包括速度设定和更新当前局部路径和拓展轨迹两部分。

所述步骤S1包括以下子步骤：

步骤S101：获取全局地图信息和本车状态信息；

全局地图数据包括路径点的坐标(x_i,y_i)，i∈N，

交通标志，本车状态信息包括无人驾驶汽车的坐标(x_c,y_c)，速度V_c，加速度a_c，朝向θ_c，朝向变化率θ_c’；

步骤S102、计算全局定位点；

计算本车的坐标(x_c,y_c)到每个全局路径点坐标(x_i,y_i)的距离，找出离本车距离最小的全局路径点作为全局坐标系下的定位点。

所述步骤S2包括以下子步骤：

步骤S201、获取感知信息；

感知信息包括障碍物信息，交通标志信息和车道线信息，

检测出的障碍物用矩形 box表示，包含长宽、朝向、速度、加速度属性，交通标志信息包括限速牌，箭头，红绿灯，车道线信息用于告诉无人车当前有几条车道，目前处于哪条车道，是否允许变道；

步骤S202、判断是否需要完整重规划；

判断是否完整重规划，首先，判断前方是否有障碍物；其次，检查车道线是否为

步骤S3、完整重规划包括以下子步骤：

步骤S301、换道分析；

首先，以车自身为中心，将车前后划分为9个子区域，

编号从左到右，从上至下分别为A1、A2…A9，其中车自身可能处于的区域有A4、A5和A6，符号表示为 A_c∈{A4,A5,A6},障碍物可能处于除A_c区域外的任意区域，符号表示为A_o∈{A}and A_o≠A_c，A为全集，包括所有子区域；

向左换道分析如下，实验车处于区域A5；

①.从感知模块获取A2区域最近车辆信息，速度记为V_o2，加速度记为a_o2。如果V_o2大于实验车速度V_c，或者仿真两秒后的速度V_o2+2×a_o2大于V_c，则认为当前车道可正常行驶，选择跟随，保持当前局部路径；

②.若步骤①中的判断结果为否，则判断A4区域是否被占据，如果是，选择跟随，保持当前局部路径；

③.若步骤②中判断结果为否，则判断A1区域是否被占据，如果是，且满足条件V_o1小于V_c且V_o1+2×a_o1小于V_c，则选择跟随，保持当前局部路径；

④.若区域A1没有被占据，或者满足条件V_o1大于V_c或V_o1+2×a_o1大于V_c，则判断A7区域是否被占据；

⑤.若步骤④中A7区域没有被占据，则选择代价最小的候选路径；

⑥.若步骤④中A7区域被占据，则判断V_o7×2+2×a_o7是否大于dis+V_c×2，dis为A7区域车头到实验车尾部的纵向距离，2为仿真时间，若是，则选择跟随，保持当前局部路径，否则，选择代价最小的候选路径；

步骤S302、模板生成；

满足以下公式：

根据简单自行车运动学模型建立车辆状态模型S＝[x,y,θ,δ,v]^T，其中S为车辆运动状态，x,y是车后轴中心相对于全局坐标原点的横向坐标和纵向坐标，θ为车辆朝向角，δ为车前轮转角，v为车当前速，L为自行车前轮中心到后轮中心的距离， 分别为x、y、θ对时间的一阶导数；

采用五次样条插值方法生成候选路径，候选路径上的点的x坐标应当满足以下公式：

a、b、c、d、e和f是五次样条的系数，s为车移动的距离，这里的x不是对时间t求导，而是对车移动距离s求导，公式推导如下：

dx/dt＝vcosθ

ds/dt＝v

首先，获取车辆自身状态S(x₀,y₀,θ₀,δ₀,v₀)，作为样条函数的起始条件，代入公式(*)，得方程组：

其中θ₀’＝tan(δ₀)/L；

然后，将局部路径终点信息，记为(x_se,y_se,θ_se)，作为终点条件，代入公式(*)，得方程组：

整理方程组后写成AB＝C矩阵方程形式：

其中

A＝[a b c]

C＝[sin(θ₀)θ₀'se²/2-cos(θ₀)se-x₀+x_se sin(θ₀)θ₀'se-cos(θ₀)+cos(θ_se) sin(θ₀)θ₀']

通过矩阵求逆运算A＝CB^-1求出矩阵A的值，于是求得了五次样条所有系数a、b、 c、d、e和f，最后，根据五次样条函数计算所有候选路径点的x坐标；

以相同的方法，计算候选路径点的y坐标；

根据候选路径每个点的x坐标和y坐标，生成从实验车自身出发到局部路径终点的一条候选路径，然后将终点集合的其他终点分别作为终点条件生成其他的候选路径，最后产生的候选路径集合，既模板；

步骤S303、备选路径选择；

根据以下公式计算左边15条候选路径的代价，找出代价最小的备选路径：

若侯选路径遇障，则代价为无穷大，不参与上述公式计算，其中(x_c,y_c)为车自身坐标，(x_i,y_i)为候选路径终点坐标，i取值范围为1到15，I代表候选路径序号。

所述步骤S4包括以下子步骤：

步骤S401、速度设定；

首先读取自身车速V_c、前方车辆速度V_o和两车的距离d，

然后使用以下公式计算目标车速V_obj：

其中t＝min{1,αV_c/d},因为t∈(0,1),所以加入取最小值的条件限制，参数k和α需要根据实车性能和控制模块性能适当调整；

步骤S402、更新当前局部路径和拓展轨迹；

当前局部路径更新的规则：

a、无人车行驶到当前局部路径的终点；

b、有障碍物在拓展路径或者在当前局部路径上；

c、拓展路径是从上次目标点到本次目标点的全局路径点，因为规划程序不断的在全局路径定位新的目标点，所以拓展路径时刻都在更新。

本发明更新局部规划结果是为了适应交通场景的不断变化，实时的调整有利于安全性和舒适性的保证。其中包括速度设定和更新当前局部路径和拓展轨迹两部分。

附图说明

图1为本发明整体框架示意图；

图2为实验车与前方车辆，当前局部路径与拓展路径位置关系示意图；

图3换道前交通场景示意图；

图4为换道分析流程示意图；

图5换道后交通场景示意图；

图6为简单自行车运动学模型；

具体实施方式

下面结合附图对本发明做进一步详细描述。

如图1所示，一种结构化环境下无人驾驶车辆的路径规划方法包含以下步骤：

步骤S1、定位。具体的，如图1第一个虚线框所示，包括以下子步骤：

步骤S101、获取全局地图信息和本车状态信息；

具体的，全局地图信息包括路径点的坐标(x_i,y_i)，i∈N，交通标志信息；本车状态信息包括无人驾驶汽车的坐标(x_c,y_c)，速度V_c，加速度a_c，朝向变化率θ_c’，朝向θ_c。

步骤S102、计算全局定位点；

具体的，计算本车的坐标(x_c,y_c)到每个全局路径点坐标(x_i,y_i)的距离，找出离本车距离最小的全局路径点作为全局定位点。

步骤S2、当前车道路况分析。具体的，如图1第二个虚线框所示，包括以下子步骤：

步骤S201、获取感知信息；

具体的，感知信息包括障碍物信息，交通标志信息和车道线信息。检测出的障碍物用矩形box表示，包含长宽、朝向、速度、加速度属性，交通标志信息主要包括限速牌，箭头，红绿灯，车道线信息用于告诉无人车当前有几条车道，目前处于哪条车道，是否允许变道。

步骤S202、判断是否需要完整重规划；

判断是否完整重规划，需要做三个判断。首先，判断是否前方是否有障碍物；其次，检查车道线是否为实线；最后，检查交通标志是否禁止换道。

具体的，判断是否前方是否有障碍物，如图2所示，实验车已经沿着当前局部路径行驶了一段距离，同时拓展路径也向前延伸了一段距离，拓展路径是从上次目标点到本次目标点的全局路径点，其目的是在自动驾驶时更早的发现前方障碍物，给予实验车充足的距离做出相应的调整。图2中前方车辆在拓展路径上，处于与实验车相同车车道，若前方车辆速度小于实验车车速，说明当前车道不能畅通行驶。然后检查车道线信息，判断是否为实线，若为实线，则不需要完整重规划，否则，检查交通标志是否禁止换道，若没有交通标志禁止换道，则进行步骤S3完整重规划。总之，完整重规划的条件是当前车道存在小于车速的障碍物，并且车道线不为实线且没有禁止换道的交通标志。否则，一律不进行完整重规划，只保持当前车道行驶。

步骤S3、完整重规划。具体的，如图1第三个虚线框所示，包括以下子步骤：

步骤S301、换道分析；

具体的，如图3所示，首先，以车自身为中心，将车前后划分为9个子区域，编号从左到右，从上至下分别为A1、A2…A9。其中车自身可能处于的区域有A4、A5 和A6，符号表示为A_c∈{A4,A5,A6},障碍物可能处于除A_c区域外的任意区域，符号表示为A_o∈{A}and A_o≠A_c，A为全集，包括所有子区域。

向左变道流程如图4所示，实验车处于区域A5。

①.从感知模块获取A2区域最近车辆信息，速度记为V_o2，加速度记为a_o2。如果V_o2大于实验车速度V_c，或者仿真两秒后的速度V_o2+2×a_o2大于V_c，则认为当前车道可正常行驶，选择跟随，保持当前局部路径；

②.若步骤①中的判断结果为否，则判断A4区域是否被占据，如果是，选择跟随，保持当前局部路径；

③.若步骤②中判断结果为否，则判断A1区域是否被占据，如果是，且满足条件V_o1小于V_c且V_o1+2×a_o1小于V_c，则选择跟随，保持当前局部路径；

④.若区域A1没有被占据，或者满足条件V_o1大于V_c或V_o1+2×a_o1大于V_c，则判断A7区域是否被占据；

⑤.若步骤④中A7区域没有被占据，则选择代价最小的候选路径；

⑥.若步骤④中A7区域被占据，则判断V_o7×2+2×a_o7是否大于dis+V_c×2(dis为 A7区域车头到实验车尾部的纵向距离，2为仿真时间)，若是，则选择跟随，保持当前局部路径，否则，选择向左换道。图3为换道前示意图，图5为换道后示意图。

步骤S302、模板生成；

图6是简单自行车运动学模型，满足以下公式：

根据简单自行车运动学模型建立车辆状态模型S＝[x,y,θ,δ,v]^T，其中S为车辆运动状态，(x,y)是车后轴中心相对于全局坐标原点的横向坐标和纵向坐标，θ为车辆朝向角，δ为车前轮转角，v为车当前速度，L为自行车前轮中心到后轮中心的距离，分别为x、y、θ对时间的一阶导数。

本发明采用五次样条插值方法生成候选路径，这里详细说明x轴坐标的计算。候选路径上的点的x坐标应当满足以下公式：

a、b、c、d、e和f是五次样条的系数，s为车移动的距离，这里的x不是对时间t求导，而是对车移动距离s求导，与简单自行车运动学模型有些不同，公式推导如下：

dx/dt＝vcosθ

ds/dt＝v

首先，获取车辆自身状态S(x₀,y₀,θ₀,δ₀,v₀)，作为样条函数的起始条件，代入公式(*)，得方程组：

其中θ₀’＝tan(δ₀)/L。

然后，将局部路径终点信息，记为(x_se,y_se,θ_se)，作为终点条件，代入公式(*)，得方程组：

整理方程组后写成AB＝C矩阵方程形式：

其中

A＝[a b c]

C＝[sin(θ₀)θ₀'se²/2-cos(θ₀)se-x₀+x_se sin(θ₀)θ₀'se-cos(θ₀)+cos(θ_se) sin(θ₀)θ₀']

接下来，通过矩阵求逆运算A＝CB^-1求出矩阵A的值，于是求得了五次样条所有系数a、b、c、d、e和f。最后，根据五次样条函数计算所有候选路径点的x坐标。同理，计算候选路径点的y坐标。

通过以上方法得到了候选路径每个点的x坐标和y坐标，生成了从实验车自身出发到局部路径终点的一条候选路径。然后将终点集合的其他终点分别作为终点条件生成其他的候选路径，最后产生的候选路径集合，既模板，如图3所示。

步骤S303、备选路径选择；

根据以下公式计算左边15条候选路径的代价，找出代价最小的备选路径：

若侯选路径遇障，则代价为无穷大，不参与上述公式计算。其中(x_c,y_c)为车自身坐标，(x_i,y_i)为候选路径终点坐标，i取值范围为1到15，I代表候选路径序号。

步骤S4、更新局部规划结果。具体的，如图1第四个虚线框所示，包括以下子步骤：

步骤S401、速度设定；

具体的，首先读取自身车速V_c、前方车辆速度V_o和两车的距离d。然后使用以下公式计算目标车速V_obj：

这里使用了三阶贝塞尔公式计算车目标速度，其中t＝min{1,αV_c/d},因为t∈(0,1), 所以加入取最小值的条件限制，参数k和α需要根据实车性能和控制模块性能适当调整。其设计的优点在于，当前方车速慢于自身车速时，无人车初始的刹车加速度会比较大，随着车速的降低和距离的拉近，刹车加速度也会逐渐变小，这样可以快速的把车速降低下来，达到一个安全的速度，然后缓慢滑行至距障碍物一定的安全距离时停止，安全舒适。

步骤S402、更新当前局部路径和拓展轨迹；

具体的，当前局部路径更新的规则：

a、无人车行驶到当前局部路径的终点；

b、有障碍物在拓展路径或者在当前局部路径上；

c、拓展路径是从上次目标点到本次目标点的全局路径点，因为规划程序不断的在全局路径定位新的目标点，所以拓展路径时刻都在更新。

综上所述，本发明提供了一种结构化环境下无人驾驶车辆的路径规划方法，主要内容包括路径生成的算法，速度规划，轨迹的选择。本规划方法具有确实的可行性和良好的扩展性，能够保证无人驾驶汽车行驶过程中的安全性和舒适性。

对于无人驾驶领域的技术人员来说，可以根据以上描述的规划方法实现无人车的自动驾驶，或者基于其进行相应的改变和适当的调整，而所有的这些改变以及调整都应属于本发明权利要求的保护范围之内。

Claims (5)

1.一种结构化环境下无人驾驶车辆的路径规划方法，其特征在于包括以下步骤：

步骤S1、定位；

首先加载全局地图数据，然后读取本车状态信息，最后找出离本车距离最短的全局路径点作为全局定位点；

步骤S2、当前车道路况分析；

第一、由传感器获取交通场景信息即障碍物信息，交通标志信息，车道线信息；第二、判断所获取的交通场景信息是否需要完整重规划，如果是，转到步骤S3完整重规划，否则，转到步骤S4，更新局部规划结果；

步骤S3、完整重规划；

首先，换道分析获取无人车与前后车辆的纵向距离，速度和加速度，计算得出无人车在当前交通场景下是否可以换道；其次，模板生成中采取五次样条插值算法，结合车辆运动学模型，生成31条备选路径，这31条备选路径的集合，称为模板；最后，计算每条备选路径的代价，采用代价最小的备选路径更新局部规划结果；

步骤S4、更新局部规划结果；

更新局部规划结果适应交通场景的不断变化，包括速度设定和更新当前局部路径和拓展轨迹两部分。

2.根据权利要求1所述的结构化环境下无人驾驶车辆的路径规划方法，其特征在于：

所述步骤S1包括以下子步骤：

步骤S101：获取全局地图信息和本车状态信息；

全局地图数据包括路径点的坐标(x_i,y_i)，i∈N，交通标志，本车状态信息包括无人驾驶汽车的坐标(x_c,y_c)，速度V_c，加速度a_c，朝向θ_c，朝向变化率θ_c’；

步骤S102、计算全局定位点；

计算本车的坐标(x_c,y_c)到每个全局路径点坐标(x_i,y_i)的距离，找出离本车距离最小的全局路径点作为全局坐标系下的定位点。

3.根据权利要求1所述的结构化环境下无人驾驶车辆的路径规划方法，其特征在于：

所述步骤S2包括以下子步骤：

步骤S201、获取感知信息；

感知信息包括障碍物信息，交通标志信息和车道线信息，检测出的障碍物用矩形box表示，包含长宽、朝向、速度、加速度属性，交通标志信息包括限速牌，箭头，红绿灯，车道线信息用于告诉无人车当前有几条车道，目前处于哪条车道，是否允许变道；

步骤S202、判断是否需要完整重规划；

判断是否完整重规划，首先，判断前方是否有障碍物；其次，检查车道线是否为实线；最后，检查交通标志是否禁止换道；完整重规划的条件是当前车道存在小于车速的障碍物，并且车道线不为实线且没有禁止换道的交通标志，否则，一律不进行完整重规划，只保持当前车道行驶。

4.根据权利要求1所述的结构化环境下无人驾驶车辆的路径规划方法，其特征在于：

步骤S3、完整重规划包括以下子步骤：

步骤S301、换道分析；

首先，以车自身为中心，将车前后划分为9个子区域，编号从左到右，从上至下分别为A1、A2…A9，其中车自身可能处于的区域有A4、A5和A6，符号表示为A_c∈{A4,A5,A6},障碍物可能处于除A_c区域外的任意区域，符号表示为A_o∈{A}andA_o≠A_c，A为全集，包括所有子区域；

向左换道分析如下，实验车处于区域A5；

①.从感知模块获取A2区域最近车辆信息，速度记为V_o2，加速度记为a_o2。如果V_o2大于实验车速度V_c，或者仿真两秒后的速度V_o2+2×a_o2大于V_c，则认为当前车道可正常行驶，选择跟随，保持当前局部路径；

②.若步骤①中的判断结果为否，则判断A4区域是否被占据，如果是，选择跟随，保持当前局部路径；

③.若步骤②中判断结果为否，则判断A1区域是否被占据，如果是，且满足条件V_o1小于V_c且V_o1+2×a_o1小于V_c，则选择跟随，保持当前局部路径；

④.若区域A1没有被占据，或者满足条件V_o1大于V_c或V_o1+2×a_o1大于V_c，则判断A7区域是否被占据；

⑤.若步骤④中A7区域没有被占据，则选择代价最小的候选路径；

⑥.若步骤④中A7区域被占据，则判断V_o7×2+2×a_o7是否大于dis+V_c×2，dis为A7区域车头到实验车尾部的纵向距离，2为仿真时间，若是，则选择跟随，保持当前局部路径，否则，选择代价最小的候选路径；

步骤S302、模板生成；

满足以下公式：

根据简单自行车运动学模型建立车辆状态模型S＝[x,y,θ,δ,v]^T，其中S为车辆运动状态，x,y是车后轴中心相对于全局坐标原点的横向坐标和纵向坐标，θ为车辆朝向角，δ为车前轮转角，v为车当前速度，L为自行车前轮中心到后轮中心的距离，分别为x、y、θ对时间的一阶导数；

采用五次样条插值方法生成候选路径，候选路径上的点的x坐标应当满足以下公式：

a、b、c、d、e和f是五次样条的系数，s为车移动的距离，这里的x不是对时间t求导，而是对车移动距离s求导，公式推导如下：

dx/dt＝vcosθ

ds/dt＝v

首先，获取车辆自身状态S(x₀,y₀,θ₀,δ₀,v₀)，作为样条函数的起始条件，代入公式(*)，得方程组：

其中θ₀’＝tan(δ₀)/L；

然后，将局部路径终点信息，记为(x_se,y_se,θ_se)，作为终点条件，代入公式(*)，得方程组：

整理方程组后写成AB＝C矩阵方程形式：

其中

A＝[a b c]

C＝[sin(θ₀)θ₀'se²/2-cos(θ₀)se-x₀+x_se sin(θ₀)θ₀'se-cos(θ₀)+cos(θ_se) sin(θ₀)θ₀']

通过矩阵求逆运算A＝CB^-1求出矩阵A的值，于是求得了五次样条所有系数a、b、c、d、e和f，最后，根据五次样条函数计算所有候选路径点的x坐标；

以相同的方法，计算候选路径点的y坐标；

根据候选路径每个点的x坐标和y坐标，生成从实验车自身出发到局部路径终点的一条候选路径，然后将终点集合的其他终点分别作为终点条件生成其他的候选路径，最后产生的候选路径集合，既模板；

步骤S303、备选路径选择；

根据以下公式计算左边15条候选路径的代价，找出代价最小的备选路径：

若侯选路径遇障，则代价为无穷大，不参与上述公式计算，其中(x_c,y_c)为车自身坐标，(x_i,y_i)为候选路径终点坐标，i取值范围为1到15，I代表候选路径序号。

5.根据权利要求1所述的结构化环境下无人驾驶车辆的路径规划方法，其特征在于：

所述步骤S4包括以下子步骤：

步骤S401、速度设定；

首先读取自身车速V_c、前方车辆速度V_o和两车的距离d，然后使用以下公式计算目标车速V_obj：

其中t＝min{1,αV_c/d},因为t∈(0,1),所以加入取最小值的条件限制，参数k和α需要根据实车性能和控制模块性能适当调整；

步骤S402、更新当前局部路径和拓展轨迹；

当前局部路径更新的规则：

a、无人车行驶到当前局部路径的终点；

b、有障碍物在拓展路径或者在当前局部路径上；

c、拓展路径是从上次目标点到本次目标点的全局路径点，因为规划程序不断的在全局路径定位新的目标点，所以拓展路径时刻都在更新。