粒子群优化算法在智能车辆轨迹跟踪的应用

丁志成，王甜甜

（1.郑州工业应用技术学院机电工程学院，河南 郑州 451100；2.郑州西亚斯学院电子工程学院，河南 郑州 451150）

1 引言

随着自动化驾驶技术的不断发展，智能车辆的控制研究正逐渐成为研究的热点。路径规划和轨迹跟踪都被视为自动驾驶技术的两个重要任务，直接影响到车辆运行过程中的安全性。

国内外研究学者在自动驾驶车辆的路径轨迹跟踪方面开展了大量研究，当前在不确定性和干扰方面，模型预测控制（MPC）已经逐渐得到了广泛的应用。针对自动驾驶汽车，文献[2]建立了一个通用运动学和动力学运动模型，设计了基于MPC的主动转向控制器。文献[3]考虑到运动过程中的干扰，提出鲁棒MPC控制器，以保证控制精度和稳定性。文献[4]基于MPC模型提出了一种自适应轨迹跟踪器，该控制器在不同车速工况下具有较小的跟踪误差，仿真结构表明该控制方法能有效控制轨迹跟踪精度。文献[5]针对车速和车道信息，提出了车辆自抗扰控制方法，仿真了不同工况下的轨迹跟踪路径，结构表明该方法具有较强的鲁棒性能。文献[6]针对农用车辆的轨迹控制，在单神经元理论基础上提出了自适应PID路径跟踪方法，通过仿真表明该控制方法具有较高的控制精度和鲁棒性。文献[7]基于MPC模型研究了履带式车辆轨迹的跟踪策略，通过建立Bezier曲线的参考拟合路径，设计了轨迹跟踪控制器并进行实时反馈校正，该方法在实车试验结果得到了有效验证。文献[8]提出了基于配点法的MPC对车辆的轨迹跟踪问题进行了优化，该方法采用二次规划序列方法进行求解覅线性动力学问题，在自动驾驶仿真环境中对所提方法的精度控制优势进行了验证。文献[9]在MPC模型的基础上研究了智能车辆在低附着条件下的轨迹跟踪问题，设计了轨迹跟踪器，通过仿真表明该跟踪器具有良好的控制精度和鲁棒性。在模型预测控制方面的研究还有比较典型的参考文献[10-12]所述，这些研究成果为自动化驾驶技术的发展提供了良好的参考价值。

基于以上分析，本研究提出一种渐进模型预测控制方案（PMPCS）来同时处理车辆路径规划和轨迹跟踪问题，提出了一种基于改进粒子群优化（IPSO）的模型预测控制（MPC）方法。

将模型预测控制与人工势场（APF）相结合，将时变安全约束作为排斥力的范围和非对称的车道势场函数，通过将时变安全约束视为排斥力的范围和非对称车道势场函数来获得无碰撞路径。在规划模块和跟踪模块采用了相同的非线性运动学模型，设计了一种考虑交通信号灯约束的伪速度规划策略，在此基础上采用改进的粒子群算法（IPSO）来解决非线性MPC问题，最后通过仿真结果验证了该算法的有效性，与一般算法相比具有明显的优越性。

2 模型预测控制结构描述

2.1 车辆路径规划和跟踪系统结构

路径规划和跟踪系统的结构原理图，如图1所示。

图1 路径规划和跟踪系统结构示意图Fig.1 Structure Diagram of Path Planning and Tracking System

系统的输入包括环境信息、智能车辆的当前状态和最新控制时间的最优解。同时，控制层需要一个合适的求解范围，该范围可以设置为合适的常数值，也可以根据路径的曲率进行计算。

本研究提出了基于IPSO模型的预测控制算法，以寻找规划和控制模块的解决方案。该方案能有效地传递中间变量，减少了计算量。与已有结果不同，本研究还考虑了交通信号灯的影响。为了在绿灯时安全地通过十字路口，需要计算新的参考速度，以确保车辆能在预设时刻之前到达目标点。

2.2 车辆运动模型参数化描述

（1）车辆运动学模型：路径规划和跟踪过程根据车辆运动学模型计算最优控制序列。假设车辆为刚体，侧滑角β为0，图2中参考坐标的数学表达式为：

图2 车辆运动学模型Fig.2 Vehicle Kinematics Model

式中：[x，y，φ]T—在车辆中心的位置和偏航角；［v，δf］T—控制输入，即速度和前轮转向角；L—轴距。

