平行泊车路径规划算法研究及验证

王瑞 孙涛 赖杰

摘要：路径规划算法研究是自动泊车系统中最常见的问题之一。为解决平行泊车场景下，车辆泊车入位较难的问题，采用反向推导法，理论分析泊车过程并推导了路径规划函数公式，在确定泊车起点后，能够安全引导车辆进入泊车位。首先建立车辆模型，在确定碰撞约束和最优目标函数后，推导了车辆三阶段单向行驶泊车路径;然后针对单向泊车路径规划中要求车位长度较长的问题，提出四阶段行驶路径规划方法;最后，根据实际车辆及车位信息，利用Matlab软件与实验车平台，分别验证了该路径规划方法的有效性。实验结果表明，采用双向四阶段泊车方式相比单向三阶段泊车方式，对车位长度的要求缩短了0.26m。

关键词：路径规划;平行泊车;车辆模型;软件仿真

DOI： 10. 11907/rjdk.192711

开放科学（资源服务）标识码（OSID）：

中图分类号：TP312

文献标识码：A

文章编号：1672-7800（2020）004-0119-06

Research and Verification of Parallel Parking Path Planning Algorithm

WANC; Rui. SUN Tao . LAI Jie

（School of Mechanical Engineering ， Univer.sity of .Shanghaifor Science and Technology ， Shanghai 200093，China ）Abstract： Research on path planning algorithms is one of the most common problems in vehicle parking systems. This research is ap-plied to the path planning probleru in the parallel parking scenario. The reverse derivation method is used to theoretically analy ze theparking process and derive the path planning function formula. After the parking starting point is determined. the vehicle can be safelyguided into the parking space. This paper first estahlishes a vehicle model. After determining the collision constraints and the optimalobjective function. the f'ormula derives the three-phase one-way parking path of' the vehicle. Then， the problem of the long parkingspace required in the one-way parking path planning is proposed. Four-stage driving path planning method. Finally， according to theactu al vehicle information and parking space information . the parking path trajectory is simulated using Matlab software. Finally ， a testvehicle is used as a platform to establish a software and hardware test platform for real vehicle verification. The results shoW the effec-tiveness of the proposed method for path derivation. The test results show that the two-way four-stage parking method requires 0.26mshorter parking space than the one-way three-stage parking method.Key Words ： path planning ; parallel parking ; vehicle model; software siiu ulation

O 引言

根據公安部教管局统计，截至2019年上半年，全国汽车保有量达到2.5亿辆，私家车达到1.98亿辆…。随着车辆保有量近年来不断攀升，城市交通拥堵和停车难的问题也尤为突出。据统计，在一线城市，司机30%的驾驶时间花在停车、找车位上[2]。采用白主泊车系统能较好地解决泊车难、泊车效率低这一问题。

泊车系统功能开发主要要求如下：控制单元必须实时监控车辆运行状态，并检测停车环境的变化。当检测到的泊车位满足泊车要求，即检测到的泊车位长度大于泊车所需的最小长度时，系统将提示驾驶员。如果驾驶员接受停车位，系统将切换到自动停车模式，接管车辆转向，自动控制停车速度，完成停车过程[3]。目前对于白动泊车方面的研究主要分为两大类。[4]：一类是基于人工智能的控制方法，通过系统的白学习能力学习驾驶员的泊车经验，利用神经网络或模糊控制实现泊车;另一类是基于对路径规划的研究，利用车辆感知到的车位信息及车辆运动状态预先规划出一条泊车路径，通过控制车辆行驶轨迹实现泊车过程。如文献[3]通过建立车辆动力学模型，运用数学方法设置泊车环境及尺寸条件，基于模糊控制理论方法白学习驾驶员倒车经验，设计控制器操纵并稳定车辆转向，最后在Matlah中验证整个泊车过程;文献[4]运用反正切函数及改进的反正切函数对两段网弧式泊车路径进行逼近拟合，并在Matlab中进行泊车仿真验证。自动泊车研究的关键问题是路径规划，大部分研究是将泊车路径简化为两段圆弧[5]，而本文对泊车路径规划问题的研究基于三段式泊车路径。汽车从寻找车位起始点至泊车人位结束，车辆行驶的3个阶段为：车辆由起始位置行驶至泊车起点的路径、第一段圆弧路径以及第二段圆弧路径。基于两段圆弧式的泊车路径能解决一般泊车路径的规划问题[6]，但在实际泊车过程中，由于车辆传感器的执行误差或系统执行上的误差累积，车辆可能在未执行完规划路径时，后边缘边界就会发出碰撞预警，使车辆无法继续行驶。针对这一问题，要考虑被迫停车后的二次路径规划甚至多次路径规划，直到车辆成功停入泊车位。

