基于图搜索和几何曲线的自主代客泊车路径规划

柏 鉴,徐元浩，冀 杰，陈云飞，张宇航

(西南大学 工程技术学院， 重庆 400715)

0 引言

自主代客泊车(automated valet parking，AVP)是目前自动驾驶汽车技术中最有可能应用落地和商业化的应用之一。路径规划问题是自动代客泊车系统所面临的主要挑战之一[1]。路径规划算法旨在许多障碍物之间找到一条从初始点到目标点的无碰撞路径[2]。目前，根据自主泊车实现方式的不同，可将广泛应用的路径规划算法分为以下4类：基于图搜索的算法、基于随机采样的算法、基于几何曲线插值的算法和数值优化法[3]。

对于图搜索算法，Yu等[4]定义了基于车辆泊车的停车场拓扑地图，采用Dijkstra算法分别得到了每个停车位到停车场入口的泊车路径。王永胜等[5]定义了考虑路径规划参数的停车场拓扑地图，并基于该地图通过A*算法得到了泊车全局路径。图搜索算法灵活直观，但对于先验地图的依赖较重；而在随机采样算法中， Zheng等[6]在传统RRT算法基础上引入车辆运动学约束，以获得垂直、平行、斜列式3种泊车路径，提高了计算效率，但所得路径质量的稳定性还需进一步提升。文献[7-9]通过几何曲线优化方式，使泊车路径满足车辆行驶的曲率连续要求。此外，Dubins等[10]基于车辆最小转弯半径，提出了满足车辆行驶且两点间最短的路径曲线，即Dubins曲线。Reeds和Shepp[11]在此基础上进行了拓展，提出了Reeds Shepp(RS)曲线。基于几何曲线的算法的共同缺点是路径规划需要建立于无障碍环境，难以直接应用于自动泊车。还有一些学者通过数值优化的算法，生成符合车辆行驶要求的无碰撞路径。钱立军等[12-13]对车辆行驶时的多种约束进行分析，通过hp伪谱法将路径规划问题转化成非线性规划问题进行求解，得出自主泊车的最佳路径。Zhang等[14]通过引入虚拟保护框架和放大参数，从几何角度提出了一种新的碰撞检测准则，优化了碰撞约束结构。但该类算法计算繁琐复杂，计算效率不高。Dolgov等[15]提出Hybrid A*算法，该算法以A*算法为基础结合车辆非完整约束及RS曲线进行拓展，生成的路径质量较好。但Hybrid A*算法在复杂的环境表现不佳，不适应停车场环境，易在一些死角处浪费大量探索时间。Jhang等[16]将双向RRT*算法和RS曲线相结合，并针对停车场环境修改了RRT*算法中的扩展方式。通过该算法直接获得车辆从停车场入口到完成泊车的完整路径。该算法考虑了停车场环境的限制，但由于RRT*算法的随机性质，使得规划路径的质量和效率均不够稳定。

提出了一种新的自动代客泊车路径规划方法，分为全局路径规划和泊车路径规划两个部分。全局路径规划算法将广义维诺图算法与Hybrid A*算法相结合，以广义维诺图生成的初步路径作为导向，引导混合A*的拓展，使规划的路径符合车辆行驶，避免了无效的搜索，节省了时间。泊车路径规划方面，通过对逆向泊车的碰撞约束，对文献[16]所提出的C字型泊车算法进行了改进，扩展了泊车起始点的选择范围，较固定航向角的泊车算法更为灵活。

1 自动泊车路径规划系统框架

以垂直停车位方案为例，得到AVP系统的路径规划方法，如图1所示。

图1 路径规划流程框图

当接收到停车命令时，全局泊车路径规划首先需要找到一条基于广义维诺图的行驶路径。Hybrid A*算法根据方向引导，生成一条从停车场入口到最佳停车点位置的全局路径；泊车路径规划采用C字型泊车算法生成停车入位的泊车路径。将全局路径和泊车路径相结合，得到AVP的最终路径。

本研究基于阿克曼转向模型完成泊车过程[17]，即在转弯过程中使所有4个车轮围绕一个公共节点滚动而不打滑。公共节点在图2中称为瞬时曲率中心O。阿克曼转向模型的运动学关系如式(1)所示：

(1)

图2中，Wd是车辆的宽度；Wx是轮距的长度；Lx是车辆轴距的长度；lf、lr分别为车辆的前悬、后悬；φ1是内侧车轮的转向角，φ2而是外侧车轮的转向角。

图2 阿克曼转向几何模型示意图

2 自动泊车全局路径规划

2.1 广义维诺图

