张家旭 卜纯研 王晨 赵健

摘   要：針对线控四轮转向汽车平行泊车路径规划与跟踪控制问题，提出一种基于改进粒子群优化算法的平行泊车路径规划方法和一种基于前馈控制和反馈控制的平行泊车路径跟踪控制策略. 首先，综合考虑线控四轮转向汽车运动学非完整约束、动力和转向子系统的过程约束和边界约束、避障约束、泊车初始位姿和目标位姿约束，建立以最小化泊车过程总时长为目标的平行泊车路径规划约束最优化问题，并采用可以处理等式约束和不等式约束的粒子群优化算法对其进行求解，得到最优平行泊车路径. 随后，利用平行泊车路径规划过程得到的车轮转向角作为前馈控制量，并利用汽车实际位姿与期望位姿的偏差构建PI反馈控制量，实现对规划的平行泊车路径快速、精确和稳定的跟踪控制. 最后，利用车辆动力学仿真软件构建模型在环仿真系统，验证所提出方法的可行性和有效性. 结果表明：所提出的方法可以快速、精确和稳定地引导线控四轮转向汽车自动完成平行泊车任务.

关键词：车辆工程;控制工程;线控四轮转向汽车;平行泊车;路径规划;路径跟踪控制;改进粒子群优化算法

中图分类号：U461.1                                文献标志码：A

Parallel Parking Path Planning and Tracking

Control for Wire-four-wheel Steering Vehicle

ZHANG Jiaxu1，2，BU Chunyan1，WANG Chen1，ZHAO Jian1

（1. State Key Laboratory of Automotive Simulation and Control，Jilin University，Changchun 130022，China;

2. Intelligent Network R&D Institute，China FAW Group Co Ltd，Changchun 130011，China）

Abstract：Aiming at the parallel parking path planning and tracking control problem for the wire-four-wheel steering vehicle，a novel parallel parking path planning method is proposed based on the improved particle swarm optimization algorithm，and a novel parallel parking path tracking control strategy is proposed based on feedforward and feedback control architecture. Firstly，considering the kinematic non-holonomic constraint of the wire-four-wheel steering vehicle，the process and boundary constraints of the power and steering sub-systems，the obstacle avoidance constraint，the initial parking pose constraint and the target pose constraint，a parallel parking path planning constraint optimization problem with minimizing the parking time is established，and the particle swarm optimization，which can deal with the equality and inequality constraints，is adopted to solve the constraint optimization problem. In order to execute the planned parking path，the desired wheel steering angle is used as the feedforward control quantity，and the PI feedback control quantity is constructed based on the deviation between the actual pose and the expected pose of the vehicle. Finally，the model-in-the-loop simulation system is built based on vehicle dynamics simulation software to verify the feasibility and effectiveness of the proposed method，and the simulation results show that the proposed method can guide the wire-four-wheel steering vehicle to complete the parallel parking task quickly，accurately and stably.

Key words：vehicle engineering;control engineering;wire-four-wheel steering vehicle; parallel parking; path planning; path tracking control; improved particle swarm optimization algorithm

汽车运动学非完整约束使得经验不足的驾驶员在狭窄环境内难以完成泊车任务，而自动泊车系统是解决此问题的有效手段之一. 在精确感知外界环境的前提下，泊车路径规划与跟踪控制方法是决定自动泊车系统性能的核心要素[1-2]. 因此，深入研究泊车路径规划与跟踪控制方法对于解决汽车运动学非完整约束导致的泊车难问题具有重要的意义.

泊车路径规划方法主要有几何规划方法[3-5]、智能规划方法[6-7]和优化规划方法[8-9]. 文献[3]通过圆弧、直线和回旋曲线的平移与旋转变换规划出曲率连续的泊车路径. 文献[4]通过参数化Gompertz曲线获得可执行的泊车路径. 文献[5]采用圆弧-直线-β样条曲线组合的方式规划出曲率连续的泊车路径. 采用几何规划方法得到的泊车路径适用场景受限于初始泊车位姿、泊车位和不规则障碍物信息等，可行的泊车起始区域较小. 文献[6]利用熟练驾驶员的泊车数据训练神经网络，使其可以模拟熟练驾驶员完成泊车任务. 文献[7]利用自适应神经模糊推理系统学习熟练驾驶员的泊车操作，进而自动完成泊车任务. 采用智能规划方法得到的泊车路径适用场景受限于训练样本的规模和多样性，泛化能力较弱. 文献[8]将泊车路径规划问题转化为约束最优化问题，并利用高斯伪谱法求解得到最优泊车路径. 为了兼顾计算效率和求解精度，文献[9]采用自适应伪谱法求解泊车路径规划问题来获得最优泊车路径. 采用优化规划方法得到的泊车路径适用场景最广，但优化求解方法的较大计算量限制了其工程应用.

