CN106515722B - 一种垂直泊车轨迹规划方法 - Google Patents

Abstract

本发明智能车辆与辅助驾驶领域，特别涉及一种基于路径‑速度解耦的垂直泊车轨迹规划方法。本发明的方法主要包括：建立车辆转向运动学模型；将垂直泊车轨迹规划解耦为路径规划和速度规划。本发明的有益效果为，本发明将车辆垂直泊车路径规划问题分解为路径规划和速度规划，并分别采用绕墙走策略和动态窗口法完成规划，有机综合了两种方法的优点，可使车辆实现有效、安全地垂直泊车。

Description

一种垂直泊车轨迹规划方法

技术领域

本发明属于智能车辆与辅助驾驶领域，特别涉及一种基于路径-速度解耦的垂直泊车轨迹规划方法。

背景技术

自动泊车系统能够代替或者辅助驾驶员进行泊车，对于提高泊车安全性，降低泊车的操纵难度有着十分重要的意义。自动泊车轨迹规划问题是自动泊车系统的一个重要技术，其主要任务是在泊车场景中，为智能驾驶车辆提供从起始位姿到目标位姿的一系列速度和方向盘转角输入。作为自动泊车系统重要组成部分,轨迹规划结果的优劣直接决定着自动泊车系统的实用性及总体性能，随着应用背景的不断复杂化以及应用领域的不断扩大，自动泊车系统轨迹规划子系统一直向着更高层次的智能化目标发展。

自动垂直泊车轨迹规划技术主要在考虑车辆的最小转弯半径等运动学特性的基础上进行规划。目前，通常通过采用几何轨线将泊车轨迹用带参数的数学模型表示，同时将障碍物以及汽车运动学性能进行约束化，从而利用非线性规划离线生成最优轨迹。在已知Reeds-Shepp路径组成的前提情况下，也可以利用Minkowski Sum在生成的无碰撞区域内选取最优路径，但这种方法没有考虑线速度等动力学约束。神经网络和模糊控制器虽然不直接进行轨迹规划，但其仿熟练驾驶员的规则本身包含了许多用于参考的轨迹先验信息，对泊车场景具有一定的要求，缺少灵活性。上述方法或者需要轨迹数学模型，或者采用开环离线规划方式，缺乏灵活性、不能动态调整路径。因此，采用无轨迹模型的在线轨迹规划是智能车辆局部狭小环境中导航的趋势。

发明内容

本发明的目的是克服现有技术的缺点，提供一种基于路径-速度解耦的垂直泊车轨迹规划基本方法。该方法利用路径-速度解耦将轨迹规划问题分解为路径规划问题和速度规划问题，在无需知道泊车轨迹模型的情况下，利用绕墙走策略和动态窗口法在线规划，实时输出自动泊车系统的速度和方向盘转角控制量，提高了泊车轨迹规划的智能化程度。

为解决上述技术问题，本发明的采用的技术方案为：

一种垂直泊车轨迹规划方法，其特征在于，包括以下步骤：

a.基于车辆运动学模型的转向特性获取：

汽车采用阿克曼转向模型，则车辆转向时，全车的转向中心位于后轮轴线的延长线上，车辆的状态由横纵坐标及航向角表示q＝(x,y,q)，车辆的输入为速度和方向盘转角(v,α)，在泊车的低速行驶模式下忽略轮胎和地面的侧滑，这样，系统存在不能侧向位移、不能原地转向的非完整性约束，转向时存在转弯半径，通过解析，可得到车辆转向的两个特性：前后轮运动轨迹是不重合的，转弯半径最小的点在内侧后车轮；其中，横纵坐标中横坐标x与车辆泊车完成后车身侧边平行，纵坐标y与横坐标x在同一水平面且相互垂直；

b.车辆轨迹规划：

根据步骤a中获取的车辆转向特性，将垂直泊车轨迹规划解耦为路径规划和速度规划；

路径规划的具体方法为：

b11.将垂直泊车地图环境进行凸包分割，形成凸多边形障碍物边缘；

