具身机器人三层控制架构：VLM规划、MPC运动与WBC全身控制实战

1. 具身机器人控制架构的整体设计思路

1.1 为什么需要三层架构而不是端到端

很多刚接触具身机器人的朋友会问一个问题：现在大模型这么强，为什么不直接用一个端到端的网络，从摄像头输入到关节力矩输出一把梭？我一开始也这么想过，但实际做过几个项目之后，发现这条路在当前阶段基本走不通，原因有三个。

第一是实时性。一个视觉语言模型（VLM）做一次推理，哪怕用上了各种加速手段，延迟通常也在几百毫秒到几秒之间。而机器人的底层控制环路，比如平衡控制，往往要求1kHz甚至更高的频率，也就是每1毫秒就要算一次。这两者差了三个数量级，硬凑在一起，机器人要么反应迟钝，要么直接摔倒。

第二是可解释性和安全性。端到端网络是个黑盒，它输出一个动作，你很难说清楚为什么。但机器人是要和物理世界打交道的，一旦出错可能就是硬件损坏或者人员受伤。分层之后，每一层都有明确的职责和可验证的边界，出问题也容易定位。

第三是任务与技能的分离。高层负责“做什么”，底层负责“怎么做”。这种分离让系统更灵活——换一个任务，只需要改高层；换一个机器人本体，只需要改底层。端到端的话，任何一点变化都可能需要重新训练整个网络。

所以三层架构的核心思路就是：用VLM做慢思考的规划，用MPC做中层的轨迹优化，用WBC做快反应的全身控制。这三层各司其职，通过明确定义的接口通信，既发挥了各自的长处，又规避了各自的短板。

1.2 三层各自的职责边界怎么划

划分职责边界是设计里最容易扯皮的地方。我见过不少项目，规划层把轨迹点给得太密，运动层又想做点“智能”的调整，结果两边打架，机器人抖得跟筛糠一样。根据我的经验，边界应该这么划：

VLM规划层：负责理解自然语言指令、识别场景中的物体和关系、生成高层任务序列或者粗略的路径点。它的输出频率很低，大概0.1到1Hz就够了。输出内容应该是“去桌子旁边，把杯子拿起来”这种级别的抽象指令，而不是具体的关节角度。

MPC运动层：接收高层给的路径点或者目标位姿，结合机器人当前的动力学模型和约束条件，优化出一条满足动力学可行性的轨迹。它的频率在10到100Hz之间。输出的是关节位置、速度或者力矩的参考轨迹。

WBC全身控制层：接收MPC给的参考轨迹，考虑全身的动力学约束（比如接触力、摩擦锥、关节力矩限幅），计算出每个关节最终的执行力矩。频率在500Hz到1kHz。这一层是保证机器人不摔倒、不超限的最后一道防线。

注意：边界划清楚之后，层与层之间的接口要尽量简单。我习惯用“目标位姿+时间戳”这种最简形式，避免传递一大堆中间状态，否则调试的时候根本不知道是哪一层出的问题。

1.3 三层架构相比其他方案的优势

有人可能会拿MVC三层架构来类比，说这不就是软件工程里的分层吗？思路确实有相通之处，但机器人控制的分层有它自己的特殊性。MVC里的层是逻辑上的分离，而机器人控制里的层是时间尺度上的分离。

和六边形架构那种强调端口适配器的模式不同，机器人控制更关心的是计算延迟和物理约束。六边形架构适合业务逻辑复杂但实时性要求不高的系统，而机器人控制恰恰相反，业务逻辑相对固定，但对实时性和确定性要求极高。

三层架构最大的优势在于每一层都可以独立替换和验证。比如你今天用了一个新的VLM，只要它的输出格式不变，下面的MPC和WBC完全不用动。明天你换了一个新的机器人本体，只要WBC适配好了，上面的规划层也不用改。这种模块化带来的工程便利性，在实际项目中能省下大量时间。

2. VLM规划层的核心细节与实操要点

2.1 VLM选型：从闭源API到本地部署

选VLM是规划层的第一步。目前主流的选择分两类：闭源API和本地部署模型。

闭源API的好处是省事，不用自己维护推理服务，模型能力也通常更强。但缺点也很明显：延迟不可控、成本随调用量线性增长、数据隐私有顾虑。如果你的机器人是在实验室里做研究，调用量不大，用API快速验证想法是没问题的。但一旦要上真实场景，尤其是需要长时间运行的场景，本地部署几乎是必然选择。

