CN109540159B - 一种快速完备的自动驾驶轨迹规划方法 - Google Patents

Abstract

本发明涉及一种快速完备的自动驾驶轨迹规划方法，包括以下步骤：1)规划基础路径；2)对基础行车路径进行碰撞检测，进行步骤3)，若不存在碰撞，则判断基础行车路径是否到达目标点，若是，则进行步骤4)，若否，则进行步骤3)；3)采用采样/搜索路径规划方法获取平滑的有效路径后，进行步骤4)；4)获取路径对应的曲率极值后进行分段速度规划，得到轨迹；5)在时间域和空间域对轨迹进行碰撞检测，若有碰撞，则判断车辆与障碍物的距离是否大于2倍最小制动距离，若是，则进行速度重规划并返回步骤4)，若否，则返回步骤1)，最终将路径与速度组合生成轨迹并输出，与现有技术相比，本发明具有提升轨迹规划的实时性，平稳可靠等优点。

Description

一种快速完备的自动驾驶轨迹规划方法

技术领域

本发明涉及城市结构化道路下的无人驾驶车辆的轨迹规划领域，尤其是涉及一种快速完备的自动驾驶轨迹规划方法。

背景技术

随着社会的发展和人民生活水平的不断提升，国内乘用车保有量从2000年到2017年的17年间激增13.5倍，未来乘用车保有量估计在4亿辆。然而，日趋严峻的能源危机、高负荷的交通压力以及社会对行车安全要求的日益增加，加速了无人驾驶技术落地的步伐。作为无人驾驶技术的核心之一，轨迹规划策略必须提升算法的实时性，以响应环境的变化，避免发生碰撞，造成不必要的人身与财产损失，同时，策略也必须提升算法的完备性，以适应交通的变化，提高舒适性，降低交通拥堵。

传统的自动驾驶轨迹规划策略，一般考虑使用采样方法(比如A*)或搜索方法(比如RRT)等这类具备解析完备性或者概率完备性算法，但是这类算法存在盲目搜索的过程，求解过程极其耗时且生成的轨迹是一段段的折线段，曲率并不连续，车辆难以执行；也有采用Dubins曲线、Reeds-Shepp曲线、样条曲线等这类具备快速性的规划方法，但是这类算法并不具备完备性，在环境相对复杂时，求解得到可行解的能力急剧下降，耗时也在不断上升。此外，在城市结构化道路条件下，轨迹规划的结果必须遵守交通规则，符合驾驶员的行为特性。

因此，如何提供一种解决上述问题的泊车策略和***是本领域技术人员亟待解决的问题。

发明内容

本发明的目的就是为了克服上述现有技术存在的缺陷而提供一种快速完备的自动驾驶轨迹规划方法。

本发明的目的可以通过以下技术方案来实现：

一种快速完备的自动驾驶轨迹规划方法，包括以下步骤：

1)规划基础路径，获取仿驾驶员行为的基础行车路径；

2)对基础行车路径进行碰撞检测，若存在碰撞，则记录碰撞发生在基础行车路径的位置，进行步骤3)，若不存在碰撞，则判断基础行车路径是否到达目标点，若是，则进行步骤4)，若否，则进行步骤3)；

3)采用采样/搜索路径规划方法获取平滑的有效路径后，进行步骤4)；

4)采用速度规划方法获取路径的曲率曲线，并获取路径对应的曲率极值后进行分段速度规划，得到轨迹；

5)在时间域和空间域对轨迹进行碰撞检测，若无碰撞，则判断车辆与障碍物的距离是否大于2倍最小制动距离，若是，则进行速度重规划并返回步骤4)，若否，则返回步骤1)，最终将路径与速度组合生成轨迹并输出。

所述的步骤1)具体包括以下步骤：

11)获取车辆关键参数，包括车辆结构参数、车辆执行器性能参数和道路特征参数，其中，车辆结构参数包括轴距、轮距、车长、车宽、车重和质心位置，车辆执行执行器性能参数包括最大车速、最小车速、最大加速度和最小加速度，道路特征参数包括路面附着系数和路面粗糙度；

12)根据车辆关键参数计算曲率限值，该曲率限值取为最小转弯半径限值的曲率、侧向最大侧向加速度限值的最大曲率和路面附着系数限值的最大曲率三者的最小值；

13)根据用户需求或者自适应决策判断是否进行高舒适性轨迹规划：

131)当舒适性要求不高，即道路限制车速不大于20km/h时，分别进行直行、换道/并道、转弯和调头四种驾驶行为导向的仿驾驶员行为路径控制点求取，然后用B样条曲线或者贝塞尔曲线对控制点进行拟合，最终得到平滑的基础行车路径；

132)当舒适性要求高时，即道路限制车速大于20km/h时，分别进行转弯和调头两种驾驶行为导向的路径控制点求取，然后用螺旋线片段和圆弧路径片段拟合控制点，最终得到平滑的基础行车路径。

所述的步骤3)具体包括以下步骤：

31)将基础行车路径进行裁剪，得到有效路径；