维诺图由多组不同的两邻点的垂直平分线所组成的多边形构成[18]。在移动机器人路径规划领域，使用广义维诺图进行路径规划可使得路径规划问题减少一个维度，从而提高计算效率[19]。

对于自主泊车系统而言，路径规划可看成一个二维的问题，假设行驶的汽车为一个质点x，在2维平面中行驶。平面内充斥着凸形障碍物c1,c2,…,cn，非凸形障碍物建模为凸集的并集，其总集合为{ci}，且平面的边界也属于不可行驶区域，由一系列凸集组成，是{ci}的子集。则质点x离障碍物的距离di(p)可被定义为：

(2)

式中：di(x)的梯度为：

(3)

式中: ▽di(x)为到x的单位方向向量，c0为集合{ci}中距离x最近的一个点。对于凸集最近点总是唯一且存在的。对于R平面往往有多个障碍物，因此，可将多障碍物距离函数定义为：

(4)

广义维诺图的基本构成单元是与障碍物ci、cj距离相等的点集，且该点集中任何一点与其他障碍物的距离更大。该点集被称为两等距面。

(5)

如图3所示，Fijk是两等距线Fij、Fik以及Fjk的交点，根据广义维诺图的定义可知该点与障碍物ci、cj、ck的距离相等且唯一。

图3 广义维诺图

基于以上定义，利用式(6)表示广义维诺图(GVG)。其中，F2为所有Fij的集合，F3为所有Fijk的集合，在二维平面内，F2可视为广义维诺边的集合，F3为所有广义维诺顶点的集合。结合广义维诺边以及广义维诺顶点，可构造针对该地图的广义维诺图。

GVG=(F2,F3)

(6)

对于静态地图而言，利用已构造的广义维诺图，将初始点和目标点与广义维诺边匹配可以迅速找到一条粗略的可行驶路径，如图4所示。图中黑色部分为障碍物等不可行驶区域，灰色实线为生成的广义维诺图，黑色实线为基于广义维诺图所生成的连接目标点与起始点的路径。可以看出，广义维诺图将原本二维平面转化成了一维的点线匹配，极大地提高了运算效率。

图4 用于路径规划的广义维诺图

将广义维诺图用于路径规划最显著的优点是操作复杂度低、搜索时间消耗少，同时，也保证了路径与障碍物之间的距离。但此种方法算法实时性相对较差，由于未考虑车辆的运动学和动力学约束，该路径难以直接用于车辆的行驶。

在自主泊车中，可利用广义维诺图对停车场地图进行预处理。即将停车场环境中所有停车位视为障碍物，从而得到对应的广义维诺图，用于表示停车场内车辆的可行驶区域。基于同一场景下生成的广义维诺图相同这一性质，对于同一停车场，该广义维诺图可反复使用。

2.2 Hybrid A*算法

Hybrid A*算法与传统A*算法之间最大的区别在于扩展节点的方式不同。Hybrid A*算法以多种转向操作(左转、右转或不转弯)从父节点进行扩展，同时结合车辆运动学模型确定新的节点，从而保证子节点与父节点之间的运动连续性要求，因此，以该种方式所生成的路径能够满足车辆行驶的需求，如图5所示。

图5 Hybrid A*拓展节点方式示意图

在路径规划中，可使用三维坐标(x,y,θ)来描述车辆的位姿。式中，x是横坐标，y是纵坐标，θ是车辆的航向角。Hybrid A*算法节点扩展过程如图6所示，图中Ni-1与Ni分别代表前一个节点与当前节点，基本原理如式(7)—(11)所示：

图6 Hybrid A*生成新节点原理示意图

(7)

(8)

xi=xi+1+Ri[sin(θi+1+β)-sin(θi-1)]

(9)

yi=xi+1-Ri[cos(θi+1+β)+cos(θi-1)]

(10)

θi=θi-1+β

(11)

式中：Larc为Hybrid A*算法每次扩展的步长，α为车辆转向角，在进行扩展时，也可直接令Ri为车辆的最小转弯半径。

Hybrid A*算法实时判断是否使用无碰撞的RS曲线连接当前点与目标点，如满足连接条件，则直接使用RS曲线作为路径输出。

2.3 结合广义维诺图的Hybrid A*算法

为了解决广义维诺图生成的路径不符合车辆行驶动力学的问题，Hybrid A*算法在路径生成时充分考虑了车辆的非完整特性。但是，Hybrid A*算法有时会在交叉口浪费大量时间，如图7中圈出的路径。在停车场中，交叉口会大量存在，这意味着若直接将传统的Hybrid A*算法应用于AVP，会浪费大量算力、效率较低。

