一种自主泊车路径反向生成规划方法与流程

本发明涉及一种自动泊车的技术，尤其是一种对泊车的路径进行规划的方法。

背景技术：

随着自动驾驶技术的发展，自动泊车技术的研究也得到了广泛重视，自动泊车的路径规划问题也受到了学术界和工业界的广泛重视。为了实现自主泊车功能，自主泊车路径规划任务是，当给定停车位位置以及类型信息时，本发明需要找到最佳的无碰撞轨迹，在车辆运动约束下操纵车辆运动到目标位置。按照泊车习惯，人们通常习惯采用倒车入库的方式进行泊车，倒车入库时，通常车辆需要向前行驶一段距离，然后向后倒车进入停车位，然而，对自主泊车来说这项任务涉及若干困难，在路径规划过程中，通常是将自动驾驶的路径规划与泊车路径规划分开进行，泊车过程也涉及到两段路径，前进段路径和倒车段路径结合问题，意味着这个过程比普通的自主驾驶路径规划任务更复杂。

传统的泊车路径规划方法通常采用间接法，前段轨迹车辆沿直线行驶搜索车位，倒车轨迹通常采用最小转弯半径法，粗略的跟踪轨迹，这种轨迹误差较大，导致车辆与周围车辆发生碰撞，会导致泊车失败。针对不同车位类型，间接法需要采用不同的轨迹规划方法。因为车辆需要沿直线行驶搜索车位，因此车辆存在微小误差时，对泊车过程会产生较大误差，通常会导致泊车失败。

在专利cn102975715a中阐述了一种车辆在任意姿态下进行平行泊车路径规划的方法，在其文章中，需要建联拟合点阵，通过遍历连接车辆起点与终点的点阵拟合出的样条曲线，最后再结合车辆的运动学约束选取一条轨迹。这种方法仅针对一种停车位类型，并且路径生成过程需要拟合点阵，增加了路径生成的难度。

申请号为201810071201.9的专利中提供一种用于自动平行停泊的路径规划方法，该方法根据待泊车车辆车型的几何参数进行仿真得到平行泊车路径规划。这种方法只适用于固定起始位姿的泊车路径规划。

上述几种方法是从倒车段轨迹开始进行规划的，虽然有效解决了不同位姿对泊车轨迹的影响，但对于自动驾驶车辆来说，泊车时车辆需要自主行驶到倒车段的起始位置，上述方法中都没有对车辆如何行驶到倒车段起点进行说明，都没有提到自主泊车路径规划与自动驾驶路径规划相结合。

技术实现要素：

本文提出了一种用于自主泊车的反向生成路径规划方法，这种方法适用于不同停车位类型，不同的起始泊车位姿的泊车路径规划，还可以与自动驾驶的路径规划相结合，用来解决现有泊车路径规划方法的不足。

本发明提出了一种自主泊车路径反向生成规划方法，包括如下步骤：

步骤1、目标停车位信息获取，包括车位类型及位置信息；

步骤2、根据目标停车位信息，确定泊车初始状态和目标状态；

步骤3、根据停车位信息和目标状态，利用几何法，生成相应的目标路径树；

步骤4、根据车辆的初始状态，利用采样法生成连接目标路径树终点的多条路径；

步骤5、根据代价函数从多条路径选取最优路径。

进一步的，所述的步骤1具体包括：自主泊车车辆通过存储的或者通过基于v2x的通信方式获取停车位信息，包括停车位相对于车辆的横纵坐标信息，以及车辆停进停车位时的航向信息，及车辆停进停车位的最终位姿e(xe,ye,θe)，其中xe,ye,θe分别为车辆最终位置横坐标、纵坐标、航向。

进一步的，所述的步骤2具体包括：当车辆行驶到停车位附近时，车辆通过传感器获取当前位姿s(xs,ys,θs)，xs、ys、θs即车辆当前位置的横坐标、纵坐标，航向。