1 泊车路径规划

1.1 车辆模型建立

在开发泊车系统功能时，主要是研究车辆泊车轨迹与速度、转向角等参数之间的关系[7]。汽车在泊车过程中的轨迹主要包括车身轮廓上关键点与后轴中点的运动轨迹。为了方便对以上参数的研究，首先基于阿克曼转角建立车辆动力学模型，在不考虑侧向加速度以及车轮打滑的情况下，根据泊车过程中的汽车位置和运动方向建立坐标系。构建车辆模型如图1所示。

其中，a、b、c、d4点为车辆矩形轮廓的4个边界点，A（Xr，Yr），B（Xf，Yf）为汽车前后轴的中点坐标。

在整个泊车过程中，车辆行驶速度都较慢，车辆后轴方向速度接近于零[8]。位移和速度的关系表现为：

其中车辆前轴中心点B（Xf，Yf）与后轴中心点A（Xr，yr）之间的关系可表示为：由图1可知，前轴中心点B的X、Y方向速度公式为：根据以上方程，经过一系列公式推导可得：通过积分可得：

由上式可知，后轴中心的运动轨迹是圆弧，也即是说在不考虑汽车打滑和侧向加速度的情况下，车辆低速行驶。车辆后轮轨迹只与车的轴距AL及转向角 有关，与车辆泊车速度无关[9]。

1.2关键点转弯半径求解

在车辆转向行驶时，路面对车辆轮胎产生横向阻力和摩檫力，会加快对轮胎的磨损。在车辆研发设计时，应保证汽车在转向时车轮仅作滚动运动，显然车身走过的圆弧线会交于一点，该交点0称为转向中心[10]。泊车动作是一种低速运动，阿克曼转向系統下的4个车轮分别围绕左、右转向系统的瞬时旋转中心滚动且不打滑。首先，计算出不同转弯半径，以E为后轨道中心，对车辆与关键点的转弯半径进行计算。

建立的阿克曼转向几何模型如图2所示，用于计算汽车的转弯半径。由图2可知，汽车后轴中心的转弯半径R为：汽车车头有角的转弯半径Rn为：汽车车后左角的转弯半径 为：

汽车车后右角的转弯半径 为：

1.3泊车路线推导

车辆在有泊车需求时，通过车位检测寻找车位，之后进入泊车初始位置。设泊车初始位置为A（Xr0，Yr0），以图3中泊车场景为例，首先将方向盘向右转一定角度（设角度大小为W，），使其以01为同心、R1为转弯半径作转向运动;然后方向盘向左转 角度，使其以Oz为同心、R2为转弯半径作转向运动;到达泊车终点时，回正汽车方向盘，此时车身航向角刚好为零[11]。

但车辆在按照既定路径规划行驶时，因受到空间限制，有可能会与周围车辆及车位障碍物发生碰撞：①泊车过程中，车辆右前端与前方障碍物发生碰撞;②泊车过程中，车辆后端与后方障碍物发生碰撞;③泊车过程中，车辆右后端与右侧车位边界障碍物发生碰撞[12]，如图4所示。

为方便描述，将图4（a）、（b）、（c）所示碰撞约束分别称为应满足的第一约束条件、第二约束条件和第三约束条件。

根据以上约束，对车辆每段运动轨迹进行推导。第三阶段泊车路线如图5所示。

为确保最后泊车成功，对整个泊车轨迹推导采用反向推导法。首先确定泊车终点位置，然后依次确定第三阶段和第二阶段运动轨迹。QWER代表车位矩形的4个边缘点，假设车辆在车位停好后，车辆后端与车库边缘距离为d。

根据图5，最优轨迹路线推导可列出如下约束关系式：将公式（7）带人公式（l0），可得：由第一约束条件知：OQ≥RFr。

将公式（II）带入第一约束条件中，得到：

根据第三约束条件，可知有一个最大值pman，p

据此，由公式（11）计算得到d的值，然后可以得出符合约束条件下车辆右轮转弯半径和车辆方向盘转角w2的大小[13]。

第二阶段泊车路线如图6所示。

由图6可知：

车辆在泊车起点时，由于汽车侧向速度为零，可得：由式（12）-式（14）得到：

