简介：ACADO工具箱源码包面向自动控制、优化算法与机器人领域的工程师和研究者，提供一整套动态优化与模型预测控制（MPC）的解决方案，支持路径规划、轨迹优化、参数辨识等典型任务，既能用于学术仿真验证，也能部署到实际工程中。包内共2000个文件，压缩后仅6.05MB，内容以C++源码（565个cpp）和头文件（360个hpp、326个h等）为主，兼顾362个MATLAB脚本（.m）、构建所需CMake配置、README等文档、tests测试用例，以及experimental实验模块；目录结构体现出acado核心库、cmake脚本、misc杂项、许可证等完整层次。已有396人关注学习，适合想理解SQP等求解器实现、或打算将ACADO集成到自研控制系统中的中高级开发者。依据包内示例和文档，可快速完成安装配置、系统建模、求解调用与C++代码生成，为机器人控制、能源系统、汽车工程、航空航天等场景提供可复用的优化算法基础。
1. ACADO 工具箱：最优控制问题的建模与求解，从入门到落地
两年前我在某实验室调一个小车倒立摆的 MPC，SQP 迭代刷了 50 轮还在发散，后来发现不是控制器的锅，而是我把时间网格 N 设成了 20，整个动力学被离散化搞得面目全非。这类问题最适合用 ACADO Toolkit 来解。它是一套面向非线性最优控制与模型预测控制的开发工具箱，底层用 C++ 实现，核心求解流程是直接多重打靶（direct multiple shooting）配合序列二次规划（SQP），每个二次规划子问题交给 qpOASES 求解。简单说，ACADO 把“最优控制问题”翻译成“带约束的非线性规划”，再交给数值求解器找最优解，能同时处理状态约束、控制约束、终端成本和非线性动力学，并提供 MATLAB 接口与 C++ 接口。适合三类人：正在调 MPC 参数却反复发散的人、想做轨迹优化学不懂数学形式的人、以及要评估路径规划算法能不能实时跑起来的开发者。下面我按从安装到排错的顺序把这套流程完整走一遍。
2. 环境准备：从源码编译到双语言接口，一次到位
2.1 源码获取与编译：cmake 与 make 的完整流程
我一般直接用源码编译，而不是碰预编译包，因为后面要调求解器容差、换 BLAS 后端，预编译包往往不给这些开关。安装前先确认系统里有 CMake、g++、BLAS/LAPACK，MATLAB 接口属于可选组件，不急的话可以先不开。
git clone <acado-toolkit仓库地址> acado cd acado mkdir build && cd build cmake .. -DWITH_MATLAB=ON -DWITH_PYTHON=OFF -DCMAKE_BUILD_TYPE=Release make -j4 export ACADO_INCLUDE_DIR=/path/to/acado export LD_LIBRARY_PATH=/path/to/acado/build/lib:$LD_LIBRARY_PATH逻辑说明：主题项目的旧版本主要靠 configure 脚本生成 Makefile，新版本把构建统一到 cmake 上。-DWITH_MATLAB=ON会额外编译 MATLAB 的 mex 接口，装完在 MATLAB 里运行一次startup.m就能让acado.开头的类被识别。-DWITH_PYTHON=OFF是因为我在这个阶段不需要 Python 绑定，开着反而多一层依赖。-DCMAKE_BUILD_TYPE=Release是必须的，Debug 模式下求解器速度直接掉一个数量级，做实时性验证会得出错误结论。
参数说明：ACADO_INCLUDE_DIR指向仓库根目录，不是 build 目录；LD_LIBRARY_PATH必须指向build/lib，否则编译链接过了但运行时找不到libacado_toolkit.so。编译完成后我习惯跑一遍自带例子验证环境，比如examples/mpc目录下的 demo，能跑通再进下一步。
2.2 接口选择：MATLAB 原型与 C++ 实时的边界
ACADO 这套东西最常被问的是“到底用 MATLAB 还是 C++”。我的分法是：验证算法逻辑、快速改目标函数、画图看轨迹对比，用 MATLAB 接口；做闭环仿真、要接真实硬件、要测微秒级求解延时，用 C++ 接口。下表是实际选择时我会参考的差异：
| 对比项 | MATLAB 接口 | C++ 接口 |
|---|---|---|
| 上手成本 | 低，变量名直观 | 中，需要理解类生命周期 |
| 求解速度 | 中，受 mex 调用开销影响 | 快，省掉一层数据拷贝 |
| 调试体验 | 好，能直接看变量 | 中等，得靠断点与日志 |
| 真实硬件部署 | 不适合 | 主流选择 |
我的建议是：不要一上来就写 C++，先把问题在 MATLAB 里跑通，确认模型和目标函数没写错，再平移成 C++ 接口。很多“C++ 跑出来的轨迹比 MATLAB 差”的案例，并不是接口差异，而是两边时间网格不一致。
cd('/path/to/acado') addpath(fullfile(pwd,'interfaces','matlab')) startup;这是 MATLAB 侧的关键三步。startup.m里会做两件事：把 mex 编译产物加入路径，以及给acado类注册构造函数。如果acado.OCP提示找不到类，先回去确认是否用了包含 MATLAB 开关的构建配置；很多人在这一步卡住，是因为偷懒用了系统自带的预编译包，里面根本没编 mex 文件。
3. 建模：把动力学方程写成 OCP，时间网格与目标函数是重点
3.1 数学形式：直接多重打靶如何把 OCP 变成 NLP
ACADO 接受的最优控制问题标准形式是：最小化目标函数，满足状态微分方程、初值、路径约束和边界约束。目标函数可以是 Mayer 项（只依赖终端状态）或 Lagrange 积分项（整个时域上的累计成本），也可以两者相加。直接用数学写是这样：
min J = E(x(tf)) + ∫0^T L(x(t), u(t)) dt
约束条件：x_dot(t) = f(x(t), u(t))，x(0) = x0，h(x, u) ≤ 0。
ACADO 做的事情是把连续问题离散化。它会建立时间网格 t0 < t1 < … < tN = T，在每个网格点上把状态和控制都变成优化变量，然后在相邻网格之间加一组连续性约束。这个思路就是直接多重打靶。相比单次打靶，多重打靶每个小段独立积分，对初值不敏感得多，这也是为什么 ACADO 在高度非线性模型上表现得比某些单段法工具箱稳定。
理解了这个数学形式，后面所有参数就好理解了：N 越大，离散化越细，结果越接近连续最优解，但同时优化变量变多，NLP 规模变大；T 是预测时域，MPC 里它决定了“你看多远”。我见过很多人在 N 上抠细节，但 T 设得完全没有物理意义，导致目标函数在数学上没问题、实际控制效果却一塌糊涂。
3.2 一个倒立摆示例：状态、控制与目标函数的 ACADO 写法
下面用一个旋转倒立摆做示范，状态取摆角 θ 与角速度 ω，控制输入 u 是底座加速度。代码如下，类名以你安装的版本为准，我用的是最常见写法：
#include <acado_optimal_control.hpp> #include <acado_gnuplot.hpp> int main() { USING_NAMESPACE_ACADO; // 定义状态与控制量 DifferentialState theta("theta"); DifferentialState omega("omega"); Control u("u"); // 时间区间：0 到 2 秒，分成 30 段 OCP ocp(0.0, 2.0, 30); // 微分方程：theta_dot = omega, omega_dot = u DifferentialEquation f; f << dot(theta) == omega; f << dot(omega) == u; // 目标函数：终端状态加权，加上控制能量积分 ocp.minimizeMayerTerm(theta * theta); ocp.minimizeLagrangeTerm(u * u); // 边界约束 ocp.subjectTo(AT_START, theta == 0.0); ocp.subjectTo(AT_START, omega == 0.0); ocp.subjectTo(AT_END, theta == M_PI); ocp.subjectTo(-1.0 <= u <= 1.0); // 求解并输出 OptimizationAlgorithm algorithm(ocp); algorithm.solve(); return 0; }逻辑说明：DifferentialState声明两个状态，Control声明一个控制输入，OCP构造函数三个参数分别是起始时刻、终止时刻和网格段数。minimizeMayerTerm对应终端状态成本，minimizeLagrangeTerm对应时域上的积分成本。AT_START和AT_END是 ACADO 里最常见的约束位置关键字，分别把约束套在第一个和最后一个状态节点上。
参数说明：ocp.minimizeLagrangeTerm(u * u)的权重系数需要根据控制量量纲调，我实际测试时习惯先设成 0.01 起跳，看轨迹形态再放大缩小；ocp.subjectTo(-1.0 <= u <= 1.0)这种区间约束最容易被忽视，因为自由解很可能把控制拉到 10 以上。N=30在这个例子里是够用的，但放到真实硬件上，30 段乘以求解器内部迭代次数，可能已经超了采样周期，这点在下一章会展开。
4. MPC 闭环仿真：从离线轨迹到实时控制，SimulationEnvironment 与热启动
4.1 用 SimulationEnvironment 搭闭环：代码结构与迭代逻辑
离线求一条最优轨迹只是第一步，MPC 的实质是反复求解“当前状态开头的有限时域最优控制问题”。ACADO 提供了一套闭环仿真环境，把“被控对象模型”和“MPC 控制器”放到同一个循环里跑，每次仿真步推进后重新算一次控制量。
// 创建闭环仿真环境：0 到 4 秒，每 0.1 秒一个采样周期 SimulationEnvironment environment(0.0, 4.0, 40); // 被控对象：真实模型用更细的时间网格积分 SimulateModel realPlant(f, 4); // 控制器：使用上一节定义的 OCP Controller mpcController(ocp); environment.setModel(realPlant); environment.setController(mpcController); environment.init(); environment.run();逻辑说明：SimulationEnvironment是 ACADO 里做闭环验证的核心类之一，它内部维护两条时间线：一条是控制器的预测时域，一条是被控对象的仿真积分步。SimulateModel的第一个参数是动力学方程，第二个参数是每个采样间隔内的细分数；这个细分数通常要大于 OCP 的网格段数，避免控制器看到的模型和仿真真实模型不一致。Controller把优化算法包进去，每次被调用时从头求解一次 OCP。
参数说明：控制器预测时域和仿真环境总时长解耦。比如控制器只看未来 2 秒，但仿真可以跑 4 秒，这样能观察闭环稳定性；采样周期则由SimulationEnvironment的第三个参数与总时长共同决定。我发现一个通用习惯是让控制器预测时域的网格数 N 不小于采样周期内被控对象仿真步数的 2 倍，否则控制器“看”到的对象动态和仿真里的对象动态差距过大，闭环容易抖。
4.2 实时性的三个闸门：采样时间、QP 容差与初值传递
很多人在这个阶段误判“能不能实时”。我判断时只看三个指标：单次求解耗时、采样周期、以及最坏情况下的求解耗时是不是稳定落在采样周期内。ACADO 里 QP 子问题由 qpOASES 求解，它的终止容差会直接影响耗时。
qpOASES 的收敛判据由 setTolerance 控制，典型取值为： - 快速原型：1e-6，足够精确，耗时中等 - 实时验证：1e-4，精度略降，但迭代次数明显减少 - 硬件部署：1e-3 起步，配合热启动使用逻辑说明：SQP 外层迭代里，每个迭代步都要解一个 QP；qpOASES 的活跃集策略在上次解的基础上继续迭代，而不是从头求解，所以上一次 QP 的解质量直接决定本次耗时。这正是“热启动”的价值所在。ACADO 的OptimizationAlgorithm会把上一次迭代的活跃集保留下来，前提是你没有在每次循环中重新构造 OCP 对象。
常见的坑是每次采样都重新 new 一个 OCP，这样活跃集信息全部丢失，等价于每次冷启动，求解耗时可能是热启动的 5 到 10 倍。把 OCP 对象放到循环外，只更新当前状态初值，是我做项目时最有效的提速手段。
5. 常见问题排查：初始化发散、自由度报错与终端状态的坑
5.1 SQP 迭代发散：现象、原因与解决
现象：求解器输出里迭代次数飙到上限，目标函数值不降反升，甚至出现 NaN。仿真曲线前几步正常，随后轨迹突然飞掉。
原因：最常见的有三种。第一种是时间网格太粗，动力学非线性强时，30 段和 50 段的结果能差出一个数量级；第二种是没有给终端状态合理的约束或权重，优化器找到了数学上可行、物理上荒谬的路径；第三种是积分器步长过大，ACADO 默认积分器在网格段内可能一步跨过奇点。
解决：先把 N 翻倍看轨迹是否还在变，如果形态大变说明离散化误差主导，把 N 固定到一个合理值再谈调权重；然后检查终端约束是否真的可达，比如倒立摆终端要摆到 π，如果控制约束太紧，终端约束就成了不可行约束，发散是必然的；最后把仿真细分数调大，排除积分器造成的数值误差。
5.2 变量未初始化与自由度报错：现象、原因与解决
现象：程序报状态变量或控制变量未定义、未初始化，或者提示自由度不足。还有一种是我在 MATLAB 接口遇到的：声明了三个状态，微分方程里只用了两个，求解报维度不一致。
原因：ACADO 的符号推导在编译期把变量列表和方程列表绑定，如果某个变量没有出现在任何微分方程、目标函数或约束里，解算器无法给它分配优化索引。这里的“未初始化”不一定是你没给初值，可能是漏了某个约束把它“锁住”。控制变量的情况类似，如果控制只出现在成本函数里，而没有出现在微分方程中，自由度仍然够，但几乎没有实际作用，优化器会随意游荡。
解决：检查变量清单：状态必须全部出现在微分方程里；控制必须至少出现在一个微分方程或一个路径约束里；所有被声明的变量要么进入方程、要么被约束固定，否则删掉声明。用ocp.printReport()这类诊断输出能直接看到每个变量的自由度分配结果，我之前排查自由度问题基本都靠它。
5.3 求解器悬停与参数敏感受版本影响：现象、原因与解决
现象：同一份代码，某一版本能秒解，另一个版本里同样的 N 和 T 卡住不动，或者某个项目的参数换到另一个项目就失效。
原因：这套工具箱对版本敏感程度超乎新手想象。它底层依赖 C++ 编译期符号推导，不同版本对类构造函数、约束关键字的处理有细微差异。例如老版本里时间网格边界写成AT_START，新版本可能要求用AT_START对应的索引节点写法，或者把 Mayer 项和 Lagrange 项的合并方式变了。
解决：如果编译不过，先看版本自带的 examples 目录，把示例代码的类名和构造方式和你手头的代码做一字不差的对比。不要相信网上的旧帖子，以安装目录下的examples/mpc为准。这听起来像玄学，但我在两个不同小版本间迁移代码时，确实因为一个ocp.minimizeLagrangeTerm的外部符号写法不一致废了一个下午。
5.4 排查流程：我每次遇到求解异常时的固定顺序
先看收敛迭代数与目标函数轨迹图，确认是发散还是陷入局部解；再改积分细分数，排除积分器误差；然后检查约束可行性，终端约束和控制约束之间是否存在矛盾；最后才碰权重。这个顺序权重最高，很多人在第二步就满足了，着急去调权重，结果来回试了一个星期。我后来形成习惯：只要轨迹形态不对，先固定 N、T、约束三个维度，单独调权重，调完权重再逐个放宽约束，一次只动一个变量。
6. 进阶：把求解器压到毫秒级，代码生成与热启动的实战顺序
6.1 代码生成：把 OCP 导出为独立 C 文件
离线调参调完后，下一步是把整个优化问题从 MATLAB 或 C++ 运行时环境里剥离出来，生成一组独立的 C 源代码，编译成可嵌入式调用的库。ACADO 的代码生成模块在处理完 OCP 对象后，会导出给定预测时域下的固定步数、固定约束结构的求解器代码，这意味着时间网格 N 在生成时就被锁死。
// 在 MATLAB 侧或 C++ 侧，求解前加一行导出 ocp.exportCode("mpc_export", "generated_mpc");导出后会得到generated_mpc目录，里面有独立的 C 文件、头文件和一个 makefile。集成到真实控制器时，只需要把生成目录考到工程里，对外调用入口会提供类似mpc_solve(u, x, x_ref)的接口。这个接口内部不再有动态内存分配，每个变量在编译期就确定位置，这是它能跑进毫秒级的关键。
参数说明：代码生成适合在实时环境里反复调用同一个问题的场景，比如无人机姿态控制、机械臂轨迹跟踪。条件很明确：问题维度和时间网格基本不再变化；如果预测时域或约束数量要随机变，代码生成反而不合适，这时候老老实实保留运行时求解器。
6.2 热启动实战：把上一次解喂给下一次
代码生成之外最值得做的优化是热启动。MPC 在相邻两个采样周期里，最优解通常差别不大，所以上一次的 QP 解是最接近当前最优解的初始点。做法很简单：在循环控制逻辑里保存上一次求解得到的控制轨迹和状态轨迹，当前状态更新后，把这两组数据作为新的初值传入求解器。
// 伪代码：循环外保留上一次解 std::vector<double> u_prev, x_prev; while (run) { ocp.setInitialState(currentX); if (!u_prev.empty()) { ocp.setInitialControl(u_prev); ocp.setInitialStateTrajectory(x_prev); } algorithm.solve(); u_prev = algorithm.getControlTrajectory(); x_prev = algorithm.getStateTrajectory(); // 只把当前时刻的控制量发到执行机构 actuator_command = u_prev[0]; }逻辑说明：setInitialControl和setInitialStateTrajectory是热启动能否生效的两个关键调用，缺一不可。只给控制初值、不给状态轨迹，QP 求解器还要从头找可行域；两个都给时，qpOASES 的活跃集基本在原地附近就能收敛。这里最忌每次循环重建 OCP 对象，那样所有初始化和热启动信息全部作废。
从那以后，我每次搭 MPC 项目都会强制走一遍：先用 MATLAB 接口验证模型正确性，再换 C++ 接口做闭环仿真，最后用代码生成锁死结构，并在仿真中验证最坏求解耗时是否落在采样周期内。这个顺序看起来多两步，实际上帮我避开了至少三次大型返工。热启动和代码生成是压耗时的核心，但真正决定项目成败的还是第一遍把动力学方程写对。希望帮到你。
本文还有配套的精品资源，点击获取