进一步的，所述的步骤3具体包括：车辆以当前位姿为起点，向终点状态g(xg,yg,θg)进行路径规划，基于反向行驶的方法，假设车辆已经停进停车位，车辆驶出停车位，先以终点状态为起点，生成目标树。

进一步的，所述的步骤4具体包括：从车辆当前的位置，即当前位姿，以目标树终点为路径生成的终点，通过生成的路径连接到目标树，生成多条泊车预测路径。

进一步的，包括生成路径时，以目标树的终点为采样点，使用具有最小曲率变化率的三次hermite样条曲线生成法生成一簇连接从当前车辆位置到目标树终点的平滑曲线，得到多条从当前位姿s(xs,ys,θs)到最终位姿e(xe,ye,θe)的路径，每条路径表示车辆可能执行的一个状态，

进一步的，所述的步骤5具体包括：生成的泊车预测路径中，对于部分路径会因为路径的曲率过大，不满足车辆运动学模型约束，部分轨迹上存在障碍物，依据与障碍物距离，平滑程度，路径长度为评估依据，通过对这些评估依据进行加权计算，结合代价函数cf选取最优路径，，即为最终的泊车路径。

进一步的，所述步骤3中相应的生成目标路径树的方法包括：平行停车位目标树生成方法、垂直停车位目标树生成方法、斜式停车位目标树生成方法。

进一步的，所述平行停车位目标树生成方法、垂直停车位目标树生成方法、斜式停车位目标树生成方法具体包括如下步骤：

假设一辆车已停在停车位中，用各种预先定义的路径将车开出停车位，e点为车辆驶出停车位的起始点，e1点为第一个转向点，g1-gi为倒车点；以e1点起始点，以不同的转弯半径r进行计算，生成多条路径e1gi，采用反向规划方法，即得到从g1到gi的多条倒车路径，即车辆倒车进入停车位的不同条路径。

e1gi[k].x＝e1.x+(r*sin(θ))

e1gi[k].y＝e1.y+(r-r*cos(θ))

式中，r为不同方向盘转角对应的车辆转弯半径，θ表示路径点生成过程中路点与路径起点在圆弧路径上的夹角，k表示多条路径即目标树对应的第k条路径，x、y分别表示路径点相对于车辆位置的横纵坐标，利用上述公式生成多条路径，多条路径组成一组供车辆选取的行驶的预测路径，即为目标树，为一组半径不同的圆弧。

进一步的，所述的最优路径包括两部分：前段路径和后段路径，前段路径来自采样法的一部分，后段路径来自目标树的一部分。

有益效果：

本文提出的泊车方法利用先验的停车位信息，针对不同的停车位，不同的车辆起始位姿，都能从起始位置生成泊入停车位内的路径。采用逆向规划的方式，假设车辆已经停进停车位，车辆驶出停车位时，按照不同的驶出策略，车辆有很多种方法驶出停车位，不同的驶出策略代表有不同的路径可以使车辆驶出停车位，从停车位驶出的路径可以看成一组路径树，车辆驶出停车位的过程的反向，可以看成车辆驶进停车位的过程，采用采样法，从当前车辆位置，生成连接路径树的路径，通过代价函数进行筛选，即可得到最终的泊车路径。

附图说明

图1停车位类型；

图2本发明平行停车位目标树；

图3本发明垂直停车位目标树；

图4本发明斜式停车位目标树；

图5本发明状态空间采样；

图6本发明平行停车位自主泊车路径规划过程示意图；

图7本发明的平行停车位自主泊车路径规划示意图；

图8本发明的不同起始位姿平行泊车路径规划过程示意图；

图9本发明的不同起始位姿垂直泊车路径规划过程示意图；

图10本发明的不同起始位姿斜式泊车路径规划过程示意图。

具体实施方式:

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

