CN109960261A - 一种基于碰撞检测的动态障碍物避让方法 - Google Patents

Abstract

本发明涉及一种基于碰撞检测的动态障碍物避让方法，属于无人车辆技术领域，通过初规划构造车辆期望轨迹，并根据接收的动态障碍物预测轨迹和所述车辆期望轨迹，进行碰撞检测，对车辆的行驶速度和/或路径进行重新规划，得到车辆无碰撞行驶速度和/或路径数据，重新构造车辆期望轨迹，避让动态障碍物；实现无人车辆自主避让动态障碍物。本发明立足于自动驾驶车辆在动态环境下，所采用碰撞检测方法保证既不失碰撞检测精度，又可以提升碰撞检测效率；所采用重规划方法可以兼顾通行效率、通行平顺性和通行安全性效果。

Description

一种基于碰撞检测的动态障碍物避让方法

技术领域

本发明涉及无人车辆技术领域，尤其涉及一种基于碰撞检测的动态障碍物避让方法。

背景技术

无人驾驶技术作为当今车辆的前沿技术受到了各国学者的广泛关注。无人车辆的运动规划技术是无人驾驶车辆的核心技术之一，可被细分为路径规划与速度规划。其中路径规划系统的目标是实现无人驾驶车辆在空间可行区域的行驶，速度规划系统的目标是实现无人驾驶车辆对于时间可行区域的行驶。

目前对于无人驾驶车辆运动规划系统大部分只针对静态障碍物的处理，对于动态障碍物大多采用近似将其看做静态障碍物作反应式躲避处理。但由于动态障碍物具有一定的速度，反应式处理往往导致避让动态障碍物失败。

发明内容

鉴于上述的分析，本发明旨在提供一种基于碰撞检测的动态障碍物避让方法，实现无人车自主避让动态障碍物。

本发明的目的主要是通过以下技术方案实现的：

一种基于碰撞检测的动态障碍物避让方法，包括：

初规划步骤：在车辆行驶过程中对车辆的行驶路径和速度进行初步规划，得到车辆的期望路径、期望速度数据，构造车辆期望轨迹；

碰撞检测步骤：基于接收的动态障碍物预测轨迹和所述车辆期望轨迹，进行碰撞检测，判断车辆是否会与障碍物发生碰撞；

重规划步骤；当判断会发生碰撞后，对车辆的行驶速度和/或路径进行重新规划，得到车辆无碰撞行驶速度和/或路径数据，重新构造车辆期望轨迹，避让动态障碍物。

进一步地，所述碰撞检测为分层碰撞检测，包括二维空间碰撞检测和三维时间空间碰撞检测。

进一步地，所述碰撞检测中的二维空间碰撞检测，根据车辆期望轨迹的路径数据和动态障碍物预测轨迹的路径数据，计算车辆与障碍物的最近距离，若最近距离小于碰撞距离阈值，则进行三维时间空间碰撞检测；否则，判断不会发生碰撞。

进一步地，所述碰撞检测中的三维时间空间碰撞检测包括：

构造三维时空轨迹；三维时空的x轴和y轴为路径点的横、纵坐标，z轴为到达每个路径点的时间；在三维时空中，根据动态障碍物的几何信息、预测轨迹数据以及车辆的几何信息、期望轨迹数据，得到动态障碍物预测轨迹和车辆期望轨迹的点阵序列；

粗略检测；根据动态障碍物预测轨迹和车辆期望轨迹的点阵序列，进行三维重构，形成以三维轨迹点为边界的动态障碍物三维轨迹凸体和车辆三维轨迹凸体，对两个三维轨迹凸体之间的冲突区域进行检测，得到粗略的发生碰撞的轨迹点；

精细检测；锁定冲突区域，进行精细阶段的碰撞检测，得到精确的发生碰撞的轨迹点。

进一步地，所述重规划步骤中，当判断会与动态障碍物轨迹发生碰撞时，

