• 写在前面
• 1. OCS2 是什么
• 2. MPC 在 OCS2 里解决什么问题
• 3. 先看整体结构图
• 4. OCS2 的 MPC 外壳：MPC_BASE
  • 4.1 run() 逐行看
  • 4.2 为什么 calculateController() 是虚函数
• 5. MPC 的关键参数：MPC_Settings
• 6. DDP 版本 MPC：GaussNewtonDDP_MPC
  • 6.1 构造函数在选择求解器
  • 6.2 calculateController() 真正触发求解
• 7. 四足机器人例子：LeggedRobotDdpMpcNode
  • 7.1 main() 入口逐段看
• 8. SQP 版本 MPC：LeggedRobotSqpMpcNode
• 9. LeggedRobotInterface 到底做了什么
• 10. MPC 和 MRT 的关系
• 11. ROS 接口代码：MPC_ROS_Interface
  • 11.1 接收 observation 后怎么触发 MPC
  • 11.2 优化结果怎么发布出去
• 12. SLQ / iLQR 大概在做什么
• 13. SolverBase：求解器统一接口
• 14. 学习时应该优先读哪些文件
• 15. 一个推荐的理解顺序
• 总结

写在前面

这篇是 OCS2 里 MPC 的学习笔记，主要参考 OCS2 官方文档和公开代码结构来梳理。

目标不是把所有源码细节一次性讲完，而是先回答几个核心问题：

1. OCS2 里的 MPC 在代码里从哪里开始？
2. MPC_BASE 做了什么？
3. GaussNewtonDDP_MPC 和 SLQ/iLQR 是什么关系？
4. 四足机器人例子里 MPC node 是怎么启动的？
5. MPC 求出来的轨迹又是怎么给控制循环使用的？

1. OCS2 是什么

OCS2 的全称可以理解为：

Optimal Control for Switched Systems

它是 ETH/Legged Robotics 开源的 C++ 最优控制工具箱，主要面向机器人这类复杂系统。

官方文档里提到，OCS2 支持多种算法：

• SLQ：continuous-time domain constrained DDP。
• iLQR：discrete-time domain constrained DDP。
• SQP：基于 multiple shooting 和 HPIPM。
• IPM：非线性内点法。
• SLP：Sequential Linear Programming。

同时它还提供：

• 动力学建模接口。
• 代价函数接口。
• 约束接口。
• 自动微分。
• ROS 接口。
• Pinocchio / URDF 相关机器人建模工具。

所以 OCS2 不是“一个 MPC 算法”，而是一套用于构建和求解最优控制问题的框架。

2. MPC 在 OCS2 里解决什么问题

MPC，也就是 Model Predictive Control，核心思想是：

每隔一小段时间，根据当前状态，重新求解一个有限时域最优控制问题，只执行最前面一小段控制，然后继续滚动。

假设当前时间是 $t$，预测时域长度是 $T_h$，那么每次 MPC 求解的问题大概是：

$$\min_{u(\tau)} \Phi(x(t+T_h))+ \int_t^{t+T_h} L(x(\tau),u(\tau),\tau)d\tau$$

约束包括系统动力学：

$$\dot{x}=f(x,u,t)$$

以及各种路径约束：

$$g(x,u,t)=0$$

$$h(x,u,t)\ge 0$$

对于四足机器人来说，约束可能包括：

• 支撑脚接触约束。
• 摆动脚轨迹约束。
• 摩擦锥约束。
• 接触力约束。
• 零速度约束。
• 关节或输入限制。

MPC 的滚动过程可以写成：

读取当前状态 x(t)
  ↓
求解 [t, t+T_h] 上的最优控制问题
  ↓
得到未来状态轨迹 x*(t) 和输入轨迹 u*(t)
  ↓
执行最前面一小段控制
  ↓
下一时刻重新读取状态，再求解

3. 先看整体结构图

最重要的主线是：

