多约束双向自动平行泊车轨迹规划

李 红 郭孔辉，2 宋晓琳

1.湖南大学汽车车身先进设计制造国家重点实验室，长沙，410082 2.吉林大学汽车仿真与控制国家重点实验室，长春，130022

0 引言

随着汽车数量的逐年增加，车辆运行空间越来越小，狭小的泊车空间及有限的视野范围给泊车增加了难度，导致泊车过程中事故发生率不断提高。自动泊车系统为近年正在研发的新技术，该技术借助监视测距系统，控制车辆自动安全进入目标车位中，减轻了驾驶员泊车操作负担，降低了泊车事故发生，国内外各大汽车公司及专家针对泊车策略做了大量研究工作：Paromtchik等［1］采用回归算法，通过正弦曲线寻找安全的泊车路线，但其泊车空间要求较大；Ryu等［2］、Nejad等［3］、Lee等［4］、Chang等［5］分别基于模糊逻辑控制对泊车系统进行了研究；刘钰等［6］采用Bezier曲线对泊车轨迹进行了拟合；宋金泽等［7］应用微分平坦理论进行了轨迹规划。泊车策略研究可分为两种：①经验法，即利用模糊控制、神经网络等算法模拟驾驶员操纵行为，控制汽车进入车位；②路径规划法，即根据汽车碰撞约束等条件规划车辆泊车路径，控制车辆跟踪预定路径进入目标车位。

1 自动泊车系统工作流程

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

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

2 泊车轨迹规划

自动平行泊车轨迹规划问题可描述如下：车辆的起点与终点之间找到一条曲率不大于1／Rmin（Rmin为车辆最小转弯半径）的平滑曲线，当车辆沿该轨迹运动时不与周围物体发生碰撞并完全进入泊车车位中。泊车方式可根据车位方向分为平行泊车、垂直泊车和斜向泊车，根据泊车复杂程度可分为后退式泊车和多次进退式泊车，当泊车空间相对狭小后退式泊车方式不能使车辆安全进入车位时，需采用多次进退式泊车方法使车辆安全进入车位，因此对进退式平行泊车方式进行研究。

2.1 平移式双向泊车轨迹规划

平行式双向泊车轨迹采用平行移库模式，使车辆通过两次后退一次前进平行移入车位中，如图2所示，车辆挂入倒挡，方向盘右转，车辆沿圆弧FG行驶，然后方向盘左转，车辆沿圆弧GH行驶，车身与车位平行时停车，完成第一次平移，然后挂入前进挡，方向盘右转，车辆沿圆弧HL行驶，然后方向盘左转，车辆沿圆弧LM行驶，车身与车位平行时停车，完成第二次平移，最后挂入倒挡以调整车辆与前、后车位车辆的距离。

图2 平移式双向泊车示意图

当后轴为非转向轴时，由后轴中点轨迹及轨迹各点切线可确定车辆车身方位角，由车身结构参数可得到车身各点轨迹曲线，故选择车辆后轴中点轨迹为研究目标，图2中，O1、O2、O3、O4分别为圆弧FG、GH、HL、LM圆心。AoBoCoDo分别为车辆终点位置，A1B1C1D1为车位。

记E为后轴中点，A、B、C、D分别为车辆轮廓右后端、右前端、左前端、左后端，F为车辆后轴中点轨迹泊车起始点。已知车辆长La，宽W，轴距为Lo，前悬架长Lf，后悬架长Lr，车辆最小转弯半径Rmin，泊车初始位置时后轴中点E分别与车位线A1D1纵向距离so，与车位线C1D1侧向距离ho，车位宽度A1D1及道路宽度为h，r1、r2、r3、r4，θ1、θ2、θ3、θ4分别为圆弧FG、GH、HL、LM的半径和圆心角，Lc和Lk分别为车位长和宽，由图2可知：θ1＝θ2，θ3＝θ4。选择r1、r2、r3、r4、θ1、θ3为轨迹曲线变量，D1点为坐标原点，由几何关系知：

（1）F点坐标为（so，ho）；