泊车路径跟踪控制方法主要有模型预测跟踪控制方法[10]、非线性鲁棒跟踪控制方法[11]和滑模跟踪控制方法[5，12-13]. 文献[10]将受控自回归积分滑动平均模型作为预测模型，采用模型预测控制方法设计泊车路径跟踪控制策略. 为了抑制外界干扰和名义模型不确定性对泊车路径跟踪精度的影响，文献[11]采用非线性鲁棒控制方法设计泊车路径跟踪控制策略. 文献[5]建立包含外界干扰的汽车运动学名义模型，并采用滑模自抗扰控制方法设计泊车路径跟踪控制策略. 文献[12]建立非时间参考泊车路径跟踪控制模型，并基于此设计了泊车路径滑模跟踪控制策略. 文献[13]采用反正切函数代替泊车路径滑模跟踪控制策略中符号函数来抑制传统滑模固有的抖振问题. 上述泊车路径跟踪控制方法的被控对象均是前轮转向汽车，无法直接应用于线控四轮转向汽车.

线控四轮转向汽车具有更好的机动灵活性，在狭窄环境内更容易实现泊车操作[14]. 本文针对线控四轮转向汽车平行泊车场景的路径规划与跟踪控制问题，提出一种基于改进的粒子群优化算法的平行泊车路径规划方法，以及一种基于前馈控制和反馈控制的平行泊车路径跟踪控制策略. 首先，综合考虑线控四轮转向汽车运动学非完整约束、动力总成子系统和转向子系统的过程约束和边界约束、直线交点法描述的避障约束、泊车的初始位姿和目标位姿约束，建立以最小化平行泊车过程总时长为目标的平行泊车路径规划约束最优化问题，并采用可以有效处理等式约束和不等式约束的粒子群优化算法对其进行求解，进而得到最优平行泊车路径. 随后，利用平行泊车路径规划过程得到的前轮转向角和后轮转向角作为前馈控制量，并利用汽车实际位姿与期望位姿的偏差构建PI反馈控制量，实现对规划的平行泊车路径快速、精确和稳定的跟踪控制. 最后，利用车辆动力学仿真软件构建模型在环仿真系统，验证所提出的线控四轮转向汽车平行泊车路径规劃方法和路径跟踪控制策略的可行性和有效性.

1   平行泊车路径规划

1.1   平行泊车路径规划问题描述

线控四轮转向汽车平行泊车过程中处于低速大转角行驶状态，可认为四个车轮无侧偏的绕同一瞬时圆心做圆周运动. 如图1所示，将线控四轮转向汽车后轴中点作为参考点，建立其平行泊车过程中的运动学方程为：

式中：xr、yr、vr和ar分别为线控四轮转向汽车后轴中点横坐标、纵坐标、速度和加速度;δf和δr分别为线控四轮转向汽车前轮和后轮转向角;ωf和ωr分别为线控四轮转向汽车前轮和后轮转向角速度;φ和L分别为线控四轮转向汽车方位角和轴距.

式中：vrmax和armax分别为线控四轮转向汽车后轴中点速度和加速度最大值;δfmax和δrmax分别为线控四轮转向汽车前轮和后轮转向角最大值;ωfmax和ωrmax分别为线控四轮转向汽车前轮和后轮转向角速度最大值.

基于汽车后轴中点坐标和方位角可得如图1所示的汽车轮廓四个顶点A、B、C和D的坐标为：

xA（k），yA（k）=xr（k）+（L+Lf）cosφ（k）-Wsinφ（k）/2，yr（k）+（L+Lf）sinφ（k）+Wcosφ（k）/2xB（k），yB（k）=xr（k）+（L+Lf）cosφ（k）+Wsinφ（k）/2，yr（k）+（L+Lf）sinφ（k）-Wcosφ（k）/2xC（k），yC（k）=xr（k）-Lr cosφ（k）+Wsinφ（k）/2，yr（k）-Lr sinφ（k）-Wcosφ（k）/2xD（k），yD（k）=xr（k）-Lr cosφ（k）-Wsinφ（k）/2，yr（k）-Lr sinφ（k）+Wcosφ（k）/21≤k≤N

（9）

式中：Lf、Lr和W分别为汽车前悬、后悬和宽度.