配置文件 / URDF / reference
        ↓
LeggedRobotInterface
        ↓
OptimalControlProblem
        ↓
MPC_BASE / GaussNewtonDDP_MPC / SqpMpc
        ↓
MPC_ROS_Interface
        ↓
MRT / 控制循环

4. OCS2 的 MPC 外壳：MPC_BASE

核心代码位置：

ocs2_mpc/include/ocs2_mpc/MPC_BASE.h
ocs2_mpc/src/MPC_BASE.cpp

MPC_BASE 是 OCS2 MPC 的基类。它的作用不是求解具体最优控制问题，而是提供 MPC 的统一外壳。

核心接口大概是：

class MPC_BASE {
 public:
  virtual bool run(scalar_t currentTime, const vector_t& currentState);
  virtual SolverBase* getSolverPtr() = 0;
  virtual const SolverBase* getSolverPtr() const = 0;

 protected:
  virtual void calculateController(
      scalar_t initTime,
      const vector_t& initState,
      scalar_t finalTime) = 0;
};

最关键的是 run()。

源码逻辑可以简化为：

bool MPC_BASE::run(scalar_t currentTime, const vector_t& currentState) {
  const scalar_t finalTime = currentTime + mpcSettings_.timeHorizon_;

  calculateController(currentTime, currentState, finalTime);

  initRun_ = false;
  return true;
}

也就是说：

MPC_BASE::run()
  ↓
计算 finalTime = currentTime + timeHorizon
  ↓
调用 calculateController()
  ↓
具体子类负责真正求解

所以 MPC_BASE 很薄，但它定义了 OCS2 里 MPC 的共同接口。

4.1 run() 逐行看

MPC_BASE::run() 是理解 OCS2 MPC 的第一段关键代码。可以把源码简化成下面这样：

bool MPC_BASE::run(scalar_t currentTime, const vector_t& currentState) {
  if (!initRun_ && currentTime >= getSolverPtr()->getFinalTime()) {
    return false;
  }

  const scalar_t finalTime = currentTime + mpcSettings_.timeHorizon_;

  calculateController(currentTime, currentState, finalTime);

  initRun_ = false;
  return true;
}

逐行理解：

if (!initRun_ && currentTime >= getSolverPtr()->getFinalTime())

这句是在检查：当前时间是不是已经超过上一次求解器给出的最终时间。

如果 MPC 上一次只优化到 5.0 秒，但当前机器人观测已经到了 5.1 秒，那么控制器就没有可用的未来轨迹了。这种情况下继续使用旧策略是不安全的，所以直接返回 false。

const scalar_t finalTime = currentTime + mpcSettings_.timeHorizon_;

这句定义本轮 MPC 的优化终点：

$$T_{\mathrm{final}} = t_{\mathrm{current}} + T_h$$

例如：

currentTime = 12.3 s
timeHorizon = 1.0 s
finalTime   = 13.3 s

本轮优化就是在 [12.3, 13.3] 上重新规划。

calculateController(currentTime, currentState, finalTime);

这句是最核心的调用。MPC_BASE 不知道你到底用 SLQ、iLQR、SQP 还是 IPM，所以它只调用一个抽象函数。真正怎么求解，由子类实现。

initRun_ = false;

第一次 MPC 运行之后，initRun_ 被置为 false。后续 MPC 会开始检查当前时间是否超过 solver 的 final time。

所以 run() 的本质就是：

检查当前策略是否还覆盖当前时间
  ↓
根据 timeHorizon 计算本轮终点
  ↓
调用具体求解器重新算 controller
  ↓
返回是否成功

4.2 为什么 calculateController() 是虚函数

MPC_BASE 里有这一句：

virtual void calculateController(
    scalar_t initTime,
    const vector_t& initState,
    scalar_t finalTime) = 0;

这是一个纯虚函数，说明 MPC_BASE 只定义接口，不定义具体算法。