1)首先启动速度重规划，进行纵向时间层面的状态空间采样，基于同一条路径形成多条不同时间序列的轨迹，与动态障碍物进行碰撞检测，得到车辆无碰撞速度数据，控制车辆避让动态障碍物；

2)若全部轨迹都发生碰撞，则此时切换成路径重规划，进行横向空间层面的状态空间采样，形成多条基于不同路径的轨迹，并与动态障碍物进行碰撞检测，得到车辆无碰撞行进路径和速度数据，控制车辆避让动态障碍物；

3)若全部轨迹都发生碰撞，则计算当前所处轨迹与动态障碍物碰撞的最近距离，基于当前所处轨迹减速停车。

进一步地，所述粗略检测阶段，采用基于BVH的快速碰撞检测方法；所述精细检测阶段，采用GJK算法进行碰撞检测。

进一步地，所述初规划步骤中行驶路径的初步规划为根据车辆当前状态，生成一条从车辆当前位置到参考线的行驶曲线，具体包括：

1)给定车辆当前车辆状态q_init(x_init,y_init,θ_init,κ_init)、车辆终点状态q_end(x_end,y_end,θ_end,κ_end)；其中x_init,y_init为车辆当前的x，y坐标，θ_init为车辆当前航向角，κ_init为车辆当前轨迹曲率；其中x_end,y_end为车辆终点位于参考线上的x，y坐标，θ_end为车辆终点航向角，κ_end为车辆终点轨迹曲率；

2)将路径规划问题转化为解

为路径平滑目标函数，

s_max为曲线弧长的上界；

函数κ(p)的参数a为已知的车辆当前状态所对应的曲率，q为车辆状态；通过求解得到函数κ(p)的参数b、c、d、s_f；

3)通过公式：使用不同的弧长s进行采样，得到路径规划的路径点集数据。

进一步地，所述初规划步骤中行驶速度的初步规划为包括：

1)输入路径点每点最大限速ν_max、最大纵向加速度最大纵向减速度最大横向加速度曲率κ、最大冲击度j_max、收敛阈值ε；

2)对每个路径点限制横向加速度和纵向加、减速度：

3)根据相邻三个路径点进行冲击度计算，得到每个路径点的冲击度j_i；

4)判断冲击度j_i值，当冲击度j_i>j_max时，根据最大冲击度j_max重新确定该点的速度，当冲击度j_i<-j_max时，根据最小冲击度-j_max重新确定该点的速度；得到周期速度序列V_f；

5)计算本周期速度序列V_f与上周期的差值，判断差值是否小于输入的阈值ε；如果小于则生成速度曲线输出；否则，返回2)再次进行速度迭代；其中初始周期的路径点速度值为其对应的最大速度值ν_max。

进一步地，所述速度重规划方法包括：

1)将初步规划产生的轨迹作为初始轨迹；

2)进行纵向的状态空间采样，形成同一空间，不同时间的多条车辆期望轨迹；

3)将多条车辆期望轨迹与动态障碍物轨迹进行碰撞检测；

4)判断碰撞检测结果，如存在无碰撞期望轨迹，则将无碰撞最优轨迹作为最终轨迹输出；如果不存在无碰撞期望轨迹，则进行路径重规划。

进一步地，所述路径重规划方法包括：

1)根据参考线，进行横向状态空间采样，得到终点状态集q_end(θ_end,κ_end,x_end,y_end)，

2)根据初步规划中的路径规划法生成多条根据车辆当前位置到多个目标采样状态的路径；

3)将多条路径按照初步规划中的速度规划法形成多条轨迹，与动态障碍物轨迹进行碰撞检测；

4)如果存在无碰撞轨迹，则选取最优无碰撞轨迹，作为最终轨迹输出；如果不存在无碰撞期望轨迹，则计算当前所处轨迹与动态障碍物碰撞的最近距离，基于当前所处轨迹减速停车。

