基于Matlab的多约束自动平行泊车轨迹规划

李红¹，郭孔辉^{1, 2}，宋晓琳¹，李飞龙¹

(1. 湖南大学 汽车车身先进设计制造国家重点实验室，湖南 长沙，410082；

2. 吉林大学 动态模拟国家重点实验室，吉林 长春，130022)

摘 要：

线特点，在已有轨迹函数基础上提出新的轨迹函数，通过对实际泊车轨迹的拟合证明该函数的可用性。分析泊车环境中存在的可能碰撞点，建立相应的避撞约束函数。以泊车环境障碍约束、车辆自身参数约束、泊车初始点方位约束、泊车终点位置约束为轨迹函数的约束函数，以泊车终点车辆与车位夹角最小为目标函数建立单目标多约束轨迹方程。分别对一般泊车环境和狭小空间泊车环境进行泊车轨迹规划，利用Matlab软件非线性约束优化函数求得轨迹函数参数。仿真结果表明：对于一般泊车环境，该方法能满足泊车轨迹曲率连续性，使车辆无碰撞进入车位，并使车辆与车位平行；对于狭小空间泊车环境，虽未能使车辆与车位平行，但实现了车辆无碰撞地泊车入位并保证轨迹曲率的连续性。由此可知该方法可实现车辆无碰撞地泊车入位，并满足轨迹曲率连续性要求，有效地解决了泊车过程中停车转向问题。

关键词：

轨迹规划；自动泊车；避撞；多约束；优化；

中图分类号：TP273          文献标志码：A         文章编号：1672-7207(2013)01-0101-07

Trajectory planning of automatic parallel parking with multi-constraints based on Matlab

LI Hong¹, GUO Konghui^{1, 2}, SONG Xiaolin¹, LI Feilong¹

(1. State Key Laboratory of Advanced Design and Manufacturing for Vehicle Body, Hunan University, Changsha 410082, China;

2. State Key Laboratory of Automobile Dynamic Simulation, Jilin University, Changchun 130022, China)

Abstract: Parking trajectory characteristics were analyzed, and a new trajectory function was proposed based on the known trajectory functions. It was proved that the new function was feasible for trajectory planning by parking trajectory fitting comparison. Potential collision points were analyzed and constraint equations were established accordingly. The orientation constraints at the start point, the location constraint at the parking point and the restriction of vehicle were considered, and the constraint equations were established respectively. Parking trajectory optimization function was presented to minimize the vehicle orientation angle at the parking point with restrictions mentioned above. The approach was used by trajectory planning for general parking space and tight parking space, and the trajectory function was solved using Matlab software. The simulation results show that the car can reverse into parking set safely in general parking space, with the continuously trajectory curvature and the car paralleled with the parking set at the parking point. This approach fails to make the car paralleled with the parking set in tight parking space, but the car can move into parking set safely with the continuous trajectory curvature. It is proved that the approach proposed can find a collision-free path in different parking spaces and meet the demand of continuous trajectory curvature, so the problem of shutting down to steer the wheel in parking process is solved.

Key words: trajectory planning; automatic parking; collision-free; multi-constraints; optimization

自动泊车技术(auto-parking system, APS)是智能汽车技术的重要组成部分，其目的是辅助或者替代驾驶员完成车辆的安全泊位。随着汽车行业的发展，乘用车辆的普及，城市泊车空间越来越小，增加了驾驶员泊车的困难，因此，自动泊车技术在汽车智能技术中愈来愈重要。国内外学者对自动泊车技术进行了研究，如：Mukherjee等^[1]对自动泊车的非完整约束路径规划进行了研究；Divelbiss等^[2]对车辆轨迹跟踪控制进行了研究；Chang等^[3]采用模糊逻辑对车辆进行了自动泊车控制；Paromtchik^[4]研究了自动泊车对转向及速度的要求；Zhao等^[5]基于模糊逻辑对狭小空间泊车鲁棒性进行了研究；Young等^[6]基于改进的模糊逻辑控制对未采用Odometry的自动泊车鲁棒性了研究；Lee等^[7]进行了基于轨迹跟踪控制器的泊车控制研究；张辉^[8]进行了轿车自动倒车转向控制几何推到算法和模糊逻辑算法研究；魏言华^[9]进行了基于视觉的车辆后方障碍物检测算法研究；宋金泽等^[10]融合动力学约束对自动平行泊车轨迹进行了研究；姜辉^[11]基于模糊逻辑对自动平行泊车转向控制器进行了设计；单恩忠^[12]基于微分平坦与样条理论进行了自主泊车轨迹规划研究。目前，只有少数汽车公司如大众、丰田、雷诺等已开发出相关的自动泊车产品，但仅仅安装到高档轿车中，而我国暂未有自主知识产权的自动泊车系统。路径规划是自动泊车系统中的重要组成部分，为此，本文作者对多约束情况下的泊车轨迹进行研究。

