一种适用于环形交叉口的自动驾驶行为决策系统与运动规划方法

技术领域

本发明涉及自动驾驶技术领域，尤其涉及环形交叉口场景下的自动驾驶行为决策系统与运动规划方法。

背景技术

自动驾驶技术与智能交通系统的自动化能力不断发展，适用场景已由高速公路、泊车等相对简单的路况，开始向相对复杂的城市交叉口场景扩展。环形交叉口作为一种特殊的交通节点形式，具有自组织交通流的特性。在减少交通冲突点、交通事故率、交叉口延误时间、大气污染物排放，以及提高路网通行效率和燃油利用率等方面具有突出优势。因此在世界范围内，尤其在欧美等国得到广泛地应用。

与此同时，随着汽车保有量的不断上涨，传统交通环境下，环形交叉口的最大通行能力是有限的，不能适应较大的车流量，若此时驾驶员不能严格遵守环形交叉口内部车辆优先行驶原则(简称“环内优先”原则)，则很容易造成环内“死锁”现象，进而造成交通拥堵与交通事故。

如果将自动驾驶与环形交叉口相结合，自动驾驶车辆能够严格准确地执行指令，则不会产生不文明的驾驶行为。与此同时，通过制定合理的自动驾驶行为决策也能提升环形交叉口车道利用率，大大提高城市交通的通行效率与安全性。

针对环形交叉口这种交互性强、车流交织程度高的交通场景，自动驾驶车辆需要同时考虑交通状况、行驶安全、通行效率、行驶平顺性等多种因素，因此需要构建合理、精细、全面的驾驶行为决策与运动规划方法，从而实现安全、舒适、高效地环形交叉口自动驾驶场景。

发明内容

为了解决现有技术中存在的不足，本发明提供一种适用于环形交叉口的自动驾驶行为决策系统与运动规划方法，旨在实现自动驾驶车辆安全、舒适、高效、灵活地驶入与驶出环形交叉口。

本发明所采用的技术方案如下：

一种适用于环形交叉口场景的自动驾驶行为决策系统，自动驾驶行为决策系统包括顶层状态机、转移条件以及扩展底层状态机；以环形交叉口道路模型M

所述转移条件是由自动驾驶车辆入环与出环的安全距离模型、自动驾驶车辆入环与出环规则以及自动驾驶车辆入环、环内保持、出环的目标车道选择规则构成；触发转移条件根据顶层状态机输出的行驶状态集合判断自动驾驶车辆的行驶状态；

顶层状态机的行驶状态集合根据触发转移条件，在扩展底层状态机中产生相应的动作集合，根据速度规划与轨迹规划得到轨迹目标，传到下层控制器进行执行。

进一步的，根据不同的环形交叉口场景以及不同的全局路径规划要求制定行驶状态集合，行驶状态集合分为：

当检测前方行驶区域无障碍时，则行驶状态集合包括直线行驶状态S

当检测前方行驶区域有障碍物时，则行驶状态集合包括直线行驶状态S

进一步的，所述自动驾驶车辆入环与出环的安全距离模型，具体分为跟车安全距离模型与超车安全距离模型，分别表示如下：

跟车安全距离模型：

S

超车安全距离模型：

S

以自动驾驶车辆在入环、出环位置时的航向角为X轴，垂直X轴为Y轴建立局部直角坐标系；其中，S

进一步的，根据自动驾驶车辆在入环与出环处的S

当S

当S

其中，S

进一步的，所述自动驾驶车辆入环、环内保持与出环目标车道的选择规则：

(4)自动驾驶车辆入环时：

1)当入环车道大于等于三条时，若自动驾驶车辆右转，则目标入环车道为右侧车道；若自动驾驶车辆直行时，则目标入环车道为中间车道；若自动驾驶车辆左转时，则目标入环车道为左侧车道；

2)当入环车道小于等于两条时，若自动驾驶车辆右转，则目标入环车道为右侧车道；若自动驾驶车辆直行或左转时，则目标入环车道为左侧车道；

(5)自动驾驶车辆环内保持时：

1)当环内车道大于等于三条时，若自动驾驶车辆右转，则目标环内车道为外侧车道；若自动驾驶车辆直行时，则目标环内车道为中间车道；若自动驾驶车辆左转时，则目标环内车道为内侧车道；

