基于预瞄模糊控制的自动泊车算法研究

熊周兵，杨 威，丁 可，梁锋华，郑 玲，黎予生

(1.重庆长安汽车股份有限公司 汽车研究总院，重庆 401120；2.重庆大学 机械传动国家重点实验室，重庆 400044)

基于预瞄模糊控制的自动泊车算法研究

熊周兵1，杨 威2，丁 可1，梁锋华1，郑 玲2，黎予生1

(1.重庆长安汽车股份有限公司 汽车研究总院，重庆 401120；2.重庆大学 机械传动国家重点实验室，重庆 400044)

针对目前自动泊车系统转向不连续问题，提出一种两段式泊车路径规划算法，采用Sigmoid函数对路径进行函数拟合，确定泊车路径。建立了横向预瞄模型，对路径追踪误差进行估计，采用模糊控制方法，对拟合后的理想路径进行追踪；搭建了PreScan虚拟仿真环境，通过超声波传感器获取车位信息，对提出的基于预瞄模糊控制的自动泊车算法进行了仿真验证。仿真结果表明：Sigmoid函数对两段式泊车路径的拟合决定系数可达0.99以上；采用该预瞄模糊控制泊车算法，汽车能很好地追踪规划的路径，并成功泊入预定车位。

两段式泊车；函数拟合；预瞄模糊控制；PreScan

随着汽车数量的逐年攀升，城市停车场空间趋于饱和，经验不足的驾驶员很难轻松地将汽车泊入拥挤的泊车位，因泊车造成的事故也屡见不鲜。自动泊车系统为解决泊车困难的问题提供了先进的技术手段。

目前，国内外主要采用两种方法进行自动泊车控制算法的研究[1]：① 路径规划：借助传感器获取泊车空间几何形状与障碍位置，根据车辆运动学模型，预先规划出一条理想的路径，设计控制算法执行路径跟踪，实现自动泊车；② 利用经验知识模拟驾驶员操作控制，如采用模糊逻辑、神经网络算法等。但这些算法都过于复杂，不利于泊车实时性要求。Paromtchik等[2]利用回归算法拟合正弦曲线得到理想路径实现路径规划，但该算法需要很大的泊车空间。李红等[3]采用多约束优化的方法拟合反正切曲线设计理想路径，但该算法计算效率较低，不利于实际应用。ZHAO、CHANG等[4-5]应用泊车经验知识设计模糊控制器来控制泊车过程，但该算法对汽车初始位置敏感，对泊车空间要求较大。吴冰等[6]采用粒子群方法、结合RBF神经网络实现自动泊车路径规划，其仿真成功率高，但对初始位置要求较高，计算量大。

自动泊车系统的设计难度在于汽车能在尽可能小的空间内快速决策出泊入过程中的方向盘转角，实现多种空间状态下的自动泊入。考虑到工程适用性与算法鲁棒性，本文采用路径规划的方法寻找理想路径。以此为基础，研究基于预瞄模糊控制的平行泊车控制算法，以实现汽车稳定追踪理想路径，降低泊入过程中的计算积分误差，提高不同环境下的泊车成功率。

1 自动泊车路径规划算法

一个完整的自动泊车系统包括4个部分：环境感知、路径规划、路径跟踪、人机交互[7]。自动泊车工作流程如图1所示。当驾驶员启动自动泊车系统后，在车辆运动过程中，传感器会自动探测周围空间环境。当有适合尺寸的泊车空间时[7]，人机交互系统会提示驾驶员找到车位，系统自动进行路径规划。若可以规划出一条泊入路径则提示驾驶员可以进行泊车，否则驾驶员只能放弃当前泊车位置。之后按照系统提示，驾驶员挂入倒挡，路径跟踪控制开始执行，最终完成泊车。

图1 自动泊车工作流程

自动泊车系统的路径规划是根据车载超声波传感器探测到泊车需求方向上的车位空间信息，再利用这些信息规划出一条理想的路径。相比智能算法，路径规划可大大降低计算的复杂程度，并预先确定汽车能否在此泊车位完成自动泊车。

1.1 路径规划运动学模型

图2为基于阿克曼转向机构的车辆运动学模型[8]。 由于正常情况下泊车速度很低(约≤5 km/h)，故后轮可视为无滑移，即后轮轨迹的y方向速度为0，根据图2可知车辆后轴中心点坐标：

(1)

前后轴中心点坐标之间存在如下关系：

xr=xf-lcosθ

yr=yf-lsinθ

(2)

对式(2)进行求导得：

(3)

将式(3)代入式(1)得

(4)

图2 车辆运动学模型

设δ为阿克曼转向机构中前轴中心点等效转向角，则前轴中心点的速度为：

(5)

将式(5)代入式(4)得

(6)