原因是 OCS2 想支持多种 MPC 后端：

MPC_BASE
  ├── GaussNewtonDDP_MPC  → SLQ / iLQR
  ├── SqpMpc              → SQP
  ├── IpmMpc              → Interior Point Method
  └── 其他求解器

这样设计的好处是：

1. ROS 接口不需要关心底层求解器。
2. 四足机器人建模可以复用。
3. 同一个 OptimalControlProblem 可以换不同 solver。
4. MPC 的滚动调用逻辑统一。

换句话说：

MPC_BASE 管“什么时候求解”，具体子类管“怎么求解”。

5. MPC 的关键参数：MPC_Settings

代码位置：

ocs2_mpc/include/ocs2_mpc/MPC_Settings.h

核心结构体：

struct Settings {
  scalar_t timeHorizon_ = 1.0;
  scalar_t solutionTimeWindow_ = -1;
  bool debugPrint_ = false;
  bool coldStart_ = false;
  scalar_t mpcDesiredFrequency_ = -1;
  scalar_t mrtDesiredFrequency_ = 100.0;
};

几个参数很重要：

其中最关键的是：

timeHorizon_

它决定本轮优化区间：

$$[t_{\mathrm{current}},\ t_{\mathrm{current}}+T_h]$$

比如当前时间是 3.2 s，timeHorizon_ = 1.0，那么本轮 MPC 求解区间就是：

$$[3.2,\ 4.2]$$

6. DDP 版本 MPC：GaussNewtonDDP_MPC

代码位置：

ocs2_ddp/include/ocs2_ddp/GaussNewtonDDP_MPC.h

这个类继承 MPC_BASE：

class GaussNewtonDDP_MPC final : public MPC_BASE

构造函数里，根据配置选择 SLQ 或 ILQR：

switch (ddpSettings.algorithm_) {
  case ddp::Algorithm::SLQ:
    ddpPtr_.reset(new SLQ(...));
    break;
  case ddp::Algorithm::ILQR:
    ddpPtr_.reset(new ILQR(...));
    break;
}

核心求解逻辑：

void calculateController(
    scalar_t initTime,
    const vector_t& initState,
    scalar_t finalTime) override {

  if (settings().coldStart_) {
    ddpPtr_->reset();
  }

  ddpPtr_->run(initTime, initState, finalTime);
}

所以它的结构是：

GaussNewtonDDP_MPC
  ↓ 持有
std::unique_ptr<GaussNewtonDDP> ddpPtr_
  ↓ 具体类型可以是
SLQ 或 ILQR

运行过程是：

MPC_BASE::run()
  ↓
GaussNewtonDDP_MPC::calculateController()
  ↓
SLQ / ILQR 的 run()
  ↓
得到最优控制策略

6.1 构造函数在选择求解器

GaussNewtonDDP_MPC 的构造函数里最关键的是这段：

switch (ddpSettings.algorithm_) {
  case ddp::Algorithm::SLQ:
    ddpPtr_.reset(new SLQ(
        std::move(ddpSettings),
        rollout,
        optimalControlProblem,
        initializer));
    break;

  case ddp::Algorithm::ILQR:
    ddpPtr_.reset(new ILQR(
        std::move(ddpSettings),
        rollout,
        optimalControlProblem,
        initializer));
    break;
}

这里有几个对象特别重要：

其中 rollout 很关键。DDP 类算法需要不断做：

给一条控制轨迹
  ↓
用动力学往前积分
  ↓
得到一条状态轨迹
  ↓
沿这条轨迹线性化/二次近似
  ↓
更新控制律

所以如果没有 rollout，求解器就没法知道当前控制策略会把系统带到哪里。

6.2 calculateController() 真正触发求解

GaussNewtonDDP_MPC 里真正触发求解的是：