本发明至少可实现如下有益效果之一：

本发明立足于自动驾驶车辆在动态环境下，提出了一套自动驾驶车辆动态障碍物避让框架。

在初步运动规划阶段，采用非线性优化和基于迭代约束的方法生成期望轨迹，实现自动驾驶车辆能够在无动态障碍物的情况下生成一条平滑、舒适的局部行驶轨迹；

在存在动态障碍物时，提出采用构造融合碰撞检测的方法，首先在二维空间筛选无碰撞轨迹，为三维碰撞检测减小计算负担，然后在三维空间构造层次包围盒树(BVH)的方法进行快速碰撞检测，保证既不失碰撞检测精度，又可以提升碰撞检测效率；

在避让动态障碍物阶段，本方法提出速度重规划优先的思路，优先采用纵向状态空间采样，进行速度重规划，选取最贴近原期望轨迹但又与动态障碍物轨迹不发生碰撞的速度重规划轨迹进行动态障碍物躲避，实现通行效率与平顺性的效果。

当纵向速度曲线束全部与动态障碍物轨迹发生碰撞时，进行路径重规划，选取最贴近原期望轨迹但又与动态障碍物轨迹不发生碰撞的路径重规划轨迹，实现安全躲避的效果。

附图说明

附图仅用于示出具体实施例的目的，而并不认为是对本发明的限制，在整个附图中，相同的参考符号表示相同的部件。

图1为本发明实施例中的动态障碍物避让方法流程图；

图2为本发明实施例中的速度规划方法流程图；

图3为本发明实施例中的二维空间碰撞检测示例图；

图4为本发明实施例中的构造三维时空轨迹示例图；

图5为本发明实施例中的BVH层次包围盒树示例图；

图6为本发明实施例中的粗略阶段确定的存在碰撞的区域示意图；

图7为本发明实施例中的精细阶段确定的存在碰撞的区域示意图；

图8为本发明实施例中的动态障碍物避让方法细节流程图。

具体实施方式

下面结合附图来具体描述本发明的优选实施例，其中，附图构成本申请一部分，并与本发明的实施例一起用于阐释本发明的原理。

本发明实施例公开了一种基于碰撞检测的动态障碍物避让方法，可用于无人车实时躲避动态障碍物的运动规划。

如图1所示，具体方法包括：

S101、初规划步骤：在车辆行驶过程中对车辆的行驶路径和速度进行初步规划，得到车辆的期望路径、期望速度数据，构造车辆期望轨迹；

S102、碰撞检测步骤：基于接收的动态障碍物预测轨迹和所述车辆期望轨迹，进行碰撞检测，判断车辆是否会与障碍物发生碰撞；

对于具有动态障碍物监测功能的无人车辆通常能够探测到的动态障碍物，并预测出动态障碍物的运动路径和运动速度，构造出动态障碍物预测轨迹。本实施例中对动态障碍物轨迹预测可采用目前公开文献中的方法。

S103、重规划步骤；当判断会发生碰撞后，对车辆的行驶速度和/或路径进行重新规划，得到车辆无碰撞行驶速度和/或路径数据，重新构造车辆期望轨迹，避让动态障碍物。

具体的，在初规划步骤中，对无人车的正常行进中局部路径和速度进行规划；

对于路径规划采用基于参考线根据非线性优化生成局部路径；

其中，参考线是根据无人车当前位置，从无人车辆全局路径规划结果中截取的一段局部参考路径；

其中，全局路径规划可以在无人车辆行驶之前，根据设置的车辆起点、车辆终点以及起点到终点的道路情况，进行事先规划并存储于无人车辆中，用于引导车辆行驶。

在无人车行驶过程中，根据车辆当前状态去生成一条从车辆当前位置到参考线的曲线。对曲线上路径点的X坐标、Y坐标、车辆航向角θ和曲线曲率κ进行规划。

