多约束平行泊车路径规划

侯晓阳 黄勇 孙思 季学武

（清华大学 汽车安全与节能国家重点实验室）

多约束平行泊车路径规划

侯晓阳 黄勇 孙思 季学武

（清华大学 汽车安全与节能国家重点实验室）

针对不能满足平行泊车要求的狭小车位，提出了单向多次和双向多次路径规划方法。在确定了两种泊车轨迹的变量、碰撞约束函数以及规划目标后，利用matlab软件的非线性约束函数的优化功能求解泊车轨迹方程，并对两种路径规划方法进行仿真试验，结果表明，在相同的泊车环境下，两种路径规划方法都可实现安全顺利的泊车入位；通过实车试验进一步验证了所提出的路径规划方法的安全性和有效性。

1 前言

随着汽车保有量的增加，随之带来的问题也日益凸显。其中，最为突出的就是停车难问题，而自动泊车系统可以很好地解决这一问题，因此其已成为汽车行业的研究重点。平行泊车理论研究的关键部分是路径规划阶段，对此国内外学者已经进行了大量研究，研究方法多采用两圆弧相切或圆弧切直线的路径规划方法，并且路径规划的结果需要满足环境约束[1～5]。虽然这些路径规划方法可满足平行泊车的基本需要，但是由于实际泊车过程中传感器测量的误差以及机械机构操作误差等，会导致车辆无法按照预定轨迹泊车入位，而两圆弧相切法与圆弧切直线法无法解决这一问题。

为解决上述问题，本文通过分析在误差产生时无法正常泊车入位的约束条件，提出双向与单向路径规划方法，在减少泊车位长度的同时保证了泊车过程顺利进行。

2 自动平行泊车系统介绍

自动平行泊车系统的工作流程如图1所示。系统开启后，位于车辆前方的超声波传感器开始寻找合适的泊车位；如果找到合适的泊车位，则根据车辆的位置关系建立泊车轨迹坐标系，并根据周围的环境以及车辆参数进行路径规划；路径规划完成后，系统提示驾驶员进行下一步操作；如果驾驶员采用自动泊车，则系统会根据传感器采集到的车速以及环境信息控制转向盘完成泊车。首次泊车结束后，若车辆未能顺利泊车入位，则根据车辆的位置信息进行二次路径规划并泊车入位，直到达到最佳泊车位置。如果驾驶员不采用自动泊车，则泊车系统自动退出。此外，泊车系统还需保证在泊车过程中驾驶员能随时接管转向盘以保证泊车过程中的安全性。

3 自动平行泊车路径规划

3.1 单向路径规划平行泊车

在实际泊车过程中，会出现无法按照理论上的路径规划过程完全地泊车入位情况，如图2所示。由图2可看出，车辆在图示位置进行倒车入位过程中，理论倒车轨迹为圆弧EN段与圆弧NA段，由于在实际操作过程中传感器的测量误差及驾驶员的操作误差，车辆如果按照理论情况进行泊车入位，将导致车辆在B点与后方障碍物发生碰撞，因此圆弧AB段泊车轨迹无法实现，车辆无法按照规划好的泊车轨迹泊车入位，泊车过程无法完成。此时，需要进一步的路径规划才能完成车辆泊车入位。

为解决上述自动泊车问题，提出了单向路径规划方法。单向路径规划采用平移库模式，使车辆通过1次前进就可顺利进入泊车位中。由图2可知，由于AB段的泊车轨迹无法继续进行，所以当车辆在B点时不能继续倒车入位。假设车辆在B点时的驶过角为θ，此时可采用图3所示路径规划方案进行泊车。首先转向盘右转并挂前进挡，车辆沿圆弧BC行驶到C点，然后转向盘左转使车辆沿圆弧CD行驶到D点，最后车辆挂入倒挡以调整车辆与前、后障碍物距离。

