自主泊车路径规划一致性方法*

余卓平 夏浪 熊璐

（同济大学，上海 201804）

主题词：自主泊车 一致性路径规划 凸优化

1 前言

自主泊车路径规划作为智能汽车技术的典型代表，要求在极其狭窄的环境中找寻汽车从当前区域到指定停车位之间的可行驶路径。轮式机器人所具有的不完整约束性质，以及车辆在复杂、狭窄环境里的碰撞检测，都使自主泊车路径规划更具挑战性。

在对当前泊车环境完全理解的基础上，几何法以其简单、高效的特点，在路径规划中得到大量应用，它通过获取当前车辆、环境障碍物、目标泊车位三者的几何关系，采用直线或曲线求解车辆可行驶泊车路径。具有开创性的几何法当属Dubins曲线[1]和可倒车Dubins曲线[2]。为弥补Dubins曲线曲率不连续的缺点，更多学者采用样条曲线[3]、多项式曲线[4]以及回旋曲线[5]路径规划。然而，基于几何的方法对环境要求高，通常每一套算法仅对应一种或一类环境，在停车环境日益复杂化的今天，几何法已很难满足自主泊车的“自主”需求，以快速随机扩展树（Rapidly Random Tree，RRT）或A*为基础的各种变形搜索算法普遍用于轮式机器人的路径规划。然而，这些算法并未广泛应用于自主泊车路径规划研究。Schwesinger U等人[6]在对欧洲代客充电（Valet Charge，V-Charge）项目的调研中提出将Hybrid A*算法应用于第3阶段自主泊车路径规划，但该文仅限于思路，并未提及相关仿真及实车试验。Kwon H等人[7]提出KPP（Korea University Path Planner）算法，对标基础RRT算法，仿真结果表明，在泊车工况下，KPP算法在提供可选路径宽度、曲率连续性等方面较基础RRT算法有更佳表现。此外，一种基于数值优化求解的方法引起了关注。浙江大学的Li Bai[8]采用拉格朗日差值点的方法，将动态连续规划问题离散为非线性优化问题，以数值求解的方式迭代求解出自主泊车可行路径。然而仿真结果表明，该方法存在实时性差、优化过程初值敏感等缺点，实用效果并不理想。Zips P[9]与Gao H[10]等人分别针对泊车过程道路狭窄以及保障泊车过程安全性开展研究，核心部分同样以数值求解的方式离散车辆状态点，求解车辆路径。

针对自主泊车工况，现有研究大多针对平行库位与垂直库位，鲜见关于斜库位的泊车研究。根据我国公安部1988年颁布的《停车场规划设计规则（试行）》，标准库位包含不同角度的斜库位。本文基于数值优化求解的特点，提出一种自主泊车路径规划的一致性方法，针对不同角度的库位均可规划出满足需求的路径，环境适应性强。

2 问题描述

2.1 问题背景

路径规划本质是两点边值问题的求解。针对泊车规划的场景，统一约束方程，构造最优目标函数：

式中，gi(x)为等式约束；hj(x)为不等式约束；f(x)为优化目标。

针对一般的轨迹优化问题，优化对象包含T·K个变量，其中T为整个问题离散的状态数目，K为优化对象状态数。本文依次分析泊车问题中的等式约束、不等式约束以及优化目标，对整个泊车问题进行全面的数学表述。

2.2 等式约束

现实生活中，泊车过程车速较慢，本文忽略悬架作用，避免复杂的动力学模型，采用经典线性2自由度汽车运动学模型如图1所示。

图1 汽车运动学模型

其运动学方程为：

式中，(x(t),y(t))为车辆当前位置参考点（车辆后轴中心）P的坐标；φ(t)为车辆航向角；v(t)为车辆后轴中心线速度；δ(t)为前轮转向角；a(t)为车辆切向加速度；L为轴距。

本文将整个泊车过程离散为(N+1)个状态，假设任意两相邻状态时间间隔top相等，离散后车辆的前后时刻状态变换满足：

式中，zt∈Rnz为系统在（tt∈{t0,t1,…,tN}）时刻的所有状态变量；zs为系统初始状态；zf表示系统目标状态；nz为状态变量个数，包括x(t)、y(t)、φ(t)、v(t)；ut∈Rnu为系统在t时刻的输入变量；nu为输入变量个数，包括δ(t)、a(t)；Rnu→Rnz表示系统的状态变换；TF为整个泊车过程所需时间。

