CN113895463A - 一种适用于自动驾驶车辆掉头的路径规划方法 - Google Patents

Abstract

本发明公开了一种适用于自动驾驶车辆掉头的路径规划方法，属于自动驾驶领域；具体为，首先针对当前车辆，获得当前车辆前方150m范围内道路Reference_line上的所有轨迹点信息，并计算各个轨迹点处的曲率；判断是否存在至少三个连续曲率都大于阈值，如果是，则认为前方有车辆掉头场景，将第一个满足曲率阈值要求的连续轨迹点作为开放空间规划的触发点，最远的轨迹点作为终点；否则，车辆继续跟随原本的Reference_line规划路径行驶。判断车辆和触发点的距离s是否小于15m，如果是，进行开放空间路径规划，得到车辆掉头的规划轨迹；否则，车辆继续按照原本的路径行驶，直到距离满足阈值要求。本发明在面对车辆掉头场景时，车辆顺利通过，解决了自动驾驶车辆的掉头问题。

Description

一种适用于自动驾驶车辆掉头的路径规划方法

技术领域

本发明属于自动驾驶领域，具体涉及一种适用于自动驾驶车辆掉头的路径规划方法。

背景技术

近些年来，自动驾驶领域迅速发展；自动驾驶车辆依靠感知、定位、路径规划以及控制等模块的协同合作，实现了在无人情形下自动安全地驾驶车辆。其中路径规划模块主要是实现从车身位置到目标位置的轨迹规划，通过接收感知/定位/地图信息，构建可行驶范围，生成安全舒适的全局轨迹，并实时传输局部轨迹给车辆控制模块。

当前在城市道路环境中，自动驾驶车辆的路径规划主要是通过计算道路参考线的方式得到，即通过感知得到的道路环境信息，包括道路边界、障碍物、行人和车辆等，考虑车辆的动力学特征，通过轨迹平滑处理，规划出一条能够让车辆安全行驶的平滑轨迹。然而，在车辆掉头过程，当前所使用的基于参考线的路径规划方法虽然能够规划出一条轨迹，但是当轨迹包含较大的连续转向时，自动驾驶车辆在连续大幅度转向的极限情况下会给下游的控制模块产生极大的压力，如在车辆掉头场景中会由于控制模块无法跟上，导致车辆连续点刹甚至停车的现象，因此车辆掉头路径规划方法的研究对于车辆自动驾驶的实现具有重要的意义。

发明内容

针对当前基于参考线(Reference_line)的路径规划方法在面对车辆掉头场景时，车辆难以顺利通过的问题，本发明提出一种适用于自动驾驶车辆掉头的路径规划方法，基于开放空间规划路径，解决自动驾驶车辆的掉头问题。

所述的适用于自动驾驶车辆掉头的路径规划方法，具体步骤如下：

步骤一、针对当前车辆，获得当前车辆前方150m范围内道路Reference_line上的所有轨迹点信息，并计算Reference_line上各个轨迹点处的曲率；

曲率计算公式如下：

其中：

为道路Reference_line上相邻轨迹点之间的角度差值；Δs为道路Reference_line上相邻轨迹点之间曲线的弧长；

步骤二、判断是否存在至少三个连续轨迹点的曲率都大于阈值范围，如果是，则认为前方有车辆掉头场景，进入步骤三；否则，车辆继续跟随原本的Reference_line规划路径行驶。

曲率阈值的设置根据车辆的最小转弯半径确定，车辆最小转弯半径R的计算公式为：

其中：L为车身长度，ψ为车辆方向的最大转角。

步骤三、从满足阈值要求的连续轨迹点中，将第一个满足曲率阈值要求的连续轨迹点作为开放空间规划的触发点，最远的连续轨迹点作为开放空间规划的终点；

步骤四、判断车辆和触发点的距离s是否小于15m，如果是，进入步骤五，进行开放空间规划，否则，车辆继续按照原本的路径行驶，直到距离满足阈值要求。

步骤五、进行开放空间路径规划，得到车辆掉头的规划轨迹；

具体步骤为：

步骤501、根据车辆的长、宽信息以及车辆抵达终点位置的姿态(角度)，预处理得到车辆在终点位置停车的矩形区域；

将原本车辆在终点处的停车位置从坐标表示转换成矩形区域表示，矩形区域的信息以其四个顶点的坐标位置来表征。

步骤502、根据车辆进入开放空间规划后的环境信息以及车辆信息，生成车辆掉头的感兴趣区域。

具体为：

从触发点开始，到终点位置后约15m的位置结束，整个区间内，以0.5m的距离分辨率进行道路边界采样，从采样点中选择道路中曲率变化明显的边界点和路宽度有明显变化的边界点加入到道路边界信息中；判断能否将道路边界处理成凸约束的形式，如果是，则道路边界信息处理完成，所有的道路边界点构成的区域为车辆掉头的感兴趣区域。否则，无法获得车辆掉头的感兴趣区域，报告错误信息，车辆停止在当前位置。

曲率变化明显的点是指：拟合的道路边界曲线上相邻采样点之间切线角度的差值大于0.15rad，则认为这两个采样点之间的道路边界有明显的曲率变化，将采样点信息加入到道路边界信息中。

道路宽度明显变化是指：车辆当前轨迹点与前轨迹点形成的角度的正切值tan(theta1)，车辆当前轨迹点与后轨迹点形成的角度正切值tan(theta2)，若二者的差值大于阈值0.4，则认为当前轨迹点所在处的道路宽度与前后相比有较为明显的变化，将其加入道路边界信息中。

