四足机器人MPC控制实战：OCS2与Pinocchio完整搭建指南-育师

1. 不要把四足MPC想得太玄，OCS2帮我省掉了最痛苦的那部分

做四足机器人控制的人，应该都有过这种经历：仿真里跑得挺好，一上实物就各种抖；或者看论文里MPC（模型预测控制）效果好得不行，自己一写就发现光是把动力学模型从URDF里读出来、再变着法求导，就能耗掉几个星期。我最初接触OCS2这套工具箱，就是为了省掉这部分工作量，结果发现它确实把“从模型到控制器”这条路走顺了不少。

OCS2是瑞士苏黎世联邦理工（ETH Zürich）开源的最优控制工具箱，全称是Optimal Control for Switched Systems，专门处理一类带切换机制的优化控制问题——四足机器人的支撑相切换、飞腿相切换，天然就适合这套框架。它内部已经帮你封装好了MPC的滚动优化流程：要做的就是提供动力学模型、代价函数、约束，然后给定参考轨迹，它就能在毫秒级时间内求解出当前时刻的控制量。对于做足式机器人控制的人来说，这意味着你可以把精力放在“控制逻辑怎么写”上，而不是反复造QP（二次规划）求解器的轮子。

这篇文章我打算按自己实操的顺序来写：先讲整体设计思路，再讲环境配置（重点说Pinocchio，因为这是OCS2跑起来最容易出问题的一环），然后是建立机器人的动力学模型、配置MPC参数、写参考轨迹生成，最后列出我实际踩过的坑和排查方法。我用的机器人模型是四足机器人，仿真环境是RaiSim，但大部分内容对Gazebo、MuJoCo或者实物平台同样适用。

需要先说清楚：OCS2不是那种装完就能跑的库，它要求你对刚体动力学、凸优化、甚至是代码模板特化都有一定了解。这篇文章的目标是让你跟着我一步步走完整个流程，同时理解每一步为什么要这么做。看这篇文章之前，你最好已经熟悉C++和CMake，对ROS有一个基础认知，并且听说过URDF是什么。如果你这些都不熟，建议先把这些基础补一补再回头看OCS2，否则直接上可能有点难受。

下面我尽量按大家最容易卡壳的顺序来写，尤其是Pinocchio配置那一段，我会把编译时的坑、模型转换的坑、数据和传感器对齐的坑都讲透。毕竟工具是死的，人是活的，大多数问题不是OCS2本身太难，而是环境没配对、模型没弄对、参数没调好。

2. 整体思路拆分：OCS2解决什么问题，我为什么选了它

动手之前，先理清楚“用OCS2搭MPC控制器”到底是一条什么样的技术路线，这在真正上手时非常重要。

2.1 MPC对四足机器人到底做什么，框架怎么选

四足机器人走路，本质上是一个“在动态不稳定的状态下不断找平衡”的过程。MPC做的事情可以通俗理解为：我有一整个未来的参考轨迹（比如下一步迈到哪儿、身体怎么保持水平），我需要在当前时刻，基于当前的状态和动力学模型，计算出接下来一小段时间内最合适的关节力矩。然后每隔几个毫秒，随着状态变化，再重新求解一遍，所以叫“滚动优化”或者“模型预测控制”。

在OCS2里，这套框架分了几层：

最底层是刚体动力学接口，它通过Pinocchio或者RaiSim提供的动力学算法，拿到质量矩阵、科氏力、重力项；
中间层是系统模型定义，包括状态向量、输入向量、连续动力学方程；
上层是最优控制问题设置，包括代价矩阵、输入限制、状态约束；
最上层是SQP求解器（Sequential Quadratic Programming），它负责在有限时间内把整个NLP（非线性规划）问题求解出来。

为了跑四足MPC，有不少替代方案：你可以直接用ACADO、CasADi加IPOPT，也可以手动推导动力学然后用qpOASES解QP。但这些方案要么公式推导量大，要么求解速度不够快。OCS2的优势在于它本身针对足式机器人做了大量优化，包括切换系统处理、线性化点选取、以及确定性SQP求解器的高效实现，在学术界和工业界都有成功落地的案例。