2.3 不等式约束

假设障碍物OM与车辆E均为定义在R2上的真锥（Proper Cone），即含有非空内部且有界[11]。当前环境t时刻车辆占据的空间表示为Et⊂R2，障碍物占据的空间为OM⊂R2，OM=O1∪O2∪…∪Om。t时刻车辆与障碍物不发生冲突可表示为Et∩OM=∅。通常上述问题是非凸的，因此需要对该问题进行进一步描述。

假设车辆与障碍物均为定义在R2上的多胞形（Poly⁃tope），那么，障碍物与车辆所占据的空间可表示为：

式中，A∈Rl·n；b∈Rl；Cm∈Rkm·n；dm∈Rkm；n为空间维度；l、k为组成凸集的超平面个数。

定义在R2上两凸集Et、Om间的距离可表示为dist(Et,Om)=inf{||x-y||2|x∈Et,y∈Om}，如图2所示，其中连接两凸集的虚线为欧几里得2范数（Euclidean Norm 2）最小值，即车辆与障碍物的距离。对于泊车过程的避障约束，通常设定车辆与障碍物之间的安全距离dmin，即dist(Et,Om)>dmin,∀t∈{1,2,…,N}。进一步描述为：

图2 车辆与障碍物间距离示意

由于OM、Et的非空凸集性质，原问题满足Slater条件，即∃x∈Et，y∈Om，Ax=b，Cmy=dm，满足强对偶性要求[11-12]，且对偶问题的最优解即原问题的最优解。上述不等式的对偶问题可以表示为 maxλ,μ{-bTλ-dTmμ}>dmin，ATλ+CTmμ=0，||ATλ||*≤1，λ≽0，μ≽0[13]，即：

式中，λ、μ均为拉格朗日变量；||∙||*为对偶范数。

针对障碍物，可通过获得障碍物角点坐标求取矩阵Cm、dm。针对车辆本身，当车辆在P(xt,yt)点以航向角φ行驶时，车辆4个角点坐标分别为：

式中，e1、e3分别为车辆后轴到前、后端的距离；e2为车辆后轴中心至左、右端的距离。

将式（7）代入式（4），通过变换可得：

2.4 优化目标

文献[8]针对自主泊车工况，将总时长TF设为优化目标，即整个过程用时最少、路径最短。然而最短路径在实际工况中可能并非最佳路径。泊车过程希望在获得最短路径以及最小时长的前提下，车辆控制输入应尽量小，在实际工况中表现为转向角度尽量小、加速度尽量小，因此，本文设定目标函数为：

式中，ut表示每一个迭代步骤的控制量（前轮转角δ(t)、切向加速度a(t)）；p∈R、q∈R分别为时间和控制量的权重，在实际仿真过程作为经验值进行适当选取。

2.5 内点法

内点法（Interior-Point Methods）是一类解决线性或非线性凸优化问题的算法，通过引入惩罚函数，将约束问题转换成无约束问题，随后在迭代过程中不断更新惩罚函数，将可行解“固定”在一定区域内，最终得到最优解。代表性的内点法有障碍法（Barrier Methods）和原始对偶法（Primal-Dual Methods）。无论面对线性规划（Linear Programming，LP）问题还是二次规划（Quadratic Programming，QP）问题，内点法在算法复杂度上都展示了极好的性能。

对泊车问题的统一优化框架包含上述等式约束、不等式约束以及优化目标，依据该优化框架，以及对当前环境的完整描述，运用内点法即可进行泊车路径的求解。

3 仿真验证

3.1 仿真描述

本文基于Ubuntu 16.04.4 LTS环境Atom JunoLab Plug-in:Julia-client/uber-juno对提出的方法进行仿真验证，程序语言为Julia，处理器为Intel Core i5-7200U，最高睿频3.1 GHz，内存8 GB，DDR4，2 133 MHz。数值优化求解器为开源优化库IPOPT v3.12.9。仿真车辆模型采用某小型纯电动轿车的参数，库位大小参照国家停车场规划设计规则，其余具体参数见表1。

表1 泊车场景相关参数