步骤503、在车辆掉头的感兴趣区域中，利用Hybrid A*的路径搜索算法，规划一条从起点到终点的无碰撞轨迹；

步骤504、遍历轨迹上的所有轨迹点，根据是否包含倒车过程，插入轨迹点后进行轨迹的分割；

具体为：判断当前轨迹点与后一轨迹点之间的角度差值是否大于90度，如果是，在当前轨迹点位置插入大量位置相同，速度、加速度为零的点，作为分割用的预瞄点；否则，不予处理，继续遍历下一个轨迹点，直至遍历完毕所有轨迹点；

在轨迹分割过程中，将车辆正向前进和倒车的轨迹按照次序分段存储，其中，轨迹分割的位置点既是前一段轨迹的最后一点也是下一段轨迹的起点。

步骤505、对分割后的每个轨迹段进行编码，车辆按规划的轨迹行驶，根据各轨迹点和车辆的交并比IOU，得到车辆实际位置对应的轨迹点；

具体为：

首先，遍历每一个轨迹段的每一个轨迹点，将车辆在该轨迹点处的位置和姿态，与当前车辆实际的位置和姿态作差，判断俩个差值是否都在阈值范围内，如果是，通过车辆当前的位置和车辆的长，宽等参数信息建立车辆的ego_box和轨迹点所在位置的path_point_box,，计算二者的交并比(IOU)，记录当前轨迹点的信息，包括轨迹点所在的轨迹段，轨迹点在轨迹段中的索引，IOU值等。，否则，不予处理，重复上述过程，直到遍历完所有轨迹段。

然后，将保存的各轨迹点按IOU值从大到小排序，依次检验，删除历史轨迹点后，最大的IOU值对应的轨迹点，即为与车辆当前实际位置对应的轨迹点。

在确定车辆当前位置后，以车辆当前所在的轨迹点为矩阵，对后续轨迹点距离和时间进行调整，从而减小在车辆行驶过程中所带来的误差。

步骤506、实时判断车辆行驶过程中是否有障碍物，如果是，进入步骤507；否则，进入步骤508；

步骤507、判断车辆是否与障碍物发生碰撞，如果是，以车辆当前实际位置的轨迹点为规划起点，返回步骤503重新规划；否则，将该轨迹的最后一个轨迹点作为规划起点，返回步骤503重新规划；

碰撞检测：根据车辆所在的位置和长、宽参数，建立车辆的ego_box(矩形)，从预测模块得到障碍物的obstacle_box(矩形)，采取分离轴定理检测二者是否发生重叠，如果是，则说明车辆在该轨迹点会和障碍物发生碰撞，如果存在以下两种情形则认为会发生碰撞：1)：从碰撞检测模块中接收到反馈的碰撞信息；2)车辆的速度和加速度低于0.01。

步骤508、实时判断车辆当前的位置、姿态与车辆在终点的位置、姿态的差值，是否都在阈值范围内，如果是，则车辆抵达终点，车辆掉头过程结束。否则，根据规划起点调用规划算法，进行新的规划。

本发明的优点为：

(1)一种适用于自动驾驶车辆掉头的路径规划方法，首次将开放空间规划应用于自动驾驶车辆掉头场景，解决了原本的基于参考线的路径规划方法在面对掉头场景时存在的车辆控制问题，使车辆能够安全、顺利进行掉头。

(2)一种适用于自动驾驶车辆掉头的路径规划方法，开创性提出根据参考线规划方法的路径点曲率来进行车辆掉头的场景判断，从本质上解决自动驾驶车辆在面对大弯道场景连续转向时车辆控制难以有效实现的问题。

(3)一种适用于自动驾驶车辆掉头的路径规划方法，改进开放空间规划的逻辑流程。对参数条件的设定，模块的判断逻辑进行了改进，使得开放空间规划方法能够真正适应车辆掉头场景。

附图说明

图1为本发明一种适用于自动驾驶车辆掉头的路径规划方法的流程图；

图2为本发明利用车辆当前位置与前后轨迹点处的道路宽度得到道路边界信息的示意图；

图3为本发明快速剔除非碰撞的Box的示意图；

图4为本发明分离轴定理检测中在自车横轴上的投影示意图；

图5为本发明分离轴定理检测中在自车纵轴上的投影示意图；

图6为本发明分离轴定理检测中在障碍物横轴上的投影示意图；

图7为本发明分离轴定理检测中在障碍物纵轴上的投影示意图；

图8为本发明车辆掉头场景示意图；

图9为本发明开放空间规划总体流程图；

图10为本发明车辆掉头规划结构图。

具体实施方式

下面结合附图及相应公式对本发明的具体实施方法进行详细说明。

本发明提出了一种适用于自动驾驶车辆掉头的路径规划方法，在获得道路环境信息以及自动驾驶车辆信息的基础上，运用开放空间规划的基本思想，构建了车辆掉头场景的判断逻辑，设定了具体的路径规划过程。通过车辆掉头场景的判断逻辑，决定自动驾驶车辆是否要进行掉头，在车辆进入到掉头场景后，通过路径规划获得一条车辆能够顺利通过掉头场景的规划轨迹，实现自动驾驶车辆的掉头功能。

所述的适用于自动驾驶车辆掉头的路径规划方法，如图1所示，具体步骤如下：