汽车平行泊车过程中其轮廓边缘AB与如图2所示的泊车位前方障碍物无碰撞的约束条件可表示为：

汽车平行泊车过程中其轮廓边缘BC与如图2所示的泊车位前方障碍物无碰撞的约束条件可表示为：

汽车平行泊车过程中其轮廓边缘BC与如图2所示的泊车位侧方路基无碰撞的约束条件可表示为：

yB（k） + Wp ≥ 0yC（k） + Wp ≥ 01 ≤ k ≤ N             （12）

式中：Wp为平行泊车位宽度.

汽车平行泊车过程中其轮廓边缘CD与如图2所示的泊车位后方障碍物无碰撞的约束条件可表示为：

式中：Lp为平行泊车位长度.

已知平行泊车的初始位姿和目标位姿分别为（x0，y0，φ0）和（xf，yf，φf），则有

xr（0） = x0，yr（0） = y0，φ（0） = φ0xr（N） = xf，yr（N） = yf，φ（N） = φf   （14）

将式（5） 、式（8）和式（14）描述的等式约束条件记为c（x，u） = 0，将式（7）、式（10）～式（13）描述的不等式约束条件记为g（x，u） ≤ 0，则平行泊车路径规划问题转化为以平行泊车过程总时长为目标的约束最优化问题，使线控四轮转向汽车在最短时间内完成平行泊车任务.

（15）g（x，u） ≤ 0

由式（6）和式（15）可知，给定离散点数量N，最小化平行泊车过程总时长tf约束最优化问题可等价为最小化计算步长h约束最优化问题.

1.2   平行泊车路径规划问题求解

粒子群优化算法是一种基于启发式信息的随机搜索算法，具有收敛速度快和设置参数少等优点，特别适用于无约束最优化问题求解[16]. 本节利用最优化问题的约束条件改进粒子群优化算法的适应度函数，使改进的粒子群优化算法可以求解式（15）描述的平行泊车路径规划约束最优化问题. 已知系统控制向量u（k），1≤k≤N包含三个分量，则表征系统控制向量的粒子位置向量和速度向量包含3N个分量. 若按照第一个系统控制向量三个分量至最后一个系统控制向量三个分量先后方式排序，则第i个粒子的位置向量和速度向量可分别表示为：

pi（s） = [ p1i  p2i  …  p3Ni]T      （16）

vi（s） = [ v1i  v2i  …  v3Ni]T      （17）

式中：s为粒子群优化算法的当前迭代次数.

利用第i个粒子的历史最优解pbesti    （s）和群体的历史最优解pbest    （s）更新第i个粒子的速度为：

vi（s+1） = q（s）vi（s）+d1r1（pbesti    （s）-pi（s））+

d2 r2（pbest    （s）-pi（s））       （18）

式中：r1，r2∈[0，1]为均匀分布的随机数;d1和d2分别为粒子速度自身部分和社会部分权重系数，二者的有机组合可以使粒子群优化算法保持收敛速度和搜索效果的均衡;q（s）为控制粒子历史速度对当前速度影响的惯性部分权重系数，可表示为：

式中：qmax和qmin分别为惯性部分权重的最大值和最小值;smax为总迭代次数. 通过自适应调整惯性部分权重系数，使粒子群优化算法在前期有较高的全局搜索能力，在后期有较高的局部搜索能力.

利用第i個粒子在迭代计算次数s+1的速度更新其位置为：

pi（s+1） = pi（s） + vi（s）     （20）

利用最优化问题的约束条件计算第i个粒子的适应度函数为：

Fi（s）=Ji（s）                       if Vi（s）=0J *（s）+J *（s）Vi（s）   if Vi（s）>0     （21）

式中：Ji（s）为第i个粒子在迭代计算次数s的优化目标值;J *（s）为第i个粒子的历史最差优化目标值;Vi（s）表示为

式中：Nc和Ng分别为最优化问题等式约束条件数量和不等式约束条件数量;μij（s）和ηij（s）表示为

μij（s）=0，                cj（x，u）=0|cj（x，u）|/εj，|cj（x，u）|≤εj， j = 1，2，…Nc1，                |cj（x，u）|>εj

（23）

ηij（s）=0，                gj（x，u）≤0|gj（x，u）|/ξj，0ξj

（24）

式中：εj和ξj分别为最优化问题等式约束条件允许的容差和不等式约束条件允许的容差.

综上所述，可得如图3所示的基于改进的粒子群优化算法求解平行泊車路径规划约束最优化问题的具体流程.

2   平行泊车路径跟踪控制