本地部署的话，模型尺寸是个关键权衡。7B级别的模型在单张消费级显卡上就能跑起来，但理解复杂场景的能力有限。13B到34B的模型效果明显更好，但需要更好的硬件。我个人的经验是，对于大多数桌面操作任务，13B左右的模型已经够用了。如果任务涉及复杂的空间推理或者多步规划，再考虑上更大的模型。

实操心得：不要一上来就追求最大的模型。先用小模型把整个流水线跑通，确认接口和通信没问题，再逐步换大模型。我见过太多人卡在模型部署上，结果整个项目进度被拖慢。

2.2 提示词工程：让VLM输出结构化指令

VLM的输出必须是结构化的，否则下游的MPC没法解析。我通常会用JSON格式来约束输出，比如：

{ "task": "pick_and_place", "target_object": "red_cup", "target_location": "table_center", "constraints": ["avoid_obstacle", "keep_upright"] }

提示词的设计有几个要点。第一，要明确告诉模型输出格式，最好给一个例子。第二，要把机器人的能力边界写进去，比如“机器人只有两个手臂，没有移动底盘”，避免模型规划出无法执行的任务。第三，要加入安全相关的约束，比如“不要规划超过机器人工作空间的路径”。

我试过用AutoGLM-Phone这类模型来做任务切换，效果还不错。它的优势是支持多尺寸部署，可以根据硬件条件灵活选择。但要注意，这类模型对提示词的格式比较敏感，换一个提示词模板可能效果就差很多。所以提示词最好固定下来，不要频繁改动。

2.3 场景理解与任务分解的常见坑

场景理解是VLM规划层最容易出问题的地方。我踩过的坑包括：模型把背景里的物体误认为是目标物体、对空间关系的判断经常出错、多步任务中间步骤遗漏。

解决这些问题，我的经验是不要完全依赖VLM的原始输出。可以在VLM后面加一个轻量级的校验模块，用传统的计算机视觉方法（比如目标检测）来验证VLM提到的物体是否真的存在、位置是否合理。如果校验不通过，就重新规划或者请求人工干预。

任务分解方面，我习惯把复杂任务拆成原子动作的序列，每个原子动作对应一个MPC可以处理的目标。比如“把杯子放到架子上”可以拆成：移动到杯子附近、抓取杯子、移动到架子附近、放置杯子。每个步骤之间的过渡要平滑，不能让机器人突然急停或者急转。

3. MPC运动层的原理与实现细节

3.1 MPC为什么适合做中层控制

MPC，也就是模型预测控制，它的核心思想是在每个控制周期，基于当前的系统状态，求解一个有限时域的优化问题，得到一串控制输入，但只执行第一个，然后下一个周期重新求解。这种“滚动优化”的机制，让它天然适合处理带约束的控制问题。

在具身机器人里，MPC主要处理的是轨迹跟踪和避障。相比PID这类无模型控制器，MPC的优势在于它能显式地处理约束，比如关节角度限幅、速度限幅、避障距离约束。而且因为它是基于模型的，所以对系统的未来行为有预测能力，跟踪效果更平滑。

但MPC的缺点也很明显：计算量大。尤其是当系统维度高、约束多的时候，求解优化问题可能来不及在控制周期内完成。所以实际应用中，通常会对模型做简化，比如用线性化的动力学模型，或者降低控制频率。

3.2 动力学模型的选择与线性化处理

MPC需要一个动力学模型来预测未来状态。对于机械臂，常用的模型有刚体动力学模型和简化模型。刚体动力学模型精度高，但计算复杂。简化模型比如双积分器模型，计算简单但精度差。

我的建议是：如果控制频率在100Hz以下，可以用简化模型，因为简化带来的误差可以通过反馈来补偿。如果频率更高，或者对精度要求很高，那就需要用完整的刚体动力学模型。

线性化是另一个关键点。非线性MPC求解起来很慢，所以很多实际系统会用线性时变MPC，也就是在每个采样点对非线性模型做线性化。线性化的精度取决于工作点附近的变化范围，如果机器人运动很剧烈，线性化误差就会比较大。

