1. 项目概述
轨迹跟踪是自动驾驶和机器人控制领域的核心问题之一。传统PID控制器在复杂动态环境下往往表现不佳,而模型预测控制(MPC)因其能够显式处理系统约束和优化未来行为的特性,成为解决这类问题的理想选择。本项目采用CasADi这一强大的符号计算框架,实现了针对质点车辆模型的MPC控制器,并在Matlab环境中完成了从建模到控制的完整实现流程。
CasADi作为一个开源的优化工具包,特别适合用于非线性优化问题的快速原型开发。它提供了自动微分、符号计算等关键功能,能够显著简化MPC控制器的实现难度。相比直接使用Matlab内置优化工具箱,CasADi在计算效率和处理复杂非线性约束方面具有明显优势。
2. 核心需求解析
2.1 质点车辆模型特性
质点车辆模型虽然简化了车辆动力学,但保留了位置、速度和加速度等关键状态变量,足以描述基本的运动特性。在二维平面中,系统状态通常表示为:
code复制x = [px, py, vx, vy]^T
u = [ax, ay]^T
其中px,py表示位置,vx,vy表示速度,ax,ay表示加速度输入。对应的离散时间状态方程可表示为:
code复制x_{k+1} = A x_k + B u_k
这个线性模型虽然简单,但已经能够体现MPC处理多变量系统的基本原理。在实际应用中,可以根据需要扩展为更复杂的非线性模型。
2.2 轨迹跟踪问题描述
轨迹跟踪的目标是使车辆状态x尽可能接近参考轨迹x_ref,同时满足各种物理约束。这可以表述为一个优化问题:
code复制min Σ(||x_k - x_ref||_Q + ||u_k||_R)
s.t. x_{k+1} = f(x_k,u_k)
u_min ≤ u_k ≤ u_max
x_min ≤ x_k ≤ x_max
其中Q和R是权重矩阵,用于平衡跟踪精度和控制代价。MPC的核心思想是在每个控制周期求解这个有限时域优化问题,并只执行第一个控制输入。
3. 工具链选型与配置
3.1 CasADi框架优势
选择CasADi主要基于以下几个考虑:
- 符号计算能力:可以自动处理复杂的导数计算,省去手动推导雅可比矩阵的麻烦
- 高效代码生成:支持将优化问题编译为高效C代码,适合实时应用
- 接口友好:提供Matlab和Python接口,便于快速原型开发
- 求解器支持:可集成IPOPT、qpOASES等多种开源和商业求解器
3.2 环境配置步骤
- 从CasADi官网下载对应Matlab版本的预编译包
- 将解压后的文件夹添加到Matlab路径:
matlab复制addpath('casadi-3.6.3-windows64-matlab2018b')
import casadi.*
- 安装依赖求解器(如IPOPT):
matlab复制opts = struct('ipopt', struct('print_level',5));
solver = nlpsol('solver','ipopt', nlp, opts);
注意:CasADi版本需要与Matlab版本匹配,否则可能出现兼容性问题。建议使用较新的Matlab版本(R2018b及以上)。
4. MPC控制器实现细节
4.1 问题建模阶段
首先定义符号变量和模型方程:
matlab复制% 定义状态和控制输入符号变量
x = SX.sym('x',4); % [px,py,vx,vy]
u = SX.sym('u',2); % [ax,ay]
% 定义离散时间模型
dt = 0.1; % 采样时间
A = [1 0 dt 0; 0 1 0 dt; 0 0 1 0; 0 0 0 1];
B = [0 0; 0 0; dt 0; 0 dt];
x_next = A*x + B*u;
% 创建函数句柄
f = Function('f',{x,u},{x_next});
4.2 预测时域设置
预测时域N的选择需要在计算复杂度和控制性能之间权衡:
- 较短时域(N=10):计算快但可能视野不足
- 较长时域(N=20):控制更优但计算负担重
本项目采用N=15作为平衡点,对应1.5秒的预测范围(dt=0.1s)。
4.3 目标函数构建
目标函数包含状态跟踪误差和控制输入惩罚:
matlab复制% 权重矩阵
Q = diag([10,10,1,1]); % 位置误差权重高于速度
R = diag([0.1,0.1]); % 控制输入权重
% 初始化目标函数
J = 0;
% 构建预测时域内的目标函数
for k = 1:N
J = J + (X(:,k)-X_ref(:,k))'*Q*(X(:,k)-X_ref(:,k));
J = J + U(:,k)'*R*U(:,k);
end
4.4 约束条件处理
考虑车辆物理限制,添加速度和加速度约束:
matlab复制% 初始化约束向量
g = [];
% 系统动力学约束
for k = 1:N-1
g = [g; X(:,k+1) - f(X(:,k),U(:,k))];
end
% 控制输入约束
for k = 1:N
g = [g; U(1,k)]; % ax
g = [g; U(2,k)]; % ay
end
% 约束上下界
lbg = [zeros(4*(N-1),1); % 动力学约束
repmat([-2;-2],N,1)]; % 加速度限制
ubg = [zeros(4*(N-1),1);
repmat([2;2],N,1)];
5. 实时控制循环实现
5.1 控制框架结构
MPC实时控制的基本流程如下:
- 获取当前状态x0和参考轨迹X_ref
- 求解优化问题得到控制序列U*
- 应用第一个控制输入u = U*(:,1)
- 状态更新,进入下一周期
matlab复制while ~isDone(tracker)
% 获取当前参考轨迹
[X_ref, ~] = getReferenceTrajectory(tracker);
% 设置初始状态约束
args.x0 = [x0(:); zeros(nx*N+nu*N,1)];
args.lbx(1:nx) = x0;
args.ubx(1:nx) = x0;
% 求解优化问题
res = solver('x0', args.x0, 'lbx', args.lbx, 'ubx', args.ubx,...
'lbg', args.lbg, 'ubg', args.ubg);
% 提取最优控制
U_opt = full(res.x(nx+1:nx+nu));
% 应用控制并更新状态
x0 = f(x0, U_opt);
end
5.2 参考轨迹生成
参考轨迹的质量直接影响控制效果。常用生成方法包括:
- 多项式插值:适合简单路径
- 样条曲线:平滑性更好
- 运动规划算法:如RRT*、A*等
本项目采用三次样条曲线生成参考轨迹:
matlab复制% 创建样条插值
pp = spline(waypoints(1,:), waypoints(2:3,:));
% 生成参考轨迹
t_ref = linspace(0, tf, N);
X_ref = ppval(pp, t_ref);
6. 性能优化技巧
6.1 求解器参数调优
IPOPT求解器的性能高度依赖参数配置,关键参数包括:
matlab复制opts.ipopt = struct(...
'max_iter', 500, % 最大迭代次数
'tol', 1e-6, % 收敛容差
'print_level', 0, % 输出详细程度
'mu_strategy', 'adaptive',...
'linear_solver', 'mumps'); % 线性求解器
提示:对于实时应用,可以适当降低tol(如1e-4)以减少计算时间,平衡精度和速度。
6.2 热启动策略
利用上一周期的解作为当前优化的初始猜测,可显著减少迭代次数:
matlab复制if k > 1
args.x0 = [x0; U_prev(1:end-nu); X_prev(nx+1:end)];
else
args.x0 = [x0; zeros(nx*N+nu*N,1)];
end
6.3 代码生成加速
对于固定结构的MPC问题,可以使用CasADi的代码生成功能:
matlab复制% 创建代码生成选项
opts = struct('main', true,...
'mex', true);
% 生成C代码并编译为Mex函数
solver.generate('mpc_solver.c', opts);
mex mpc_solver.c -DMATLAB_MEX_FILE
7. 常见问题与调试技巧
7.1 求解失败分析
当求解器返回非最优解时,可采取以下排查步骤:
- 检查约束可行性:确保初始状态和参考轨迹不违反硬约束
- 调整权重矩阵:增大R可以缓解控制量饱和问题
- 简化问题:缩短预测时域或先尝试无约束情况
- 检查雅可比矩阵:使用
jacobian函数验证导数计算正确性
7.2 数值不稳定问题
MPC中常见的数值问题及解决方法:
- 状态量纲不统一:将位置(米)和速度(米/秒)缩放到相近范围
- 权重不平衡:调整Q和R使各项代价量级相当
- 采样时间不当:dt过大导致离散化误差,dt过小增加计算负担
7.3 实时性保障
对于需要高频率控制的应用(>50Hz),可采取以下措施:
- 减少预测时域N
- 使用更高效的线性求解器(如ma57)
- 采用显式MPC或近似方法
- 在快速硬件(如GPU)上部署
8. 扩展与改进方向
8.1 模型复杂度提升
当前质点模型可扩展为:
- 自行车模型:考虑转向动力学
- 轮胎摩擦模型:引入非线性摩擦约束
- 多体动力学:更精确的车辆建模
8.2 鲁棒MPC实现
考虑模型不确定性和外部干扰:
matlab复制% 添加扰动项
d = SX.sym('d',nx);
x_next = f(x,u) + d;
% 设置扰动边界
lbg = [..., -0.1*ones(nx*(N-1),1)];
ubg = [..., 0.1*ones(nx*(N-1),1)];
8.3 轨迹重规划集成
结合局部路径规划算法,实现动态避障:
- 在MPC层添加障碍物约束
- 设计安全走廊约束
- 采用分层架构:规划层+跟踪层
在实际测试中,我发现初始权重选择对控制效果影响很大。一个实用的调参技巧是先从较大的控制权重R开始,确保系统基本稳定,然后逐步增加状态权重Q来提高跟踪精度。对于大多数轨迹跟踪场景,位置误差权重通常设为速度误差权重的5-10倍效果较好。