综上可得w2与d值大小，再将一个确定的o带入公式（1）即可得到w1。显然，在起点坐标X最接近x值时，可得到一个最优的o和w1对于o和w1.的确定，主要是基于实际倒车起点最接近最优倒车起点，每次实际泊车过程中的起点与反向推导法推导出来的泊车起点肯定有一定误差，因此只要控制实际倒车起点与最优倒车起点的误差值最小即可[14]。

差值可表示为：

该阶段需要满足的约束条件如下：

（1）汽车方向盘转角必须小于最大方向盘转角，即

（2）汽车右前边角不能碰到车位边界，即 （△L为车辆轨迹距离车位左上角边界的最小值）。

如图5所示，在确定泊车起始点位置后，可首先将方向盘左打一定角度设为 ，设由点2到点3曲线运动时车辆方向盘转角为 ，汽车运动到点2时，汽车车身与X轴的夹角为 ，则可得到不同的平行泊车轨迹。在众多停车轨迹中，寻求最优停车路径。

在实际工程应用过程中，在成功识别车位后，车辆按照路径规划情况进行泊车入位，可能由于执行传感器精度不足或平行车位较短，在第三阶段泊车路径规划还未执行完成时，车辆就发出后方碰撞预警[15]，如图7所示。针对这种情况，提出更多一阶段的路径规划，即双向泊车路径规划，此时车辆需要向前行驶一段距离，以准确进入车位。

类似于三阶段路径推导方法，在考虑避障约束条件下，首先确定泊车终点位置。在约束条件下，为了尽量加大车辆与后方车位边界的距离，应使点3、4之间的水平位移尽量小。设水平距离为：

由此可知，由点3到点4，该阶段车辆应采用最大的方向盘转角 。

最后联立以上两式。

点2到点3轨迹的确定可参考三阶段泊车路径规划思想，在碰撞约束条件下进行最优泊车路径选择，由式（13）得到V2和f9：，接着使实际倒车起点与规划最优起点横坐标差值AX最小，计算得到，和的值，即为所求结果。

2仿真实例与实车验证

2.1路径规划仿真

针对已有车辆信息，基于实际车辆参数，分别对三阶段和四阶段路径规划进行平行泊车仿真。在Matlab仿真平台中，利用GUI模块T具，制作一个简易的上位机，在窗口中输入车辆信息和车位约束条件后，即可对完整的泊车过程进行仿真。若输入的约束条件不能满足泊车要求，则无法显示泊车过程，意味着泊车失败。分别采用三阶段和四阶段路径规划方式进行平行泊车仿真验证。如图8、图9所示，当车位长度为6.3m时，在单向三阶段路径规划下可以顺利泊车入位。当长度减少为6m时，进行单向路径规划时后方有碰撞隐患，无法继续执行下去，泊车过程失败。此时采用双向轨迹路径规划可以成功泊车人位。

综上所述，在同一泊车环境中，针对于长度不同的平行车位，车辆可按照推导轨迹安全进入泊车位，表明采用单向三阶段路径规划方法与双向四阶段路径规划方法均能满足车辆泊车入位要求，并且采用双向四阶段路径规划对泊车车位长度要求较低。

2.2实验验证

以某品牌SUV为实验平台，如图10所示。首先搭建软硬件系统平台，系统计算平台由工控机、控制器等设备组成。软件系统以OT与ROS平台为基础，建立白动泊车软件操作平台，通过OBD接口中的CAN信号实时监测车辆状态，获取车辆运动信息，并实现与汽车底盘系统的通讯。

工控机中集成了丰富的1/0接口，其内部运行程序主要是传感器数据处理和路径规划等。实验平台选择DSPACE公司的MicroAutoBox作为控制器，其包含丰富的CAN总线接口和模拟信号输入输出接口，能够运行控制程序并与底层控制系统相连，将控制信号发送到车辆底盘系统。

软件系统选择ROS（Rohot Operating Systein）系统作为操作平台，ROS也被称为机器人操作系统，但在车辆无人驾驶技术功能开发中，也会更多地用到这个平台。与嵌入式操作系统相比，ROS系统对进程间通信的实现更加容易。另外，其强大的调试工具也是智能驾驶开发者更倾向于选择使用它的一个重要原因[16]。

实际泊车过程如图12所示，车辆参数如表1所示，实车实验结果如表2所示。

通过仿真及实车验证，两者实验结果虽然存在一定误差，但大致能得出相同结论。在相同的泊车环境下，实验结果表明，采用双向四阶段泊车方式相比单向三阶段泊车方式对车位长度的要求缩短了0.26m，双向泊车规划方法有效减小了所需车位长度，从而放宽了泊车约束条件。因此，在平行泊车场景下，车辆能够适应的泊车条件也更为宽松。