void calculateController(
    scalar_t initTime,
    const vector_t& initState,
    scalar_t finalTime) override {

  if (settings().coldStart_) {
    ddpPtr_->reset();
  }

  ddpPtr_->run(initTime, initState, finalTime);
}

这里最值得注意的是 coldStart_。

如果：

coldStart_ = true;

每次 MPC 都会先：

ddpPtr_->reset();

这表示不要用上一轮结果热启动，而是重新开始。

如果：

coldStart_ = false;

通常可以复用上一轮的控制策略和轨迹作为初值，这就是 MPC 里常说的 warm start。

为什么 warm start 有用？因为相邻两次 MPC 的优化问题通常很像：

第 k 次：求 [t, t+T_h]
第 k+1 次：求 [t+Δt, t+Δt+T_h]

两次时间窗口只平移了一点点，上一轮解通常是下一轮很好的初值，可以减少迭代次数，提高实时性。

一句话：

GaussNewtonDDP_MPC 本身不直接写 SLQ 细节，它只是把 MPC 的时间窗口交给 ddpPtr_->run()。

7. 四足机器人例子：LeggedRobotDdpMpcNode

代码位置：

ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotDdpMpcNode.cpp

这个文件是四足机器人 DDP-MPC 的 ROS 入口。

关键逻辑可以简化为：

LeggedRobotInterface interface(taskFile, urdfFile, referenceFile);

GaussNewtonDDP_MPC mpc(
    interface.mpcSettings(),
    interface.ddpSettings(),
    interface.getRollout(),
    interface.getOptimalControlProblem(),
    interface.getInitializer());

mpc.getSolverPtr()->setReferenceManager(rosReferenceManagerPtr);
mpc.getSolverPtr()->addSynchronizedModule(gaitReceiverPtr);

MPC_ROS_Interface mpcNode(mpc, robotName);
mpcNode.launchNodes(nodeHandle);

这段代码说明了几件事：

1. LeggedRobotInterface 负责把机器人问题组装好。
2. GaussNewtonDDP_MPC 只是拿到这些组件后开始求解。
3. RosReferenceManager 负责接收参考轨迹。
4. GaitReceiver 负责接收/同步步态信息。
5. MPC_ROS_Interface 负责把 MPC 包装成 ROS node。

因此四足机器人的 MPC 不是直接散落在一个巨大 main 函数里，而是拆成了几个层次：

ROS node
  ↓
LeggedRobotInterface
  ↓
MPC solver
  ↓
ROS interface

7.1 main() 入口逐段看

四足机器人 DDP-MPC 的入口代码可以拆成几段看。

第一段：初始化 ROS 和读取参数。

::ros::init(argc, argv, robotName + "_mpc");
::ros::NodeHandle nodeHandle;

bool multiplot = false;
std::string taskFile, urdfFile, referenceFile;
nodeHandle.getParam("/multiplot", multiplot);
nodeHandle.getParam("/taskFile", taskFile);
nodeHandle.getParam("/referenceFile", referenceFile);
nodeHandle.getParam("/urdfFile", urdfFile);

这几个参数非常重要：

第二段：创建机器人接口。

LeggedRobotInterface interface(taskFile, urdfFile, referenceFile);

这行看起来简单，但背后做了很多事：

读取配置
读取 URDF
构造机器人动力学模型
构造代价函数
构造约束
构造 reference manager
构造 rollout
构造 initializer

可以把 interface 理解成“四足机器人最优控制问题生成器”。

第三段：创建 gait receiver。

auto gaitReceiverPtr = std::make_shared<GaitReceiver>(
    nodeHandle,
    interface.getSwitchedModelReferenceManagerPtr()->getGaitSchedule(),
    robotName);

腿式机器人和普通机械臂不一样，它有接触模式切换：

哪只脚支撑？
哪只脚摆动？
什么时候换脚？

GaitReceiver 就是用来接收步态命令，并把 gait schedule 同步给求解器。

第四段：创建 ROS ReferenceManager。