通常将车辆的纵向运动描述成一维的非线性方程：s＝f(t)；

同时有，

根据弧长s可以表示曲线上的x的坐标：

同理可得：

对于航向角：

根据车辆半车运动学模型的几何关系可知，方向盘转角φ与车辆转弯半径r以及曲线曲率κ之间的关系为：其中L为车辆轴距。

由此可以得到，

将曲线上的曲率表示成随弧长变化的三次曲线：

κ(s)＝a+bs+cs²+ds³

笛卡尔坐标系下路径的弧长参数化表达形式为：

r(p)＝(x(p),y(p))

s.t.

以车辆当前位置作为原点，因此x₀＝y₀＝0，a为已知的车辆当前状态所对应的曲率，b、c、d、s_f是需要优化的变量，s_f同时也是到目标状态的弧长。

因此，给定当前车辆状态q_init和车辆终点状态q_end，其中车辆终点状态可以在参考线上选取，路径生成问题可表示为：

给定q_init＝(x_init,y_init,θ_init,k_init)

q_end＝(x_end,y_end,θ_end,κ_end)；

解Minimize

s.t.q＝q_init,s＝0；

q＝q_end,s＝s_f；

0≤s_f≤s_max；

其中参数s_max已知，为用户给定的曲线弧长的上界，q为车辆状态；路径平滑目标函数是曲线曲率平方对于弧的线积分，其叫做最小弯曲能量曲线。

通过解Minimize得到需要优化的变量b、c、d、s_f；

通过公式：以不同弧长s进行采样计算，得到路径规划的路径点集数据。

对于速度规划，

考虑到实车应用的时效性，采用基于多约束的迭代方法进行正常的速度规划。如图2所示，具体包括如下步骤：

1)速度规划以路径点信息、路径点每点最大限速ν_max、最大纵向加速度最大纵向减速度最大横向加速度曲率κ、最大冲击度j_max、收敛阈值ε作为输入。

2)对每个路径点限制横向加速度和纵向加、减速度：

对每个路径点限制横向加速度：

对每个路径点纵向加速度限制：

对每个路径点纵向减速度限制：

3)根据相邻三个路径点进行冲击度计算，得到每个路径点的冲击度j_i；

假设可以插入一个基于弧长变量的速度二次函数：

ν＝α·s²+β·s+γ

则：

其中s是弧长，v是速度，从上面的函数表示可以得到，当未知变量α、β、γ计算出来后，冲击度j就可以计算表示出来。

因此在计算第i个路点的冲击度jerk的时候，我们可以根据前一个点i-1以及后一个点i+1的信息，计算三个未知量(其中首点和末点jerk均为0)：

也就得到：

4)根据最大冲击度jerk来检查每个点的速度是否满足要求

判断冲击度j_i值，当冲击度j_i>j_max时，根据最大冲击度j_max重新确定该点的速度，当冲击度j_i<-j_max时，根据最小冲击度-j_max重新确定该点的速度；得到周期速度序列V_f；

当j_i>j_max时：

当j_i<-j_max时：

5)计算本周期速度序列V_f与上周期的差值判断差值是否小于输入的阈值ε；如果小于则生成速度曲线输出；否则，返回2)再次进行速度迭代，其中每个路径点迭代初始速度值为其对应的最大速度值ν_max。

本实施例中的碰撞检测为分层碰撞检测，包括二维空间碰撞检测和三维时间空间碰撞检测。

具体的，二维空间碰撞检测，根据车辆期望轨迹的路径数据和动态障碍物预测轨迹的路径数据，计算车辆与障碍物的最近距离，若最近距离小于碰撞距离阈值，则进行三维时间空间碰撞检测；否则，判断不会发生碰撞。

可选的，碰撞距离阈值可选车辆和障碍物二者的半径和，二维空间碰撞检测示例图如图3所示。

具体的，三维时间空间碰撞检测可包括：