1  自动泊车系统工作流程

自动泊车系统工作过程如图1所示。自动泊车系统开启后，车辆沿道路缓慢行驶，通过传感器感知泊车环境信息，检测停车空间及环境障碍。若停车空间过小不能满足要求，则继续行驶；找到满足泊车空间要求的车位后，确定泊车初始点及目标位置，建立泊车轨迹坐标，根据环境障碍信息及车辆参数信息规划泊车路径。若规划成功，则提示驾驶员是否采取自动泊车；若不成功，则继续行驶，寻找下一个车位。泊车路径规划成功后，若驾驶员采用自动泊车，泊车系统控制根据传感器采集的车速及环境障碍信息控制方向盘完成泊车；若驾驶员不采用自动泊车，则自动泊车系统关闭。另外，自动泊车系统还需保证驾驶员可随时接管并控制车辆，以保证泊车过程中的安全性。

自动平行泊车的轨迹规划问题可描述如下：车辆的起始点与终止点之间找到1条曲率不大于1/R_min(R_min为车辆最小转弯半径)平滑的曲线，使车辆沿该轨迹运动时不与周围物体发生碰撞。为避免车辆在泊车过程中原地转向，要求曲线曲率连续；为避免车辆在泊车初始点原地转向，则要求曲线初始曲率尽量小甚至为0。车辆停放的规范性要求车辆应尽量保持与车位平行，故泊车轨迹曲线在终点处的切线与车位对称线夹角尽量小。泊车方式可根据车位方向分为平行泊车、垂直泊车和斜向泊车，根据泊车复杂程度可分为后退式泊车和多次进退式泊车，本文仅对后退式平行泊车进行研究。

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

Fig.1  Flow chart of parallel parking process

2  泊车轨迹规划

2.1  泊车轨迹函数

在自动泊车过程中，为避免停车转向造成的轮胎磨损，要求泊车轨迹曲线曲率连续，且轨迹起点曲率为0或尽量小，以降低车轮初始转向角；同时，泊车轨迹曲线起始点切线尽量保持与车位纵向线平行，以降低对车辆泊车初始方位角的要求。据文献[11]，选择Sigmoid函数与反正切函数为泊车轨迹函数：

             (1)

          (2)

式(1)和(2)均满足轨迹曲线曲率的连续性，但不能满足轨迹起点为小曲率或零曲率的要求，故出现原地转向问题，且轨迹初始点切线与车位纵向线夹角较大，车辆需具有较大的初始方位角才能成功进入停车车位。综合考虑反正切函数及多项式函数，采用origin软件自定义公式拟合功能编辑函数，对已有的泊车轨迹曲线进行拟合并改进，得出新的轨迹拟合函数：

       (3)

通过对某一泊车轨迹曲线拟合图像图可知：该轨迹函数更能满足泊车轨迹曲率连续性及初始点小曲率或无曲率的要求，且初始点切线与车位纵向线夹角较小。

图2  3个函数轨迹拟合结果对比

Fig.2  Fitting result of three functions

2.2  建立多非线性约束轨迹模型

轨迹规划是找到1条平滑连续的曲线，当车辆沿着这条曲线行驶时，绕过周围环境的障碍，安全地进入停车位中：因此，在进行轨迹规划时，首先应对泊车过程中的障碍约束进行分析。