32)将有效路径或者有效节点更新到路径树当中，所述的路径树为动态kd树，树中节点的位置调整将综合归一化的欧氏距离和累积航向角偏差实现；

33)判断采样/搜索的节点扩展次数是否达到设定阈值，若是，则进行步骤37)，若否，则进行步骤34)；

34)选择采样法或者搜索法扩展节点；

35)对扩展节点及其与父节点的连接线进行碰撞检测，若存在碰撞，则该节点不存入路径树当中，并返回步骤33)；

36)判断节点是否扩展到目标点，若否，则返回步骤32)，若是，则进行37)；

37)取出路径树当中有效的节点，用B样条曲线或者贝塞尔曲线对控制点进行拟合，得到平滑的有效路径。

所述的步骤131)中，根据曲率限值获取仿驾驶员行为路径控制点，控制点满足以下约束：

其中，L为两控制点间的欧氏距离，α为控制点间夹角，k_limit为曲率限值。

所述的步骤131)中，

根据驾驶行为导向的直行路径由n1段控制直线段组成，直线段满足以下约束：

其中，l_end-start1为规划终点到规划起点的欧氏距离，l_i1为第i1段直线段长度，α_i1为第i1段直线段的控制点间夹角；

根据驾驶行为确定的换道/并道路径由n2段控制直线段组成，直线段满足以下约束：

其中，l_end-start2为规划终点到规划起点的欧氏距离，l_i2为第i2段直线段长度，α_i2为第i2段直线段的控制点间夹角；

根据驾驶行为确定的转弯路径由n3段控制直线段组成，直线段满足以下约束：

不借道的转弯约束：

左借道的转弯约束：

向上借道的转弯约束：

其中，θ_j3是起点航向角，θ_g3是终点航向角，x_g3、y_g3为终点坐标，x_j3、y_j3为起点坐标，l_i3为第i3段直线段长度，α_i3为第i3段直线段的控制点间夹角；

根据驾驶行为确定的调头路径由n4段控制直线段组成，直线段满足以下约束：

不借道的转弯约束：

左借道的转弯约束：

右借道的转弯约束：

左右均借道的转弯约束：

其中，θ_j4是起点航向角，θ_g4是终点航向角，x_g4、y_g4为终点坐标，x_j4、y_j4为起点坐标，l_i4为第i4段直线段长度，α_i4为第i4段直线段的控制点间夹角。

所述的步骤132)中，根据驾驶行为改进的转弯和调头路径采用一段圆弧和两段螺旋线组成，满足以下约束：

第一条螺旋线：

a₁s²+b₁s+c₁＝k₁

c₁＝k_start

第二条螺旋线：

a₂s²+b₂s+c₂＝k₂

c₂＝k_end

圆弧曲线：

x₁＝R_x-Rsinθ₁

y＝R_y+Rcosθ₁

x₂＝R_x-Rsinθ₂

y₂＝R_y+Rsinθ₂

其中，s为路径长度，s₁为第一段螺旋线长度，s₂为第二段螺旋线长度，a₁、b₁、c₁为第一段螺旋线待求特征参数，a₂、b₂、c₂为第二段螺旋线待求特征参数，k₁为第一段螺旋线曲率，k₂为第二段螺旋线曲率，k_start为起点曲率，k_end为终点曲率，θ_i为起点航向，θ_end为终点航向，θ₁为第一段螺旋线终点航向，θ₂为第二段螺旋线起点航向，R为圆弧半径，R_x，R_y为圆弧圆心坐标，x₁，y₁圆弧起点坐标，x₂，y₂圆弧终点坐标，旋线的积分采用辛普森公式求解，以降低计算复杂度，提升算法实时性。

所述的步骤31)中，路径裁剪具体为：

通过碰撞检测到的碰撞点，沿基础路径回溯到最近的一个未碰撞控制点，该未碰撞控制点作为采样/搜索路径规划的起点，且该未碰撞控制点到基础路径规划的起点间的平滑路径作为裁剪得到的有效路径。

所述的步骤3)中，采样/搜索路径规划的切换方式包括用户设定以及由轨迹规划的策略依据环境的复杂度进行自适应选择，当环境中存在较多障碍物或者环境地图较大时，采用RRT进行快速的扩展，当环境中存在较简单或者环境地图较小时，采用A*进行启发式搜索。

所述的步骤34)中，

采样法为改进RRT方法，其节点扩展方式包括强制扩展、驾驶行为偏向性扩展和随机扩展三种方式，三种扩展方式具体的自适应选择方法如下：

在环境单一以及成功率高于设定阈值时，采用强制扩展；在多次强制扩展的成功率小于设定阈值时，则采用驾驶行为偏向性扩展；在成功率较低和环境复杂度较大时，采用标准扩展方式；

