一种任意起始位姿的连续曲率泊车路径在线规划方法

摘要

针对目前泊车路径规划算法对起始位姿要求严格，且难以兼顾最终位姿精度、路径质量及计算效率等问题，提出了一种任意起始位姿的连续曲率路径在线规划方法。该方法将整个泊车路径规划分为库位内调整与入库过程两部分。库位内调整采用以最终位姿高精度与调整次数最少为目标函数的最优化方法进行逐段规划；入库过程采用连续曲率曲线组用于混合A*算法的状态节点扩展以直接生成无需后处理的可执行路径。设计考虑路径的曲率变化与方向改变次数的估价函数；采用由路径几何形状特征点构建特征多边形的碰撞检测方法以提高计算效率。离线仿真与实车试验结果验证了该方法的有效性。

关键词

自动泊车; 路径规划; 连续曲率路径; 混合A*算法

汽车保有量逐年增加造成城市泊车空间日益狭窄，可帮助驾驶员安全高效泊车的自动泊车技术应运而生^［1］。自动泊车路径规划作为其中的一项关键技术，用于规划一条连接车辆当前位姿与期望目标位姿的可执行路径，使得车辆在跟踪控制过程中无碰撞且避免原地转向^［2］。尽管目前有诸多路径规划相关研究，但对于泊车场景，鲜有兼顾任意泊车起始位姿、路径质量与计算效率的路径规划方法^［3］。任意起始位姿可规划有助于提高泊车成功率；高路径质量指路径长度短、方向改变次数少、满足安全性要求、平滑性好，这些都有助于提高泊车效率、泊车体验与路径跟踪精度；高计算效率有助于减少泊车规划等待时间。同时满足以上三者的路径规划方法可被直接应用于具有重规划能力的自动泊车系统中，有助于提高其环境适应性。

目前流行的路径规划方法主要有几何法、图搜索法、基于采样法、基于最优化法和人工智能法等5类。几何方法使用曲线组^［4-5］、多项式曲线^［6-7］、贝塞尔曲线^［8-9］、B样条曲线^［10-11］等连接起始位姿与目标位姿，其规划路径平滑且计算效率高，但采用给定形式的曲线规划造成规划不灵活，难以满足任意起始位姿的可规划性。图搜索方法中的Hybrid A*算法在泊车场景应用广泛^［12-14］，可解决了任意起始位姿可规划问题，但部分位姿下计算效率低且求解得到的路径质量不稳定。基于采样方法中采用RS曲线扩展的Bi-RRT*算法可用于泊车路径规划^［15］，但由于基于采样方法内在的随机机制，难以可靠保证任意起始位姿的规划效率。基于最优化方法将泊车问题建模为非线性规划问题后采用数值优化方式迭代求解^［16-19］，其规划路径质量高，但由于整个泊车过程难以直接建模为凸优化问题，其初始值对求解成功率与计算效率影响很大^［20-22］。人工智能方法使用机器学习^［23-24］、强化学习^［25-27］等通过数据训练神经网络得到车辆起始、目标状态与泊车轨迹的映射关系，路径质量与计算效率高，但其泛化能力有限，对于不同泊车起始位姿与泊车场景需要大量数据重新学习训练。

综上所述，目前在泊车场景下尚未有兼顾任意泊车起始位姿、路径质量与计算效率的路径规划方法。为此，本文提出了一种如图1虚线框所示的任意起始位姿的连续曲率泊车路径在线规划方法。

图1  任意起始位姿的连续曲率泊车路径规划方法

Fig.1  Schematic diagram of path planning method

