自动驾驶车辆的平行泊车轨迹规划*

胡 杰，张敏超，徐文才，陈瑞楠，钟鑫凯，朱令磊

（1.武汉理工大学，现代汽车零部件技术湖北省重点实验室，武汉 430070；2.武汉理工大学，汽车零部件技术湖北省协同创新中心，武汉 430070；3.新能源与智能网联车湖北工程技术研究中心，武汉 430070）

前言

泊车场景下的自动驾驶技术渐渐成为研究重点，其中代客泊车便是典型的应用。目前，对自动泊车路径规划方法的研究大致可以分为几何法、多步法、采样法和数值优化法4类。

几何法大多考虑采用圆弧加直线来构造路径，但是采用该法构造的路径曲率不连续，这会导致车辆在进行路径跟踪时出现原地转向问题，并且该方法对泊车初始位姿要求比较严格。针对这一问题，文献［6］～文献［14］中采用拟合曲线对路径进行优化，解决了路径曲率不连续问题，但增加了对泊车位大小的要求，而且并未解决初始位姿的问题。文献［15］中将平行泊车路径分为两段，采用五次多项式生成路径曲线解决了泊车初始位姿问题，但其第二段曲线也增加了对泊车位大小的要求。

多步法是将自动泊车路径约束进行逐步考虑，然后分步获取满足约束的路径。文献［16］和文献［17］中采用一种非完整系统的三步路径规划方法，该方法首先计算无碰撞路径，然后通过考虑非完整约束和其他运动学约束的局部规划器计算的线段连接来逼近第一步中获得的路径，最后将第二步生成的路径优化为更短、更平滑路径，但是多步法通常会导致规划出的路径有较多的转折点，这会使车辆频繁地前进后退。

采样法是机器人和计算机科学中研究的基于采样的寻径算法。在文献［18］中开发了一种基于有序采样的算法，该算法对汽车从泊车位后退的可能轨迹进行有序采样，然后根据安全、机动等性能标准对停车路径进行优化。该方法对各种初始起始位姿以及泊车场景有很好的适应能力，但在每个计算周期内都须对轨迹进行采样计算，计算时间较长。

数值优化方法通过引入最优目标，将非完整系统的运动规划问题重新表述为开环最优控制问题，并进行数值求解。文献［19］中找到了所有3种停车情况下的最小时间的路径，并使用序列二次规划（SQP）来求解优化问题。在文献［20］中也进行了类似的研究，采用内点法求解优化问题。但数值优化方法计算时间非常长，在当前典型的CPU上进行计算可能需要40 s以上的时间。表1所示为自动泊车路径规划各种方法对比。

表1 自动泊车路径规划方法对比

针对以上方法存在的问题，本文中对自动驾驶车辆平行泊车场景进行分析，结合采样法对场景的适应能力较强和几何法计算简单的特点，提出一种基于自动驾驶车辆的平行泊车轨迹规划方法。该方法对各种泊车初始位姿有较强的适应能力，同时计算时间较短，能较好地满足自动驾驶车辆平行泊车的需求。

1 车辆运动学模型

泊车是一个低速行驶且基本不受侧向力的过程，行驶中轮胎没有发生滑移作纯滚动，其转向轴线均交于一点作圆周运动。因此将整车视为一个刚体，从而能够基于阿克曼转向原理来进行运动学研究。

车辆运动学模型和车辆尺寸参数如图1所示，其中x、y为车辆后轴中心坐标，θ为车辆航向角，φ为前轮等效转向角，v为后轴中心速度，L为整车长度，W为整车宽度，L为车辆前悬长度，L为轴距，L为车辆后悬长度。A(x，y)、B(x，y)、C(x，y)、D(x，y)为汽车车身4个顶点坐标。

图1 汽车运动学模型

以车辆后轴中心为参考点，（x，y，θ，φ）为车辆的位姿向量，车辆在沿给定路径转向行驶时，根据车辆运动学关系可知：

