1. 项目概述与背景
在自动驾驶技术快速发展的今天,轨迹跟踪控制作为核心算法之一,直接决定了车辆的行驶安全性和乘坐舒适性。基于模型预测控制(MPC)的轨迹跟踪方法因其能够显式处理系统约束和优化控制性能,成为当前研究的热点。本文将详细介绍如何使用Matlab和Simulink搭建一个完整的车辆运动学模型MPC跟踪仿真系统,包含圆形、直线和双移线三种典型轨迹的跟踪场景。
提示:本文所有代码和模型均基于Matlab R2021b开发,建议使用相同或更高版本运行。
需要模型API调用? 免费领10W Token,多模型网关一键接入 Claude、DeepSeek 等主流模型。
2. 车辆运动学模型构建
2.1 自行车模型原理
车辆运动学模型采用经典的自行车模型,该模型做了以下合理假设:
- 忽略轮胎侧偏特性
- 将前后轮分别简化为单个轮子
- 仅考虑平面运动(忽略垂向运动)
模型的核心参数包括:
- L:轴距(前后轮距离)
- δ:前轮转向角
- v:车速
- θ:车辆航向角
运动学方程推导过程如下:
- 瞬时转向中心(ICC)位于后轴延长线上
- 转向半径R = L/tan(δ)
- 角速度ω = v/R = v·tan(δ)/L
- 由此得到运动学微分方程
2.2 Matlab实现细节
matlab复制function [x_next, y_next, theta_next] = kinematic_model(x, y, theta, v, delta, dt, L)
% 参数说明
% x,y: 当前车辆位置(m)
% theta: 航向角(rad)
% v: 车速(m/s)
% delta: 前轮转向角(rad)
% dt: 时间步长(s)
% L: 轴距(m)
% 位置更新
x_next = x + v * cos(theta) * dt;
y_next = y + v * sin(theta) * dt;
% 航向角更新
theta_next = theta + v / L * tan(delta) * dt;
% 角度归一化到[-pi, pi]
theta_next = wrapToPi(theta_next);
end
注意:实际实现中需要添加转向角限制(通常|δ|≤30°)、速度限制等物理约束条件。
3. 参考轨迹生成方法
3.1 圆形轨迹生成
圆形轨迹是最基础的测试场景,可用于验证控制器的稳态跟踪性能。
matlab复制function [x_ref, y_ref, theta_ref] = generate_circle(radius, center_x, center_y, num_points, direction)
% direction: 1-逆时针, -1-顺时针
t = linspace(0, 2*pi, num_points);
x_ref = center_x + radius * cos(direction * t);
y_ref = center_y + radius * sin(direction * t);
% 计算参考航向角
theta_ref = atan2(direction * (x_ref - center_x), -(y_ref - center_y));
end
参数选择建议:
- 半径:5-20m(模拟不同弯道曲率)
- 点数:根据仿真时长和步长确定,通常100-500点
3.2 直线与双移线轨迹
直线轨迹生成相对简单:
matlab复制function [x_ref, y_ref] = generate_line(start, finish, num_points)
x_ref = linspace(start(1), finish(1), num_points);
y_ref = linspace(start(2), finish(2), num_points);
end
双移线轨迹模拟车道变更场景:
matlab复制function [x_ref, y_ref] = generate_double_lane_change(length, width, num_points)
x = linspace(0, length, num_points);
y = width * (1 - cos(2*pi*x/length))/2;
x_ref = x';
y_ref = y';
end
4. MPC控制器设计与实现
4.1 MPC基本原理
模型预测控制的核心思想可以概括为:
- 预测:基于当前状态和模型预测未来状态
- 优化:求解最优控制序列使目标函数最小
- 执行:应用控制序列的第一个元素
- 滚动:下一时刻重复上述过程
4.2 控制器参数设置
matlab复制% 预测时域
N = 10; % 步数
% 权重矩阵
Q = diag([10, 10, 1]); % 状态误差权重[x,y,θ]
R = 0.1; % 控制量权重
Rd = 0.01; % 控制变化率权重
% 约束条件
delta_max = deg2rad(30); % 最大转向角
v_max = 5; % 最大速度(m/s)
参数调优经验:
- 增大Q可提高跟踪精度,但可能导致控制量振荡
- 增大R会使控制更平滑,但响应变慢
- N的选择需要平衡计算量和预测效果
4.3 优化问题求解
使用fmincon求解非线性优化问题:
matlab复制options = optimoptions('fmincon',...
'Algorithm','sqp',...
'MaxIterations',100,...
'Display','none');
[u_opt, fval] = fmincon(@(u) cost_function(x0, u, ref_traj, Q, R, Rd, N, L),...
u0, [], [], [], [],...
lb, ub, [], options);
代价函数设计示例:
matlab复制function J = cost_function(x0, u, ref, Q, R, Rd, N, L)
J = 0;
x = x0;
for k = 1:N
% 状态更新
x = kinematic_model(x(1), x(2), x(3), u(2*k-1), u(2*k), dt, L);
% 状态误差代价
J = J + (x - ref(:,k))' * Q * (x - ref(:,k));
% 控制量代价
if k > 1
J = J + (u(2*k-1)-u(2*k-3))^2 * Rd;
end
end
end
5. Simulink联合仿真实现
5.1 仿真框架搭建
推荐采用以下模块化结构:
- 轨迹生成模块 - 选择不同轨迹类型
- 车辆模型模块 - 实现运动学模型
- MPC控制器模块 - 封装优化算法
- 可视化模块 - 实时显示轨迹跟踪效果
5.2 关键模块配置
- MPC控制器模块:
- 使用Matlab Function模块封装优化代码
- 设置适当的采样时间(通常0.05-0.1s)
- 添加输入输出端口
- 车辆模型模块:
- 可直接调用kinematic_model函数
- 添加初始状态参数设置接口
- 可视化模块:
- 使用XY Graph显示参考轨迹和实际轨迹
- 添加Vehicle Animation工具箱模块增强可视化
5.3 仿真参数设置技巧
- 解算器选择:
- 固定步长ode4(Runge-Kutta)
- 步长与控制器采样时间一致
- 性能优化:
- 启用加速模式(Acelerator)
- 合理设置工作空间I/O
6. 典型问题与调试方法
6.1 跟踪误差过大
可能原因及解决方案:
- 预测时域N过小 → 增大N值
- 权重矩阵设置不合理 → 调整Q/R比值
- 车辆模型不准确 → 检查模型参数
6.2 控制量振荡
常见解决方法:
- 增加控制变化率权重Rd
- 添加转向角变化率约束
- 降低车速
6.3 实时性不足
优化建议:
- 减少预测时域N
- 简化代价函数计算
- 使用代码生成加速
7. 扩展应用与进阶方向
- 考虑动力学模型扩展:
- 加入轮胎力模型
- 考虑载荷转移影响
- 复杂场景应用:
- 障碍物避碰
- 多车协同控制
- 硬件在环测试:
- 连接实时仿真器
- 实际控制器测试
在实际项目中,我发现MPC参数初始化对控制效果影响很大。一个好的实践是先用PID控制器让车辆粗略跟踪轨迹,获取合理的初始控制量序列,再切换到MPC控制。这样可以避免初始阶段的大幅振荡。