3 总结与展望

本文通过研究平行泊车场景下的路径规划问题，基于反向推导法推导了平行泊车路径规划函数公式，详细分析了整个泊车过程碰撞约束条件。通过明确泊车轨迹中的碰撞约束，确定了约束函数及规划目标，进而推导得出整个泊车路径。对两种轨迹方法进行仿真及实车实验，结果表明，在相同的泊车环境下，双向泊车规划方法有效减小了所需车位长度。因此，在车位长度进一步减小的情况，可基于该思想推导出更多阶段的路径规划方法，对应泊车过程相对也会更加繁琐。值得注意的是，在实际应用中，考虑传感器及其它硬件设备T作误差，推导路径规划方法时，应适当增加车辆在车位内的安全距离余量[17]。另外，路径规划的工程实现还要考虑许多其它因素，如车辆行驶的平稳性以及对车辆传感器磨损影响程度等。在泊车实验时发现，本文提出的泊车方法中，在上一阶段转向下一阶段路径的衔接处，车辆行驶出现了中间停顿的情况。因此，针对该问题，算法仍存在需要改进的地方。

参考文献：

[1]搜狐网交管局：2019年上半年全国汽车保有量达2.5亿辆[EB/OL]. http：肌www.sohu.com/a/327997780_242272.

[2]新浪科技.李彦宏：AI虽然不能产生万物但是可以唤醒万物[EB/OL l.https： //tech.sina.cn/i/gn/201 9-07-03/detail-ihvtcitk9339362.d.html：l vt=4.

[3]SHIDIAN M. HAOBIN J.Research on automatic parking systemshased on parking scene recognition[J].IEEE Access， 2017，7：44-45.

[4]李红，王文军，李克强基于B样条理论的平行泊车路径规划[J]中国公路学报，2016，29（9）：143-151.

[5]穆加彩，张振东，管登诗，等.自动泊车系统路径规划与控制研究[J]软件导刊，2017（5）：117-121.

[5]PAROMTCHIK I E，LAUGIER C.Autonomous parallel parking andreturning to traffic maneuvers[J]. Intelligent Rnbotics andSvstem，1997.3：21-23.

[3]趙海兰，高松，孙宾宾，等全自动平行泊车路径规划方法研究[J].科学技术与工程，2017（7）：268-274.

[7]李大森.自主泊车系统关键技术研究[D].广州：华南理工大学，2014

[8]PIAO C H.ZHANG L. LL S，et al. Research on path planning of auto-matic p arking system[J] Advanced Materials Research. 2012， 590：416-420.

[9]宋金泽.自主泊车系统关键技术研究[D].长沙：国防科技大学，2009

[10]王道斌，梁华为，杨妮娜，等.两种自主泊车路径规划方法的对比研究[J]电子测量技术，2011，34（1）：27-30，38.

[II]LIU S， AN X. SHANG E.et al.A path planning method for assistantparallel car-parking[C].Computational Intelligence and Design（IS一CID）， 2012 Fifth International Symposium on. IEEE Computer Soci-etv.2012.

[12]郭夜啼多段式曲线拟合的平行泊车路径规划研究[D]兰州：兰州交通大学，2019.

[13]侯晓阳.基于路径规划的平行泊车系统研究[D].北京：清华大学，2015.

[14]吴中伟乘用车自动泊车系统路径规划与仿真分析[D]广州：华南理工大学，2018.

[15]VOROBIEVA H，CLASER S， MINOIL： -ENACHE N，et al. automaticparallel parking in tiny spots： path planning and control[J]. IEEETransactions on Intelligent Transportation Svstems. 2015. 16（1）：396-410

[16]高瑞金.基于毫米波雷达和视觉信息融合的自动泊车系统研究[D].上海：上海交通大学，2018

[17]侯晓阳，黄勇，孙思，等.多约束平行泊车路径规划[J].汽车技术，

2015（ 9）：52-56

（责任编辑：黄健）

收稿日期：2019-12-05

作者简介：王瑞（1992-），男，上海理工大学机械工程学院硕士研究生，研究方向为智能驾驶路径规划及电子控制;孙涛（1974-），男，博

士，上海理工大学机械工程学院副教授、硕士生导师，研究方向为汽车系统动力学及电子控制;赖杰（1984-），男，上海理工

大学机械工程学院教授级高级工程师，研究方向为汽车智能驾驶。