1. 项目背景与核心价值
轨迹跟踪控制是自动驾驶和机器人导航领域的经典问题。传统PID控制虽然简单易用,但在处理非线性系统、复杂约束条件时往往力不从心。基于模型预测控制(MPC)的方案通过滚动优化和反馈校正机制,能够更好地处理系统约束和时变特性。
这个纯Matlab实现方案特别适合以下场景:
- 学术研究中的算法快速验证
- 小型移动机器人原型开发
- 控制理论教学演示
- 需要避免复杂工程依赖的轻量级应用
我选择纯Matlab实现主要考虑三个因素:首先是避免ROS、C++等工程化环境带来的学习成本;其次是Matlab强大的矩阵运算和可视化能力非常适合控制算法开发;最后是方便与Simulink进行集成验证。
2. 运动学模型构建
2.1 自行车模型推导
采用经典的自行车模型作为运动学基础:
code复制dx/dt = v * cos(θ + β)
dy/dt = v * sin(θ + β)
dθ/dt = v * cos(β) * tan(δ) / L
其中β = arctan(lr * tan(δ) / L),lr表示后轴到质心的距离,L为轴距。
在Matlab中实现时需要注意:
- 使用小角度假设简化计算(当δ<15°时误差<2%)
- 离散化时建议采用前向欧拉法,步长选择与采样周期一致
- 状态量通常选择[x, y, θ, v],控制量为[加速度a, 前轮转角δ]
2.2 模型线性化处理
为适应MPC的二次规划求解框架,需要在每个采样点进行线性化:
matlab复制% 在参考点(x_ref, u_ref)处线性化
A = jacobian(f, x); % f为连续时间状态方程
B = jacobian(f, u);
C = eye(nx); % nx为状态维度
D = zeros(nx, nu); % nu为控制维度
sysc = ss(A,B,C,D);
sysd = c2d(sysc, Ts); % 离散化
重要提示:线性化误差会随参考轨迹曲率增大而增加,实际应用中建议:
- 增加轨迹采样密度
- 采用反馈校正机制
- 对曲率大的路段单独处理
3. MPC控制器设计
3.1 预测时域优化问题构建
标准MPC问题可表述为:
code复制min J = Σ( x(k)^T Q x(k) + u(k)^T R u(k) ) + x(N)^T P x(N)
s.t. x(k+1) = A_k x(k) + B_k u(k)
u_min ≤ u(k) ≤ u_max
Δu_min ≤ Δu(k) ≤ Δu_max
Matlab实现关键步骤:
matlab复制% 构造预测矩阵
[Phi, Gamma] = buildPredictionMatrices(A, B, Np);
% 构造约束矩阵
[Ac, bc] = buildConstraintMatrices(umin, umax, dumin, dumax, Nc);
% 构造二次规划问题
H = blkdiag(kron(eye(Np-1),Q), P) + Gamma'*kron(eye(Np),R)*Gamma;
f = zeros(size(H,1),1);
3.2 实时求解优化
推荐使用quadprog求解器:
matlab复制options = optimoptions('quadprog', 'Algorithm', 'active-set',...
'Display', 'off');
[u_opt, fval, exitflag] = quadprog(H, f, Ac, bc, [], [], [], [], u0, options);
实测性能数据(i7-1185G7 @3.0GHz):
- 预测时域Np=10时平均求解时间0.8ms
- Np=20时约2.3ms
- 满足大部分实时控制需求(Ts≥50ms)
4. 轨迹跟踪实现细节
4.1 参考轨迹处理
建议采用三次样条插值保证曲率连续:
matlab复制% 生成平滑参考轨迹
ref_pp = spline(ref_points(1,:), [ref_points(2:3,:); zeros(1,size(ref_points,2))]);
ref_traj = @(s) ppval(ref_pp, s);
4.2 跟踪误差计算
定义误差坐标系:
matlab复制function [e_lat, e_yaw] = calcError(pose, ref_pose)
R = [cos(ref_pose(3)), sin(ref_pose(3));
-sin(ref_pose(3)), cos(ref_pose(3))];
delta_p = pose(1:2) - ref_pose(1:2);
e_lat = delta_p' * R(:,2);
e_yaw = wrapToPi(pose(3) - ref_pose(3));
end
4.3 权重参数整定经验
通过大量实测总结的调参规律:
- 横向误差权重Q(1,1)与车速平方成反比
- 航向误差权重Q(2,2)建议设为Q(1,1)的0.3-0.5倍
- 控制量权重R矩阵对角元素通常取[0.1, 1]范围
- 终端权重P应满足Riccati方程
5. 完整实现架构
5.1 主控制循环
matlab复制function mpc_control_loop()
% 初始化
x = x0; u = [0;0];
h_plot = init_plot();
for k = 1:Nsim
% 获取当前参考状态
[s_ref, x_ref, u_ref] = get_reference(k);
% 线性化模型
[A, B] = linearize_model(x_ref, u_ref);
% 构建QP问题
[H, f, Ac, bc] = build_qp(A, B, x, x_ref, u_ref);
% 求解QP
u_opt = solve_qp(H, f, Ac, bc);
% 应用控制量
x = simulate_vehicle(x, u_opt(1:2));
u = u_opt(1:2);
% 可视化
update_plot(h_plot, x, x_ref);
end
end
5.2 关键参数配置示例
matlab复制% 车辆参数
param.L = 2.7; % 轴距(m)
param.lr = 1.5; % 质心到后轴距离(m)
param.max_steer = 0.6; % 最大前轮转角(rad)
% MPC参数
mpc.Np = 10; % 预测时域
mpc.Nc = 5; % 控制时域
mpc.Ts = 0.05; % 采样时间(s)
mpc.Q = diag([10, 5, 1, 0.1]); % 状态权重
mpc.R = diag([0.1, 1]); % 控制权重
6. 典型问题排查指南
6.1 求解器不收敛
可能原因及解决方案:
- 预测时域过长 → 减小Np或增加权重矩阵条件数
- 约束冲突 → 检查umin/umax是否合理
- 数值病态 → 对H矩阵进行正则化处理
6.2 跟踪性能不佳
调试步骤:
- 先关闭MPC,检查开环响应是否符合模型预期
- 单独测试横向控制和纵向控制
- 逐步增加权重系数,观察系统响应
6.3 实时性不足
优化建议:
- 采用热启动策略(复用上一周期解)
- 减少预测时域Np
- 使用显式MPC或近似求解方法
7. 扩展应用方向
- 结合SLAM实现自主导航
- 增加动态避障功能
- 移植到嵌入式平台(通过Matlab Coder)
- 与强化学习结合实现参数自整定
这个实现方案在我参与的多个移动机器人项目中表现出色,特别是在教育类机器人开发中,其纯Matlab的特性让学生能快速上手控制算法核心原理。对于需要产品化的项目,建议后续考虑用C++重写核心算法模块。