所述的强制扩展是直接将目标点作为扩展的节点存入路径树当中，并进行碰撞检测，验证扩展节点的有效性，有效则保留，无效则删除，所述的驾驶行为偏向性扩展首先根据驾驶行为，将扩展域用扇形标注，并在适当的位置生成中继节点集、路口节点集和目标节点集，种子点将采用高斯随机形式从扇形标注域内选取，节点存入路径树当中，并进行碰撞检测，验证扩展节点的有效性，有效则保留，无效则删除，所述的标准扩展首先根据驾驶行为，将扩展域用矩形或者矩形拼接的规则多边形标注，并在适当的位置生成中继节点集、路口节点集和目标节点集，种子点将采用标准随机形式从标注域内选取，节点存入路径树当中，并进行碰撞检测，验证扩展节点的有效性，有效则保留，无效则删除。

搜索法采用改进A*方法，其节点扩展方式包括驾驶行为偏向性搜索、转向角约束搜索和标准搜索三种方式，三种扩展方式具体的自适应选择方法如下：

在环境单一以及成功率高于设定阈值时，采用驾驶行为偏向性搜索的扩展；在多次驾驶行为偏向性搜索的成功率小于设定阈值时，则采用转向角约束搜索的扩展；在成功率较低和环境复杂度较大时，采用标准搜索的扩展方式。

所述的驾驶行为偏向性搜索的扩展，将扩展域用扇形标注，并在适当的位置生成中继节点集、路口节点集和目标节点集，节点扩展将满足车辆的最小转弯半径约束，共220个路径基的方向进行，扩展的节点存入路径树当中，并进行碰撞检测，验证扩展节点的有效性，有效则保留，无效则删除，所述的转向角约束搜索的扩展，将扩展域用矩形或者矩形拼接的规则多边形标注，并在适当的位置生成中继节点集、路口节点集和目标节点集，节点扩展将满足车辆的最小转弯半径约束，共220个路径基的方向进行，扩展的节点存入路径树当中，并进行碰撞检测，验证扩展节点的有效性，有效则保留，无效则删除，所述的标准搜索的扩展，将扩展域用矩形或者矩形拼接的规则多边形标注，并在适当的位置生成中继节点集、路口节点集和目标节点集，节点扩展将沿着垂向和纵向共20个路径基的方向进行，扩展的节点存入路径树当中，并进行碰撞检测，验证扩展节点的有效性，有效则保留，无效则删除。

所述的步骤4)中，曲率极值分段速度规划在基础路径的两个曲率极值点间进行匀速的速度规划，极值点外采用受路面附着约束和执行器性能约束的加减速速度规划，

或者在基础路径和采样/搜索的路径的四个曲率极值点的前后两点间进行匀速的速度规划，极值点外采用受路面附着约束和执行器性能约束的加减速速度规划。

所述的采样/搜索路径规划的切换方式可以是用户设定，也可以交由轨迹规划的策略依据环境的复杂度进行自适应选择，在环境中存在较多障碍物或者环境地图较大时，采用RRT进行快速的扩展；在环境中存在较简单或者环境地图较小时，采用A*进行启发式搜索。

所述的步骤5)中，时间域和空间域碰撞检测是以速度代表时间域，路径代表空间域构筑三维坐标系，并分别以传感器的探测范围、道路边界为空间域上下限，以道路限速、路面条件限速和车辆执行器性能限速为时间域上下限；

所述的步骤5)中，重规划决策具体为对碰撞点以及碰撞时间是否在安全可规划域内的基础上，结合环境复杂度和交通规则决策出是否优先考虑速度规划。

与现有技术相比，本发明具有以下优点：

本发明提供了一种驾驶行为导向的轨迹规划方法，适用于城市结构化道路，减少了规划过程的盲目性，降低了轨迹规划的耗时，生成的轨迹符合驶员的行为特性，自动驾驶过程更加平稳舒适。

一、本发明规划的轨迹综合考虑了车辆动力学约束、车辆执行器性能约束和路面条件，采用B样条或者贝赛尔曲线平滑轨迹，生成的轨迹曲率连续，可执行性强。

二、本发明设计了一种仿驾驶员行为的基础路径规划方法，将驾驶行为划分为直行、换道/并道、转弯和调头，使规划过程更加简洁，降低了算法的复杂度，实时性高。

三、本发明设计了一种改进采样规划和改进搜索方法，通过与基础路径方法形成互补，使轨迹规划方法具有完备性，轨迹规划结果可靠稳定。

四、本发明设计了一种曲率极值分段的速度规划方法，该方法与路径规划解耦，实现路径规划与速度规划的模块化，同时，前后路径的速度规划具有可移植性，速度规划算法复杂度低，实时性强。

五、本发明在轨迹规划具体实施过程中，考虑了环境的复杂程度、车辆状态、驾驶员需求以及算法成功率，对算法设置了多个自适应环节，策略对环境和驾驶员需求的适应能力强。

附图说明

图1为本发明的具备快速性与完备性的自动驾驶轨迹规划策略流程图；

图2为本发明的轨迹规划策略以驾驶行为为导向的路径与改机RRT融合的具体实施流程图；

图3为本发明的轨迹规划策略以驾驶行为为导向的路径与改进A*融合的具体实施流程图；