图7 Hybrid A*算法规划结果示意图

为解决该问题，提出了定向的Hybrid A*算法，首先基于停车场场景生成广义维诺图，然后通过广义维诺图得到通往泊车起始点的大体路径V-path。该路径为混合动力A*提供正确的方向，以便在考虑车辆非完整约束的情况下快速生成可行驶的路径。

Hybrid A*算法在选取拓展节点时，会计算周边可拓展点的成本，如式(12)所示：

F(x)=g(x)+h(x)

(12)

式中：F(x)为该节点总成本，g(x)为初始节点到该节点所花费的成本，h(x)为A*算法的启发式函数，表示该节点到目标节点的估计成本，一般采用曼哈顿距离进行描述。定向Hybrid A*算法计算拓展成本时，引入了函数V(x)，描述该点与V-path之间的关系，如式(13)所示：

F(x)=kgg(x)+khh(x)+kvV(x)

(13)

式中：kg、kh、kv分别表示不同的权重系数。

定向Hybrid A*规划结果如图8所示。由于V(x)函数的引入，使得该算法在选取拓展节点时，具有了一个方向上的指引，从而避免了冗余搜索。其次，维诺图具有良好的避障能力，V(x)函数的引入使得该算法所规划的路径更倾向于远离障碍物，提高了安全性能，解决了传统路径规划绕墙走的问题。因此在实际使用中，无须膨胀障碍物，进一步提升了算法的计算效率。

图8 定向Hybrid A*规划结果示意图

故定向Hybrid A*算法可以有效地为AVP生成一条可行的全局路径。全局路径规划算法的具体步骤如下：

1) 获取基于停车场的广义维诺图、车辆初始位姿、泊车目标位姿和障碍物位置。

2) 确立泊车坐标系，对垂直停车而言，以车辆后轴中心为原点定义坐标系，如图9所示，图中d为预留安全距离(即车体与后方障碍物的距离)，车体与车位边界平行，车辆后轴中心与泊车目标点重合。

图9 泊车位移计算示意图

3) 计算泊车起始点，通过改进的基于几何曲线的泊车路径规划算法(具体内容见下节)，计算出泊车起始点的位姿。

4) 通过广义维诺图，找到一条从起始位置到全局路径目标位置的大体路径(V-path)，为下一步的全局路径规划提供参考方向。

5) 通过定向Hybrid A*算法，生成最终的全局路径。

该全局路径规划算法不仅考虑了车辆的非完整特性，还避免了冗余搜索，极大提高了路径规划效率。

3 局部泊车路径规划

泊车路径规划可以根据停车位和车辆轮廓等参数，为指定的停车位生成停车路径。选用停车场常见的垂直停车场景进行局部泊车路径规划。C字型泊车规划能够使车辆实现垂直泊车入位的目标[16]。然而，在大多数的研究中，此类泊车算法中的每个停车位的停车起始节点、泊车路线相对固定，并且停车起始航向角必须为零，此外，汽车距车位边界的临界侧向距离h需大于hmin。因此，难以适用于一些复杂的泊车场景。基于无碰撞约束对C型泊车规划加以优化，使得每个泊车位有多条泊车路线以及多个泊车起始点，能够扩大泊车起始点的选择范围。

(14)

式中：Rmin为小转弯半径。

如图10所示，C型泊车规划可分为两段：方向盘右转车辆从M1倒退至M2点；最终方向盘保持平衡车辆从M2倒退至M3点即泊车结束点。可将其一般化，车辆先沿着一段圆弧倒退行驶一段距离，再沿着垂直方向倒退一段距离最终达到泊车位置。基于上文所建立的坐标系，设泊车过程中车辆的竖直和水平位移分别为W、H：

图10 C型泊车算法示意图

H=R(1-sinθ)

(15)

W=Rcosθ+S

(16)

式中：R为泊车过程中车辆转弯半径，S为车辆最后一段垂直倒车的距离，θ为泊车起始时的航向角，当θ=0时，此时路径为传统C型规划。

得到W、H的数值后，结合已知的泊车位置等信息便可得到泊车起始位姿，再通过C型泊车规划得到相应泊车路径。由于左、右两侧的停车过程是对称的，此处只讨论从停车位右侧停车的情况。

为了得到最优的泊车起始点，需得到泊车起始点的选取范围。根据式(15)和(16)，参数R、θ、S决定了W、H的值，而W、H的值直接决定了泊车起始点的位置。因此，需确定参数R、θ、S的范围。

对于泊车起始点航向角θ来说，其最大值为π/2，即车辆与停车位平行时。当θ取最小值时，车辆相对停车位的位置如图11所示。此时，θmin满足：

