CN115903828A - 一种基于车辆四轮转向的自动泊车路径规划方法 - Google Patents

Abstract

本发明涉及汽车自动驾驶路径规划领域，公开了一种基于车辆四轮转向的自动泊车路径规划方法。所述规划方法包括如下步骤：运用基于泊车场景并结合四轮转向改进的RRT*Reeds‑Sheep曲线进行初始路径规划，得到一条包含车辆位置信息和车辆位姿角信息的初始路径；对所得到的初始路径进行速度粗规划，得到一条初始轨迹；通过建立最优控制问题，设置基于四轮转向的车辆系统动态运动方程约束、两点边值约束和流形约束，对所述最优控制问题求解得到优化后的最终轨迹，以备后续泊车轨迹跟踪控制器进行目标轨迹跟踪控制。所述自动泊车路径规划方法能够实现在狭窄区域进行泊车，并且能够实现泊车过程的动态避障，提高汽车自动泊车机动性。

Description

一种基于车辆四轮转向的自动泊车路径规划方法

技术领域

本发明涉及汽车自动驾驶路径规划领域，特别涉及一种基于车辆四轮转向的自动泊车路径规划方法。

背景技术

近年来，随着私家车保有量的逐年递增，停车难的问题逐渐凸显，为减轻驾驶人员的驾驶压力，自动泊车系统已经得到了广泛的应用。目前自动泊车系统大多采用自动驾驶系统当中常见的路径规划与跟踪控制模块作为实现系统功能的方案，泊车路径的规划质量决定了车辆跟踪控制模块是否能够在允许的偏差范围内实现路径的跟踪。因此，在面对较复杂的泊车场景时规划出适应当前场景且能够满足车辆运动学约束要求的泊车路径规划研究对于自动泊车系统发展具有重要的研究意义。

目前在自动泊车中应用的路径规划方法有很多，例如：运用五次多项式进行平行泊车工况下的路径规划，以保证所规划出来的路径曲率的连续性；运用Rapidly-exploringRandom Trees(RRT)算法，从起始点开始以树状结构进行扩展，直到目标点或目标点附近的点也被扩展到该树上，这时就可以在该树上找到一系列从起点到目标点的离散点，然后通过建立好的离散点对考虑车辆运动学约束的路径规划过程进行引导，从而得到一条考虑车辆运动学约束的路径。

然而上述这些路径规划的方法主要针对实现从起始点到终止点的路径规划，忽略了一系列车辆运动学系统动态约束，并且随着停车位紧张的现象逐渐加剧，上述方法由于车辆的最小转弯半径而不能够实现在更加狭小的空间进行泊车。此外停车场内部结构较为复杂，上述方法忽略了动态障碍物的存在，没有考虑泊车过程中的障碍物碰撞约束条件，导致车辆在泊车过程中可能存在碰撞事故。

而基于四轮转向的路径规划方法，在考虑车辆运动学的基础上，四轮转向车辆具有更小的车辆转弯半径，其特点能够实现在较为狭小的空间完成转向进一步进行泊车，并且具有四轮转向的车辆具有更好的转向响应，因此可以通过建立障碍物碰撞约束能够更加轻易的躲避动态障碍物，实现实时动态避障。

因此本发明将基于四轮转向的基础上进行路径规划，并且本发明可以实现如下效果：生成一条符合车辆运动学规律的轨迹，该轨迹不仅可以实现在狭窄区域进行泊车，并且能够实现泊车过程的动态避障。

发明内容

本发明的目的在于提出一种基于四轮转向的自动泊车路径规划方法。目的在于：生成一条符合车辆运动学规律的轨迹，该轨迹不仅可以实现在狭窄区域进行泊车，并且能够实现泊车过程的动态避障。

为实现上述目的，采用如下技术方案：

一种基于四轮转向的自动泊车路径规划方法，包括：

步骤一、运用结合Reeds-Sheep曲线的RRT*算法进行初始路径规划，在保证路径规划过程概率完备性的同时，实现了不同起始位置与不同类型泊车场景下泊车轨迹的可行解搜索，并且基于先验知识快速排除不可能出现的采样区域，显著提升了算法的规划效率，利用改进后的算法生成一条曲率连续的包含车辆位置信息和车辆姿态角信息的初始路径。

步骤二、对步骤一所获初始路径进行速度粗规划，首先利用随机搜索规划中的步长信息

与设定的采样间隔时间DT获取到每个状态点的速度信息