图4为本发明的换道/并道驾驶行为导向的路径控制点和平滑后的路径示意图。其中，图(4a)为换道/并道驾驶行为导向的路径控制点构造的控制直线段示意图，图(4b)为换道/并道驾驶行为导向的路径控制点构造的控制直线段经B样条曲线平滑后的示意图；

图5为本发明的一种转弯驾驶行为导向的路径控制点和平滑后的路径示意图。其中，图(5a)为不借道转弯驾驶行为导向的路径控制点构造的控制直线段示意图，图(5b)为不借道转弯驾驶行为导向的路径控制点构造的控制直线段经B样条曲线平滑后的示意图；

图6为本发明的一种转弯驾驶行为导向的路径控制点和平滑后的路径示意图。其中，图(6a)为左借道转弯驾驶行为导向的路径控制点构造的控制直线段示意图，图(6b)为左借道转弯驾驶行为导向的路径控制点构造的控制直线段经B样条曲线平滑后的示意图；

图7为本发明的一种转弯驾驶行为导向的路径控制点和平滑后的路径示意图。其中，图(7a)为上借道转弯驾驶行为导向的路径控制点构造的控制直线段示意图，图(7b)为上借道转弯驾驶行为导向的路径控制点构造的控制直线段经B样条曲线平滑后的示意图；

图8为本发明的一种调头驾驶行为导向的路径控制点和平滑后的路径示意图。其中，图(8a)为不借道调头驾驶行为导向的路径控制点构造的控制直线段示意图，图(8b)为不借道调头驾驶行为导向的路径控制点构造的控制直线段经B样条曲线平滑后的示意图；

图9为本发明的一种调头驾驶行为导向的路径控制点和平滑后的路径示意图。其中，图(9a)为左借道调头驾驶行为导向的路径控制点构造的控制直线段示意图，图(9b)为左借道调头驾驶行为导向的路径控制点构造的控制直线段经B样条曲线平滑后的示意图；

图10为本发明的一种调头驾驶行为导向的路径控制点和平滑后的路径示意图。其中，图(10a)为右借道调头驾驶行为导向的路径控制点构造的控制直线段示意图，图(10b)为右借道调头驾驶行为导向的路径控制点构造的控制直线段经B样条曲线平滑后的示意图；

图11为本发明的一种调头驾驶行为导向的路径控制点和平滑后的路径示意图；其中，图(11a)为左右均借道调头驾驶行为导向的路径控制点构造的控制直线段示意图，图(11b)为左右均借道调头驾驶行为导向的路径控制点构造的控制直线段经B样条曲线平滑后的示意图；

图12为本发明的一种改进的转弯和调头驾驶行为导向的平滑路径示意图。其中，图(12a)为改进的转弯驾驶行为导向的平滑路径示意图，图(12b)为改进的调头驾驶行为导向的平滑路径示意图；

图13为本发明的换道/并道驾驶行为为导向的路径裁剪得到有效路径的示意图；

图14为本发明的直行和换道/并道驾驶行为为导向的三种采样/搜索空间示意图。其中，图(14a)为直行和换道/并道驾驶行为场景示意图，图(14b)为直行和换道/并道驾驶行为的随机扩展/标准搜索示意图，图(14c)为直行和换道/并道驾驶行为的偏向性扩展/转角约束搜索/偏向性搜索示意图，图(14d)为直行和换道/并道驾驶行为的强制扩展示意图；

图15为本发明的转弯驾驶行为为导向的环境示意图。

图16为本发明的转弯驾驶行为为导向的一种采样/搜索空间示意图；

图17为本发明的转弯驾驶行为为导向的一种采样/搜索空间示意图；

图18为本发明的调头驾驶行为为导向的环境示意图；

图19为本发明的调头驾驶行为为导向的一种采样/搜索空间示意图；

图20为本发明的调头驾驶行为为导向的一种采样/搜索空间示意图；

图21为本发明的一种节点扩展示意图；

图22为本发明的一种节点扩展示意图；

图23为本发明的一种节点扩展示意图；

图24为本发明的一种路径曲率示意图；

图25为本发明的一种速度规划曲线示意图；

图26为本发明的一种路径曲率示意图；

图27为本发明的一种速度规划曲线示意图；

图28为本发明的一种速度规划曲线示意图。

具体实施方式

下面结合附图和具体实施例对本发明进行详细说明。

实施例

图1为本发明的具备快速性与完备性的自动驾驶轨迹规划策略的流程图，图2和图3为本发明的具备快速性与完备性的自动驾驶轨迹规划策略的两种不同实现的流程图。该方法具体步骤包括：

步骤1：如图1所示，获取车辆关键参数，包括车辆结构参数、车辆执行器性能参数和道路特征参数。其中，车辆结构参数包括轴距、轮距、车长、车宽、车重、质心位置等，车辆执行执行器性能参数包括最大车速、最小车速、最大加速度、最小加速度等，道路特征参数包括路面附着系数、路面粗糙度等；

步骤2：如图1所示，根据车辆关键参数计算曲率限值，曲率限值＝min{最小转弯半径限值的曲率，侧向最大侧向加速度限值的最大曲率，路面附着系数限值的最大曲率}；

