做机器人任务规划和运动控制的人一定都经历过这样的场景仿真环境里一切正常一换到真实场景机器人要么原地发呆要么反复重规划要么干脆撞上前方障碍物。问题往往不是某个单独算法写得不好而是你把感知、任务规划、运动规划全塞进了同一个状态空间里去处理不同时间尺度、不同抽象级别的信息互相干扰最终整个系统被拖垮。H-WM分层世界模型的思路是把机器人的“大脑”拆成语义层、状态层、运动层三个明确的层次任务规划管“做什么”运动规划管“怎么做”中间靠状态模型衔接让每一层都在自己适合的抽象级别和时间尺度上运转。这篇文章就围绕H-WM这个架构把分层背后的原理、工程落地方式、参数调校思路以及我自己实际调试中踩过的坑一次性讲清楚。适合正在做轮式移动机器人、机械臂抓取或者复合机器人项目的朋友参考也适合想打通任务规划与运动规划之间隔阂的人看看。1. 为什么需要分层世界模型从单体感知到“三层大脑”的跃迁1.1 传统世界模型在复杂任务面前的崩溃现场拿一个非常具体的场景说起一台服务机器人要完成“去厨房拿一瓶水再放到客厅桌上”的任务。传统做法通常是把所有信息揉进一个统一的世界模型里——视觉SLAM得到的栅格地图、物体识别输出的品类和位姿、机械臂的关节角度、任务意图的符号状态全部塞进同一个向量空间处理。看起来简单实际上处处是矛盾。首先是状态空间维度爆炸。地图本身就是几百维的数组加上物体属性、机器人状态、动态障碍物信息状态空间动辄上千维。其次是规划尺度不一致任务规划需要在秒级甚至分钟级的尺度上做推理运动规划却要在几十毫秒的周期里输出轨迹混在同一层之后任务层的决策频率会被运动层拖慢反过来运动层又会被任务层的重规划逻辑干扰。最麻烦的是语义和几何的冲突任务层说“拿水”几何层说“水在架子上够不着”中间缺少一个能同时理解“拿水”这个意图和“够不着”这个事实的中间层。最终你会发现每个模块单独跑都问题不大凑到一起就是不动、乱动、反复重规划。这不是算法能力不够而是分层结构设计不合理。H-WM解决的就是这个问题在“任务意图”和“关节动作”之间插入一个结构化的中间层让符号化的高层推理和几何化的底层执行各归各位。1.2 分层到底分的是什么一个日常类比用一个特别朴素的生活类比你从办公室回家。顶层是“任务层”我要回家先出公司走到地铁站坐几站出站走回家。这一层只关心步骤顺序和依赖关系完全不管迈哪条腿、步幅多大。底层是“运动层”迈左腿、抬右腿、保持身体平衡、避开前方水坑。这一层只关心怎么执行不关心为什么走这条路。中间层是“状态层”你现在在哪个位置地铁是否拥挤哪条路临时封了。它把世界的实时状况翻译成上下两层都能理解的格式。H-WM就是把这套思维工程化把人脑天然具有的分层抽象变成机器可计算的结构。核心收益是每一层都能在适合的时间尺度、用适合的表示做决策互不干扰又通过定义良好的接口协作。需要提醒的是分层规划这个概念并不新但“分层世界模型”和传统分层规划有个关键区别它不仅分层规划还分层建模。模型本身是三层的状态更新和预测在每一层以不同的抽象程度进行决策只是模型的一种使用方式。这个点很多人会忽略导致最后实现的其实是一个分层模块的堆叠而不是真正统一的世界模型。2. H-WM的整体架构设计与核心模块拆解2.1 三层世界模型各自负责什么H-WM里最常见的工程定义是三层结构。第一层是语义层也叫符号层它用类似PDDL谓词的方式表示世界的高层状态比如“(robot_at kitchen)”“(object_on table)”“(door_open)”。这一层的数据格式适合任务规划器做逻辑推理它不关心机器人具体在厨房的哪个坐标只关心“是不是在厨房”这个符号事实。第二层是状态层负责维护物体的位姿、速度、可通行区域等几何信息通常是欧拉向量加协方差的形式。这一层承担两个职责把语义层的谓词“翻译”成具体的几何目标比如把“(object_on table)”解析成“水瓶位姿是(x, y, z, roll, pitch, yaw)”把运动层的结果“抽象”成语义事件比如“机器人到达目标点”这个几何事实转换成“(robot_at kitchen)”这个符号谓词。第三层是运动层直接对接机器人控制包含当前关节角、里程计、碰撞检测结果等。这一层面向控制回路使用局部规划器、状态机来完成短时间尺度的执行比如导航里的TEB、机械臂里的RRT*或者MPC。三个层次的典型时间尺度是这样的语义层1到10秒状态层100毫秒到1秒运动层10到50毫秒。层级表示方式更新频率典型算法语义层PDDL谓词 / 符号图秒级HTN、PDDL规划器、LLM任务规划状态层几何状态 概率表示百毫秒级卡尔曼滤波、粒子滤波、成本地图运动层关节空间 / 位姿空间十毫秒级RRT*、TEB、MPC、DWA2.2 任务规划与运动规划如何在分层中被连接任务规划器在语义层工作输入是一组初始谓词和目标谓词输出是一串抽象动作序列例如“移动到厨房 - 打开柜门 - 抓取水瓶 - 移动到客厅 - 放置”。这串动作是符号级的不能直接发给底盘和机械臂因为它们不知道“移动到厨房”在地图上的具体坐标是什么。所以在H-WM里最核心的设计是跨层接口。每个抽象动作都要配一个状态层的“子目标规范”。举例来说“移动到厨房”在状态层被转化为目标点位于地图坐标(x, y)朝向theta允许位置误差0.2米角度误差0.1弧度超时30秒。“抓取水瓶”在状态层则被转化为水瓶当前位姿机械臂末端可达性检查一组可抓取姿态集合。这个转化过程是H-WM实现中工作量最大、最容易出错的地方。我的建议是把子目标规范设计成独立的数据结构而不是在任务层代码里硬编码。这样做的好处是扩展性以后新增一个“搬运箱子”任务不需要改任务规划器只需要在状态层写一个新的子目标解释器。运动层拿到子目标规范后启动对应算法执行完的结果由状态层做事件检测再反馈回语义层更新谓词。闭环跑起来后任务规划器和运动规划器实现了比较彻底的解耦任务层看不到关节角的细节运动层看不到任务语义各自的搜索空间大幅缩小规划速度和鲁棒性同时提升。2.3 为什么推荐HTN加采样规划而不是端到端我这两年踩过最大的坑就是一开始迷信端到端。端到端学习在封闭场景的感知到动作映射上效果不错但室内任务场景里任务层需要长时程的逻辑推理比如“如果客厅桌子被占用了就放到餐桌上”。这种逻辑用纯学习方案很难保证一致性而且场景一换重训成本非常高。HTN层次任务网络的优势在于它天然支持分层。一个“完成一次送水”的方法里可以把子任务定义为“导航到厨房-抓取水瓶-导航到客厅-放置水瓶-任务完成”每一层都可以继续向下分解成原语动作。任务层的搜索空间比平面PDDL搜索小很多因为HTN利用领域知识把大块的搜索跳过了。运动层则采用RRT* TEB这类采样加优化的混合路线。原因非常实际不需要非常精确的环境模型只要成本地图和碰撞检测就能跑实现简单调参透明出了bug好定位。当然H-WM并不绑定具体算法你完全可以把任务层的HTN换成LLM加语义解析运动层换成MPC只要跨层接口保持一致系统的其他部分不动也能转。架构的意义在解耦不在某一种具体实现。3. 实操落地从仿真到真机的完整路径3.1 环境与工具链选型实际做H-WM项目我建议首选轮式移动平台比如差速底盘加2自由度机械臂加RGBD相机。选这个平台是因为导航和抓取两个子问题足够典型又不会像双足机器人那样把运动控制的复杂度顶上来。工具链方面我常用的组合是仿真环境用开源机器人仿真器地图与定位用当前主流的SLAM方案加自适应蒙特卡洛定位成本地图用三层叠加的方式任务规划用HTN库加自定义域文件路径规划用采样规划器库加时间弹性带状态层用行为树或状态机承接事件流。这里有个现实的约束选型时一定要确认各库之间的版本兼容性我见过不少项目在联调阶段才发现通信协议对不上非常崩溃。先把仿真里最简版本跑通再逐步加复杂度这是最稳妥的路径。3.2 分层状态表示与地图体系的关键细节状态层的设计我强烈建议用统一的“世界模型节点”来管理而不是散落在各个模块里各维护一份。每一层的数据结构不同但通过消息传递机制保持同步。语义层维护谓词库状态层维护物体和机器人位姿、可通行区域网格运动层维护里程计、关节角和碰撞检测结果。地图覆盖层的设计也要讲究。我通常把栅格地图拆成三层静态地图层、动态障碍物层、膨胀层。静态层来自SLAM构建动态层实时更新行人和移动障碍物膨胀层根据机器人外形对静态和动态层做处理。导航规划器查询的是三项叠加后的成本地图。膨胀半径的设置需要根据机器人实际外形来定不建议拍脑袋。我常用的公式是膨胀半径 机器人外接半径 安全余量安全余量一般在0.1到0.3米之间取决于场景中人的密度和机器人的最高速度。设置过小会导致贴墙剐蹭设置过大则会让机器人无法通过正常宽度的门。我在一个通道1.2米、机器人宽0.6米的场景里实测安全余量取0.1米机器人能过门但转弯时后轮偶尔蹭到门框取0.15米体感最稳。这类参数必须现场标定。3.3 任务层HTN分解与子目标生成实例以“送水到客厅”为例HTN域可以设计成这样方法bring_object(target_container, target_location)前提机器人位于厨房水杯在柜子上分解子任务navigate_to(kitchen_table)grasp_object(water_bottle)navigate_to(living_room_table)place_object(water_bottle)方法grasp_object(object)前提物体位姿已知机器人可达分解动作移动到抓取位执行抓取状态层检测“物体在手”这些方法描述的是领域知识写好后HTN规划器会以状态层的当前事实作为输入输出一棵可执行的任务树。任务层的输入输出都是符号而符号的真假由底层的状态估计来更新这就是分层交互的重点。子目标规范的数据结构可以写成这样class SubGoal: action_type: str # navigate / grasp / place / open_door target: dict # { x: 3.2, y: 1.8, theta: 0.785 } tolerance: dict # { pos: 0.2, angle: 0.1 } timeout: float # 30.0 preconditions: list # [ object_at(water_bottle, kitchen_table) ] on_success: list # [ set_fact(robot_at, kitchen_table) ] on_failure: list # [ set_fact(navigation_failed, kitchen_table) ]这个结构把“做什么”和“怎么做”之间的翻译变成了一个固定接口。以后每增加一种任务类型只需要写对应的子目标解释器任务规划器和运动规划器几乎不动。我在多个项目里验证过这种设计能显著降低系统的维护成本。3.4 运动层规划参数调校RRT* 与 TEB 的实际调整心得运动规划这一层是最容易出玄学问题的地方我给出两个最常见规划器的调参经验。RRT*用于机械臂和全局路径规划核心参数有三个。步长step size一般在0.05到0.2米之间步长太大会导致窄障碍物区采样点跳过可通行区域太小则搜索太慢我常用的初始值是0.1米。最大采样次数取决于地图复杂度简单房间5000次足够密集货架区我开到2万次以上否则找不到可行路径。到达半径goal tolerance根据任务需求设置移动底盘取0.1米机械臂抓取前取0.02米并且最后会加一段轨迹优化来平滑路径。TEB用于局部导航关键参数是时间最优权重weight_optimaltime和避障权重weight_obstacle。这两个参数直接决定了机器人是贴着障碍物快跑还是绕远路慢跑但更安全。我一般从weight_optimaltime1.0、weight_obstacle100开始再根据实测微调。最大速度和加速度参数必须与底盘真实能力匹配否则规划出的轨迹电机跟不上机器人就会出现抖动、打滑、轨迹异常。参数调校的本质是取舍。用一个代价函数来理解就是J w1 * 完成任务时间 w2 * 障碍物距离惩罚 w3 * 速度平滑惩罚权重不同行为完全不同。希望机器人更稳妥就把w2调大希望它高效就把w1调大。没有任何一组参数能通吃所有场景必须在任务部署场景里做现场标定。我的做法是在仿真里先标定一组大概参数再放到真机上做3组对照测试取行为最符合预期的那组做微调。3.5 闭环与重规划触发策略避免系统震荡的关键我见过不少方案在任务层、运动层都能跑但整体闭环逻辑一团糟。H-WM的闭环应该是这样语义层监控任务谓词是否成立比如“物体是否到达目标位置”状态层定时或事件触发地做感知刷新更新物体位姿和地图变化如果状态层发现当前路径被动态障碍物挡住或者执行结果与预期不符就触发运动层重新规划如果状态层检测到子目标长期无法实现比如超时3次就上报语义层让任务规划器重新做高层决策。这里的问题是重规划触发条件设计不当会导致系统震荡。我最开始把触发条件设得过于灵敏机器人在狭窄走廊里反复重启规划器原地打转根本没法走。后来加了两道防护重规划冷却时间同一原因10秒内最多触发2次以及三级降级策略再规划 - 降速 - 停止并上报。加上这两条之后系统的稳定性提升非常明显。4. 常见问题与排查技巧实录4.1 任务层规划了物理上不可行的动作现象任务层让机器人“穿过走廊到达储物间”但走廊实际宽度小于机器人宽度运动层始终找不到路径超时上报后任务层又给出了另一个同样不可行的子目标陷入死循环。排查思路是这样的。先检查状态层的机器人尺寸和地图的可通行宽度是否一致很多情况是尺寸设置和真实底盘的外轮廓对不上。其次在语义层的每个动作下发前做一次可行性前置校验“navigate_to(目标点)”先查目标点是否落在成本地图的免费区域并且目标点周围的邻域是否足够容纳机器人。最后把“可行性校验失败”作为一个谓词反馈给任务规划器让它把这个失败当成一个可解决的子问题来处理比如换一扇门、换一条路线而不是盲目重试同一个方案。我在实际项目中增加了状态层的“前置条件检查器”在任务规划器输出每个抽象动作后、下发运动层之前统一做校验。校验通过才执行不通过直接返回失败原因。这个改动省掉大量无效运动规划也让任务层的失败分支变得肉眼可见。4.2 状态估计误差跨层放大的问题现象视觉定位对目标物体的位姿估计有5厘米误差状态层把抓取点发给机械臂机械臂按这个点规划轨迹每次抓空。每一层单独看都正常但误差在“语义层只知道物体在桌上”和“状态层只知道位姿大概是(x, y, theta)”之间的传递过程中被放大了。根源是层间翻译精度的不匹配语义层只关心“在不在”状态层关心“在哪”运动层关心“怎么抓”。解决思路有两条。第一状态层不单给一个位姿估计还给协方差矩阵抓取规划时把协方差融合进“可抓取位姿集合”里。第二运动层的RRT*可以直接把末端姿态容差作为目标区域的半径比如抓取目标不是单点而是半径5厘米、角容差0.05弧度的集合。真机部署时还有一个技巧值得分享在执行抓取前增加一个“视觉伺服微调”阶段。机械臂运动到预抓取位之后用相机主动重新识别物体位置再把末端对准后执行抓取。这一步能吃掉90%的位姿误差。我加了这一步之后抓取成功率从不到60%直接拉到90%以上。4.3 重规划触发过于频繁导致系统震荡现象动态场景中机器人每遇到一个新障碍物都要重新规划全局路径频繁切换小车原地打转或者呈Z字形前进。这个问题的根源是把重规划决策交给了直觉判断而不是明确的度量标准。最终我用了三层触发阈值来解决。第一层是路径碰撞成本地图上当前路径被障碍物占据超过阈值比如连续3个栅格被占才触发重规划。第二层是路径偏离机器人相对期望轨迹的横向偏移超过0.3米才触发。第三层是子目标不可达子目标位置在成本地图上被判定为障碍区才触发。加上冷却时间和三级降级策略之后整个系统的行为变得可预测。规划和重规划的频率不应该由拍脑袋决定而是要把每种异常情况都定义成可度量的条件由状态层统一做决策。4.4 抽象粒度到底怎么选分层模型里被问得最多的问题就是任务要不要分更多层每个子任务拆多细才算合理我的经验法则是如果任务层输出的一个动作状态层要花超过5秒去解释说明这个抽象动作太粗。如果任务层输出的动作序列超过20步说明抽象粒度太细应该把连续同类型的动作聚合起来。一个合理的抽象动作应该满足状态层可以为其定义明确的前置条件、执行时间估计和后置条件并且运动层可以找到一个相对独立的规划策略来执行它。分层不是越细越好。有一次我把一个导航任务拆成四个子导航动作结果任务层的规划速度没有提升反而因为接口转换次数变多整体延迟还增加了一些。适度抽象才是分层世界模型的关键。5. 参数速查表与极简代码骨架参考5.1 参数速查表从项目实践中收敛出来的初始值我把前面提到的关键参数整理成了一张速查表适合在项目初期直接抄作业模块参数推荐初始值备注膨胀层膨胀半径0.15米按机器人实际尺寸标定全局规划最大采样次数10000密集货架场景提到2万RRT*步长0.1米窄道场景降低到0.05TEB避障权重100行人密集场景可调高TEB时间最优权重1.0效率优先场景调高重规划冷却冷却时间10秒同一原因10秒内最多2次子目标超时超时时间30秒按任务时长调整视觉伺服预抓取距离0.1到0.2米在预抓取位先视觉对齐再抓这些参数只能作为起点到真机阶段必须根据场地条件实测调整。每次调整只动一个参数改完就测一组实验不然参数之间互相耦合出了问题根本定位不到原因。5.2 极简H-WM代码骨架展示三个层次怎么协作# 语义层任务规划 class TaskPlanner: def build_domain(self): # 用HTN/PDDL描述bring_object等抽象动作 # 返回任务方法定义 pass def plan(self, facts, goal): # 输入状态层的符号事实输出子目标序列 return subgoal_list # 状态层子目标翻译与状态维护 class StateModel: def interpret(self, subgoal): # 把抽象动作翻译成几何子目标 if subgoal.action_type navigate: target_pose self.map.get_pose(subgoal.target) return GeometricGoal(target_pose, tolerance0.2) if subgoal.action_type grasp: obj_pose self.object_db.get_pose(subgoal.object_id) grasp_poses self.inverse_kinematics(obj_pose) return GraspGoal(grasp_poses, covobj_pose.cov) def perceive(self): # 更新事实库、物体位姿、地图障碍信息 pass # 运动层执行几何子目标 class MotionPlanner: def plan(self, geometric_goal): if geometric_goal.type navigation: path rrt_star(global_costmap, robot_pose, geometric_goal.target) return teb_follow(path) if geometric_goal.type grasp: traj rrt_star_arm(arm_state, geometric_goal.grasp_poses) return traj # 主循环闭环执行 state_model StateModel() while not task_done: subgoals task_planner.plan(facts, goal) for sg in subgoals: ok, result execute_subgoal(sg, state_model, motion_planner) if not ok: retry_or_replan(sg, result)这段骨架去掉了大量工程细节只保留三个层次最核心的交互关系。实际项目中每个环节都要考虑失败分支、日志记录、超时处理、状态同步。代码结构理清后H-WM的价值会体现得非常直接新增一个任务类型时你不需要改运动规划器只需要在状态层写一个新的子目标解释器。6. 最后分享一点我的个人体会分层世界模型本质上不是某个惊艳的算法而是一种工程组织方式。我做了几个机器人项目之后再回头看最大的感受是分层带来的收益不是性能提升而是可调试性。出问题时你不需要在一个几千维的状态空间里大海捞针而是先看语义层的谓词对不对再看状态层的估计准不准最后查运动层的路径有没有问题。每一层都可以单独验证再逐层联调问题范围大幅缩小。另一个很实际的建议是先用最简闭环跑通一个导航子任务再逐步扩展成完整的H-WM。我第一次尝试直接搭好三层并接满传感器结果在联调阶段花费了大量时间处理接口问题。如果当初先做一个“导航到桌子”的最小闭环验证好跨层接口的稳定性后面的进展会顺利得多。如果你正在做一个类似的实物机器人项目建议优先把“子目标规范”这个跨层接口数据结构设计好后面你会感谢自己在早期做的这个决定。 
长河君 · 2026/10/17 17:13:27 · 阅读 1.2k 分层世界模型H-WM：打通机器人任务规划与运动规划的关键架构
做机器人任务规划和运动控制的人一定都经历过这样的场景仿真环境里一切正常一换到真实场景机器人要么原地发呆要么反复重规划要么干脆撞上前方障碍物。问题往往不是某个单独算法写得不好而是你把感知、任务规划、运动规划全塞进了同一个状态空间里去处理不同时间尺度、不同抽象级别的信息互相干扰最终整个系统被拖垮。H-WM分层世界模型的思路是把机器人的“大脑”拆成语义层、状态层、运动层三个明确的层次任务规划管“做什么”运动规划管“怎么做”中间靠状态模型衔接让每一层都在自己适合的抽象级别和时间尺度上运转。这篇文章就围绕H-WM这个架构把分层背后的原理、工程落地方式、参数调校思路以及我自己实际调试中踩过的坑一次性讲清楚。适合正在做轮式移动机器人、机械臂抓取或者复合机器人项目的朋友参考也适合想打通任务规划与运动规划之间隔阂的人看看。1. 为什么需要分层世界模型从单体感知到“三层大脑”的跃迁1.1 传统世界模型在复杂任务面前的崩溃现场拿一个非常具体的场景说起一台服务机器人要完成“去厨房拿一瓶水再放到客厅桌上”的任务。传统做法通常是把所有信息揉进一个统一的世界模型里——视觉SLAM得到的栅格地图、物体识别输出的品类和位姿、机械臂的关节角度、任务意图的符号状态全部塞进同一个向量空间处理。看起来简单实际上处处是矛盾。首先是状态空间维度爆炸。地图本身就是几百维的数组加上物体属性、机器人状态、动态障碍物信息状态空间动辄上千维。其次是规划尺度不一致任务规划需要在秒级甚至分钟级的尺度上做推理运动规划却要在几十毫秒的周期里输出轨迹混在同一层之后任务层的决策频率会被运动层拖慢反过来运动层又会被任务层的重规划逻辑干扰。最麻烦的是语义和几何的冲突任务层说“拿水”几何层说“水在架子上够不着”中间缺少一个能同时理解“拿水”这个意图和“够不着”这个事实的中间层。最终你会发现每个模块单独跑都问题不大凑到一起就是不动、乱动、反复重规划。这不是算法能力不够而是分层结构设计不合理。H-WM解决的就是这个问题在“任务意图”和“关节动作”之间插入一个结构化的中间层让符号化的高层推理和几何化的底层执行各归各位。1.2 分层到底分的是什么一个日常类比用一个特别朴素的生活类比你从办公室回家。顶层是“任务层”我要回家先出公司走到地铁站坐几站出站走回家。这一层只关心步骤顺序和依赖关系完全不管迈哪条腿、步幅多大。底层是“运动层”迈左腿、抬右腿、保持身体平衡、避开前方水坑。这一层只关心怎么执行不关心为什么走这条路。中间层是“状态层”你现在在哪个位置地铁是否拥挤哪条路临时封了。它把世界的实时状况翻译成上下两层都能理解的格式。H-WM就是把这套思维工程化把人脑天然具有的分层抽象变成机器可计算的结构。核心收益是每一层都能在适合的时间尺度、用适合的表示做决策互不干扰又通过定义良好的接口协作。需要提醒的是分层规划这个概念并不新但“分层世界模型”和传统分层规划有个关键区别它不仅分层规划还分层建模。模型本身是三层的状态更新和预测在每一层以不同的抽象程度进行决策只是模型的一种使用方式。这个点很多人会忽略导致最后实现的其实是一个分层模块的堆叠而不是真正统一的世界模型。2. H-WM的整体架构设计与核心模块拆解2.1 三层世界模型各自负责什么H-WM里最常见的工程定义是三层结构。第一层是语义层也叫符号层它用类似PDDL谓词的方式表示世界的高层状态比如“(robot_at kitchen)”“(object_on table)”“(door_open)”。这一层的数据格式适合任务规划器做逻辑推理它不关心机器人具体在厨房的哪个坐标只关心“是不是在厨房”这个符号事实。第二层是状态层负责维护物体的位姿、速度、可通行区域等几何信息通常是欧拉向量加协方差的形式。这一层承担两个职责把语义层的谓词“翻译”成具体的几何目标比如把“(object_on table)”解析成“水瓶位姿是(x, y, z, roll, pitch, yaw)”把运动层的结果“抽象”成语义事件比如“机器人到达目标点”这个几何事实转换成“(robot_at kitchen)”这个符号谓词。第三层是运动层直接对接机器人控制包含当前关节角、里程计、碰撞检测结果等。这一层面向控制回路使用局部规划器、状态机来完成短时间尺度的执行比如导航里的TEB、机械臂里的RRT*或者MPC。三个层次的典型时间尺度是这样的语义层1到10秒状态层100毫秒到1秒运动层10到50毫秒。层级表示方式更新频率典型算法语义层PDDL谓词 / 符号图秒级HTN、PDDL规划器、LLM任务规划状态层几何状态 概率表示百毫秒级卡尔曼滤波、粒子滤波、成本地图运动层关节空间 / 位姿空间十毫秒级RRT*、TEB、MPC、DWA2.2 任务规划与运动规划如何在分层中被连接任务规划器在语义层工作输入是一组初始谓词和目标谓词输出是一串抽象动作序列例如“移动到厨房 - 打开柜门 - 抓取水瓶 - 移动到客厅 - 放置”。这串动作是符号级的不能直接发给底盘和机械臂因为它们不知道“移动到厨房”在地图上的具体坐标是什么。所以在H-WM里最核心的设计是跨层接口。每个抽象动作都要配一个状态层的“子目标规范”。举例来说“移动到厨房”在状态层被转化为目标点位于地图坐标(x, y)朝向theta允许位置误差0.2米角度误差0.1弧度超时30秒。“抓取水瓶”在状态层则被转化为水瓶当前位姿机械臂末端可达性检查一组可抓取姿态集合。这个转化过程是H-WM实现中工作量最大、最容易出错的地方。我的建议是把子目标规范设计成独立的数据结构而不是在任务层代码里硬编码。这样做的好处是扩展性以后新增一个“搬运箱子”任务不需要改任务规划器只需要在状态层写一个新的子目标解释器。运动层拿到子目标规范后启动对应算法执行完的结果由状态层做事件检测再反馈回语义层更新谓词。闭环跑起来后任务规划器和运动规划器实现了比较彻底的解耦任务层看不到关节角的细节运动层看不到任务语义各自的搜索空间大幅缩小规划速度和鲁棒性同时提升。2.3 为什么推荐HTN加采样规划而不是端到端我这两年踩过最大的坑就是一开始迷信端到端。端到端学习在封闭场景的感知到动作映射上效果不错但室内任务场景里任务层需要长时程的逻辑推理比如“如果客厅桌子被占用了就放到餐桌上”。这种逻辑用纯学习方案很难保证一致性而且场景一换重训成本非常高。HTN层次任务网络的优势在于它天然支持分层。一个“完成一次送水”的方法里可以把子任务定义为“导航到厨房-抓取水瓶-导航到客厅-放置水瓶-任务完成”每一层都可以继续向下分解成原语动作。任务层的搜索空间比平面PDDL搜索小很多因为HTN利用领域知识把大块的搜索跳过了。运动层则采用RRT* TEB这类采样加优化的混合路线。原因非常实际不需要非常精确的环境模型只要成本地图和碰撞检测就能跑实现简单调参透明出了bug好定位。当然H-WM并不绑定具体算法你完全可以把任务层的HTN换成LLM加语义解析运动层换成MPC只要跨层接口保持一致系统的其他部分不动也能转。架构的意义在解耦不在某一种具体实现。3. 实操落地从仿真到真机的完整路径3.1 环境与工具链选型实际做H-WM项目我建议首选轮式移动平台比如差速底盘加2自由度机械臂加RGBD相机。选这个平台是因为导航和抓取两个子问题足够典型又不会像双足机器人那样把运动控制的复杂度顶上来。工具链方面我常用的组合是仿真环境用开源机器人仿真器地图与定位用当前主流的SLAM方案加自适应蒙特卡洛定位成本地图用三层叠加的方式任务规划用HTN库加自定义域文件路径规划用采样规划器库加时间弹性带状态层用行为树或状态机承接事件流。这里有个现实的约束选型时一定要确认各库之间的版本兼容性我见过不少项目在联调阶段才发现通信协议对不上非常崩溃。先把仿真里最简版本跑通再逐步加复杂度这是最稳妥的路径。3.2 分层状态表示与地图体系的关键细节状态层的设计我强烈建议用统一的“世界模型节点”来管理而不是散落在各个模块里各维护一份。每一层的数据结构不同但通过消息传递机制保持同步。语义层维护谓词库状态层维护物体和机器人位姿、可通行区域网格运动层维护里程计、关节角和碰撞检测结果。地图覆盖层的设计也要讲究。我通常把栅格地图拆成三层静态地图层、动态障碍物层、膨胀层。静态层来自SLAM构建动态层实时更新行人和移动障碍物膨胀层根据机器人外形对静态和动态层做处理。导航规划器查询的是三项叠加后的成本地图。膨胀半径的设置需要根据机器人实际外形来定不建议拍脑袋。我常用的公式是膨胀半径 机器人外接半径 安全余量安全余量一般在0.1到0.3米之间取决于场景中人的密度和机器人的最高速度。设置过小会导致贴墙剐蹭设置过大则会让机器人无法通过正常宽度的门。我在一个通道1.2米、机器人宽0.6米的场景里实测安全余量取0.1米机器人能过门但转弯时后轮偶尔蹭到门框取0.15米体感最稳。这类参数必须现场标定。3.3 任务层HTN分解与子目标生成实例以“送水到客厅”为例HTN域可以设计成这样方法bring_object(target_container, target_location)前提机器人位于厨房水杯在柜子上分解子任务navigate_to(kitchen_table)grasp_object(water_bottle)navigate_to(living_room_table)place_object(water_bottle)方法grasp_object(object)前提物体位姿已知机器人可达分解动作移动到抓取位执行抓取状态层检测“物体在手”这些方法描述的是领域知识写好后HTN规划器会以状态层的当前事实作为输入输出一棵可执行的任务树。任务层的输入输出都是符号而符号的真假由底层的状态估计来更新这就是分层交互的重点。子目标规范的数据结构可以写成这样class SubGoal: action_type: str # navigate / grasp / place / open_door target: dict # { x: 3.2, y: 1.8, theta: 0.785 } tolerance: dict # { pos: 0.2, angle: 0.1 } timeout: float # 30.0 preconditions: list # [ object_at(water_bottle, kitchen_table) ] on_success: list # [ set_fact(robot_at, kitchen_table) ] on_failure: list # [ set_fact(navigation_failed, kitchen_table) ]这个结构把“做什么”和“怎么做”之间的翻译变成了一个固定接口。以后每增加一种任务类型只需要写对应的子目标解释器任务规划器和运动规划器几乎不动。我在多个项目里验证过这种设计能显著降低系统的维护成本。3.4 运动层规划参数调校RRT* 与 TEB 的实际调整心得运动规划这一层是最容易出玄学问题的地方我给出两个最常见规划器的调参经验。RRT*用于机械臂和全局路径规划核心参数有三个。步长step size一般在0.05到0.2米之间步长太大会导致窄障碍物区采样点跳过可通行区域太小则搜索太慢我常用的初始值是0.1米。最大采样次数取决于地图复杂度简单房间5000次足够密集货架区我开到2万次以上否则找不到可行路径。到达半径goal tolerance根据任务需求设置移动底盘取0.1米机械臂抓取前取0.02米并且最后会加一段轨迹优化来平滑路径。TEB用于局部导航关键参数是时间最优权重weight_optimaltime和避障权重weight_obstacle。这两个参数直接决定了机器人是贴着障碍物快跑还是绕远路慢跑但更安全。我一般从weight_optimaltime1.0、weight_obstacle100开始再根据实测微调。最大速度和加速度参数必须与底盘真实能力匹配否则规划出的轨迹电机跟不上机器人就会出现抖动、打滑、轨迹异常。参数调校的本质是取舍。用一个代价函数来理解就是J w1 * 完成任务时间 w2 * 障碍物距离惩罚 w3 * 速度平滑惩罚权重不同行为完全不同。希望机器人更稳妥就把w2调大希望它高效就把w1调大。没有任何一组参数能通吃所有场景必须在任务部署场景里做现场标定。我的做法是在仿真里先标定一组大概参数再放到真机上做3组对照测试取行为最符合预期的那组做微调。3.5 闭环与重规划触发策略避免系统震荡的关键我见过不少方案在任务层、运动层都能跑但整体闭环逻辑一团糟。H-WM的闭环应该是这样语义层监控任务谓词是否成立比如“物体是否到达目标位置”状态层定时或事件触发地做感知刷新更新物体位姿和地图变化如果状态层发现当前路径被动态障碍物挡住或者执行结果与预期不符就触发运动层重新规划如果状态层检测到子目标长期无法实现比如超时3次就上报语义层让任务规划器重新做高层决策。这里的问题是重规划触发条件设计不当会导致系统震荡。我最开始把触发条件设得过于灵敏机器人在狭窄走廊里反复重启规划器原地打转根本没法走。后来加了两道防护重规划冷却时间同一原因10秒内最多触发2次以及三级降级策略再规划 - 降速 - 停止并上报。加上这两条之后系统的稳定性提升非常明显。4. 常见问题与排查技巧实录4.1 任务层规划了物理上不可行的动作现象任务层让机器人“穿过走廊到达储物间”但走廊实际宽度小于机器人宽度运动层始终找不到路径超时上报后任务层又给出了另一个同样不可行的子目标陷入死循环。排查思路是这样的。先检查状态层的机器人尺寸和地图的可通行宽度是否一致很多情况是尺寸设置和真实底盘的外轮廓对不上。其次在语义层的每个动作下发前做一次可行性前置校验“navigate_to(目标点)”先查目标点是否落在成本地图的免费区域并且目标点周围的邻域是否足够容纳机器人。最后把“可行性校验失败”作为一个谓词反馈给任务规划器让它把这个失败当成一个可解决的子问题来处理比如换一扇门、换一条路线而不是盲目重试同一个方案。我在实际项目中增加了状态层的“前置条件检查器”在任务规划器输出每个抽象动作后、下发运动层之前统一做校验。校验通过才执行不通过直接返回失败原因。这个改动省掉大量无效运动规划也让任务层的失败分支变得肉眼可见。4.2 状态估计误差跨层放大的问题现象视觉定位对目标物体的位姿估计有5厘米误差状态层把抓取点发给机械臂机械臂按这个点规划轨迹每次抓空。每一层单独看都正常但误差在“语义层只知道物体在桌上”和“状态层只知道位姿大概是(x, y, theta)”之间的传递过程中被放大了。根源是层间翻译精度的不匹配语义层只关心“在不在”状态层关心“在哪”运动层关心“怎么抓”。解决思路有两条。第一状态层不单给一个位姿估计还给协方差矩阵抓取规划时把协方差融合进“可抓取位姿集合”里。第二运动层的RRT*可以直接把末端姿态容差作为目标区域的半径比如抓取目标不是单点而是半径5厘米、角容差0.05弧度的集合。真机部署时还有一个技巧值得分享在执行抓取前增加一个“视觉伺服微调”阶段。机械臂运动到预抓取位之后用相机主动重新识别物体位置再把末端对准后执行抓取。这一步能吃掉90%的位姿误差。我加了这一步之后抓取成功率从不到60%直接拉到90%以上。4.3 重规划触发过于频繁导致系统震荡现象动态场景中机器人每遇到一个新障碍物都要重新规划全局路径频繁切换小车原地打转或者呈Z字形前进。这个问题的根源是把重规划决策交给了直觉判断而不是明确的度量标准。最终我用了三层触发阈值来解决。第一层是路径碰撞成本地图上当前路径被障碍物占据超过阈值比如连续3个栅格被占才触发重规划。第二层是路径偏离机器人相对期望轨迹的横向偏移超过0.3米才触发。第三层是子目标不可达子目标位置在成本地图上被判定为障碍区才触发。加上冷却时间和三级降级策略之后整个系统的行为变得可预测。规划和重规划的频率不应该由拍脑袋决定而是要把每种异常情况都定义成可度量的条件由状态层统一做决策。4.4 抽象粒度到底怎么选分层模型里被问得最多的问题就是任务要不要分更多层每个子任务拆多细才算合理我的经验法则是如果任务层输出的一个动作状态层要花超过5秒去解释说明这个抽象动作太粗。如果任务层输出的动作序列超过20步说明抽象粒度太细应该把连续同类型的动作聚合起来。一个合理的抽象动作应该满足状态层可以为其定义明确的前置条件、执行时间估计和后置条件并且运动层可以找到一个相对独立的规划策略来执行它。分层不是越细越好。有一次我把一个导航任务拆成四个子导航动作结果任务层的规划速度没有提升反而因为接口转换次数变多整体延迟还增加了一些。适度抽象才是分层世界模型的关键。5. 参数速查表与极简代码骨架参考5.1 参数速查表从项目实践中收敛出来的初始值我把前面提到的关键参数整理成了一张速查表适合在项目初期直接抄作业模块参数推荐初始值备注膨胀层膨胀半径0.15米按机器人实际尺寸标定全局规划最大采样次数10000密集货架场景提到2万RRT*步长0.1米窄道场景降低到0.05TEB避障权重100行人密集场景可调高TEB时间最优权重1.0效率优先场景调高重规划冷却冷却时间10秒同一原因10秒内最多2次子目标超时超时时间30秒按任务时长调整视觉伺服预抓取距离0.1到0.2米在预抓取位先视觉对齐再抓这些参数只能作为起点到真机阶段必须根据场地条件实测调整。每次调整只动一个参数改完就测一组实验不然参数之间互相耦合出了问题根本定位不到原因。5.2 极简H-WM代码骨架展示三个层次怎么协作# 语义层任务规划 class TaskPlanner: def build_domain(self): # 用HTN/PDDL描述bring_object等抽象动作 # 返回任务方法定义 pass def plan(self, facts, goal): # 输入状态层的符号事实输出子目标序列 return subgoal_list # 状态层子目标翻译与状态维护 class StateModel: def interpret(self, subgoal): # 把抽象动作翻译成几何子目标 if subgoal.action_type navigate: target_pose self.map.get_pose(subgoal.target) return GeometricGoal(target_pose, tolerance0.2) if subgoal.action_type grasp: obj_pose self.object_db.get_pose(subgoal.object_id) grasp_poses self.inverse_kinematics(obj_pose) return GraspGoal(grasp_poses, covobj_pose.cov) def perceive(self): # 更新事实库、物体位姿、地图障碍信息 pass # 运动层执行几何子目标 class MotionPlanner: def plan(self, geometric_goal): if geometric_goal.type navigation: path rrt_star(global_costmap, robot_pose, geometric_goal.target) return teb_follow(path) if geometric_goal.type grasp: traj rrt_star_arm(arm_state, geometric_goal.grasp_poses) return traj # 主循环闭环执行 state_model StateModel() while not task_done: subgoals task_planner.plan(facts, goal) for sg in subgoals: ok, result execute_subgoal(sg, state_model, motion_planner) if not ok: retry_or_replan(sg, result)这段骨架去掉了大量工程细节只保留三个层次最核心的交互关系。实际项目中每个环节都要考虑失败分支、日志记录、超时处理、状态同步。代码结构理清后H-WM的价值会体现得非常直接新增一个任务类型时你不需要改运动规划器只需要在状态层写一个新的子目标解释器。6. 最后分享一点我的个人体会分层世界模型本质上不是某个惊艳的算法而是一种工程组织方式。我做了几个机器人项目之后再回头看最大的感受是分层带来的收益不是性能提升而是可调试性。出问题时你不需要在一个几千维的状态空间里大海捞针而是先看语义层的谓词对不对再看状态层的估计准不准最后查运动层的路径有没有问题。每一层都可以单独验证再逐层联调问题范围大幅缩小。另一个很实际的建议是先用最简闭环跑通一个导航子任务再逐步扩展成完整的H-WM。我第一次尝试直接搭好三层并接满传感器结果在联调阶段花费了大量时间处理接口问题。如果当初先做一个“导航到桌子”的最小闭环验证好跨层接口的稳定性后面的进展会顺利得多。如果你正在做一个类似的实物机器人项目建议优先把“子目标规范”这个跨层接口数据结构设计好后面你会感谢自己在早期做的这个决定。