2)当环内车道小于等于两条时，若自动驾驶车辆右转，则目标环内车道为外侧车道；若自动驾驶车辆直行或左转时，则目标环内车道为内侧车道；

(6)自动驾驶车辆出环时：

1)当出环车道大于等于三条时，若自动驾驶车辆右转，则目标出环车道为右侧车道；若自动驾驶车辆直行时，则目标出环车道为中间车道；若自动驾驶车辆左转时，则目标出环车道为左侧车道；

2)当出环车道小于等于两条时，若自动驾驶车辆右转，则目标出环车道为右侧车道；若自动驾驶车辆直行或左转时，则目标出环车道为左侧车道。

进一步的所述扩展底层状态机的输入是顶层状态机输出的“行驶状态集合”，且扩展底层状态机的将“目标候选点集、期望车速以及预瞄距离”作为输出，构建底层扩展状态机A＝(Q,M,δ,S

一种适用于环形交叉口场景的自动驾驶运动规划方法，包括以下步骤：

步骤一：建立环形交叉口平面坐标系，所述坐标系以环形交叉口中心岛的中心为原点O，以自动驾驶车辆入环前直线行驶的方向为Y轴，垂直Y轴为X轴；

步骤二：分别确定车辆转向执行器限制、平顺性、舒适性、规划速度损失的约束条件；

步骤三：基于全局路径规划的目标环内车道驶入与驶出规则、环形交叉口的形状参数、车辆转向执行器限制、平顺性、舒适性、规划速度损失的约束条件，确定自动驾驶车辆入环与出环路径的六个B样条曲线控制点，以及控制点对应的目标曲线轨迹；

进一步的，步骤二中确定车辆转向执行器限制、平顺性、舒适性、规划速度损失的约束条件分别表示如下：

所述车辆转向执行器限制表示为：

其中，k

所述平顺性是指在曲线内部以及曲线与曲线连接处实现G2连续，对于曲线P(t)和Q(t)，参数t∈[0,1]，若要求曲线P(t)和Q(t)在连接处达到G2连续，通过下式表示：

P(1)＝Q(0)

P′(1)＝Q′(0)

P″(1)＝Q″(0)

其中，P(1)＝Q(0)表示两曲线在连接处位置连续，P′(1)＝Q′(0)表示两曲线在连接处相切连续，P″(1)＝Q″(0)表示两曲线在连接处曲率连续；

所述舒适性具体通过舒适度损失函数式来表示，

其中，C

所述规划速度损失度具体表示为：

C

其中，s′(te)为自动驾驶车辆入环与出环的速度，V

进一步的，步骤三中的环形交叉口的形状参数，包括但不限于中心岛坐标、中心岛半径、入环车道数、环内车道数、出环车道数、车道宽度、入环车道方向角以及出环车道方向角。

进一步的，步骤三中的B样条曲线指准均匀三阶B样条曲线，六个控制点分别为p0,p1,p2,p3,p4,p5，则三阶B样条曲线的数学表达式表示为：

其中，p

约定：0/0＝0

该递推公示表明：欲确定第i个k阶B样条N

曲线方程中，n+1个控制点p

第一曲线控制点p0坐标为自动驾驶车辆进入入环车道时车道中心线的坐标；

第二曲线控制点p1坐标位于自动驾驶车辆进入入环车道时航向角的延长线上，p1的纵坐标为p0与p2的中点纵坐标，p1的横坐标通过航向角的延长线方程求得；

第三曲线控制点p2坐标为入环车道线的末端坐标；若环内车流方向为逆时针，则为自动驾驶车辆当前所在入环车道左侧车道线的末端坐标；若环内车流方向为顺时针，则为自动驾驶车辆当前所在车道右侧车道线的末端坐标；

第四曲线控制点p3坐标为p2点延长线与目标环内车道中心线的交点坐标，p2点延长线的倾斜角为自动驾驶车辆位于入环车道末端时的航向角；

第五曲线控制点p4坐标位于p5所在圆切线段，若环内车流方向为顺时针，则p4在p5左侧；若环内车流方向为逆时针，则p4在p5右侧；p4与p5的距离由p5点所在目标环内车道中心线的曲率确定；

第六曲线控制点p5坐标为自动驾驶车辆首次驶入目标环内车道中心线的坐标，p3与坐标系原点的连线和Y轴的夹角角度为迭代初始角度θ，p5通过p3在目标环内车道中心线上累加一定迭代角度Δ得到；若环内车流方向为逆时针，迭代角度Δ为正；若环内车流方向为顺时针，迭代角度Δ为负，迭代角度Δ为0.1°，迭代次数为g次，迭代终止角度为θ+g×Δ；

其中，各个车道线坐标通过车载摄像头的车道线检测功能获得。

与现有技术相比，其具有如下优势：

(1)本发明提供的自动驾驶行为决策系统针对环形交叉口场景，通过将驾驶行为决策抽象为一系列的行驶场景与行为动作状态机集合，简化了复杂场景驾驶行为决策任务，针对这些状态集合根据安全性、舒适性、通行效率等原则，构建分层扩展有限状态机(EFSM)驾驶行为决策模型，解决了自动驾驶车辆在环形交叉口中的行为决策问题，提高了自动驾驶行为决策模型的实时能力与环形交叉口内车道的利用率与通行效率，同时保证了自动驾驶车辆的安全性与行驶效率。

(2)本发明提供的运动规划方法在建立环形交叉口全局坐标系的基础上，根据环形交叉口的形状参数、全局路径规划的目标环内车道驶入与驶出规则、车辆转向执行器限制、平顺性、舒适性、规划速度损失等约束条件设定目标函数，确定自动驾驶车辆的目标环内车道以及B样条曲线的六个控制点，解决了自动驾驶车辆入环与出环行驶路径平顺性、舒适性的技术问题。

(3)一般来说，在具有清晰车道线的情况下，自动驾驶车辆会基于车道保持辅助系统根据车道线进行行驶，轨迹相对固定。本发明提供的B样条曲线轨迹规划方法，前三个控制点由入环车道线生成，后三个控制点位于环内车道。基于B样条曲线的局部支撑性，对于环内车道中部分入环轨迹的优化并不会改变由入环车道线确定的部分入环轨迹。因此，基于B样条曲线进行轨迹规划更具灵活性。

附图说明

此处所说明的附图用来提供对本发明的进一步理解，构成本发明的一部分，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。在附图中：

图1为本发明实施例的自动驾驶行为决策规划框图。

图2为本发明实施例的结构化环形交叉口示意图。

图3为本发明实施例的自动驾驶决策顶层状态机结构图。

图4为本发明实施例的安全距离模型示意图。

图5为本发明实施例的自动驾驶决策底层状态机结构图。

图6为本发明实施例的状态转移链图。

图7为本发明实施例的多车道环形交叉口坐标系及曲线控制点坐标构成示意图。

图8为本发明实施例的曲线控制点坐标、控制多边形、目标曲线轨迹和轨迹曲率变化图。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚，下面将结合本发明具体实施例及相应的附图对本发明技术方案进行清楚、完整地描述。显然，所描述的实施例仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

如图1所示，本发明具体实施例以两个入环车道、两个出环车道与三个环内车道的结构化环形交叉口为例。其结构包括但不限于中心岛、环内车道、入环车道、出环车道、分隔岛以及斑马线等。

如图2所示，一种适用于环形交叉口场景的分层扩展有限状态机(EFSM)行为决策系统。该系统包括顶层状态机、转移条件以及扩展底层状态机；顶层状态机根据不同的环形交叉口场景以及不同的全局路径规划要求制定不同的行驶状态集合；且顶层状态机的行驶状态集合根据触发转移条件，在扩展底层状态机中产生相应的动作集合，根据速度规划与轨迹规划得到轨迹目标，传到下层控制器进行执行。

顶层状态机的构建方法是：根据道路、场景环境和本车信息，分别建立道路模型M

(1)根据组合导航系统与电子地图实现全局路径规划，结合道路坡度、不平度、道路曲率、路面附着系数等构建环形交叉口道路模型M

(2)分析车载摄像头、激光雷达等环境传感器或高精地图提供的车道线、道路边缘、障碍物等环境信息。定义环形交叉口为直道行驶场景、入环行驶场景、环内行驶场景、出环行驶场景以及避障场景，构建环形交叉口场景模型M

(3)根据自动驾驶车辆的位姿信息、航迹信息与执行机构传感器，构建自动驾驶车辆的运动模型M

(4)如图3所示，根据不同的环形交叉口场景以及不同的全局路径规划要求制定不同的行驶状态集合。

1)当检测前方行驶区域无障碍时，则行驶状态集合包括直线行驶状态S