再通过下采样的方式得到与N+1个时刻相对应的初始轨迹状态点，进而得到一条与步骤一初始路径相对应的初始速度变化曲线。

步骤三、将自动泊车路径规划问题看为非线性最优控制Bolza型问题，即对连续Bolza形式最优控制问题进行求解，将步骤二所得轨迹作为求解问题的初始解进行输入，通过建立基于四轮转向的系统动态方程约束，两点边值约束以及流行约束，确定终止时刻t_f，以及控制变量u(t),t∈[0,t_f]，在沿着相应的状态轨线x(t)即满足全部约束条件的前提下，使得代价函数J取值最小。

有此得到优化后的最终泊车轨迹，提供给车辆泊车轨迹跟踪控制器即可进行目标泊车轨迹跟踪控制。

本发明的有益效果是：

1.本发明所述一种基于四轮转向的自动泊车路径规划方法，运用改进的RRT*算法进行初始路径规划，改进后的算法既保证了路径规划过程的概率完备性又根据不同的泊车场景确认采样区域范围，基于先验知识快速排除不可能出现的采样区域，显著提升了算法的规划效率。

2.本发明所述一种基于四轮转向的自动泊车路径规划方法，在RRT*算法进行初始路径规划时，节点的扩张方式更新为基于四轮转向的Reeds-Sheep曲线扩展方式，使得车辆能够具有更小的转弯半径，能够满足在更加狭小的空间进行泊车。

3.本发明所述一种基于四轮转向的自动泊车路径规划方法，在进行车辆运动学方程约束时，进行了四轮转向的运动学方程约束，使车辆具有更好的转向响应，并结合所建立的流形约束，能够实现泊车过程中的实时动态避障。

附图说明

图1为本发明所述的一种基于四轮转向自动泊车路径规划方法的整体流程图。

图2为本发明所述的一种基于四轮转向自动泊车路径规划方法中步骤一的RRT*Reeds-Shepp算法流程图。

图3为本发明所述的一种基于四轮转向自动泊车路径规划方法中步骤三的基于四轮转向的车辆运动学模型图。

图4为本发明所述的一种基于四轮转向自动泊车路径规划方法中步骤三的流形约束中的三角形面积法原理图。

图5为本发明所述的一种基于四轮转向自动泊车路径规划方法实施算例的初始路径规划结果。

图6为本发明所述的一种基于四轮转向自动泊车路径规划方法实施算例的速度粗规划结果图。

图7为本发明所述的一种基于四轮转向自动泊车路径规划方法实施算例的优化前后轨迹对比图。

具体实施方式

下面结合附图对本发明做进一步的详细说明，以令本领域技术人员参照说明书文字能够据以实施。

如图1所示，本发明所述的一种基于四轮转向的自动泊车路径规划方法，其整体流程包括如下步骤：

步骤一、运用结合Reeds-Sheep曲线的RRT*算法(即，RRT*RS算法)进行初始路径规划，在保证路径规划过程概率完备性的同时，实现了不同起始位置与不同类型泊车场景下泊车轨迹的可行解搜索，并且基于先验知识快速排除不可能出现的采样区域，显著提升了算法的规划效率，利用改进后的算法生成一条曲率连续的包含车辆位置信息和车辆姿态角信息的初始路径。

步骤二、对步骤一所获初始路径进行速度粗规划，首先利用随机搜索规划中的步长信息

与设定的采样间隔时间DT获取到每个状态点的速度信息

再通过下采样的方式得到与N+1个时刻相对应的初始轨迹状态点，进而得到一条与步骤一初始路径相对应的初始速度变化曲线。

步骤三、将自动泊车路径规划问题看为非线性最优控制Bolza型问题，即对连续Bolza形式最优控制问题进行求解，将步骤二所得轨迹作为求解问题的初始解进行输入，通过建立基于四轮转向的系统动态方程约束，两点边值约束以及流行约束，确定终止时刻t_f，以及控制变量u(t),t∈[0,t_f]，在沿着相应的状态轨线x(t)即满足全部约束条件的前提下，使得代价函数J取值最小。

有此求解优化问题，即可得到优化后的最终泊车轨迹，提供给车辆泊车轨迹跟踪控制器即可进行目标泊车轨迹跟踪控制，达到四轮转向车辆在狭窄区域可以进行自动泊车，并且能够实现泊车过程的动态避障的目的和效果。

如图2所示，步骤一中所述RRT*RS算法的具体算法流程是在传统的RRT*算法的基础上进行如下改进获得的：