b12.计算离起始位姿q₀＝(x₀,y₀,q₀)、终止位姿q_e＝(x_e,y_e,q_e)距离最近的障碍物点O₀和O_e；

b13.从O₀和O_e沿初始姿态和终止姿态方向作车辆和障碍物边缘作距离为w/2的等距线段，w为车宽，记录线段曲率不连续的点为O_i，并计算在该点姿态角的变化量；

b14.对于每个点O_i，设置一个与之对应的中间等效状态q_i＝(x_i,y_i,q_i)；该点位于线段q_eO_i和线段q₀O_i延长线所包围的区域内；

b15.求最短圆弧路径q₀q_i和q_eq_i，且设置对应的半径为R₁和R₂；

b16.设置一个最小碰撞距离d_c，此时对应的参考点的值为d＝w/2+d_c；

b17.计算最小碰撞距离时对应的车辆路径，获得车辆路径规划；

速度规划的具体方法为：

假设车辆本身存在速度和方向盘转角限制如下：

v∈[v_min,v_max],α∈[α_min,α_max]

采用动态窗口法进行速度规划，假设动态窗口法单步模拟的时间间隔为△t，那么加速度、转角速度约束如下：

其中，分别对应最大加速度和方向盘转角速度；

采用考虑离参考路径偏差的轨迹选取策略，选择目标函数代价最低的轨迹，目标函数cost为：

其中，pd,hdiff,gd分别为距离参考路径的距离偏差、航向偏差以及离目标点距离偏差的系数因子。

本发明的有益效果为：(1)能有效安全地完成车辆垂直泊车，本发明将车辆垂直泊车路径规划问题分解为路径规划和速度规划，并分别采用绕墙走策略和动态窗口法完成规划，有机综合了两种方法的优点，可使车辆实现有效、安全地垂直泊车；(2)无需先验轨迹数学模型，本发明通过对轨迹规划问题进行解耦后，通过车辆转向运动的分析，采用绕墙走策略完成垂直泊车轨迹规划中的路径规划，该路径不需要先验的轨迹数学模型，提高了轨迹规划的灵活性；(3)轨迹局部可调，本发明轨迹规划解耦后的速度规划中提出运用动态窗口法，实现了优化与反馈的结合，使轨迹规划由开环变为闭环，大大提高了轨迹规划的鲁棒性；(4)算法可扩展性强，本发明也可根据实际情况运用于其他狭小情况下的智能车辆导航，使智能车辆能在运动中快速找到合理的路径，满足未来智能驾驶汽车的发展要求。

附图说明

图1是本发明中轨迹规划方法的流程图；

图2是本发明中车辆阿克曼转向示意图；

图3是本发明中垂直泊车场景示意图；

图4是本发明中前进式垂直泊车和后退式垂直泊车对应的绕墙走路径；

图5是本发明中动态窗口单步模拟的输出控制量所对应的轨迹图。

具体实施方式

下面结合附图和实施例，详细描述本发明的技术方案：

如图1所示，本发明主要包括以下步骤：

1、建立基于运动学模型的转向运动特性

汽车普遍采用阿克曼转向模型，如图2所示，车辆转向时，全车的转向中心位于后轮轴线的延长线上。车辆的状态由横纵坐标及航向角表示q＝(x,y,q)，车辆的输入为速度和方向盘转角(v,α)。在泊车的低速行驶模式下忽略轮胎和地面的侧滑，这样，系统存在不能侧向位移、不能原地转向的非完整性约束，转向时存在转弯半径。

通过解析，车辆转向有以下特性：

(1).前后轮运动轨迹是不重合的；

(2).转弯半径最小的点在内侧后车轮。

2、垂直泊车轨迹规划问题解耦

垂直泊车场景如图3所示，轨迹规划问题是规划出一系列的控制输入(v,α)，使车辆安全快速地完成泊车。利用Path-Velocity Decomposition对路径和速度进行解耦，将轨迹规划问题分解成路径规划和速度规划。

3、基于绕墙走策略解决路径规划

