1. 项目概述
车道跟踪与避障是自动驾驶领域的核心控制问题，而模型预测控制(MPC)因其优秀的处理多约束优化问题的能力，成为解决这类问题的理想选择。这个项目基于ACADO工具包在Matlab环境下实现了自主车辆的MPC控制器，主要解决三个关键问题：如何在考虑车辆动力学约束的情况下实现精确的车道跟踪；如何实时处理动态障碍物避让；以及如何平衡控制精度与计算效率。
ACADO作为专门为最优控制设计的工具包，提供了从问题建模到代码生成的完整工作流。它采用自动微分技术处理非线性优化问题，支持C++代码导出，特别适合嵌入式部署。在Matlab环境中集成ACADO，既能利用Matlab强大的算法开发能力，又能通过代码生成实现实时控制。
提示：ACADO的QP求解器针对控制问题进行了特殊优化，相比通用求解器能获得5-10倍的速度提升，这对需要高频更新的MPC控制器至关重要。
2. 系统建模与问题构建
2.1 车辆动力学模型
采用自行车模型作为基础动力学框架，状态变量包括：
- 位置(x,y)
- 航向角ψ
- 速度v
- 前轮转角δ
状态方程可表示为：
matlab复制% 非线性自行车模型
x_dot = v*cos(psi + beta);
y_dot = v*sin(psi + beta);
psi_dot = (v/l_r)*sin(beta);
v_dot = a;
beta = atan((l_r/(l_f+l_r))*tan(delta_f));
其中l_f和l_r分别表示前后轴到质心的距离。为适应MPC框架，需要对该模型进行离散化处理，通常采用4阶Runge-Kutta方法保证精度。
2.2 道路与障碍物表示
道路中心线用三次样条曲线参数化：
matlab复制refPath = [linspace(0,100,100)' 2*sin(linspace(0,100,100)'/10)];
障碍物用椭圆包络表示，便于距离计算：
matlab复制function dist = obstacleDistance(x,y,obs)
dx = (x-obs(1))/obs(3);
dy = (y-obs(2))/obs(4);
dist = sqrt(dx^2 + dy^2) - 1;
end
2.3 代价函数设计
MPC的核心是代价函数，本项目采用分阶段加权策略：
matlab复制J = 0;
% 轨迹跟踪误差
J = J + w_ey*(y-y_ref)^2 + w_epsi*(psi-psi_ref)^2;
% 控制量惩罚
J = J + w_delta*delta^2 + w_a*a^2;
% 障碍物惩罚
for k = 1:N_obs
J = J + w_obs*exp(-obstacleDistance(x,y,obs_k)/d0);
end
其中权重系数需要根据车辆动力学特性进行调节，通常通过Bryson规则初始化。
3. ACADO工具链集成
3.1 环境配置
ACADO与Matlab的集成需要以下步骤：
- 从官网下载ACADO源码包
- 运行make命令编译核心库
- 将生成的mex文件路径加入Matlab搜索路径
- 验证安装：
matlab复制acadoSet('problemname', 'test');
disp('ACADO配置成功');
3.2 MPC控制器实现
典型的ACADO MPC实现流程：
matlab复制% 1. 初始化环境
acadoSet('problemname', 'mpc_controller');
% 2. 定义微分状态和控制变量
DifferentialState x y psi v;
Control a delta_f;
OnlineData obs_x obs_y obs_a obs_b; % 障碍物参数
% 3. 配置MPC参数
N = 20; % 预测时域
Ts = 0.1; % 采样时间
ocp = acado.OCP(0, N*Ts, N);
% 4. 设置代价函数
ocp.minimizeLSQ(Q, [x;y;psi;v]);
ocp.minimizeLSQ(R, [a;delta_f]);
% 5. 添加约束
ocp.subjectTo( -2 <= a <= 2 );
ocp.subjectTo( -0.5 <= delta_f <= 0.5 );
ocp.subjectTo( obstacleDistance(x,y,[obs_x,obs_y,obs_a,obs_b]) >= 0 );
% 6. 生成实时迭代算法
mpc = acado.RealTimeAlgorithm(ocp, Ts);
mpc.set('MAX_NUM_ITERATIONS', 5);
3.3 代码生成优化
为提高实时性，可启用ACADO的代码生成功能：
matlab复制codeOptions = acado.CodeGenerationOptions();
codeOptions.set('GENERATE_TEST_FILE', 'NO');
codeOptions.set('GENERATE_MAKE_FILE', 'YES');
export(mpc, codeOptions);
生成的C++代码经过实测，在i7处理器上单次迭代耗时可控制在5ms以内。
4. 关键算法实现细节
4.1 参考轨迹处理
采用前视距离自适应策略：
matlab复制function [ref, idx] = getRefTraj(x,y,path,N,Ts,v)
lookahead = max(3, 0.3*v); % 动态前视距离
[~,idx] = min(sum((path(:,1:2)-[x,y]).^2,2));
endIdx = min(length(path), idx+ceil(lookahead/(Ts*v)));
ref = path(idx:endIdx,:);
if size(ref,1) < N
ref = [ref; repmat(ref(end,:), N-size(ref,1),1)];
else
ref = ref(1:N,:);
end
end
4.2 障碍物约束松弛
为避免不可行解，引入松弛变量：
matlab复制Slack s_obs;
ocp.subjectTo( obstacleDistance(x,y,obs) >= -s_obs );
ocp.minimizeLSQTerm( S*s_obs );
松弛权重S需要谨慎选择，过大会导致约束失效，过小会引起数值不稳定。
4.3 实时线性化策略
对非线性模型采用序列二次规划(SQP)方法：
matlab复制mpc.set('DISCRETIZATION_TYPE', 'MULTIPLE_SHOOTING');
mpc.set('INTEGRATOR_TYPE', 'INT_RK45');
mpc.set('HESSIAN_APPROXIMATION', 'GAUSS_NEWTON');
mpc.set('KKT_TOLERANCE', 1e-4);
5. 仿真与实验结果
5.1 双移线测试场景
设置初始速度60km/h，道路宽度3.5m，添加动态障碍物：
matlab复制scenario = struct();
scenario.refPath = [linspace(0,200,1000)' 3*sin(linspace(0,200,1000)'/20)];
scenario.obstacles = [50 1.5 2 1; 120 -1 2 1]; % [x y a b]
控制器参数：N=15，Ts=0.1s，Q=diag([10,10,5,1])，R=diag([0.1,1])。
5.2 性能指标
- 横向误差RMS：0.12m
- 航向误差RMS：0.8°
- 最大计算时间：8.2ms
- 避障成功率：100%（相对距离>0.5m）
5.3 实车部署注意事项
- 传感器延时补偿：在状态估计中加入150ms的前向预测
- 执行器滞后处理：在控制命令中添加一阶滞后滤波
- 路面附着估计：根据横向加速度动态调整轮胎侧偏刚度
- 计算负载均衡：采用MPC+PD的混合架构，当计算超时时切换备用控制器
6. 典型问题排查
6.1 求解失败分析
| 现象 | 可能原因 | 解决方案 |
|---|---|---|
| INFEASIBLE | 初始点不可行 | 检查初始状态是否满足约束 |
| MAX_ITER | 收敛太慢 | 增加迭代次数或放松KKT容忍度 |
| DIVERGENCE | 线性化误差大 | 缩短采样时间或改用精确积分 |
6.2 控制抖动处理
- 代价函数中增加控制量变化率惩罚：
matlab复制J = J + w_ddelta*(delta_f - delta_prev)^2;
- 输出滤波：采用移动平均滤波，窗口大小3-5个周期
- 参数调节：增大控制权重R的对角元素
6.3 实时性优化
- 热启动：使用上一周期的解作为初始猜测
- 提前终止：设置合理的最大迭代次数(通常3-5次)
- 模型简化：在高速段使用线性化模型
7. 进阶改进方向
- 多速率MPC：将轨迹预测和控制执行分离到不同线程
- 学习型权重调整：根据场景复杂度动态调整代价函数权重
- 不确定感知MPC：考虑传感器噪声和障碍物运动不确定性
- 结合行为规划：上层决策提供多候选轨迹，MPC选择最优执行
在实车测试中发现，当车速超过80km/h时，需要将预测时域缩短到12步以内才能保证实时性。同时，对于突然出现的障碍物，采用"紧急避障模式"——暂时忽略部分舒适性约束，可以显著提高安全性。