节点的扩展方式更新为基于四轮转向的Reeds-Sheep曲线扩展方式；为保证算法具有较高运算效率，在最近节点搜索过程中采用了忽略车辆航向角信息的欧氏距离作为距离度量；函数根据泊车场景不同进行随机采样区域的确认；在临近节点搜索过程中加入起始节点；在父节点选择过程中应用了如式(1)所示的考虑代价项的Reeds-Shepp曲线长度；在每次随机节点扩展循环结束时从当前节点出发尝试向泊车目标位姿进行扩展，来提升泊车规划搜索速度，在扩展过程中检测搜索随机树中已添加的连接至泊车目标位姿的节点数目来判断是否达到设定的可行路径数目，若达到规定的成功路径数目N_suc则提前结束搜索过程。

式中，t,u,v表示归一化后对应每段圆弧或直线的弧度或长度；dir_i表示曲线组合中第i段路径的行驶方向，在此处设正向为1，反向为-1；在x<0时，f(x)＝0，在其他情况时，f(x)＝1。

改进的RRT*RS算法伪代码如表1所示。

表1改进的RRT*RS算法伪代码

图3为步骤三中所述基于四轮转向的车辆运动学模型图，结合图3，建立如下式(2)所示基于四轮转向的车辆运动学模型方程，来描述车辆运动学状态：

式中，δ_f为四轮转向汽车的前轮转角；δ_r为四轮转向汽车的后轮转角；

为汽车方位角；L为轴距；后轴中点横坐标位移、纵坐标位移和速度分别为x_r、y_r、v_r。

在步骤三中，建立的两点边值约束用于提供车辆在起始时刻t＝0及终止时刻t＝t_f的运动状态：

当t＝0时，

式中，

对应着由车载传感器记录的初始运动状态信息；

当t＝t_f时，车辆最终稳定地停泊某一特定区域：

υ_i(t_f)＝0；a_i(t_f)＝0；要求车轮在终止时回正

ω_i(t_f)＝0；终止时刻达到某一既定姿态角sin(θ_i(t_f))＝sin(θ_fi)，cos(θ_i(t_f))＝cos(θ_fi)；根据车辆后轮轴中点坐标、车宽、车辆前悬距离、车辆后悬距离、轴距、位姿角等可得到车辆各个顶点坐标，根据停车位地四个顶点构成的矩形区域四条边所在直线，确定车辆各个顶点处于直线的哪一侧，以此来对x_i(t_f)、y_i(t_f)、θ_i(t_f)进行约束。

在步骤三中，所建立的流行约束主要是将控制变量u(t)以及状态变量x(t)限制在解空间中的某一高维曲面上或曲面所包络的区域内。具体限制包括：运动能力限制以及外部环境限制；

对于运动能力限制，可用如下关系式表达，

式中，Φ_fmax代表车辆前轮转角的最大允许偏转角度值；Φ_rmax代表车辆后轮转角的最大允许偏转角度值；v_max是车辆在低速场景中的安全行驶速度上限；为保证乘客舒适性，a_max、Ω_fmax、Ω_rmax分别为线加速度，前轮转角速度及后轮转角速度设置的幅值。

在步骤三中的流形约束中，对于外部环境限制，则需满足：

建立车身矩形与凸多边形障碍物之间的碰撞躲避约束条件，通过“三角形面积法”，来保证每一时刻车身矩形顶点处于障碍物多边形外部，障碍物的顶点处在车身矩形外部，以此来对x_i(t_f)、y_i(t_f)、θ_i(t_f)进行约束。

“三角形面积法”的原理如图4所示，以障碍物为某五边形为例，五边形的顶点分别为Q₁,Q₂,Q₃,Q₄,Q₅。将点P与凸多边形每两个相邻顶点分别组成三角形，并将这些三角形的面积累加，如果面积之和大于凸多边形，则点P处于凸多边形外部，否则点P处于处在多边形的某一边上或凸多边形内部。点P处于凸多边形Q₁～Q_n外部的约束条件为：

式中，S_Δ代表相应三角形面积；S_Y代表凸多边形面积；S_Δ应通过三角形顶点坐标进行求解，以

为例，假设P＝(x,y)、Q_k＝(x_Qk,y_Qk)、Q_k+1＝(x_Q(k+1),y_Q(k+1))，则

在步骤三中，所建立的代价函数作为筛选优质轨迹的指标式：

J＝ω₁J₁+ω₂J₂+ω₃J₃+ω₄J₄#(7)