注意：线性化之后一定要做验证。我习惯在仿真里对比线性MPC和非线性MPC的轨迹，如果偏差超过5%，就要考虑缩小线性化的范围或者换用非线性MPC。

3.3 约束设置与求解器选型

约束设置是MPC调参里最费时间的部分。约束太松，机器人可能做出危险动作；约束太紧，优化问题可能无解。常见的约束包括：

• 关节位置约束：不能超过机械限位
• 关节速度约束：不能超过电机最大速度
• 关节力矩约束：不能超过电机最大力矩
• 末端执行器约束：不能超出工作空间
• 避障约束：与障碍物保持安全距离

求解器方面，常用的有OSQP、qpOASES、ACADO等。OSQP适合稀疏的二次规划问题，速度快，我用的比较多。qpOASES适合小规模问题，精度高。ACADO支持非线性MPC，但配置起来比较复杂。

选求解器的时候，除了看速度，还要看它的鲁棒性。有些求解器在问题不可行的时候会直接报错，这时候需要有降级策略，比如放松约束或者切换到备用控制器。

4. WBC全身控制层的实现与调试

4.1 WBC与MPC的分工与协作

WBC，全身控制，是三层架构里最底层也是频率最高的一层。它的任务是接收MPC给的参考轨迹，考虑全身的动力学约束，计算出每个关节的实际控制力矩。

WBC和MPC的分工可以这样理解：MPC负责“规划”，它给出一个理论上可行的轨迹；WBC负责“执行”，它要保证在实际执行过程中，机器人不会因为模型误差、外部扰动而失控。所以WBC通常会用更精确的模型，考虑更多的约束，比如接触力约束、摩擦锥约束。

协作方面，MPC的输出是WBC的输入。但WBC并不是简单地跟踪MPC的轨迹，它有一定的自主权。比如当机器人遇到意外接触时，WBC可以暂时偏离MPC的轨迹，优先保证平衡。这种“下层可以否决上层”的机制，是保证安全性的关键。

4.2 全身动力学建模与任务优先级

WBC的核心是全身动力学建模。对于一个人形机器人，全身动力学方程可以写成：

M(q)q_ddot + C(q, q_dot)q_dot + G(q) = S^T tau + J^T F

其中M是质量矩阵，C是科氏力和离心力项，G是重力项，S是驱动矩阵，tau是关节力矩，J是接触雅可比，F是接触力。

任务优先级是WBC的另一个核心概念。机器人的任务通常不止一个，比如既要保持平衡，又要移动手臂。这些任务之间可能有冲突，所以需要定义优先级。高优先级的任务先满足，低优先级的任务在满足高优先级任务的前提下尽量满足。

常见的优先级设置是：平衡 > 末端执行器位置 > 姿态 > 其他。平衡的优先级最高，因为机器人摔倒了什么都做不了。

4.3 接触力分配与摩擦锥约束

接触力分配是WBC里比较难的部分。对于双足机器人，每只脚与地面的接触力需要满足摩擦锥约束，也就是接触力的切向分量不能超过法向分量乘以摩擦系数。否则脚就会打滑。

摩擦锥约束通常用线性化的形式来表示，比如用多边形近似圆锥。这样可以把非线性约束转化成线性约束，方便求解。但线性化会带来保守性，也就是实际可用的摩擦力范围比理论值小。这个保守性在大多数情况下是可以接受的，因为安全第一。

接触力分配还需要考虑力矩平衡。机器人受到的合力和合力矩必须满足牛顿-欧拉方程，否则机器人会翻倒。这个约束通常作为等式约束加入优化问题。

实操心得：接触力分配对参数很敏感。摩擦系数设得太大，机器人可能打滑；设得太小，机器人可能过于保守，动不起来。我通常会在仿真里做扫参，找到一个既能保证安全又能保证灵活性的值。

5. 三层之间的通信与实时性保障

5.1 通信中间件的选择

三层之间的通信，我试过几种方案：ROS话题、共享内存、自定义的UDP协议。ROS话题最方便，但延迟和抖动比较大，适合做原型验证。共享内存延迟最低，但需要自己处理同步问题。自定义UDP协议介于两者之间，灵活性高，但需要自己实现可靠性机制。

对于实时性要求高的场景，我推荐用共享内存或者实时以太网。共享内存的延迟可以做到微秒级，但要注意避免竞态条件。实时以太网比如EtherCAT，延迟也很低，而且有成熟的工业级实现。