2)当检测前方行驶区域有障碍物时，则行驶状态集合包括直线行驶状态S

转移条件是根据自动驾驶车辆入环与出环的安全距离模型、自动驾驶车辆入环与出环规则和自动驾驶车辆入环、环内保持、出环的目标车道选择规则构成。触发转移条件根据顶层状态机输出的行驶状态集合判断自动驾驶车辆的行驶状态。具体过程如下：

(1)构建自动驾驶车辆入环与出环的安全距离模型的具体方法如下：

建立环形交叉口平面坐标系，所述坐标系以环形交叉口中心岛的中心为原点O，以自动驾驶车辆入环前准备行驶方向过原点的射线为Y轴，垂直Y轴为X轴；

如图4所示，自动驾驶车辆入环、出环的安全距离模型具体分为跟车安全距离模型与超车安全距离模型，分别表示如下：

1)跟车安全距离模型

S

2)超车安全距离模型

S

以自动驾驶车辆在入环、出环位置时的航向角为X轴，垂直X轴为Y轴建立局部直角坐标系。其中，S

其中，自动驾驶车辆的入环与出环时间t

(2)根据自动驾驶车辆在入环与出环处的S

1)当S

2)当S

(3)所述自动驾驶车辆入环、环内保持与出环目标车道的选择规则：