该方法将整个泊车路径规划分为库位内调整过程与入库过程路径规划两部分并分别求解。其中库位内调整过程将问题转化为求解连续曲率圆弧曲率与弧长的凸优化问题并采用内点法求解，路径质量与计算效率高；采用重规划与重跟踪的策略，最终泊车位姿精度高。入库过程路径规划采用改进Hybrid A*算法以clothoid连续曲率曲线组形式扩展状态节点，可在任意起始位姿直接生成到达目标位姿的曲率连续的可执行路径，无需进行后处理因而计算效率高；设计了针对泊车问题的最短路径启发项与最少路径段数启发项以进一步提高计算效率；设计了考虑路径长度、方向改变次数与平滑性的代价项函数以提高路径规划质量；将泊车路径按几何形状分段后提取特征点得到车辆行驶区域特征多边形，采用“象限法”进行碰撞检测以确保规划路径安全性。

1 车辆状态模型

根据文献［13］，车辆从初始状态经过一段长为的距离，曲率变化率为的clothoid曲线后，状态改变为：

（1）

式中：、为车辆后轴中点坐标；为车辆航向角；为车辆运动方向；和分别为车辆后轴中点的路径曲率和曲率变化率，与分别表示最大曲率与最大曲率变化率，受限于执行机构的物理约束；为初始状态对应的clothoid曲线长度； 为路径长度。由于式（1）中的菲涅尔积分运算会造成较大的计算量，而其积分运算仅与、及有关，因此可离线计算各离散曲率的积分表，在线只需查表调用。

车辆从经过一段长为的圆弧和的直线后的状态改变分别如式（2）、（3）所示。采用clothoid曲线连接圆弧与直线，形成连续曲率圆弧，即CC（continuous curvature）圆，其曲率由0线性变化到某一曲率，保持曲率经过长度为的圆弧后，再从该曲率线性变化回0。

车辆从状态经过一段CC圆后的状态改变如式（4）所示。当CC圆圆弧的曲率与长度确定时，CC圆的形状确定。

（2）

（3）

（4）

2 库位内调整路径规划

根据图1，在入库路径规划前与入库路径跟踪结束后两个阶段进行库位内调整路径规划：前者目的在于确定最优的入库路径规划目标位姿；后者则是由于跟踪长度短且曲率大的路径跟踪误差大，需不断进行路径重规划与重跟踪才能满足最终泊车目标位姿要求。为了避免曲率突变造成的车辆原地转向，库位内调整路径规划采用式（4）的CC圆进行规划。

2.1　入库路径规划前库位内调整规划

入库路径规划前的库位内调整路径规划采用从目标位姿开始以最小半径CC圆向外逐步规划的方法，直至满足车辆可无碰撞出库的要求。图2为两次库位内调整的规划过程示意图，车辆从目标位姿开始依次计算得到位姿和位姿，若在位姿时，车辆可由以为圆心的最小转弯半径CC圆弧无碰撞出库，无碰撞条件为前轮右前角点转弯半径小于CC曲线圆心与前障碍车左后角点连线的距离，则可将位姿作为入库路径规划目标位姿。

图2  入库路径规划前库位内调整规划过程

Fig.2  Path planning before reversing into parking lot

2.2　入库路径跟踪后库位内调整规划

在入库路径跟踪结束后的库位内调整采用由当前车辆位姿向目标位姿规划的方法，目的在于规划出在当前位姿的下一段库位内调整路径，在每一段库位内调整路径规划结束后进行重规划与跟踪，直至车辆位姿满足目标位姿要求，规划过程为入库路径规划前的库位内调整路径规划的逆过程。以图3所示的库位内调整规划过程为例，假定入库路径跟踪结束后车辆位于位姿，规划得到路径并跟踪该路径，由于存在跟踪误差，实际行驶路径如虚线所示，该段路径跟踪结束时车辆位于位姿，在进行路径重规划计算到达目标位姿的路径并跟踪，当车辆位姿位于目标位姿附近误差允许范围内时泊车结束。

图3  入库路径跟踪后库位内调整规划过程

Fig.3  Path planning within parking lot

基于式（4）的CC圆，可将库位内调整路径规划转化为以下凸优化问题：

（5）

式中：

（6）

（7）

（8）

在式（6）—式（8）中，为库位内调整路径规划起始状态，其经过一段CC圆后的状态为。控制输入，其中为CC圆圆弧待求曲率，为CC圆圆弧待求弧长。