图11 最小起始航向角示意图

(17)

RRF×sin(θi-θmin)=R

(18)

(19)

整理可得：

(20)

R、S均属于泊车路径中的几何参数，理论上车辆的泊车轨迹和车辆逆向泊车轨迹保持一致。因此，可通过定义逆向泊车过程中的无碰撞约束从而确定其取值范围。

逆向停车过程中可能发生的碰撞情况包括：

1) 车辆驶出停车位时，车辆右侧边线可能和边界相撞，如图12所示。为避免碰撞发生，应满足式(21)中的条件：

图12 车辆右侧的碰撞示意图

(21)

2)车辆驶出停车位时，车辆左后侧可能和边界相撞，如图13所示。为避免碰撞发生，应满足式(22)中的条件：

图13 车辆左后角的碰撞示意图

(22)

3) 车辆驶出停车位时，车辆左前侧可能和边界相撞，如图14所示。为避免碰撞发生，式(25)、(26)应被满足：

图14 车辆左前角的碰撞示意图

(23)

(24)

(25)

RLF

(26)

4) 车辆驶出停车位时，车辆右前侧可能和边界相撞，如图15所示。为避免碰撞发生，应满足式(27)(28)中的条件：

图15 车辆右前角的碰撞示意图

L-d-lr

(27)

(28)

在整个逆向泊车的过程中，一共有上述4种潜在碰撞情况，可建立C型泊车规划算法的完整约束方程，如式(29)所示：

(29)

通过该方程可确定参数R、S、θ的范围，即泊车起始点的范围。为得到最佳的泊车起始点，引入评价函数f(x)：

f(x)=S+θ+R

(30)

取可行域内f(x)计算结果最小值为最佳参数组合。根据式(15)、(16)即可得到对应泊车过程中的水平和竖直位移，进而得到泊车起始点。

4 仿真实验分析

4.1 算法效率比较

建立模拟停车场结构的栅格地图，并以同一起始点，3种不同的终点位姿作为输入，分别对所提出的全局规划算法和Hybrid A*算法进行仿真和比较。为进一步体现算法性能，仿真过程中障碍物均未做膨胀处理，使用的处理器为Intel Core i5-10400 CPU 2.90 GHz。

仿真结果如图16、表1所示。图16中，菱形为起始点，圆点为目标点，黑色粗实线为规划路径。细实线和虚线分别代表算法向前、向后探索的过程。第一行的三幅图分别为传统Hybrid A*算法在场景1、场景2、场景3的规划结果；第二行的三幅图分别为定向Hybrid A*算法在场景1、场景2、场景3的规划结果。

表1 算法仿真结果对比

图16 算法仿真结果

具体的试验数据见表1，在场景1中，改进算法用时2.3 s左右，而传统Hybrid A*算法用时3.7 s左右；场景2中改进算法用时3.8 s左右，而传统算法用时6.2 s左右；在场景3中改进算法用时6.8 s左右，传统算法用时9.12 s左右。在运行时间方面，改进算法所用的时间均小于传统的Hybrid A*算法，且随着环境的复杂，其优势越明显。在拓展节点方面，改进算法的节点远远少于Hybrid A*的节点数量。

结果显示，在效率方面，即使场景1中传统Hybrid A*没有交叉口的冗余探索，改进算法的仿真时间和节点数仍小于传统Hybrid A*算法；在场景2、场景3中改进算法避免了传统Hybrid A*算法在交叉口的冗余探索，改进算法的仿真时间和节点数均小于传统Hybrid A*算法。在路径质量方面，改进算法所规划的路径在路径前半段存在一些曲折的现象，原因在于所规划广义维诺图在三方障碍物交界时，多为Y型，对拓展的引导造成了干扰。且传统Hybrid A*算法所规划的路径十分贴近未经膨胀的障碍物，而定向Hybrid A*算法所规划的路径具有显著的远离障碍物的特性。

因此,相对于传统Hybrid A*算法，改进算法在规划效率以及避障性能方面有着不小的提升。

4.2 仿真试验结果

为了验证该路径规划方法的可行性，以实际停车场的测量数据为依据，建立栅格地图并进行仿真。车辆参数见表2，停车场地见图17。

表2 仿真参数

图17 停车场地

图18中的4张图分别为第1个泊车位的全局路径、泊车路径及其车辆轮廓图。图中黑色曲线是所得的泊车路径，黑色方框表示车辆轮廓线。图19为第2个泊车位对应的路径规划结果。

图18 泊车场景1规划路径

图19 泊车场景2规划路径