自动驾驶车辆入环时：

1)当入环车道大于等于三条时，若自动驾驶车辆右转，则目标入环车道为右侧车道；若自动驾驶车辆直行时，则目标入环车道为中间车道；若自动驾驶车辆左转时，则目标入环车道为左侧车道；

2)当入环车道小于等于两条时，若自动驾驶车辆右转，则目标入环车道为右侧车道；若自动驾驶车辆直行或左转时，则目标入环车道为左侧车道；

自动驾驶车辆环内保持时：

1)当环内车道大于等于三条时，若自动驾驶车辆右转，则目标环内车道为外侧车道；若自动驾驶车辆直行时，则目标环内车道为中间车道；若自动驾驶车辆左转时，则目标环内车道为内侧车道；

2)当环内车道小于等于两条时，若自动驾驶车辆右转，则目标环内车道为外侧车道；若自动驾驶车辆直行或左转时，则目标环内车道为内侧车道；

自动驾驶车辆出环时：

1)当出环车道大于等于三条时，若自动驾驶车辆右转，则目标出环车道为右侧车道；若自动驾驶车辆直行时，则目标出环车道为中间车道；若自动驾驶车辆左转时，则目标出环车道为左侧车道；

2)当出环车道小于等于两条时，若自动驾驶车辆右转，则目标出环车道为右侧车道；若自动驾驶车辆直行或左转时，则目标出环车道为左侧车道。

在本实施例中，如图1所示，以两个入环车道与三个环内车道的结构化环形交叉口为例，由内向外记环内车道为第一环内车道、第二环内车道、第三环内车道。基于全局路径规划、驾驶经验以及交通规则等约束要求，确定所述自动驾驶车辆入环、环内保持与出环目标车道的选择规则：

当自动驾驶车辆右转时，则沿着右侧入环车道行驶，目标环内车道为第三环内车道，并从右侧出环车道驶出。如图1中——·线所示。

当自动驾驶车辆直行时，则沿着左侧入环车道行驶，目标环内车道为第二环内车道，并从左侧出环车道驶出。如图1中——线所示。

当自动驾驶车辆左转时，则沿着左侧入环车道行驶，目标环内车道为第一环内车道，并从左侧出环车道驶出。如图1中—·—线所示。