式中，ω₁、ω₂、ω₃、ω₄为各个性能指标的权重系数；J₁＝t_f，意味着期待车辆尽早运动至能够符合终止时刻约束的状态；

其汇总了车辆i在整个运动时域内因车轮转角变化和车速变化而消耗的能量，用于表征车辆轨迹的平滑性；

用于表征使车辆轨迹适当远离障碍物j，系数κ_j>0决定了期待车辆远离障碍物j的程度，N_obs为凸多边形障碍物的数目，

用于描述车辆i与障碍物j的距离，障碍物几何中心为G_j＝(x_Gi,y_Gi)。

接下来通过一个算例来演示本发明所提出的方法的运用过程和规划效果。

本算例车辆的相关参数：前悬L_f＝0.985m，后悬L_r＝0.885m，轴距L＝2.625m，车宽L_b＝1.82m；结合车辆相关参数，步骤三中流形约束运动能力限制：a_max＝0.3m/s²，v_max＝1.4m/s，w_max＝0.5rad/s²，Φ_fmax＝35°，Φ_rmax＝5°；步骤三流形约束中静态障碍物边界信息：[-10.0,1.2]，[-3.0,1.2]所组成直线，[-3.0,1.2]，[-3.0,-1.5]所组成直线，[-3.0,-1.5]，[4.0,-1.5]所组成直线，[4.0,-1.5]，[4.0,1.2]所组成直线，[4.0,1.2]，[10.0,1.2]所组成直线，[-10.0,6.0]，[10.0,6.0]所组成直线，动态障碍物显示到地图上为一个1×1的矩形，以矩形右下角顶点为参考顶点，起始坐标为(5，4)，终点坐标为(-3，4)，移动步长为0.2；车辆起始点位置信息及位姿角：[-4.0,3.0,0.0]，车辆终点位置信息及位姿角[-1.0,0.0,0.0]，将各个信息作为输入进行步骤一所述RRT*RS算法探索，所规划的路径如图5所示，可见基于四轮转向的车辆具有更小的转弯半径，能够实现狭小区域的泊车。

如图6所示，设定泊车过程中车辆的速度大小保持不变始终为0.8m/s(当车辆前向行驶时，速度为0.8m/s；当车辆后向行驶时，速度为-0.8m/s)，速度变化曲线结合步骤一所规划出的初始路径，可得到一条包含速度信息的初始轨迹。

将步骤二所得初始轨迹作为初始解进行输入，通过算例已知数据及上述方法，建立基于四轮转向的系统动态方程约束，两点边值约束和流形约束，得到优化后的轨迹，初始轨迹(如图中短线虚线所示轨迹和车体轮廓姿态)与优化后的轨迹(如图中点虚线所示轨迹和车体轮廓姿态)对比如图7所示，可见优化后的轨迹更加舒展且通过建立流形约束并结合基于四轮转向的运动学模型，优化后的轨迹能够实现躲避运动障碍物(由图中上方所示多个重叠的矩形框代表运动障碍物轨迹)。

尽管本发明的实施方案已公开如上，但其并不仅仅限于说明书和实施方式中所列运用，它完全可以被适用于各种适合本发明的领域，对熟悉本领域的人员而言，可容易地实现另外地修改，因此在不背离权利要求及等同范围所限定地一般概念下，本发明并不限于特定的细节和这里示出与描述的图例。

Claims (7)

1.一种基于车辆四轮转向的自动泊车路径规划方法，其特征在于，包括：

步骤一、对传统的RRT*算法进行改进后完成初步的路径规划，在保证路径规划过程概率完备性的同时，实现了不同起始位置与不同类型泊车场景下泊车轨迹的可行解搜索，并且基于先验知识快速排除不可能出现的采样区域以提升算法的规划效率，利用改进后的算法生成一条曲率连续的包含车辆位置信息和车辆姿态角信息的初始路径；

步骤二、对步骤一所获初始路径进行速度粗规划，利用随机搜索规划中的步长信息

与设定的采样间隔时间DT获取到每个状态点的速度信息

通过下采样的方式得到与N+1个时刻相对应的初始轨迹状态点，进而得到一条与步骤一初始路径相对应的初始速度变化曲线；

步骤三、将自动泊车路径规划问题看为非线性最优控制Bolza型问题，即对连续Bolza形式最优控制问题进行求解，将步骤二所得初始轨迹作为求解问题的初始解进行输入，通过设置基于四轮转向的车辆系统动态方程约束，两点边值约束以及流行约束，确定终止时刻t_f，以及控制变量u(t),t∈[0,t_f]，在沿着相应的状态轨线x(t)即满足全部约束条件的前提下，使得代价函数J取值最小。