步骤3：如图1所示，根据用户的需求，或者以当前车速、道路曲率变化等为自适应条件选择是否需要进行高舒适性轨迹规划，是则转入步骤31，否则转入步骤32：

步骤31：分别进行直行、换道/并道、转弯和调头四种驾驶行为导向的路径控制点求取；

步骤311：根据步骤2的曲率限值计算控制点，控制点需要满足的约束方程为

其中L为两控制点间的欧氏距离，α为控制点间夹角，k_limit为曲率限值。根据驾驶行为决策分别转入步骤312、步骤313、步骤314或步骤315；

步骤312：直线路径采用n1段长度的控制直线段组成，直线段需要满足的约束方程为：

其中，l_end-start1是规划终点到规划起点的欧氏距离，l_i1为第i1段直线段长度，α_i1第i1段直线段的控制点间夹角。

步骤313：如图4所示，换道/并道路径采用n2段长度的控制直线段组成，直线段需要满足的约束方程为：

其中，l_end-start2是规划终点到规划起点的欧氏距离，l_i2为第i2段直线段长度，α_i2第i2段直线段的控制点间夹角。

步骤314：如图5-图7所示，转弯路径采用n3段长度的控制直线段组成，直线段需要满足的约束方程为：

不借道的转弯约束：

左借道的转弯约束：

向上借道的转弯约束：

其中，θ_j3是起点航向角，θ_g3是终点航向角，x_g3、y_g3为终点坐标，x_j3、y_j3为起点坐标，l_i3为第i3段直线段长度，α_i3第i3段直线段的控制点间夹角；

步骤315：如图8-图11所示，调头路径采用n段长度的控制直线段组成，直线段需要满足的约束方程为：

不借道的调头约束：

左借道的调头约束：

右借道的调头约束：

左右均借道的调头约束：

其中，θ_j4是起点航向角，θ_g4是终点航向角，x_g4、y_g4为终点坐标，x_j4、y_j4为起点坐标，l_i4为第i4段直线段长度，α_i4第i4段直线段的控制点间夹角。

步骤32：在舒适性要求高时转入该步骤，根据行为决策结果，分别进行转弯和调头两种驾驶行为导向的路径控制点求取，然后用螺旋线片段和圆弧路径片段拟合控制点，最终得到平滑的基础行车路径；

步骤321：如图12所示，改进的转弯和调头路径采用一段圆弧和两段螺旋线组成，需要满足的约束方程为:

第一条螺旋线

a₁s²+b₁s+c₁＝k₁

c₁＝k_start

第二条螺旋线

a₂s²+b₂s+c₂＝k₂

c₂＝k_end

圆弧曲线

x₁＝R_x-Rsinθ₁

y＝R_y+Rcosθ₁

x₂＝R_x-Rsinθ₂

y₂＝R_y+Rsinθ₂

其中，s为路径长度，s₁为第一段螺旋线长度，s₂为第二段螺旋线长度，a₁，b₁，c₁为第一段螺旋线待求特征参数，a₂，b₂，c₂为第二段螺旋线待求特征参数，k₁为第一段螺旋线曲率，k₂为第二段螺旋线曲率，k_start为起点曲率，k_end为终点曲率，_i为起点航向，_end为终点航向，₁为第一段螺旋线终点航向，₂为第二段螺旋线起点航向，R为圆弧半径，R_x，R_y为圆弧圆心坐标，x₁，y₁圆弧起点坐标，x₂，y₂圆弧终点坐标。

步骤322：螺旋线的积分采用辛普森公式求解，以降低计算复杂度，提升算法实时性。

步骤4：对基础行车路径进行碰撞检测，确定是否存在碰撞，若有碰撞，则记录碰撞发生在基础行车路径的位置，然后跳转到步骤6。；

步骤5：判断基础行车路径是否到达目标点，如果到达目标点，跳转到步骤13；

步骤6：如图13所示，将基础行车路径进行裁剪，得到有效路径。路径裁剪是通过碰撞检测到的碰撞点，沿基础路径回溯到最近的一个未碰撞控制点，该未碰撞控制点作为采样/搜索路径规划的起点，且该未碰撞控制点到基础路径规划的起点间的平滑路径作为裁剪得到的有效路径；

步骤7：将得到的有效路径或者有效节点更新到路径树当中。路径树是一个动态的kd树，树中节点的位置调整将综合归一化的欧氏距离和累积航向角偏差实现；

步骤8：判断采样/搜索的节点扩展次数是否达到设定阈值，是则跳转到步骤12；

步骤9：选择采样法或者搜索法扩展节点。采样/搜索路径规划的切换方式可以是用户设定，也可以交由轨迹规划的策略依据环境的复杂度进行自适应选择，在环境中存在较多障碍物或者环境地图较大时，采用RRT进行快速的扩展，执行步骤91；在环境中存在较简单或者环境地图较小时，采用A*进行启发式搜索，跳转到步骤92；

