自主泊车路径规划一致性方法 | 厚势汽车

厚势按：为满足自主泊车过程中针对不同角度库位的泊车需求，以凸优化理论为核心，统一车辆运动学约束、车辆执行机构物理约束及环境约束，构建自主泊车路径规划的最优目标函数，采用内点法进行求解，规划出满足车辆运动学的泊车路径。

仿真结果表明，与现有规划方法相比，一致性自主泊车路径规划可有效提高规划算法对环境的适应性。同时，提出的算法框架具有较强的通用性与扩展性，可对能以等式与不等式形式统一描述的各种场景进行路径求解。

本文来自 2018 年 8 月 7 日出版的《 汽车技术 》，作者是同济大学汽车学院余卓平教授、熊璐教授和夏浪。

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 问题背景

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



式中，g_i(x) 为等式约束；h_j(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 相等，离散后车辆的前后时刻状态变换满足：



2.3 不等式约束

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

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

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

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



由于 O_M、E_t 的非空凸集性质，原问题满足 Slater条件，即∃x ∈ E_t，y ∈ O_m，Ax=b，C_m*y=d_m，满足强对偶性要求 [11-12]，且对偶问题的最优解即原问题的最优解。上述不等式的对偶问题可以表示为：

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



式中，e_1、e_3 分别为车辆后轴到前、后端的距离；e_2 为车辆后轴中心至左、右端的距离。

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

2.4 优化目标

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

式中，u_t 表示每一个迭代步骤的控制量（前轮转角 δ(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 Juno Lab Plug-in: Julia-client/uber-juno 对提出的方法进行仿真验证，程序语言为 Julia，处理器为 Intel Core i5-7200U，最高睿频 3.1 GHz，内存 8 GB，DDR4，2133 MHz。数值优化求解器为开源优化库 IPOPT v3.12.9。仿真车辆模型采用某小型纯电动轿车的参数，库位大小参照国家停车场规划设计规则，其余具体参数见表 1。

表 1  泊车场景相关参数

3.2 仿真结果与讨论

仿真泊车场景模拟实际生活中各种角度的库位，与道路夹角分别为 0°、30°、45°、60°、90°，仿真结果如图 3 ~ 图 7 所示。其中，阴影矩形表示障碍物，在实际工况中可能表示非目标库位、对向车道等。



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



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

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

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

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

从图 3 ~ 图 7可以看出，针对各种角度的库位，本文提出的一致性算法均可成功规划出车辆起点至目标库位的可行路径，各车辆状态与运用学参数均满足车辆本身物理要求，满足了自主泊车「自主」的要求。

3.2.1 实时性

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

表 2  不同场景规划用时

由表 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 可以看出，车辆泊车过程中，相比于调整自身位置，车辆希望更快向目标库位靠近，遇到障碍物后再进行转向操作。这与优化过程的初值密切相关，特别是车辆坐标初值。依上文所述，泊车过程初值选取的是不存在障碍物情况下的可行解，由于该解无视环境中障碍物，在当前迭代优化目标（见前文式 9）的驱动下，轨迹希望更快靠近目标库位。

由此，该初值所得出的最终解通常会有一段路径离障碍物很近。实际工况中，驾驶员可能希望车辆距离障碍物较远，然而从数值优化本身的角度看，所求出的路径满足不等式约束，且满足优化目标的最小值，便是可行解。

4. 结束语

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

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

参考文献

[1] Dubins L E. On Curves of Minimal Length with a Constraint on Average Curvature, and with Prescribed Initial and Terminal Positions and Tangents[J]. American Journal of Mathematics, 1957, 79(3): 497-516

[2] Reeds J A, Shepp L A. Optimal Paths for a Car that Goes Both Forwards and Backwards[J]. Pacific Journal of Mathematics, 1990, 145(2): 367-393

[3] Gómez- Bravo F, Cuesta F, Ollero A, et al. Continuous Curvature Path Generation Based on β- spline Curves for Parking Maneuvers[J]. Robotics & Autonomous Systems, 2008, 56(4): 360-372

[4] Liang T C, Liu J S, Hung G T, et al. Practical and Flexible Path Planning for Car- Like Mobile Robot using Maximal Curvature Cubic Spiral[J]. Robotics & Autonomous Systems, 2005, 52(4): 312-335

[5] Marcus L. Path Planning for Autonomous Vehicles Using Clothoid Based Smoothing of A* Generated Paths and Optimal Control[D]. Stockholm, Sweden: KTH Royal Institute of Technology. 2017

[6] Schwesinger U, Bürki M, Timpner J, et al. Automated Valet Parking and Charging for E- Mobility[C]// Intelligent Vehicles Symposium. IEEE, 2016: 157-164

[7] Kwon H, Chung W. Performance Analysis of Path Planners for Car-Like Vehicles Toward Automatic Parking Control[J]. Intelligent Service Robotics, 2014, 7(1): 15-23

[8] Li B, Zhang Y, Shao Z, et al. Simultaneous Versus Joint Computing: A Case Study of Multi-Vehicle Parking Motion Planning[J]. Journal of Computational Science, 2017: 30-40

[9] Zips P, Böck M, Kugi A. Optimization Based Path Planning for Car Parking in Narrow Environments[J]. Robotics and Autonomous Systems, 2016, 79: 1-11

[10] Gao H, Liu Y, Zhang X, et al. Path Planning Algorithm Regarding Rapid Parking Based on Static Optimization[C]// IEEE, International Conference on Cloud Computing and Intelligence Systems. IEEE, 2015: 672-678

[11] Patel R B, Goulart P J. Trajectory Generation for Aircraft Avoidance Maneuvers Using Online Optimization[J]. Journal of Guidance Control & Dynamics, 2011, 34(1): 218-230

[12] 赵千川, 王梦迪. 凸优化理论[M]. 北京：清华大学出版社，2015

[13] Boyd S, Vandenberghe L. Convex Optimization[M]. Cambridge University Press, 2004.

[14] Survey of Optimisation Software [EB/OL]. (2015- 08- 13)

http://ascend4. org/Survey_of_optimisation_software

编辑整理：厚势分析师拉里佩

转载请注明来自厚势和厚势公号：iHoushi

-END-

文章精选

企业家

马斯克和贾跃亭 ｜福特CEO下台｜正道汽车仰融

任正非裁员｜电池大牛凯尔提离开特斯拉

智能驾驶

BBC自动驾驶纪录片

为什么说百度阿波罗画虎不成反类犬

车载雷达通信系统综述

智能时代测绘与位置服务领域的挑战与机遇

新能源汽车

全国50个新能源汽车项目大盘点

锂电池发展趋势｜中国汽车产业电动化进程

苹果收购特斯拉？｜丰田和特斯拉决裂

新能源乘用车的补贴综述及展望 

新能源汽车驱动电机发展现状及趋势分析

项目和评论

以色列最强10家自动驾驶创业公司

 37个汽车分时项目盘点｜百度投资蔚来汽车

马化腾或为共享单车最大赢家｜汽车产业3大趋势

Momenta获$4000万B轮

百度系自动驾驶初创公司 Pony.ai 的突围之路

这些大神从Google出走，创办了五家（命运各异的）无人车公司

无需基础知识，理解自动驾驶高精度行车定位技术

厚

势

汽

车

为您对接资本和产业

新能源汽车 自动驾驶 车联网

联系邮箱

sasa@ihoushi.com

点击阅读原文，查看文章「自动驾驶陷入低谷期，数百亿美元投资将打水漂」