（2）O1点坐标（x1，y1）＝（so，ho－r1）；

（3）O2点 坐 标 （x2，y2）＝ （x1－ （r1＋r2）sinθ1，y1＋ （r1＋r2）cosθ1）；

（4）O3点坐标（x3，y3）＝（x2，y2－r2－r3）；

（5）O4点 坐 标 （x4，y4）＝ （x3＋ （r3＋r4）sinθ3，y3＋ （r3＋r4）cosθ3）；

（6）C1点坐标（C1x，C1y）＝（Lc，0）；

（7）B1点坐标（B1x，B1y）＝（Lc，－Lk）；

（8）H点坐标（Hx，Hy）＝（x2，y2－r2）；

（9）M点坐标（Mx，My）＝（x4，y4－r4）；

泊车过程中，车辆与周围障碍物碰撞情况如下：①车辆左前端与左侧道路边界或对面行驶车辆碰撞；②车辆右侧与前车位碰撞；③ 完成第一次平移时车辆后端与后方障碍物碰撞；④第二次平移时车辆前端与右侧障碍物碰撞；⑤完成第二次平移时车辆前端与前方障碍物碰撞；⑥泊车结束时车辆未完全进入车位，如图3所示。

图3 平移式双向泊车碰撞示意图

为保证车辆无碰撞地完全进入车位，车辆轨迹需满足以下要求。

（1）当车辆沿圆弧FG行驶时，由图3a可知，要求C点轨迹半径O1C满足：

（2）由图3b可知，车辆A点轨迹半径O1A及沿圆弧GH行驶时车辆B点轨迹半径O2B需满足：

（3）当车辆到达H点时，由图3c知，车辆后端D点x向坐标Dx需满足：

（4）当车辆到达点L时，由图3d知，车辆在点L处B点y向坐标需满足：

（5）当车辆沿圆弧LM行驶，到达M点时，由图3e、图3f知，需满足：

（6）车辆最小转弯半径约束要求：r1≥Rmin，r2≥Rmin，r3≥Rmin，r4≥Rmin。

由上述分析建立轨迹约束函数：

当求解车辆可实现泊车的最小车位长度时，车辆起始位置已知，车位尺寸未知，则以r1、r2、r3、r4、θ1、θ3、Lk为变量，以Lc最小化为目标，利用MATLAB的非线性优化功能求解式（8）；当进行泊车轨迹规划时，车辆起始位置及车位尺寸已知，为减小泊车过程中转向的繁忙程度，以方向盘转角最小化为目标，以r1、r2、r3、r4最小值的最大化为目标，求解式（8）。

2.2 自由式双向泊车轨迹规划

自由式双向泊车轨迹如图4所示，车辆挂入倒挡，方向盘右转，车辆沿圆弧FG行驶，然后方向盘左转，沿圆弧GH行驶，车身与车位具有一定夹角时挂入前进挡，右转方向盘沿圆弧HL行驶，车身与车位平行时停车，最后再挂入倒挡调整车辆与前后车位车辆的距离。

选择车辆后轴中点轨迹为研究目标，由后轴中点轨迹切线得到车身方位角，结合车辆结构参数可得到车辆各端点轨迹，如图4所示，O1、O2、O3分别为圆弧FG、GH、HL的圆心，AoBoCoDo分别为车辆泊车终点，A1B1C1D1为车位。

图4 自由式双向泊车示意图

记E为后轴中点，A、B、C、D分别为车辆轮廓右后端、右前端、左前端、左后端，F为车辆后轴中点轨迹泊车起始点。已知车辆轴距Lo，前悬架长Lf，后悬架长Lr，车宽W，车辆最小转弯半径Rmin，泊车初始位置时后轴中点分别与车位线A1D1纵向距离so，车位线C1D1侧向距离ho，车位宽度A1D1，道路宽度为h，r1、r2、r3、θ1、θ2、θ3分别为圆弧FG、GH、HL半径与圆心角，Lc和Lk分别为车位长与宽，由图4知θ1＝θ2＋θ3。选择r1、r2、r3、θ1、θ3为变量，选择D1点为坐标原点，由图4几何关系可得：