选择后轴中点轨迹为本文泊车轨迹规划对象，以该点初始位置为坐标原点，则后轴中点轨迹终点为(s_o-L_r，h_o+W/2)，如图3所示。图3中：s_o为后轴中点距车位底线距离；h_o为后轴中点距障碍物横向距离；h为路宽；L_r为后轴中点距车辆后端距离；L_f为前轴中点距车辆前端距离；L为车辆轴距；W为车宽；L₁和L₂分别为车位长和宽；A，B，C和D分别为车辆右后端点、右前端点、左前端点和左后端点；θ为车辆方位角；φ为前轴中点等效转角。

图3  平行泊车参数示意图

Fig.3  Parameters of parallel parking process

由后轴中点轨迹函数可得泊车过程中车辆在任意点方位角θ及曲率ρ为：

              (4)

              (5)

由几何关系可得A，B，C和D随后轴中点移动轨迹为：

       (6)

       (7)

       (8)

       (9)

车辆与周围障碍有可能发生碰撞的位置共有4处：(a) 车辆泊车过程中与左侧道路边界的碰撞或者与对面行驶车辆间的碰撞；(b) 车辆进入车位前与右侧障碍物或者车辆的碰撞；(c) 车辆进入车位后右前端与右侧障碍物或者前方车位中的车辆的碰撞；(d) 车辆进入车位后右后端与右侧障碍物或者道路边界的碰撞。如图4所示。

图4  泊车过程中车辆可能碰撞点示意图

Fig.4  Possible collision points in parallel parking

为避免车辆与周围障碍物碰撞，轨迹函数须满足以下要求：

当x_A∈[0 s_o-L₁]时，y_A＜h_o；

当x_A∈[0 s_o-L_r]时，y_A＜h_o+L₂；

当x_B∈[0 s_o-L₁]时，y_B＜h_o；

当x_B∈[0 s_o-L_r]时，y_B＜h_o+L₂；

当x∈[0 s_o-L_r]时，y_C＞h_o+L₂-h；

当x=s_o-L_r时，y_C＞h_o；

当x=s_o-L_r时，y_D＞h_o；

为使轨迹曲线通过汽车起始点和终止点，须满足以下要求：

当x=0时，y=0；

当x=s_o-L_r时，y=h_o+L₂/2。

为避免车辆在泊车在初始点原地转向及方位角问题，须满足以下要求：

当x=0时，θ=0；

当x=0时，ρ=0。

为保正停车的规范性，轨迹终点方位角应很小最好为0。汽车参数约束如下。

由Ackerman关系可知：

               (10)

                 (11)

由式(5)，(10)和(11)可得前轴转角：

          (12)

                 (13)

            (14)

由式(13)~(14)可得：

              (15)

车辆性能的约束主要体现在2个方面：

(1) 最小转弯半径约束，即后轴中点轨迹曲率ρ≥1/R_min。

(2) 方向转角角速度约束，即。

因此，建立车辆参数约束：

当x∈[0 s_o-L_r]时，，且 。

以泊车终点方位角最小化为目标建立多非线性约束轨迹方程：

      (16)

3  仿真实例及分析

考虑实际自动操纵中传感器的测量误差，为保证泊车的安全性，选择安全余量^[13]，整车外形尺寸增加0.1 m，车辆参数及安全裕量见表1。建立泊车约束数学模型，据文献[14]，选择一般泊车环境和狭小空间泊车环境参数，并分别对其进行泊车轨迹规划，利用Matlab软件非线性多约束优化函数fmincon求得相应泊车轨迹函数参数，具体参数见表2；据仿真模型所得仿真结果见图5和图6。

表1  车辆参数

Table 1  Vehicle parameters

表2  环境参数及轨迹参数

Table 2  Parameters of parking environment and trajectory functions

图5  环境1中泊车轨迹及曲率

Fig.5  Parallel parking trajectory and curvature curves in the first condition