2.如权利要求1所述一种基于车辆四轮转向的自动泊车路径规划方法，其特征在于，所述步骤一中，在传统的RRT*算法进行如下改进的具体方法为：

节点的扩展方式更新为基于四轮转向的Reeds-Sheep曲线扩展方式；为保证算法具有较高运算效率，在最近节点搜索过程中采用了忽略车辆航向角信息的欧氏距离作为距离度量；函数根据泊车场景不同进行随机采样区域的确认；在临近节点搜索过程中加入起始节点；在父节点选择过程中应用了考虑代价项的Reeds-Shepp曲线长度；在每次随机节点扩展循环结束时从当前节点出发尝试向泊车目标位姿进行扩展，来提升泊车规划搜索速度，在扩展过程中检测搜索随机树中已添加的连接至泊车目标位姿的节点数目来判断是否达到设定的可行路径数目，若达到规定的成功路径数目N_suc则提前结束搜索过程。

3.如权利要求1所述一种基于车辆四轮转向的自动泊车路径规划方法，其特征在于，所述步骤三中，建立基于四轮转向的车辆运动学模型描述车辆运动学性质来进行所述车辆系统动态方程约束：

式中，δ_f为四轮转向汽车的前轮转角；δ_r为四轮转向汽车的后轮转角；

为汽车方位角；L为轴距；后轴中点横坐标、纵坐标和速度分别为x_r、y_r、v_r。

4.如权利要求1所述一种基于车辆四轮转向的自动泊车路径规划方法，其特征在于，所述步骤三中，所述两点边值约束用于提供车辆在起始时刻t＝0及终止时刻t＝t_f的运动状态，具体方法为，

当t＝0时：

式中，

对应着由车载传感器记录的初始运动状态信息；

当t＝t_f时，车辆最终稳定地停泊某一特定区域，此时：

v_i(t_f)＝0，a_i(t_f)＝0；要求车轮在终止时回正

ω_i(t_f)＝0；终止时刻达到某一既定姿态角sin(θ_i(t_f))＝sin(θ_fi)，cos(θ_i(t_f))＝cos(θ_fi)；根据车辆后轮轴中点坐标、车宽、车辆前悬距离、车辆后悬距离、轴距、位姿角等可得到车辆各个顶点坐标，根据停车位地四个顶点构成的矩形区域四条边所在直线，确定车辆各个顶点处于直线的哪一侧，以此来对x_i(t_f)、y_i(t_f)、θ_i(t_f)进行约束。

5.如权利要求1所述一种基于车辆四轮转向的自动泊车路径规划方法，其特征在于，所述步骤三中，所述流行约束主要是将控制变量u(t)以及状态变量x(t)限制在解空间中的某一高维曲面上或曲面所包络的区域内，具体限制包括：运动能力限制以及外部环境限制；

所述运动能力限制时满足：

式中，Φ_fmax代表车辆前轮转角的最大允许偏转角度值；Φ_rmax代表车辆后轮转角的最大允许偏转角度值；v_max是车辆在低速场景中的安全行驶速度上限；为保证乘客舒适性，a_max、Ω_fmax、Ω_rmax分别为线加速度，前轮转角速度及后轮转角速度设置的幅值；

所述外部环境限制时满足：建立车身矩形与凸多边形障碍物之间的碰撞躲避约束条件，通过三角形面积法，来对x_i(t_f)、y_i(t_f)、θ_i(t_f)进行约束。

6.如权利要求2所述一种基于车辆四轮转向的自动泊车路径规划方法，其特征在于，所述步骤一中，在父节点的选择过程中应用了的所述考虑代价项为：

式中，t,u,v表示归一化后对应每段圆弧或直线的弧度或长度；dir_i表示曲线组合中第i段路径的行驶方向，在此处设正向为1，反向为-1；在x<0时，f(x)＝0，在其他情况时，f(x)＝1。

7.如权利要求5所述一种基于车辆四轮转向的自动泊车路径规划方法，其特征在于，所述步骤三中，所述三角形面积法的原理为：

车辆每一顶点与凸多边形每两个相邻顶点分别组成三角形，将三角形面积累加，累加之和大于凸多边形面积则顶点在多边形外部，来保证每一时刻车身矩形顶点处于障碍物多边形外部，障碍物顶点处在车身矩形外部。

Images