通过欧拉法对式（1）离散为：

（2）障碍物车辆描述：为保证所生成路径的安全，相邻障碍物的运动状态可用两自由度运动学模型来描述：

其中，输入为第i个障碍物的角速度ωobs和切向速度vobs，状态为位置和航向角。

（3）纵向安全距离：在典型超车场景中，智能车辆以较高的速度行驶在障碍车后面。规划模块需要生成无碰撞路径，以便安全超车。智能车辆在超越障碍物后应沿原车道中心行驶。为了确定合适的超车距离，将安全距离Ds定义为[13]：

式中：vh—智能汽车的车速；vobs—障碍车辆的车速；amax—智能车辆的最大减速带；aobs，max—障碍车辆的最大减速度，对紧急情况有显著影响；tx—系统的响应延时，两车之间的最小纵向距离用d0表示。

3 基于模型预测控制的轨迹规划

由于交通的动态特性，在样本周期T内需要计算智能车辆的动态响应。本研究提出了APF方法来处理时变安全约束，并将势函数作为代价函数的惩罚项。

3.1 时变安全约束

为防止车辆与障碍物、道路边界相干涉，应约束智能车辆的横向位移，使车辆保持在车道中心行驶，则存在以下约束条件：

其中，ymin和ymax—最小和最大横向位移。此外，受转向系统和电机的限制，车辆的控制输入也受到限制，即：

其中，vmin和vmax受交通规则限制。当智能车辆接近障碍物时，需要考虑安全距离约束［14］：

式中：xr，i（k）—在时间k时，两车沿纵向之间的相对距离；yr，i（k）—两车沿横向之间的相对距离；Lx—纵向安全距离，等于前述的Ds。Ly—横向安全距离。在式（7）中，如果选择左侧车道超车且左侧车道可用，则不等式符号为“-”，否则为“+”

3.2 与车道相关的势场函数

为了找到一个无碰撞的路径，在考虑到上述约束条件下，首先将安全约束转换为势场的范围［15］（表示不可驾驶区域，并将该区域定义为Ω），如式（8）所示。此时，不等式的符号由“≤”替换为“≥”，得到不可行区域。此外还应解决输出约束问题，势场的范围，如图3所示。

图3 障碍物的势场描述Fig.3 Description of the Obstacle Potential Field

在本研究中，ymin，Ly（k）和ymax，Ly（k）需要满足横向安全距离。此外，智能车辆与第i个障碍车辆的相对距离用do，i表示，在该区域中，排斥函数可以表述为：

其中，Ao和γo分别表示为势场的幅度和标准差。此外，还定义了车道势场函数。为了从左侧超车，对要超车一侧的车道线给予较低的权重。通过这种方式，可以通过启发式的方式将决策过程部分引入路径规划模块中，如图4所示。

图4 不同振幅车道边界的APF值Fig.4 APF Values of the Lane Boundaries with Different Amplitude

与障碍物势场函数类似，车道势场定义为：

式中：γr—车道收敛系数；Ar，j—第j车道的势场幅值，本研究只考虑dr，j（k）≤Ds时的车道势函数，即车辆超车。

3.3 特定情况下的参考速度计算

本研究考虑典型的运行工况，当司机驾驶车辆靠近交通灯附近时，会进行适当的加速∕减速动作，为了使所设计的路径规划算法达到与人类驾驶员相似的性能，在分析过程中做一些简化。

首先，将交通灯问题视为避障物问题，即红灯可以描述为某一时刻发生的静态障碍物（灯障）。进一步构造二维状态空间ΨTL，包括时间t和纵向位置x，可以定义为：

同样地，灯障状态OTL被描述为：

式中：s、Δt—光障的纵向位置和持续时间。因此，涉及灯障的避障问题可表述为：

即车辆q'的实际状态不属于车辆状态所描述的障碍状态集，因此可以在优化过程中引入状态约束来实现避障。为了简化避障问题，本研究构建车辆纵向距离的时序图，如图5所示。红线和绿线表示交通信号灯的状态，蓝线表示车辆运行状态。

图5 不同速度下的交通灯状态Fig.5 Traffic Light Status at Different Speeds

图中虚线斜率较大，该状态可以在红灯状态前通过目标点，同时保持相同的斜率。实线由于速度较慢，需要等待红灯状态（斜率为0，速度为0），当信号灯变为绿色时，车辆将以原来的相同速度运行。

