狭小车位平行泊车路径规划方法研究*

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

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

前言

随着社会经济的发展，汽车保有量不断增加，停车位逐渐紧张的同时，泊车位的空间愈加狭窄，对于驾驶经验不足的驾驶员将是一个巨大的挑战。为解决这一难题，能够在狭窄车位下完成泊车任务的自动泊车技术逐渐成为研究重点。

目前已有的自动泊车算法主要分为4 种：一是基于几何的方法，主要包括曲线组合和插值拟合方法，如文献［1］中设计“圆弧-线段-圆弧”的曲线组合路径，通过双步库内调整策略解决狭窄泊车问题，文献［2］中综合考虑汽车运动学约束、泊车碰撞约束等，提出基于回旋曲线的平行泊车路径规划，避免了原地转向现象，该类方法运算耗时短，规划路径基本可控，便于工程的应用，但泊车路径存在突变点，曲率并不连续，且对起始位姿要求比较高；二是基于数值优化的方法，通过求解泊车路径约束优化问题获得最优泊车路径，如文献［3］中建立以泊车时间最短作为优化目标的优化框架，可在狭窄车位的情况规划出泊车路径，但未考虑路径约束，文献［6］中综合考虑避障和车辆运动学约束，建立最优化多段式泊车路径规划，解决了泊车过程中停车原地转向的问题，该类算法求解能力强，但计算量过大、耗时长难以满足工程的需求；三是基于采样的方法，在构型空间中生成样本点，并寻找满足任务需求的样本点序列作为规划结果，如文献［8］中采用RRT*算法进行路径搜索，找到可行路径后，通过Dijkstra 算法对路径的长度和前后换向的次数进行优化，但并不能保证规划路径曲率的连续性，文献［10］中结合RRT*算法与非线性优化算法，实现曲率连续的泊车轨迹，该类算法对泊车场景的适应性强；四是基于学习的方法，即设计从环境到控制的策略神经网络，利用行为克隆或强化学习对策略网络进行训练，如文献［13］中利用非线性规划获得演示数据，通过模仿学习初始化策略神经网络（PNN），经过强化学习对策略网络进行改进，优点是通过融合消除各模块间误差、泛化性好，但学习算法的训练难以收敛，且应用受限于仿真环境与实际的差异。

据此，本文中详细分析了狭小车位自主泊车路径规划的问题，结合几何法设计简单耗时短和数值优化计算能力强的优点，提出计算能力强、适用范围广、曲率连续的平行泊车路径规划方法。首先建立了平行泊车路径规划框架，将平行泊车问题分解为调整规划和入库规划问题，然后基于回旋-圆弧组合的约束最优化问题设计调整模块，求解最佳出库点，随后基于五次多项式和圆弧-直线-回旋组合的方式设计起始点到出库点的入库最优化问题，求解最佳泊车点。最后，通过仿真和实车测试验证算法的适应性和有效性。

1 车辆运动学模型

由于自动泊车过程中，车辆速度较低且无侧向偏移，可假定车辆的转弯过程作圆周运动，将四轮阿克曼转向模型简化为两轮自行车模型，同时汽车外轮廓直接影响了泊车的成功率。为了避免与环境发生碰撞，同时给予富裕的泊车空间，以车辆最大长度和最大宽度包裹的矩形框来描述汽车外轮廓。建立的车辆运动学模型如图1所示。

图1 汽车运动学模型

本文中仿真和实车试验参数如表1 所示，选择车辆后轴中心作为车辆参考点，根据图1 所示的几何关系，可计算得出汽车后轴中心点最小转弯半径为6.5 m。

表1 汽车主要参数

此外，车辆外轮廓的各坐标点可表示为

式中、为参考点，即后轴中心的坐标，其余参数见图1和表1。

2 平行泊车路径规划

本文中所建立的平行泊车路径规划框架包括入库规划模块和调整规划模块。调整规划模块根据车位大小自行计算调整次数，入库规划模块根据出库关键点和自车位置计算最佳泊车点，具体规划流程如图2所示。首先，建立以为原点车位坐标系，将泊车过程逆向化，以泊车终点作为起点，以最小转弯半径转弯出库直至车辆右顶点到达车位右边界，计算此时的车辆状态和车辆右顶点坐标，如图3 所示。若点纵坐标小于车位宽度，则表示车位长度无法满足一次泊入，执行调整规划模块，计算点新坐标，直至满足出库要求或调整次数达到限值，则结束调整规划模块。然后，以出库点作为新的泊车目标点，将泊车问题简化为一次入库问题，执行入库规划模块，寻找最佳泊车点，通过该泊车点若能生成一条起点到终点的无碰撞的满足曲率要求的路径，则入库规划结束。最后，将入库路径和调整路径结合得到一条完整的平行泊车路径。

