CN113619603B - 一种双阶段自动驾驶车辆调头轨迹规划方法 - Google Patents

Abstract

本发明公开了一种双阶段自动驾驶车辆调头轨迹规划方法，本发明基于车辆运动学模型约束，使用数学物理方程合理判断调头位点以及所需的倒车调姿，推导出理论最优轨迹并对是否可以一次完成调头给出判断；接着在此基础上通过智能仿真决策算法实现了一种结合全局欧氏距离场和改进的局部动态窗口法得到的无人驾驶车辆调头轨迹生成方案，可以在更复杂的对侧道路条件下规划出合理的调头路径，满足自动驾驶需求，兼具实时性和高鲁棒性。本发明车辆调头轨迹规划方法中加入调头的可行性判断这一步骤，使得整个方法的计算量小，同时可以避免车辆调头过程的碰撞；调头引导点生成的引导效果更好，有“逐步引导”的作用，使得在车辆调头轨迹规划效果更佳。

Description

一种双阶段自动驾驶车辆调头轨迹规划方法

技术领域

本发明涉及无人驾驶车辆的轨迹规划领域，特别是一种双阶段自动驾驶车辆调头轨迹规划方法。

背景技术

自动驾驶涉及环境感知、智能决策和规划、智能控制等多门学科，其中“如何利用环境信息进行智能决策”又成为了限制无人驾驶发展的关键技术和瓶颈技术。

本发明从自动驾驶中实际的调头场景出发，根据实际的车辆运动学和机械结构约束，通过建立数学方程模型和智能仿真决策算法模型并将两者有机的结合，解决了实际情况中的车辆调头场景路径规划问题，并通过程序对完整调头过程进行模拟。

发明内容

本发明的目的在于克服现有技术的不足，提供一种双阶段自动驾驶车辆调头轨迹规划方法，以解决上述技术背景中提出的问题。

为实现上述目的，本发明通过以下技术方案来实现：

一种双阶段自动驾驶车辆调头轨迹规划方法，包含以下步骤：

步骤一、前置准备，包括：

1.1、车辆模型中车辆控制点的选取；

1.2、车辆模型的数理方程约束条件；

1.3、车辆当前地图获取：获取当前时刻以车辆为中心前后左右50m的正方形地图；

步骤二、车辆调头的可行性判断

2.1、在车辆进入调头路口区域地图，即车辆当前地图为调头路口区域时，先通过步骤1.2中的数理方程约束条件判断在车辆在理想情况下能否一次掉头；

2.2、若能一次掉头，则进入后续阶段的自动规划，若不行，则给出理想情况下需要倒车几次完成调头，并绘制出调头、倒车路径，辅助驾驶员进行倒车调头；

步骤三、车辆调头引导点生成

3.1、对车辆当前时刻地图(调头路口区域部分)进行欧氏距离变化，并设置自适应阈值生成安全区域；

3.2、使用改进的A-star算法在3.1得到的安全区域生成引导路径；

3.3、对引导路径进行离散化取点得到引导点集，引导点集所含的引导目标点作为后续步骤的目标点；

步骤四、使用DWA算法完成起始点到目标点的车辆调头规划路线

4.1、根据车辆运动空间，获得在时间间隔内所有可能的车辆速度和车轮转角组合；

4.2、针对每一种车辆速度和车轮转角组合，得到经过下一时间间隔后车辆的位置，从而得到每种车辆速度和车轮转角组合所对应的预测轨迹；

4.3、对每一预测轨迹计算一个代价，并选代价最小的那个作为确切的下一步；

4.4、重复4.1-4.3，获得车辆调头的最佳轨迹，即车辆调头规划路线。

上述技术方案中，车辆模型使用车辆后轴中心作为控制点。

上述技术方案中，车辆模型的数理方程约束条件包括下述方程：

其中，方程(1)表示车辆的运动学约束方程s.t.，方程(2)表示车辆的轨迹方程，方程(3)为定解方程；

方程(1)中，a_min、a_MAX分别表示车辆的最小、最大加速度,

表示车辆加速度，

表示方向盘转角，

表示方向盘转角最大值，

表示方向盘转速，Ω_MAX表示方向盘转速最大值，k表示传动比(方向盘转k°，车轮转1°)，θ表示车轮转角，

表示车轮转速，t为时间，A为坐标系中车辆轮廓点的集合，C为坐标系中障碍物轮廓点的集合，

表示A和C的交集为空集——表示车辆不能与任何障碍物发生碰撞；

方程(2)中，(x_c(t),y_c(t))为当前时刻t的车辆控制点坐标，(x₀,y₀)为车辆控制点的初始坐标，ω为控制点角速度，v为车辆速度；

方程(3)中，R(t)为时刻t的转弯半径；θ(t)为时刻t的车轮转角,ω(t)为时刻t的车辆控制点角速度，v(t)为时刻t的车辆速度，l为车辆轴距；

在方程(1)的约束条件下，根据方程(3)解出v和ω，带入方程(2)得到车辆在理想状态下的轨迹。

上述技术方案中，步骤2.1中，通过步骤1.2中的数理方程约束条件判断在车辆在理想情况下能否一次掉头，具体步骤为：

2.1.1获得车辆控制点开始调头时的坐标(x₀,y₀)，以及开始调头时的速度v，

2.1.2根据步骤1.2中确定的数理方程约束条件，得到理想状态下整个调头过程中车辆控制点的运动轨迹；

2.1.3进行能否一次完成调头的判断：

