1. 项目概述
在自动驾驶技术快速发展的今天,模型预测控制(MPC)因其优秀的处理约束和多目标优化能力,已成为轨迹跟踪控制的核心算法之一。本文将基于《无人驾驶车辆模型预测控制》第四章内容,详细解析如何利用车辆运动学模型和MPC实现精确的轨迹跟踪控制,并通过CarSim/Simulink联合仿真验证算法效果。
这个项目特别适合两类读者:一是正在学习自动驾驶控制算法的在校学生,二是需要快速掌握工业级仿真工具链的工程师。通过本文,你将获得从理论推导到工程实现的完整知识链条,包括运动学模型建立、MPC控制器设计、联合仿真平台搭建等核心内容。
需要模型API调用? 免费领10W Token,多模型网关一键接入 Claude、DeepSeek 等主流模型。
2. 运动学模型建立与离散化
2.1 车辆运动学模型选择
在低速场景(通常低于15m/s)下,我们选择自行车模型(Kinematic Bicycle Model)作为基础模型。这个选择基于三个关键考量:
- 计算效率:相比复杂的动力学模型,运动学模型仅需4个状态变量,计算负担大幅降低
- 适用场景:低速条件下轮胎侧偏效应不明显,运动学模型足以准确描述车辆行为
- 教学价值:作为MPC入门,可以更清晰地展示控制算法的核心原理
模型的核心假设包括:
- 忽略轮胎侧偏力(无侧滑假设)
- 前后轮速度方向与车体方向一致
- 车辆为刚体且仅考虑二维平面运动
2.2 连续时间模型推导
定义车辆状态向量为:
x = [x, y, ψ, v]ᵀ
其中:
- (x,y)为车辆质心位置
- ψ为航向角
- v为纵向速度
控制输入为:
u = [a, δf]ᵀ
其中:
- a为纵向加速度
- δf为前轮转角
基于几何关系,建立连续时间运动学方程:
code复制ẋ = v·cos(ψ)
ẏ = v·sin(ψ)
ψ̇ = (v/L)·tan(δf)
v̇ = a
其中L为车辆轴距,典型轿车取值约2.8m。
2.3 离散化处理
采用一阶欧拉法进行离散化,设采样周期为Ts,得到离散状态更新方程:
matlab复制x(k+1) = x(k) + Ts*v(k)*cos(psi(k));
y(k+1) = y(k) + Ts*v(k)*sin(psi(k));
psi(k+1) = psi(k) + Ts*v(k)/L*tan(delta_f(k));
v(k+1) = v(k) + Ts*a(k);
在实际工程实现时,需要注意:
离散化误差会随Ts增大而显著增加,建议Ts不超过0.05s。对于高速场景,可考虑更高阶的离散化方法如Runge-Kutta。
3. MPC控制器设计
3.1 预测模型构建
MPC的核心是利用模型预测系统未来行为。我们首先需要对非线性模型进行线性化处理:
- 在当前状态点x(k)进行一阶泰勒展开
- 得到线性时变模型:
x(k+1) = A_k·x(k) + B_k·u(k) + c_k
通过递推推导,可以建立预测输出Y与控制序列U的线性关系:
Y = H·x(k) + G·U
其中H和G为预测矩阵,反映了系统动态特性。在实际编程实现时,可以采用递推法高效计算这些矩阵。
3.2 目标函数设计
采用标准的二次型代价函数:
math复制J = (Y-R)ᵀQ_y(Y-R) + UᵀQ_uU + ΔUᵀQ_ΔuΔU
各项含义:
- 第一项:跟踪误差惩罚(R为参考轨迹)
- 第二项:控制量惩罚
- 第三项:控制变化率惩罚
权重矩阵选择经验:
- Q_y对角线元素通常取[1,1,0.5,0.1],强调位置跟踪
- Q_u取值较小(如0.01),避免过度抑制控制量
- Q_Δu对平滑性至关重要,建议从0.1开始调试
3.3 约束处理
实际车辆存在物理限制,必须作为约束条件:
- 前轮转角限幅:|δf| ≤ δ_max(通常30°)
- 转角变化率:|Δδf| ≤ Δδ_max(通常10°/step)
- 加速度范围:a_min ≤ a ≤ a_max
在MATLAB中,这些约束可转化为QP问题的A_ineq·U ≤ b_ineq形式,通过quadprog求解。
4. CarSim/Simulink联合仿真实现
4.1 接口配置详解
CarSim与Simulink的协同工作需要精确的接口配置:
-
在CarSim中设置:
- 选择"Send/receive via Simulink"模式
- 定义输出变量:x,y,ψ,v,ψ̇等
- 定义输入变量:δf, 加速度指令
-
信号匹配要点:
- 单位统一(CarSim默认使用km/h和deg)
- 采样时间对齐(建议0.02s)
- 添加适当的信号滤波(防止高频振荡)
4.2 Simulink模型架构
推荐的分层建模结构:
-
顶层:
- CarSim S-Function接口模块
- MPC控制器模块
- 参考轨迹生成模块
-
MPC控制器实现方式:
- MATLAB Function Block(适合快速原型)
- S-Function(执行效率更高)
- MPC Toolbox(提供现成模块)
-
参考轨迹处理:
- 预存轨迹点(x_ref,y_ref,ψ_ref)
- 实时插值获取当前参考点
- 可加入前视距离调节(look-ahead)
4.3 参数调试技巧
经过多次实践,总结出以下调试经验:
-
预测时域选择:
- Np=20(对应0.4s@Ts=0.02s)
- Nc=5(控制时域不宜过长)
-
权重调整策略:
- 先调Q_y确保基本跟踪性能
- 再调Q_Δu平滑控制量
- 最后微调Q_u限制控制幅值
-
常见问题处理:
- 震荡:增大Q_Δu
- 滞后:减小Q_u或增加前馈
- 发散:检查线性化是否正确
5. 仿真结果与分析
5.1 双移线场景测试
在经典的双移线(Double Lane Change)场景下:
-
跟踪性能:
- 横向误差<0.1m
- 航向角误差<1°
- 速度跟踪平滑
-
控制量观察:
- 前轮转角变化连续
- 加速度在±2m/s²内
- 无控制量饱和现象
5.2 圆形轨迹测试
为验证算法对不同曲率路径的适应性:
-
低速工况(v=5m/s):
- 跟踪误差几乎为零
- 控制量呈现周期性变化
-
中速工况(v=10m/s):
- 出现约0.15m的相位滞后
- 需引入前馈补偿改善
5.3 性能优化建议
根据实测结果,给出三点优化方向:
-
自适应预测时域:
- 高速时增大Np
- 弯道处减小Np
-
曲率前馈:
- 根据路径曲率预计算δf_ff
- 显著减少跟踪滞后
-
权重在线调整:
- 直道侧重舒适性
- 弯道侧重跟踪精度
6. 关键代码解析
6.1 MPC求解核心代码
matlab复制function [delta_f, a] = mpc_kinematic_controller(x_current, ref_traj, params)
% 线性化模型
[A, B, C] = linearize_model(x_current, params);
% 构建预测矩阵
[H, G] = build_prediction_matrices(A, B, params.Np);
% 构造QP问题
[H_qp, f_qp] = build_qp_problem(H, G, x_current, ref_traj, params);
% 设置约束
[A_ineq, b_ineq] = build_constraints(params);
% 求解QP
U_opt = quadprog(H_qp, f_qp, A_ineq, b_ineq, [], [], [], [], [], ...
optimoptions('quadprog', 'Display', 'off'));
% 提取控制量
delta_f = U_opt(1);
a = U_opt(2);
end
6.2 模型线性化实现
matlab复制function [A, B, c] = linearize_model(x, params)
% 提取状态
psi = x(3); v = x(4);
L = params.L; Ts = params.Ts;
% 计算雅可比矩阵
A = eye(4);
A(1,3) = -Ts*v*sin(psi);
A(1,4) = Ts*cos(psi);
A(2,3) = Ts*v*cos(psi);
A(2,4) = Ts*sin(psi);
A(3,4) = Ts*tan(params.delta_f)/L;
B = zeros(4,2);
B(3,1) = Ts*v/(L*cos(params.delta_f)^2);
B(4,2) = Ts;
% 常数项
c = [Ts*v*cos(psi) - A(1,3)*psi;
Ts*v*sin(psi) - A(2,3)*psi;
Ts*v*tan(params.delta_f)/L - A(3,4)*v;
0];
end
6.3 工程实现建议
-
代码优化技巧:
- 预分配数组内存
- 将不变计算移到循环外
- 使用MEX加速关键函数
-
调试工具:
- Simulink Data Inspector
- CarSim Plot Manager
- MATLAB Profiler
-
版本控制:
- 使用Git管理模型和代码
- 为每次参数调整创建分支
- 记录完整的仿真条件
7. 常见问题解决方案
7.1 联合仿真问题排查
-
通信失败:
- 检查CarSim模型年份设置
- 验证信号名称大小写
- 确认采样时间同步
-
数据异常:
- 添加Signal Conditioning模块
- 检查单位换算(deg/rad,kmph/mps)
- 验证初始状态一致性
7.2 MPC性能问题
-
求解失败:
- 检查H_qp正定性
- 放宽约束条件
- 尝试不同求解器
-
实时性不足:
- 减少预测步长
- 使用显式MPC
- 尝试代码生成
7.3 车辆模型失配
当出现模型失配时,可以考虑:
- 增加模型不确定性描述
- 引入扰动观测器
- 采用鲁棒MPC框架
经过实际项目验证,这套基于运动学模型的MPC控制器在低速场景下表现可靠,可作为更复杂控制算法的基础框架。建议读者先从本文介绍的基础版本入手,理解核心原理后再逐步扩展功能。
