基于动态窗口和绕墙走的自动垂直泊车轨迹规划

基于动态窗口和绕墙走的自动垂直泊车轨迹规划
郭奕璀;蒋涛;张葛祥
【摘要】针对现有自动垂直泊车轨迹规划因需要轨迹数学模型而灵活性差和采用开环离线规划而无法动态调整路径的问题,提出一种基于动态窗口和绕墙走策略的垂直泊车轨迹规划方法.将轨迹规划问题解耦为与时间无关的路径规划和与时间相关的速度规划,并在分析车辆阿克曼转向特性的基础上,通过绕墙走策略实现无先验轨迹模型的路径规划.同时将绕墙走路径作为全局启发信息,用于驱动基于模型预测控制的动态窗口法进行速度规划,其反馈优化特点能够对路径进行局部动态调整.仿真实验结果表明,该方法能够在控制、定位精度0.2 m的情况下,安全有效地完成车辆垂直泊车,且能动态地对车辆路线进行局部调整.%Existing methods of traj ectory planning for automatic vertical parking are lack of flexibility.This is due to the fact that the existing mathematical models for traj ectory planning cannot adj ust the paths dynamically because of open-loop and off-line planning.Therefore,a method for traj ectory planning of automatic vertical parking based on dy-namic window and wall following approach is developed.The traj ectory planning problem is first decomposed into a time-free path planning process and a time-related velocity planning process.Then,a wall following strategy is introduced to a-chieve traj ectory planning without a priori traj ectory model,based on an analysis of the Ackermann steering characteris-tics of vehicles.Meanwhile,the wall-following path is used as global heuristic guidance for propelling the dynamic window for speed planning.The feedback and optimization of two processes will dynamically adj ust path planning in each step. The results
of a simulation experiment shows that the introduced method can faciliate safe and effective parking,and is capable to make local adjustments dynamically,under an observed accuracy of controlling and location within 0.2 m.
【期刊名称】《交通信息与安全》
【年(卷),期】2017(035)001
【总页数】6页(P92-97)
【关键词】智能驾驶;自动泊车;轨迹规划;垂直泊车;绕墙走策略;动态窗口法
【作者】郭奕璀;蒋涛;张葛祥
【作者单位】西南交通大学电气工程学院成都 610031;成都信息工程大学控制工
程学院成都 610225;西南交通大学电气工程学院成都 610031
【正文语种】中文
【中图分类】TP273
日益拥挤的泊车环境和狭小的车位，要求驾驶员泊车时更加娴熟地对车辆进行操纵。

自动泊车系统能够辅助或者代替驾驶员进行泊车，减轻泊车操控难度，提高泊车安全性。

自动泊车系统一般主要由感知子系统、轨迹规划子系统、轨迹跟踪控制子系统组成，轨迹规划作为自动泊车系统规划决策层的核心技术，是整个自动泊车系统智能化程度的体现，具有重要的研究意义。

轨迹规划技术主要在考虑车辆的非完整性约束等运动学特性的基础上进行规划[1]。

Kim等[2]在已知Reeds-Shepp路径组成的前提情况下，利用Minkowski Sum
在生成的无碰撞区域内选取最优路径，但这种方法没有考虑线速度等动力学约束。

文献[4-6]通过采用几何轨线将泊车轨迹用带参数的数学模型表示，同时将障碍物
以及汽车运动学性能进行约束化，从而利用非线性规划离线生成最优轨迹。

Zips
等[7-8]通过龙格库塔法将路径离散化，在每一个路径离散点利用SQP方法进行非线性规划，虽然能够动态规划实时生成轨迹，但其借鉴了人类泊车经验的启发将泊车过程进行两步划分以简化轨迹规划。

文献[9]虽然利用连续曲率的曲线进行狭小
范围内的泊车轨迹规划，但是同样是借鉴了先验知识将泊车分解为两个阶段。

驶神经网络[10-11]和模糊控制器[12-14]虽然不直接进行轨迹规划，但其仿熟练驾驶员的规则本身包含了许多用于参考的轨迹先验信息，对泊车场景具有一定的要求，缺少灵活性。

文献[15]通过环视相机进行了地图构建，但在规划方法上没有进行改进。

上述方法或者需要轨迹数学模型，或者采用开环离线规划方式，缺乏灵活性、不能动态调整路径。

由此，笔者提出了一种动态窗口和绕墙走策略的垂直泊车轨迹规划方法。

该方法将轨迹规划分解为与时间无关的路径规划和与时间相关的速度规划，通过分析车辆转向特性，对绕墙走路径中存在的路径曲率不连续的点进行了等效替换，实现了无先验轨迹模型的路径规划，解决了传统泊车轨迹灵活性差的问题；同时，受绕墙走路径信息启发，该方法采用基于模型预测控制的动态窗口速度规划法，根据环境信息和车辆状态实时输出速度、方向盘转角，能动态调整车辆路径。