如果可以通过V2X或车载传感器得到交通灯与智能车辆的相对距离Sleft，以及剩余的通过时间Tleft，则可计算出最小速度为：

其中，Atra—增益系数，使参考速度略高于最小速度；Vref，old—最新样本周期计算出的参考速度。常数ε是在剩余时间不足时，避免超速的阈值（在这种情况下，驾驶员可能会减速或者停车）。此外，考虑到超车时间的约束，可以通过增大初始速度以使得车辆加速，直到超车动作结束，因此有：

式中：γv，o—障碍车辆速度收敛系数；γv，tra—交通信号灯速度收敛系数；ζ—指定的距离，用于确定是否需要考虑红绿灯；α、ρ—加速因子。

3.4 轨迹规划的模型预测控制

为了得到无碰撞路径，提出的优化问题为：

成本函数为：

式中：Np—预测层；Nc—控制层；Ψ—预测状态；U—控制输入，表述为：

在每个采样瞬间k处，将计算出最优控制序列，并得到预测范围内相应的车辆状态Np。为了达到跟踪的目的，使用这两个序列来初始化跟踪控制器的优化问题和跟踪控制器的参考路径。这种处理方法允许在两层优化之间进行无缝连接，从而可以大大减少计算量。

4 跟踪控制器设计

本节针对PMPCS设计了一种基于改进的粒子群优化算法的MPC跟踪控制器，传统的MPC方法需要根据成本函数预测状态并计算最优控制，计算量较大。因此，一般采用上层基于MPC的规划控制算法来提供满足要求的参考控制量。另一方面，我们使用诸如PID这样的低计算负担的控制算法在底层实现跟踪过程。

4.1 轨迹跟踪的模型预测控制

与上述规划部分类似，本研究设计了特定的横向控制器，采用相同的运动学模型来预测智能车辆的未来状态，以确保车辆能够跟踪参考路径。由于优化准则和优化目标不同，因此需要设计跟踪控制器。为了在跟踪过程中获得良好的性能，提出以下优化问题：

式中：Ψt—控制模块的预测状态；Ψp—参考状态；Uc—控制输入。定义成本函数为：

其中第一项的目的是使输出状态尽可能接近参考状态，第二项是控制输入增量的约束，以保证乘客的舒适性。Ξ和Θ是每部分的权重。较小的J表示控制器输出位置与规划轨迹的偏差趋于0，转向更平稳。

4.2 基于改进粒子群优化的控制方案

考虑式（16）和式（19）中的优化问题，采用粒子群优化算法（PSO）将两个独立的粒子初始化为控制输入δf和v，由式（2）计算相应的状态输出，并将适应度定义为成本（20）的负值。选取全局最优粒子来更新粒子，当算法迭代次数达到最大或满足适应度阈值时，输出执行器的最优控制序列。速度和位置的更新公式可表示为：

其中，在p.δ和p.S是两个独立的粒子，分别对应于控制输入δf和v。p.vδ和p.vs表示相应的速度。正整数c1，c2，c3，c4表示学习因子。r1、r2、r3、r4是在区间［0，1］内随机分布的常数。νδ和νs分别代表δf和v的惯性因子，pb和gb分别为当前迭代的最佳粒子和全局最佳粒子。

为了解决优化问题，将每个粒子的长度扩展到Nc来计算成本，对每个粒子进行迭代计算，以找到最优粒子。在每个样本周期内，最优解不会发生显著变化，本研究存在两个改进的方面：

（1）PMPCS：PMPCS是一种渐进式方案，具有规划和跟踪的能力，可以在两层之间有效地传输信息。在该方案中，规划模块从最优问题和运动学模型中计算出最优序列和对应的状态，然后将每个采样周期的状态作为跟踪模块的参考。由于相同的预测模型和相同的优化变量，这两个优化解位于狭窄的区域，使得初始粒子成本较低。

由于多项式的导数是一阶和二阶连续的，本研究采用四次多项式拟合参考偏航角和横向位移随预测水平Np的变化曲线，即加速度和角加速度是连续的，则有：

采样获取的拟合曲线，如图6所示。可以看出这些曲线的偏差较小，可作为下一次优化的初始值。对于每个粒子，可以根据给出的初始值U'=［U（2：Np），0］进行迭代计算，在后续的迭代中将基于最新的最优解开始，以减少搜索空间。