根据车身的几何尺寸，得到车辆左、右前端点的横坐标与车身控制点横坐标的相对位置关系，能够一次完成调头的判定条件为：

式(4)中，x_l(t)＝x_c(t)-L·cos(α_l-γ)

x_r(t)＝x_c(t)+L·cos(α_r+γ)

方程(4)中，x_l(t)为车辆左前端点的横坐标，x_r(t)为车辆右前端点的横坐标，x_c(t)车身控制点横坐标，x_bord为对向道路外边界的横坐标，L为控制点到左右前端点的距离，α_l为控制点和左端点的连线与x轴负向夹角、α_r为控制点和右端点连线与x轴正向夹角、γ为控制点速度方向与y轴正方向夹角；

2.1.4若2.1.3判断能一次掉头，则进入后续阶段的规划。

上述技术方案中，步骤3.1中，生成安全区域的具体方法为：

3.1.1对车辆当前时刻地图进行二值化处理，将道路边界、障碍物的闭合边缘设为1，即背景点为1，其余可行驶区域设为0，即前景点为0；

3.1.2计算出地图上可行驶区域中每个前景点距离其最近障碍，即背景点的欧式距离；

3.1.3根据3.1.2计算出的欧氏距离图的梯度信息设定自适应阈值，计算筛选出在有可通行道路的前提下距离障碍较远的位点；

3.1.4生成安全行驶区域的二值化地图，不安全区域设为1，安全行驶区域设为0。

上述技术方案中，步骤4.1中，车辆运动空间的符合下述三个约束条件：

4.1.1首先根据一般车辆运动的运动学方程，经过一段时间微元dt后车辆位置变化具体表现的公式如下：

式中x_t、y_t为经过时间微元dt后车辆控制点在俯视方向上的二维坐标，x₀、y₀为当前时刻车辆控制点的二维坐标，θ₀、θ_t则分别为时间微元dt前后的车辆姿态角或车轮转角，v为该时间微元内车辆线速度，ω为该时间微元内车辆角速度；

4.1.2根据4.1.1的运动学方程，对车辆在时间间隔内的转角和加减速进行限制，体现在动态窗口采样中，即速度的增量不能超过速度加速度与时间间隔的乘积、车体姿态角的增量不能超过角加速度与时间间隔的乘积；

4.1.3一般车辆对速度和轮胎转角会有机械约束上的限制，结果呈现为车辆速度和轮胎转角不可超过某一阈值。

上述技术方案中，步骤4.2中，每一种车辆速度和车轮转角组合，以步骤4.1.1所述的运动学方程进行模拟，得出预测轨迹，将所有速度和转角组合的预测轨迹进行暂时存储形成轨迹空间，以便后面步骤进行最佳的轨迹选择。

上述技术方案中，步骤4.3中，对每一预测轨迹计算一个代价，代价cost的评价函数表达式为：

cost＝cost_goal+cost_security+cost_speed

其中，cost_goal代表目标点代价，cost_security代表障碍物代价，cost_speed代表速度代价；

1)所述目标点代价函数为：

cost_goal＝W_angle×cost_angle+W_distance×cost_distance

其中cost_goal代表目标点代价函数，cost_angle和cost_distance分别代表姿态角的代价和距离的代价,W_angle和W_distance分别代表姿态角的代价的权值和距离的代价的权值；

其中，cost_angle＝tan(θ_car-θ_goal)

其中，θ_goal和θ_car分别代表目标点和车辆目前的姿态角，x_goal和y_goal代表目标点的横、纵坐标，x_t和y_t分别代表车辆目前(时刻t)的横、纵坐标。

2)所述障碍物代价函数为：

其中，cost_security代表安全代价函数，W_wall和W_ob分别代表车道线边界的和障碍物的权重，r_{wall_min}和r_{ob_min}分别代表车辆距离车道线边界和障碍物的最近距离；

3)所述速度代价函数为：

cost_speed＝W_speed×(v_max-v_t)

其中，cost_speed代表速度代价函数、W_speed代表速度代价的权重，v_t代表当前(时刻t)车辆速度，v_max代表车辆最大速度。

与现有技术相比，本发明的有益效果是：

1、本发明车辆调头轨迹规划方法中加入调头的可行性判断这一步骤，使得整个方法的计算量小，同时可以避免车辆调头过程的碰撞；

2、本发明中，调头引导点生成的引导效果更好，有“逐步引导”的作用，使得在车辆调头轨迹规划效果更佳。

3、本发明基于车辆运动学模型约束，使用数学物理方程合理判断调头位点以及所需的倒车调姿，推导出理论最优轨迹并对是否可以一次完成调头给出判断；接着在此基础上通过智能仿真决策算法实现了一种结合全局欧氏距离场和改进的局部动态窗口法得到的无人驾驶车辆调头轨迹生成方案，可以在更复杂的对侧道路条件下规划出合理的调头路径，满足自动驾驶需求，兼具实时性和高鲁棒性。

附图说明

图1为本发明的流程图；

图2为阿克曼转向原理图；

图3为选择车身控制点时车身结构分析图；

图4为数理方程硬约束下车辆理想调头轨迹形状示意图；

图5为一次完成调头示意图；图中浅灰色矩形为车辆，星号“×”表示车辆控制点，弯曲的虚线为车辆轨迹，深灰色长矩形为道路中央隔离带，对侧道路虚线为车道分隔线，黑色实线为对向车道外边界真实位置，左侧黑色虚线与黑色实线的水平距离为0.2m，0.2m的距离为实际算法中为了与路边保持安全距离使用的阈值；

