蔡 云，王 广，张又水，吴澳琦，陈 锐

基于狭窄垂直车位的多段式路径规划与仿真分析

蔡 云1,2，王 广1,2，张又水1,2，吴澳琦1,2，陈 锐3

（1.西华大学 汽车与交通学院，四川 成都 610039；2.汽车测控与安全四川省重点实验室，四川 成都 610039；3.成都路行通信息技术有限公司，四川 成都 610041）

自动泊车系统（APS）目前受到广泛的关注，但大部分研究忽略了车辆初始航向角偏差对整体泊车效果的影响，对此，论文研究了狭窄垂直车位场景下车辆初始航向角不为零的多段式路径规划方法。首先基于车辆最小转弯半径建立了车辆后轴中心运动轨迹方程，通过五次多项式对路径规划进行优化，实现路径曲率更连续、泊车更平顺和舒适。CarSim/Simulink联合仿真验证表明，五次多项式泊车路径优化方法可行，在满足泊车安全的基础上，泊车位所需实际宽度减少10.4%，长度减少15.3%。

垂直泊车；路径规划；五次多项式；模型预测控制；联合仿真；CarSim/Simulink

随着高级驾驶辅助系统（Advanced Driving Assistant System, ADAS）在汽车市场的逐渐普及，作为ADAS重要组成部分的自动泊车系统（Auto- matic Parking System, APS）也得到了更多的关注[1]。我国汽车保有量的逐年增加与泊车位供应不平衡之间的问题愈发突出，狭窄车位逐渐增多，泊车环境日益复杂，交通事故频发。开发基于狭窄垂直车位的自动泊车系统，一方面可以将驾驶员从繁琐的泊车操纵中解脱出来，降低交通安全事故发生概率，促进自动驾驶技术的发展[2]；另一方面可以大幅提升土地资源利用率，缓解当今“车多位少”的局面。国内外很多学者对路径规划技术做了研究[3]，包括logistic曲线法[4]、回旋曲线法[5-7]、B样条曲线法[8]、Bezier曲线[9]、三角函数曲线法[10]、ReedsShepp（RS）曲线法[11]以及多项式曲线法[12]等。虽然针对自动泊车系统研究取得了不俗的进展，但是当前的大部分泊车路径规划算法未考虑车辆初始航向角带来的误差影响，同时针对交通事故更易发生的狭窄垂直车位泊车工况研究较少。因此，本文对狭窄垂直车位的多段式路径规划问题开展研究，采用最小转弯半径设计了泊车路径，运用五次多项式进行路径优化，并联合CarSim/Simulink搭建垂直自动泊车仿真实验平台来验证所提出的狭窄垂直车位多段式路径规划算法的合理性和可靠性。

1 自动泊车系统简介及运动学模型

自动泊车系统工作流程如图1所示。驾驶员选择自动泊车功能后，自动泊车系统基于环境感知的车位进行路径规划，然后控制汽车进行轨迹跟踪，控制汽车安全泊入停车位[13]。泊车完成后转向盘回正并挂驻车挡，自动泊车系统关闭。

汽车轮廓外形对泊车成功率有直接影响。由于实际汽车外形繁杂多样，在给予一定余量的条件下，设计以最大长度和最大宽度的矩形来代替汽车外形[14]。简化后如图2所示。本文仿真与计算的参数设定如表1所示。

图1 自动泊车系统工作流程图

图2 汽车模型简化图

表1 汽车主要参数表 单位：m

参数数值 车长L4.634 车宽W1.855 轴距Lz2.661 前悬Lf0.785 后悬Lr1.070

选择停车时后轴的中心点作为车辆参考点，在低速泊车工况下，根据阿克曼（Ackeman）转向几何理论，在低速工况下将车辆视为平面刚体，即只存在方向，方向和横摆3个自由度。汽车在坐标系下的后轴中心坐标为（r,r），航向角为，汽车运动学模型为

式中，z为轴距；r为车速；f为等效前轮转角。

2 狭窄垂直泊车位的路径规划策略

