acados 快速 NMPC 求解器：从 RTI 到嵌入式代码生成实战

简介acados 是一套面向非线性最优控制的开源求解器主打快速与嵌入式部署适合从事控制算法、机器人、自动驾驶及嵌入式优化的开发者与研究人员使用。它提供 C、Python、MATLAB 与 Octave 多语言接口便于在仿真与实机之间迁移算法。资源包共 1227 个文件约 21.72MB以 371 个 C 源文件、296 个 m 脚本、122 个头文件、88 个 Python 文件为主辅以 Makefile、CMake 构建脚本、sh 安装脚本及 md 文档覆盖核心求解器、代码生成模板与示例工程。内容预览可见 vde_hess_chain、casadi_erk4_chain 等链式模型算例涉及 Hessian 计算与 ERK 积分器实现便于理解算法结构与测试流程。目前已有 1979 人学习下载适合需要将非线性 MPC 落地到嵌入式平台、研究实时优化求解的读者参考。1. acados 到底解决什么问题从「模型预测控制跑不动」说起如果你做过非线性模型预测控制NMPC的落地大概率遇到过这个场景算法在仿真里跑得挺好一上真实控制器单步求解时间从 2ms 飙到 40ms控制周期直接被撑爆。问题往往不在控制律本身而在求解器——通用非线性规划求解器为了兼容各种问题形式牺牲了太多结构信息。acados 就是冲着这个痛点来的它是一个面向非线性最优控制OCP的快速、嵌入式求解器核心思路是把 OCP 的结构多阶段、稀疏、固定维度吃透用实时迭代RTI和序列二次规划SQP把每步计算压到微秒到毫秒级。它适合谁做机器人运动控制、自动驾驶轨迹跟踪、无人机姿态控制、化工过程控制的工程师尤其是那些需要把 NMPC 部署到嵌入式平台如 ARM Cortex-A、x86 工控机的团队。acados 不是 MATLAB 工具箱那种「先仿真再说」的东西它从设计之初就考虑代码生成和内存确定性能生成 C 代码并交叉编译到目标板。换句话说它解决的是「NMPC 从论文到产品」中间那段最脏最累的工程化问题。2. acados 的求解器选型SQP-RTI 与 SQP 全步长怎么选2.1 实时迭代 RTI 的适用边界acados 默认推荐的是 SQP-RTIReal-Time Iteration。它的逻辑是在每个采样周期只做一次 SQP 迭代而不是迭代到收敛。这听起来很激进但控制问题有个特点——上一时刻的解已经离新解很近一次迭代足够把残差压到可接受范围。RTI 把每个周期的计算量固定下来最坏情况执行时间WCET可预测这对硬实时系统是刚需。但 RTI 不是万能的。如果系统动态变化剧烈、参考轨迹跳变、或者初始猜测离解很远单次迭代可能发散。这时候需要看两个指标一是 SQP 迭代后的 KKT 残差二是控制输入的变化率。如果残差在 1e-3 以上且控制量抖动明显说明 RTI 撑不住得换全步长 SQP 或者增加迭代次数。2.2 全步长 SQP 与 RTI 的配置差异在 acados 的 Python 接口里切换求解器类型只需要改一个字段。下面是一个最小示例展示如何为一个倒立摆模型配置 RTI 和全步长 SQPfrom acados_template import AcadosOcp, AcadosOcpSolver import numpy as np # 创建 OCP 对象 ocp AcadosOcp() ocp.model.name pendulum # 设置求解器选项 ocp.solver_options.nlp_solver_type SQP_RTI # 或 SQP ocp.solver_options.qp_solver PARTIAL_CONDENSING_HPIPM ocp.solver_options.hessian_approx GAUSS_NEWTON ocp.solver_options.integrator_type ERK ocp.solver_options.sim_method_num_stages 4 ocp.solver_options.sim_method_num_steps 1 # RTI 专用参数每周期迭代次数固定为 1 if ocp.solver_options.nlp_solver_type SQP_RTI: ocp.solver_options.nlp_solver_max_iter 1 else: ocp.solver_options.nlp_solver_max_iter 20 ocp.solver_options.nlp_solver_tol_stat 1e-6 # 创建求解器 solver AcadosOcpSolver(ocp)这段代码的关键参数有三个nlp_solver_type决定用 RTI 还是全步长 SQPqp_solver选 HPIPM 是因为它针对 OCP 的带状稀疏结构做了优化比通用 QP 求解器快一个量级hessian_approx用 GAUSS_NEWTON 而不是精确 Hessian省掉二阶导数计算代价是收敛速度略慢但对 RTI 来说够用。逻辑说明RTI 模式下nlp_solver_max_iter必须设为 1否则就退化成普通 SQP 了。全步长 SQP 则需要设置收敛容差tol_stat通常 1e-6 到 1e-4 之间太松会导致控制精度不够太紧会拖长计算时间。sim_method_num_stages和sim_method_num_steps控制积分器精度ERK 四阶一步在多数机械系统上够用如果是刚性系统比如含化学反应的过程控制得换 IRK 并增加步数。2.3 代码生成与嵌入式部署的配置要点acados 的真正杀伤力在代码生成。它能把上面定义的 OCP 编译成一个独立的 C 库不依赖 Python 运行时。生成命令如下# 在 Python 脚本中调用生成接口 ocp.code_export_directory c_generated_code AcadosOcpSolver(ocp, generateTrue, buildTrue) # 生成的目录结构 # c_generated_code/ # acados_solver_pendulum.c/h # Makefile # CMakeLists.txt生成后的 C 代码可以直接链入嵌入式工程。交叉编译时注意两点一是关闭 BLASFEO 的运行时 CPU 检测嵌入式平台通常不需要在CMakeLists.txt里设BLASFEO_LAHIGH_PERFORMANCE二是如果目标板没有硬件浮点单元得把ACADOS_WITH_OPENMP关掉否则线程调度会引入不可预测的延迟。3. 用 acados 搭建一个 NMPC 控制器的完整流程3.1 定义模型从符号表达式到 C 代码acados 支持两种模型定义方式一是用 CasADi 符号变量写微分方程二是直接写 C 函数。前者适合快速原型后者适合最终部署。下面用 CasADi 定义一个简单的二维运动模型import casadi as ca # 状态位置 p_x, p_y速度 v_x, v_y x ca.SX.sym(x, 4) # 控制加速度 a_x, a_y u ca.SX.sym(u, 2) # 连续时间动态 p x[0:2] v x[2:4] xdot ca.vertcat(v, u) # 创建 CasADi 函数 f_expl ca.Function(f_expl, [x, u], [xdot])这段代码定义了xdot f(x,u)acados 会用积分器把它离散化。注意状态和控制的维度必须固定acados 不支持变维度 OCP。如果模型里有参数比如质量、摩擦系数用ca.SX.sym(p, n_p)单独定义通过ocp.model.p传入这样可以在线更新而不重新生成代码。3.2 代价函数与约束的配置模板代价函数决定了控制器的行为。acados 支持线性最小二乘和非线性最小二乘两种形式。对于轨迹跟踪问题通常用非线性最小二乘# 参考轨迹 x_ref ca.SX.sym(x_ref, 4) y_ref ca.SX.sym(y_ref, 4) # 阶段代价跟踪误差的加权平方 W ca.diag([10.0, 10.0, 1.0, 1.0]) cost_y x - y_ref ocp.cost.cost_type NONLINEAR_LS ocp.cost.W W ocp.cost.yref np.zeros(4) ocp.model.cost_y_expr cost_y # 终端代价 ocp.cost.cost_type_e NONLINEAR_LS ocp.cost.W_e W * 10 ocp.cost.yref_e np.zeros(4) ocp.model.cost_y_expr_e cost_y # 约束控制量限幅 ocp.constraints.lbu np.array([-5.0, -5.0]) ocp.constraints.ubu np.array([5.0, 5.0]) ocp.constraints.idxbu np.array([0, 1])参数说明W是状态跟踪权重位置权重给 10、速度权重给 1是因为位置误差直接影响任务完成度速度误差可以容忍。终端代价W_e放大 10 倍是为了保证终端稳定性这是 NMPC 的常见做法。lbu和ubu是控制量上下界idxbu指定哪些控制量受约束。如果要对状态加约束比如避障用ocp.constraints.lbx和ubx但注意状态约束可能让 QP 不可行需要加松弛变量。3.3 仿真闭环与求解时间统计搭好 OCP 后跑一个闭环仿真验证性能import time N_sim 100 x_current np.array([0.0, 0.0, 0.0, 0.0]) solver AcadosOcpSolver(ocp) solve_times [] for i in range(N_sim): # 更新参考轨迹 yref np.array([1.0, 1.0, 0.0, 0.0]) solver.set(0, yref, yref) # 求解 t0 time.perf_counter() solver.solve() solve_times.append(time.perf_counter() - t0) # 获取控制量并模拟一步 u_opt solver.get(0, u) x_current x_current 0.01 * np.array([x_current[2], x_current[3], u_opt[0], u_opt[1]]) print(f平均求解时间: {np.mean(solve_times)*1000:.3f} ms) print(f最大求解时间: {np.max(solve_times)*1000:.3f} ms)这段代码的关键是solver.set(0, yref, yref)它在线更新参考轨迹而不重新生成代码。solve_times统计的是 Python 调用开销加求解时间实际嵌入式部署时只有 C 求解时间通常比这个数字小 30% 到 50%。如果最大求解时间远大于平均值说明某些时刻 QP 迭代次数飙升需要检查约束是否太紧或者初始猜测质量太差。4. acados 避坑记录从编译失败到控制抖动的五个典型问题4.1 编译时报 BLASFEO 链接错误现象cmake配置阶段报undefined reference to blasfeo_dgemm_nn。原因通常是 BLASFEO 没有正确编译或者目标架构不匹配。acados 默认用X64_AUTOMATIC但在 ARM 上需要手动指定ARMV8A_ARM_CORTEX_A57之类的目标。解决方法是清理build目录重新运行cmake -DBLASFEO_TARGETARMV8A_ARM_CORTEX_A57 ..然后make install。如果还不行检查LD_LIBRARY_PATH是否包含 BLASFEO 的lib目录。4.2 RTI 模式下控制量高频抖动现象控制输入在稳态附近以 10Hz 以上频率抖动。原因通常是 RTI 单次迭代没有收敛到足够精度残差在 QP 边界附近振荡。解决方法是先切到全步长 SQP 确认问题是否消失如果消失说明 RTI 的精度不够。可以尝试增加sim_method_num_steps提高积分精度或者在代价函数里加控制量变化率惩罚项W_u抑制高频抖动。4.3 状态约束导致 QP 不可行现象求解器返回QP_INFEASIBLE控制量卡在边界。原因是状态约束和动态方程冲突比如要求位置在 0.1 秒内从 0 跳到 1但速度限幅不允许。解决方法是给状态约束加松弛变量在 acados 里设置ocp.constraints.idxsbx和ocp.cost.Zl、ocp.cost.Zu允许约束被轻微违反代价是牺牲一点安全性。松弛变量的权重需要调太小起不到作用太大等于没加。4.4 代码生成后嵌入式平台运行超时现象在 x86 上跑 1ms 的求解器交叉编译到 ARM 后变成 8ms。原因可能是 BLASFEO 没有针对目标 CPU 优化或者编译器优化等级不够。解决方法是检查CMakeLists.txt里的CMAKE_C_FLAGS是否包含-O3 -marchnative交叉编译时-march要换成目标架构并确认 BLASFEO 的TARGET设置正确。另外关闭ACADOS_WITH_OPENMP可以避免线程调度开销单线程反而更快。4.5 参考轨迹更新后求解器不响应现象调用solver.set(yref, ...)后控制量没变化。原因是 acados 的set接口对字段名敏感yref和yref_e要分开设置且索引必须对应阶段。如果 OCP 有 N 个阶段yref要设 N 次yref_e设 1 次。另外如果用了NONLINEAR_LS参考轨迹是通过cost_y_expr里的符号变量传入的set的字段名应该是yref而不是cost_y_expr。检查solver.get_stats(residuals)可以看到残差是否真的更新了。5. 进阶技巧用 acados 做参数自适应与多速率控制5.1 在线更新模型参数acados 支持在运行时更新模型参数不需要重新生成代码。假设模型里有个质量参数m定义时用ca.SX.sym(p, 1)然后在闭环里调用solver.set(0, p, np.array([m_new]))。这个接口的延迟在微秒级适合做自适应控制。但注意参数更新后之前的最优解可能不再是最佳初始猜测如果参数变化超过 20%建议重置求解器状态或者多跑几次 SQP 迭代。5.2 多速率 NMPC 的实现方式有些系统需要控制周期和估计周期不同比如视觉伺服里相机 30Hz、控制 100Hz。acados 本身不直接支持多速率但可以通过两个求解器实例实现一个高频短时域求解器负责稳定一个低频长时域求解器负责规划。两个求解器共享状态估计高频求解器的终端代价用低频求解器的解来构造。这种架构在 acados 的示例里有类似实现关键是保证两个求解器的采样时间比是整数倍否则终端代价对不齐。5.3 验证求解器性能的三个指标部署前建议跑三组测试一是阶跃响应测试看超调量和调节时间二是正弦跟踪测试看相位滞后和幅值衰减三是扰动抑制测试在稳态时加脉冲扰动看恢复时间。每组测试记录平均求解时间、最大求解时间和 QP 迭代次数。如果最大求解时间超过控制周期的 80%说明没有余量需要简化模型或者降低时域长度。我一般会留 30% 的余量因为嵌入式平台的负载不是恒定的中断和 DMA 传输会抢 CPU。5.4 一个容易忽略的细节初始猜测的质量acados 默认用上一时刻的解作为初始猜测这在大多数情况下够用。但如果系统有模式切换比如从位置控制切到力控制上一时刻的解可能离新解很远导致 RTI 发散。这时候可以手动设置初始猜测solver.set(0, x, x_guess)和solver.set(0, u, u_guess)。更好的做法是维护一个解的热启动缓冲区在模式切换时用插值或者上一轮同模式的解来初始化。这个技巧在机械臂抓取任务里特别有用能减少 50% 以上的求解时间波动。希望帮到你。本文还有配套的精品资源点击获取