本节基于前馈控制和反馈控制相结合的方式设计平行泊车路径跟踪控制策略. 其中，前馈控制环节用于提高平行泊车路径跟踪控制策略的响应速度和精度，而反馈控制用于抑制外界扰动对平行泊车路径跟踪控制性能的影响.

如图4所示，首先利用汽车实际位置与期望位置的偏差eD（k）、汽车实际方位角与期望方位角的偏差eφ（k）计算前轮转向角反馈控制量为：

Δδf（k+1）=Δδf（k）+

K PD（eD（k+1）-eD（k））+K IDeD（k+1）+

K Pφ（eφ（k+1）-eφ（k））+K Iφeφ（k+1）      （25）

式中：K PD和K ID分别为汽车实际位置与期望位置偏差的比例系数和积分系数;K Pφ和K Iφ分别为汽车实际方位角与期望方位角偏差的比例系数和积分系数;eD（k）和eφ（k）可表示为：

式中：（xr（k），yr（k），φ（k））和（xrd（k），yrd（k），φd（k））分别为汽车实际位姿和期望位姿.

若平行泊车路径规划过程得到的汽车前轮转向角前馈控制量为δfd（k + 1），则汽车前轮转向角控制量可表示为：

δf（k + 1） = δfd（k + 1） + Δδf（k + 1）    （28）

由汽车前轮转向角最大值与后轮转向角最大值的比例关系，可得汽车后轮转向角控制量为：

δr（k + 1） = δrd（k + 1） + Ks Δδf（k + 1）    （29）

式中：δrd（k + 1）为汽车后轮转向角前馈控制量;Ks = δrmax /δfmax为比例系数.

3   仿真分析

本节利用车辆动力学仿真软件建立线控四轮转向汽车平行泊车路径规划与跟踪控制方法的模型在环仿真系统，对其可行性和有效性进行验证. 以天津一汽夏利N5整车参数为基础设计线控四轮转向汽车的仿真参数为L = 4.155 m，W = 1.645 m，Lf=0.8 m，Lr = 0.95 m，vrmax = 1 m·s-1，armax = 2 m·s-2，δfmax = 0.524 rad，δrmax = 0.087 3 rad，ωfmax = ωrmax = 0.524 rad·s-1;依据工程经验，仿真验证过程中粒子群优化算法的基础参数分别设置为d1 = 1.2，d2 = 1.5，qmin = 0.3，qmax = 1.5，smax = 1 000和εj = ξj = 10-3，种群规模设置为30、粒子速度向量边界区域设置为[-1，1]、粒子位置向量中表征汽车后轴中点加速度信息的分量的边界区域设置为[-armax，armax]，粒子位置向量中表征汽车前轮转向角速度信息的分量的边界区域设置为 [-ωfmax，ωfmax]，粒子位置向量中表征汽车后轮转向角速度信息的分量的边界区域设置为[-ωrmax，ωrmax]，算法迭代收敛判断容差设置为10-4;仿真结果如图5所示.

如图5（a） （b）所示，采用基于改进粒子群优化算法的平行泊车路径规划方法得到的平行泊车路径可以安全、舒适地引导线控四轮转向汽车进入平行泊车位，并且目标平行泊车路径曲率的连续性有效避免了线控四轮转向汽车原地转向现象.

如图5（c）～（f）所示，采用基于前馈控制和反馈控制相结合方式设计的平行泊车路径跟踪控制策略可以使线控四轮转向汽车快速、精确和稳定地跟踪目标泊车路径，进而安全无碰撞地完成平行泊车任务.

4   结   论

本文提出了一种线控四轮转向汽车平行泊车路径规划与跟踪控制方法. 综合考虑了线控四轮转向汽车运动学非完整约束、动力和转向子系统约束、避障约束和泊车位姿约束，建立了平行泊车路径规划约束最优化问题，并采用可以改进的粒子群优化算法对其进行求解来获得最优平行泊车路径. 采用前馈控制和PI反馈控制相结合的方式设计了平行泊车路径跟踪控制策略，实现对规划的平行泊车路径快速、精确及稳定的跟踪控制. 利用车辆动力学仿真软件构建模型在环系统，验证所提出的线控四轮转向汽车平行泊车路径规划与跟踪控制方法的可行性和有效性，结果表明：所提出的方法可以快速、精确、稳定地引导线控四轮转向汽车自动完成平行泊车任务.

参考文献

[1]    JANG C，KIM C，LEE S，et al. Re-plannable automated parking system with a standalone around view monitor for narrow parking lots[J]. IEEE Transactions on Intelligent Transportation Systems，2020，21（2）：777—790.

