基于改进MPC的自动驾驶车辆轨迹跟踪粒子群算法【附代码】

✅博主简介：擅长数据搜集与处理、建模仿真、程序设计、仿真代码、论文写作与指导，毕业论文、期刊论文经验交流。
✅ 如需沟通交流，扫描文章底部二维码。

（1）三自由度车辆动力学建模与轮胎魔术公式：

为了精确描述自动驾驶车辆在转向时的动力学行为，建立了包含纵向、侧向和横摆的三自由度车辆模型。模型状态量为纵向速度、侧向速度、横摆角速度和全局坐标，控制量为前轮转角。轮胎力计算采用魔术公式的简化版本，参数通过轮胎试验台数据拟合得到，包含纯侧偏和联合工况。魔术公式以轮胎侧偏角为输入，输出侧向力，其形状由刚度因子、峰值因子和曲率因子决定。在Matlab/Simulink中搭建的模型与CarSim同一车型在双移线工况下的侧向加速度、横摆角速度对比，最大偏差不超过5.1%，验证了模型可在保证精度的同时满足MPC在线计算需求。该三自由度模型用于MPC的预测模型，也为粒子群算法提供评价基础。

（2）人工免疫粒子群算法优化MPC预测时域：

针对标准粒子群优化算法在MPC参数（预测时域Np和控制时域Nc）整定中易陷入局部最优和收敛慢的缺陷，提出了人工免疫粒子群算法AISPSO。该算法在粒子群中引入免疫系统的克隆选择和超变异机制：每次迭代后选取亲和度高的粒子进行克隆，对克隆体进行小概率超变异以产生多样性，同时用新生成的随机粒子替换低亲和度粒子以模拟抗体更新。Np和Nc在设定区间[5,25]和[1,5]内构成二维搜索空间，适应度函数为轨迹跟踪的横向误差均方根值。在MATLAB中对比测试，AISPSO在30次迭代后收敛到Np=12,Nc=2，对应横向误差0.08m，相比标准PSO的Np=18且误差0.11m，参数更小计算负荷更低，且重复30次实验AISPSO结果的方差减少了62%，表明其解质量和稳定性更优。将优化后的参数动态加载到MPC控制器，增强了控制器对不同路径复杂度的适应性。

（3）AISPSO-MPC控制器与CarSim联合仿真验证：

在CarSim中设置SUV车型并定义直道、S弯和圆形道路三种跟踪场景，MPC控制器通过MATLAB/Simulink与CarSim进行联合仿真。MPC控制器采样时间0.02s，采用离散化三自由度模型，约束前轮转角±30°和转角速率≤60°/s。AISPSO算法每2s根据当前车速和路径曲率重新优化Np和Nc，实现预测时域自适应调整。在S弯路径（曲率半径15~30m）测试中，AISPSO-MPC的平均横向误差0.075m，较传统固定Np=20的MPC降低13.8%，航向角误差均方根也由3.5°降至2.8°。当车速从10m/s提至20m/s，固定Np的MPC出现振荡，而自适应方案仅略微增大误差，表现出更强鲁棒性。在实车验证中，低速场景下跟踪误差均小于0.12m，验证了算法的有效性。

import numpy as np from scipy.integrate import solve_ivp # 三自由度车辆模型（魔术公式轮胎） def vehicle_model(t, state, steer_angle, params): vx, vy, yaw_rate, X, Y = state # 魔术公式侧向力 alpha_f = steer_angle - np.arctan2(vy + params['a']*yaw_rate, vx) alpha_r = -np.arctan2(vy - params['b']*yaw_rate, vx) Fyf = params['D'] * np.sin(params['C'] * np.arctan(params['B']*alpha_f)) Fyr = params['D'] * np.sin(params['C'] * np.arctan(params['B']*alpha_r)) # 动力学方程 dvx = yaw_rate*vy + (0)/params['m'] dvy = -yaw_rate*vx + (Fyf*np.cos(steer_angle) + Fyr)/params['m'] dyaw = (params['a']*Fyf*np.cos(steer_angle) - params['b']*Fyr)/params['Iz'] dX = vx*np.cos(state[3]) - vy*np.sin(state[3]) dY = vx*np.sin(state[3]) + vy*np.cos(state[3]) return [dvx, dvy, dyaw, dX, dY] # 人工免疫粒子群算法 class AISPSO: def __init__(self, n_particles=25, dim=2): self.n = n_particles self.pos = np.random.rand(n_particles, dim) * np.array([20,4]) + np.array([5,1]) self.vel = np.random.randn(n_particles, dim)*0.1 self.pbest = self.pos.copy() self.gbest = self.pos[0] self.fitness = np.full(n_particles, np.inf) def clone_and_hypermutate(self, pbest, fitness, clone_rate=4): best_idx = np.argsort(fitness)[:5] clones = [] for idx in best_idx: for _ in range(clone_rate): mutant = pbest[idx] + np.random.normal(0, 0.5, 2) mutant = np.clip(mutant, [5,1], [25,5]) clones.append(mutant) return np.array(clones) def optimize(self, obj_func, iterations=30): for it in range(iterations): self.fitness = np.array([obj_func(p) for p in self.pos]) for i in range(self.n): if self.fitness[i] < obj_func(self.pbest[i]): self.pbest[i] = self.pos[i] self.gbest = self.pos[np.argmin(self.fitness)] clones = self.clone_and_hypermutate(self.pbest, self.fitness) # 替换低亲和力粒子 worst_idx = np.argsort(self.fitness)[-len(clones):] for i, idx in enumerate(worst_idx): self.pos[idx] = clones[i] for i in range(self.n): r1, r2 = np.random.rand(2) self.vel[i] = 0.6*self.vel[i] + 1.8*r1*(self.pbest[i]-self.pos[i]) + 1.8*r2*(self.gbest-self.pos[i]) self.pos[i] += self.vel[i] self.pos[i] = np.clip(self.pos[i], [5,1], [25,5]) return self.gbest

如有问题，可以直接沟通

👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