步骤91：采样法是一个改进了的RRT，其节点扩展方式有强制扩展、驾驶行为偏向性扩展、随机扩展三种方式：

改进了的RRT节点扩展方式选择是自适应地，综合节点扩展的成功率、环境复杂度以及随机种子概率要素，在环境单一以及成功率高于某阈值时，采用强制扩展；在多次强制扩展的成功率小于某阈值时，则采用驾驶行为偏向性扩展；在成功率较低和环境复杂度较大时，采用标准扩展方式。

强制扩展是直接将目标点作为扩展的节点存入路径树当中，并进行碰撞检测，验证扩展节点的有效性，有效则保留，无效则删除。

驾驶行为偏向性扩展首先根据驾驶行为，将扩展域用扇形标注，如图14、图16和图19所示，并在适当的位置生成中继节点集、路口节点集和目标节点集，种子点将采用高斯随机形式从扇形标注域内选取，节点存入路径树当中，并进行碰撞检测，验证扩展节点的有效性，有效则保留，无效则删除。

标准扩展首先根据驾驶行为，将扩展域用矩形或者矩形拼接的规则多边形标注，如图14、图17和图20所示，，并在适当的位置生成中继节点集、路口节点集和目标节点集，种子点将采用标准随机形式从标注域内选取，节点存入路径树当中，并进行碰撞检测，验证扩展节点的有效性，有效则保留，无效则删除。

步骤92：搜索法是一个改进了的A*，其节点扩展方式有驾驶行为偏向性搜索、转向角约束搜索、标准搜索三种方式：

改进了的A*节点扩展方式选择是自适应地，综合节点扩展的累计成功率、环境复杂度要素，在环境单一以及成功率高于某阈值时，采用驾驶行为偏向性搜索的扩展；在多次驾驶行为偏向性搜索的成功率小于某阈值时，则采用转向角约束搜索的扩展；在成功率较低和环境复杂度较大时，采用标准搜索的扩展方式。

驾驶行为偏向性搜索的扩展，将扩展域用扇形标注，如图14、图16和图19所示，，并在适当的位置生成中继节点集、路口节点集和目标节点集，节点扩展将满足车辆的最小转弯半径约束，共220个路径基的方向进行，如图21-图23所示，扩展的节点存入路径树当中，并进行碰撞检测，验证扩展节点的有效性，有效则保留，无效则删除。

转向角约束搜索的扩展，将扩展域用矩形或者矩形拼接的规则多边形标注，如图14-图20所示，并在适当的位置生成中继节点集、路口节点集和目标节点集，节点扩展将满足车辆的最小转弯半径约束，共220个路径基的方向进行，如图21-图23所示，扩展的节点存入路径树当中，并进行碰撞检测，验证扩展节点的有效性，有效则保留，无效则删除。

标准搜索的扩展，将扩展域用矩形或者矩形拼接的规则多边形标注，如图14-图20所示，并在适当的位置生成中继节点集、路口节点集和目标节点集，节点扩展将沿着垂向和纵向共20个路径基的方向进行，如图21所示，扩展的节点存入路径树当中，并进行碰撞检测，验证扩展节点的有效性，有效则保留，无效则删除。

步骤10：对扩展节点及其与父节点的连接线进行碰撞检测，在有碰撞时，该节点不存入路径树当中，并跳转回步骤8；

步骤11：判断节点是否扩展到目标点，如果没有，则跳转到步骤7；

步骤12：取出路径树当中有效的节点，用B样条曲线或者贝塞尔曲线对控制点进行拟合，最终得到平滑的有效路径；

步骤13：求解路径的曲率曲线，并查找到路径的曲率极值(极大值和极小值)，然后进行分段速度规划，得到轨迹：

如图24-图25所示，曲率极值分段速度规划将在在基础路径的两个曲率极值点间进行匀速的速度规划，极值点外采用受路面附着约束和执行器性能约束的加减速速度规划；

如图26-图28所示，曲率极值分段速度规划将在在基础路径和采样/搜索的路径的四个曲率极值点的前后两点间进行匀速的速度规划，极值点外采用受路面附着约束和执行器性能约束的加减速速度规划。

步骤14：在时间域和空间域对轨迹进行碰撞检测，如果无碰撞，则跳转到步骤16。时间域和空间域碰撞检测是以速度代表时间域，路径代表空间域构筑三维坐标系，并分别以传感器的探测范围、道路边界为空间域上下限，以道路限速、路面条件限速和车辆执行器性能限速为时间域上下限；

步骤15：进行重规划决策，如果进行速度规划，则跳转回步骤13，如果进行不进行速度重规划，则跳转回步骤2。重规划决策是在步骤14的基础上对碰撞点以及碰撞时间是否在安全可规划域内的基础上，结合环境复杂度和交通规则决策出是否优先考虑速度规划。；

步骤16：将路径与速度组合成轨迹，并输出。