我做选型时也纠结过要不要直接用整程最优控制求解器做全轨迹优化，但考虑到四足控制需要高频在线计算——一般控制频率至少500Hz，步态周期大概0.2到0.4秒，MPC预测时域也就0.5到1秒——OCS2在速度和稳定性上的平衡做得最好。

2.2 OCS2支持的结构和各模块作用

OCS2从功能上看，大致可以分成这么几个模块：

ocs2_core：定义系统模型、代价函数、状态输入约束等核心数据结构；
ocs2_oc：最优控制问题的基础求解框架，包括SQP求解器接口；
ocs2_ddp：基于DDP（微分动态规划）的求解器实现；
ocs2_mpc：MPC循环的封装，包括状态更新、模型更新、控制输出；
ocs2_pinocchio：对接Pinocchio的动力学接口；
ocs2_robotic_assets：官方提供的机器人模型文件，比如Anymal、Quadruped等；
ocs2_ros：ROS接口和Rviz可视化工具。

我自己搭建时，只依赖了其中一部分：核心、MPC、Pinocchio、以及少量官方工具。ROS虽然OCS2也支持得比较好，但我个人为了减少环境变量干扰，直接用纯C++的App跑，只是最后用Rviz看轨迹。如果你想用ROS 2，OCS2也提供了对应的支持包，只是我这边没有细测过。

从工程实现角度讲，OCS2给的是一个框架，但不是一个开箱即用的完整控制器。如果你要用四足机器人，你需要自己定义状态向量、输入向量、摩擦锥约束、正常力模型等等。OCS2自带几个官方例子，比如Anymal和Quadruped，建议你先跑通官方例子再换成自己的机器人。

这里要特别强调一个点：OCS2的MPC依赖于一个相对准确的动力学模型。如果你的机器人质量分布、执行器延迟补偿做得不好，MPC的预测效果会很差。我之前试过随便改了一个质量参数，结果直接在仿真里摔跤，所以模型校准这一步绝对不能省。

2.3 工作流程总览：从URDF到MPC控制器

整体工作流程可以总结成一条主线：

准备机器人URDF模型；
利用Pinocchio读取URDF并做动力学计算；
基于OCS2的系统模型定义自己的四足系统模型；
根据机器人结构设计状态和输入，配置代价矩阵；
实现参考轨迹生成器（步态规划器）；
配置MPC参数（预测时域、控制时域、迭代次数）；
启动MPC循环，观察仿真或实物表现。

后面我会按这个流程展开。遇到具体的代码片段，我会直接展示我在实际项目中用过的版本。由于不同版本的OCS2接口有差异，这里我会尽量基于一个相对稳定的版本来讲，但也会提一下版本更新带来的变化，避免你对着老教程抄的时候被坑。

3. 环境配置：Pinocchio是整个流程最容易卡住的地方

3.1 为什么单独把Pinocchio拎出来讲

OCS2本身需要求解大规模稀疏矩阵和做刚体动力学计算，它支持Pinocchio和RaiSim两种后端。我最后选择了Pinocchio，因为它更轻量、安装相对简单，而且从URDF导入模型非常方便。RaiSim虽然仿真精度高，但它是一个商业库，需要许可证，个人用户申请起来麻烦。

Pinocchio是一个用C++实现的刚体动力学库，由巴黎六尺实验室和LAAS-CNRS联合开发。它支持URDF模型导入、正向运动学、逆向运动学、惯性矩阵计算、科氏力、重力项计算等，OCS2通过一个叫做PinocchioInterface的类来调用这些能力。

问题在于，Pinocchio本身依赖比较多：Eigen3、Boost、urdfdom、urdfdom_headers、console_bridge、tinyxml等。而且OCS2对Pinocchio的版本有要求，如果版本不匹配，编译会出现一堆莫名其妙的报错，比如找不到头文件、函数签名不匹配，甚至模板实例化失败。

3.2 一步步配置Pinocchio环境