（1）车辆的后轴中心瞬时转向半径R与路径曲线的曲率半径r相等；

（2）路径曲线对x的1阶导数与车辆的航向角正切值相等。

根据以上两点可推出如下关系：

式中：y′为路径曲线方程对x的1阶导数；y″为路径曲线方程对x的2阶导数。

同时，由于车辆本身机械结构的限制，前轮等效转向角φ存在如下关系：

式中：φ为前轮最大等效转向角；φ˙为前轮最大转向角速度。

泊车过程为了避免车辆与周围其他物体发生碰撞，须计算车身4个顶点的坐标。根据图1所示的几何关系，可以得出车身各顶点坐标与后轴中心坐标的关系表达式：

2 自动平行泊车轨迹规划

在平行泊车场景中，自动驾驶车辆需要在没有人工干预的情况下完成在停车场中寻找车位并进行泊车的任务。通常泊车位前方有一条结构化道路，道路为双向单车道，如图2所示。自动驾驶车辆沿着一侧的道路边缓慢行驶边检测空闲泊车位。当行驶到泊车位附近P处时，自动驾驶车辆检测到泊车位，此时车辆需要继续向前行驶至合适的起始泊车点P。车辆到达起始泊车点P处之后，驾驶员将车辆挡位切入倒挡并进行泊车，直到车辆到达P点泊车成功。根据上述描述场景，本文中把平行泊车轨迹分为两段，第一段为前进轨迹，第二段为泊车轨迹，通过选取合适的起始泊车点生成平行泊车轨迹。

图2 平行泊车场景

2.1 泊车路径设计

如图3所示，以泊车位中心为坐标原点，建立泊车路径规划直角坐标系。其中D为道路宽度，W为泊车位宽度，L为泊车位长度。

图3 泊车路径设计示意图

2.1.1 第一段泊车路径设计

泊车路径的第一段路径曲线要求是一段能连接P点位姿（x，y，θ，φ）和P点位姿（x，y，θ，φ）的曲线，并且车辆在沿第一段路径曲线行驶时，其后轴中心坐标（x，y）、航向角θ和前轮等效转角φ都应连续变化。考虑到这些约束，本文中采用五次多项式来生成第一段路径曲线。

本文中采用的是五次多项式曲线，故需要6个约束方程，联立求解式（10）、式（11）、式（13）、式（14）、式（16）和式（17）即可得五次多项式系数a～a。

2.1.2 第二段泊车路径设计

为了尽量减小平行泊车时车辆对泊车位大小的需求，本文中将第二段路径设计成两段曲线，如图4所示。曲线1是一段五次多项式曲线，其作用是连接P点位姿（x，y，θ，φ）和P点的位姿（x，y，θ，φ）。曲线2是一段半径大小等于车辆最小转弯半径R的圆弧。

图4 第二段泊车路径

图5 第二段泊车路径中圆弧段路径

由于曲线1是五次多项式曲线，令曲线1的表达式为

式中k～k为表达式系数。

采用第一段路径曲线计算方法，将P点和P的位姿分别代入之后即可确定系数k～k，故第二段路径曲线为

2.2 最佳泊车起始点的选取

2.2.1 起始泊车点的采样

在平行泊车中车辆在泊车起始点时应尽量与泊车位平行，同时车辆转向角为零。所以在本文中起始泊车点的航向角θ=0，转向角φ=0。故只需对在泊车起始区域对车辆后轴中心坐标（x，y）进行采样。如图6所示，在车辆前进方向上确定一块矩形区域作为泊车起始点采样区域，其可以表示为

图6 泊车起始区域

式中：x为左边界横坐标值；x为右边界横坐标值；y为下边界纵坐标值；y为上边界纵坐标值。

矩形区域边界计算方法为

在矩形区域内以左下角（x，y）为起点，横向间距为Δx，纵向间距为Δy均匀取点。以此每个点P都可以确定一个位姿（x，y，0，0），从而获得两段路径曲线。

2.2.2 路径约束分析