auto rosReferenceManagerPtr = std::make_shared<RosReferenceManager>(
    robotName,
    interface.getReferenceManagerPtr());
rosReferenceManagerPtr->subscribe(nodeHandle);

ReferenceManager 管理：

目标轨迹 TargetTrajectories
模式调度 ModeSchedule

比如你给机器人发送目标速度或目标位姿，这些参考信息最终要进入最优控制问题的 cost / constraints。

第五段：创建 MPC。

GaussNewtonDDP_MPC mpc(
    interface.mpcSettings(),
    interface.ddpSettings(),
    interface.getRollout(),
    interface.getOptimalControlProblem(),
    interface.getInitializer());

这行代码把前面准备好的组件全部交给 MPC：

mpcSettings       → MPC 时间窗口、频率、冷/热启动
DDP settings      → SLQ/iLQR 求解参数
rollout           → 向前模拟动力学
OptimalControlProblem → 动力学 + 代价 + 约束
initializer       → 初始轨迹/控制初始化

第六段：把 reference manager 和 gait receiver 挂到 solver 上。

mpc.getSolverPtr()->setReferenceManager(rosReferenceManagerPtr);
mpc.getSolverPtr()->addSynchronizedModule(gaitReceiverPtr);

这里的 getSolverPtr() 返回底层 solver，比如 SLQ/iLQR solver。

这两句的意思是：

求解前后，solver 要同步最新参考轨迹和步态信息。

第七段：启动 ROS MPC node。

MPC_ROS_Interface mpcNode(mpc, robotName);
mpcNode.launchNodes(nodeHandle);

MPC_ROS_Interface 负责：

订阅当前机器人状态 observation
  ↓
触发 mpc.run()
  ↓
获取优化结果 primal solution
  ↓
发布 flattened controller 给 MRT/控制端

所以整个 main() 可以浓缩成：

读参数
  ↓
构造机器人最优控制问题
  ↓
构造 MPC solver
  ↓
挂接参考轨迹和步态
  ↓
启动 ROS 通信

8. SQP 版本 MPC：LeggedRobotSqpMpcNode

代码位置：

ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotSqpMpcNode.cpp

它和 DDP 版本非常像，只是 MPC 类型换成了 SqpMpc：

SqpMpc mpc(
    interface.mpcSettings(),
    interface.sqpSettings(),
    interface.getOptimalControlProblem(),
    interface.getInitializer());

然后也是：

mpc.getSolverPtr()->setReferenceManager(rosReferenceManagerPtr);
mpc.getSolverPtr()->addSynchronizedModule(gaitReceiverPtr);

MPC_ROS_Interface mpcNode(mpc, robotName);
mpcNode.launchNodes(nodeHandle);

这说明 OCS2 的设计是：

同一个机器人问题 OptimalControlProblem
可以交给不同求解器：
SLQ/iLQR/SQP/IPM...

也就是说，MPC 外壳和问题建模相对独立，求解器可以替换。

9. LeggedRobotInterface 到底做了什么

LeggedRobotInterface 是学习四足机器人 OCS2 MPC 时最值得重点看的部分。

它大致负责：

读取 taskFile / urdfFile / referenceFile
  ↓
构造机器人模型
  ↓
构造质心动力学 / centroidal model
  ↓
构造 rollout
  ↓
构造 initializer
  ↓
构造 OptimalControlProblem
  ↓
加入 cost / constraints / dynamics
  ↓
提供给 MPC 使用

可以把它理解成：

机器人业务建模层。

相比 MPC_BASE，它更接近机器人控制真正需要关注的内容。

对四足机器人来说，它通常会涉及：

动力学 dynamics

例如质心动力学：

$$\dot{x}=f(x,u,t)$$

其中状态 $x$ 可能包含：

• base pose。
• base velocity。
• joint position。
• joint velocity。

输入 $u$ 可能包含：