本发明提供了一种具备快速性与完备性的自动驾驶轨迹规划策略，策略以驾驶员行为为导向，将基础路径规划策略、采样/搜索路径规划策略、碰撞检测策略、速度规划策略和重规划策略五部分有机组合。基础路径规划策略和采样/搜索路径规划策略均以驾驶行为为导向，实现算法的加速，同时策略的有机组合保障了算法的完备性，确保路径存在的情况下，该策略能够输出有效轨迹。其中基础路径规划策略首先需要获取关键参数(车辆结构参数、车辆执行器性能参数和道路特征参数)；其次进行路径曲率限值计算；然后根据舒适性要求选取的驾驶行为导向的基础路径规划或者改进的驾驶行为导向的基础路径规划；最后输出基础行车路径。采样/搜索路径规划策略是在基础路径规划策略难以满足需求(碰撞检测不通过或者路径未到达目标点)时的补充策略，首先将基础行车路径裁剪得等到有效路径并更新路径树；其次，进行采样/搜索节点扩展；然后，依次进行碰撞检测和停止检测(到达目标点或者规划次数限制)；最后，对有效部分路径进行平滑。速度规划策略采用曲率极值分段速度规划。重规划决策策略用以决策是进行路径重规划还是速度重规划。本发明将驾驶行为用以引导基础路径规划，并融入采样/搜索规划策略，极大提升轨迹规划的实时性和算法的完备性，生成的轨迹符合人类驾驶员特性，自动驾驶过程更加平稳与可靠。

以上所述仅是本发明的优选实施方式，应当指出：对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。

Claims (10)

1.一种快速完备的自动驾驶轨迹规划方法，其特征在于，包括以下步骤：

1)规划基础路径，获取仿驾驶员行为的基础行车路径；

2)对基础行车路径进行碰撞检测，若存在碰撞，则记录碰撞发生在基础行车路径的位置，进行步骤3)，若不存在碰撞，则判断基础行车路径是否到达目标点，若是，则进行步骤4)，若否，则进行步骤3)；

3)采用采样/搜索路径规划方法获取平滑的有效路径后，进行步骤4)；

4)采用速度规划方法获取路径的曲率曲线，并获取路径对应的曲率极值后进行分段速度规划，得到轨迹；

5)在时间域和空间域对轨迹进行碰撞检测，若有碰撞，则判断车辆与障碍物的距离是否大于2倍最小制动距离，若是，则进行速度重规划并返回步骤4)，若否，则返回步骤1)，最终将路径与速度组合生成轨迹并输出。

2.根据权利要求1所述的一种快速完备的自动驾驶轨迹规划方法，其特征在于，所述的步骤1)具体包括以下步骤：

11)获取车辆关键参数，包括车辆结构参数、车辆执行器性能参数和道路特征参数，其中，车辆结构参数包括轴距、轮距、车长、车宽、车重和质心位置，车辆执行执行器性能参数包括最大车速、最小车速、最大加速度和最小加速度，道路特征参数包括路面附着系数和路面粗糙度；

12)根据车辆关键参数计算曲率限值，该曲率限值取为最小转弯半径限值的曲率、侧向最大侧向加速度限值的最大曲率和路面附着系数限值的最大曲率三者的最小值；

13)根据用户需求或者自适应决策判断是否进行高舒适性轨迹规划：

131)当舒适性要求不高，即道路限制车速不大于20km/h时，分别进行直行、换道/并道、转弯和调头四种驾驶行为导向的仿驾驶员行为路径控制点求取，然后用B样条曲线或者贝塞尔曲线对控制点进行拟合，最终得到平滑的基础行车路径；

132)当舒适性要求高时，即道路限制车速大于20km/h时，分别进行转弯和调头两种驾驶行为导向的路径控制点求取，然后用螺旋线片段和圆弧路径片段拟合控制点，最终得到平滑的基础行车路径。

3.根据权利要求1所述的一种快速完备的自动驾驶轨迹规划方法，其特征在于，所述的步骤3)具体包括以下步骤：

31)将基础行车路径进行裁剪，得到有效路径；

32)将有效路径或者有效节点更新到路径树当中，所述的路径树为动态kd树；

33)判断采样/搜索的节点扩展次数是否达到设定阈值，若是，则进行步骤37)，若否，则进行步骤34)；

34)选择采样法或者搜索法扩展节点；

35)对扩展节点及其与父节点的连接线进行碰撞检测，若存在碰撞，则该节点不存入路径树当中，并返回步骤33)；

36)判断节点是否扩展到目标点，若否，则返回步骤32)，若是，则进行37)；

37)取出路径树当中有效的节点，用B样条曲线或者贝塞尔曲线对控制点进行拟合，得到平滑的有效路径。

4.根据权利要求2所述的一种快速完备的自动驾驶轨迹规划方法，其特征在于，所述的步骤131)中，根据曲率限值获取仿驾驶员行为路径控制点，控制点满足以下约束：

其中，L为两控制点间的欧氏距离，α为控制点间夹角，k_limit为曲率限值。

5.根据权利要求4所述的一种快速完备的自动驾驶轨迹规划方法，其特征在于，所述的步骤131)中，