5.2 时间同步与延迟补偿

三层架构里，每一层的计算延迟都不一样。VLM可能几百毫秒，MPC几毫秒，WBC不到一毫秒。如果不做时间同步，下层拿到的可能是上层几百毫秒之前的状态，这会导致控制效果变差。

时间同步的方法有两种：一种是给每个消息打时间戳，下层根据时间戳做延迟补偿；另一种是用硬件同步信号，比如PTP协议。软件时间戳实现简单，但精度受限于系统时钟。硬件同步精度高，但需要额外的硬件支持。

延迟补偿方面，我通常会在MPC里用状态预测来补偿VLM的延迟。比如VLM给出目标的时候，MPC已经根据当前状态预测了未来几百毫秒的轨迹，这样即使VLM的输出有延迟，MPC也能给出合理的参考。

5.3 异常处理与降级策略

三层架构里，任何一层出问题都可能导致整个系统失效。所以异常处理和降级策略是必须的。

VLM层出问题（比如超时或者输出格式错误），系统应该保持当前任务或者回到安全位置。MPC层出问题（比如优化无解），应该切换到备用控制器，比如PD控制器。WBC层出问题（比如求解失败），应该立即切换到阻尼控制或者零力矩控制，让机器人安全停下。

降级策略的设计原则是：每一层都要有独立的失效检测和应对机制，不能依赖上层来发现下层的问题。比如WBC自己检测到求解失败，就应该立即执行安全动作，而不是等MPC来告诉它怎么做。

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

6.1 VLM输出不稳定怎么办

VLM输出不稳定是常见问题，表现为同样的输入，有时候输出正确，有时候输出错误。原因可能是模型本身的随机性，也可能是提示词不够明确。

我的解决办法是：第一，把温度参数调低，减少随机性。第二，在提示词里加入更多的约束和例子。第三，加一个后处理模块，对VLM的输出做校验和修正。比如如果VLM输出的物体不在场景里，就用目标检测的结果来纠正。

如果还是不稳定，可以考虑用多个VLM投票，或者用规则引擎来兜底。规则引擎虽然不智能，但胜在稳定可靠。

6.2 MPC求解超时怎么处理

MPC求解超时通常是因为问题规模太大或者约束太复杂。解决办法有：降低控制频率、简化模型、减少约束、用更快的求解器。

我习惯在MPC里加一个看门狗，如果求解时间超过阈值，就放弃当前解，用上一次的解或者一个简单的PD控制代替。这样虽然控制效果会变差，但至少不会因为超时导致机器人失控。

另外，MPC的热启动也很重要。也就是用上一次的解作为这一次的初始猜测，可以大大加快求解速度。我实测下来，热启动能减少30%到50%的求解时间。

6.3 WBC抖动或发散的排查思路

WBC抖动或发散，通常有以下几个原因：接触力分配不合理、任务优先级冲突、求解器参数不合适、传感器噪声太大。

排查的时候，我会先看接触力。如果接触力在正负之间快速切换，说明摩擦锥约束可能太紧或者求解器在边界附近震荡。这时候可以适当放松约束或者增加求解器的正则化项。

然后看任务优先级。如果两个任务优先级相同但目标冲突，求解器可能会在两个解之间跳来跳去。这时候需要重新审视优先级设置，确保高优先级任务不会和低优先级任务冲突。

最后看传感器噪声。如果IMU或者力传感器的噪声太大，WBC的输入就不准，输出自然也会抖。这时候需要对传感器数据做滤波，但滤波会引入延迟，需要权衡。

6.4 常见问题速查表

提示：这张表是我在实际项目中总结的，但每个项目的情况都不一样。遇到问题的时候，最重要的是先定位是哪一层的问题，然后再深入排查。不要一上来就改参数，那样只会越改越乱。

7. 从零搭建三层架构的实操步骤

7.1 环境准备与依赖安装

搭建三层架构，我建议从仿真环境开始。常用的仿真平台有MuJoCo、PyBullet、Isaac Sim。MuJoCo轻量，适合快速验证算法。Isaac Sim功能强大，但配置复杂，适合做高保真仿真。