• contact force。
• joint velocity / torque 相关量。
• foot placement 相关控制量。

具体取决于 OCS2 legged robot 例子的建模方式。

代价 cost

常见代价包括：

• base 位置跟踪。
• base 姿态跟踪。
• 速度跟踪。
• 输入惩罚。
• 足端位置惩罚。
• terminal cost。

形式上常见为二次型：

$$L(x,u)= \frac{1}{2}(x-x_{\mathrm{ref}})^T Q (x-x_{\mathrm{ref}}) + \frac{1}{2}(u-u_{\mathrm{ref}})^T R (u-u_{\mathrm{ref}})$$

约束 constraints

四足机器人 MPC 最关键的是接触约束，例如：

• 支撑脚不能滑。
• 摆动脚不能穿地。
• 接触力满足摩擦锥。
• 支撑腿足端速度为 0。
• 接触切换按照 gait schedule 发生。

这些约束决定了 MPC 的可行性和运动质量。

ReferenceManager 和 GaitSchedule

腿式机器人是 switched system，因为不同时间段接触模式不同。

例如：

左前脚支撑 / 摆动
右前脚支撑 / 摆动
左后脚支撑 / 摆动
右后脚支撑 / 摆动

这些接触模式随时间切换，因此 OCS2 特别适合这类系统。

10. MPC 和 MRT 的关系

OCS2 里经常看到两个概念：

MPC: Model Predictive Control
MRT: Model Reference Tracking

可以粗略理解为：

MPC 低频求解未来轨迹。
MRT 高频读取并执行当前控制。

例如：

MPC: 20 Hz
MRT: 100 Hz / 200 Hz / 500 Hz

MPC 求出的是一段未来轨迹和反馈策略：

x*(t), u*(t), K(t)

MRT 在控制循环中根据当前时间插值，得到当前要执行的控制：

$$u(t)=u^*(t)+K(t)\bigl(x(t)-x^*(t)\bigr)$$

这里：

• $u^*(t)$：前馈控制。
• $K(t)$：反馈增益。
• $x^*(t)$：优化出来的参考状态。
• $x(t)$：机器人当前真实状态。

这点很重要：

OCS2 的 MPC 不是简单开环播放轨迹，而是可以输出带反馈的控制策略。

11. ROS 接口代码：MPC_ROS_Interface

代码位置：

ocs2_ros_interfaces/include/ocs2_ros_interfaces/mpc/MPC_ROS_Interface.h
ocs2_ros_interfaces/src/mpc/MPC_ROS_Interface.cpp

MPC_ROS_Interface 是 MPC 和 ROS 系统之间的桥。它自己不负责求解最优控制问题，而是负责通信和触发。

头文件里有几个关键成员：

MPC_BASE& mpc_;

::ros::Subscriber mpcObservationSubscriber_;
::ros::Subscriber mpcTargetTrajectoriesSubscriber_;
::ros::Publisher mpcPolicyPublisher_;
::ros::ServiceServer mpcResetServiceServer_;

这些成员对应：

11.1 接收 observation 后怎么触发 MPC

最关键的回调函数是：

void MPC_ROS_Interface::mpcObservationCallback(
    const ocs2_msgs::mpc_observation::ConstPtr& msg) {

  const auto currentObservation =
      ros_msg_conversions::readObservationMsg(*msg);

  bool controllerIsUpdated =
      mpc_.run(currentObservation.time, currentObservation.state);

  if (!controllerIsUpdated) {
    return;
  }

  copyToBuffer(currentObservation);

  readyToPublish_ = true;
  msgReady_.notify_one();
}

这段代码非常关键，它说明 OCS2 MPC 的触发源是 observation：

收到当前状态 observation
  ↓
读取 current time 和 current state
  ↓
调用 mpc_.run(time, state)
  ↓
MPC 重新优化
  ↓
把结果复制到 buffer
  ↓
通知发布线程发布策略