构造三维时空轨迹；三维时空的x轴和y轴为路径点的横、纵坐标，z轴为到达每个路径点的时间；在三维时空中，根据动态障碍物的几何信息、预测路径、预测速度数据以及车辆的几何信息、期望路径、期望速度数据，得到动态障碍物预测轨迹和车辆期望轨迹的点阵序列；

如图4所示，为构造三维时空轨迹示例。

粗略检测；根据动态障碍物预测轨迹和车辆期望轨迹的点阵序列，进行三维重构，形成以三维轨迹点为边界的动态障碍物三维轨迹凸体和车辆三维轨迹凸体，其中基本构成单元为三角片。对两个三维轨迹凸体之间的冲突区域进行检测，得到粗略的发生碰撞的轨迹点。

优选的，粗略检测采用基于BVH的快速碰撞检测方法，根据动态障碍物的轨迹凸体以及无人驾驶车辆的轨迹凸体，形成BVH层次包围盒树；如图5所示。BVH迭代的终止条件为每个叶节点只包含一个三角面片单元。因为层次包围盒树是二叉树结构，具有二叉树的一系列特点，在根节点不发生碰撞的情况下，其子节点也一定不会发生碰撞，可以通过此特点加速碰撞检测的进行。

如图6所示，为粗略阶段确定的存在碰撞的区域示意图。

精细检测；锁定冲突区域，进行精细阶段的碰撞检测，得到精确的发生碰撞的轨迹点。

其中精细阶段的碰撞检测采用GJK算法，从而得到真正发生碰撞的轨迹点。如图7所示，为精细阶段确定的存在碰撞的区域示意图。

具体的，本实施例的重规划步骤中，当判断会与动态障碍物轨迹发生碰撞时，

1)首先启动速度重规划，进行纵向时间层面的状态空间采样，基于同一条路径形成多条不同时间序列的轨迹，与动态障碍物进行碰撞检测，得到车辆无碰撞速度数据，控制车辆避让动态障碍物；

2)若全部轨迹都发生碰撞，则此时切换成路径重规划，进行横向空间层面的状态空间采样，形成多条基于不同路径的轨迹，并与动态障碍物进行碰撞检测，得到车辆无碰撞行进路径和速度数据，控制车辆避让动态障碍物；

3)若全部轨迹都发生碰撞，则计算当前所处轨迹与动态障碍物碰撞的最近距离，基于当前所处轨迹减速停车。

优选的，速度重规划方法包括：

1)将正常速度规划产生的轨迹作为初始轨迹；

2)进行纵向的状态空间采样，形成同一空间，不同时间的多条车辆期望轨迹；

3)将多条车辆期望轨迹与动态障碍物轨迹进行碰撞检测；

碰撞检测优选三维时间空间碰撞检测；

4)判断碰撞检测结果，如存在无碰撞期望轨迹，则将无碰撞最优轨迹作为最终轨迹输出；如果不存在无碰撞期望轨迹，则进行路径重规划。

在速度重规划中，碰撞最优无轨迹的选取，按照纵向上终点时间与原期望轨迹偏差最小原则进行选取。

更具体的，由于纵向状态空间采样的终点状态为q_end(s,t)，基于约束迭代的速度规划在期望速度曲线确定前无法确定到达终点的最终时间，所以无法将终点状态q_end作为约束进行迭代计算。因此，速度重规划采用二次规划的优化方法进行速度曲线生成。与路径规划表示路径的曲线模型相似，可以将车辆的纵向运动表示为关于弧长关于时间的五次多项式：s＝f(t)＝a+bt+ct²+dt³+et⁴+ft⁵；

将问题转化为求解：Minimize

s.t.

1)初始状态：q_init＝(s_init,t_init,v_init)

2)末端状态：q_end＝(s_end,t_end)，

和分别代表加速度和冲击度，目标使其最小意味着让生成的速度曲线更加平缓。其中t_end可以根据正常速度规划产生的轨迹得到的t进行终点采样，t_end＝t±η_iε，生成多个不同t_end值。