图6.1、6.2为向右平移对向车道外边界，使对向道路变窄至车辆右或左前端点碰撞时的示意图；

图7为继续向右平移对向车道外边界，使对向道路缩短到一次倒车可以调头的极限情况；

图7.1为图7的局部放大图；

图8.1表示对障碍和道路边界进行边界提取后的示意图；

图8.2表示使用欧式距离变换后进行可视化的效果图；

图8.3为使用自适应阈值得到的安全区域和危险区域的示意图，其中非小块封闭的白色部分为安全区域，黑色部分和小块封闭的白色部分为危险区域或不可达区域；

图9为运动学约束示意图；

图10为碰撞检测；灰色矩形代表车辆，两边黑色长条和L形表示障碍物；

图11为障碍物代价函数选取示意图；图中，左边的长条表示边界，右边的五边形和六边形表示障碍物；

图12.1和图12.2均为复杂场景下阶段2规划效果；图12.1中实线矩形为车辆，并采用车辆位置连成车辆轨迹，图12.2中，曲线表示车辆轨迹，实线矩形为车辆，

图13.1和图13.2均为考虑交通规则(斑马线不能进行调头)的情况下，阶段2规划效果；图13.1和图13.2中，横向设置的矩形框表示斑马线；

图14.1至14.8为在有移动障碍(对向来车)的情况下，阶段2规划效果；图14.1至图14.8中在对向车道上的虚线矩形为移动障碍(对向来车)、且对向来车在图14.1至图14.8的位置不同，表示其移动。

具体实施方式

以下通过特定的具体实例说明本发明的实施方式，本领域技术人员可由本说明书所揭露的内容轻易地了解本发明的其他优点与功效。本发明还可以通过另外不同的具体实施方式加以实施或应用，本说明书中的各项细节也可以基于不同观点与应用，在没有背离本发明的精神下进行各种修饰或改变。需说明的是，在不冲突的情况下，以下实施例及实施例中的特征可以相互组合。

需要说明的是，以下实施例中所提供的图示仅以示意方式说明本发明的基本构想，遂图式中仅显示与本发明中有关的组件而非按照实际实施时的组件数目、形状及尺寸绘制，其实际实施时各组件的型态、数量及比例可为一种随意的改变，且其组件布局型态也可能更为复杂。

本发明提供了一种双阶段自动驾驶车辆调头轨迹规划方法，基于车辆运动学模型约束，通过两个阶段进行自动驾驶车辆调头轨迹规划：阶段1、使用数学物理方程合理判断调头位点以及所需的倒车调姿，推导出理论最优轨迹并对是否可以一次完成调头给出判断；阶段2、在阶段1判断可一次调头的基础上通过智能仿真决策算法实现了一种结合全局欧氏距离场和改进的局部动态窗口法得到的无人驾驶车辆调头轨迹生成方案。

阶段1判断是否可以一次调头或需要几次完成调头，阶段2不进行多次调头的“最优”路径规划，因为多次调头只需要算出极限情况即可按照每次打满的动作在低速下完成调头动作，不含有可选择性，因此不像一次调头可完成的情况那样需要进行最优路径规划；

参阅图1，一种双阶段自动驾驶车辆调头轨迹规划方法具体包含以下步骤：

阶段1：

步骤一、前置准备：建立车辆的运动学习模型

1.1、车辆模型中车辆控制点的选取；

1.1.1转向原理及车辆控制点的选取：

显示应用中，绝大部分机动车采用的转向方式为前轮转向后轮驱动的阿克曼转向结构。其理论转向特性是以汽车前轮定位角均为零、行走系统为刚性、汽车行驶过程中无侧向力为假设条件的。该转向特性的特点为：

1、汽车直线行驶时，4个车轮的轴线都相互平行，且垂直于汽车纵向中心面；

2、汽车在转向行驶过程中，全部车轮都必须绕一个瞬时中心点做圆周滚动，而且前内轮与前外轮的转角应满足以下关系式：

式中α、β分别为汽车前内、外轮转角，W为车宽，l为车辆轴距。

后续调头路径的规划中，部分步骤需要将对象车辆视为这点，所以需要结合阿克曼转向原理和车辆结构，选择调头模型中对象车辆上的某一点作为控制点。

参阅图2，由阿克曼转向原理可知，阿克曼转向两个前轮转角不一致，查阅资料可知市面上一般车辆最大角度转向时，内外侧车轮转角差不超过10°，为了方便后续建模，我们需要将这两个角度做一个统一。实际中，车辆的前轮转角是由方向盘的转角通过传动装置控制的，所以我们选择前轮中轴处作为统一车轮转角的位置(参阅图3)，由方向盘转过角度和传动比的商作为统一车轮转角的度数。统一角度之后，有几何关系可以得到车辆转弯时的半径为：

其中l为车辆轴距；

式中R为车辆转弯半径，θ为车辆转角；

车辆控制点一般选取在车辆对称轴上，用来描述车辆的运动轨迹和执行对车辆的控制。在阿克曼转向的情况下，车辆运动学模型往往使用车辆后轴中心作为控制点。这是因为，一般在车辆转弯时从安全角度考虑，会有曲率有上限限制，应尽量选择对称轴上选择曲率最大、即曲率半径最小的点作为控制点，这样可以保证如果控制点满足了曲率要求，对称轴上其他点一定也满足曲率要求。由阿克曼转向原理可知，车辆转向中心位于后轴的延长线上，故选取转向半径最大的后轴中心点作为控制点。

