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 常见问题速查表问题现象可能原因排查方法解决方案VLM输出格式错误提示词不明确检查提示词模板加入格式示例和约束MPC求解超时问题规模太大记录求解时间降低频率或简化模型WBC抖动接触力震荡观察接触力曲线放松摩擦锥约束机器人动作迟缓通信延迟大测量端到端延迟改用共享内存任务执行失败场景理解错误对比VLM输出和实际场景加入校验模块机器人摔倒平衡任务优先级低检查优先级设置提高平衡优先级提示这张表是我在实际项目中总结的但每个项目的情况都不一样。遇到问题的时候最重要的是先定位是哪一层的问题然后再深入排查。不要一上来就改参数那样只会越改越乱。7. 从零搭建三层架构的实操步骤7.1 环境准备与依赖安装搭建三层架构我建议从仿真环境开始。常用的仿真平台有MuJoCo、PyBullet、Isaac Sim。MuJoCo轻量适合快速验证算法。Isaac Sim功能强大但配置复杂适合做高保真仿真。依赖方面Python环境是必须的。MPC可以用CasADi或者ACADOWBC可以用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需要GPUMPC和WBC需要CPU。如果都在一台机器上跑资源竞争会很严重。我的做法是VLM单独一台机器通过高速网络和主控通信。MPC和WBC在主控上跑用实时内核保证确定性。如果主控性能不够可以把MPC也放到另一台机器上。优化方面VLM可以用量化、剪枝、蒸馏来加速。MPC可以用代码生成、并行计算来加速。WBC可以用查表、近似来加速。但要注意加速的同时不能牺牲太多精度否则控制效果会变差。8.3 安全性与可靠性设计安全性和可靠性是具身机器人的底线。我的设计原则是任何一层失效系统都能安全停下。具体措施包括WBC里加入关节力矩限幅和速度限幅MPC里加入工作空间约束和避障约束VLM里加入任务可行性检查。另外还要有独立的硬件急停回路不依赖软件。可靠性方面我会做故障注入测试比如故意让VLM超时、让MPC无解、让WBC求解失败看系统能不能正确处理。这种测试能发现很多平时发现不了的问题。我个人在实际操作中的体会是三层架构的难点不在于每一层单独实现而在于层与层之间的协作和异常处理。把接口定义清楚把异常处理做好整个系统就稳了。最后再分享一个小技巧在每一层的输出上加一个“置信度”字段下层可以根据置信度决定是否信任上层的输出。这个简单的机制能避免很多因为上层输出错误导致的连锁故障。