依赖方面，Python环境是必须的。MPC可以用CasADi或者ACADO，WBC可以用qpOASES或者OSQP。VLM的话，如果本地部署，需要PyTorch或者TensorFlow，以及对应的推理框架比如ONNX Runtime或者TensorRT。

我的习惯是先在一个简单的模型上把整个流水线跑通，比如一个两自由度的机械臂。确认通信、时间同步、异常处理都没问题之后，再换到复杂的机器人模型上。

7.2 逐层实现与联调

实现顺序建议从下往上：先实现WBC，再实现MPC，最后实现VLM。因为WBC是基础，如果WBC都不稳定，上面的层做得再好也没用。

WBC实现的时候，先用简单的任务，比如保持关节位置。确认稳定之后，再加入平衡任务、末端执行器任务。每加一个任务，都要重新调参和验证。

MPC实现的时候，先用简单的轨迹跟踪，比如让机械臂跟踪一个正弦轨迹。确认跟踪效果之后，再加入避障约束、动力学约束。

VLM实现的时候，先用固定的指令测试，比如“移动到A点”。确认输出格式正确之后，再测试更复杂的指令。

联调的时候，最容易出问题的是时间同步和接口格式。我习惯在每一层的输入输出都加日志，方便定位问题。

7.3 参数调优与性能测试

参数调优是个体力活，但也有一些技巧。我的经验是：先调下层，再调上层。因为下层的性能直接影响上层的效果。如果WBC都抖，MPC调得再好也没用。

WBC的调参重点是任务权重和求解器参数。任务权重决定了各个任务的相对重要性，求解器参数决定了求解的精度和速度。我通常会用网格搜索或者贝叶斯优化来找最优参数。

MPC的调参重点是预测时域和控制时域。预测时域太长，计算量大；太短，预测能力不足。控制时域通常比预测时域短，可以减少计算量。我一般会从预测时域1秒、控制时域0.2秒开始调。

性能测试方面，我会测端到端延迟、控制频率、跟踪误差、成功率。端到端延迟是从VLM输入到WBC输出的时间，控制频率是WBC的实际运行频率，跟踪误差是实际轨迹和参考轨迹的偏差，成功率是任务完成的百分比。

8. 实际项目中的经验与教训

8.1 仿真到真机的迁移问题

仿真到真机的迁移是具身机器人里最头疼的问题之一。仿真里跑得好好的，一到真机就各种问题。原因主要有：模型误差、传感器噪声、执行器延迟、通信延迟。

我的经验是：仿真里要故意加入噪声和延迟。比如在关节角度上加高斯噪声，在控制指令上加随机延迟。这样训练出来的控制器，迁移到真机的时候鲁棒性会好很多。

另外，真机上的安全措施一定要做好。比如关节力矩限幅、紧急停止按钮、安全绳。我见过太多因为安全措施不到位导致硬件损坏的案例。

8.2 计算资源分配与优化

三层架构对计算资源的需求不一样。VLM需要GPU，MPC和WBC需要CPU。如果都在一台机器上跑，资源竞争会很严重。

我的做法是：VLM单独一台机器，通过高速网络和主控通信。MPC和WBC在主控上跑，用实时内核保证确定性。如果主控性能不够，可以把MPC也放到另一台机器上。

优化方面，VLM可以用量化、剪枝、蒸馏来加速。MPC可以用代码生成、并行计算来加速。WBC可以用查表、近似来加速。但要注意，加速的同时不能牺牲太多精度，否则控制效果会变差。

8.3 安全性与可靠性设计

安全性和可靠性是具身机器人的底线。我的设计原则是：任何一层失效，系统都能安全停下。

具体措施包括：WBC里加入关节力矩限幅和速度限幅，MPC里加入工作空间约束和避障约束，VLM里加入任务可行性检查。另外，还要有独立的硬件急停回路，不依赖软件。

可靠性方面，我会做故障注入测试，比如故意让VLM超时、让MPC无解、让WBC求解失败，看系统能不能正确处理。这种测试能发现很多平时发现不了的问题。

我个人在实际操作中的体会是，三层架构的难点不在于每一层单独实现，而在于层与层之间的协作和异常处理。把接口定义清楚，把异常处理做好，整个系统就稳了。最后再分享一个小技巧：在每一层的输出上加一个“置信度”字段，下层可以根据置信度决定是否信任上层的输出。这个简单的机制，能避免很多因为上层输出错误导致的连锁故障。