在Ubuntu 20.04 + ROS Noetic环境下，我的完整配置路径如下。如果你用的是Ubuntu 22.04 + ROS 2 Humble，原理相同，只是部分包名可能有变化。

第一步，安装基础依赖：

sudo apt update sudo apt install -y git build-essential cmake libeigen3-dev libboost-all-dev liburdfdom-dev liburdfdom-headers-dev libconsole-bridge-dev libtinyxml-dev

注意，这几个依赖一个都不能少。特别是liburdfdom-dev，如果缺失，Pinocchio在解析URDF时会编译失败。我当时漏掉了libconsole-bridge-dev，结果编译OCS2时报了一个跟日志系统相关的错误，愣是排查了好几个小时。

第二步，编译安装Pinocchio。推荐用源码编译，不要用apt安装老版本，因为OCS2官方要求Pinocchio的版本不能太低。我的做法是：

cd ~ git clone --recursive https://github.com/stack-of-tasks/pinocchio.git cd pinocchio mkdir build && cd build cmake .. -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=/usr/local make -j$(nproc) sudo make install

这里要注意：--recursive参数一定要带，因为Pinocchio依赖一些子模块，比如eigenpy（用于Python绑定）、hpp-fcl（用于碰撞检测），不递归拉取的话后面编译会报找不到eigenpy头文件。

编译过程中我遇到过一个问题：Eigen3版本太新，导致Pinocchio中某些代码使用了Eigen3内部已经不推荐使用的头文件。解决办法是把C++标准调到C++17，并且在使用时加上-DEIGEN_DONT_VECTORIZE宏定义，避免一部分对齐问题。

第三歩，验证Pinocchio是否装好。最简单的验证方式是写一个测试代码，读取任意一个URDF文件，做一次正向运动学计算。如果你在编译OCS2之前先把Pinocchio自带的单元测试跑一遍，会省很多事：

cd build ctest

但注意，Pinocchio的测试依赖一些额外的数据包，如果下载不完整，部分测试会跳过，这并不影响正常使用。

3.3 编译OCS2时的版本匹配与常见报错

OCS2建议从源码编译，GitHub直接拉取即可：

git clone https://github.com/leggedrobotics/ocs2.git cd ocs2

OCS2是一个多模块仓库，你不需要全部编译。在ocs2根目录下有ocs2_pinocchio、ocs2_mpc、ocs2_core等子目录，建议使用CMake单独编译需要的模块。

我用的编译顺序是：

先编译ocs2_core和ocs2_ddp，它们不依赖ROS；
再编译ocs2_pinocchio，这会依赖Pinocchio和ocs2_robotic_assets；
最后编译ocs2_mpc和ocs2_ros_tools（如果你不用ROS，可以跳过ROS相关模块）。

一个典型的CMake配置过程如下：

cd ~/ocs2 mkdir build && cd build cmake .. -DCMAKE_BUILD_TYPE=Release -DPINOCCHIO_INCLUDE_DIR=/usr/local/include -DPINOCCHIO_LIBRARY_DIR=/usr/local/lib make -j$(nproc)

如果一切顺利，你会看到编译进度条跑完。如果报错，我遇到的几个比较典型的情况是：

fatal error: pinocchio/multibody/model.hpp: No such file or directory这说明CMake找不到Pinocchio头文件，多半是PINOCCHIO_INCLUDE_DIR设错了。检查一下/usr/local/include下有没有pinocchio目录，如果没有，说明你安装时CMAKE_INSTALL_PREFIX设置得跟CMake搜索路径不一致。
undefined reference to pinocchio::...这是链接错误，说明Pinocchio库路径没对。在CMakeLists.txt中，确保target_link_libraries里加了pinocchio，同时检查链接顺序——一般pinocchio要放在所有依赖它的库之后。
Eigen3::Eigentarget not found
这个通常是Eigen3没装好或者CMake没找到。如果用的是系统Eigen3，应该没问题；如果是conda装的其他版本，路径可能冲突。建议直接用系统源安装的Eigen3。

OCS2官方文档还推荐使用docker，如果你不想被环境问题纠缠，使用官方Docker镜像是一个更稳妥的选择。我之前纯粹是为了搞清楚底层依赖关系才选择手动配置，目前手动配置对我来说已经比较熟悉了，所以后续直接用源码编译。