步骤一、针对当前车辆，通过原本的基于参考线的路径规划结果，获得当前车辆前方150m范围内道路Reference_line上的所有轨迹点信息，并计算Reference_line上各个轨迹点处道路的曲率；

车辆掉头场景的确定主要是通过参考线轨迹上路径的点的曲率来进行判断，因为在车辆掉头场景中，车辆控制难以实现的问题很大程度是由于控制模块要去预瞄大曲率的路径点，来控制车辆进行连续大幅度转向造成的，路径点曲率越大，则道路弯曲程度越大，对控制系统造成的压力越大。因此，本发明选择以路径点的曲率为进入车辆掉头场景的条件，从本质上解决车辆掉头时车辆控制难以实现的问题。

曲率计算公式如下：

其中：

为道路Reference_line上相邻轨迹点之间的角度差值；Δs为道路Reference_line上相邻轨迹点之间曲线的弧长；

步骤二、判断是否存在至少三个连续轨迹点的曲率都大于阈值范围，如果是，则认为前方有车辆掉头场景，进入步骤三；否则，车辆继续跟随原本的Reference_line规划路径行驶。

曲率阈值的设置根据车辆的最小转弯半径确定，车辆最小转弯半径R的计算公式为：

则进一步设定：

其中：L为车身长度，ψ为车辆方向的最大转角。

本发明中将道路的曲率阈值设置为：

k_thres＝0.9*k_max (4)

步骤三、从满足阈值要求的连续轨迹点中，将第一个满足曲率阈值要求的距离车辆最近的连续轨迹点作为开放空间规划的触发点，最远的连续轨迹点作为开放空间规划的终点；

步骤四、判断车辆和触发点的距离s是否小于15m，如果是，进入步骤五，进行开放空间规划，否则，车辆继续按照原本的路径行驶，直到距离满足阈值要求。

步骤五、进行开放空间路径规划，得到车辆掉头的规划轨迹；

具体步骤为：

步骤501、根据车辆的长、宽信息以及车辆抵达终点位置的姿态(角度)，预处理得到车辆在终点位置停车的矩形区域；

将原本车辆在终点处的停车位置从坐标表示转换成矩形区域表示，矩形区域的信息以其四个顶点的坐标位置来表征。

步骤502、通过感知、定位、地图等获得车辆进入开放空间规划后的环境信息以及车辆信息，生成道路区域边界以及障碍物信息，同时对上述信息进行相应处理，生成车辆掉头的感兴趣区域。

车辆信息包括车辆的位置和姿态信息；环境信息是从预测和地图模块中获得的障碍物和周围道路信息；

具体为：

从触发点开始，到终点位置后约15m的位置结束，整个区间内，以0.5m的距离分辨率进行道路边界采样，从采样点中选择道路中曲率变化明显的边界点和路宽度有明显变化的边界点加入到道路边界信息中；该过程是为了在车辆遇到弯道或车道宽度变化等影响车辆行驶状态的道路中，获得更加详细的道路边界信息，保证车辆行驶安全。

判断能否将道路边界处理成凸约束的形式，如果是，则道路边界信息处理完成，所有的道路边界点构成的区域为车辆掉头的感兴趣区域。否则，无法获得车辆掉头的感兴趣区域，报告错误信息，车辆停止在当前位置。

曲率变化明显的点是指：拟合的道路边界曲线上相邻采样点之间切线角度的差值大于0.15rad，则认为这两个采样点之间的道路边界有明显的曲率变化，将采样点信息加入到道路边界信息中。

道路宽度明显变化，是指：车辆当前轨迹点与前轨迹点形成的角度的正切值tan(theta1)，车辆当前轨迹点与后轨迹点形成的角度正切值tan(theta2)，若二者的差值大于阈值0.4，则认为当前轨迹点所在处的道路宽度与前后相比有较为明显的变化，将其加入道路边界信息中。

如图2所示，车辆当前位置的道路宽度与前后轨迹点处的道路宽度有明显差别，图中最下边的点为前一轨迹点，中间的点为车辆当前位置所在的轨迹点，最上边的点为后一轨迹点，delta_width1和delta_width2分别是当前轨迹点与前后轨迹点处道路宽度的差值，通过轨迹点之间的距离distace_s，得到：

注意：delta_width1＞0，delta_width2＜0。

通过以更小的分辨率对道路边界进行采样，获得了更加详细的道路边界信息。同时，判断能否将获取的道路边界信息处理成凸约束的形式，如果是，则道路边界信息处理完成，车辆成功获取感兴趣区域，进行后续的轨迹规划和处理过程。否则，无法获得车辆掉头的感兴趣区域，报告错误信息，车辆停止在当前位置。

步骤503、在车辆掉头的感兴趣区域中，根据车辆信息，环境信息以及终点位置信息，调用Hybrid A*的路径搜索算法，规划一条从起点到终点的无碰撞平滑轨迹；

Hybrid A*规划中，在确定了起点，终点以及障碍物信息后，进行轨迹规划过程。

Hybird A*算法在2010年由斯坦福首次提出，是一种满足车辆运动学的算法，并在(DARPA)的城市挑战赛中得以运用。Hybrid A*是一种高效的路径规划算法，以车辆当前位置，目标位置以及障碍物信息作为输入，通过规划输出一条车辆可行驶的光滑轨迹。

