模型预测控制(MPC)-用ACADO解决无人机控制大姿态稳定问题

前言在四旋翼无人机控制领域飞机大姿态稳定例如抛飞功能是一个极具挑战性的场景——无人机从被抛出到稳定悬停的过程中需要在极短时间内完成姿态恢复和高度控制。传统的PID控制难以应对这种强非线性、快时变的系统而**非线性模型预测控制NMPC**提供了一个优雅的解决方案。本文将从实践角度介绍ACADO工具箱生成的NMPC控制器并结合实际项目中的抛飞控制模块分享开发经验。一、模型预测控制(MPC)基础回顾1.1 什么是MPCMPC是一种基于优化的先进控制策略其核心思想1. 使用系统的数学模型预测未来一段时间内的行为2. 构建一个优化问题使未来轨迹尽可能接近期望参考3. 求解优化问题得到一组控制量4. 只执行当前时刻的第一步控制量下一步重新预测与优化。MPC 在飞控里能干啥对比 PID✅多变量一起调姿态 高度 水平位置一次性优化天然耦合。✅硬约束直接写死 “电机不超过 100%”“倾斜角不超过 35°”不会出现 PID 那种指令超现实、执行器饱和失控。✅前馈 预测能提前预判未来大机动、大风、高速轨迹比 PID 稳得多。❌计算量大每步要解一个优化问题QP 二次规划比 PID 重。这种方式特别适合处理- 多变量耦合系统- 约束输入和状态- 需要在线自适应权重或补偿扰动的场景┌────────────────────────────────────────────────────────────┐ │ MPC 预测模型 滚动优化 反馈校正 │ │ │ │ ┌──────────┐ ┌──────────┐ ┌──────────┐ │ │ │ 预测 │───▶│ 优化 │───▶│ 执行 │ │ │ │ 模型 │ │ 求解 │ │ 第一步 │ │ │ └──────────┘ └──────────┘ └──────────┘ │ │ ↑ │ │ │ └──────────── 反馈校正 ◀─────────────────────┘ │ └────────────────────────────────────────────────────────────┘1.2 NMPC 是什么Nonlinear MPC非线性模型预测控制一句话MPC 的 “完整版”直接用非线性动力学不做线性化适合强非线性、大机动场景。1.3 非线性MPC vs 线性MPC项目MPC线性NMPC非线性模型线性化模型原始非线性模型优化类型QP二次规划快NLP非线性规划慢适用工况悬停、小机动、稳定飞行大姿态、高速、翻滚、强扰动计算资源中可跑在 F7/F4高一般需要 H7、MCUFPGA/AI 加速对于无人机抛飞这类大角度姿态变化的场景非线性MPC是更合适的选择。二、ACADO工具箱详解2.1 ACADO简介ACADO Toolkit是一个用于自动生成MPC代码的C工具箱由KU Leuven开发。它的特点✅ 自动代码生成无需手写QP求解器✅ 支持非线性MPC✅ 支持多种QP求解器qpOASES, HPMPC等✅ 实时优化RTI架构✅ 开源LGPL许可2.2 典型使用流程┌─────────────────────────────────────────────────────────────┐ │ ACADO 代码生成流程 │ ├─────────────────────────────────────────────────────────────┤ │ │ │ 1. 定义模型 (MATLAB/Python) │ │ ├── 微分状态: vx, vy, vz, q0-q3, wbx, wby, wbz │ │ ├── 控制输入: u1, u2, u4 (角速度参考, 推力) │ │ └── 动态方程: 四旋翼动力学模型 │ │ │ │ 2. 定义优化目标 │ │ ├── 代价函数: J Σ||y - yref||²_W │ │ ├── 约束条件: 角速度限幅、推力限幅 │ │ └── 预测时域: N8, Ts0.15s │ │ │ │ 3. 配置求解器 │ │ ├── 离散化: Multiple Shooting RK4 │ │ ├── Hessian近似: Gauss-Newton │ │ └── QP求解器: qpOASES │ │ │ │ 4. 导出代码 │ │ └── 生成 ~10个C文件 (移植到嵌入式平台) │ │ │ └─────────────────────────────────────────────────────────────┘参考博客https://blog.csdn.net/lee_zj123/article/details/134219579?ops_request_miscelastic_search_miscrequest_idfc1dd89f23f2c12e63a43afd82ab4716biz_id0utm_mediumdistribute.pc_search_result.none-task-blog-2~all~baidu_landing_v2~default-5-134219579-null-null.142^v102^pc_search_result_base1utm_termACADO%E5%B5%8C%E5%85%A5%E5%BC%8Fspm1018.2226.3001.4187https://blog.csdn.net/lee_zj123/article/details/134219579?ops_request_miscelastic_search_miscrequest_idfc1dd89f23f2c12e63a43afd82ab4716biz_id0utm_mediumdistribute.pc_search_result.none-task-blog-2~all~baidu_landing_v2~default-5-134219579-null-null.142^v102^pc_search_result_base1utm_termACADO%E5%B5%8C%E5%85%A5%E5%BC%8Fspm1018.2226.3001.4187三、程序设计3.1. 整体架构设计┌─────────────────────────────────────────────────────────────────┐ │ NmpcInterface │ ├─────────────────────────────────────────────────────────────────┤ │ 位置环控制 NMPC求解器 输出/监控 │ │ ┌──────────┐ ┌──────────┐ ┌──────────┐ │ │ │ Position │ ────▶│ ACADO │ ────▶│ Controller│ │ │ │ Loop │ │ Solver │ │ Msg │ │ │ └──────────┘ └──────────┘ └──────────┘ │ │ │ │ │ │ ▼ ▼ │ │ ┌──────────┐ ┌──────────┐ │ │ │ RK4积分器│ │ qpOASES │ │ │ │ (扰动估计)│ │ QP求解 │ │ │ └──────────┘ └──────────┘ │ └─────────────────────────────────────────────────────────────────┘3.2.、核心数据结构3.2.1. 状态变量 (NX 10)索引变量含义0-2vx, vy, vz速度3-6q0,q1,q2,q3四元数(姿态)7-9wbx, wby, wbz角速度3.2.2 控制变量 (NU 3)索引变量含义0u1滚转角速度参考1u2俯仰角速度参考2u4推力3.2.3 输出变量 (NY 7)索引变量含义0-2vx,vy,vz速度跟踪3e_tilt倾转角误差4-5wbx,wby角速度跟踪6thrust推力跟踪3.3 项目中的ACADO配置% 四旋翼NMPC配置 DifferentialState vx vy vz q0 q1 q2 q3 wbx wby wbz; Control u1 u2 u4; % 角速度参考, 推力 OnlineData mass wbz_d Tx Ty Tz disturbance_n/e/d; % 非线性动力学模型 (省略部分项) mpc_equ_dynamic [ dot(vx) -((2*q0*q2 2*q1*q3)*(T - disturbance_f))/mass disturbance_n/mass; dot(vy) ((2*q0*q1 - 2*q2*q3)*(T - disturbance_f))/mass disturbance_e/mass; dot(vz) (9.81 - (q0² - q1² - q2² q3²)/mass*(T - disturbance_f)) disturbance_d/mass; dot(q) 0.5 * Omega(q) * omega; % 四元数运动学 dot(wbx) (u1 - wbx) / Tx; % 一阶响应模型 dot(wby) (u2 - wby) / Ty; dot(wbz) (wbz_d - wbz) / Tz; ]; % 预测时域配置 N 8; % 8步预测 Ts_mpc 0.15; % 每步0.15秒总预测时域1.2秒 % 求解器配置 mpc.set(HESSIAN_APPROXIMATION, GAUSS_NEWTON); mpc.set(QP_SOLVER, QP_QPOASES); mpc.set(INTEGRATOR_TYPE, INT_RK4);3.4、实时迭代(RTI)架构3.4.1 为什么需要RTI传统的SQP方法需要在每个控制周期求解完整的非线性优化问题计算时间不可预测。RTI(Real-Time Iteration)将优化过程分为两个阶段┌─────────────────────────────────────────────────────────────┐ │ RTI 架构 │ ├─────────────────────────────────────────────────────────────┤ │ │ │ ┌─────────────────────┐ ┌─────────────────────┐ │ │ │ Preparation Step │ │ Feedback Step │ │ │ │ (离线计算) │ │ (在线求解) │ │ │ │ │ │ │ │ │ │ • 线性化 │ │ • QP求解 │ │ │ │ • QP构建 │ │ • 获取第一个控制量 │ │ │ │ • Condensing │ │ • 执行 │ │ │ │ │ │ │ │ │ │ 【可以提前完成】 │ │ 【必须在采样时刻完成】│ │ │ └─────────────────────┘ └─────────────────────┘ │ │ ↓ ↓ │ │ ┌────────────────────────────────────────┐ │ │ │ 确定性时间 保证收敛性 │ │ │ └────────────────────────────────────────┘ │ └─────────────────────────────────────────────────────────────┘3.5算法核心3.5.1.NMPC问题形式(非线性模型预测控制)动态模型:dot(vx) -((2q0q2 2q1q3)*(T - disturbance_f))/mass disturbance_n/mass dot(vy) ((2q0q1 - 2q2q3)*(T - disturbance_f))/mass disturbance_e/mass dot(vz) (9.81 - (q0² - q1² - q2² q3²)/mass*(T - disturbance_f)) disturbance_d/mass dot(q) 0.5 * Omega(q) * omega (四元数运动学) dot(wbx) (u1 - wbx) / Tx (一阶响应模型) dot(wby) (u2 - wby) / Ty dot(wbz) (wbz_d - wbz) / Tz代价函数:plainTextJ Σ ||y - y_ref||²_W Σ ||u - u_ref||²_R其中 y [vx, vy, vz, e_tilt, wbx, wby, thrust]3.5.2.QP求解(qpoases)使用Gauss-Newton近似Hessian将非线性MPC转化为稀疏QP:min 0.5 * uHu gu s.t. lb ≤ u ≤ ubQP变量数: 24 (8步 × 3控制)使用块 condensing技术3.5.3.RTI (Real-Time Iteration) 方案┌─────────────┐ ┌─────────────┐ │ Preparation │ │ Feedback │ │ Step │ │ Step │ └─────────────┘ └─────────────┘ (离线计算) (在线求解)3.5.4.倾转-偏航解耦控制// 倾转误差计算 (tilt-yaw decoupling) e_tilt || q_e_tilt ||² q_e qd * inv(qb) // 当前姿态的逆 q_e_tilt [q0; q1; q2; 0] / sqrt(q0² q3²) // 剔除偏航分量3.5.5.自适应权重调整weightUpdate() { switch(fusion_source) { case NONE: // 仅IMU weight.pos / 5; weight.vel / 5; break; case HEIGHT: // 高度IMU weight.pos.xy / 5; weight.vel.xy / 5; break; // 完整融合: 保持原权重 } }四、抛飞控制模块设计4.1 抛飞场景分析抛飞是一种零速起飞的特殊场景面临以下挑战挑战描述初始状态不确定被抛出时速度、姿态未知大姿态角可能超过±90°甚至翻转时间窗口短需要在几百毫秒内完成控制安全要求高防止失控伤人4.2 状态机设计enum class TakeoffStage : int8_t { NONE 0, // 初始状态 ARM 1, // 解锁等待自由落体 ATT_TILT 2, // 姿态恢复阶段 HOVER 3, // 悬停阶段 FINISHED 4 // 任务完成 };4.3 状态转换逻辑┌──────────────────────────────────────────────────────────────┐ │ 抛飞状态机 │ ├──────────────────────────────────────────────────────────────┤ │ │ │ ┌─────┐ 解锁命令 ┌─────┐ 检测到自由落体 │ │ │ │ ──────────▶ │ │ ───────────────────┐ │ │ │ NONE│ │ ARM │ │ │ │ │ │ ◀────────── │ │ ◀──────────────────┘ │ │ └─────┘ 超时/翻转 └─────┘ 超时/异常 │ │ │ │ │ ▼ │ │ ┌────────────────┐ │ │ │ │ 姿态20° │ │ │ ATT_TILT │ 且角速度360°/s │ │ │ (姿态恢复) │ ──────────────────▶ │ │ │ │ │ │ └────────────────┘ │ │ │ │ │ ▼ │ │ ┌──────────┐ │ │ │ HOVER │ ── 稳定1秒 ──▶ FINISHED │ │ └──────────┘ │ │ │ └──────────────────────────────────────────────────────────────┘程序架构设计主循环流程 (ApiTickManager) ┌──────────────────────────────────────────────────────────────┐ │ NMPC 主循环 (100Hz) │ ├──────────────────────────────────────────────────────────────┤ │ │ │ 1. [数据获取] │ │ ├── fusion (位置/速度/姿态融合) │ │ ├── imu (陀螺仪角速度) │ │ └── nmpc_command (控制命令) │ │ │ │ 2. [位置环控制 - PID] │ │ pos_d(i) enable ? pos_d(i) : fusion_msg.position(i) │ │ vel_d(i) Kp*(pos_d - pos) - Kd*vel │ │ vel_d saturate(vel_d, -vmax, vmax) │ │ │ │ 3. [扰动积分估计 - RK4] │ │ ┌────────────────┐ │ │ │ 积分器状态: │ │ │ │ k1 f(err) │ │ │ │ k2 f(err...) │ │ │ │ k3 f(err...) │ │ │ │ k4 f(err...) │ │ │ │ y dt/6*(k12k22k3k4) │ │ └────────────────┘ │ │ 输出: disturbance_xyz (扰动估计) │ │ │ │ 4. [稳定性检测] ────▶ is_stable_ │ │ │ │ 5. [NMPC求解] ──────────────────────────────────────────── │ │ │ │ │ ├─ setReferance() 设置参考轨迹 │ │ ├─ getState() 更新当前状态 │ │ ├─ updateOnlineData() 更新扰动/在线数据 │ │ ├─ weightUpdate() 自适应权重调整 │ │ ├─ constrainUpdate() 更新约束 │ │ ├─ step() 调用ACADO求解器 ⭐ │ │ └─ calcCost() 计算各代价项 │ │ │ │ 6. [输出] │ │ ├── w_sp[0-1] acadoVariables.u[0-1] (角速度) │ │ ├── thrust_sp[0] acadoVariables.u[2] (推力) │ │ └── publish nmpc_controller_msg │ │ │ └──────────────────────────────────────────────────────────────┘4.4 翻转检测逻辑抛飞模块的一个关键点是翻转检测——判断无人机是否处于倒置状态// 检测条件飞机翻转(倾角90°)且静止 bool is_inversed plate_tilt M_PI / 2.f; bool gyr_static imu.lpf_gyr.norm() deg2rad(30.f); // 角速度30°/s bool hori_vel_static velocity.head2().norm() 0.2f; // 水平速度0.2m/s bool vert_vel_static fabs(velocity.z()) 0.4f; // 垂直速度0.4m/s bool is_acc_static fabs(imu.lpf_acc.norm() - 9.8f) 1.0f; // 加速度接近重力 if (is_inversed gyr_static hori_vel_static vert_vel_static is_acc_static) { // 无人机正在翻转且处于静止状态 }4.5 NMPC状态切换抛飞过程中NMPC控制器的工作状态也需要相应切换switch (takeoff_stage_) { case TakeoffStage::ARM: nmpc_cmd.state NmpcState::WAITING; // 等待自由落体 break; case TakeoffStage::ATT_TILT: nmpc_cmd.state NmpcState::RECOVER; // 恢复控制 break; case TakeoffStage::HOVER: nmpc_cmd.state NmpcState::TURNOFF; // 切换到正常悬停模式 break; }五、开发经验总结5.1 NMPC调参技巧(1) 权重调整原则WeightList default_weight { .vel {1, 1, 1}, // 速度跟踪权重 .q_tilt 200, // 倾转误差权重最重要 .u_ref {0.01, 0.01}, // 控制量平滑权重 .u_thrust 3 // 推力权重 };增大效果q_tilt姿态更稳但响应变慢u_ref控制更平滑但追踪变慢vel速度跟踪更好(2) 自适应权重策略void weightUpdate(FusionSource source, Vector3f vel, Vector3f omega) { switch (source) { case FusionSource::NONE: // 仅IMU无位置 weight.pos / 5; // 降低位置权重 weight.vel / 5; break; case FusionSource::HEIGHT: // 有高度 weight.pos.xy / 5; weight.vel.xy / 5; break; default: // 完整融合 break; } }(3) 约束设置// 限幅约束 ocp.subjectTo(-pi u1 pi); // 角速度 ±180°/s ocp.subjectTo(-pi u2 pi); ocp.subjectTo(0 u4 4); // 推力 0-4kg5.2 位置-速度双环设计项目中采用位置环 NMPC的双环结构// 位置环 (PID) pos_d(i) enable ? pos_d(i) : position(i); // 保持/跟随 vel_d(i) Kp * (pos_d - pos) - Kd * vel; // PD控制 vel_d saturate(vel_d, -vmax, vmax); // 限幅 // NMPC处理速度跟踪和姿态控制 // 预测时域: 1.2秒提前规划优势位置环处理设定点跟踪NMPC专注姿态动态和约束5.3 扰动估计与补偿// 使用积分器估计扰动 integratorRK4(reset, dt, integrator[i], vel_error); // 更新到NMPC在线数据 updateOnlineData(disturbance_n, disturbance_e, disturbance_d);5.4 抛飞模块关键点关键点实现方式翻转检测倾角90° 静止判定安全超时4秒无进展则紧急上锁姿态判定倾角20° 角速度360°/s悬停判定稳定1秒后切换5.5 调试建议先仿真后实机使用ACADO的仿真模式验证记录数据使用DataLogCollect记录关键变量逐步启用TURNOFF → WAITING → RECOVER → HOVER异常检测NaN检测、状态一致性检查六、结语NMPC在无人机控制中展现了强大的能力但也带来了计算复杂度的挑战。通过ACADO自动代码生成和RTI架构我们成功将NMPC部署到嵌入式平台实现了✅ 1.2秒预测时域100Hz控制频率✅ 姿态倾转误差解耦控制✅ 自适应权重调整✅ 抛飞等特殊场景支持未来方向更大的预测时域多无人机协同学习型MPC结合强化学习希望这篇博客对你有帮助如果需要更深入某个部分欢迎继续讨论