[2]    CHAI R Q，TSOURDOS A，SAVVARIS A，et al. Two-stage trajectory optimization for autonomous ground vehicles parking maneuver[J]. IEEE Transactions on Industrial Informatics，2019，15（7）：3899—3909.

[3]    张家旭，赵健，施正堂，等. 基于回旋曲线的平行泊车路径规划和跟踪控制[J]. 吉林大学学报（工学版），2020，50（6）：2247—2257.

ZHANG J X，ZHAO J，SHI Z T，et al. Path planning and tracking control for parallel parking based on clothoid curve[J]. Journal of Jilin University （Engineering and Technology Edition），2020，50（6）：2247—2257. （In Chinese）

[4]    CHAND A，KAWANISHI M，NARIKIYO T. Application of sigmoidal gompertz curves in reverse parallel parking for autonomous vehicles[J]. International Journal of Advanced Robotic Systems，2015，12（9）：127.

[5]    王健. 汽車在两种转向工况下的路径规划与路径跟踪研究[D]. 南京： 南京航空航天大学，2015：32—35.

WANG J. Research on path planning and path tracking for automobile in two steering conditions[D]. Nanjing：Nanjing University of Aeronautics and Astronautics，2015：32—35. （In Chinese）

[6]    LIU W，LI Z H，LI L，et al. Parking like a human：a direct trajectory planning solution[J]. IEEE Transactions on Intelligent Transportation Systems，2017，18（12）：3388—3397.

[7]    DEMIRLI K，KHOSHNEJAD M. Autonomous parallel parking of a car-like mobile robot by a neuro-fuzzy sensor-based controller[J]. Fuzzy Sets and Systems，2009，160（19）：2876—2891.

[8]    LI B，WANG K X，SHAO Z J. Time-optimal maneuver planning in automatic parallel parking using a simultaneous dynamic optimization approach[J]. IEEE Transactions on Intelligent Transportation Systems，2016，17（11）：3263—3274.

[9]    张家旭，赵健，施正堂，等. 采用hp自适应伪谱法的全自动泊车系统轨迹规划与跟踪控制[J]. 西安交通大学学报，2020，54（6）： 176—184.

ZHANG J X，ZHAO J，SHI Z T，et al. A trajectory planning and tracking control method for fully-automatic parking system using hp-adaptive pseudo spectral method[J]. Journal of Xian Jiaotong University，2020，54（6）：176—184. （In Chinese）

[10]  赵海兰. 全自动平行泊车路径规划及路径跟踪研究[D]. 淄博： 山东理工大学，2017：25—26.

ZHAO H L. Study on path planning and path tracking method for fully-automatic parallel parking[D]. Zibo：Shangdong University of Technology，2017：25—26. （In Chinese）

[11]  张家旭，赵健，施正堂，等. 基于回旋曲线的垂直泊车轨迹规划与跟踪控制[J]. 东南大学学报（自然科学版），2020，50（1）：182—191.

ZHANG J X，ZHAO J，SHI Z T，et al. Trajectory planning and tracking control for perpendicular parking based on clothoid curve[J]. Journal of Southeast University （Natural Science Edition），2020，50（1）：182—191.  （In Chinese）

[12]  ZHANG J X，SHI Z T，YANG X，et al. Trajectory planning and tracking control for autonomous parallel parking of a non-holonomic vehicle[J]. Measurement and Control，2020，53（9/10）：1800—1816.

[13]  DU X X，TAN K K. Autonomous reverse parking system based on robust path generation and improved sliding mode control[J]. IEEE Transactions on Intelligent Transportation Systems，2015，16（3）：1225—1237.

[14]  ZHAO W Z，QIN X X，WANG C Y. Yaw and lateral stability control for four-wheel steer-by-wire system[J]. IEEE/ASME Transactions on Mechatronics，2018，23（6）：2628—2637.

[15]  HAGER W W. Runge-Kutta methods in optimal control and the transformed adjoint system[J]. Numerische Mathematik，2000，87（2）：247—282.

[16]  SUN J，WU X J，PALADE V，et al. Random drift particle swarm optimization algorithm：convergence analysis and parameter selection[J]. Machine Learning，2015，101（1/2/3）：345—376.

收稿日期：2020-07-10

基金项目：国家自然科学基金资助项目（51775235），National Natural Science Foundation of China（51775235）

作者简介：张家旭（1985—），男，黑龙江双城人，吉林大学高级工程师，博士后

通信联系人，E-mail：zhaojian@jlu.edu.cn