也就是说，真实运行时不是某个 while 循环直接不停调用 MPC，而是 ROS observation callback 在驱动 MPC 更新。

11.2 优化结果怎么发布出去

MPC 求解完成后，会调用：

copyToBuffer(currentObservation);

其中核心逻辑是：

scalar_t finalTime = mpcInitObservation.time + mpc_.settings().solutionTimeWindow_;
if (mpc_.settings().solutionTimeWindow_ < 0) {
  finalTime = mpc_.getSolverPtr()->getFinalTime();
}

mpc_.getSolverPtr()->getPrimalSolution(
    finalTime,
    bufferPrimalSolutionPtr_.get());

这里的 PrimalSolution 可以理解为 MPC 的主要优化结果，里面通常包含：

timeTrajectory
stateTrajectory
inputTrajectory
controllerPtr
modeSchedule

然后发布线程会调用：

createMpcPolicyMsg(
    *publisherPrimalSolutionPtr_,
    *publisherCommandPtr_,
    *publisherPerformanceIndicesPtr_);

这个函数会把 C++ 内部数据打包成 ROS 消息：

ocs2_msgs::mpc_flattened_controller

其中包括：

initObservation
planTargetTrajectories
modeSchedule
performanceIndices
timeTrajectory
stateTrajectory
inputTrajectory
controller data

最后：

mpcPolicyPublisher_.publish(mpcPolicyMsg);

所以 MPC 的输出链路是：

solver 内部 PrimalSolution
  ↓
copyToBuffer()
  ↓
createMpcPolicyMsg()
  ↓
mpc_flattened_controller ROS message
  ↓
MRT / 控制端订阅并执行

这部分代码对于理解 OCS2 很重要，因为它回答了一个工程问题：

MPC 求解器算出来的 C++ 对象，怎么变成控制端能用的 ROS 消息？

12. SLQ / iLQR 大概在做什么

SLQ / iLQR / DDP 的核心思想是：

把非线性最优控制问题，在当前轨迹附近近似成一系列时变 LQR 问题，然后迭代改进轨迹。

大概流程：

给一条初始轨迹
  ↓
沿轨迹线性化动力学
  ↓
二次近似代价函数
  ↓
反向求 Riccati 方程，得到反馈增益
  ↓
前向 rollout，生成新轨迹
  ↓
重复直到收敛或达到迭代次数

动力学线性化：

$$\delta \dot{x}=A(t)\delta x+B(t)\delta u$$

代价二次近似：

$$L(x,u)\approx \frac{1}{2} \begin{bmatrix} \delta x \\ \delta u \end{bmatrix}^T \begin{bmatrix} Q & P \\ P^T & R \end{bmatrix} \begin{bmatrix} \delta x \\ \delta u \end{bmatrix}$$

然后求出局部控制律：

$$\delta u = k(t)+K(t)\delta x$$

在 MPC 语境里，求解器每次都在当前状态附近重新做这个过程。

13. SolverBase：求解器统一接口

代码位置：

ocs2_oc/include/ocs2_oc/oc_solver/SolverBase.h

SolverBase 是底层求解器的统一接口。MPC 不直接关心 SLQ、SQP、IPM 的所有细节，而是通过这个接口拿结果。

几个重要函数：

void run(scalar_t initTime,
         const vector_t& initState,
         scalar_t finalTime);

这是求解器主入口。MPC 最终会调用到这里。

virtual scalar_t getFinalTime() const = 0;

返回当前 solver 优化到的最终时间。MPC_BASE::run() 会用它检查当前时间是否已经超过已有策略覆盖范围。

virtual void getPrimalSolution(
    scalar_t finalTime,
    PrimalSolution* primalSolutionPtr) const = 0;

获取优化结果。ROS 接口发布策略时会调用它。

void setReferenceManager(
    std::shared_ptr<ReferenceManagerInterface> referenceManagerPtr);

设置参考轨迹管理器。四足机器人例子里就是：