垂直泊车路径规划问题就是找到一系列从泊车起始位姿到目标位姿的位姿序列q_i＝(x_i,y_i,q_i)。车辆在泊车过程中，通过车身周围的超声波传感器能够感知泊车环境，构建泊车地图，基于绕墙走策略的路径规划步骤分为：

(1).将垂直泊车地图环境进行凸包分割，形成凸多边形障碍物边缘；

(2).计算离起始位姿q₀＝(x₀,y₀,q₀)、终止位姿q_e＝(x_e,y_e,q_e)距离最近的障碍物点O₀和O_e；

(3).分别从O₀和O_e沿初始姿态和终止姿态方向作车辆和障碍物边缘作距离为w/2的等距线段(w为车宽)，记录线段曲率不连续的点为O_i，并计算在该点姿态角的变化量；

由于原地转向中间状态处的曲率是不连续的，为了满足车辆的非完整性运动学约束，基于车辆的转向特性对路径作如下处理：

(1).对于每个点O_i，设置一个与之对应的中间等效状态q_i＝(x_i,y_i,q_i)；那么，该点位于线段q_eO_i和线段q₀O_i延长线所包围的区域内；

(2).求最短圆弧路径q₀q_i和q_eq_i，且对应的半径为R₁和R₂；

(3).设置一个最小碰撞距离d_c，该距离可由传感器精度确定，此时对应的参考点的值为d＝w/2+d_c；

(4).计算最小碰撞距离时对应的绕墙走路径。

满足车辆转向特性的垂直泊车绕墙走路径如图4所示。

4、运用动态窗口法进行速度规划

车辆本身存在速度和方向盘转角限制：

v∈[v_min,v_max],α∈[α_min,α_max] (1)

假设动态窗口法单步模拟的时间间隔为△t，那么加速度、转角速度约束为：

其中，分别对应最大加速度和方向盘转角速度。

如图5所示，时间窗口内的控制量可以映射为轨迹。采用考虑离参考路径偏差的轨迹选取策略，选择目标函数代价最低的轨迹，目标函数cost为：

其中，pd,hdiff,gd分别为距离参考路径的距离偏差、航向偏差以及离目标点距离偏差的系数因子。

实施例

本例为采用ROS Indigo与Gazebo作为实现工具的仿真，具体包括：

步骤1.基于车辆运动学模型建立转向特性

车辆的转向模型如图2所示，后轮轴线中心P_r作为全车轨迹的参考点，车辆的状态由横纵坐标及航向角表示为q＝(x,y,q)，车辆的输入为速度和方向盘转角(v,α)。Lf、Lr以及w为车辆的前悬、后悬长度和宽度。在泊车的低速行驶模式下忽略轮胎和地面的侧滑，这样，系统存在如下非完整性约束：

该转角状态下对应路径的曲率半径和曲率：

以速度和方向盘转角(v,α)为控制量，以q＝(x,y,q)为状态量的车辆运动学模型：

通过分析，车辆转向有以下特性：

(1).前后轮运动轨迹是不重合的；

(2).转弯半径最小的点在内侧后车轮。

同时，两个车轮的转弯半径为：

其中，R₁,R₂代表转向内侧后轮和外侧前轮的转弯半径，R_r通常称为内轮差。

步骤2.轨迹规划问题解耦

车辆的速度只影响行驶该轨迹所用的时间，行驶的轨迹形状以及在各个轨迹点上的姿态角仅仅与移动距离以及在不同位置的曲率(转向角)有关。

车辆的线速度以及路径的曲率可以表示为：

结合公式(2)和(6)，可以得到：

公式(7)表明，车辆的速度只影响行驶该轨迹所用的时间，行驶的轨迹形状以及在各个轨迹点上的姿态角仅仅与移动距离以及在不同位置的曲率(转向角)有关。那么，垂直泊车轨迹规划问题就解耦为路径规划问题和速度规划问题。

步骤3.基于绕墙走策略解决路径规划

