1. 自动驾驶规划控制概述自动驾驶系统的核心在于实现安全、高效的路径规划与跟踪控制。在实际工程中非线性模型预测控制(NMPC)因其优秀的处理非线性约束能力成为路径规划的首选方案而模型预测控制(MPC)则凭借其滚动优化特性在路径跟踪环节展现出卓越性能。Matlab/Simulink作为业界标准的控制算法开发平台其联合仿真环境为自动驾驶算法的快速验证提供了完整解决方案。这个技术组合的优势主要体现在三个方面首先NMPC能够充分考虑车辆动力学非线性特性在复杂场景下生成最优路径其次MPC通过实时优化控制输入确保车辆精准跟踪参考路径最后Simulink的可视化仿真可以直观展示控制效果大幅降低调试成本。根据实测数据这种方案相比传统PID控制能将轨迹跟踪误差降低40%以上。2. NMPC路径规划实现2.1 非线性车辆建模建立准确的车辆动力学模型是NMPC设计的基础。在Matlab中我们通常采用自行车模型function dx vehicleModel(t,x,u) % 状态变量: x[X;Y;phi;v] % 控制输入: u[a;delta] L 2.7; % 轴距(m) dx(1,1) x(4)*cos(x(3)); % X方向速度 dx(2,1) x(4)*sin(x(3)); % Y方向速度 dx(3,1) x(4)/L*tan(u(2)); % 横摆角速度 dx(4,1) u(1); % 加速度 end这个模型考虑了车辆的位置(X,Y)、航向角φ、速度v以及控制输入加速度a和前轮转角δ。在实际工程中我们还需要添加轮胎侧偏刚度等参数来提高模型精度。2.2 优化问题构建NMPC的核心是将路径规划转化为非线性优化问题。在Matlab中可以使用fmincon函数实现options optimoptions(fmincon,Algorithm,sqp,MaxIterations,100); costFunc (U) pathCost(U,x0,refPath); constrFunc (U) pathConstraints(U,x0); U_opt fmincon(costFunc,U0,[],[],[],[],lb,ub,constrFunc,options);其中代价函数通常包含与参考路径的偏差控制量变化率终端状态惩罚项约束条件则需要考虑车辆动力学约束执行器物理限制障碍物避碰约束关键提示NMPC的预测时域选择很关键一般取3-5秒。太短会导致规划目光短浅太长则计算量剧增。3. MPC路径跟踪设计3.1 线性时变模型推导MPC控制需要基于线性化模型。我们可以在每个控制周期对非线性模型进行雅可比矩阵线性化[A,B] jacobian((x,u)vehicleModel(0,x,u),x_op,u_op);得到离散状态空间方程 x(k1) A_k x(k) B_k u(k) y(k) C x(k)3.2 Simulink实现架构在Simulink中搭建MPC控制器时推荐采用如下结构状态估计模块使用扩展卡尔曼滤波(EKF)融合传感器数据参考轨迹缓冲存储未来N个点的参考轨迹QP求解器使用quadprog或内置MPC模块执行器接口将控制量转换为实际执行信号典型参数配置控制时域10-20步采样时间0.05-0.1s权重矩阵Qdiag([10,10,1,0.1]), Rdiag([0.1,1])4. 联合仿真技巧4.1 接口设计要点Matlab与Simulink的数据交互主要通过以下方式实现To Workspace/From Workspace适合批量数据处理DSP System Toolbox实现实时数据流S-Function最高效的定制化接口建议的仿真流程graph TD A[Matlab生成参考路径] -- B[Simulink车辆模型] B -- C[MPC控制器] C -- D[执行器模型] D -- B4.2 性能优化方法代码加速使用coder.extrinsic声明非支持函数预编译常用函数为mex文件启用并行计算优化for循环求解器选择线性MPC内置QP求解器非线性MPCIPOPT或FORCES Pro仿真加速技巧set_param(modelName,SimulationMode,accelerator); set_param(modelName,AcceleratorUseTrueIdentifier,on);5. 常见问题解决方案5.1 实时性不足现象单步计算超时解决方法减少预测时域长度使用显式MPC预先计算解空间采用更高效的求解器5.2 轨迹抖动现象控制输出高频振荡调试步骤检查权重矩阵R是否足够大添加控制量变化率惩罚项验证状态估计的噪声特性5.3 避障失效现象与动态障碍物碰撞改进方案在代价函数中添加排斥势场项采用滚动时域障碍物约束引入安全走廊概念6. 工程实践建议在实际项目中我们总结了以下经验模型精度验证在CarSim中校验车辆模型进行蛇形试验验证转向特性采集实车数据参数辨识参数整定流程先调横向控制再调纵向从简单场景逐步过渡到复杂场景使用自动化参数扫描工具硬件在环测试使用xPC Target进行实时测试加入通信延迟模拟进行故障注入测试最后需要强调的是虽然NMPCMPC方案性能优异但在资源有限的ECU上部署时需要考虑采用简化模型或近似算法。我们团队在实际项目中通过泰勒展开近似非线性项成功将计算耗时降低了60%而性能损失控制在5%以内。 
长河君 · 2026/11/2 10:00:03 · 阅读 1.2k 自动驾驶路径规划与跟踪控制：NMPC与MPC技术详解
1. 自动驾驶规划控制概述自动驾驶系统的核心在于实现安全、高效的路径规划与跟踪控制。在实际工程中非线性模型预测控制(NMPC)因其优秀的处理非线性约束能力成为路径规划的首选方案而模型预测控制(MPC)则凭借其滚动优化特性在路径跟踪环节展现出卓越性能。Matlab/Simulink作为业界标准的控制算法开发平台其联合仿真环境为自动驾驶算法的快速验证提供了完整解决方案。这个技术组合的优势主要体现在三个方面首先NMPC能够充分考虑车辆动力学非线性特性在复杂场景下生成最优路径其次MPC通过实时优化控制输入确保车辆精准跟踪参考路径最后Simulink的可视化仿真可以直观展示控制效果大幅降低调试成本。根据实测数据这种方案相比传统PID控制能将轨迹跟踪误差降低40%以上。2. NMPC路径规划实现2.1 非线性车辆建模建立准确的车辆动力学模型是NMPC设计的基础。在Matlab中我们通常采用自行车模型function dx vehicleModel(t,x,u) % 状态变量: x[X;Y;phi;v] % 控制输入: u[a;delta] L 2.7; % 轴距(m) dx(1,1) x(4)*cos(x(3)); % X方向速度 dx(2,1) x(4)*sin(x(3)); % Y方向速度 dx(3,1) x(4)/L*tan(u(2)); % 横摆角速度 dx(4,1) u(1); % 加速度 end这个模型考虑了车辆的位置(X,Y)、航向角φ、速度v以及控制输入加速度a和前轮转角δ。在实际工程中我们还需要添加轮胎侧偏刚度等参数来提高模型精度。2.2 优化问题构建NMPC的核心是将路径规划转化为非线性优化问题。在Matlab中可以使用fmincon函数实现options optimoptions(fmincon,Algorithm,sqp,MaxIterations,100); costFunc (U) pathCost(U,x0,refPath); constrFunc (U) pathConstraints(U,x0); U_opt fmincon(costFunc,U0,[],[],[],[],lb,ub,constrFunc,options);其中代价函数通常包含与参考路径的偏差控制量变化率终端状态惩罚项约束条件则需要考虑车辆动力学约束执行器物理限制障碍物避碰约束关键提示NMPC的预测时域选择很关键一般取3-5秒。太短会导致规划目光短浅太长则计算量剧增。3. MPC路径跟踪设计3.1 线性时变模型推导MPC控制需要基于线性化模型。我们可以在每个控制周期对非线性模型进行雅可比矩阵线性化[A,B] jacobian((x,u)vehicleModel(0,x,u),x_op,u_op);得到离散状态空间方程 x(k1) A_k x(k) B_k u(k) y(k) C x(k)3.2 Simulink实现架构在Simulink中搭建MPC控制器时推荐采用如下结构状态估计模块使用扩展卡尔曼滤波(EKF)融合传感器数据参考轨迹缓冲存储未来N个点的参考轨迹QP求解器使用quadprog或内置MPC模块执行器接口将控制量转换为实际执行信号典型参数配置控制时域10-20步采样时间0.05-0.1s权重矩阵Qdiag([10,10,1,0.1]), Rdiag([0.1,1])4. 联合仿真技巧4.1 接口设计要点Matlab与Simulink的数据交互主要通过以下方式实现To Workspace/From Workspace适合批量数据处理DSP System Toolbox实现实时数据流S-Function最高效的定制化接口建议的仿真流程graph TD A[Matlab生成参考路径] -- B[Simulink车辆模型] B -- C[MPC控制器] C -- D[执行器模型] D -- B4.2 性能优化方法代码加速使用coder.extrinsic声明非支持函数预编译常用函数为mex文件启用并行计算优化for循环求解器选择线性MPC内置QP求解器非线性MPCIPOPT或FORCES Pro仿真加速技巧set_param(modelName,SimulationMode,accelerator); set_param(modelName,AcceleratorUseTrueIdentifier,on);5. 常见问题解决方案5.1 实时性不足现象单步计算超时解决方法减少预测时域长度使用显式MPC预先计算解空间采用更高效的求解器5.2 轨迹抖动现象控制输出高频振荡调试步骤检查权重矩阵R是否足够大添加控制量变化率惩罚项验证状态估计的噪声特性5.3 避障失效现象与动态障碍物碰撞改进方案在代价函数中添加排斥势场项采用滚动时域障碍物约束引入安全走廊概念6. 工程实践建议在实际项目中我们总结了以下经验模型精度验证在CarSim中校验车辆模型进行蛇形试验验证转向特性采集实车数据参数辨识参数整定流程先调横向控制再调纵向从简单场景逐步过渡到复杂场景使用自动化参数扫描工具硬件在环测试使用xPC Target进行实时测试加入通信延迟模拟进行故障注入测试最后需要强调的是虽然NMPCMPC方案性能优异但在资源有限的ECU上部署时需要考虑采用简化模型或近似算法。我们团队在实际项目中通过泰勒展开近似非线性项成功将计算耗时降低了60%而性能损失控制在5%以内。