1.1.2车辆模型：

为了方便进行自动驾驶车辆调头轨迹规划方法的叙述，我们假设车辆的运动学参数如下：

车长4.3m、车宽W为2m、轴距l为2.8m，传动比k为16(方向盘转16°，车轮转1°)、方向盘最大转角

为470°、方向盘最大转速Ω_MAX为400°/s，车辆加速度取值范围为[-5,3]m/s²；

1.2车辆的实际模型，满足数理方程约束条件(得到后续用于阶段1判断、阶段2的应用的车的数理方程约束)

车辆模型的数理方程约束条件包括下述方程：

其中，方程(1)表示车辆的运动学约束方程s.t.，方程(2)表示车辆的轨迹方程，方程(3)为定解方程；

方程(1)中，

表示车辆加速度，

表示方向盘转角，

表示方向盘转速，θ表示车轮转角，θ_max表示车轮转角最大值，

表示车轮转速，t为时间，A为坐标系中车辆轮廓点的集合，C为坐标系中障碍物轮廓点的集合，

表示A和C的交集为空集——表示车辆不能与任何障碍物发生碰撞；

方程(2)中，(x_c(t),y_c(t))为当前时刻t的车辆控制点坐标，(x₀,y₀)为车辆控制点的初始坐标，ω为控制点角速度，v为车辆速度；

方程(3)中，R(t)为时刻t的转弯半径；θ(t)为时刻t的车轮转角,ω(t)为时刻t的车辆控制点角速度，v(t)为时刻t的车辆速度；

在方程(1)的约束条件下，根据方程(3)解出v和ω，带入方程(2)得到车辆在理想状态下的轨迹。

1.3车辆当前地图获取：

由于阶段1中的后续所有步骤都涉及到在坐标系下使用解析法进行分析、阶段2中的后续所有步骤都需要涉及到在环境地图的信息基础上进行规划，因此我们需要首先获得车辆周围的环境情况。由于本专利主要涉及在已知环境地图前提下的轨迹规划，且目前激光雷达建图、视觉建图、高精度地图的技术较为成熟，因此，我们假设俯视视角下的车道、障碍信息已知。

车辆每个时刻当前的地图，以车辆为原点，获取车辆周围50m范围内的地图分布和背景交通流、背景障碍分布，将地图和背景交通流、背景障碍映射到俯视画面中，得到转换后的车辆周围环境地图。

首先，启用我们这个调头算法的前提是车辆已经接近路口、且在这个路口有调头需求。这一点可以通过环境地图判断，涉及到GPS导航也不再赘述。

其次，下文中涉及到的除“示意图”以外的部分，所使用的均为此张地图，为方便展示，对下述步骤中涉及的地图进行了切分，只保留了与调头相关的部分(15*32)，并且重新设置了坐标系。

步骤二、车辆调头的可行性判断

2.1、在车辆进入调头路口区域地图，即车辆当前地图为调头路口区域时，先通过步骤1.2中的数理方程约束条件判断在车辆在理想情况下能否一次掉头；

在车辆当前地图，即调头路口区域地图(使用环境和交通流信息进行建图)，并按照如下步骤实施：

2.1.1获得车辆控制点开始调头时的坐标(x₀,y₀)，以及开始调头时的速度v；

2.1.2由于考虑的是极限情况下的车辆调头步数，因此我们倾向于使得在所有步骤中车辆的调头半径尽可能小、调头占用的道路空间尽可能少。由阿克曼转向原理和车辆结构可知，转弯半径R与车轮转角θ的关系如下：

由该式可知，当车轮转角达到最大时转弯半径最小。为了尽快达到最小的转弯半径，应在车轮未达到最大转角时以最大转速打方向盘。由此确定约束方程中的定解方程(3)如下：

根据步骤1.2中确定的参数和运动学方程及约束条件，可以得到整个调头过程中车辆控制点的运动轨迹，得到的调头轨迹形状如图4所示。

2.1.3接下来进行能否一次完成调头的判断。

2.1.3.1根据车身的几何尺寸，得到车辆左、右前端点的横坐标与车身控制点横坐标的相对位置关系。交通情景中以左转调头情况居多，所以以左转调头为例：

x_l(t)＝x_c(t)-L·cos(α_l-γ)

x_r(t)＝x_c(t)+L·cos(α_r+γ)

式中x_l(t)和x_r(t)分别为车辆左、右前端点的横坐标，L为控制点到左右前端点的距离，α_l为控制点和左端点的连线与x轴负向夹角、α_r为控制点和右端点连线与x轴正向夹角、γ为控制点速度方向与y轴正方向夹角:

2.1.3.2假设现已有路况图如图5所示。考虑安全距离后，对向道路外边界的横坐标为x_bord＝0.2。在γ＜90°时，x_l(t)＜x_r(t)；在γ＞90°时，x_r(t)＜x_l(t)。所以当车辆转过的角度小于90°时，在此图6.2所示情况中车辆左前端点更容易与对向道路边界发生碰撞，要求满足x_l(t)＞x_bord才能不发生碰撞；当车辆转过的角度大于90°时，在图6.1所示情况车辆右前端点更容易与向道路边界发生碰撞，要求满足x_r(t)＞x_bord才能不发生碰撞。

综上，能够一次完成调头的判定条件为：

2.1.4若2.1.3判断能一次掉头(参阅图5)，则进入后续阶段的规划；

在无法一次完成调头的情况下，考虑倒一次车完成调头。由于考虑的是极限理想情况下需要完成调头动作的步数，所以在开始倒车前先将方向盘打满，再开始倒车的过程；倒完车后，同样考虑先将方向盘打满，再继续调头的过程。