再将式(5)与(6)代入式(3)，可获得后轮中心点速度:

(7)

将式(7)对时间积分，可推导出后轮中心点的运动轨迹：

(8)

左后轮的运动轨迹为：

(9)

右后轮的运动轨迹为：

(10)

其中：θ为汽车方位角；l为轴距；w为轮距。

由式(9)与(10)可以看出：当汽车低速泊车时，后轮的运动轨迹与车速无关，只与汽车的轴距、轮距与转向轮转角有关，并且运行轨迹是一段圆弧。上述轨迹规划的运动学方程为两段式泊车提供了理论依据。

1.2 两段式泊车路径拟合方法

依据汽车低速泊车后轮轨迹为圆弧的特点，根据泊车空间的尺寸大小确定两段相切圆弧，汽车后轮可以沿着圆弧运动顺利泊入车位，这就是两段式泊车。通过控制汽车的最小转弯半径，两段式泊车不仅能够减少泊车空间，降低泊车换挡次数，还能使汽车平行泊入车库内走过的路径最短。两段式泊车几何路径规划如图3所示。

图3 两段式泊车几何路径规划

根据阿克曼转向原理，车辆运行过程中转弯半径与前轮转角之间的关系为

(11)

其中αout为前外轮转角。从图3可知:汽车以最小转弯半径转向，在两段圆弧相切点处车辆方位角达到最大值，其值为

(12)

其中:Δ为安全缓冲距离;p2为侧边库底距车轴对称线的纵向距离;rmin为最小转弯半径;B为车宽。

(13)

此函数具有4个拟合参数，分别为a1，a2，a3，a4，其2阶导数为

(14)

该拟合函数的2阶导数连续，表明函数曲率连续变化，即车辆沿这条路径运动时转向角无突变。同时，-a2a3的取值可以保证该函数的曲率在x=0时趋近于0，即车辆在泊车起始位置处前轮转向角为0。为验证两段式泊车路径规划函数拟合效果，设计3种仿真工况参数，如表1所示。表2是根据3种工况得到的拟合参数。路径拟合结果见图4。

表2 函数拟合参数

图4 路径拟合结果

为检验函数拟合优度，可用决定系数来定量评价，其计算公式为

(15)

由式(15)计算得到仿真试验中3种工况下的拟合决定系数分别为0.999 7，0.999 4，0.999 5，说明采用4个拟合参数的Sigmoid函数拟合效果较好，对两段式泊车路径的近似程度较高。

2 路径追踪控制

路径追踪控制是无人驾驶智能汽车技术中的核心技术之一，目前常用的追踪控制方法主要有PID算法、模糊推理、模糊PID、神经网络或多种算法相结合的综合控制[9]。无论采用何种控制方法，首先需要建立路径追踪误差模型。

2.1 横向预瞄模型

预瞄的引入，相当于建立驾驶员模型，实现路径追踪的闭环控制。斯坦福大学Stanley智能车研究所提出了距离-角度偏差预瞄模型[10]；CLA提出了环形预瞄模型[11]；北京理工大学采用了Follow the Carrot模型[12]；Pure Persuit模型[13]设计了一条圆弧，车辆可沿这条圆弧到达目标点，由此圆弧计算车辆的前轮偏角控制量。以上模型主要用于智能汽车行驶过程中的路径追踪。自动泊车有别于智能汽车的行驶控制，其运动速度低、运动空间小，且速度方向为负，采用以上预瞄模型难以快速准确地实现误差计算。本文在距离-角度偏差预瞄模型基础上提出横向预瞄模型[14]，如图5所示。

当车辆探测到可行的泊车空间时停止，进行路径规划，以当前车辆停止的位置与方位角建立大地坐标系，如图5所示。图5中的坐标系一旦确定，整个自动泊车过程都将在此坐标系下完成。车辆运动前方一定距离的预瞄点K与相应横向位置K′的偏差Δy称之为横向距离误差，预瞄点的方位角θ与相应横向位置K′方位角的偏差Δφ称之为横向角度误差，横向距离误差与横向角度误差统称为横向预瞄误差。在实际计算过程中，设定一定权值来分配距离误差和角度误差的影响权重，其关系式为：

(16)

其中：Xr为后轴中心x坐标；Yr为后轴中心y坐标；XK为预瞄点x坐标；P为预瞄距离；Y(X)为拟合曲线函数表达式；i为权重分配系数，考虑到泊车过程中位置误差与角度误差对追踪都具有直接影响，因此取权重分配系数i=0.5。

图5 横向预瞄模型

2.2 路径追踪模糊控制

汽车运动学系统具有强非线性与耦合性，采用模糊控制既可实现路径规划与路径追踪的实时性、鲁棒性，又能发挥经验知识对泊车精度提升的优势。本文以横向预瞄误差与误差变化率为模糊控制输入、转向外轮转角为模糊控制输出设计模糊控制器，其控制系统如图6所示。