对泊车的路径曲线需要满足两个约束条件，一是路径曲线的曲率半径r不能小于车辆最小转弯半径R，二是车辆沿着路径曲线行驶时不能与道路边界以及车位边界发生碰撞。

图7 路径碰撞

2.2.3 多目标评价函数设计

针对生成路径曲线满足路径约束的采样点，需要建立一个评价函数来评价每条路径的优劣，从中选取路径曲线最优的采样点作为最佳泊车起始点。首先，曲线应尽量的平滑，曲线越平滑路径跟踪越容易，车辆的平顺性越好；其次，泊车路径曲线长度还应该尽可能短，较短的泊车路径曲线能缩短泊车时间；最后，整个泊车过程所占用空间大小应该尽可能的小，并且泊车过程应该尽量少占对向车道。

路径曲线平滑程度可以用路径曲线的最小曲率表示，最小曲率越小路径曲线的平滑程度越好。根据路径曲线方程，可以得到路径曲线曲率计算公式，离散路径曲线计算该段路径的最小曲率，通过比较获得整个泊车路径曲线的最小曲率：

式中：s为第i个采样点的第一段路径曲线弧长；s为第i个采样点的第二段路径曲线的弧长。

从而，第i个采样点泊车路径曲线的弧长为

泊车空间占有对向车道的程度可以用起始泊车点的纵坐标表示，起始泊车点的纵坐标越小，对向车道的占用越少。同时由于目标评价函数中考虑了泊车曲线路径长度这一指标，所以起始泊车点的纵坐标越小，泊车空间也越小。

结合上述分析，对于一个生成路径曲线满足路径约束的采样点，可以采用以下评价函数计算其代价值：

式中w、w、w为各权重系数，本文中采取的权重值为w=40，w=3，w=1。

比较各采样点生成的泊车路径曲线代价值，代价值最低的采样点即为最佳泊车起始点。

2.3 速度规划

本文中泊车路径分为两段，故需要对两段路径分别进行速度规划，但由于对两段路径进行速度规划的方法相同，所以本小节以第一段路径为例来讨论泊车路径的速度规划。

自动驾驶车辆在泊车时是一个低速的路径跟踪过程，在进行速度规划时，需要限制泊车过程中的最大速度，本文中限定泊车过程中的最大速度v=5 km/h。同时泊车轨迹需要满足式（4），即泊车过程中前轮转向角速度要小于车辆允许的最大转向角速度φ˙。

如图8所示，针对一条已知路径，可以计算出其路径长度S，当自动驾驶车辆以最大速度跟随这段路径时，通过这段路径的时间t=S/v。假定S-t曲线符合三次多项式：

图8 速度规划

将式（34）代入式（32）和式（33）中即可确定一条路径长度-时间曲线，并通过计算泊车过程中速度和转向角速度是否满足约束条件来修正S-t曲线，若通过路径的时间为t时生成的曲线不满足约束，则以Δt（Δt＞0）为步长，依次向前搜索，直到寻找到满足约束的S-t，通过对曲线求导获得速度-时间曲线。

3 仿真分析

为了验证平行泊车轨迹规划方法的可行性，利用Python脚本语言编写平行泊车轨迹规划模块进行仿真分析。同时为保证仿真的真实性，在仿真过程中使用的参数与实车试验参数一致，如表2所示。

表2 车辆以及环境参数

通过与双圆弧式平行泊车路径对比，分析本文中规划的曲率连续性，结果如图9所示。从图9中可以看出，双圆弧式平行泊车路径的曲率在双圆弧切点处会发生曲率突变，而本文中规划的两条路径曲线曲率都是连续变化，同时在第一段路径的结束点和第二段路径的起始点曲率值相等，这意味着车辆在前进和后退切换点处不需要原地调整转向角。同时，规划出的路径曲率半径最小值均大于车辆的最小转弯半径，满足车辆运动学要求。

图9 与双圆弧平行泊车路径规划对比

