姬轩亦1. 项目概述
在智能驾驶技术快速发展的今天，车辆轨迹跟踪控制作为核心技术之一，其性能直接影响着行驶安全性、稳定性和舒适性。传统控制方法如PID控制在面对车辆动态特性变化和复杂路况干扰时，往往难以兼顾跟踪精度与控制实时性。模型预测控制(MPC)凭借其多步预测、滚动优化和反馈校正的特点，成为解决这一问题的有效方案。
本项目基于二自由度车辆动力学模型，在Matlab环境下实现了常规模型预测控制和自适应模型预测控制两种算法，用于车辆轨迹跟踪。通过构建完整的仿真验证平台，我们能够对控制算法进行参数化配置和性能评估，为智能驾驶车辆的轨迹跟踪控制提供理论与仿真参考。
提示：本文所有实验均在Matlab2018b环境下完成，代码兼容2018及以上版本，无需额外工具箱支持。
需要模型API调用？ 免费领10W Token，多模型网关一键接入 Claude、DeepSeek 等主流模型。
2. 核心原理与技术路线
2.1 模型预测控制基本原理
模型预测控制的核心思想可以概括为"预测-优化-反馈"三个步骤：
- 预测模型：基于当前状态和控制输入，预测系统未来一段时间内的行为
- 滚动优化：在每个控制周期求解一个有限时域的最优控制问题
3.反馈校正：将第一个控制量应用于系统，下一周期重新开始整个过程
这种控制方式的优势在于：
- 能够显式处理多输入多输出系统的耦合问题
- 可以方便地加入各种约束条件
- 通过滚动优化实现闭环控制，对模型误差和干扰具有一定鲁棒性
2.2 二自由度车辆模型
本项目采用的二自由度车辆模型(自行车模型)是车辆动力学研究中的经典模型，它考虑了车辆的横向运动和横摆运动，忽略了纵向、垂向等运动耦合影响。模型的基本假设包括：
- 忽略悬架系统的影响
- 假设左右轮胎特性相同
- 小角度假设，即侧偏角较小
车辆动力学方程可以表示为：
code复制m(v̇y + vxφ̇) = Fyf + Fyr
Izφ̈ = lfFyf - lrFyr
其中：
- m为车辆质量
- vx,vy分别为纵向和横向速度
- φ为横摆角
- Iz为横摆转动惯量
- lf,lr为前后轴到质心的距离
- Fyf,Fyr为前后轮胎侧向力
2.3 自适应模型预测控制
在常规MPC基础上，我们引入了自适应机制，主要改进包括：
- 参数自适应：根据车辆状态实时调整模型参数
- 权重自适应：根据跟踪误差动态调整优化目标中各分量的权重
- 时域自适应：根据行驶状况调整预测时域和控制时域
这种自适应策略能够显著提升系统对车辆参数变化和外部干扰的鲁棒性。
3. 实现细节与代码解析
3.1 仿真环境搭建
整个项目采用模块化设计，主要包含以下文件：
code复制MPC_Vehicle_Tracking/
├── main.m # 主程序入口
├── param.m # 参数初始化脚本
├── vehicle_model.m # 车辆动力学模型
├── mpc_controller.m # MPC控制器实现
├── adaptive_mpc.m # 自适应MPC实现
├── trajectory_gen.m # 参考轨迹生成
└── plot_results.m # 结果可视化
3.2 核心代码实现
3.2.1 车辆模型实现
matlab复制function [x_dot, y] = vehicle_model(t, x, u, param)
% 状态变量: x = [vy, phi, phidot, Y, X]
% 控制输入: u = delta_f (前轮转角)
% 参数解包
m = param.m;
Iz = param.Iz;
lf = param.lf;
lr = param.lr;
Cf = param.Cf;
Cr = param.Cr;
vx = param.vx;
% 计算侧偏角
alpha_f = atan((x(1) + lf*x(3))/vx) - u;
alpha_r = atan((x(1) - lr*x(3))/vx);
% 计算侧向力
Fyf = -Cf * alpha_f;
Fyr = -Cr * alpha_r;
% 状态方程
vy_dot = (Fyf + Fyr)/m - vx*x(3);
phi_ddot = (lf*Fyf - lr*Fyr)/Iz;
X_dot = vx*cos(x(2)) - x(1)*sin(x(2));
Y_dot = vx*sin(x(2)) + x(1)*cos(x(2));
x_dot = [vy_dot; x(3); phi_ddot; Y_dot; X_dot];
y = [x(4); x(5); x(2)]; % 输出Y,X,phi
end
3.2.2 MPC控制器实现
matlab复制function [u, info] = mpc_controller(x, ref, param, mpc_param)
% 建立优化问题
opti = casadi.Opti();
% 决策变量
U = opti.variable(mpc_param.nu, mpc_param.Nc);
X = opti.variable(mpc_param.nx, mpc_param.Np+1);
% 初始条件
opti.subject_to(X(:,1) == x);
% 动力学约束
for k = 1:mpc_param.Np
x_next = rk4(@(t,x,u)vehicle_model(t,x,u,param), ...
0, X(:,k), U(:,min(k,mpc_param.Nc)), mpc_param.dt);
opti.subject_to(X(:,k+1) == x_next);
end
% 控制量约束
opti.subject_to(-param.delta_max <= U <= param.delta_max);
opti.subject_to(-param.ddelta_max <= diff(U,1,2)/mpc_param.dt <= param.ddelta_max);
% 代价函数
obj = 0;
for k = 1:mpc_param.Np
obj = obj + (X(4:5,k)-ref(1:2,k))'*mpc_param.Q*(X(4:5,k)-ref(1:2,k));
obj = obj + (X(3,k)-ref(3,k))^2*mpc_param.q_phi;
end
for k = 1:mpc_param.Nc
obj = obj + U(:,k)'*mpc_param.R*U(:,k);
end
opti.minimize(obj);
% 求解
opti.solver('ipopt');
sol = opti.solve();
u = sol.value(U(:,1));
info = sol.stats;
end
3.3 自适应策略实现
自适应机制主要通过以下方式实现：
- 模型参数在线更新：
matlab复制function param = update_parameters(param, x, u, est_error)
% 根据估计误差调整轮胎侧偏刚度
if est_error > param.error_threshold
param.Cf = param.Cf * (1 + param.adapt_gain * est_error);
param.Cr = param.Cr * (1 + param.adapt_gain * est_error);
end
end
- 权重矩阵调整：
matlab复制function mpc_param = adjust_weights(mpc_param, tracking_error)
% 根据跟踪误差调整权重
if norm(tracking_error) > mpc_param.error_norm_threshold
mpc_param.Q = mpc_param.Q * (1 + mpc_param.weight_gain);
mpc_param.R = mpc_param.R / (1 + mpc_param.weight_gain);
end
end
4. 仿真结果与分析
4.1 双移线工况测试
我们首先在双移线工况下测试控制器的性能。参考轨迹如下：
matlab复制function ref = double_lane_change(t, param)
% 生成双移线参考轨迹
X_ref = param.vx * t;
Y_ref = zeros(size(t));
% 第一段变道
idx1 = find(X_ref >= 20 & X_ref < 40);
Y_ref(idx1) = 3.5/2 * (1 - cos(pi*(X_ref(idx1)-20)/20));
% 第二段变道
idx2 = find(X_ref >= 60 & X_ref < 80);
Y_ref(idx2) = 3.5/2 * (1 + cos(pi*(X_ref(idx2)-60)/20));
% 横摆角参考
phi_ref = zeros(size(t));
phi_ref(idx1) = atan(3.5*pi/40 * sin(pi*(X_ref(idx1)-20)/20));
phi_ref(idx2) = -atan(3.5*pi/40 * sin(pi*(X_ref(idx2)-60)/20));
ref = [Y_ref; X_ref; phi_ref];
end
测试结果显示：
- 常规MPC最大横向误差：0.15m
- 自适应MPC最大横向误差：0.08m
- 自适应MPC在变道区域表现出更好的跟踪性能
4.2 参数摄动测试
为验证控制器的鲁棒性，我们在仿真中引入以下参数变化：
- 车辆质量增加20%
- 轮胎侧偏刚度减小15%
- 加入横向风干扰
测试结果表明：
- 常规MPC跟踪误差增大约50%
- 自适应MPC能够维持与标称工况相近的跟踪精度
- 自适应机制有效补偿了参数变化带来的影响
4.3 实时性分析
在Intel i7-10750H处理器上运行测试：
- 常规MPC单步求解时间：8-12ms
- 自适应MPC单步求解时间：10-15ms
- 满足50Hz控制频率要求
注意：实际应用中可能需要进一步优化代码或降低预测时域以满足更高频率的控制需求。
5. 关键参数调节指南
5.1 权重矩阵调节
权重矩阵Q和R的选择对控制性能有重要影响：
-
Q矩阵：状态误差权重
- 增大Q(1,1)和Q(2,2)：加强横向位置跟踪
- 增大Q(3,3)：加强横摆角跟踪
-
R矩阵：控制量权重
- 增大R：减小控制量幅值，但可能降低跟踪精度
- 减小R：提高跟踪精度，但可能导致控制量波动
建议调节步骤：
- 初始设置Q=diag([1,1,0.1]), R=0.01
- 根据跟踪效果逐步调整
- 使用Bryson法则进行归一化
5.2 时域参数选择
预测时域(Np)和控制时域(Nc)的选择原则：
-
预测时域：
- 一般选择Np=10-20
- 太短：预测不足，性能下降
- 太长：计算负担增加，可能引入过多未来不确定信息
-
控制时域：
- 通常Nc=3-5
- 可以小于Np以减少决策变量
经验公式：
code复制Np = ceil(prediction_time / dt)
Nc = ceil(control_time / dt)
其中prediction_time一般取1-2秒，control_time取0.3-0.5秒
5.3 自适应参数设置
自适应机制的关键参数：
-
误差阈值：
- error_threshold：触发参数调整的误差阈值
- 一般设为期望跟踪精度的1.5-2倍
-
调整增益：
- adapt_gain：参数调整幅度
- 通常取0.1-0.3
- 过大可能导致振荡
-
调整速率限制：
- 应限制参数的最大变化率
- 避免参数突变导致系统不稳定
6. 常见问题与解决方案
6.1 求解失败问题
问题现象：优化求解器报错或无法找到可行解
可能原因及解决方案：
-
初始猜测不合理：
- 提供更好的初始猜测
- 使用上一时刻的解作为初始猜测
-
约束过于严格：
- 适当放宽约束条件
- 逐步收紧约束测试可行性
-
数值问题：
- 检查模型方程是否正确
- 对变量进行缩放
6.2 跟踪误差大
问题现象：车辆无法准确跟踪参考轨迹
排查步骤：
- 检查车辆模型参数是否准确
- 验证参考轨迹生成是否正确
- 调整MPC权重矩阵
- 检查控制量是否达到饱和
- 考虑增加预测时域
6.3 控制量抖动
问题现象：控制输入频繁大幅变化
解决方案：
- 增加控制量变化率约束
- 增大控制量权重R
- 考虑添加低通滤波
- 检查采样时间是否合适
6.4 实时性不足
问题现象：单步求解时间超过控制周期
优化方法：
- 减少预测时域和控制时域
- 使用热启动技巧
- 考虑显式MPC或近似方法
- 代码优化：向量化运算，减少内存分配
7. 扩展与改进方向
7.1 非线性MPC实现
当前实现使用了线性化模型，可以考虑：
- 直接使用非线性车辆模型
- 采用更精确的轮胎模型（如Pacejka模型）
- 使用符号计算自动推导动力学方程
7.2 多车协同控制
扩展应用场景：
- 添加通信模块实现车车通信
- 设计协同MPC框架
- 考虑避碰约束
7.3 硬件在环测试
提升工程实用性：
- 与CarSim/PreScan等专业软件联合仿真
- 开发快速原型控制系统
- 实车测试验证
7.4 机器学习增强
结合AI技术：
- 使用神经网络拟合复杂动力学
- 强化学习优化MPC参数
- 数据驱动的不确定性建模
在实际项目中，我们发现自适应MPC的参数调整需要特别注意稳定性问题。一个实用的技巧是先用常规MPC获得满意的基准性能，再逐步引入自适应机制，这样可以有效避免参数调整过程中的不稳定现象。另外，对于不同的参考轨迹类型，可能需要不同的自适应策略参数，这在实际应用中需要积累经验。