式（5）的优化目标如式（6）所示，为车辆航向角与目标航向角的差值。式（5）中的等式约束具体形式如式（7）所示，以式（4）所示CC圆形式进行状态变化。式（5）中的第一个不等式约束为环境约束，具体表达式如式（8）所示，其中的、、、、、分别表示车辆前、后角点到前、后障碍车的距离与路缘侧前、后轮胎到路缘的距离。式（5）中第2个不等式为控制输入约束，， $u_{\mathrm{m}\mathrm{a}\mathrm{x}}=[{\kappa }_{\mathrm{m}\mathrm{a}\mathrm{x}},s_{\mathrm{m}\mathrm{a}\mathrm{x}}]$。采用内点法对式（5）—（8）所描述的凸优化问题进行求解。

3 入库过程路径规划

3.1　连续曲率曲线组设计

结合人类驾驶员的泊车经验与RS曲线^［28］的9种组合形式，将适用于平行泊车场景的曲线组归纳为与两种，将适用于垂直泊车场景的曲线组归纳为、与等3种。其中：、、分别表示逆时针转弯CC圆（）、顺时针转弯转CC圆（）和直线（）；上标、分别表示车辆前进与后退；下标表示各段曲线长度。

上述5种连续曲率曲线组合的示意如图4所示。

图4  五种连续曲率曲线组示意图

Fig.4  Continuous curvature curve groups

3.2　状态节点扩展

定义第个状态节点为四元组，以图5所示的连续曲率方式扩展状态节点，图中的表示第次扩展得到的第个节点。每次状态节点扩展得到的路径由两部分组成，在图5中分别以点线与实线表示，其中点线为曲率从变化到的clothoid曲线，实线为曲率为的圆弧或直线。

图5  连续曲率状态节点扩展示意图

Fig.5  Node expansion in curvature-continuous way

每一次状态节点扩展固定到达下一状态节点所经过的路径长度为，下一状态节点的曲率从曲率集合中选取，集合中，N为曲率集合元素个数。扩展得到状态节点所经过的clothoid曲线长为，圆弧或直线长为。根据式（1）—式（3），可得式（9）、（10）所示的状态节点表达式。

（9）

（10）

式中：为经过clothoid曲线后的航向角。

3.3　估价函数设计

估价函数的一般表达形式如式（11）所示^［29］：

（11）

式中：代价项函数用于衡量泊车起始状态节点到中间状态节点的实际代价值；启发项函数为中间状态节点到目标状态节点的代价估计值，用于衡量与目标状态的接近程度。本文设计的如式（12）—（16）所示，由路径长度项、换挡惩罚项、大曲率惩罚项与曲率变化惩罚项组成。

（12）

（13）

（14）

（15）

（16）

式中：、、、、分别为各惩罚项权重系数，对于曲率方向改变应赋予更大的惩罚值，令>。各个权重系数的取值与各对应项的优先级有关，可针对不同泊车场景与泊车需求根据试验确定各个权重系数。

本文设计的如式（17）所示，从左至右分别为最短路径启发项、最少路径段数启发项与避障启发项。其中的避障启发项与原始Hybrid A*算法相同，目的是为了引导搜索远离U形障碍物与死区。

（17）

不同于原始Hybrid A*算法，本文重新设计了最短路径启发项以适应本文算法并提高搜索效率，并加入了最小路径段数启发项弥补了原始算法只考虑路径长度造成规划结果包含一些改变路径方向的“尖点”的不足。

最短路径启发项与最少路径段数启发项为不考虑环境障碍物时，状态节点到目标位姿的连续曲率路径长度与路径段数。对于泊车问题而言，最小库位下的路径规划约束更为严格，在此场景下求得的启发项值更大，可有效保证在其他大库位场景下的搜索效率。由于与的计算不考虑环境中的其他障碍物，可离线进行计算，在线只需进行简单的平移与旋转计算即可查表获取启发项值。