3.4 URDF模型准备：从SolidWorks到URDF要注意的几个点

URDF是四足机器人模型的源头，OCS2通过Pinocchio从URDF中读取连杆质量、惯性矩阵、关节限位等信息。所以URDF的质量直接决定了后续动力学计算的准确性。

如果你的机器人是自己设计的，通常从SolidWorks或者Fusion 360导出URDF。这一步有一个非常关键的点：导出的URDF中，碰撞几何体可能会非常复杂，比如每个连杆都是一堆细碎的mesh网格，这会严重拖慢Pinocchio的碰撞检测计算。我一般会把碰撞模型简化成基本几何体：长方体、圆柱体、球体，因为MPC阶段其实不需要非常精细的碰撞体，简化之后的计算效率提升明显。

另外一个点是惯性矩阵。很多CAD导出的URDF惯性参数是在CAD定义坐标系下的，跟连杆坐标系对不齐，这会导致动力学计算出现错误。建议每根杆件都检查一下<inertial>标签里的<origin>是否正确，是否和视觉/碰撞坐标系一致。我踩过这个坑：有个关节的惯性主轴偏了，导致机器人在仿真里每个周期都往一个方向倾斜，后来排查发现是旋转轴定义反了。

还有关节限位。OCS2的MPC支持关节位置约束，如果你的URDF里没写关节限位，那么优化器会认为关节可以无限旋转，最终结果可能非常离谱。所以在URDF里一定要为每个关节设置合理的<limit>。

最后，URDF里的<material>标签对Pinocchio没有影响，可以忽略。OCS2的Pinocchio接口主要读取的是连杆质量和惯性信息，以及关节的运动学参数。视觉网格对于动力学纯计算并不需要，但为了Rviz可视化，建议保留。

4. 搭建MPC控制器：从系统模型定义到求解器配置

4.1 系统模型定义与状态向量设计

OCS2的核心操作对象是最优控制问题：

[ \min_{x(\cdot), u(\cdot)} \int_0^{T} l(x,u) dt + \phi(x(T)) ]

其中(x)是状态，(u)是输入，(l)是运行代价，(\phi)是末端代价。对于四足机器人，常见状态向量是底座位姿（位置+姿态）、底座速度（线速度+角速度）、各关节角度和角速度。输入向量就是各关节的力矩，外加底座的虚拟控制力（用于模拟漂浮基座的反作用力）。

我自己用的状态维度是36维：7维底座位姿（四元数+位置） + 6维底座速度 + 4条腿 × 3个关节角 + 4条腿 × 3个关节角速度，加起来是7+6+12+12=37维。看起来比较接近，但因为四元数的约束是等式约束，OCS2内部通常会做处理。

在设计状态向量时，排列顺序需要和Pinocchio接口的数据顺序保持一致。OCS2的Pinocchio接口默认状态是7维底座位姿（位置+四元数） + n维关节角度，速度是6维底座速度 + n维关节角速度。如果你自己定义的时候不按照这个顺序，那么在你实现computeMapping时要把顺序重新映射一遍，非常容易出错。建议直接沿用Pinocchio接口的顺序，省去不少麻烦。

输入向量方面，底座虚拟力有6维（力+力矩），腿关节力矩有12维，输入向量总共18维。这里有一点要注意：底座零重力空间速度实际上是6，但如果你的机器人有浮动基座，那么除了躯干6自由度以外，还需要考虑角动量的影响。OCS2的处理方式是把底座角动量当作一个状态约束来近似。

4.2 MPC代价函数配置：权重怎么调才靠谱

代价函数是MPC表现好坏的核心。OCS2的代价函数被抽象为一个标量函数，通常可以写成二次型：

[ l(x,u) = (x - x_{ref})^T Q (x - x_{ref}) + (u - u_{ref})^T R (u - u_{ref}) ]

其中Q是状态权重矩阵，R是输入权重矩阵。在四足机器人MPC中，Q的对角元通常这样设：