（1）F点坐标为（so，ho）；

（2）O1点坐标（x1，y1）＝（so，ho－r1）；

（3）O2点 坐 标 （x2，y2）＝ （x1－ （r1＋r2）sinθ1，y1＋ （r1＋r2）cosθ1）；

（4）O3点 坐 标 （x3，y3）＝ （x2＋ （r2＋r3）sinθ3，y2－ （r2＋r3）cosθ3）；

（5）C1点坐标（C1x，C1y）＝（Lc，0）；

（6）B1点坐标（B1x，B1y）＝（Lc，－Lk）；

泊车过程中，车辆与周围障碍物碰撞情况如下：①车辆左前端与左侧道路边界或对面行驶车辆碰撞；②车辆右侧与前车位碰撞；③ 车辆左后端与后方障碍物碰撞；④车辆右后点与后侧障碍物碰撞；⑤车辆前端与前方障碍物碰撞；⑥车辆最终未能完全进入车位。

为避免车辆与周围障碍物发生碰撞，且能完全进入车位，车辆轨迹需满足以下要求。

（1）当车辆沿圆弧FG行驶时，由图5a可知，车辆C点轨迹半径O1C需满足：

（2）当车辆沿圆弧FG和GH行驶时，由图5b可知，沿圆弧FG行驶时车辆A点轨迹半径O1A和沿圆弧GH行驶时车辆B点轨迹半径O2B需满足：

（3）当车辆沿圆弧GH行驶到达H点时，由图5c、图5d知，D点x向坐标及A点y向坐标需满足：

车辆沿圆弧GH行驶时A点与D点轨迹半径O2A和O2D的表达式为

（4）当车辆沿圆弧HL行驶，由图5e、图5f知，L点处B点x向坐标Bx、L点y向坐标Ly需满足：

图5 自由式双向泊车碰撞示意图

（5）车辆最小转弯半径约束要求：r1≥Rmin，r2≥Rmin，r3≥Rmin。

由上述分析建立轨迹约束函数：

求解车辆所需最小车位长度，车辆起始位置已知，车位尺寸未知，则以r1、r2、r3、θ1、θ3、Lk为变量，以Lc最小化为目标，求解非线性约束函数式（16）；进行泊车轨迹规划时，车辆起始位置及车位尺寸已知，为减小泊车过程中转向的繁忙程度，以方向盘转角最小化为目标，即以r1、r2、r3最小值的最大化为目标，求解式（16）。

3 仿真实例及分析

车辆参数如下：轴距Lo＝2.5m，前悬架长Lf＝0.9m，后悬架长Lr＝0.8m，车宽W＝1.6m，最小转弯半径Rmin＝4.7m。

有限的停车场所空间随着车辆数量的增加显得逐渐拥挤，若泊车过程中车辆所需车位较小，不但可提高停车场所的空间利用率，还可提高车辆泊车过程中的安全性，因此本文对平移式平行泊车和自由式平行泊车两种泊车方式所需最小车位进行了研究，参考文献［8］选择道路宽度为4m，车辆处于起始位置时，so＝8m，ho＝2m，根据上述分析对两种泊车方式所需最小车位进行求解。由求解结果可知平移式双向泊车方法可实现的最小车位长5.86m宽2.10m，自由式双向泊车方法可实现的最小车位长5.28m宽1.93m，两种方法各自最小车位泊车轨迹参数见表1，泊车轨迹见图6、图7。

当其他参数不变，车位尺寸固定，车位长6.2m、宽2.5m时，由前述轨迹规则方法求解泊车轨迹参数，求解结果见表2，泊车轨迹见图8、图9。

表1 最小车位轨迹参数

图6 平移式双向最小车位平行泊车轨迹

图7 自由式双向最小车位平行泊车轨迹

表2 轨迹规划轨迹参数

图8 车位尺寸固定时平移式双向平行泊泊车轨迹