图2 泊车规划流程图

图3 最小转弯半径出库示意图

2.1 调整规划

现有研究普遍采用圆弧-直线组合通过让汽车产生侧向位移来不断逼近目标点，但是该几何法曲率不连续，车轮须频繁原地转向，对转向机构不利，其次通过双圆弧来实现，想要获得较大的侧向位移且每次车姿都要平行车位，对纵向空间和最小转弯半径的要求较高。故本文中设计了一种通过库内调整车姿以快速达到目标位置的调整规划方法，如图4 所示。该调整规划路段由后退路径和前进路径组成，后退路径为了调整车辆位姿以创造调整空间使前进路径更容易满足出库要求。前进路径是为了达到车位右边界时尽可能满足出库要求。

图4 库内调整规划

回旋线大多作为过渡曲线，其曲率半径随曲线上某一点至该曲线起点距离成反比，或说，其曲率与距离成正比，即=/，其中为曲率对距离的变化率。由于泊车环境比较狭窄，往往需要快速转动转向盘，所以本文采用转向盘所需要的转动时间确定回旋曲线的长度，转向盘转动时间和回旋曲线长度分别为

式中：、为始末曲率；为转向盘的转动角速度；为转向传动比。

根据曲率变化和曲线长度计算曲率变化率，得到初始方位角为零的回旋曲线段终点曲率的点的坐标和夹角为

式中：为总弧长；为曲率变化率；为终点曲率；为幂级数展开个数，展开数越高，求解精度越高，但相应计算量也越大，本文选取=15。当、和数值选取不同正负号时，可得到在不同象限的回旋曲线。

（1）回旋曲线的构建方法

作为通用的方法，假设该回旋曲线段要连接的两段圆弧，且已知第1 个圆弧终点的位置、方位角和半径以及第2 个圆弧的半径。建立初始曲率和方位角为0的回旋曲线坐标系，如图5所示。

图5 回旋曲线坐标系xoy与车位坐标系XOY

图中和分别为点在泊车位坐标系和回旋曲线坐标系下的车辆航向角，坐标系和的转换关系为

构建步骤：

①计算回旋曲线坐标系原点在泊车位坐标系下的位姿；根据曲率变化率和点的曲率计算弧长S= k，由式（8）～式（10）可计算点在回旋曲线坐标系下的位姿（，，），随后将点的坐标（泊车位坐标系）代入式（11）～式（13）反求点在泊车位坐标系的位姿（X，Y，）。

②计算点车位坐标系下的位姿；根据车辆运动方向处的曲率，由式（6）和式（7）计算的弧长S，计算弧长S= S+ S由式（8）～式（10）计算点在回旋曲线坐标系下的位姿（，，），将它们代入式（11）～式（13），求得点的在泊车位坐标系的位姿。

当回旋曲线段一端的曲率为0 时，则只须进行步骤①即可，此时即为要求的回旋曲线段。需要说明的是本节和后面的调整规划和入库归划关于回旋曲线的构建，只须确定连接点的位姿即达到目的。只当通过调整规划和入库规划，得到最佳泊车路径后，要通过实车试验验证时，才需要各回旋曲线段的具体轨迹，即离散点的坐标，届时由式（8）～式（10）计算即可。

（2）圆弧的构建方法

圆弧线是主要路径曲线，由车辆以某一旋转中心、固定转向半径转过一定角度所生成的路径。设车辆参考点走过一段圆弧，不论前进或后退，若定义从泊车位坐标系横坐标向量旋转至与参考点的速度向量方向一致时所转过的角度为方位角（逆时针为正，顺时针为负），则圆弧上任一点，包括两个端点的坐标、圆心坐标和方位角存在如下关系：

式中：X、Y和X、Y分别为圆弧上某点和圆心在泊车位坐标系中的坐标；为该点的方位角；为圆弧半径，当右手4 个手指顺着圆弧运动方向，若拇指由纸面指向外（或方位角增加，即Δ0）时取正值，若拇指由纸面指向内（或方位角减小，即Δ0）时取负值。因此，构建圆弧段的具体步骤为：先将圆弧起点的坐标作为X、Y连同方位角代入式（14），求得圆心坐标X、Y，再将它们和圆弧终点的方位角（注意此时已有增量Δ）同样代入式（14），求得的X、Y即为圆弧终点的坐标，而其方位角自然是Δ

2.1.1 调整路径设计