Hybrid A*算法发展自A*算法，A*算法本质上来说是一种基于网格的搜索算法。在算法中，每个网格被分配了两种cost，actual cost或path cost，记为g，即已经实际走过的路径的cost，实际上一般为从起点开始所经历的路径的距离和；另外一种为heuristiccost即预计到达终点还需要的cost，记为h，实际上一般定义为到终点的欧式距离，两种cost的和记为f。Hybrid A*算法沿用了A*算法的基本思想，在A*算法的基础上考虑了车辆的动力学模型和实际运动方向，因此生成的路径能够满足实际车辆的运动学要求：

在Hybrid A*中用到了RS曲线，RS全称为Reeds_Shepp曲线，由Reeds,J.A.和Shepp,L.在1990首次提出；RS曲线一个非常重要的特征就是考虑车辆倒车的情况，而能够倒车也是Open Space规划与其他规划方法的重要不同。

J Reeds和L Shepp证明Reeds Shepp Car从起点到终点的最短路径一定是如表1所示的六种基本情形组成的一种，六种基本情形为L⁺,L^-,R⁺,R^-,S⁺,S^-；其中L⁺表示车辆左转前进；L^-表示车辆左转后退；R⁺表示车辆右转前进；R^-表示车辆右转后退；S⁺表示车辆直行前进；S^-表示车辆直行后退，"|"表示车辆运动朝向由正向转为反向或者由反向转为正向。

表1

Hybrid A*规划中，在确定了起点，终点以及障碍物信息后，进行轨迹规划过程。在每一次生成新的路径点之前，首先会调用RS扩展获得一条从当前路径点到目标位置的RS曲线，若生成的RS曲线与障碍物无碰撞，则Hybrid A*规划结束，获得最终的规划轨迹；否则继续执行Hybrid A*中的节点搜索过程。

在应用过程中，由于RS曲线省去了部分的节点搜索过程，因此缩短了轨迹规划的时间，在障碍物较为稀疏的区域，RS曲线的效果尤为显著，能够大大提高轨迹规划的效率。

通过Hybrid A*的路径搜索过程，获得了从起点到终点的所有路径点信息，每个路径点包含了在世界坐标系下的位置和姿态信息，即(x,y,phi)，要使车辆按照轨迹行驶，路径点中还应该要包含车辆的速度，加速度，转向等车辆动态信息，在算法中上述的动态信息通过车辆的位置和姿态信息计算得到，计算过程如下：

速度计算：

其中：v_x[i]为第i个路径点在x轴方向上的速度，v_y[i]同理；v[i]为第i个路径点的速度；x[i]为第i个路径点的x轴位置；y[i]为第i个路径点的y轴位置；Δt为相邻路径点的时间差；phi[i]为在第i个路径点处车辆纵轴与世界坐标系水平轴的夹角，即车辆的姿态

加速度是通过Δt时间内速度差值，计算得到的平均加速度；表达式为：

a[i]＝(v[i+1]-v[i])/Δt (7)

转向计算：

steer＝(phi[i+1]-phi[i])*wheel_base/step_size_ (8)

其中：wheel_base为车辆的轴距；step_size_为车辆行驶的步长信息；

因此，车辆的转向是通过车辆的姿态信息以及对应公式计算得到；自动驾驶车辆行驶所需要的路径点信息包括各个路径点的位置、姿态等静态信息以及路径点处对应的速度、加速度、转向等动态信息。

通过上述的轨迹规划和计算过程，得到了自动驾驶车辆行驶所需要的路径点信息，主要包括各个路径点的位置、姿态等静态信息以及路径点处对应的速度、加速度、转向等动态信息。对Hybrid A*轨迹进行优化后得到更加平滑的轨迹。

步骤504、遍历轨迹上的所有轨迹点，根据是否包含倒车过程，插入轨迹点后进行轨迹的分割；

通过Hybrid A*算法规划出来的轨迹可能会包含倒车(Gear改变)操作，轨迹分割是找到这些Gear发生改变的轨迹点，通过在该处插入一系列位置相同，速度、加速度为零的点，为控制系统提供足够的点去预瞄，使得车辆能够停下，然后进行倒车操作。

首先，遍历轨迹上的所有轨迹点，根据车辆角度差值对车辆状态进行判断，具体公式如下：

其中：

为当前轨迹点处车辆的角度；

为上一轨迹点处车辆的角度；

为下一轨迹点处车辆的角度；

然后，是否满足

如果是，则认为车辆的Gear发生改变，在该位置对轨迹进行分割处理，即在当前轨迹点位置插入大量位置相同，速度、加速度为零的点，同时将轨迹分段存储；否则，不予处理，继续遍历下一个轨迹点，重复判断是否插入点，直至遍历完毕所有轨迹点；

将每个插入点的轨迹点连同插入的点整体作为一段，各段前后没有插入点的所有轨迹点整体为一段；如果整个轨迹都没有轨迹点需要插入点，整个轨迹作为一整段进行存储。

步骤505、对分割后的每个轨迹段进行编码，车辆按规划的轨迹行驶，根据各轨迹点和车辆的交并比IOU，得到车辆实际位置对应的轨迹点；

具体为：

首先，遍历每一个轨迹段的每一个轨迹点，将车辆在该轨迹点处的位置和姿态，与当前车辆实际的位置和姿态作差，判断俩个差值是否都在阈值范围内，如果是，计算该轨迹点处的IOU，否则，根据车辆当前位置对后续轨迹点进行调整，直至差值在阈值范围内；重复上述过程，直到遍历完所有轨迹段。