位置误差权重：10～100；
姿态误差权重（欧拉角或者旋转矩阵误差）：50～200；
线速度误差权重：1～10；
角速度误差权重：10～100；
关节位置误差权重：0.1～1；
关节角速度误差权重：0.01～0.1。

具体数值取决于你机器人的质量和步态频率。如果Q设得太大，机器人会显得非常“僵硬”，小扰动就会产生很大的纠正力矩，容易抖；如果Q设得小，机器人会变得懒散，轨迹跟踪误差大。

R矩阵（输入权重）一般设得较小，因为关节力矩不太容易出现过大的问题，但如果R太小，会出现力矩震荡。我在Anymal模型上常用R = 0.01~0.05。

还有一个关键参数是状态约束。OCS2支持关节位置限制、速度限制、摩擦力约束等。摩擦锥约束尤为重要：四足机器人脚底和地面之间的摩擦力是有限的，你不能期望脚底产生无限大的水平力。OCS2是通过在优化问题中加入线性化的摩擦锥约束来实现的，一般取摩擦系数0.5~0.8。

我的经验是，把摩擦系数设成0.6左右比较稳。设得太低，机器人会频繁打滑；设得太高，MPC会过于乐观，最终因真实摩擦力不足而摔倒。

4.3 参考轨迹生成：步态规划的做法

MPC的输入之一是参考轨迹，也就是未来一段时间内，机器人希望达到的状态序列。四足机器人的参考轨迹通常由步态规划器生成，核心是脚底落点的位置规划。

OCS2官方示例中提供了一个基于“时间轴”的步态生成器：设定一个步态周期（比如0.4秒）、一个占空比（每条腿着地时间占整个周期的比例，比如0.75），把这些参数输入给GaitPatternGenerator，它就能生成每条腿的接触状态序列和足端参考轨迹。

最常见的四步步态是行走步态（walk）：四条腿轮流抬起，任意时刻有三条腿着地。还有一种是对侧小跑（trot）：对角的两条腿同时抬起，任意时刻两条腿着地。我用得比较多的是trot，因为它在动态平衡和实现难度上比较均衡。

步态规划器输出的参考轨迹至少包含：

身体期望位置和姿态轨迹；
身体速度期望轨迹；
各腿足端期望位置轨迹；
各腿接触状态切换时间。

在MPC控制器中，参考轨迹是实时接收的。我的做法是：用一个单独的线程跑步态规划器，以200Hz的频率更新参考轨迹，MPC的预测时域内取参考轨迹的时间切片。如果参考轨迹频率太低，会导致MPC预测的轨迹跟实际期望脱节，动作会变得迟滞。

对于四足上楼梯、斜坡等复杂地形，步态规划器还需要考虑地形高度、接触点可达性等等。OCS2官方提供了一个TerrainModel接口，你可以根据自己的地形感知算法去更新地面高度，从而调整支撑腿的参考高度和接触点。

4.4 MPC求解器参数配置与步态周期对齐

OCS2的MPC求解器有几个关键参数：

mpc_dt：控制周期，一般设0.01秒（100Hz）或者0.002秒（500Hz）。越小的控制周期，实时性压力越大；
mpc_horizon：预测时域，一般设0.5~1.0秒；
mpc_iterations：每次MPC求解的迭代次数，一般设1~3。设多了时间不够，设少了优化效果差；
mpc_nthreads：多线程求解的线程数，可设为CPU核数；
mpc_use_qp_solver：是否使用QP求解器，而不是全NLP求解器。

我常用的一组参数是：控制周期0.002秒（500Hz），预测时域0.6秒，求解迭代次数3次，线程数4。在CPU 8核的台式机上，单次求解大约耗时1~3毫秒，可以满足500Hz频率要求。

有一点很关键：MPC的求解时间和机器人步态周期必须对齐。如果步态周期是0.4秒，MPC时域0.6秒覆盖了1.5个周期，这样规划器可以看到“下一步”的状态变化，对提前规划有帮助。如果预测时域太短，MPC只看到当前步态周期内的事情，表现会偏“近视”，动态性能下降。