2.1.4.1当判断车辆左前端或右前端在调头过程中会与对向道路外边界发生碰撞时，取左前端或右前端恰不发生碰撞的时刻t₀开始倒车。由于现将方向盘打满再开始倒车，所以此时倒车轨迹是一可确定的圆弧，且圆心与调头时的圆心关于车身控制点对称，即：

式中(x₁,y₁)为调头时圆心坐标，(x₂,y₂)为倒车时圆心坐标，(x_c(t₀),y_c(t₀))为开始倒车的t₀时刻车辆控制点的位置。

2.1.4.2基于2.1.4中假设的倒车调头情景，在倒车轨迹确定以后，对于倒车轨迹上的每一个控制点位置，都能确定一个“若此时停止倒车，继续调头”时的调头轨迹。所以在倒车过程中不断重复步骤2.1.3.2的判定，当此时对应的“继续调头轨迹”满足2.1.3.2中一次能够完成调头的约束条件时，停止倒车过程，开始第二次调头过程。

2.1.4.3当对向车道过窄时，按照2.1.4.2的步骤，为了找到倒车轨迹上满足2.1.3.2条件的点，车辆可能需要倒车较远的距离。当倒车距离过长，车辆再执行第二次调头时可能会因为无法满足步骤1.2中的基本约束条件中的

而与环境中的其他一些障碍物发生碰撞而无法完成调头。其中最容易发生碰撞的就是分割对向车道的道路中间护栏。此时只倒车一次无法完成调头，但是考虑实际情况市内主干道至少都是双向4车道，出现一次倒车都无法完成调头的情况极其少见，且若是需要多次倒车步骤进行车辆调头，策略与1.2.4类似，所以不再重复讨论。图7和图7.1给出了一种一次倒车能完成调头的极限情况，此时的情况就是倒车距离使得车辆第二次调头时恰好满足2.1.3.2中的一次调头条件，且使得车辆满足1.2中的基本条件

即恰不与路中间隔离带发生碰撞

阶段2：

使用软约束进行符合车辆运动学、符合乘客坐乘体验的调头路径规划算法。算法分为两部分：全局路径规划和局部路径规划。前者主要目标为生成离散的路径点，在局部导航中作为逐步推进的引导目标，后者则以此为参考，动态规划出合理路径。

全局路径规划算法如下：

步骤三、车辆调头引导点生成

3.1首先，对前序步骤中建立的俯视地图进行数字图像处理，生成安全区域，如图8.1、8.2和8.3所示；具体如下：

3.1.1对地图(车辆当前时刻地图，即调头路口区域部分地图)进行二值化处理，将道路边界、障碍物的闭合边缘设为1(背景点)，其余可行驶区域设为0(前景点)；

3.1.2计算出地图上可行驶区域中每个前景点距离其最近障碍(背景点)的欧式距离；

3.1.3根据1.2计算出的欧氏距离图的梯度信息设定自适应阈值，计算筛选出在有可通行道路的前提下距离障碍较远的位点；

3.1.4生成安全行驶区域的二值化地图，不安全区域设为1，安全形势区域设为0。

3.2使用改进的A-star算法在3.1得到的安全区域生成引导路径；

其中，改进的A-star算法是在原有A-star算法算法基础上，增加了算法探索路径时向各个方向搜索的权重，使得生成的路径尽可能贴合安全区域内边界，得到更具有显著特征的引导路径。

3.2.1根据对侧道路的障碍选定目标车道，以减少车辆在掉头过程中的压线情况

3.2.2使用A_star算法在步骤3.1得到的安全区域中规划出最短路径。由于我们倾向于尽量使此引导路径以45°方向延伸，尽可能减小引导路径朝向与车体姿态的方差，因此我们调整了A_star中的路径搜寻规则，使其沿斜线搜索代价更低；为了使得全局引导路径更贴合模型一中的规划的理想路线，我们改进A_star算法使其更易在x轴方向上延伸，即生成的路径会拥有尽可能大的曲率半径。

3.3根据步骤3.2规划出的全局路径得到引导点集，引导点集所含引导目标点作为后续步骤的目标点；

3.3.1对得到的路径进行等间距离散采点，得到一系列用于引导局部路径规划模块中动态窗口算法进行路径规划的引导目标点，即引导点集。

步骤三的目标是生成离散的引导目标点，为步骤四的局部规划做准备，这个步骤中生成的路径也不是最终路径，但是会影响最终结果的好坏：加入这步的优点是更好的引导DWA算法，降低碰撞的概率，也可以是说步骤四中DWA算法的优化。

在全局路径的导引目标点基础上，由于附近障碍物可能由于视野问题被遮挡，导致调头过程中周围环境信息发生变化，加之在实际的车辆行驶问题中，障碍物的情况是复杂多变的，因此无人车轨迹需要随着调头的过程而实时更新。这里采用局部路径规划算法来满足此需求，具体的算法步骤如下：

步骤四、使用DWA算法完成起始点到目标点(此处，目标点是步骤3.3所述的引导点集。DWA算法中，在这个目标点集中选取车辆当前位置前方的目标点作为引导目标点)的车辆调头规划路线

4.1在某一运动时刻t，在车辆的运行空间中，对时间间隔dt内的车辆速度和转角进行动态窗口采样；

4.1.1车辆运动空间受到车辆运动学和机械结构的约束，运动学约束示意图如图9所示，具体的限定方法如以下步骤：