本文采用的栅格地图尺寸为16 m×25 m×2π rad×0.54/m，X⁃Y方向分辨率为0.5 m，航向角θ分辨率为0.1 rad，路径曲率分辨率为0.01/m。

3.4　碰撞检测

本文规划路径由直线、圆弧与clothoid过渡曲线组成，车辆跟踪这3种曲线行驶过的区域有不同的轮廓特征，如图6所示。提取不同轮廓的特征点组成车辆行驶区域的外接多边形，通过检查由规划路径生成的一系列外接多边形组是否与库位边界相交进行碰撞检测。

a  直线

b  圆弧

c  曲率增大clothoid曲线

d  曲率减小clothoid曲线

图6  不同曲线车辆行驶区域特征多边形

Fig.6  Vehicle driving region feature polygon of different curves

图6a所示为直线路径，车辆驶过区域为矩形，根据G₂、G₁点车辆前、后方角点坐标即可得区域轮廓。图6b所示为圆弧路径，轮廓内侧由线段P₁P₂、P₄P₅与一段圆弧组成，在一定精度范围内可用内侧折线段P₂P₃P₄拟合圆弧；轮廓外侧由车辆右后角点走过的圆弧、右前角点走过的圆弧与连接二者的线段P₈P₉组成，在一定精度范围内可用外侧折线段P₆P₇P₈与P₉P₁₀P₁₁拟合圆弧。对于clothoid曲线，其曲率增大与曲率减小时车辆驶过的区域轮廓具有不同的特征，因此应当按照不同的规则提取特征点。图6c所示为曲率增大的clothoid曲线路径，轮廓特征与圆弧相似，可用与之相同的方法提取特征点。图6d所示为曲率减小的chothoid曲线路径，区域轮廓与图6c主要在车辆右前角点走过的曲线形状有较大差异，为一凹凸性变化的曲线，因此先找到该曲线的拐点P₇，再分别用曲线段P₆P₇与P₇P₈P₉拟合两侧曲线。

将行驶区域与障碍物均用多边形表示后，采用文献［30］所述的“象限法”快速判断点在多边形的内外。当障碍物多边形各角点均在行驶区域多边形之外且行驶区域多边形各角点均在障碍物多边形之外时，判定为路径安全，反之则判定为路径有碰撞风险。

4 试验验证

4.1　仿真验证

为了评估本文算法，将本文算法计算时间、路径长度、换挡次数与原始Hybrid A*算法进行比较。测试场景如图7所示，点表示起始位姿的X-Y坐标，箭头方向表示车辆起始航向角方向。平行与垂直泊车场景均测试了153个起始位姿，X方向范围为-8.0~8.0 m，Y方向范围为1.2~3.2 m，相邻位点之间间隔1.0 m，每个位点的航向角为｛-0.1 rad， 0， 0.1 rad｝。车辆参数与测试库位相关参数如表1所示，库位相关参数与图7对应。算法运行平台为dSPACE AutoBox Ⅱ 1401/1513，处理器主频为900 MHz。

图7  平行库位与垂直库位泊车测试场景示意图

Fig.7  Schematic diagram of test scenarios for parallel parking and vertical parking

a 平行库位 b 垂直库位

表1  车辆参数与测试库位相关参数

Tab.1  Vehicle parameters and related parameters of test parking spaces

车辆参数	轴距/m	前悬/m	后悬/m	车宽/m	最大曲率/m-1	最大曲率变化率/m-2
参数值	2.305	0.72	0.544	1.551	0.27	0.4
库位参数	WR/m	DP/m	WP/m	DV/m	WV/m
参数值	4.5	2	4.85	3.769	2.251

图8为原始Hybrid A*算法与本文算法分别在153个起始位姿的平行、垂直泊车场景下的计算时间、路径长度与路径段数的箱型图。箱型图可以直观地反映值的分布范围，便于对仿真结果进行统计分析与比较。

a  计算时间

b  路径长度

c  路径段数

图8  原始Hybrid A*算法与本文算法在平行、垂直泊车场景下路径规划比较