本文提出的泊车方法利用先验的停车位信息，针对不同的停车位，不同的车辆起始位姿，都能从起始位置生成泊入停车位内的路径。采用逆向规划的方式，假设车辆已经停进停车位，车辆驶出停车位时，按照不同的驶出策略，车辆有很多种方法驶出停车位，不同的驶出策略代表有不同的路径可以使车辆驶出停车位，从停车位驶出的路径可以看成一组路径树，车辆驶出停车位的过程的反向，可以看成车辆驶进停车位的过程，采用采样法，从当前车辆位置，生成连接路径树的路径，通过代价函数进行筛选，即可得到最终的泊车路径。

一种自主泊车路径反向生成规划方法，用于不同起始位姿不同停车位类型的车辆自主泊车路径规划，具体步骤如下：

步骤1、目标停车位信息获取，包括车位类型及位置信息；

步骤2、根据目标停车位信息，确定泊车初始状态和目标状态；

步骤3、根据停车位信息和目标状态，利用几何法，生成相应的目标路径树；

步骤4、根据车辆的初始状态，利用采样法生成连接目标路径树终点的多条路径；

步骤5、根据代价函数选取最优路径；

该方法对泊车时车辆初始位姿无要求，路径生成以几何法为基础，车辆驶出停车位的路径通过几何法得到，几何法计算简单，车辆按照不同的转弯半径驶出停车位，生成路径树，不同的停车位生成不同的路径树，解决了几何法的泊车策略受限于停车位形状以及几何法对泊车起始位置要求严格,不适当的起始位置会导致泊车操作失败的问题。

所述的泊车路径规划还包括泊车路径生成步骤，如下：

参见图1，停车位通常分为三种：垂直停车位，平行停车位和斜式停车位。本发明先假设，汽车已经停放在停车位，本发明根据不同的停车位生成了三种不同的目标树，它们应用于三种不同的停车位。每个目标树都有数十个路径。车辆停进停车位后的位姿为最终位姿e(xe,ye,θe)(即车辆位置横纵坐标，航向)，从最终位姿e(xe,ye,θe)开始，以不同的转弯半径生成一组数十条路径的目标树，目标树的终点状态为g(xg,yg,θg)(即目标树终点位置的横纵坐标，方向)然后本发明采用采样法，从车辆的起始位姿s(xs,ys,θs)(即车辆当前位置的横纵坐标，航向)开始，将生成的目标树的终点g(xg,yg,θg)作为采样的目标点，最后结合运动学约束，障碍物约束以及轨迹长度等条件选出最优轨迹，这时就可以生成最终路径。该路径由两部分组成：前段路径和后段路径，前段路径来自采样法的一部分，后段路径来自目标树的一部分。

步骤1，自主泊车车辆通过存储的或者通过基于v2x的通信等获取停车位信息，包括停车位相对于车辆的横纵坐标信息，以及车辆停进停车位时的航向信息，及车辆停进停车位的最终位姿e(xe,ye,θe)。

步骤2，当车辆行驶到停车位附近时，车辆通过传感器获取当前的位姿s(xs,ys,θs)，并以当前位姿为起点，向终点状态g(xg,yg,θg)进行路径规划。

所述步骤3中相应的生成目标路径树的方法包括：

(1)平行停车位目标树生成方法

如图2所示，平行停车位目标树：一辆汽车驶出一个平行的停车位，它将保持最大转弯角度，向停车位外行驶，在图中，e点为车辆驶出停车位的起始点，e1点为第一个转向点，g1-gi为倒车点。在e到e1点的方向盘转角使得车辆在最小半径下行驶，显而易见，这样对停车环境的要求最为宽松，更加地节约空间。同时，当车辆驶出到e1点时，车辆将有更多种方法驶出停车位，当车辆以固定的方向盘转角行驶时，会生成一条转弯半径固定的路径，当车辆的方向盘转角不同时，这时从g1到gi，将生成多条路径。