由图5所示的泊车轨迹可知：在整个泊车过程中，车辆轮廓线与周围环境障碍无交点，可证明车辆实现了无碰撞地进入车位，并且泊车结束后，车身与车位基本平行，满足了对自动泊车系统泊车规范性要求。由泊车轨迹曲率可知：轨迹曲率连续变化且初始点处曲率为0，解决了泊车过程中需停车转向的问题。由车辆最小转弯半径可知泊车轨迹曲率不大于1/R_min，即0.191。由轨迹曲率曲线可知：轨迹曲率在[-0.19,0.19]范围内，满足车辆最小转弯半径约束。由图6可知：由于泊车空间的限制，泊车结束后未能使车身与车位平行，但实现了无碰撞地进入车位，满足了避障约束要求。由轨迹曲率曲线可知：轨迹曲率连续变化，初始点处曲率为0.005 7 m^-1，即车辆初始转角为0.41°，可忽略不计，轨迹曲率在[-1.9,1.9]范围内，满足了车辆自身参数约束。因此可知：当泊车空间足够大时，均可通过上述方法求得一组函数参数构成1条满足车辆避障约束和车辆最小转弯半径约束条件的轨迹曲线，在实际泊车过程中，通过车辆纵向位置判断合适的方向盘转角，连续调节方向盘转角，使车辆后轴中点沿所规划的轨迹移动实现车辆安全进入车位，且具有规范泊车能力。

图6  环境2中泊车轨迹及轨迹曲率

Fig.6  Parallel parking trajectory and curvature curves in the second condition

4  结论

(1) 为避免泊车过程中停车转向现象对轮胎的磨损，在已有泊车轨迹函数基础上提出新的泊车轨迹函数，并通过对实车泊车轨迹曲线的拟合对比证明了曲线的可行性。

(2) 对泊车过程中可能碰撞点进行分析，通过车辆运动学关系建立避障约束函数。对车辆初始位置及方位角约束进行分析，建立位置及角度约束函数。根据车辆自身参数约束，建立泊车轨迹最小转弯半径约束函数。为满足泊车规范性，选择轨迹终点处车身与车位夹角最小为优化函数，建立单目标多约束泊车轨迹方程。

(3) 选择2种泊车环境，利用Matlab软件多非线性约束优化功能求解泊车轨迹方程，得到可行轨迹曲线。仿真结果表明：当泊车空间足够大时，通过该方法均能求得1条轨迹曲线可使车辆无碰撞地进入车位，且与车位保持平行停放；当泊车空间狭小时，仍能求得轨迹曲线，该曲线虽然不能保证车辆停放的规范性，但能保证车辆无碰撞地进入车位，满足了泊车安全性要求。

(4) 与圆弧—直线—圆弧轨迹规划方法相比，该方法得到的泊车轨迹曲率连续，解决了泊车过程中需原地转向的问题，降低了转向电机的工作负荷与轮胎磨损；与模糊逻辑泊车控制相比无需大量实验，节省时间提高了效率，可为自动泊车提供一定参考。

参考文献：

[1] Mukherjee R, Anderson D P. A surface integral approach to the motion planning of nonholonomic systems[J]. ASME Journal of Dynamic Systems, Measurement and Control, 1994, 116(9): 315-325.

[2] Divelbiss W A, Wen J T. Trajectory tracking control of a car-trailer system[J]. IEEE Transactions on Control Systems Technology, 1997, 5(3): 269-278.

[3] Chang S J, Li T H S. Design and implementation of fuzzy parallel-parking control for a car-type mobile robot[J]. Journal of Intelligent and Robotic Systems, 2004, 34(2): 175-194.

[4] Paromtchik I. Steering and velocity commands for parking assistance[C]//Proceedings of the 10th IASTED International Conference on Robotics and Applications. Honolulu, Hawaii, USA, 2004: 178-183.

[5] ZHAO Yanan, Jr Cdlins E G. Robust automatic parallel parking in tight space via fuzz logic[J]. Robotics and Autonomous Systems, 2005, 51: 111-127.