通过自动泊车系统环境感知子系统可以获得如图4所示的泊车环境地图，带箭头线段表示了完整性约束的绕墙走路径，通过车辆转向特性，对该绕墙走路径进行满足非完整约束的等效，等效后的路径可以由图4中曲线表示。在仿真环境中，最小碰撞距离d_c设置为0.2m，车辆参数如表1所示，后退式垂直泊车场景参数设置如表2所示：

表1 车辆参数

表2 垂直泊车场景参数

参数	符号	单位	参数值
				车位长	b	m	3.0
车位宽	a	m	2.0
				车道宽	ds	m	4.0
起始位姿	q0	(m,m,rad)	(0,0,1.57)
				终止位姿	qe	(m,m,rad)	(5.5,3.2,3.14)

步骤4.运用动态窗口法进行速度规划

动态窗口法通过在当前位姿状态下，前向模拟，通过参考绕墙走路径偏差因子、目标点偏差因子、参考航向偏差因子动态的选取车辆在该状态下的最优速度和方向盘转角输出，动态窗口法所采用的各个参数如表3所示。

表3 动态窗口法参数

动态窗口法可以根据实时的车辆状态信息对轨迹进行调整。

综上，本发明提出的基于路径-速度解耦的轨迹规划方法可以实现无先验轨迹知识的、局部动态可调的垂直泊车，提高了垂直泊车规划决策的智能化程度。基于绕墙走策略的路径规划可以实现无轨迹模型的路径规划，同时在不进行精确的轨迹跟踪控制的情况下，也能通过轨迹规划中动态窗口法本身所具备的闭环特点对轨迹进行局部调整。

Claims (1)

1.一种垂直泊车轨迹规划方法，其特征在于，包括以下步骤：

a.基于车辆运动学模型的转向特性获取：

汽车采用阿克曼转向模型，则车辆转向时，全车的转向中心位于后轮轴线的延长线上，车辆的状态由横纵坐标及航向角表示q＝(x,y,θ)，车辆的输入为速度和方向盘转角(v,α)，在泊车的低速行驶模式下忽略轮胎和地面的侧滑，因此存在不能侧向位移、不能原地转向的非完整性约束，转向时存在转弯半径，通过解析得到车辆转向的两个特性：前后轮运动轨迹是不重合的，转弯半径最小的点在内侧后车轮；

b.车辆轨迹规划：

根据步骤a中获取的车辆转向特性，将垂直泊车轨迹规划解耦为路径规划和速度规划；

路径规划的具体方法为：

b11.将垂直泊车地图环境进行凸包分割，形成凸多边形障碍物边缘；

b12.计算离起始位姿q₀＝(x₀,y₀,θ₀)、终止位姿q_e＝(x_e,y_e,θ_e)距离最近的障碍物点O₀和O_e；

b13.从O₀和O_e沿初始姿态和终止姿态方向作车辆和障碍物边缘作距离为w/2的等距线段，w为车宽，记录线段曲率不连续的点为O_i，并计算在该点姿态角的变化量；

b14.对于每个点O_i，设置一个与之对应的中间等效状态q_i＝(x_i,y_i,θ_i)；该点位于线段q_eO_i和线段q₀O_i延长线所包围的区域内；

b15.求最短圆弧路径q₀q_i和q_eq_i，且设置对应的半径为R₁和R₂；

b16.设置一个最小碰撞距离d_c，此时对应的参考点的值为d＝w/2+d_c；

b17.计算最小碰撞距离时对应的车辆路径，获得车辆路径规划；

速度规划的具体方法为：

假设车辆本身存在速度和方向盘转角限制如下：

v∈[v_min,v_max],α∈[α_min,α_max]

采用动态窗口法进行速度规划，假设动态窗口法单步模拟的时间间隔为Δt，那么加速度、转角速度约束如下：

其中，分别对应最大加速度和方向盘转角速度；

采用考虑离参考路径偏差的轨迹选取策略，选择目标函数代价最低的轨迹，目标函数cost为：

其中，pd,hdiff,gd分别为距离参考路径的距离偏差、航向偏差以及离目标点距离偏差的系数因子。