3.2 仿真结果与讨论

仿真泊车场景模拟实际生活中各种角度的库位，与道路夹角分别为0°、30°、45°、60°、90°，仿真结果如图3～图7所示。其中，阴影矩形表示障碍物，在实际工况中可能表示非目标库位、对向车道等。从图3～图7可以看出，针对各种角度的库位，本文提出的一致性算法均可成功规划出车辆起点至目标库位的可行路径，各车辆状态与运用学参数均满足车辆本身物理要求，满足了自主泊车“自主”的要求。

图3 0°库位角仿真场景及相应运动学参数

图4 30°库位角仿真场景及相应运动学参数

3.2.1 实时性

由于泊车工况车辆自身速度较低，且周围通常无其他行驶车辆，与无人车路径规划相比，自主泊车路径规划实时性不足通常不会带来安全隐患。然而，如果当前环境突然变化等情况发生，车辆不得不停止并等待计算出新的路径，等待时间过长势必极大影响用车体验。表2给出了5个场景的计算总用时。

由表2可知，针对不同库位角度，规划时间有些许差异，总体上，实时性在可接受范围，但并不能令人满意。本次模型的一致性，使得问题所需要的求解器强大而可行，由于工具有限，本文采取数值求解的方法。该方法将连续变量进行离散化处理，变量个数为离散点个数与系统变量个数之积。庞大的变量数量给提升算法实时性带来不小的挑战。

图5 45°库位角仿真场景及相应运动学参数

图7 90°库位角仿真场景及相应运动学参数

表2 不同场景规划用时

众所周知，对每一个优化问题来说，初值的选取极为重要。求解的过程是否收敛、求解所需的时间、求得全局解还是局部解，都与初值的选取息息相关。理想状况下，所选取的初值应满足上述所有约束，包括等式约束和不等式约束，然而由于车辆航向角的存在，适当的初值选取并不容易。本文首先假设泊车过程无任何障碍物，将线性离散起始点至目标点坐标值作为x(t)、y(t)的初值，其余变量φ(t)、v(t)、δ(t)、a(t)各值均置为零，求取该泊车场景的可行解，随后将该解所对应的所有变量作为初值，求取障碍物存在时的泊车轨迹。

然而仿真表明，随意的初值选取，使得算法的实时性并不理想。针对此问题，本文进行了进一步仿真，将上一个场景得出的解作为下一个场景的初值进行计算，如90°库位角度得出的解作为60°的初值，60°库位角度的解作为45°的初值，依次类推，90°采取最新60°库位角求得的解。通过这样初值选取，仿真时间如表3所示。

表3 改变优化初值后不同场景规划用时

从表3可以看出，重新选取初值后，计算速度得到了显著提高。这给实际泊车提供了一个新思路：为了提升泊车规划实时性，可预先存取一定数量的解在规划器中，针对实际具体泊车场景，提取与当前场景相近的离线场景所对应的解作为初值，在提高算法实时性的同时也极大地保障了问题的可收敛性。

3.2.2 车辆行为

由图4～图6可以看出，车辆泊车过程中，相比于调整自身位置，车辆希望更快向目标库位靠近，遇到障碍物后再进行转向操作。这与优化过程的初值密切相关，特别是车辆坐标初值。依上文所述，泊车过程初值选取的是不存在障碍物情况下的可行解，由于该解无视环境中障碍物，在当前迭代优化目标的驱动下，轨迹希望更快靠近目标库位。由此，该初值所得出的最终解通常会有一段路径离障碍物很近。实际工况中，驾驶员可能希望车辆距离障碍物较远，然而从数值优化本身的角度看，所求出的路径满足不等式约束，且满足优化目标的最小值，便是可行解。

4 结束语

本文验证了自主泊车路径规划一致性方法在仿真环境中的可行性，尚未进行实车试验。本算法要求车辆当前信息与环境信息准确可靠，然而实际情况下，传感器输入信息极有可能前、后时刻不一致。此时须在当前基础上进一步提升算法实时性，根据更新后的输入信息实时求取路径。

此外，非线性数值优化求解器数量多达数十种[14]，本文仅讨论Ipopt使用情况。针对自主泊车路径规划问题，是否有其他求解器在求解成功率、实时性等方面有更佳表现，值得研究。