由于图3中的路径规划方案并没有考虑碰撞约束，因此需要在碰撞约束条件下分析该方案的可行性。记车辆后轴中点为E，车辆轮廓右后端、左后端、右前端、左前端分别为F、G、H、I。假设作为障碍物的前、后车辆与所泊车辆的参数（表1）相同，则：A点坐标为（a，0）；O1点坐标为（a，R）；B点坐标为（a+Rsinθ，R（1-cosθ））；O2点坐标为(a+2Rsinθ，R（2cosθ-1））；C点坐标为（a+3Rsinθ，R（1-cosθ））；D点坐标为（a+4Rsinθ，0）；O3点坐标为（a+ 4Rsinθ，R）。

表1 前、后车辆与所泊车辆的参数

在单向路径规划时，车辆与周围障碍物可能发生碰撞的情况有如下几种：泊车入位时车辆左后端与后方障碍物发生碰撞；泊车入位时车辆右后端与右方障碍物发生碰撞；泊车入位时车辆前端与前方障碍物发生碰撞；泊车入位时车辆右前方与右侧障碍物发生碰撞，如图4所示。

为避免在泊车过程中车辆与周围障碍物发生碰撞并能安全进入车位，车辆的泊车轨迹需满足以下要求：

a.当车辆沿圆弧AB行驶时，由图4a可知，车辆左后方G点的约束条件为：

式中，Gx为G在点X方向坐标。

b.当车辆沿圆弧AB行驶时，由图4b可知，车辆右后方F点的约束条件为：

式中，Fy为F点在Y方向坐标。

当Fy取最小值时，

c.当车辆沿圆弧CD行驶时，由图4c可知，车辆前方的约束条件为：

式中，Dx为D点在X方向坐标。

d.当车辆沿圆弧CD行驶时，由图4d可知，车辆右前方H点的约束条件为：

当Hy取最小值时：

式中，Hy为H点在Y方向坐标。

由上述分析可以建立规划轨迹的约束函数：

在实际求解车位长度中，车辆初始位置已知，车位尺寸未知，则以θ和Wa作为变量，以Le最小化作为目标，利用matlab的非线性优化功能求解式（10）。

3.2 双向路径规划平行泊车

在工程实践中，当使用两圆弧相切法进行路径规划时，若取圆弧半径为车辆的最小转弯半径，则由于电动助力转向系统（EPS）控制器设置有过流保护模式，将导致转向盘无法长时间处于最大转角，因此在使用两圆弧相切法进行路径规划时，一般不使用最小圆弧半径进行车辆转弯动作，这就导致所需最小泊车位长度明显增加，而双向路径规划可有效减小所需的泊车位长度。

图5为双向路径规划方案。取后轴中点为参考点，由于AB段的泊车轨迹无法继续进行，所以当车辆在B点时不能继续倒车入位。此时，假设车辆的驶过角为θ，根据几何关系可得到圆弧AB段的圆心角为θ。首先车辆沿圆弧BC段行驶，并驶过对应圆心角为2θ的弧长后，左打转向盘，车辆沿圆弧CD1段行驶，并驶过对应圆心角为θ的弧长后，挂倒挡右打转向盘，驶过对应圆心角为β的弧长后，再左打转向盘驶过对应圆心角为β的弧长到达D2点，最后转向盘回正并在车位中调整车辆与前、后车间距。车辆在BC与D1D2段行驶时的转弯半径为R，在CD1段行驶时的转弯半径为R1。

图5中，O1、O2、O3、O4、O5分别为各圆弧的圆心；A点坐标为（a，0）；O1点坐标为（a，R）；B点坐标为（a+Rsinθ，R（1-cosθ））；O2点坐标为（a+2Rsinθ，R（2cosθ-1））；C点坐标为（a+3Rsinθ，R（1-cosθ））；O3点坐标为（a+3Rsinθ+R1sinθ，R（1-cosθ)）+R1cosθ）D1点坐标为（a+3Rsinθ+R1sinθ，R（1-cosθ）-R1（1-cosθ））；O4点坐标为（a+3Rsinθ+R1sinθ-2Rsinβ，R）；D2点坐标为（a+ 3Rsinθ+R1sinθ-2Rsinβ，0）；O5点坐标为（a+3Rsinθ+R1sinθ，-Rcosθ-R1（1-cosθ））。

与前述相同，在碰撞分析时只需考虑二次路径规划时可能发生的碰撞，碰撞情况与单向路径规划时相同。除第3种碰撞情况外，其它3种情况的碰撞分析关系式与前述相同，为此仅对第3种碰撞情况进行分析。

当车辆沿圆弧BC与圆弧CD1行驶时，由图4c可知，车辆前方约束条件为：

式中，D2x与Bx分别为D点与B点在x方向坐标。

除此之外，根据车辆本身的约束条件，可以得到约束函数：

由上述分析可建立二次规划轨迹约束函数：

在实际求解车位长度中，车辆初始位置已知，车位尺寸未知，则以θ、β和Wa作为变量，以Le最小化作为目标，利用matlab的非线性优化功能进行求解式（13）。

4 仿真实例与工程应用

4.1 仿真实例与分析

利用软件matlab进行仿真试验，试验用车辆参数为：车长4.4 m，车宽1.8 m，轴距2.7 m，后悬长0.5 m，最小转弯半径4.2 m，障碍物宽度取为2.5 m。根据式（10）与式（13）得出仿真参数如表2所列。

表2 仿真参数

在未考虑车辆与前、后方障碍物所需间隙的情况下，如果未采用单向路径规划，得出的最小车位长度为6.1 m，如果采用单向路径规划平行泊车进行泊车入位，得出的最小车位长度为5.8 m，仿真结果如图6和图7所示。