4.5 实战代码骨架：一个最小可跑的MPC循环

这里给出一个简化版但结构完整的MPC循环骨架，基于OCS2官方示例改写。核心思想是：初始化系统模型、参考轨迹管理器、MPC控制器，然后在一个循环中不断更新状态、求解、下发控制指令。

#include <ocs2_mpc/MPC_MRT_Interface.h> #include <ocs2_pinocchio/PinocchioInterface.h> #include "MyQuadrupedModel.h" int main(int argc, char** argv) { // 1. 加载URDF并构建模型接口 std::string urdfFile = "path/to/your/robot.urdf"; PinocchioInterface pinocchioInterface = buildPinocchioInterface(urdfFile); auto model = std::make_shared<MyQuadrupedModel>(pinocchioInterface); // 2. 创建MPC控制器 MPC_MRT_Interface mpcInterface(*model); mpcInterface.getMpc().setSolverType(SolverType::SQP); mpcInterface.getMpc().setHorizon(0.6); mpcInterface.getMpc().setTolerance(1e-4); // 3. 初始化参考轨迹管理器并启动 auto refManager = std::make_shared<MyReferenceManager>(); mpcInterface.getMpc().setReferenceManager(refManager); mpcInterface.reset(); // 4. 主循环 scalar_t time = 0.0; vector_t state = model->getInitialState(); vector_t input = vector_t::Zero(model->getInputDim()); while (time < 10.0) { mpcInterface.setCurrentState(state); mpcInterface.updatePolicy(time); input = mpcInterface.getOptimizedInput(); // 这里把input发给仿真器或机器人 // 用RaiSim或Gazebo做状态更新 // state = simulateOneStep(state, input, dt); time += dt; } return 0; }

上面这个骨架里，最关键的是MyQuadrupedModel和MyReferenceManager，它们分别对应系统模型和参考轨迹。实际项目中，这两个类需要自己实现，建议参考OCS2自带的Anymal示例进行修改。

4.6 关节执行器限制与安全保护

MPC求解出来的关节力矩，最终是要交给执行器的。如果执行器有最大输出限制，比如单个关节最大力矩30Nm，你必须在MPC的输入约束里加上这一条，否则优化器可能给出超过硬件极限的力矩，在仿真里看不出来，一上实物就直接把电机烧了。

OCS2支持输入约束的定义。实现方法是在你的系统模型类中，覆写getInputLimits()函数，返回一个下界和上界向量。同时在代价函数中也建议加入输入变化率惩罚，防止相邻控制周期之间的力矩跳变过大，这在实际系统上非常重要。

我还习惯在MPC输出之后做一次低通滤波，滤波系数0.8~0.95之间。这个滤波器能有效抑制高频抖振，代价是微小的相位延迟。对于四足机器人，这点延迟完全在可接受范围内。

另外，不要忘记给关节速度加限制。腿在摆动相时的角速度可能非常大，如果你的关节速度限制了180度/秒，MPC在没有这个约束时可以规划出远高于该速度的轨迹，最终导致跟踪失败甚至损坏机械结构。OCS2里可以直接用状态约束来实现速度限制，加上之后求解器会自动把轨迹约束在限速范围之内。

5. 常见问题与排查技巧实录

5.1 编译期错误速查表

这里整理了一些我在配置和编译OCS2时碰到的问题，全部是我自己实测过的，不是从论坛上抄的：

错误现象	可能原因	排查方法
`pinocchio/multibody/model.hpp`找不到	Pinocchio库未安装或路径不对	检查`/usr/local/include`是否存在`pinocchio`目录
`libpinocchio.so: cannot open shared object file`	动态库路径未配置	在`~/.bashrc`里加`export LD_LIBRARY_PATH=/usr/local/lib:$LD_LIBRARY_PATH`
编译OCS2报`urdfdom`相关错误	urdfdom版本过旧	用`sudo apt install liburdfdom-dev liburdfdom-headers-dev`升级
`Eigen3`相关宏定义错误	Eigen3版本过新导致不兼容	编译时加`-DEIGEN_DONT_VECTORIZE`，或者降级到Eigen3.3.x
OCS2编译时找不到`ocs2_core`包	没有先编译安装`ocs2_core`	按依赖顺序编译，先`make install ocs2_core`
`Could not find a package configuration file provided by "ocs2_ros"`	未安装ROS相关依赖	如果不用ROS，可以不编译这个模块；如果要用，先安装ROS和OCS2 ROS依赖