4.1.1.1首先根据一般车辆运动的运动学方程，经过一段时间微元dt后车辆位置变化具体表现的公式如下：

式中x_t、y_t为经过时间微元dt后车辆控制点在俯视方向上的二维坐标，x₀、y₀为当前时刻车辆控制点的二维坐标，θ₀、θ_t则分别为时间微元dt前后的车辆姿态角或车轮转角，v为该时间微元内车辆线速度，ω为该时间微元内车辆角速度。

4.1.1.2根据上一步的运动学方程，对车辆在时间间隔内的转角和加减速进行限制，体现在动态窗口采样中，即速度的增量不能超过速度加速度与时间间隔的乘积、车体姿态角的增量不能超过角加速度与时间间隔的乘积。

4.1.1.3另外的，一般车辆对速度和轮胎转角会有机械约束上的限制，结果呈现为车辆速度和轮胎转角不可超过某一阈值。

以上三个约束共同构成车辆的可行运动空间。

4.1.2在上述步骤的可行运动空间确定后，我们对时间间隔dt内的车辆速度和转角进行动态窗口采样，具体步骤如以下：

4.1.2.1对车辆速度的可行范围内，进行一定分辨率(即速度增量间隔)的采样

4.1.2.2对车轮转角的可行范围内，进行一定分辨率(即转角增量间隔)的采样

4.1.2.3由于车辆在一定时间内的运动状态由速度和转角所决定，将速度采样和转角采样进行组合，即可得到时间间隔dt内的所有动态窗口采样值

4.2对每一组可能的速度和转角进行模拟时间内的前向预测，记录下每一组采样结果的模拟结果；

4.2.1对于步骤一中得到的在时间间隔dt内的可能的速度和转角采样，以前面1.1所述的运动学方程进行模拟，得出预测轨迹；

4.2.2将所有速度和转角组合的预测轨迹进行暂时存储形成轨迹空间，以便后面步骤进行最佳的轨迹选择

4.3根据给定的评价规则对每一条模拟轨迹进行评价，得出cost最小的轨迹所对应的速度和转角，并依此对下一个dt内小车运动做出控制决策；

4.3.1首先根据实际需要构建代价函数来评价上述根据运动学方程得出的所有轨迹的表现优劣状况，并得出最优的轨迹作为下一段dt内的运动轨迹。代价函数的构成如下：

4.3.1.1在实际应用中，首要需求是车辆需要到达目标点(即步骤三所得的引导点集所含的引导目标点)，因此轨迹到目标点的距离和小车方向与目标点的方向的差别需考虑在内——需要在接近目标点的同时尽可能朝着目标点的方向转向。具体如下：

4.3.1.1.1距离方面，我们采用欧氏距离计算轨迹末端到目标点的距离作为参考值，该距离越小代表轨迹朝着离目标更近的方向前进，显然离目标点越近越好，所以这里可以直接将距离作为代价。

4.3.1.1.2姿态方面，我们可以计算出车辆姿态朝向与轨迹末端所在位置与目标点连线的夹角，同样是夹角越小代表着方向越趋向正确的轨迹。

4.3.1.1.3将以上两点公式化表示为：

cost_angle＝tan(θ_car-θ_goal)

其中，cost_angle和cost_distance分别代表姿态角的代价和距离的代价，θ_goal和θ_car分别代表目标点和车辆目前的姿态角，x_goal和y_goal代表目标点的坐标，x_t和y_t分别代表车辆目前的坐标。

我们对二者取不同权值并进行线性叠加，得出最后的目标点代价函数：

cost_goal＝W_angle×cost_angle+W_distance×cost_distance

其中，cost_goal代表目标点代价函数，W_angle和W_distance分别代表姿态角的代价的权值和距离的代价的权值；W_angle和W_distance根据实际情况取值，例如，(与目标点)姿态角代价权值W_angle取值范围[0.01,0.2]，W_angle为0.15；(与目标点)距离代价权值W_distance的取值范围[0.02,0.4]，W_distance为0.15；

4.3.1.2另一方面，在车辆行驶的实际问题中，乘客的安全应该是放在首位的，所以我们还需要对碰撞检测的代价函数进行设置，具体如下：

4.3.1.2.1首先，采用碰撞检测的方法避免车辆与障碍物的碰撞。对任一轨迹上的每一个点，假设无人车会沿着轨迹行驶到此点，判断车辆到达此点时，车体的四个边界是否会与障碍有交点，如图10所示，如果此轨迹上的任意一个点与障碍有交点，则代表通过此轨迹行驶会与障碍相撞，那么就将这条轨迹的障碍代价设置为无穷大，通过这种方法，显然能把所有与障碍相撞的轨迹剔除，这样就能保证无论轨迹如何选择都不会有安全问题。

4.3.1.2.2另一方面，在碰撞检测之后，同样为了保证驾驶的安全性，车辆的行驶路线同样应该尽量远离障碍物，这里我们通过遍历一条轨迹上的所有点，找出所有点中距障碍物最近的距离，即可以代表这条曲线距离任何一个障碍物的最小距离。为了更好的使得车辆区分车道线边界和障碍物，我们对障碍和车道线边界设置了不同的权值，使得车辆对道路边界和障碍均时刻保持警惕，且降低道路边界对于车辆的轨迹的“排斥作用”，降低车辆压线行驶的持续时段。

如图11所示，线条r_{ob_min}所示距离为最近障碍物的距离，而线条r_{wall_min}所示距离为最近车道边线距离。其均满足代价与距离成反比的关系，因此取倒数作为其代价，分别乘以不同的权值