用于自动泊车系统的垂直停车场景如图1所示。

图中，灰色阴影部分代表了障碍物，左右的2条黑线代表了车道线边界，其中a为垂直停车位的宽度，b为垂直
停车位的长度，ds为车道宽度。

智能驾驶汽车的轨迹规划问题可以描述为规划一系列的位姿状态qi=(xi,yi,θi)，使得智能驾驶汽车能从初始状态q0=(x0,y0,θ0)无碰撞地到达目标状态
qe=(xe,ye,θe)。

2.1 车辆运动学模型及转向运动分析
汽车普遍采用阿克曼转向模型[16]，车辆转向时，全车的转向中心位于后轮轴线的延长线上，如图2所示。

本文将后轮轴线中心Pr作为全车轨迹的参考点，车辆的状态由横纵坐标及航向角
表征，表示为q=(x,y,θ)，车辆的输入为速度和方向盘转角，表示为(v,α)。

Lf、Lr
和w为前悬、后悬长度及车宽。

在泊车的低速行驶模式下可以忽略轮胎和地面的
侧滑，这样，系统存在如下非完整性约束。

通过数学推导，可以得到该转角状态下对应路径的曲率半径ρ和曲率κ。

综上，可以得到以速度和转向盘转角(v,α)为控制量，以q=(x,y,θ)为状态量的车辆运动学模型。

通过分析，车辆转向有以下特点。

1) 前后轮运动轨迹是不重合的。

2) 转弯半径最小的点在内侧后车轮。

同时，2个车轮的转弯半径为
式中：R1，R2分别为转向内侧后轮和外侧前轮的转弯半径差；Rr通常称为内轮，当转向盘转角α越大时，R1越小，此时的内轮差越大。

2.2 轨迹规划解耦
为了简化泊车轨迹规划，利用Path-velocityDecomposition[17]对路径和速度进行解耦，将轨迹规划问题分为路径规划和速度规划。

车辆的线速度以及路径的曲率可以表示为
结合式(2)和(6)，可得
式(7)表明，车辆的速度只影响行驶该轨迹所用的时间，行驶的轨迹形状和在各个
轨迹点上的姿态角仅仅与移动距离以及在不同位置的曲率(转向角)有关。

2.3 基于绕墙走策略的路径规划
绕墙走策略[18]是一种基于局部环境信息的避障方法，本文将绕墙走策略概括为以
下几个步骤。

1) 将垂直泊车地图环境进行凸包分割，形成凸多边形障碍物边缘。

2) 计算离起始位姿q0=(x0,y0,θ0)、终止位姿qe=(xe,ye,θe)2个状态距离最近的障碍物点O0和Oe。

3) 分别从O0和Oe沿初始姿态和终止姿态方向作车辆和障碍物边缘作距离为
w/2的等距线段，记录线段曲率不连续的点为Oi，并计算在该点姿态角的变化量。

由于原地转向中间状态处的曲率是不连续的，为了满足车辆的非完整性运动学约束，对路径作如下处理。

1) 对于每个点Oi，设置一个与之对应的中间等效状态qi=(xi,yi,θi)；那么，该点
位于线段qeOi和线段q0Oi延长线所包围的区域内。

2) 求最短圆弧路径q0qi和qeqi，且对应的半径为R1和R2。

3) 本文设置一个最小碰撞距离dc，该距离可由传感器精度确定，由式(5)可以得到，汽车转向内侧后车轮对应的转弯半径，此时可得参考点距障碍物值为+dc。

4) 计算最小碰撞距离时对应的R1和R2。

图3为2种垂直泊车的绕墙走策略示意图。

中间等效状态将轨迹分为了2步：①
可以视为从初始位姿q0=(x0,y0,θ0)调整到中间状态qi=(xi,yi,θi)；②进入狭小的
泊车位作准确的位姿调整直至到达终止状态qe=(xe,ye,θe)。

2.4 基于动态窗口法的速度规划
前述绕墙走策略虽然考虑了不能侧向位移的非完整性约束，但没有考虑速度、转向盘转角速度等基于时间的运动学约束，由此，下面采用基于模型预测控制的动态窗口法(dynamicwindowapproach,DWA)[19]进行满足这些约束的垂直泊车轨迹规划。

DWA核心思想是在输入输出空间根据机器人对象本身的性能进行采样，并模拟机器人在这些输入下在一段时间间隔内的轨迹，通过选取适当的目标函数对这些轨迹
进行评价，选取最优轨迹对应的输入来驱动机器人的运动。