调整路径如图4 所示。首先调整后退路径，以车辆达到车位右边界的极限位置作为后退路径起点。和为回旋曲线段，和为圆弧段。给定已知条件为起点的位姿与曲率、点的曲率、圆弧的圆心角Δ、点的曲率和圆弧的圆心角Δ。遵循曲率连续的原则，即可按照前面“（1）回旋曲线的构建方法”和“（2）圆弧的构建方法”完成后退路径的构建。

然后调整前进路径，为回旋曲线段，为圆弧段。由于前进段目的是尽可能驶出车位，故以最小转弯半径作为圆弧段半径行驶最佳。这意味着原须给定的点的曲率已确定为与车辆最小转弯半径相对应的最大曲率。因此，可采用同样方法构建前进路径的曲线。至于圆弧段的终点，由于在出库过程中，车辆外轮廓点首先与车位边界发生碰撞，故建立以圆心、||为半径的圆弧方程（X-X）+（Y-Y）=，计算当X到车位右边界时的Y，即为终点的坐标。

2.1.2 调整路径优化设计

为使车辆通过后退和前进调整更快满足出库要求，则每次调整后的Y不断接近车位宽，故本文以、Δ、、Δ作为待定参数，将最大化Y作为优化目标，则上述平行泊车进入段路径规划问题可转化为如下约束优化问题：

式中约束、、、为对车辆轮廓的约束。库内调整过程中，车辆的障碍物为车位边界，故车辆轮廓须满足的约束为：、点不与停车位右边界碰撞，即X，X≤X-；、点不与停车位左边界碰撞，即X，X≥X+；、点不与停车位下边界碰撞，即Y，Y≥，其中，为车辆与泊车位壁面的最小距离,取0.1 m。而曲率和弧度的约束为：、小于最大曲率，圆弧圆心角小于π/2。

2.2 入库规划

现有研究的平行泊车入库规划普遍没有考虑当车辆位于车位后方时路径是否有可行解的情况，而且通过单曲线组合方式所确定的可泊车区域往往把航向角约束为零，并不能找到一个最佳的泊车点，因此本文设计了一个两段式入库规划，如图6 所示，第1 段为前进路径，第2 段为后退路径，通过建立非线性规划模型确定合适的泊车点生成入库规划路径。

图6 平行泊车入库规划

2.2.1 第1段入库路径设计

第1 段入库路径是生成泊车最佳点到调整规划出库点的曲线，本节采用结合回旋-圆弧-直线组合的方式，具体以出库点作为起点逆向推导至泊车点。考虑到为使车辆尽可能接近出库点，调整规划的前进路径的圆弧段选用最小转弯半径，该路径未必是最优路径，故入库规划以调整规划的退后路径与前进路径的衔接点作为起点，并重新编号为。如图6 所示，第1 段路径中，、、和为回旋曲线段，和为圆弧段，而为直线段。给定已知条件为：起点的位姿与曲率、点的曲率、圆弧段的圆心角∆、直线长度为、点的曲率、圆弧段的圆心角∆和点的曲率为0。遵循曲率连续的原则，即可按照同样的方法，构建第1段入库路径。

2.2.2 第2段入库路径设计

第2 段入库路径，即后退段主要是为了指引车辆行驶到泊车点，为了路径满足连续变化，同时增加算法的适应性，本文采用五次多项式设计第2 段入库路径。五次多项式的公式可以表示为

假设起始点和泊车点的位姿分别为（，，）和（，，），可得：

根据式（20）～式（25）建立6 个等式方程，可求解一条唯一的五次多项式曲线，该曲线即为规划的路径。

2.2.3 最佳泊车起点选取

同时连接两段路径，该点的选择直接影响了整段入库规划的效率，本文中将、、、、Δ作为待优化参数，将最小化两段路径总长度作为优化目标，则上述平行泊车入库路径规划问题可转化为如下约束最优化问题：

实际上，车辆的泊车区除停车位之外，还有一个上边界，如图6 所示。其与停车位下边界的距离，称为泊车区总宽度，以‘’表示，8.5 m。

式（26）中的约束、、、为对路径中车辆轮廓与泊车区边界的碰撞约束，即车辆轮廓与泊车区边界应保持最小距离=0.1 m。约束须满足不与泊车区上边界碰撞，即Y≤-；约束须满足不与停车位右边界碰撞，即X≤X-；约束须满足不与停车位下边界碰撞，即Y≥；约束须满足不与停车位左边界和泊车区上边界碰撞，即X≥和Y≤-；约束须满足第1 段几何入库规划的可能性，即X+(+)cos+sin/2 ≥X

3 仿真分析

本文在Matlab环境下对路径规划算法进行仿真验证，首先根据最小转弯半径计算该车型一次泊入所需要的最小车位长度，建立以下等式：