Fig.8  Comparison of path planning between the original Hybrid A* algorithm and the proposed algorithm in parallel and vertical parking scenarios

注：  “1”表示Hybrid A*算法；“2”表示本文算法。

由图8a可见，本文算法计算时间小于原始Hybrid A*算法，求解效率高且表现稳定，在所有测试位姿下的计算时间均小于500 ms，可满足在线规划要求，表明本文的连续曲率曲线组扩展状态节点与碰撞检测方法并未带来过大的计算量，且估价函数的合理设计可引导算法快速收敛得到可行解。

由图8b和8c可见，本文算法求得的路径长度与原始Hybrid A*算法相差不大，且在某些场景下优于原始Hybrid A*算法，而路径段数相较于原始Hybrid A*算法更少，表明尽管本文算法考虑了路径曲率变化率约束，且为了兼顾舒适性在估价函数中加入了路径大曲率惩罚项与曲率变化惩罚项，但最短路径启发项与最少路径段数启发项有效地引导了搜索方向使得路径长度与路径段数较优。

4.2　实车验证

为了验证规划路径的实车可跟踪性，在基于荣威E50纯电动车改制而成的智能驾驶测试平台上进行实车试验。将编译后的路径规划模型烧写到dSpace MicroAutobox控制器中运行，在线实时规划路径后，采用文献［31］设计的控制器跟踪路径。

图9a、9b分别为入库路径为一段与两段的平行泊车规划跟踪结果，库位内调整次数为两次；图9c—9e分别为一段、两段与三段路径的垂直泊车规划跟踪结果。图9中实线为规划路径，虚线为跟踪结果，浅色矩形为跟踪过程中的车辆轮廓，深色矩形为最终泊车位姿。

a  一段路径平行泊车

b  两段路径平行泊车

c  一段路径垂直泊车

d  两段路径垂直泊车

e  三段路径垂直泊车

图9  实车路径跟踪结果

Fig.9  Real vehicle path tracking results

图10和图11分别为两段平行泊车与三段垂直泊车转向盘转角跟踪与车速跟踪结果。根据图9，规划路径可很好地被车辆无碰撞跟踪执行；根据图10与图11，泊车过程中转向盘转角变化较为平滑且不存在原地转向。图9的5个场景下最终泊车位姿与目标泊车位姿的X、Y方向偏差与航向角偏差如表2所示，其中X方向偏差均在0.07 m以内，Y方向偏差均在0.08 m以内，航向角偏差均在0.8°以内，最终泊车位姿具有较高精度。

a  转向盘转角跟踪

b  车速跟踪

图10  两段路径平行泊车跟踪结果

Fig.10  Two-segment parallel parking path tracking results

a  转向盘转角跟踪

b  车速跟踪

图11  三段路径垂直泊车跟踪结果

Fig.11  Three-segment vertical parking path tracking results

表2  实车试验最终泊车位姿与目标位姿偏差

Tab.2  Error between the final parking pose and the target pose of real vehicle tests

场景	X方向/m	Y方向/m	航向角/（°）
一段平行泊车	-0.067	-0.076	-0.560
两段平行泊车	-0.022	-0.020	-0.339
一段垂直泊车	-0.006	-0.062	-0.037
两段垂直泊车	0.036	0.018	0.724
三段垂直泊车	0.011	-0.020	0.186

6 结语

本文提出了一种可在任意起始位姿规划出连续曲率路径的分段式路径规划方法，其中库位内路径调整采用非线性规划方法，入库过程路径规划采用改进Hybrid A*算法。仿真结果表明：该算法可在任意起始位姿规划出泊车路径，且求解时间均小于500 ms，具备了在线规划能力；所提出的改进Hybrid A*算法规划路径相较于原始Hybrid A*算法的计算效率更高，规划路径长度更短，路径段数更少，泊车效率更高。实车试验结果表明，规划的路径能被车辆很好地跟踪且不存在原地转向问题，其中横、纵向的跟踪偏差均在8 cm以内，航向角跟踪偏差均在0.8°以内。