在路径曲率方面，图20(a)为场景1全局路径的路径曲率曲线；图20(b)为场景2全局路径的路径曲率曲线。可见在整条路径中，曲率不存在阶跃式的突变，路径的曲率连续，第1个场景中路径最大曲率为0.097 m-1；第2个场景中路径最大曲率为0.107 3 m-1，满足车辆的行驶要求。泊车路径均为圆弧直线的组合，圆弧上曲率不变，故不做比较。在避障方面，场景1全局路径中车辆轮廓与障碍物的最短距离为0.73 m；泊车路径中车辆轮廓与停车位车道线的最短距离为0.45 m。场景2全局路径中车辆轮廓与障碍物的最短距离为1.34 m；泊车路径中车辆轮廓与停车位车道线的最短距离为 0.15 m。可见，该算法成功规划了对应泊车位的无碰撞全局路径和泊车路径，保证了车辆的安全行驶。

图20 不同场景下全局路径的曲率

通过以上仿真结果可以发现，该方法在停车场一类的环境下，克服了传统Hybrid A*算法容易过度冗余搜索的问题；同时在障碍物未经膨胀处理的情况下，所规划的路径仍然能保持一定的安全距离。因此该方法具有计算效率高、避障性好等优点，在停车场场景中能够得到较好的应用。

为了验证规划路径的可行性，对所规划的路径采用了基于Matlab/Simulink和CarSim的联合仿真。在路径跟踪方面，采用基于五次多项式的方式生成速度曲线，纵向控制采用了PI控制，横向控制采用了Sanley控制器[20]。结果如图21所示，其中，图21(a)为路径跟踪时的实际轨迹与规划路径，实线表示路径跟踪车辆的实际轨迹，虚线是规划路径；图21(b)为路径跟踪时实际的车辆轮廓图，用来显示路径跟踪过程中的车辆姿态，判断是否发生碰撞。

图21 路径跟踪仿真实验结果

路径规划和路径跟踪联合仿真的结果表明，提出的路径规划方法能够生成一条无碰撞路径，并能够实现自动泊车目标。

4.3 实车试验结果

为进一步验证所设计路径规划方法的可行性，以图22所示的符合阿克曼转向几何模型的无人驾驶移动平台为试验样车进行了实车试验。

图22 实车试验平台

试验结果如图23所示。其中红色曲线为该方法所规划的自主泊车路径，蓝色的线为司南导航系统输出的GPS数据即车辆轨迹。

图23 实车跟踪轨迹

结果显示，所规划的路径有较好的可跟踪性能，使得车辆能够成功地泊进车位。

针对试验数据进行分析，图24为规划路径的曲率。在规划路径中，路径点1～88为全局路径，88～100为泊车路径。可见在路径曲率方面，整条路径中，曲率仅有一些轻微抖动，这是由于路径取点存在间隔以及未经平滑处理导致的，但曲率不存在阶跃式的突变，足以满足车辆的行驶需求。

图24 规划路径曲率

图25为车辆自主泊车过程中当前位置与最近规划路径点的航向角误差与横向误差。从图25(a)中可以看出，在自主泊车过程中航向角误差不超过±0.2 rad，完成泊车时，航向角误差为 -0.005 4 rad；从图25(b)中可以看出，在路径跟踪过程中横向误差不超过±0.23 m，在泊车过程中横向误差不超过0.102 m。

图25 泊车过程航向角误差与横向误差

造成航向角和横向误差在一定程度上振荡的主要原因有：试验车辆部分动力学参数存在不确定性，试验时采用简单的几何运动学模型，同时控制算法精度不够。基于以上无法避免的影响因素，其自主泊车过程跟踪误差仍在合理范围内，且该规划路径能够实现自主泊车的目标，同时具有较好的可跟踪性，进而证明了所设计的自主泊车路径规划算法在真实停车场的有效性。

5 结论

1) 提出了一种新的自动泊车路径规划算法，即基于广义维诺图的Hybrid A*算法。仿真结果表明，该算法比传统的Hybrid A*算法具有效率更高、避障性更好等优点。该算法所规划的路径前段可能存在一些不必要的曲折，需要对路径优化做进一步研究。

2) 通过对几何曲线的碰撞约束进行分析，改进了C型泊车路径规划算法，使停车起始节点选择范围更广，提高了适用性。其他类型的停车几何曲线也可以用同样的方法改进。

3) 在同一场景下，对提出的算法和Hybrid A*算法进行了对比分析，证明了该算法的优势。再通过联合仿真及实车试验的方式表明所得路径具有良好的可跟踪性，验证了该算法在真实停车场的有效性。