e1gi[k].x＝e1.x+(r*sin(b))

e1gi[k].y＝e1.y+(r-r*cos(b))

其中r为不同方向盘转角对应的车辆转弯半径，b表示路径点生成过程中路点与路径起点在圆弧路径上的夹角，k表示多条路径即目标树对应的第k条路径，x、y分别表示路径点相对于车辆位置的横纵坐标，利用上述公式可以生成多条路径，多条路径组成一组车辆可选取的行驶的预测路径，即为目标树，目标树如图2所示，为一组半径不同的圆弧组成。

(2)垂直停车位目标树生成方法

如图3所示，垂直停车位目标树：一辆汽车驶出一个垂直的停车位，它将先保持直线，向停车位外行驶，在图中，e点为车辆驶出停车位的起始点，e1点为第一个转向点，g1-gi为目标树终点。在e到e1点车辆保持直线行驶状态，向前行驶一段距离，这时当车辆驶出到e1点时，车辆将有更多种方法驶出垂直停车位，这时从g1到gi，以最小转弯半径为约束，生成其他固定转弯角度的路径，垂直停车位目标树生成方法同平行停车位目标树生成方法，为一组半径不同的圆弧组成的泊车预测路径。

(3)斜式停车位目标树生成方法

如图4所示，斜式停车位目标树：一辆汽车驶出一个斜式的停车位，它将先保持直线，向停车位外行驶，在图中，e点为车辆驶出停车位的起始点，e1点为第一个转向点，g1-gi为目标树终点。在e到e1点车辆保持直线行驶状态，向前行驶一段距离，这时当车辆驶出到e1点时，车辆将有更多种方法驶出垂直停车位，这时从g1到gi，以最小转弯半径为约束，生成其他固定转弯角度的路径，斜式停车位目标树生成方法同平行停车位目标树生成方法，为一组半径不同的圆弧组成的泊车预测路径。

前段路径生成方法

参考路径是引导无人驾驶车辆通过任务路段抵达终点的一条路径曲线，当存在参考路径时，采样法沿参考路径在状态空间采样，使用具有最小曲率变化率的三次hermite样条曲线生成法生成一簇平滑曲线，表示方向盘变动最小的曲线，生成可行的路径集，使得运动控制器可以轻松跟踪，同时符合道路形状约束。

首先采用多分辨率采样根据道路几何特征对状态空间的一组终端状态进行采样，如图5所示。采样终端状态在预瞄距离ρ下沿参考路径均匀取点。预瞄距离ρ，即图中p0到pg之间的距离，应大于最小碰撞距离dmin并小于最大感知距离dmax。

选取轨迹的评估依据：与障碍物的距离；平滑程度；以及与历史轨迹数据的误差。最终根据这些评估选择最优轨迹。

在本发明中，在泊车过程中当车辆处于泊车区域内时，从车辆的当前位姿s(xs,ys,θs)到目标树的终点，无参考路径，因此生成路径时，以目标树的终点为采样点，使用具有最小曲率变化率的三次hermite样条曲线生成法生成一簇连接从当前车辆位置到目标树的平滑曲线，这样可以得到多条从当前位姿s(xs,ys,θs)到最终位姿e(xe,ye,θe)的路径，每条路径表示车辆可能执行的一个状态，最后依据与障碍物距离，平滑程度，路径长度为评估依据，通过对这些评估依据进行加权计算，选取最优路径。

代价函数计算

车辆与障碍物位置的距离：

cobdis＝f((xs,ys),(xob,yob))

(xs,ys)为当前时刻车身位置坐标，(xob,yob)为障碍物位置坐标；

路径长度：

cleng.h＝f((xs,ys),(xg,yg))+f((xg,yg),(xe,ye))

对路径长度的评估分为两部分，主要包括车辆当前位置(xs,ys)到目标树终点的位置(xg,yg)以及目标树终点位置(xg,yg)到最终位置的距离(xe,ye)；