由图6～图9可知，在相同泊车环境中，车辆沿着两条轨迹经过一退一进进入车位，且与周围障碍物无交点，表明了两种方法均能在满足车辆最小转弯半径约束条件下使车辆安全进入车位；对比图6与图7知，与自由式双向平行泊车方法相比，平移式双向平行泊车方法所需车位空间大，平移式泊车方法所需最小泊车车位长5.86m宽2.10m，自由式泊车方法所需最小车位长5.28m宽1.93m；由表2可知，相同泊车环境及车位尺寸时，自由式泊车方法所需方向盘转角较小，因此可减小泊车系统转向电机的工作负荷。综上可知，自由式泊车方法更具安全性与实用性。另外，两种轨迹规划方法仅规划了车辆一次后退与一次前进的轨迹，在实际泊车过程中，当车辆完成一退一进过程后，还需一次前进或后退来调节车辆与前后车位的距离，该操作相对简单，因此未进行研究。

图9 车位尺寸固定时自由式双向平行泊车轨迹

4 结论

（1）泊车空间过小不能满足车辆最小转弯半径约束及环境避障约束无法实现后退式平行泊车时，需采用进退式泊车即双向平行泊车。分析泊车轨迹特点，提出两种双向平行泊车轨迹：平移式双向平行泊车轨迹与自由式双向平行泊车轨迹。确定两种泊车轨迹规划的变量、规划目标及碰撞约束函数。

（2）利用MATLAB软件非线性约束函数优化功能求解泊车轨迹方程，并对两种泊车轨迹进行仿真。结果表明相同泊车环境下，两种轨迹规划方法均可实现小车位安全泊车，通过比较可知，自由式双向平行泊车轨迹规划法在满足车辆参数约束及碰撞约束的条件下能实现相对更小的泊车车位；对同一泊车环境进行轨迹规划，自由式双向平行泊车方法能更轻便地完成泊车。

（3）考虑传感器测量精度及车辆系统的响应滞后特性，对实车进行轨迹规划时，应给车辆尺寸增加合适的安全余量后对车辆进行泊车轨迹规划，车辆在轨迹圆弧节点处受方向盘转向电机转速及泊车车速的影响，需适当增加车位尺寸以提高泊车系统的泊车安全性。另外，车辆沿轨迹行驶完全进入车位后，还需根据前后雷达自动调整车辆与前后障碍距离。由于传感器测量精度及车辆动态特性，在泊车过程中车辆位置估算存在一定误差，轨迹规划研究结束后还需对车辆方位估算进行研究，为泊车系统路径跟踪控制研究作准备。

［1］Paromtchik I E，Laugier C.Autonomous Parallel Parking and Returning to Traffic Maneuvers［C］／／Proceedings of the 1997IEEE／RSJ International Conference on Intelligent Robot and Systems.Grenoble，France，1997，3：21－23.

［2］Ryu Y W，Oh S Y，Kim S Y.Robust Automatic Parking without Odometry Using an Evolutionary Fuzzy Logic Controller［J］.International Journal of Control，Automation and Systems，2008，6（3）：434－443.

［3］Nejad M K.Fuzzy Logic Based Autonomous Parallel Parking of a Car－like Mobile Robot［D］.Montreal：Concordia University，2006.

［4］Lee J Y，Lee J J.Multiple Designs of Fuzzy Control for Car Parking Using Evolutionary Algorithm［C］／／Proceedings of International Conference on Mechatronics.Kumamoto，Japan，2007：1－6.

［5］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.

［6］刘钰，马艳丽，李涛.基于Bezier曲线拟合的自主平行泊车轨迹模型仿真［J］.科技导报，2011，29（11）：59－61.Liu Yu，Ma Yanli，Li Tao.Parallel Parking Path Generation Based on Bezier Curve Fitting［J］.Science ＆ Technology Review，2011，29（11）：59－61.

［7］宋金泽，戴斌，单恩忠，等.融合动力学约束的自主平行泊车轨迹生成方法［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 Technoloty），2009，40（1）：135－141.

［8］北京市市政设计研究院.CJJ37－90，城市道路设计规范［S］.北京：中国建筑工业出版社，1991.