IOU全称为Intersection-over-Union，意为交并比，指两个区域的交集和并集的面积比值，IOU值越大，说明两个区域重合度越高，在算法中IOU值越大，说明车辆的ego_box与轨迹点处建立的path_point_box重合度越高，轨迹点离车辆位置越近。是通过车辆当前的实际位置和车辆的长，宽等参数信息建立车辆的ego_box，与轨迹点所在位置的path_point_box，二者的交并比；

调整的过程为：根据车辆的实际位置，确定车辆所在的轨迹段和当前轨迹段上离车辆最近的轨迹点，以该轨迹点为基准，调整当前轨迹段上轨迹点的距离和时间。

因为车辆在行驶过程中行驶的距离和时间都会存在误差，为了消除位置和时间误差，使车辆达到更加精确的控制效果，根据车辆当前位置对后续轨迹点做一些调整，使得信息更加准确；

以车辆当前位置为起点，进行轨迹规划时，以车辆当前所在轨迹为第一条轨迹，车辆当前所在位置为第一个轨迹点，将车辆所在的轨迹的距离s和时间t设置为零，根据相邻轨迹点之间的距离差Δs，时间差Δt计算得到后续轨迹点的相对距离和相对时间，从而计算得到整个轨迹段上所有轨迹点的相对距离和相对时间。

由于对轨迹进行了分割处理，因此上一次的规划轨迹可能会包含多个轨迹段，又因为规划的路径可能包含了倒车轨迹，不同轨迹段之间可能存在重叠与交叉，所以想要对当前轨迹段上的轨迹的s和t信息进行调整，需要确定车辆当前所在的轨迹段以及车辆在轨迹段上对应的轨迹点位置。具体处理过程如下：

首先，对分割后的每个轨迹段进行编码；这是为了记录已经走过的轨迹段信息，因为Hybrid A*规划可能会出现不同轨迹段重叠的现象，在判断当前车辆所在轨迹段时为避免在一些情况下找到错误的轨迹段上，选择将轨迹的历史信息记录下来，排除错误的轨迹。

第二步，确定当前车辆所在轨迹段和轨迹点。遍历每一个轨迹段的每一个轨迹点，若轨迹点和当前车辆位置和姿态信息都在一定阈值范围内，通过IOU记录当前轨迹点的信息，包括轨迹点所在的轨迹段，轨迹点在轨迹段中的索引，IOU值等。重复上述过程，直到遍历完所有轨迹。

在轨迹点的搜索过程中，将满足阈值要求的IOU对应的轨迹点保存，由于可能会存在不同轨迹段上的轨迹点都满足阈值要求，因此在算法中会根据IOU值的从大到小的顺序，依次检验各个轨迹点，若该轨迹点属于历史轨迹，则排除该轨迹点，直到找到与车辆当前位置相对应的轨迹点。

第三步，在路径点搜索过程中判断车辆是否抵达当前轨迹段的最后一点，即车辆是否将要进行倒车操作；

判断方法同样是利用IOU阈值。若车辆的ego_box与当前轨迹段上最后一点的path_point_box的IOU在一定阈值范围内，说明车辆抵达当前轨迹的最后一点。

第四步，判断是否能够找到满足要求的轨迹点。在上述的路径点检索过程中需要ego_box与path_point_box位置和姿态都能够满足阈值，在某些情况下可能会找不到满足要求的轨迹点，在此情形下会放宽搜索条件，只进行位置条件的判断，同样计算IUO，然后找到对应轨迹点。若仍然找不到满足IOU阈值要求的轨迹点，则报告错误信息，车辆停止在当前位置。

将各轨迹点按IOU值从大到小排序，依次检验，删除历史轨迹点后，最大的IOU值对应的轨迹点，即为与车辆当前实际位置对应的轨迹点。

最后，根据车辆位置确定后续轨迹点的s和t信息经过上面的过程，找到了车辆所在的轨迹段和当前轨迹段上离车辆最近的轨迹点，以所在轨迹段上的轨迹点为基准，来调整当前轨迹段上轨迹点的s和t。

经过上述流程，轨迹的分割和轨迹点的信息调整过程结束。

步骤506、实时判断车辆行驶过程中是否有障碍物，如果是，进入步骤507；否则，进入步骤508；

步骤507、判断车辆是否与障碍物发生碰撞，如果是，以车辆当前实际位置的轨迹点为规划起点，返回步骤503重新规划；否则，将该轨迹的最后一个轨迹点作为规划起点，返回步骤503重新规划；

碰撞检测根据车辆所在的位置和长、宽参数，建立车辆的ego_box(矩形)，从预测得到障碍物的obstacle_box(矩形)，采取分离轴定理检测是否发生重叠，如果是，则说明车辆在该轨迹点会和障碍物发生碰撞；

重叠检测分为两步：

(1)快速剔除非碰撞的Box

因为检测过程是根据采样点遍历进行的，可以理解为逐帧进行，精确的碰撞检测其实是比较繁琐的，对于显而易见的不会碰撞，采用快速判断的方法，如图3所示，凡是在灰色区域外障碍物box，都是一定没有发生碰撞的，这是一个充分不必要条件，即在灰色区域外一定不会发生碰撞，但是在灰色区域内不一定就说明二者发生了碰撞，因为会有类似t3时刻的发生。所以只用于剔除一些明显不会发生碰撞的Box。

(2)精确的碰撞检测

对于t3时刻出现的情形，采取分离轴定理检测，即分别在各个不同轴上的投影过程，在planning中，会进行四次的投影检测，分别是：