3)上下限约束：在t_init和t_end间采样n个点:

a_min≤a_i≤a_max；

求解后得到[a b c d e f]^T即可得到速度曲线表达式。

优选的，路径重规划算法包括：

1)根据参考线，进行横向状态空间采样，得到终点状态集

2)根据正常路径规划法生成多条根据车辆当前位置到多个目标采样状态的路径；

3)将多条路径按照正常速度进行速度规划，形成多条轨迹，与动态障碍物轨迹进行碰撞检测；

4)如果存在无碰撞轨迹，则选取最优无碰撞轨迹，作为最终轨迹输出；如果不存在无碰撞期望轨迹，则计算当前所处轨迹与动态障碍物碰撞的最近距离，基于当前所处轨迹减速停车。

在路径重规划中，最优无碰撞轨迹的选取，按照与初规划轨迹(原期望轨迹)横向上距离偏差最小的原则进行选取。

如图8所示，为本实施例基于碰撞检测的动态障碍物避让方法更为细节的流程示意图。

以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。

Claims (10)

1.一种基于碰撞检测的动态障碍物避让方法，其特征在于，包括：

初规划步骤：在车辆行驶过程中对车辆的行驶路径和速度进行初步规划，得到车辆的期望路径、期望速度数据，构造车辆期望轨迹；

碰撞检测步骤：基于接收的动态障碍物预测轨迹和所述车辆期望轨迹，进行碰撞检测，判断车辆是否会与障碍物发生碰撞；

重规划步骤；当判断会发生碰撞后，对车辆的行驶速度和/或路径进行重新规划，得到车辆无碰撞行驶速度和/或路径数据，重新构造车辆期望轨迹，避让动态障碍物。

2.根据权利要求1所述的动态障碍物避让方法，其特征在于，所述碰撞检测为分层碰撞检测，包括二维空间碰撞检测和三维时间空间碰撞检测。

3.根据权利要求2所述的动态障碍物避让方法，其特征在于，

所述碰撞检测中的二维空间碰撞检测，根据车辆期望轨迹的路径数据和动态障碍物预测轨迹的路径数据，计算车辆与障碍物的最近距离，若最近距离小于碰撞距离阈值，则进行三维时间空间碰撞检测；否则，判断不会发生碰撞。

4.根据权利要求2所述的动态障碍物避让方法，其特征在于，所述碰撞检测中的三维时间空间碰撞检测包括：

构造三维时空轨迹；三维时空的x轴和y轴为路径点的横、纵坐标，z轴为到达每个路径点的时间；在三维时空中，根据动态障碍物的几何信息、预测轨迹数据以及车辆的几何信息、期望轨迹数据，得到动态障碍物预测轨迹和车辆期望轨迹的点阵序列；

粗略检测；根据动态障碍物预测轨迹和车辆期望轨迹的点阵序列，进行三维重构，形成以三维轨迹点为边界的动态障碍物三维轨迹凸体和车辆三维轨迹凸体，对两个三维轨迹凸体之间的冲突区域进行检测，得到粗略的发生碰撞的轨迹点；

精细检测；锁定冲突区域，进行精细阶段的碰撞检测，得到精确的发生碰撞的轨迹点。

5.根据权利要求1-4任一所述的动态障碍物避让方法，其特征在于，所述重规划步骤中，当判断会与动态障碍物轨迹发生碰撞时，

1)首先启动速度重规划，进行纵向时间层面的状态空间采样，基于同一条路径形成多条不同时间序列的轨迹，与动态障碍物进行碰撞检测，得到车辆无碰撞速度数据，控制车辆避让动态障碍物；

2)若全部轨迹都发生碰撞，则此时切换成路径重规划，进行横向空间层面的状态空间采样，形成多条基于不同路径的轨迹，并与动态障碍物进行碰撞检测，得到车辆无碰撞行进路径和速度数据，控制车辆避让动态障碍物；

