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_mpcMPC循环的封装包括状态更新、模型更新、控制输出ocs2_pinocchio对接Pinocchio的动力学接口ocs2_robotic_assets官方提供的机器人模型文件比如Anymal、Quadruped等ocs2_rosROS接口和Rviz可视化工具。我自己搭建时只依赖了其中一部分核心、MPC、Pinocchio、以及少量官方工具。ROS虽然OCS2也支持得比较好但我个人为了减少环境变量干扰直接用纯C的App跑只是最后用Rviz看轨迹。如果你想用ROS 2OCS2也提供了对应的支持包只是我这边没有细测过。从工程实现角度讲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_TYPERelease -DCMAKE_INSTALL_PREFIX/usr/local make -j$(nproc) sudo make install这里要注意--recursive参数一定要带因为Pinocchio依赖一些子模块比如eigenpy用于Python绑定、hpp-fcl用于碰撞检测不递归拉取的话后面编译会报找不到eigenpy头文件。编译过程中我遇到过一个问题Eigen3版本太新导致Pinocchio中某些代码使用了Eigen3内部已经不推荐使用的头文件。解决办法是把C标准调到C17并且在使用时加上-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 ocs2OCS2是一个多模块仓库你不需要全部编译。在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_TYPERelease -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个关节角速度加起来是76121237维。看起来比较接近但因为四元数的约束是等式约束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的对角元通常这样设位置误差权重10100姿态误差权重欧拉角或者旋转矩阵误差50200线速度误差权重110角速度误差权重10100关节位置误差权重0.11关节角速度误差权重0.010.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_sharedMyQuadrupedModel(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_sharedMyReferenceManager(); 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.xOCS2编译时找不到ocs2_core包没有先编译安装ocs2_core按依赖顺序编译先make install ocs2_coreCould 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这条路走顺。环境配置确实折磨人但一旦跑通后面就是纯粹的乐趣了。
四足机器人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_mpcMPC循环的封装包括状态更新、模型更新、控制输出ocs2_pinocchio对接Pinocchio的动力学接口ocs2_robotic_assets官方提供的机器人模型文件比如Anymal、Quadruped等ocs2_rosROS接口和Rviz可视化工具。我自己搭建时只依赖了其中一部分核心、MPC、Pinocchio、以及少量官方工具。ROS虽然OCS2也支持得比较好但我个人为了减少环境变量干扰直接用纯C的App跑只是最后用Rviz看轨迹。如果你想用ROS 2OCS2也提供了对应的支持包只是我这边没有细测过。从工程实现角度讲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_TYPERelease -DCMAKE_INSTALL_PREFIX/usr/local make -j$(nproc) sudo make install这里要注意--recursive参数一定要带因为Pinocchio依赖一些子模块比如eigenpy用于Python绑定、hpp-fcl用于碰撞检测不递归拉取的话后面编译会报找不到eigenpy头文件。编译过程中我遇到过一个问题Eigen3版本太新导致Pinocchio中某些代码使用了Eigen3内部已经不推荐使用的头文件。解决办法是把C标准调到C17并且在使用时加上-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 ocs2OCS2是一个多模块仓库你不需要全部编译。在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_TYPERelease -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个关节角速度加起来是76121237维。看起来比较接近但因为四元数的约束是等式约束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的对角元通常这样设位置误差权重10100姿态误差权重欧拉角或者旋转矩阵误差50200线速度误差权重110角速度误差权重10100关节位置误差权重0.11关节角速度误差权重0.010.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_sharedMyQuadrupedModel(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_sharedMyReferenceManager(); 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.xOCS2编译时找不到ocs2_core包没有先编译安装ocs2_core按依赖顺序编译先make install ocs2_coreCould 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这条路走顺。环境配置确实折磨人但一旦跑通后面就是纯粹的乐趣了。