5.2 运行时调试技巧：看模型、看约束、看输出

OCS2这类的工具箱，最怕的就是“仿真里稳了但原因没搞明白”。我建议从一开始就养成调试的习惯，每改一个参数，都把对应状态曲线记录下来看，而不是只看机器人倒没倒。

第一个必须看的量是质心轨迹。四足MPC的稳定性核心是零力矩点（ZMP）或者质心动力学。如果质心偏离支撑多边形太多，机器人一定会倒。OCS2提供了Rviz可视化工具，可以显示预测的质心轨迹、下一步的足端位置。我在调参初期基本就盯着这个看。

第二个必须看的是每条腿的接触力。MPC会计算腿部法向力和摩擦力。如果某条腿的接触力在某些时刻变成0（表明腿离地），这是正常的；但如果所有腿的接触力同时接近0，那说明MPC认为机器人腾空了，这通常是因为状态估计或者接触状态切换时间出了问题。

第三个需要关注的是关节力矩的饱和情况。如果某个关节力矩持续在限位值附近震荡，说明对应关节的权重或者执行器约束设置太大。可以调低相关关节的Q权重，或者增加输入变化率惩罚。

还有一个通用建议：不要一上来就上完整模型，先跑一个简化模型（比如不考虑角动量影响、只用线性倒立摆近似），确认基本流程通了，再切换到完整动力学。这样可以大大减少调试时间。我自己第一次跑OCS2时，直接上完整模型，结果状态直接发散到天上去了，后来简化之后才发现是状态向量的顺序没有对齐。

5.3 从仿真到实物的几个关键检查项

在把OCS2 MPC从仿真迁移到实物之前，有幾個环节不做，基本必摔：

第一，检查执行器延迟。MPC是模型预测控制，对系统延迟极其敏感。如果你的真实系统从发给力矩到实际产生力矩之间存在延迟，比如50ms，那么MPC预测的轨迹和实际轨迹会错位，造成震荡。解决办法是把执行器延迟模型加入系统动力学中，或者在MPC输出之前对状态进行补偿。具体做法是在当前阶段使用延迟补偿后的状态进行求解。

第二，检查真实的关节力矩是否与命令一致。电机驱动器一般有电流环，你可以对比命令力矩和实际反馈力矩。如果差异过大，说明力矩控制PID参数没调好。这个不调好，MPC再厉害也没用。

第三，检查状态估计和机体重量的标定。MPC依赖本身对质量、质心位置、惯量矩阵的精确估计。如果这些参数偏差超过5%，动态性能下降会非常明显。建议先用F/T传感器做一次系统辨识，验证URDF中的动力学参数是否正确。

第四，步态相位和接触力检测之间的同步。如果机器人用足底力传感器或者虚拟接触开关来估计接触状态，需要确保MPC认为某条腿支撑的时候，真实的接触信号确实存在，否则MPC会在支撑相和摆动相之间错误切换，动作会很乱。

5.4 参数调优心得：从抖到稳的调参顺序

很多刚接触MPC的人，第一步就是调Q和R矩阵，但我的经验是，这个顺序其实是错的。我的调参顺序如下：

先把步态参数调起来：步态周期、占空比、足端抬高量。先用一个非常保守的步态参数（步高5cm，周期0.4s，占空比0.7）跑通全流程。
调摩擦系数：先设低一点（0.4），确认不发飘；再逐步升高，观察腿部打滑情况。
调Q权重：先调位置权重和姿态权重，把机器人站姿稳住，不需要快速移动；确认不抖之后，再逐步加速度权重。
调R权重：R太小，力矩震荡明显；R太大，响应迟钝。找到一个中间值，结合过渡时间来判断。
最后调MPC迭代次数和时域：在性能允许的前提下，迭代次数越多效果越好；时域太长会增大计算负担，时域太短看不到未来状态，一般取0.5~0.8秒。