在自动泊车过程中，由于车辆后轴中心轨迹能完全体现整车的轨迹特征[15]，故采用后轴中心代表整车泊车轨迹。采取一步式泊入的路径规划方法所需的路径较长，泊车区域较大，当车辆处于狭窄垂直车位工况时，道路条件很难满足要求。为了使自动泊车系统更好地应用于实际生活中，提高乘客舒适度。针对始航向角不为零时的狭窄垂直车位工况，提出基于车辆最小转弯半径的圆弧-直线-圆弧的多段式路径规划设计。

2.1 初始航向角不为零时的路径规划分析

在泊车过程中，以车辆前进方向为轴方向，以车位右顶点为坐标原点，垂直方向轴设立坐标系。设汽车后轴中心点为1（x1，y1），泊车位宽度为P，泊车开始后，在第一段试探性圆弧泊入车库的过程中，当后轴中心点到达2时，若继续后退，车辆最右侧与原点会有碰撞风险，如图3所示。此时选择回正方向盘前进一段距离后再右打方向盘安全泊入车库中，其后轴中心轨迹如图4所示。

图3 第一段泊车路径几何图

图4 航向角不为零时泊车路径轨迹图

汽车相对于泊车位的初始航向角为

=arctan(b/b) （2）

为方便计算，将整个坐标系逆时针旋转角度，旋转后坐标系11，假设泊车起始点1坐标为(x1,y1)，汽车从1开始以最小半径开始泊车，根据车辆自身参数以及几何关系可以得出：

0=arctan(r/(+/2)) （3）

根据汽车一般性泊车运动规律，易证车身最右侧在第一段泊车过程中与原点距离最近。设防撞阈值S=0.2 m，可计算出点1(x1,y1)的坐标为

由1可以得出2点的坐标为

当车辆到达2点后，此时调整方向盘转角为零，沿着直线路径到3点，设直线路径长度为d，则d为