3)若全部轨迹都发生碰撞，则计算当前所处轨迹与动态障碍物碰撞的最近距离，基于当前所处轨迹减速停车。

6.根据权利要求5所述的动态障碍物避让方法，其特征在于，所述粗略检测阶段，采用基于BVH的快速碰撞检测方法；所述精细检测阶段，采用GJK算法进行碰撞检测。

7.根据权利要求1所述的动态障碍物避让方法，其特征在于，所述初规划步骤中行驶路径的初步规划为根据车辆当前状态，生成一条从车辆当前位置到参考线的行驶曲线，具体包括：

1)给定车辆当前车辆状态q_init(x_init,y_init,θ_init,κ_init)、车辆终点状态q_end(x_end,y_end,θ_end,κ_end)；其中x_init,y_init为车辆当前的x，y坐标，θ_init为车辆当前航向角，κ_init为车辆当前轨迹曲率；其中x_end,y_end为车辆终点位于参考线上的x，y坐标，θ_end为车辆终点航向角，κ_end为车辆终点轨迹曲率；

2)将路径规划问题转化为解

为路径平滑目标函数，

s.t. q＝q_init,s＝0；

q＝q_end,s＝s_f；

0≤s_f≤s_max；s_max为曲线弧长的上界；

函数κ(p)的参数a为已知的车辆当前状态所对应的曲率，q为车辆状态；；通过求解得到函数κ(p)的参数b、c、d、s_f；

3)通过公式：使用不同的弧长s进行采样，得到路径规划的路径点集数据。

8.根据权利要求7所述的动态障碍物避让方法，其特征在于，所述初规划步骤中行驶速度的初步规划包括：

1)输入路径点每点最大限速ν_max、最大纵向加速度最大纵向减速度最大横向加速度曲率κ、最大冲击度j_max、收敛阈值ε；

2)对每个路径点限制横向加速度和纵向加、减速度：

3)根据相邻三个路径点进行冲击度计算，得到每个路径点的冲击度j_i；

4)判断冲击度j_i值，当冲击度j_i>j_max时，根据最大冲击度j_max重新确定该点的速度，当冲击度j_i<-j_amx时，根据最小冲击度-j_max重新确定该点的速度；得到周期速度序列V_f；

5)计算本周期速度序列V_f与上周期的差值，判断差值是否小于输入的阈值ε；如果小于则生成速度曲线输出；否则，返回2)再次进行速度迭代；其中初始周期的路径点速度值为其对应的最大速度值ν_max。

9.根据权利要求7所述的动态障碍物避让方法，其特征在于，所述速度重规划方法包括：

1)将初步规划产生的轨迹作为初始轨迹；

2)进行纵向的状态空间采样，形成同一空间，不同时间的多条车辆期望轨迹；

3)将多条车辆期望轨迹与动态障碍物轨迹进行碰撞检测；

4)判断碰撞检测结果，如存在无碰撞期望轨迹，则将无碰撞最优轨迹作为最终轨迹输出；如果不存在无碰撞期望轨迹，则进行路径重规划。

10.根据权利要求9所述的动态障碍物避让方法，其特征在于，所述路径重规划方法包括：

1)根据参考线，进行横向状态空间采样，得到终点状态集q_end(θ_end,κ_end,x_end,y_end)，

2)根据初步规划中的路径规划法生成多条根据车辆当前位置到多个目标采样状态的路径；

3)将多条路径按照初步规划中的速度规划法形成多条轨迹，与动态障碍物轨迹进行碰撞检测；

4)如果存在无碰撞轨迹，则选取最优无碰撞轨迹，作为最终轨迹输出；如果不存在无碰撞期望轨迹，则计算当前所处轨迹与动态障碍物碰撞的最近距离，基于当前所处轨迹减速停车。