另外一个诀窍是：先把MPC迭代次数设为1，看基本跟随效果；如果稳定，再增加到2或者3，看动态性能的改善。如果迭代次数从1增加到3之后效果反而变差，说明模型或代价函数本身有问题，迭代次数不是主要矛盾。

5.5 我踩过的“教科书不会写”的坑

最后分享几个我实际踩过、而且排查很久才发现的冷门问题，这几个问题在最开始如果没留意，后面会浪费很多时间。

第一个坑：URDF中质量单位的问题。SolidWorks导出的URDF默认质量单位是千克，惯性矩阵单位是千克·平方米，但如果你从别的软件导出，可能单位是克或者用错了坐标系，Pinocchio读进去后MPC算出来的量纲就不对，表现出来就是机器人无力支撑自己，或者站姿直接发散。建议在导入OCS2之前，先在Pinocchio里打印一下模型总质量，跟自己机器人标称质量对比，误差超过1%就要回头查URDF。

第二个坑：四元数带来的不连续性问题。OCS2内部处理姿态时用的是四元数，但是最优化问题对姿态误差的约束是高度非线性的。如果你在代价函数里直接使用四元数差值，可能会导致求解器在某些奇异姿态附近收敛差。更推荐使用旋转矩阵的Log映射来计算姿态误差，OCS2官方示例里有现成的函数可以调用。

第三个坑：关于PINOCCHIO_JOINT_TYPE的设定。Pinocchio的JointType需要与OCS2期望的模型类型匹配，比如是自由飞行基座（FloatingBase）还是固定基座（FixedBase）。在建PinocchioInterface时，必须把基座设为浮动类型，OCS2才能正确计算六自由度的基座动力学信息。如果设错了，初看数据也是正常的，但控制时飞腿相或摆动相会出现奇怪的位移偏差。

第四个坑： MPC输出频率和执行器更新频率不一致。我最早把MPC控制周期设成0.002秒，但仿真器的物理步长是0.004秒，导致控制量被重复或丢失了一拍，表现出来就是关节抖动。解决办法是把控制周期设成物理步长的整数倍，或者做线性插值补偿。

6. 写在最后的实操体会

我整个搭建下来，最大的感受是：OCS2把“最优控制求解”这个痛点解决得很好，但真正决定项目成败的反而是那些看起来不起眼的环节——URDF模型准不准、Pinocchio版本对不对、状态向量顺序有没有对齐、参考轨迹和步态周期有没有同步。可以说，OCS2帮你把“最后一公里”铺平了，但前面几千公里的准备，还是得自己实实在在地走一遍。

如果你正在折腾OCS2的四足MPC，我的建议是：先别急着改官方示例的算法逻辑，老老实实把Anymal或者Quadruped官方例程跑通，再换成自己的机器人模型。而且每一步都做小步验证：模型能加载，先站着不动；能站住，再走两步；能走路，再上坡、跑起来。这样定位问题会快很多，心情也不会太崩。

最后再分享一个小技巧：调参的时候，建议把每一步改动都记录下来，包括改了哪个参数、机器人表现怎么样。MPC超级敏感，有时候只是改了一个0.001量级的权重，机器人动作就会从稳变抖。有了记录，你才能知道改回哪个值能复现之前的效果，而不是每次重新盲调。

配套的Rviz可视化确实好用，可以同时看到MPC预测的轨迹、实际状态和参考轨迹三者的对比关系，我几乎每调一个参数都会开着看。比如姿态误差权重从50调到100之后，你能明显看到预测的底座姿态更接近参考姿态，但代价是关节力矩变“硬”。如果看到机器人站姿没问题但行走时摇晃严重，我一般会先看足端接触力曲线，大概率是接触状态或者摩擦系数设置有问题，而不是Q矩阵的问题。

希望这篇文章能帮你把OCS2和Pinocchio这条路走顺。环境配置确实折磨人，但一旦跑通，后面就是纯粹的乐趣了。