如图5所示，扩展底层状态机，将顶层状态机输出的“行驶状态集合”作为扩展底层状态机的输入，将“目标候选点集、期望车速以及预瞄距离”作为扩展底层状态机的输出，由此构建底层扩展状态机A＝(Q,M,δ,S

如图6所示，为分层扩展有限状态机(EFSM)的状态转移链图。

S

(1)无障碍行驶时：

状态转移函数为δ(S

(2)避障行驶时：

若避障前状态为S

若避障前状态为S

若避障前状态为S

若避障前状态为S

基于本申请所设计的一种适用于环形交叉口场景的自动驾驶运动规划方法，包括如下步骤：

步骤一：以环形交叉口中心岛的中心为原点O，以自动驾驶车辆入环前直线行驶的方向为Y轴，垂直Y轴为X轴建立环形交叉口平面坐标系；

步骤二：分别确定车辆转向执行器限制、平顺性、舒适性、规划速度损失的约束条件。

具体如下：

车辆转向执行器限制，具体指的是所规划轨迹的最大曲率因小于等于车辆最大转向曲率，具体通过式(1)来表示，从而满足车辆转向执行器的限制。

其中，k

平顺性是指在曲线内部以及曲线与曲线连接处实现G2连续。假设存在两条曲线P(t)和Q(t)，对于曲线P(t)和Q(t)，参数t∈[0,1]，若要求曲线P(t)和Q(t)在连接处达到G2连续，可通过式(2)来表示：

P(1)＝Q(0)表示两曲线在连接处位置连续，P′(1)＝Q′(0)表示两曲线在连接处相切连续，P″(1)＝Q″(0)表示两曲线在连接处曲率连续。

舒适性具体通过舒适度损失函数式(3)来表示，

其中，C

当速度轨迹的舒适度值相差不大，相对来讲都比较平缓时，更关注最终规划末状体速度是否可以达到期望速度，规划速度损失度具体通过式(4)来表示，

C

其中，s′(te)为自动驾驶车辆入环与出环的速度，V

步骤三：基于全局路径规划的目标环内车道驶入与驶出规则、环形交叉口的形状参数、车辆转向执行器限制、平顺性、舒适性、规划速度损失的约束条件，确定自动驾驶车辆入环与出环路径的六个B样条曲线控制点，以及控制点对应的目标曲线轨迹。

其中，所述环形交叉口的形状参数，包括但不限于中心岛坐标、中心岛半径、入环车道数、内环车道数、出环车道数、车道宽度、入环车道方向角以及出环车道方向角等。

所述B样条曲线具体指准均匀三阶B样条曲线，其控制点有p0,p1,p2,p3,p4,p5，则三阶B样条曲线的数学表达式表示为式(5)，

其中，n＝5，k＝3，N

约定：0/0＝0

由于自动驾驶车辆驶入与驶出环形交叉口的步骤相似，因此本发明实施例根据自动驾驶车辆由南向北直行通过某城市一个多车道环形交叉口入环示例对本发明作进一步阐述。

如图7所示，是本发明实施例中的多车道环形交叉口坐标系及曲线控制点坐标构成示意图，环内车流方向为逆时针。所述p0,p1,p2,p3,p4,p5分别为目标曲线第一、第二、第三、第四、第五和第六曲线控制点坐标。

通过高精地图或自动驾驶车辆的环境感知系统获取环形交叉口结构参数。其中，中心岛半径59.5米、环内车道宽4.5米、环内车道数3条、入环与出环车道数各2条。

第一曲线控制点p0坐标为自动驾驶车辆进入入环车道时车道中心线的坐标，则p0＝(-0.96,-99.27)；

第二曲线控制点p1坐标位于自动驾驶车辆进入入环车道时航向角的延长线L

第三曲线控制点p2坐标为入环车道线的末端坐标。本实施例中环内车流方向为逆时针，通过车道线检测提取到自动驾驶车辆所在入环车道左侧车道线的末端坐标p2＝(4.81,-73.11)；

第四曲线控制点p3坐标为p2点延长线L

第五曲线控制点p4坐标位于p5所在圆切线段L

第六曲线控制点p5坐标为自动驾驶车辆首次驶入目标环内车道中心线的坐标。p3与坐标系原点的连线和Y轴的夹角角度为迭代初始角度θ为-81.3°，本实施例中环内车流方向为逆时针，迭代角度Δ为正，迭代条件为步骤二中所述的约束条件，迭代角度Δ为0.1°，迭代次数为118次，迭代终止角度为-69.5°，则p5＝(23.17,-62.07)。

如图8中左图所示，空心圆点表示本发明实施例的曲线控制点坐标，实线表示本发明实施例的曲线的控制多边形，虚线表示本发明实施例的目标曲线轨迹。右图为目标曲线轨迹的曲率变化图。轨迹曲线的峰值曲率为0.159小于车辆最大转向曲率；目标环内车道中心线曲率为0.015与轨迹曲线端点曲率与一致，满足G2连续要求；最大加加速度jerk为0.108。

本发明还可有其它多种实施例，在不背离本发明精神及其实质的情况下，本领域技术人员当可根据本发明作出各种相应的改变和变形，但这些相应的改变和变形都应属于本发明所附的权利要求的保护范围。