在路径规划时，如果未采用双向路径规划平行泊车，则所需泊车位长度为7.3 m，如果采用双向路径规划平行泊车，则所需泊车位长度为7 m，仿真结果如图8和图9所示。

由图7和图9可看出，在相同泊车环境下，车辆沿泊车轨迹进入泊车位中，且与周围障碍物无交点，表明单向路径规划方法与双向路径规划方法均能在满足车辆最小转弯半径的情况下使车辆安全入位。此外，双向路径规划所需转向盘转角较小，因此可减少泊车系统EPS的工作负荷。

综上所述，单向路径规划对泊车空间要求较小，但是对EPS系统的要求较高；双向路径规划对泊车空间要求较高，但是对EPS系统的要求较低。在实际运用中，可以根据EPS系统的使用限制选择合适的泊车方法进行泊车。

4.2 工程实例

以景逸X5为试验车，硬件系统以XC164为基础，软件系统以Dave与Keil为基础，建立自动泊车系统，通过OBD接口中的CAN信号实时监测车辆状态，获取车辆运动信息，并实现与EPS系统通讯，如图10所示。实际泊车过程中，利用超声波传感器测得车辆与侧方障碍物距离并计算得到泊车起始点信息与泊车过程中拐点信息，由单片机计算出车辆行驶的实际距离并与理论距离进行比较来控制转向盘转动的时机，最后根据电子罗盘中航向角信息进行泊车过程路径规划。

试验采用与表2相同的参数，试验结果如表3所列。由于在实际泊车过程中，车辆与前、后方障碍物留有间隙，因此所需车位长度比理论值大。由表3可知，使用2种路径规划方案后可有效降低泊车位所需长度。

表3 实车试验结果 m

5 结束语

本文通过采用单向路径与双向路径规划方法，有效解决了车辆无法按照预定轨迹泊车入位问题。在确定了2种泊车轨迹的变量、碰撞约束函数以及规划目标后，利用matlab软件的非线性约束函数的优化功能求解泊车轨迹方程，并对2种轨迹方法进行仿真试验，结果表明，在相同的泊车环境下，2种轨迹规划方法都可实现安全顺利的泊车入位。在实际工程运用中，考虑到传感器、单片机以及EPS系统在工作过程中会产生误差，对实际车辆进行路径规划时，应根据车辆尺寸增加合适的安全余量。实践表明，本文所述路径规划方法可在工程中实现。

1 中汽协会行业信息部.2013年汽车工业产销情况简析.中国汽车工业协会官网,2013.

2 I.E.Paromtchik,C.Laugier.Autonomous parallel parking and returning to traffic maneuvers.Intelligent Robotics and System,1997,3:21-23.

3 C.Laugier,Th.Fraichard.Sensor-based control architec⁃ture for a car-like vehicle.Intelligent Robotics and System, 1998,1:216-222.

4 Richard M.Murray,S.Shankar Sastry.Steering nonholonom⁃ic systems using sinusoids.IEEE proceeding of 29thConfer⁃ence on Decision and Control,1990:2097-2101.

5 Derrick H.Nguyen,Bernard Widrow.Neural Neural net⁃works for self-learning control sysyems.IEEE Intelligent Vehicle Symposium,1990:450-454.

6 Derrick H.Nguyen,Bernard Widrow.The truck back-upper an example of self-learning Inneural network.Proc.Int.Joint.Neural Networks,1989,7:357-363

7 I.E.Paromtchik,C.Laugier.Autonomous parallel parking of nonholonomomic Vehicle.Proceedings of the IEEE Intel⁃ligent Vehicle Symposium,1996:3117-3122

8 I.E.Paromtchik,C.Laugier.Motion generation and control for parking an autonomous vehicle.Proceedings of the IEEE international Conference on Robots and Automation,1996: 13-18

9 宋金泽.自主泊车系统关键技术研究：[学位论文].湖南：国防科技技术大学研究生院,2009.

（责任编辑文 楫）

修改稿收到日期为2015年5月27日。

Multi-route Planning for Parallel Parking System with Multi-constraints

Hou Xiaoyang,Huang Yong,Sun Si,Ji Xuewu
（State Key Laboratory of Automotive Safety and Energy,Tsinghua University）

For the narrow parking space which can not satisfy the requirement of parallel parking,we present two route planning methods:one-way multiple-route planning method and two-way multiple-route planning method.When variables,collision constraint function and planning objective of two parking trajectories are defined,we make use of optimization function of the nonlinear constraint function in MATAB to solve the parking trajectory equation,and these two route planning methods are simulated and tested,which show that,in the same parking environment,both of two route planning methods can make the car park safely with smaller parking length;safety and validity of the presented route planning methods are verified with vehicle test.

Auto parallel parking，Route planning，Constraint function

自动平行泊车 路径规划 约束函数

U471.1

A

1000-3703（2015）09-0048-05