[6] Young W R, Se Y O, Senior M. Robust automatic parking without odomety using enhanced fuzzy logic controller[J]. IEEE International Conference on Fuzzy System. Vancouver B C, Canada, 2006: 521-527.

[7] Lee K, Kim D, Chung W, et al. Car parking control using a trajectory tracking controller[C]//SICE-ICASE International Joint Conference. Bexco, Korea, 2006: 2058-2063.

[8] 张辉. 轿车自动倒库转向控制几何推到算法和模糊逻辑算法研究[D]. 长春: 吉林大学汽车工程学院,2007: 11-57.

ZHANG Hui. Research on geometry algorithm and fuzzy logic for automatic garage parking of sedan[D]. Changchun: Jilin University. College of Automotive Engineering, 2007: 11-57.

[9] 魏言华. 基于视觉的车辆后方障碍物检测算法研究与实现[D]. 沈阳: 东北大学信息科学与工程学院, 2008: 19-59.

WEI Yanhua. Research and implementation of vehicle back-over obstacle detection algorithm based on vision[D]. Shenyang: Northeastern University. College of Information and Engineering, 2008: 19-59.

[10] 宋金泽, 戴斌, 单恩忠, 等. 融合动力学约束的自主平行泊车轨迹生成方法[J]. 中南大学学报: 自然科学版, 2009, 40(1): 135-141.

SONG Jinze, DAI Bin, SHAN Enzhong, et al. Trajectory planning approach of autonomous parallel parking with kinodynamic constraints[J]. Journal of Central South University: Science and Technology, 2009, 40(1): 135-141.

[11] 姜辉. 自动平行泊车系统转向控制策略的研究[D]. 长春: 吉林大学汽车工程学院, 2010: 25-105.

JIANG Hui. Research on strategies of automatic parallel parking steering control[D]. Changchun: Jilin University. College of Automotive Engineering, 2010: 25-105.

[12] 单恩忠. 基于微分平坦与样条理论的自主泊车轨迹规划研究[D]. 长沙: 国防科学技术大学机电工程与自动化学院, 2009: 32-61.

SHAN Enzhong. Research on trajectory planning of auto-parking based on differential flatness and spline theory[D]. Changsha: National University of Defense Technology. College of Mechatronic Engineering and Automatic, 2009: 32-61.

[13] Moritz O. Parkmate enhanced odometry[D]. Zurich: Swiss Federal Institute of Technology Zurich, Department of Information Technology and Electrical Engineering, 2004: 47-76.

[14] JGJ 100—98, 汽车库建筑设计规范[S].

JGJ 100—98, Design code for garage[S].

(编辑  陈灿华)

收稿日期：2011-11-28；修回日期：2012-03-15

基金项目：国家自然科学基金资助项目(51175159)；湖南省科技计划项目(2012GK3161)

通信作者：李红(1984-)，女，山东定陶人，博士研究生，从事汽车动态模拟与仿真研究；电话：15604465313；E-mail: lihong3729@163.com

摘要：分析泊车轨迹曲线特点，在已有轨迹函数基础上提出新的轨迹函数，通过对实际泊车轨迹的拟合证明该函数的可用性。分析泊车环境中存在的可能碰撞点，建立相应的避撞约束函数。以泊车环境障碍约束、车辆自身参数约束、泊车初始点方位约束、泊车终点位置约束为轨迹函数的约束函数，以泊车终点车辆与车位夹角最小为目标函数建立单目标多约束轨迹方程。分别对一般泊车环境和狭小空间泊车环境进行泊车轨迹规划，利用Matlab软件非线性约束优化函数求得轨迹函数参数。仿真结果表明：对于一般泊车环境，该方法能满足泊车轨迹曲率连续性，使车辆无碰撞进入车位，并使车辆与车位平行；对于狭小空间泊车环境，虽未能使车辆与车位平行，但实现了车辆无碰撞地泊车入位并保证轨迹曲率的连续性。由此可知该方法可实现车辆无碰撞地泊车入位，并满足轨迹曲率连续性要求，有效地解决了泊车过程中停车转向问题。