图6 规划模块在超车过程中计算的参考路径Fig.6 Reference Paths Calculated by the Planning Module During Overtaking

（2）改进粒子群算法（Improved PSO）：为了提高粒子群算法（PSO）的收敛速度，采用以下公式来计算线性惯性因子［17］：

式中：k—迭代次数；M—最大迭代次数；wmax—惯性因子的最大值；wmin—惯性因子的最小值，一般取值为0.9和0.4。

为了验证所提出的IPSO算法能够有效地加快收敛速度，本研究采用式（24）进行优化：

在模拟中，最大迭代次数为500，种群大小为50。迭代终止条件为适应度误差小于0.00001或达到最大迭代量。两种优化算法的函数图像和收敛速度，如图7所示。可以看出IPSO可以更快地达到较优的适应度，而PSO的迭代精度较差。10次测试数据的均值，如表1 所示。可以看出PSO 算法的平均迭代次数为339.3，远远大于IPSO算法的84.9。因此可以看出IPSO算法可以有效地提高算法的收敛速度。

表1 PSO与IPSO的比较Tab.1 Comparison for PSO and IPSO

图7 粒子群优化算法的比较Fig.7 Comparison of Particle Swarm Optimization Algorithms

5 仿真结果

本研究在所提模型的基础进行仿真，以验证算法的有效性，参数，如表2所示。如果首选左侧超车，可以将Ar，2=0和Ar，3=15，否则选择Ar，2=15，Ar，3=0。

表2 算法的设计参数Tab.2 Design Parameters of Algorithm

超车轨迹，如图8 所示。车辆以18m∕s 的速度在中心车道行驶，障碍车辆以12m∕s 的速度在同一车道行驶，直道长150m，首选从左侧超车。智能车辆采用红线和符号表示，障碍用黑色标记表示。可以看出两辆车没有发生碰撞，智能车辆也很少偏离车道中心。

图8 路径规划的仿真结果Fig.8 Simulation Results for Path Planning

在仿真中还考虑了交通灯，交通灯距离原点80m，绿灯在前4s工作。在图9（a）中，实线表示智能车辆首先加速通过红绿灯，即实现穿过绿色部分，虚线穿过红色部分。随后速度下降到最初的参考速度。每个采样瞬间的最佳参考速度，如图9（b）所示。

图9 考虑交通灯的仿真结果Fig.9 Simulation Results by Considering Traffic Lights

为了证明控制算法的有效性，提出了Simulink∕CarSim 联合仿真。场景是在惯性坐标系上跟踪一条幅度为3.6m 的步进轨迹，即跟踪一条初始误差为3.6m的直线。纵向速度为5m∕s。控制性能，如图10所示。平均横向误差为0.0272m，可以看出所提的IPSO-NMPC 跟踪控制器是有效的。

图10 跟踪算法的仿真结果Fig.10 .Simulation Results of Tracking Algorithm

优化后的规划结果与最终跟踪结果的对比，如图11 所示。其中图11（a）、图11（b）为规划和跟踪结果。由于跟踪算法限制了控制增量的幅度，因此速度曲线存在较大差异。结果表明，当参考速度变化较大时，输出速度呈斜坡状。

图11 协同控制仿真结果Fig.11 Simulation Results for Cooperative Control

从图11（c）可以看出，最终的输出曲线比计划的输出曲线更加平滑。在不考虑规划模块传递到跟踪参数（这种情况相当于给定跟踪轨迹）的情况下，图11（d）和图11（e）显示了相应的计算时间，可以看出由于更大的搜索空间和迭代次数，计算时间明显高于PMPCS。这表明所提的协调策略和IPSO 方法大幅减少了计算时间。

6 结论

本研究针对智能车辆的局部规划与路径跟踪的协同控制问题，提出了一种基于改进粒子群优化（IPSO）的模型预测控制（MPC）方法。在规划模块中提出了由时变约束确定的非对称车道势场函数和障碍场函数的范围，以获得无碰撞路径。

在此基础上，将APF与IPSO-MPC相结合，采用伪速度规划方法对交通灯进行规划，从而有效地解决了路径优化问题。采用考虑线性惯性因素的粒子群算法（IPO），在解决非线性规划和系统计算效率方面具有一定优势。最后该算法的有效性通过仿真得到了验证。