通过设计不同的规划起点位姿来验证方法不同的泊车起始位姿的适应能力，如表3所示。经过测试，通过Python脚本语言编写的平行泊车轨迹规划模块规划一次的时间约1.19 s。

表3 规划起点的位姿

图10～图12所示为3种工况的仿真结果，可以看出在3种工况下轨迹规划模块都能规划出两段满足约束轨迹，并且能适应各种起始位姿。

对比图10（a）、图11（a）、图12（a）可以发现，当规划起点的位姿变化在第一段轨迹曲线的调整范围内（工况②、③）时，规划出的第二段轨迹基本一致，但当规划起点位姿变化较大（工况①）且超出第一段轨迹曲线的调整范围时，选择的二段路径起点（即泊车起始点）生成的泊车路径长度和泊车空间的需求就会明显增大。该方法可以尽量地满足较短的泊车路径和较小的泊车空间需求。

图10 工况①仿真结果

图11 工况②仿真结果

图12 工况③仿真结果

同样，观察3种工况下的速度变化图和转向角速度变化图可以知道，3种工况下速度曲线变化平滑，转向角速度满足要求。

4 实车试验

试验车辆具有线控底盘控制系统，能够实现对转向盘转角、油门、制动踏板和挡位的控制，可直接通过CAN通信对车辆进行控制。同时车辆配备有激光雷达、毫米波雷达、控制器、GPS、IMU等设备，因此不需要再对车辆进行相应的改装。整车结构参数在表2中已经列出。试验平台软件架构如图13所示，底层驱动程序采用LabVIEW编写，通过NI控制器经过CAN信号直接与试验车辆进行通信。上层程序基于ROS平台建立轨迹规划和路径跟踪节点，将车辆参数和环境信息传入轨迹规划节点规划出轨迹，然后将轨迹输入至路径跟踪节点输出车辆控制信号。ROS平台部署在上位机上通过UDP与Labview驱动程序通信。其中，路径跟踪模块中横向控制采用单点预瞄控制，纵向控制采用PID进行控制。

图13 试验平台软件架构

车辆的试验过程如图14所示，其中（a）为车辆的初始位置，（b）为车辆第一段轨迹结束的位置，（c）为车辆入库的位置，（d）为车辆泊车结束的位置。试验过程中实车轨迹与规划轨迹如图15所示，其中红色为实车轨迹。从图中可以看出试验效果良好，在不同的起始位姿都能规划出一条满足车辆行驶的无碰撞路径，并且最后都成功泊车。但是由于跟踪误差的存在，泊车结束时车辆的航向角还是存在一定的偏差。同时由于泊车的两段轨迹是通过一次规划出来的，所以在试验中发现车辆在跟踪第一段轨迹时的误差会影响第二段轨迹的跟踪。另外，在试验过程中还出现了车辆无法准确在轨迹切换点停车问题，从图中可以看出车辆跟踪第一段轨迹时每次停车点都超出第一段轨迹。

图14 试验过程

图15 实车轨迹与规划轨迹对比

5 结论

（1）针对自动驾驶车辆平行泊车轨迹规划问题，本文中将轨迹规划解耦成路径规划和速度规划。规划的路径在保证平行泊车路径的曲率连续性的同时利用圆弧曲线降低了泊车时对最小泊车位大小的要求，并且通过对泊车起始区域采样和引入多目标评价函数实现了车辆任意起始位姿的平行泊车路径规划。在路径规划的基础上，通过对时间采样，选取时间最短、满足约束的S-t曲线生成平行泊车轨迹。

（2）设计3种不同的起始位姿对所提出的轨迹规划方法进行仿真验证，结果表明：本文提出的方法在不同的起始位姿下都能求解出满足汽车运动学约束、碰撞约束和计算实时性等要求的轨迹信息，具有较好的场景适应能力。

（3）搭建实车试验平台进行实车试验，试验结果表明：本文提出的方法能为自动驾驶车辆提供有效的平行泊车参考轨迹，指导自动驾驶车辆进行自主平行泊车。