1. 自动驾驶规划控制方案概述
最近在自动驾驶领域,基于模型预测控制(MPC)的规划控制方案越来越受到关注。我尝试了一种结合非线性MPC(NMPC)路径规划和线性MPC路径跟踪的双层控制架构,通过Matlab和Simulink的联合仿真验证了其有效性。这种方案特别适合处理复杂道路场景下的自动驾驶控制问题。
在传统方案中,路径规划和路径跟踪往往是分开设计的,容易导致跟踪误差累积。而我们的方案通过将两者有机结合,实现了更平滑、更鲁棒的控制效果。上层NMPC负责根据全局路径和障碍物信息生成最优轨迹,下层线性MPC则专注于精确跟踪这条轨迹。
2. 系统架构设计
2.1 整体框架
系统采用分层设计,结构清晰明了:
- 规划层:运行非线性MPC算法,处理全局路径和动态障碍物信息
- 控制层:执行线性MPC控制,实现精确轨迹跟踪
- 车辆模型:作为桥梁连接两层控制器,同时作为仿真验证的基础
这种架构的最大优势在于模块化程度高,调试和参数调整非常方便。在实际开发中,我们可以独立优化每一层的性能,而不用担心影响其他模块。
2.2 Simulink实现
在Simulink中搭建的模型如下图所示(图1):
关键组件包括:
- NMPC Planner模块:实现非线性优化求解
- Linear MPC Controller模块:负责轨迹跟踪
- Vehicle Plant模块:包含车辆动力学模型
- Environment模块:模拟道路环境和障碍物
3. 非线性MPC路径规划实现
3.1 成本函数设计
非线性MPC的核心在于成本函数的设计。我们的实现考虑了以下几个关键因素:
matlab复制function [optTraj, exitflag] = nmpc_planner(current_state, obstacles)
% 状态权重矩阵
Q = diag([10, 10, 5, 2]); % x,y,heading,speed
R = diag([0.5, 0.2]); % 加速度和方向盘角权重
% 非线性约束生成
nonlcon = @(u) dynamic_constraints(u, current_state, obstacles);
% 求解器配置
options = optimoptions('fmincon','MaxIterations',50,...
'Algorithm','sqp','Display','notify');
% 初始猜测很重要!用上一时刻最优解
initial_guess = load('last_optimal_u.dat');
[opt_u, ~, exitflag] = fmincon(@(u) cost_function(u, Q, R),...
initial_guess, [], [], [], [], lb, ub, nonlcon, options);
% 轨迹预测
optTraj = predict_trajectory(current_state, opt_u);
end
这里有几个关键设计点:
- 状态权重矩阵Q中y方向权重较大,防止车辆横向摆动
- 控制量权重矩阵R中方向盘角权重较小,确保紧急避障能力
- 使用上次优化结果作为初始猜测,显著提高求解速度
3.2 动态避障实现
动态避障是路径规划的核心功能,我们通过非线性约束实现:
matlab复制function [c, ceq] = dynamic_constraints(u, state, obstacles)
ceq = []; % 等式约束暂时不用
c = zeros(1, length(obstacles));
% 预测未来5秒轨迹
pred_traj = predict_trajectory(state, u);
% 计算每个障碍物的安全距离
for i = 1:length(obstacles)
% 动态安全距离:相对速度越大,安全距离越大
rel_speed = norm(state(4) - obstacles(i).speed);
min_dist = 2 + rel_speed*0.3;
% 找最近距离点
[dist, ~] = min(vecnorm(pred_traj(:,1:2) - obstacles(i).pos, 2, 2));
c(i) = min_dist - dist; % 小于0就违反约束
end
end
这种实现方式具有以下特点:
- 安全距离随相对速度动态调整
- 考虑整个预测时域内的最小距离
- 将避障要求转化为标准非线性约束形式
4. 线性MPC路径跟踪实现
4.1 模型线性化
由于车辆动力学本质上是非线性的,我们需要在工作点附近进行线性化:
matlab复制% 在平衡点处线性化车辆模型
[Ad, Bd, Cd, Dd] = linearize_vehicle_model(...
current_speed, current_steering);
% 生成线性MPC控制器
mpcobj = setConstraints(mpcobj, 'MV', [1 1], 'RateMin', [-0.3, -0.1],...
'RateMax', [0.3, 0.1]); % 控制量变化率约束
% 设置预测时域
mpcobj.PredictionHorizon = 20;
mpcobj.ControlHorizon = 5;
线性化时需要注意:
- 考虑轮胎侧偏刚度的影响
- 根据当前车速和方向盘角度选择合适的工作点
- 定期更新线性化模型以适应不同工况
4.2 控制参数配置
MPC控制器的参数配置直接影响跟踪性能:
- 预测时域:通常设置为20步,对应2秒的时间窗口
- 控制时域:设置为5步,平衡计算复杂度和控制效果
- 控制量变化率限制:防止方向盘和加速度突变
5. 联合仿真实现
5.1 多速率执行配置
规划层和控制层需要以不同频率执行:
code复制Solver -> Type: Fixed-step
Fixed-step size: 0.05 % 对应20Hz
Tasking mode: MultiTasking
典型配置:
- 规划层:2Hz(计算量较大)
- 控制层:10Hz(需要快速响应)
5.2 仿真结果分析
仿真结果如下图所示(图2):
关键观察:
- 蓝色为NMPC规划路径,红色为实际跟踪轨迹
- 急弯处存在约0.2m的跟踪滞后
- 整体跟踪误差在可接受范围内
6. 实践经验与问题排查
6.1 常见问题及解决方案
-
非线性求解器发散
- 原因:初始猜测不合理
- 解决:使用上次优化结果作为初值,添加reset逻辑
-
车辆模型飘移
- 原因:线性化时忽略轮胎侧偏刚度
- 解决:在模型线性化时考虑轮胎特性
-
约束失效
- 原因:Simulink仿真加速导致
- 解决:勾选'Enforce constraint satisfaction'选项
6.2 性能优化技巧
-
代码优化
- 使用向量化运算替代循环
- 预分配数组内存
- 减少不必要的函数调用
-
求解器配置
- 选择合适的算法(如SQP)
- 调整最大迭代次数
- 合理设置收敛容差
-
实时性保障
- 限制最大求解时间
- 实现热启动机制
- 考虑降级策略
7. 实际应用考量
7.1 计算资源需求
在我们的工控机测试平台上:
- 完整方案运行频率可达15Hz
- 单次规划耗时约50ms
- 单次跟踪控制耗时约5ms
7.2 扩展方向
- 整合深度学习预测模块,提升对"鬼探头"等突发场景的处理能力
- 增加多车协同规划功能
- 优化求解器实现,进一步提高实时性
这套方案经过充分验证,能够满足大多数自动驾驶场景的需求。特别是在复杂城市道路环境下,其动态避障能力和轨迹跟踪精度表现突出。