轨迹的平滑性：

csmooth＝f(k)

ki表示规划路径中每一点的曲率：

其中，

4θi＝|θi+9-θi|

x,y表示路点横纵坐标，δ6i为相邻两个路点之间的距离；

路径平滑度(smoothness，f(k))：

n表示路点总数；

通过对上述公式进行加权计算得到最优路径：

cf＝αcobdis+βclength+γcsmooth

α，β，γ分别对应cobdis，clength，csmooth分配的权值可计算出轨迹的代价，择其最小值为最优路径,即最终的泊车路径。

具体路径规划实施方式：

在满足运动学模型和约束的前提下，如何在停车区域内规划出一条合理的泊车路径，是无人驾驶车辆自动泊车系统的关键。采用上述方法最终生成的泊车路径由两部分组成：前段路径来自采样法的一部分和后段路径来自目标树的一部分。

如图6所示，假设车辆当前的起始位姿为s(xs,ys,θs)，停车位类型已知为平行车位，位置已知为e(xe,ye,θe)，这时可以生成目标树路径和连接目标树的终点位姿g1-gi，共gi条路径，在gi条泊车预测路径中，有的路径会因为路径的曲率过大，不满足车辆运动学模型约束，有的轨迹上存在障碍物，因而结合代价函数cf，可以选取最优的路径。

结合代价函数cf选取的最终如图7所示，则该路径就是最终的泊车路径。车辆跟踪该条路径泊入停车位。

不同起始位姿自主泊车路径规划实施方式：

如图8所示，水平泊车时，从停车位生成多条路径(本图仅选取具有代表性的三条路径，车辆行驶到路径树终点时，与车位垂直，平行和呈一定夹角的三条轨迹)，形成路径树，车辆从任意位姿，以路径树终点为采样点，形成多条路径，最后结合运动学约束，轨迹平滑度，障碍物约束以及轨迹长度等条件选出最优路径，图中假设短线-点划线所代表路径为最优路径，则车辆跟踪短线-点划线路径，然后跟踪与短线-点划线轨迹相连接目标树路径，泊入停车位。

如图9所示，垂直泊车时，从停车位生成多条路径，形成目标路径树，车辆从任意位姿，以路径树终点为采样点，形成多条路径，最后结合运动学约束，轨迹平滑度，障碍物约束以及路径长度等条件选出最优路径，图中假设短线-点划线所代表路径为最优路径，则车辆跟踪短线-点划线路径，然后跟踪与短线-点划线轨迹相连接目标树路径，泊入停车位。

如图10所示，斜式泊车时，从停车位生成多条路径，形成目标路径树，车辆从任意位姿，以路径树终点为采样点，形成多条路径，最后结合运动学约束，轨迹平滑度，障碍物约束以及路径长度等条件选出最优路径，图中假设短线-点划线所代表路径为最优路径，则车辆跟踪短线-点划线路径，然后跟踪与短线-点划线轨迹相连接目标树路径，泊入停车位内。

本发明适用于多种泊车场景，图8，9，10分别示意了车辆以不同泊车位姿在平行停车位，垂直停车位和斜式停车位情况下应用本发明所得到的泊车路径。应当注意的是，本发明并非限定停车位周围障碍物为其他车辆，也可以是其他障碍物，本发明也并未限定停车位周围障碍物数量为1，其他数量的障碍物也适用于本发明应用场景，本发明对存在障碍物的路径生成情况并非仅针对平行停车位，也适用于垂直停车位和斜式停车位。

尽管上面对本发明说明性的具体实施方式进行了描述，以便于本技术领域的技术人员理解本发明，且应该清楚，本发明不限于具体实施方式的范围，对本技术领域的普通技术人员来讲，只要各种变化在所附的权利要求限定和确定的本发明的精神和范围内，这些变化是显而易见的，一切利用本发明构思的发明创造均在保护之列。