该方法实现了优化与反馈的合理结合，对动态环境具有良好的适应性[20]。

图4为动态窗口法单步模拟出的轨迹。

车辆存在速度和转向盘转角限制：
假设DWA单步模拟的时间间隔为t，那么加速度、转角速度约束为
式中max分别对应最大加速度和转向盘转角速度。

对于时间窗口内的轨迹，采用考虑离参考路径偏差的轨迹选取策略，选择目标函数代价最低的轨迹，目标函数cost为
cost=pd·d+hdiff·α+gd·g
式中：pd,hdiff,gd分别为距离参考路径的距离偏差，d航向偏差a，以及离目标点距离偏差g的系数因子。

为了对提出的垂直泊车轨迹规划方法进行验证，在计算机CPU为IntelCorei7-4710，内存为8GB，系统环境为Ubuntu1 4.04，仿真平台为ROSIndigo与Gazebo的联调环境中进行后退式垂直泊车的实验验证。

车辆使用激光雷达进行环境感知，车身尺寸及其动力学的各个参数如表1所示。

垂直泊车场景以及车辆初始位姿和终止位姿的参数如表2所示。

动态窗口法中各个参数的设置情况如表3所示。

仿真结果如图5所示，车辆处于垂直泊车初始状态，绿色曲线为规划的绕墙走路径，红色曲线代表了当前时刻激光雷达扫描到障碍物。

本文通过检测曲率变化是否大于π/2判断车辆前进后退档位的切换，并基于此将整个路径分为两个子目标点用以驱动DWA进行速度规划。

图6显示车辆已经行驶至第一个子目标点，即泊车入口处，准备进入车位进行小范围内的位姿调整，地图中的黑色区域代表经过一定时间累积后的障碍物。

图7为车辆到达目标状态，完成泊车。

反复的仿真实验表明，图6中的子目标点对应路径曲率不连续的中间状态对整个
泊车成功与否至关重要，在车辆定位、子目标判定误差大于 0.2m的情况下，对
于该仿真车型往往难以进入泊车位。

同时随着车辆位姿误差在泊车过程中的累积，最终的地图和初始地图有一定差别，本文提出的动态窗口绕墙走轨迹规划方法能在不进行精准的轨迹跟踪控制的情况下，在局部进行动态调整直至垂直泊车，表明了该方法的有效性。

自动泊车系统由于环境的复杂性及对控制要求的高精度而常常采用离线轨迹规划的反应式体系结构，且其轨迹规划往往需要先验的轨迹数学模型以适用于泊车场景，而本文提出的基于动态窗口和绕墙走策略的轨迹规划为反应-慎思式体系结构，可
以实现无先验轨迹知识、局部动态可调的垂直泊车，提高了泊车规划决策的智能性。

将轨迹规划问题解耦成路径规划和速度规划，其对环境信息反馈优化的闭环规划能够动态对路径进行调整。

仿真实验验证了该方法的有效性，同时，该方法对于大型停车场等其他小范围环境中的车辆自主导航也有一定的参考意义，其所涉及的环境感知和精准定位技术是系统实现的基础，与停车共享[22]等技术的结合将极大地提高泊车效率和车位利用率，是进一步研究的重点。