分别划分7个误差输入、误差变化率输入与转角输出的模糊集合，模糊量化后论域均为[-1,1]，其隶属度函数如图7所示。

图6 预瞄闭环控制系统

图7 隶属度函数

图7中，NB，NM，NS，Z，PS，PM，PB分别代表“负大”、“负中”、“负小”、“零”、“正小”、“正中”、“正大”，模糊规则采用ifEandECthenU，共产生49条规则，如表3所示。由规则生成的模糊控制面如图8所示。

表3 模糊规则

图8 模糊控制面

3 测试场景设计与仿真

某乘用车的外形参数如表4所示。

表4 某乘用车外形参数Table 4 Shape parameters of a passenger car

3.1 基于Matlab的仿真验证

针对以上的两段式泊车路径规划以及基于横向预瞄的模糊控制路径追踪控制方法，采用Matlab/Simulink，构建控制系统仿真框图，对本文提出的自动泊车控制方法进行验证。图9是3种工况下的仿真结果。

从图9可知:预瞄模糊控制能有效控制车辆沿着理想轨迹运动，其误差在可接受范围内，并且方向盘转角也在初始位置由0逐渐增大，变化过程中并无突变，满足泊车要求。车辆泊入车库后，相对车库位置比较靠后，还需要前进一段距离驶到车库中间位置。仿真结果表明:基于预瞄模糊控制的平行泊车算法计算实时性好、鲁棒性强，具有较高的工程实用价值。

3.2 基于PreScan的仿真验证

PreScan是专业的主动安全设计与环境、传感器建模仿真软件，可以很大程度上还原真实场景，提高仿真的准确度与真实性。本文以PreScan场景为基础，结合软件中的超声波传感器，建立自动泊车仿真环境，实时验证控制算法的正确性。图10是构建的PreScan场景，图11是工况2的泊车过程实时仿真结果。

图10 PreScan场景

图11 PreScan VisViewer与GUI仿真过程

工况2的自动泊车过程可分为5个阶段：(a) 汽车低速前进，启动自动泊车程序，超声波传感器启动，开始搜索车位；(b) 发现车位，减速停车，根据超声波传感器获取的泊车环境信息重构空间坐标，规划两段式泊车路径并拟合，完成后挂入倒挡开始泊车；(c) 预瞄模糊控制算法实时计算汽车前轮转角，利用转向比计算方向盘转角，实现转向控制；(d) 顺利泊入车位，为使车辆停在车库中间，还需要前进修正一次；(e) 前进修正完成泊车；(f) GUI仿真过程展示了超声波传感器获取的环境信息，其中虚线是车辆运动轨迹，实线是障碍轮廓。

4 结束语

本文针对目前自动泊车系统转向不连续问题，提出了基于两段式泊车的路径规划方法，建立了横向预瞄模型，采用模糊控制方法对理想路径进行了追踪控制，并通过仿真验证了控制算法的有效性。

采用两段式泊车，设计了一条路程最短的路径，利用Sigmoid函数，对两段式泊车路径进行了参数拟合。结果表明：其拟合决定系数达到0.99以上，函数曲线满足泊车过程中曲率要求。

研究了基于预瞄模糊控制的闭环控制平行泊车算法，通过控制汽车转向外轮转角，实现了对理想路径的追踪控制，其计算精度能满足要求，在不同的车库尺寸及初始位置条件下，均能顺利泊入车库；基于PreScan搭建了自动泊车的场景模型，联合Matlab/Simulink控制仿真，验证了自动泊车控制算法的有效性和正确性，通过真实泊车场景的构建，直观再现了控制算法的效果。

[1] 郭孔辉，姜辉，张建伟，等.基于模糊逻辑的自动平行泊车转向控制器[J].吉林大学学报(工学版)，2009，39 (S2):236-240.

GUO Konghui,JIANG hui,ZHANG Jianwei,et al.Automatic parallel parking steering controller based on fuzzy logic control theory[J].Journal of Jilin University(Engineering and Technology Edition),2009，39 (S2):236-240.

[2] PAROMTCHIK I E,LAUGIER C.Autonomous Parallel Parking and Returning to Traffic Maneuvers[C]//Proceedings of the 1997 IEEE/RSJ International Conference on Intelligent Robot and Systems.Grenoble,France:[s.n.],1997:21-23.

[3] 李红，郭孔辉，宋晓琳，等.基于Matlab的多约束自动平行泊车轨迹规划[J].中南大学学报(自然科学版)， 2013，44(1):101-107.