上述障碍物代价函数对应的表达式如下：

其中，cost_security代表安全代价函数，W_wall和W_ob分别代表车道线边界的和障碍物的权重，r_{wall_min}和r_{ob_min}分别代表车辆距离车道线边界和障碍物的最近距离；W_wall和W_ob根据实际情况取值，例如，车道线边界代价权值W_wall取值范围[0.02,0.2]，进一步的W_wall为0.05；障碍物代价权值W_ob取值范围[0.2,2.0]，进一步W_ob为1.0；

4.3.1.3最后，出于对无人车通行效率的追求，给出速度代价函数。当给出一定的最大速度限制时，为了追求更快完成调头的过程，我们应尽量以更高的速度行驶，所以轨迹中的速度越小，则代表着速度代价越大，由于此代价为线性，因此可表示为：

cost_speed＝W_speed×(v_max-v_t)

其中，cost_speed代表速度代价函数、W_speed代表此项的权重，v_t代表当前车辆速度，v_max代表车辆最大速度；W_speed根据实际情况取值，例如，速度代价权值W_speed取值范围[0.1,1.5]，进一步W_speed为1.0；

4.3.1.4以上确定了所需的三种代价函数之后，需要综合三个方面得出最后的评价函数表达式。在传统的DWA算法中，综合评价函数一般对不同种类的代价值赋予不同的权值后进行线性叠加得到最终的代价值：

cost＝cost_goal+cost_security+cost_speed

其中cost_goal代表目标点代价，cost_security代表障碍物代价，cost_speed代表速度代价。

4.3.2根据第二步中得到的所有轨迹空间，对每一条轨迹进行3.1所说的代价函数评价，遍历整个轨迹空间，以得到最优的轨迹路径

4.3.3以最优的轨迹路径，根据第一步提出的车辆运动学方程，车辆在dt时间内按此轨迹行驶，得到下一个车辆运动状态；

4.4经过dt的运动后，更新车辆位置和姿态，重复上述4.1-4.3步骤，到达下一个目标点(在引导点集中选取车辆当前位置前方的引导目标点作为目标点)，直至完成整个调头动作，得到符合车辆运动学、符合乘客坐乘体验的调头路径。

应用实施例，采用本发明所提供的双阶段自动驾驶车辆调头轨迹规划方法进行多种有难度的调头路况的车辆调头规划，经阶段1判断判断可一次调头后，经过阶段2进行线路规划展示具体实施效果，图12.1和图12.2展示了复杂场景这种有难度的情况下的车辆调头规划效果；图13.1和图13.2展示了人为添加了交通规则(考虑斑马线)这种有难度的情况下的车辆调头规划效果；图14.1至图14.8展示了移动障碍物(对向来车)这种有难度的情况下的车辆调头规划效果。

以上所述实施例仅表达了本发明的具体实施方式，其描述较为具体和详细，但并不能因此而理解为对本发明专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变形和改进，这些都属于本发明的保护范围。

Claims (6)

1.一种双阶段自动驾驶车辆调头轨迹规划方法，其特征在于，包含以下步骤：

步骤一、前置准备，包括：

1.1、车辆模型中车辆控制点的选取；

1.2、车辆模型的数理方程约束条件；

1.3、车辆当前地图获取；

步骤二、车辆调头的可行性判断

2.1、在车辆进入调头路口区域地图，即车辆当前地图为调头路口区域时，先通过步骤1.2中的数理方程约束条件判断在车辆在理想情况下能否一次掉头；

2.2、若能一次掉头，则进入后续阶段的自动规划，若不行，则给出理想情况下需要倒车几次完成调头，并绘制出调头、倒车路径，辅助驾驶员进行倒车调头；

步骤三、车辆调头引导目标点生成

3.1、对车辆当前时刻地图进行欧氏距离变化，并设置自适应阈值生成安全区域；

3.2、使用改进的A-star算法在3.1得到的安全区域生成引导路径；

3.3、对引导路径进行离散化取点得到引导点集，引导点集所含引导目标点作为后续步骤的目标点；

步骤四、使用DWA算法完成起始点到目标点的车辆调头规划路线

4.1、根据车辆运动空间，获得时间间隔内所有可能的车辆速度和车轮转角组合；

4.2、针对每一种车辆速度和车轮转角组合，得到经过下一时间间隔后车辆的位置，从而得到每种车辆速度和车轮转角组合所对应的预测轨迹；

4.3、对每一预测轨迹计算一个代价，并选代价最小的那个作为确切的下一步；

4.4、重复4.1-4.3，获得车辆调头的最佳轨迹，即车辆调头规划路线；

其中，步骤2.1中车辆模型的数理方程约束条件包括下述方程：

其中，方程(1)表示车辆的运动学约束方程s.t.，方程(2)表示车辆的轨迹方程，方程(3)为定解方程；

方程(1)中，a_min、a_MAX分别表示车辆的最小、最大加速度,

表示车辆加速度，

表示方向盘转角，

表示方向盘转角最大值，

表示方向盘转速，Ω_MAX表示方向盘转速最大值，k表示传动比，θ表示车轮转角，

表示车轮转速，t为时间，A为坐标系中车辆轮廓点的集合，C为坐标系中障碍物轮廓点的集合，

表示A和C的交集为空集——表示车辆不能与任何障碍物发生碰撞；

方程(2)中，(x_c(t),y_c(t))为当前时刻t的车辆控制点坐标，(x₀,y₀)为车辆控制点的初始坐标，ω为控制点角速度，v为车辆速度；