式中：X，Y和X，Y以及X，Y分别为车辆回转中心和后轴中心以及点的坐标；X，Y为点初始坐标。

先由第1 和第2 式分别求出回转中心的坐标X，Y和点的初始坐标X，Y；再将其作为X，Y代入第3 式求出R；最后再利用第3 式，求当Y等于泊车位宽度W时的X，即为车辆可一次泊入的泊车位最小长度L。W=2.5 m 时，实际求得L=7.225 m。

以最小车位逐渐缩小长度并以不同初始位姿建立9种仿真场景，详细信息如表2所示。

表2 工况详细信息

采用Matlab 工具箱中fmincon 求解器对上述规划进行求解，并与“圆弧-直线”库内调整规划进行对比，对本文调整规划方法的调整次数进行分析，同时采用不同初始角和泊车位，对算法适应性进行分 析，结果如图7所示。

图7 仿真结果图

由Case2 可得，车位长度大于L时，调整路段均不规划路径，由入库规划模块规划出1 次入库路径；对于Case5，车位长度略小于L时，基于圆弧-直线规划方法的调整次数为5 次，远大于本文方法的1 次；而对于长度再次缩小1 m 的Case8，圆弧-直线方法已无法规划出满足最大调整次数的路径，而本文方法调整次数仅增加了1 次，能够充分利用库内空间完成规划任务。

由Case1～Case3 可以看出，车辆位于平行车位左侧，但以-5°、0°、5°不同方位角作为起始状态时，入库规划的最佳泊车点位置也发生变化，这是因为对五次多项式的曲率进行了约束，进而寻找满足曲率约束、路径最短的泊车点。由Case4～Case6 和Case7～Case9 可进一步看出，以不同初始角面对相同泊车位时，库内调整路径相同，表明调整路段仅与车位大小有关，将起始位姿和泊车位对泊车效果的影响分开，能有效简化入库规划算法。

最后从整体仿真结果来看，本方法可以以较快效率完成库内调整，同时具备较强的适应性。

4 实车试验

为了验证规划路径的可行性，基于实车试验平台对其验证。实车试验平台如图8 所示，其主要参数如表1所示。

图8 试验平台车

实车试验平台的软件构架如图9 所示，采用分层结构。底层驱动程序通过Labview 编写，部署在NI 控制器中，通过CAN 报文控制车辆；上层路径规划和轨迹跟踪模块均在Simulink 中编写，采用ROS平台将自车信息和环境信息通过UDP 传入Simulink处理，再通过UDP 将控制信号传入到底层驱动程序进而控制车辆。为了保证实时接受信号和发送控制信号，Simulink 采用并行运算，以100 Hz 频率接受UDP 的信息，以50 Hz 频率运行路径规划和跟踪模块。其中本文所采用路径跟踪为曲率前馈和位置误差反馈控制。

图9 平台构架图

因为本文规划包含圆弧曲线，为了给控制模块一定控制余量，将规划所采用的最小转弯半径加大至7 m，根据式（27）算得所需L为7.79 m，故分别对车位大小8、7和6 m的情况进行试验，其中起始位置位于车位同侧，但位姿不固定。试验结果如图10～图12 所示，其中蓝色表示规划路径，红色表示实车轨迹。

图10 车位6 m泊车试验

图11 车位7 m泊车试验

图12 车位8 m泊车试验

由试验结果可以看出，在车位大小为8、7和6 m的情况下，均能规划出满足约束的路径，而且能够根据车位变化规划出调整次数分别为0 次、1 次、2 次，由于控制累计误差的存在和转向角速度的限制导致跟踪效果不是很好，但是均能完成泊车任务，说明规划出的路径具备有效性。

5 结论

本文中提出一种基于曲线组合和数值优化相结合的方法研究平行泊车路径规划，有效解决泊车位愈加狭窄的泊车问题，首先建立平行泊车路径规划框架，将泊车过程逆向化并划分为调整规划路径和入库规划路径，以基于圆弧-回旋曲线的约束最优化问题设计调整路径，引导车辆库内调整位姿寻找出库关键点，以基于五次多项式和圆弧-回旋曲线-直线的组合约束优化设计入库路径，引导车辆寻找最佳泊车点；然后通过Matlab 仿真证明了算法的适应性，可在不同车位大小和起始位姿下规划出路径；最后的实车试验结果表明，算法所规划出的路径满足车辆行驶要求，具备有效性。在实车试验中发现，车辆跟踪过程中的累计误差对泊车效果有较大影响，因此后续将对泊车过程中的规划路径更新问题进行进一步研究。