[1] ARISMENDI C,lVAREZ D, GARRIDO S, et al. Nonholonomic motion planning using the fast marching square method[J]. International Journal
of Advanced Robotic Systems, 2015, 12(5): 1-14.
[2] KIM D, CHUNG W, PARK S. Practical motion planning for car-parking control in narrow environment[J]. IET Control Theory &Applications, 2010,
4(1): 129-139.
[3] MAHMUD S A, KHAN G M, RAHMAN M, et al. A survey of intelligent car parking system[J]. Journal of Applied Research and Technology, 2013,
11(5): 714-726.
[4] ZIPS P, BÖCK M, KUGI A. A fast motion planning algorithm for car parking based on static optimization[C]. IEEE/RSJ International Conference on Intelligent Robots and Systems, Tokyo, Japan: IEEE, 2013.
[5] 高航. 自动垂直泊车方法研究[D]. 合肥：中国科学技术大学, 2011.
GAO Hang. Research on automatic vertical parking method[D]. Hefei: University of Science and Technology of China,2011.(in Chinese)
[6] 李红，郭孔辉，宋晓琳. 基于样条理论的自动垂直泊车轨迹规划[J]. 湖南大学学报：(自然科学版)，2012, 39(7): 25-30.
LI Hong, GUO Konghui, SONG Xiaolin.Trajectory planning of automatic vertical parking based on spline theory[J]. Journal of Hunan University (Natural Sciences Edition), 2012, 39(7): 25-30.(in Chinese)
[7] ZIPS P, BÖCK M, KUGI A. A fast motion planning algorithm for car parking based on static optimization[C]. 2013 IEEE/RSJ International Conference on Intelligent Robots and Systems, Tokyo: IEEE, 2013.
[8] ZIPS P, BÖCK M, KUGI A. Optimisation based path planning for car parking in narrow environments[J]. Robotics and Autonomous Systems, 2016, 79: 1-11.
[9] CHENG K, ZHANG Y, CHEN H. Planning and control for a fully-automatic parallel parking assist system in narrow parking spaces[C]. Intelligent Vehicles Symposium (IV), Gold Coast, Australia: IEEE, 2013. [10] GORINEVSKY D, KAPITANOVSKY A, GOLDENBERG A. Neural network architecture for trajectory generation and control of automated car parking[J]. IEEE Transactions on Control Systems Technology, 1996, 4(1): 50-56.
[11] 吴冰，钱立军，虞明，等. 基于 RBF 神经网络的自动泊车路径规划[J]. 合肥工业大学学报(自然科学版)，2012, 35(4): 459-462.
WU Bing, QIAN Lijun, YU Ming, et al.Path planning of automatic parallel parking based on RBF neural network[J].Journal of Hefei University of Technology (Natural Science Edition), 2012,35(4):459-462.(in Chinese) [12] 任坤，许艺，等. 基于机器视觉和模糊控制的自动泊车[J]. 华中科技大学学报(自然科学版)，2015(增刊1): 88-92.
REN Kun, et al. Auto-parking based on machinevision and fuzzy control[J]. Journal of Huazhong University of Science and Technology (Natural Science Edition), 2015(S1):88-92.(in Chinese)
[13] 郭孔辉，姜辉，张建伟，等. 基于模糊逻辑的自动平行泊车转向控制器[J]. 吉林大学学报(工学版)，2009(增刊1)：236-240.
GUO Konghui, JIANG Hui, ZHANG Jianwei, et al. Automatic parallelparking steering controller based on fuzzy logic control theory[J]. Journal of Jilin University (Engineering and Technology Edition), 2009 (S1): 236-240.(in Chinese)
[14] 夏增辉，杨辉跃. 基于变论域模糊控制及遗传算法的自动泊车方法[J]. 自动化与仪器仪表，2014(4): 120-122.
XIA Zenghui, YANG Huiyue. Based onvariable universe fuzzy control and automatic parking genetic algorithm approach[J]. Automation & Instrumentation, 2014(4): 120-122.(in Chinese)
[15] 叶卿，徐建闽，林培群. 基于计算机视觉的停车位车辆存在性检测方法[J]. 交通信息与安全, 2014, 32(6): 39-43.
YE Qing,XU Jianmin,LIN Penqun. Parking space occupancy detection based
on computer vision[J]. Journal of Transport Information and Safety, 2014,32(6): 39-43. (in Chinese)
[16] 任孝平，蔡自兴. 基于阿克曼原理的车式移动机器人运动学建模[J]. 智能系统学报，2009, 4(6): 534-537.
REN Xiaoping, CAI Zixing. Using the Ackerman principle for kinematic modeling of wheeled mobile robots[J]. CAAI Transactions on Intelligent Systems,2009,4(6):534-537.(in Chinese)
[17] 任孝平，蔡自兴. 基于阿克曼原理的车式移动机器人运动学建模[J]. 智能系统学报, 2009, 4(6): 534-537.
REN Xiaoping, CAI Zixing. Using the Ackerman principle for kinematic modeling of wheeled mobile robots[J]. CAAI Transactions on Intelligent Systems, 2009, 4(6): 534-537.(in Chinese)
[18] KANT K, ZUCKER S W. Toward efficient trajectory planning: The path-velocity decomposition[J]. International Journal of Robotics Research, 1986, 5(3): 72-89.
[19] THRUN D F W B S, FOX D, BURGARD W. The dynamic window approach to collision avoidance[J]. IEEE Transactions on Robotics and Automation, 1997(4): 1.
[20] 张纯刚，席裕庚. 动态未知环境中移动机器人的滚动路径规划[J]. 机器人，2002,24(1): 71-75.
ZHANG Chungang, XI Yugeng. Rolling path planning of mobile robot in dynamic unknown environment[J]. Robot, 2002, 24(1): 71-75.(in Chinese) [22] 王莹玉.城市综合体项目停车共享研究[J]. 交通信息与安全, 2016, 34(2):123-128.
WANG Yingyu. A Study of Shared Parking for Urban Complex[J]. Journal of Transport Information and Safety, 2016, 34(2):123-128. (in Chinese)
▲通信作者：张葛祥(1974-)，博士，教授. 研究方向：机器人、自然计算和智能电网等. E-mail：*****************。