方程(3)中，R(t)为时刻t的转弯半径；θ(t)为时刻t的车轮转角,ω(t)为时刻t的车辆控制点角速度，v(t)为时刻t的车辆速度，l为车辆轴距；

在方程(1)的约束条件下，根据方程(3)解出v和ω，带入方程(2)得到车辆在理想状态下的轨迹；

步骤3.1中，对车辆当前时刻地图进行欧氏距离变化，生成安全区域的具体方法为：

3.1.1对车辆当前时刻地图进行二值化处理，将道路边界、障碍物的闭合边缘设为1，即背景点为1，其余可行驶区域设为0，即前景点为0；

3.1.2计算出地图上可行驶区域中每个前景点距离其最近障碍，即背景点的欧式距离；

3.1.3根据3.1.2计算出的欧氏距离图的梯度信息设定自适应阈值，计算筛选出在有可通行道路的前提下距离障碍较远的位点；

3.1.4生成安全行驶区域的二值化地图，不安全区域设为1，安全行驶区域设为0。

2.根据权利要求1所述的一种双阶段自动驾驶车辆调头轨迹规划方法，其特征在于，车辆模型使用车辆后轴中心作为控制点。

3.根据权利要求1所述的一种双阶段自动驾驶车辆调头轨迹规划方法，其特征在于，步骤2.1中，通过步骤1.2中的数理方程约束条件判断在车辆在理想情况下能否一次掉头，具体步骤为：

2.1.1获得车辆控制点开始调头时的坐标(x₀,y₀)，以及开始调头时的速度v，

2.1.2根据步骤1.2中确定的数理方程约束条件，得到理想状态下整个调头过程中车辆控制点的运动轨迹；

2.1.3进行能否一次完成调头的判断：

根据车身的几何尺寸，得到车辆左、右前端点的横坐标与车身控制点横坐标的相对位置关系，能够一次完成调头的判定条件为：

方程(4)中，x_l(t)＝x_c(t)-L·cos(α_l-γ)，x_r(t)＝x_c(t)+L·cos(α_r+γ)；

方程(4)中，x_l(t)为车辆左前端点的横坐标，x_r(t)为车辆右前端点的横坐标，x_c(t)车身控制点横坐标，x_bord为对向道路外边界的横坐标，L为控制点到左右前端点的距离，α_l为控制点和左端点的连线与x轴负向夹角、α_r为控制点和右端点连线与x轴正向夹角、γ为控制点速度方向与y轴正方向夹角；

2.1.4若2.1.3判断能一次掉头，则进入后续阶段的规划。

4.根据权利要求1所述的一种双阶段自动驾驶车辆调头轨迹规划方法，其特征在于，步骤4.1中，车辆运动空间的符合下述三个约束条件：

4.1.1首先根据一般车辆运动的运动学方程，经过一段时间微元dt后车辆位置变化具体表现的公式如下：

式中x_t、y_t为经过时间微元dt后车辆控制点在俯视方向上的二维坐标，x₀、y₀为当前时刻车辆控制点的二维坐标，θ₀、θ_t则分别为时间微元dt前后的车辆姿态角或车轮转角，v为该时间微元内车辆线速度，ω为该时间微元内车辆角速度；

4.1.2根据4.1.1的运动学方程，对车辆在时间间隔内的转角和加减速进行限制，体现在动态窗口采样中，即速度的增量不能超过速度加速度与时间间隔的乘积、车体姿态角的增量不能超过角加速度与时间间隔的乘积；

4.1.3一般车辆对速度和轮胎转角会有机械约束上的限制，结果呈现为车辆速度和轮胎转角不可超过某一阈值。

5.根据权利要求4所述的一种双阶段自动驾驶车辆调头轨迹规划方法，其特征在于，步骤4.2中，每一种车辆速度和车轮转角组合，以步骤4.1.1所述的运动学方程进行模拟，得出预测轨迹，将所有速度和转角组合的预测轨迹进行暂时存储形成轨迹空间，以便后面步骤进行最佳的轨迹选择。

6.根据权利要求1所述的一种双阶段自动驾驶车辆调头轨迹规划方法，其特征在于，步骤4.3中，对每一预测轨迹计算一个代价，代价cost的评价函数表达式为：

cost＝cost_goal+cost_security+cost_speed

其中，cost_goal代表目标点代价，cost_security代表障碍物代价，cost_speed代表速度代价；

1)所述目标点代价函数为：

cost_goal＝W_angle×cost_angle+W_distance×cost_distance

其中cost_goal代表目标点代价函数，cost_angle和cost_distance分别代表姿态角的代价和距离的代价,W_angle和W_distance分别代表姿态角的代价的权值和距离的代价的权值；

其中，cost_angle＝tan(θ_car-θ_goal)

其中,θ_goal和θ_car分别代表目标点和车辆目前的姿态角，x_goal和y_goal代表目标点的坐标，x_t和y_t分别代表车辆目前的坐标；

2)所述障碍物代价函数为：

其中，cost_security代表安全代价函数，W_wall和W_ob分别代表车道线边界的和障碍物的权重，r_{wall_min}和r_{ob_min}分别代表车辆距离车道线边界和障碍物的最近距离；

3)所述速度代价函数为：

cost_speed＝W_speed×(v_max-v_t)

其中cost_speed代表速度代价函数,W_speed代表速度代价的权重，v_t代表当前车辆速度，v_max代表车辆最大速度。

Images