a)在自车横轴上的投影

如图4所示，上边的矩形框为自车Box，下边的为障碍物Box；a_p为两个Box中心点的连接向量在投影线上的投影长度；b_p为自车在投影线上的投影长度；c_p为障碍物在投影线上的投影长度；若a_p≤b_p+c_p，则说明二者有碰撞的可能；

b)在自车纵轴上的投影

如图5所示，明显可以看到a_p＞b_p+c_p，说明二者没有碰撞；

c)在障碍物横轴上的投影

如图6所示，a_p＜b_p+c_p，二者有碰撞可能。

d)在障碍物纵轴上的投影

如图7所示，a_p＜b_p+c_p，二者有碰撞可能。

在上述的四次投影过程中，只有a_p≤b_p+c_p始终成立时车辆才会与障碍物发生碰撞。在上述示意图中，有三次检测结果认为有可能发生碰撞，只有b)中不会碰撞，因此，检测结果为车辆与障碍物不会发生碰撞，从二维平面的示意图中也很容易得到二者未发生碰撞的结论。

分离轴理论通过投影的方式判断任意两个凸多边形在任意角度下的投影是否均存在重叠。若在某一角度投影下，两物体的投影存在间隙，则为不碰撞，否则为发生碰撞。

在车辆的碰撞检测过程中由于车辆和障碍物均处理成矩形，因此应用分离轴理论能够较好地适用于车辆的碰撞的检测过程，发生碰撞包括以下两种情形：1)：从碰撞检测模块中接收到反馈的碰撞信息；2)车辆的速度和加速度低于0.01。

若当前车辆在该轨迹点与障碍物发生碰撞，需要对规划的轨迹进行新的处理，具体判断及处理过程如下：

(1)判断在当前轨迹段上是否有足够的距离停车；

通过下述公式计算车辆从当前速度以最大加速度减速到停车过程所行驶的距离：

其中a_max为最大停车加速度，v为车辆当前的速度；则距离s_min为车辆停下所需要的最短距离。

定义车辆离障碍物的安全距离为1m，在轨迹上取距障碍物1m距离的轨迹点，在s_min与轨迹点中取距离较大者为停车距离s，在获得停车距离后通过下述公式计算得到需要的加速度a：

由上式可知，a的值小于等于最大停车加速度a_max。

若车辆当前位置加上计算得到的停车距离s仍然在当前轨迹范围内，说明在当前轨迹上有足够的距离停车，执行下一步，否则，立即停车。

(2)计算路径点速度；

由于相邻两个轨迹点之间的距离和时间相差很小，则车辆运动的速度方程可近似如下：

其中，s'为停车距离范围内各个路径点到车辆当前位置的距离；

根据公式(12)有：

得到车辆抵达各个路径的时间，则有：

v_t＝v-at (14)

通过公式(14)计算得到各个路径点对应的速度值，对速度大于1m/s的路径点进行速度限幅，设置为1m/s；随着车辆行驶，速度逐渐减小，当速度小于一定值时，速度方程无解，在当前位置停车。

(3)剪除后续轨迹点，插入相同位置点；

该过程是为了使车辆能够在指定位置停下，一方面通过剪除后续轨迹点，确保车辆不会继续运动，另一方面，在停车位置插入一定数量位置相同，速度加速度为零的点，给下游的控制系统足够的点去预瞄，使得车辆能够及时停下。

步骤508、实时判断车辆当前的位置、姿态与车辆在终点的位置、姿态的差值，是否都在阈值范围内，如果是，则车辆抵达终点，车辆掉头过程结束，生成停车轨迹。否则，根据规划起点调用规划算法，进行新的规划。

在车辆行驶过程中每隔100ms重新执行一次上述流程，根据环境信息不断进行轨迹的更新，最终实现车辆掉头。

如图8所示，图中黑色车辆从车道一行驶到的车道二过程即为车辆的掉头过程；本发明主要分为两个过程：车辆调头场景构建和路径规划；

其中，车辆调头场景中，从基于参考线的路径规划中获取轨迹点，从中提取出道路的曲率信息，直到前方有车辆掉头场景；然后，选择进行开放空间规划的触发点和终点，进行路径规划；

开放空间路径规划中，首先，生成道路区域边界以及障碍物信息，同时进行相应处理，生成车辆掉头的感兴趣区域。其次，调用规划算法，得到一条从起点到终点的规划轨迹。根据规划轨迹是否包含倒车过程，进行轨迹的分割以及轨迹点信息处理。最后，根据接收的实时环境信息，对规划轨迹进行碰撞检测。在车辆行驶过程中每隔100ms重新执行一次上述流程，根据环境信息不断进行轨迹的更新。

开放空间路径规划方法和基于参考线的路径规划方法的一个重要不同就是：开放空间路径规划方法允许车辆进行倒车，模仿了人在实际驾驶过程中行为：对于难以一次性通过的道路，通过倒车进行车辆的“腾挪”，从而顺利通过。

进入车辆掉头场景后，决策规划算法将分为四个步骤逐一实现掉头轨迹的输出，具体流程如图9所示：

其中：ROI_Decider：生成感兴趣区域，决定能否进行车辆调头；Trajectory_Provider：接收来自ROI_decider的信息，生成掉头轨迹；Trajectory_Partiton：接收来自Trajectory_Provider的轨迹，进行轨迹的分割；Fallback_Decider：判断生成的轨迹是否会与当前道路环境中的障碍物发生碰撞，从而调动重规划的执行。