d=(-(O1+(b/2))/sin(π/2-1+) （7）

式中，d为23的直线距离，此时3点的坐标为

圆弧34对应的角度2为

2=π/2-1+（9）

根据2的值可得到路径的2圆心坐标：

最后计算出4点坐标：

2.2 垂直泊车路径改进

由于圆弧-直线组合路径的曲率可能发生突变导致车辆原地转向，那么基于圆弧-直线-圆弧的路径规划大概率会导致路径跟踪控制失败。为了确保基于最小转弯半径规划的路径契合车辆连续转向的特征，同时又可以实现转向和避障约束，考虑到具有连续曲率的五次多项式[16]曲线能更好地满足路径的平滑度和灵活性，且运算量增加较少，故选用五次多项式对圆弧直线段进行拟合，使其曲率连续优化并满足车辆转向约束。

首先，针对五次多项式，曲线的公式可表达为

=55+44+33+22+1+0（12）

然后，分析车辆起点和终点的位姿状态，根据路径规划起始点1和航向角可得

同理，已知终点4的坐标和航向角，则有

最后，为了保证曲线有良好的拟合效果同时确定避障约束，根据曲线经过2、3点，可得

根据式（12）—（18），可以得到拟合的五次多项式曲线，该曲线即为垂直泊车的规划路径。

为使生成路径更好地满足狭窄垂直车位场景下的车辆转向半径约束，将3的坐标沿当前航向角方向适当延长一段距离d1，则延长后的坐标为

同时将4向下延长一段距离d2，此时延长后4的坐标为

由于不同的中间点会生成曲率不同的曲线，将2进行适当修正并设为中间点：

式中，Δ为修正值。

由车辆运动学模型和初始航向角可知，目标车辆在1点的斜率k1=，在4点的斜率k4=π/2，因tan(π/2)=+∞，所以通过将整个坐标系逆时针旋转π/4以方便计算，转换之后起点和终点的斜率为

k1=-π/4+（22）

k4=1 （23）

旋转坐标系后对应点的坐标如图5所示。

图5 逆时针旋转45°的坐标轴示意图

由图5几何关系可知

同样，为了得到旋转之前的坐标系下连续的可行曲线，再将坐标系顺时针旋转π/4，公式如下：

将上述参数代入到MATLAB中进行编程，以车辆初始航向角5°为例，求出五次多项式优化后的曲线图，如图6所示。

图6 五次多项式路径优化与原路径对比示意图

图6中的实线为五次多项式优化后的曲线。图7为优化后的曲线避撞约束验证示意图；图8为优化后生成的曲线曲率图，其最大值为0.07m-1左右，满足车辆最小转向半径约束，同时曲率变化连续，保证了泊车过程中的平稳性和顺畅性。运用五次多项式优化后的曲线更加平滑，避免了车辆的反复转向和轮胎的过度磨损。

图7 五次多项式路径规划避障约束验证示图

图8 五次多项式规划路径曲线曲率示意图

2.3 垂直泊车跟踪控制

在自动泊车工况中，通过控制车辆的方向盘转角与车速，使车辆沿期望的路径行驶称为跟踪控制。目前主要使用的跟踪算法有预瞄控制[17]、自适应控制[18]、纯跟踪控制[19]、模糊控制[20]、线性二次型调节器（Linear Quadratic Regulator, LQR）[21]和模型预测控制（Model Predictive Con- trol, MPC）[22]等。本文选择MPC法对自动垂直泊车系统进行轨迹跟踪。MPC的基本原理是根据模型科学预测系统在某一时间段内的表现进行优化控制的过程，其具有预测模型、滚动优化和反馈校正三个特点[23]，控制流程如图9所示。

图9 MPC控制流程图

将式（1）的汽车运动学方程看作输出为r=[r,r]和状态量r=[rrr]的控制系统，其一般形式为

式中，r=[rrr]T,r=[r,r]T。

将函数泰勒数级展开，通过显示欧拉法（For- ward Euler, FE）可得到离散系统状态方程：

(+1)=k()+k() （27）

设采样周期为，为当前采样时刻，+1为下一采样时刻。将性能评价函数设为

式中，、为权重系统矩阵；为松弛因子。

为了保证车辆能沿着规划路径行进，对控制量和控制增量进行约束，表示为

cmaxmin（29）

Δcmaxmin（30）

MPC的基本原理即是在满足控制约束式（28）和式（29）的基础下使性能评价函数式最小。一般将优化问题转为标准二次规划问题求解。即目标函数转化为

((),(-1),Δ())=

[Δ()T,]Tt [Δ()T,]+t [Δ()T,] （31）

3 仿真验证

3.1 仿真设置

基于CarSim/Simulink软件对所规划的泊车路径以及控制算法进行联合仿真。在CarSim中根据表1的参数建立车辆和泊车位模型并链接到Simu- link，接着在Simulink中设立基于五次多项式的路径规划模块与MPC的轨迹跟踪模块，搭建完上述模块后进行联合仿真。参考《城市道路路内停车泊位设置规范》（GA/T 850—2021），城市内垂直停车位长度一般为6 m，宽度为2.5 m，在仿真中将狭窄垂直泊车位长宽设为（5.2×2.3）m。在CarSim中设置泊车场景如图10所示。

Simulink中的联合仿真模型如图11所示，包括控制模块、信号反馈模块、参数计算模块，以及参数显示模块。车辆参数沿用表1所列出的相关参数。在CarSim输出车辆初始位置、航向角、速度等信息，通过 Simulink模块计算后将得到的速度，控制制动压力以及方向盘转角输入到，得出车辆的实时运动状态到CarSim中，最后观察泊车效果。显示模块显示实际所需的泊车位宽度、长度，以及泊车时间（车辆开始倒车时进行计时）。

图11 CarSim与Simulink联合仿真示意图

3.2 仿真结果分析

为验证初始航向角不为零时的算法有效性，选取CarSim自带车型D-class-SUV为模型，选择表1所列参数验证航向角不为零时的算法有效性。以航向角5°为例得到仿真动画如图12所示。

图12 航向角为5°时的泊车动画示意图

车辆泊车的动画效果如图13(a)所示，车辆的后轴中心参考路径曲线与后轴中心实际路径曲线基本吻合，说明车辆轨迹跟踪效果良好，车辆可沿规划路径泊入车位中。车辆前轮转角如图13(b)所示，其在最大限制转角限制范围内且最后基本回正，过程中无剧烈转向、乘车舒适性较好。航向角如图13(c)所示，从5°至最后稳定于90°，表明车辆已垂直泊入停车位。泊车参数如图13(d)所示，基于最小转弯半径的实际泊车位所需宽度为2.24 m，所需长度为5.08 m，泊车时间为11.25 s。对比《城市道路路内停车泊位设置规范》（GA/T 850—2021）小型车位标准尺寸，采用此种泊车方案停车位实际宽度减少10.4%，长度减少15.3%，能较好地适应狭窄垂直车位工况。

4 结论

本文对初始航向角不为零时的狭窄垂直车位泊车工况进行了研究，基于最小转弯半径设计了圆弧-直线-圆弧的多段式路径规划，针对圆弧直线组合路径曲率突变的情况，采用平滑度和灵活性较好的五次多项式对规划的路径进行了优化，联合Carsim/Simulink对规划路径进行了仿真验证。结果表明经过五次多项式优化后的路径曲率连续且无碰撞，泊车过程车辆无剧烈转向，优化效果较好。较城市内泊车设置规范GA/T 850—2021要求，此泊车方案停车位宽度和长度均有较为明显减少，能更好地满足城市中常见的车辆初始航向角不为零的狭窄垂直车位泊车场景。

[1] PADEN B,CAP M,YONG S Z,et al.A Survey of Motion Planning and Control Techniques for Self- driving Urban Vehicles[J].IEEE Transactions on Intel- ligent Vehicles, 2016,1(1):33-55.

[2] BANZHAF H, NIENHUSER D, KNOOP S, et al. The Future of Parking: A Survey on Automated Valet Par- king with an Outlook on High Density Parking[C]// 2017 IEEE Intelligent Vehicles Symposium.Piscata- way:IEEE,2017,1827-1834.

[3] GONZÁLEZ D,PÉREZ J,MILANÉS V,et al.A Review of Motion Planning Techniques for Automated Vehi- cles[J].IEEE Transactions on Intelligent Transportation Systems,2015,17(4):1135-1145.

[4] UPADHYAY S,RATNOO A.Continuous-curvature Path Planning with Obstacle Avoidance Using Four Parame- ter Logistic Curves[J].IEEE Robotics and Automation Letters,2016,1(2):609-616.

[5] KIM D J,CHUNG CC.Automated Perpendicular Par- king System with Approximated Clothoid Based Local Path Planning[J].IEEE Control Systems Letters, 2021,5(6):1940-1945.

[6] 张家旭,赵健,施正堂,等.基于回旋曲线的垂直泊车轨迹规划与跟踪控制[J].东南大学学报(自然科学版),2020,50(1):182-191.

[7] VOROBIEVA H, GLASER S, MINOIU-ENACHE N, et al.Automatic Parallel Parking in Tiny Spots:Path Planning and Control[J].IEEE Transactions on Intelli- gent Transportation Systems,2014,16(1):396-410.

[8] 李红,郭孔辉,基于样条理论的自动垂直泊车轨迹规划[J].湖南大学学报(自然科学版),2012,39(7):25-30.

[9] MOON J, BAE I, CHA J, et al. A Trajectory Planning Method Based on Forward Path Generation and Backward Tracking Algorithm for Automatic Parking Systems[C]//17th International IEEE Conference on Intelligent Transportation Systems.Piscataway:IEEE, 2014:719-724.

[10] 王启明,宗高强,胥津铭.多段式自动泊车最优路径规划与仿真分析[J].系统仿真学报,2022,34(2):385-395.

[11] KIM J M, LIM KI, KIM J H.Auto Parking Path Planning System Using Modified Reeds-shepp Curve Algorithm[C]//2014 11th International Conference on Ubiquitous Robots and Ambient Intelligence.Piscata- way:IEEE,2014:311-315.

[12] 高强,陆洲,段晨东.汽车垂直泊车路径规划与路径跟踪研究[J].汽车工程,2021,43(7):987-994,1012.

[13] WU Z,SUN W,WANG M,et al. Psdet:Efficient and Universal Parking Slot Detection[C]//2020 IEEE Intelligent Vehicles Symposium.Piscataway:IEEE,2020: 290-297.

[14] LI B,WANG K,SHAO Z.Time-optimal Maneuver Plan- ning in Automatic Parallel Parking Using a Simultan- eous Dynamic Optimization Approach[J].IEEE Tran- sactions on Intelligent Transportation Systems,2016, 17(11):3263-3274.

[15] MORALES D P, KERMORGANT O, QUIJADA S D, et al.Laser-based Control Law for Autonomous Paral- lel and Perpendicular Parking[C]//2018 Second IEEE International Conference on Robotic Computing. Piscataway:IEEE,2018:64-71.

[16] ZHANG S, SIMKANI M, ZADEH M H. Automatic Vehicle Parallel Parking Design Using Fifth Degree Polynomial Path Planning[C]//2011 IEEE Vehicular Technology Conference (VTC Fall).Piscataway:IEEE, 2011:1-4.

[17] 熊周兵,杨威,丁可.基于预瞄模糊控制的自动泊车算法研究[J].重庆理工大学学报(自然科学版),2017,31 (2):14-22.

[18] 周鹏.基于自适应算法的平行泊车路径规划及跟踪控制[D].哈尔滨:哈尔滨理工大学,2020.

[19] 陈龙,罗杰,杨旭.基于模糊纯追踪控制的自动泊车算法研究[J].武汉理工大学学报(信息与管理工程版), 2019,41(3): 316-321.

[20] 杨向彬.基于Fuzzy-PID控制的平行泊车仿真[J].汽车实用技术,2019,44 (12):132-135.

[21] 李翔.无人车运动控制与自主泊车路径规划方法[D]. 杭州:浙江大学, 2021.

[22] 龚建伟,姜岩,徐威.无人驾驶车辆模型预测控制[M].2版.北京:北京理工大学出版社,2020.

[23] TASHIRO T. Vehicle Steering Control with MPC for Target Trajectory Tracking of Autonomous Reverse Parking[C]//2013 IEEE International Conference on Control Applications.Piscataway:IEEE,2013:247-251.

Multi-stage Path Planning and Simulation Analysis Based on Narrow Vertical Parking Space

CAI Yun1,2, WANG Guang1,2, ZHANG Youshui1,2, WU Aoqi1,2, CHEN Rui3

( 1.School of Automobile and Transportation, Xihua University, Chengdu 610039, China;2.Vehicle Measurement, Control and Safety Key Laboratory of Sichuan Province, Chengdu 610039, China;3.Chengdu Luxingtong Information Technology Company Limited, Chengdu 610041, China )

Automated parking system (APS) is a hot topic in current research. However, most studies ignore the effect of the initial vehicle heading angle deviation on the overall parking effect, this paper investigates a multi-stage path planning method with a non-zero initial vehicle heading angle in a narrow vertical parking space scenario. Firstly, based on the minimum turning radius of the vehicle, the trajectory equation of the vehicle rear axle center motion is established, and the path planning is optimized by the fifth polynomial to achieve more continuous path curvature, smoother and more comfortable parking. CarSim/Simulink joint simulation validation shows that the five polynomial parking path optimization method is feasible, and the actual width required for the parking space is reduced by 10.4% and the length is reduced by 15.3% on the basis of satisfying the parking safety.

Vertical parking;Path planning;Quintuple polynomial;Model predictive control;Co-Simulation;CarSim/Simulink

U471.15

A

1671-7988(2023)10-22-08

10.16638/j.cnki.1671-7988.2023.010.005

蔡云（1979—），男，硕士，副教授，研究方向为智能汽车感知、规划与控制技术，E-mail:29682131@qq.com。

基于北斗双模高精度定位的智能车联大数据安全综合服务平台开发及推广示范（2020ZHCG0077）。