根据驾驶行为导向的直行路径由n1段控制直线段组成，直线段满足以下约束：

其中，l_end-start1为规划终点到规划起点的欧氏距离，l_i1为第i1段直线段长度，α_i1为第i1段直线段的控制点间夹角；

根据驾驶行为确定的换道/并道路径由n2段控制直线段组成，直线段满足以下约束：

其中，l_end-start2为规划终点到规划起点的欧氏距离，l_i2为第i2段直线段长度，α_i2为第i2段直线段的控制点间夹角；

根据驾驶行为确定的转弯路径由n3段控制直线段组成，直线段满足以下约束：

不借道的转弯约束：

左借道的转弯约束：

向上借道的转弯约束：

其中，θ_j3是起点航向角，θ_g3是终点航向角，x_g3、y_g3为终点坐标，x_j3、y_j3为起点坐标，l_i3为第i3段直线段长度，α_i3为第i3段直线段的控制点间夹角；

根据驾驶行为确定的调头路径由n4段控制直线段组成，直线段满足以下约束：

不借道的转弯约束：

左借道的转弯约束：

右借道的转弯约束：

左右均借道的转弯约束：

其中，θ_j4是起点航向角，θ_g4是终点航向角，x_g4、y_g4为终点坐标，x_j4、y_j4为起点坐标，l_i4为第i4段直线段长度，α_i4为第i4段直线段的控制点间夹角。

6.根据权利要求4所述的一种快速完备的自动驾驶轨迹规划方法，其特征在于，所述的步骤132)中，根据驾驶行为改进的转弯和调头路径采用一段圆弧和两段螺旋线组成，满足以下约束：

第一条螺旋线：

a₁s²+b₁s+c₁＝k₁

c₁＝k_start

第二条螺旋线：

a₂s²+b₂s+c₂＝k₂

c₂＝k_end

圆弧曲线：

x₁＝R_x-Rsinθ₁

y＝R_y+Rcosθ₁

x₂＝R_x-Rsinθ₂

y₂＝R_y+Rsinθ₂

其中，s为路径长度，s₁为第一段螺旋线长度，s₂为第二段螺旋线长度，a₁、b₁、c₁为第一段螺旋线待求特征参数，a₂、b₂、c₂为第二段螺旋线待求特征参数，k₁为第一段螺旋线曲率，k₂为第二段螺旋线曲率，k_start为起点曲率，k_end为终点曲率，θ_i为起点航向，θ_end为终点航向，θ₁为第一段螺旋线终点航向，θ₂为第二段螺旋线起点航向，R为圆弧半径，R_x，R_y为圆弧圆心坐标，x₁，y₁圆弧起点坐标，x₂，y₂圆弧终点坐标。

7.根据权利要求3所述的一种快速完备的自动驾驶轨迹规划方法，其特征在于，所述的步骤31)中，路径裁剪具体为：

通过碰撞检测到的碰撞点，沿基础路径回溯到最近的一个未碰撞控制点，该未碰撞控制点作为采样/搜索路径规划的起点，且该未碰撞控制点到基础路径规划的起点间的平滑路径作为裁剪得到的有效路径。

8.根据权利要求3所述的一种快速完备的自动驾驶轨迹规划方法，其特征在于，所述的步骤3)中，采样/搜索路径规划的切换方式包括用户设定以及由轨迹规划的策略依据环境的复杂度进行自适应选择，当环境中存在较多障碍物或者环境地图较大时，采用RRT进行快速的扩展，当环境中存在较简单或者环境地图较小时，采用A*进行启发式搜索。

9.根据权利要求8所述的一种快速完备的自动驾驶轨迹规划方法，其特征在于，所述的步骤34)中，

采样法为改进RRT方法，其节点扩展方式包括强制扩展、驾驶行为偏向性扩展和随机扩展三种方式，三种扩展方式具体的自适应选择方法如下：

在环境单一以及成功率高于设定阈值时，采用强制扩展；在多次强制扩展的成功率小于设定阈值时，则采用驾驶行为偏向性扩展；在成功率较低和环境复杂度较大时，采用标准扩展方式；

搜索法采用改进A*方法，其节点扩展方式包括驾驶行为偏向性搜索、转向角约束搜索和标准搜索三种方式，三种扩展方式具体的自适应选择方法如下：

在环境单一以及成功率高于设定阈值时，采用驾驶行为偏向性搜索的扩展；在多次驾驶行为偏向性搜索的成功率小于设定阈值时，则采用转向角约束搜索的扩展；在成功率较低和环境复杂度较大时，采用标准搜索的扩展方式。

10.根据权利要求1所述的一种快速完备的自动驾驶轨迹规划方法，其特征在于，所述的步骤4)中，曲率极值分段速度规划在基础路径的两个曲率极值点间进行匀速的速度规划，极值点外采用受路面附着约束和执行器性能约束的加减速速度规划，

或者在基础路径和采样/搜索的路径的四个曲率极值点的前后两点间进行匀速的速度规划，极值点外采用受路面附着约束和执行器性能约束的加减速速度规划。

Images