ROI_Decider的算法流程如下：

1)初始化环境和本车状态

从车辆定位模块中获得车辆的状态信息，主要是车辆的位置和姿态信息；从预测和地图模块中获得障碍物和周围道路环境信息；从上游模块接收开放空间规划的触发点和终点信息。

2)预处理环境信息，判断是否满足车辆掉头条件；具体如下：

首先，判断能否从上游模块获得车辆掉头的触发点和终点位置进行。若无法得到触发点和终点位置信息，则说明错误进入车辆掉头场景或上游模块出现异常，报告错误信息，车辆停止在当前；若能够正常获得终点位置信息，则执行下一步。

然后，从上游模块获得的终点位置信息以坐标形式表示，为使得自动驾驶车辆更加准确抵达目标位置，将原本的车辆停车位置从坐标表示转换成矩形区域表示。

最后，在预处理终点的位置信息后，还需要获得终点位置周围的道路边界信息，通过道路边界信息得到开放空间规划的感兴趣区域；从开放空间规划的触发点位置开始到终点位置后约15m的范围区间内，进行道路边界采样，将道路边界中曲率变化明显的边界点以及道路宽度有明显变化的边界点放入边界信息模块中，同时，根据能否将获得的道路边界信息处理成凸约束的形式，决定能否获得车辆掉头的感兴趣区域。

Trajectory_Provider具体为：

在获得了车辆的状态信息，车辆掉头区域内的道路环境和障碍物信息以及终点位置信息后，进行车辆的轨迹规划过程，以上述信息为输入，输出一条从规划起点到终点位置的无碰撞平滑路径，具体流程如下：

首先，判断上一次规划的轨迹(如果有)是否会与障碍物发生碰撞；若发生碰撞，从车辆当前位置开始重新规划路线，否则，获取上一次轨迹的最后一个轨迹点作为规划起点来规划路线；

然后，将车辆当前的位置、姿态同目标点的位置、姿态进行对比，若二者在位置和姿态上的差值都在阈值范围内，则认为车辆抵达目标位置，此时会生成停车轨迹结束。如果车辆尚未抵达目标位置，则调用规划算法，进行新的规划。

Trajectory_Partiton具体为：

(1)规划轨迹分割

(2)根据车辆当前的位置以及轨迹信息，对轨迹点的距离s，时间t进行对应的调整。

Fallback_Decider具体为：

轨迹规划过程每隔100ms执行一次，若在两次规划的中间过程中车辆行驶轨迹上出现障碍物，则车辆会发生碰撞危险，因此需要根据实时的环境信息对车辆行驶轨迹进行碰撞检测。碰撞检测模块以感知实时传入的环境信息以及规划的轨迹结果为输入，若规划轨迹与当前环境中障碍物存在碰撞风险，则进行相应处理，并向轨迹规划模块反馈碰撞信息，触发轨迹重规划过程。

车辆掉头规划总体结构如图10所示，首先，场景决策模块(Scenario Manager)通过参考线(Reference_line)信息判断是否进入车辆掉头场景，若进入车辆掉头场景，则ROI_decider模块接收预测、Routing、定位、地图信息信息，以及从场景决策模块传入的规划触发点和终点信息，对信息进行预处理，根据当前的信息生成车辆掉头的感兴趣区域，判断车辆能够进行掉头。若车辆满足掉头条件，则根据车辆的动力学模型以及Hybrid A*路径规划算法进行轨迹规划，由于在Hybrid A*规划的轨迹结果会包含前进和倒车两个不同的车辆状态，为使车辆能够在两个状态之间进行切换，则需要对规划轨迹进行相应的处理，即轨迹分割，将车辆的不同状态分离开来。由于车辆周围的环境信息在时刻变化，因此在轨迹分割完成后，需要根据当前的环境对规划的轨迹进行碰撞检测(Collision Check)，若发生碰撞，则对轨迹进行相应处理，同时会将信息传递给轨迹规划模块，触发重规划过程。最后将安全的无碰撞轨迹输出到控制模块，使车辆按照规划的轨迹行驶。上述的流程每隔100ms执行一次，确保车辆行驶过程中的安全。

经过上述整个流程设计，最终实现了车辆掉头功能。

Claims (8)

1.一种适用于自动驾驶车辆掉头的路径规划方法，其特征在于，具体步骤如下：

首先，针对当前自动驾驶车辆，获得该车辆前方150m范围内道路Reference_line上的所有轨迹点信息，并计算Reference_line上各个轨迹点处的曲率；

然后，判断是否存在至少三个连续轨迹点的曲率都大于阈值范围，如果不存在，车辆继续跟随原本的Reference_line规划路径行驶；否则，认为前方有车辆掉头场景，从满足阈值要求的连续轨迹点中，将第一个满足曲率阈值要求的连续轨迹点作为开放空间规划的触发点，最远的连续轨迹点作为开放空间规划的终点；

最后，实时判断车辆和触发点的距离s是否小于15m，如果是，进行开放空间规划，得到车辆掉头的规划轨迹；否则，车辆继续按照原本的路径行驶，直到距离满足阈值要求。

2.如权利要求1所述的一种适用于自动驾驶车辆掉头的路径规划方法，其特征在于，所述的曲率计算公式如下：

其中：

为道路Reference_line上相邻轨迹点之间的角度差值；Δs为道路Reference_line上相邻轨迹点之间曲线的弧长。