LI Hong,GUO Konghui,SONG Xiaolin，et al.Trajectory planning of automatic parallel parking with multi-constraints based on Matlab[J].Journal of Central South University(Science and Technology),2013，44 (1):101-107.

[4] ZHAO Y N,COLLINS E G.Robust automatic parallel parking in tightspaces via fuzzy logic[J].Robotics and Autonomous Systems,2005,51 (5):111-127.

[5] CHANG S J,LI J.Design and implementation of fuzzy parallel parking control for a car-type mobile robot[J].Journal of Intelligent & Robotic System,2004,34(2):175-180.

[6] 吴冰，钱立军，虞明，等.基于RBF神经网络的自动泊车路径规划[J].合肥工业大学学报(自然科学版)，2014，35 (4):459-540.

WU Bing,QIAN Lijun,YU Ming，et al.Path planning of automatic parallel parking based don RBF neural network[J].Journal of Hefei University of Technology(Natural Science),2014，35 (4):459-540.

[7] POHL J,SETHSSON M,DEGERMAN P,et al.A semi-automated parallel parking system forpassenger cars[J].J Automobile Engineering,2006,220:53-65.

[8] COLYER R E,ECONOMOU J T.Comparison of steering geometries for multi-wheeled vehicles by modelling and simulation[C]// Proceedings of the 37th IEEE Conference on Decision and Control.USA:IEEE,1998:3131-3133.

[9] 乔维高，徐学.无人驾驶汽车的发展现状及方向[J].上海汽车,2007(7):40-44.

QIAO Weigao,XU Xue.Development status and direction of driverless cars[J].Shanghai automobile,2007(7):40-44.

[10]SEBASTIAN T,MIKE M,HENDRIK D,et al.Stanley The Robot that Won the DARPA Grand Challenge[J].Journal of Field Robotics,2006,23(9):661-692.

[11]CHEN Q,OZUGUNER U,REDILL K.Ohio State University at the 2004 DARPA Grand Challenge:Developing a Completely Autonomous Vehicle[J].IEEE Intelligent.Systems,2004,19(5):8-11.

[12]CEM H,OZGUNER U,KEITH A.Redmill.Automated Lane Change Controller Design[J].IEEE Transactions on Intelligent Transportation System,2003,4(1):13-22.

[13]KRISTIJIAN M,ROLAND P,ROLAND S.Path Following for Autonomous Vehicle Navigation with Inherent Safety and Dynamic Margin[C]//IEEE Intelligent Vehicles Symposium[C].USA:IEEE,2008:108-113.

[14]赵玲.平行泊车方法研究与仿真[D].西安：长安大学，2009.

ZHAO Ling.Research and Simulation of parallel parking method[D].Xi’an:Chang’an University,2009.

(责任编辑 刘 舸)

Research on the Auto Parking Algorithm Based on the Preview Fuzzy Control

XIONG Zhou-bing1, YANG Wei2, DING Ke1, LIANG Feng-hua1, ZHENG Ling2, LI Yu-sheng1

(1.Chang’an Auto Global R&D Center, Chang’an Automobile Co., Ltd., Chongqing 401120, China; 2.The State Key Laboratory of Mechanical Transmissions, Chongqing University, Chongqing 400044, China)

The Sigmoid function was adopted to fit the two section of parking path planning, with the aim to solve the discontinuous problems of automatic parking system. The lateral preview model was established and the path tracking error was estimated. Then the preview fuzzy control algorithm was adopted to track the ideal path. Finally, PreScan virtual simulation environment was established and the information of the parking slots was obtained by ultrasonic sensors.Simulation results demonstrate that the fit coefficient of determination can be over 0.99 between the Sigmoid function and the two-section parking path. Based on the preview fuzzy control, the car can track the planning path successfully and park into parking spaces.

two-section parking; function fitting; preview fuzzy control; PreScan

2016-11-28 基金项目：重庆市自然科学基金重点资助项目(cstc2015jcyjBX0097)；重庆市重点产业共性关键技术创新专项项目(cstc2015zdcy-ztzx30001)

熊周兵， 男，博士，主要从事车辆工程研究；通讯作者：郑玲，女，教授，博士生导师，主要从事汽车系统动力学与控制、智能材料与结构系统、振动与噪声控制等方面研究，E-mail:zling@cqu.edu.cn。

熊周兵，杨威，丁可，等.基于预瞄模糊控制的自动泊车算法研究[J].重庆理工大学学报(自然科学)，2017(2):14-22.

format：XIONG Zhou-bing, YANG Wei, DING Ke,et al.Research on the Auto Parking Algorithm Based on the Preview Fuzzy Control[J].Journal of Chongqing University of Technology(Natural Science)，2017(2):14-22.

10.3969/j.issn.1674-8425(z).2017.02.003

U491.7

A

1674-8425(2017)02-0014-09