mpc.getSolverPtr()->setReferenceManager(rosReferenceManagerPtr);

void addSynchronizedModule(
    std::shared_ptr<SolverSynchronizedModule> synchronizedModule);

添加需要和 solver 同步的模块，例如 gait receiver。

所以 SolverBase 在架构里的角色是：

MPC_BASE
  ↓ 调用 run()
SolverBase
  ↓ 具体实现
SLQ / ILQR / SQP / IPM

从工程角度看，这种接口设计让 ROS 层、MPC 层和具体 solver 层解耦。

14. 学习时应该优先读哪些文件

我建议按层次读。

第一层：MPC 外壳

ocs2_mpc/include/ocs2_mpc/MPC_BASE.h
ocs2_mpc/src/MPC_BASE.cpp
ocs2_mpc/include/ocs2_mpc/MPC_Settings.h

目标：理解 OCS2 的 MPC 如何滚动调用求解器。

第二层：DDP-MPC 包装

ocs2_ddp/include/ocs2_ddp/GaussNewtonDDP_MPC.h
ocs2_ddp/include/ocs2_ddp/SLQ.h
ocs2_ddp/include/ocs2_ddp/ILQR.h
ocs2_ddp/include/ocs2_ddp/GaussNewtonDDP.h

目标：理解 SLQ / iLQR 如何被封装成 MPC。

第三层：四足机器人 ROS 入口

ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotDdpMpcNode.cpp
ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotSqpMpcNode.cpp

目标：理解实际例子如何创建并启动 MPC node。

第四层：机器人问题建模

ocs2_robotic_examples/ocs2_legged_robot

重点找：

LeggedRobotInterface
OptimalControlProblem
dynamics
cost
constraint
reference manager
gait schedule

目标：理解机器人问题如何变成 OCS2 可以求解的 optimal control problem。

15. 一个推荐的理解顺序

如果你刚开始学，我建议不要一上来就钻 Riccati 方程。可以先从代码结构入手：

第一步：看 MPC_BASE::run()

记住一句话：

run() 根据当前时间和状态，计算 finalTime，然后调用 calculateController()。

第二步：看 GaussNewtonDDP_MPC

记住一句话：

GaussNewtonDDP_MPC 把 SLQ / iLQR 包成 MPC。

第三步：看 LeggedRobotDdpMpcNode.cpp

记住一句话：

这个文件展示了四足机器人例子如何创建 interface、mpc、reference manager、gait receiver 和 ROS node。

第四步：看 LeggedRobotInterface

记住一句话：

这里才是机器人建模的核心：动力学、代价、约束、参考、步态都在这里汇总。

第五步：再补 SLQ / iLQR 原理

这个时候再看：

• 线性化。
• 二次近似。
• Riccati backward pass。
• rollout forward pass。
• line search。
• constraint handling。

会更容易理解。

总结

OCS2 里的 MPC 可以概括为：

用 MPC_BASE 做滚动时域外壳，用 SLQ/iLQR/SQP/IPM 做单次最优控制求解器，用 OptimalControlProblem 描述机器人动力学、代价和约束，再通过 ROS/MRT 把优化轨迹变成实际控制输入。

最重要的代码主线是：

LeggedRobotDdpMpcNode.cpp
  ↓
LeggedRobotInterface
  ↓
GaussNewtonDDP_MPC
  ↓
MPC_BASE::run()
  ↓
SLQ / iLQR solver
  ↓
MPC_ROS_Interface / MRT

如果只是为了入门，先别被算法细节吓到。先把代码结构和数据流看懂：

当前状态从哪里来？
参考轨迹从哪里来？
动力学在哪里定义？
代价和约束在哪里加入？
MPC 每次在哪里被调用？
求出的控制策略在哪里被执行？

这些问题想清楚后，再深入 DDP、SQP、约束处理和腿式机器人的接触切换，会顺很多。