3.如权利要求1所述的一种适用于自动驾驶车辆掉头的路径规划方法，其特征在于，所述的曲率阈值的设置根据车辆的最小转弯半径R确定，计算公式为：

其中：L为车身长度，ψ为车辆方向的最大转角。

4.如权利要求1所述的一种适用于自动驾驶车辆掉头的路径规划方法，其特征在于，所述的进行开放空间路径规划，具体步骤为：

步骤501、根据车辆的长、宽信息以及车辆抵达终点位置的姿态或角度，预处理得到车辆在终点位置停车的矩形区域；

将原本车辆在终点处的停车位置从坐标表示转换成矩形区域表示，矩形区域的信息以其四个顶点的坐标位置来表征；

步骤502、根据车辆进入开放空间规划后的环境信息以及车辆信息，生成车辆掉头的感兴趣区域；

步骤503、在车辆掉头的感兴趣区域中，利用Hybrid A*的路径搜索算法，规划一条从起点到终点的无碰撞轨迹；

步骤504、遍历轨迹上的所有轨迹点，根据是否包含倒车过程，插入轨迹点后进行轨迹的分割；

步骤505、对分割后的每个轨迹段进行编码，车辆按规划的轨迹行驶，根据各轨迹点和车辆的交并比IOU，得到车辆实际位置对应的轨迹点；

步骤506、实时判断车辆行驶过程中是否有障碍物，如果是，进入步骤507；否则，进入步骤508；

步骤507、判断车辆是否与障碍物发生碰撞，如果是，以车辆当前实际位置的轨迹点为规划起点，返回步骤503重新规划；否则，将该轨迹的最后一个轨迹点作为规划起点，返回步骤503重新规划；

步骤508、实时判断车辆当前的位置、姿态与车辆在终点的位置、姿态的差值，是否都在阈值范围内，如果是，则车辆抵达终点，车辆掉头过程结束；否则，根据规划起点调用规划算法，进行新的规划。

5.如权利要求4所述的一种适用于自动驾驶车辆掉头的路径规划方法，其特征在于，所述的步骤502具体为：

从触发点开始，到终点位置后约15m的位置结束，整个区间内，以0.5m的距离分辨率进行道路边界采样，从采样点中选择道路中曲率变化明显的边界点和路宽度有明显变化的边界点加入到道路边界信息中；判断能否将道路边界处理成凸约束的形式；如果是，则道路边界信息处理完成，所有的道路边界点构成的区域为车辆掉头的感兴趣区域；否则，无法获得车辆掉头的感兴趣区域，报告错误信息，车辆停止在当前位置；

曲率变化明显的点是指：拟合的道路边界曲线上相邻采样点之间切线角度的差值大于0.15rad，则认为这两个采样点之间的道路边界有明显的曲率变化，将采样点信息加入到道路边界信息中；

道路宽度明显变化是指：车辆当前轨迹点与前轨迹点形成的角度的正切值tan(theta1)，车辆当前轨迹点与后轨迹点形成的角度正切值tan(theta2)，若二者的差值大于阈值0.4，则认为当前轨迹点所在处的道路宽度与前后相比有较为明显的变化，将其加入道路边界信息中。

6.如权利要求4所述的一种适用于自动驾驶车辆掉头的路径规划方法，其特征在于，所述的步骤504具体为：判断当前轨迹点与后一轨迹点之间的角度差值是否大于90度，如果是，在当前轨迹点位置插入大量位置相同，速度、加速度为零的点，作为分割用的预瞄点；否则，不予处理，继续遍历下一个轨迹点，直至遍历完毕所有轨迹点；

在轨迹分割过程中，将车辆正向前进和倒车的轨迹按照次序分段存储，其中，轨迹分割的位置点既是前一段轨迹的最后一点也是下一段轨迹的起点。

7.如权利要求4所述的一种适用于自动驾驶车辆掉头的路径规划方法，其特征在于，所述的步骤505具体为：

首先，遍历每一个轨迹段的每一个轨迹点，将车辆在该轨迹点处的位置和姿态，与当前车辆实际的位置和姿态作差，判断俩个差值是否都在阈值范围内，如果是，通过车辆当前的位置和车辆的长，宽参数信息建立车辆的矩形ego_box和轨迹点所在位置的矩形path_point_box，计算二者的交并比IOU，记录当前轨迹点的信息，包括轨迹点所在的轨迹段，轨迹点在轨迹段中的索引，IOU值；否则，不予处理，重复上述过程，直到遍历完所有轨迹段；

然后，将保存的各轨迹点按IOU值从大到小排序，依次检验，删除历史轨迹点后，最大的IOU值对应的轨迹点，即为与车辆当前实际位置对应的轨迹点；

在确定车辆当前位置后，以车辆当前所在的轨迹点为矩阵，对后续轨迹点距离和时间进行调整，从而减小在车辆行驶过程中所带来的误差。

8.如权利要求4所述的一种适用于自动驾驶车辆掉头的路径规划方法，其特征在于，所述的步骤507中，碰撞检测是指：根据车辆所在的位置和长、宽参数，建立车辆的矩形ego_box，从预测模块得到障碍物的矩形obstacle_box，采取分离轴定理检测二者是否发生重叠，如果是，则说明车辆在该轨迹点会和障碍物发生碰撞，如果存在以下两种情形则认为会发生碰撞：1)：从碰撞检测模块中接收到反馈的碰撞信息；2)车辆的速度和加速度低于0.01。

Images