香港中文大学合作项目：图书馆书架智能整理与双臂移动操作机器人

在半结构化物理环境中，“把一本书放回书架”看似平淡无奇，对自主移动双臂机器人系统而言却涉及空间几何、接触力学与长时序决策的多重强约束：

• 书架上的书籍紧密排列，相邻书本存在倾斜挤压与大面积侧向摩擦力，操作间隙通常处于毫米量级；
• 书脊表面纹理差异大，且覆有塑封膜反光，常导致视觉位姿估计出现角度漂移；
• 移动底盘只要停偏数厘米，机械臂在伸入书架进深操作时就会迅速触碰关节奇异点或机械限位，亦或遮挡手腕相机的观测光路；
• 抽取或插入单本书籍时若缺乏对侧向力的约束，极易引发相邻书本的多米诺式倾覆。

本项目与香港中文大学刘云辉教授团队开展深入合作，研发面向图书馆书籍自主归架与书架整理的移动双臂机器人系统。我负责其中的 AI Agent 决策与物理执行闭环架构，探索如何将高层整理意图转化为可靠、可核验的连续物理动作。

项目背景

我在项目中的工作

作为项目负责人，搭建移动双臂操作框架，负责同轴视觉伺服与智能体闭环执行；推进密集书架整理方案研发，以 1.5–3.0 mm 缝隙下双臂协同抽取成功率超 92%、插入卡阻自愈率超 80% 为预期攻关目标。

物理挑战与移动操作的本质转变

将双臂操作从传统的固定工业工作站迁移到可移动的轮式平台上，系统面临的科学问题与工程约束发生了本质转变。在固定工位中，未知物体的状态通常归结为局部的视线遮挡问题；机械臂基座坐标系与工件台相对刚性固定，逆运动学求解存在确定的几何包络。而在移动操作场景下，问题被重塑为三个核心维度：

1. 环境尺度的部分可观测性（Environment-Scale Partial Observability）：目标书籍可能位于完全不同的架位、被读者翻阅后错放、或者被厚重的大开本书籍遮挡在货架纵深处。机器人不仅需要推断当前视野中被遮挡的局部，更需要进行全局尺度的空间推理，在多层书架与通道间规划主动探索路径，并维持一张持久化的环境拓扑与对象记忆图谱。
2. 底盘停靠作为一等决策变量（Base Placement as a First-Class Decision）：移动底盘的最终停靠位姿直接决定了后续机械臂所有候选抓取轨迹的运动学可达性。底盘离书架过近会导致机械臂无法弯曲展开，肘部甚至可能与窄通道后方的墙面干涉；底盘离得过远则无法探入书架内侧进深。此外，底盘停靠角度直接制约机载相机对目标书脊与抓取缝隙的视线锥。底盘停靠绝不能仅仅委托给传统的全局路径导航模块，而必须作为由后续操作收益驱动的一等决策变量。
3. 导航与操作转换周期的不确定性累积（Uncertainty Accumulation across Cycles）：移动底盘在经过长距离行进与制动后，由于轮面打滑、机械回差以及悬挂系统的微小形变，实际停车位姿往往存在厘米级的平移漂移与数度的航向角偏差。如果直接采用导航前的全局目标坐标驱动机械臂开环下发轨迹，末端执行器必定会发生硬碰撞。系统必须在底盘制动后建立严格的“二次局部感知—状态精修—力控闭环”执行链条。

软硬件分工与确定性控制边界

为确保复杂的 AI 推理能够安全受控地落地到物理硬件上，系统在架构上划定了明确的职责边界：AI Agent 与世界模型仅负责任务级与技能级的高层逻辑推理与参数派发，而底层的轨迹规划、运动学求解、碰撞检测、力矩限制与急停响应则由确定性的机器人控制栈严格承载。

• 高层智能体规划系统：基于 ROS 2 架构部署，运行独立的大模型推理适配器 agent_planner，结合当前的结构化任务状态、技能库定义与对象世界模型，输出符合严格 JSON 模式声明的结构化操作方案；
• 底层确定性控制栈：采用 ROS 2 与 MoveIt 2 作为运动学与规划中枢。臂部运动基于 OMPL 进行无碰撞自由空间寻优，局部切入与抽离则强制采用笛卡尔空间直线插补算法；底盘导航采用 Nav2 维护分层代价地图，结合全向轮移动底盘实现横向与旋转复合平滑制动；
• 计算平台与通信架构：机载端部署 NVIDIA 高算力工控主机，负责感知分割、世界模型维护、局部轨迹生成与技能状态监控；底层执行器通过实时以太网总线与机械臂关节控制器及全向底盘建立高频双向通信，确保高层异常发生时能够在毫秒级触发安全受控刹车。

移动底盘停靠与双臂可操作度联合建模

在狭窄书架通道中（通道净宽通常仅为 0.8 米至 1.0 米），将底盘停靠位姿与机械臂灵巧操作解耦规划会导致严重的运动学死锁。针对这一问题，我们将底盘停靠位姿求解形式化为一个多目标约束优化问题，综合考量机械臂关节可操作度（避开奇异点构型与行程极限）、手腕相机对书脊的视线投影余量（避免机械臂自遮挡光路）以及底盘移动能耗与安全避障裕度。

底盘运动到位后，系统并不立即伸出手臂，而是触发“两阶段局部精修流程”：机载双目与手腕相机对书架标志物及目标书脊进行二次立体视觉重构，解算出微调偏置矩阵，将底盘打滑带来的 1-3 厘米残余误差在前置规划阶段彻底吸收。

仿生双臂协同与接触力控攻关

书架整理最脆弱的物理环节在于书籍抽离与重新插入时的侧向受力。在密集挤压排布下，单臂强行抽出书本会因强大的表面静摩擦力带动整排书籍同向滑动并倾倒；而在插入时，若目标缝隙宽度仅比书本厚度大 1 至 2 毫米，微小的几何偏角都会产生硬性卡滞。为此，我们设计了分工明确的仿生双臂协同作业机制。

1. 辅助臂的侧向支护与防倾倒约束

在主操作臂接触目标书籍前，辅助机械臂首先执行空间展开，将带有柔性防滑垫的末端平贴于目标书籍相邻侧的书堆边缘，施加轻微且受控的侧向预载荷（通常维持在 5 N 至 8 N 的恒定压力）：

• 这一恒定侧向力相当于在书堆一侧临时构建了刚性侧壁，有效抑制了相邻倾斜书籍在摩擦力牵引下的姿态坍塌；
• 辅助臂末端配置六维力矩传感器，实时监测侧向反力变化；若检测到相邻书堆产生滑动突变，系统会动态微调支撑位姿，确保书架整体处于静力学稳定状态。

2. 主操作臂切向顺应抽取

主臂末端装配薄型自适应平行夹爪。视觉算法首先定位目标书脊的上下轮廓顶点与局部法向，主臂沿书脊外法线切向缓缓滑入书缝缝隙：

• 夹爪闭合时执行电流-力矩双重反馈控制，确保夹持力维持在刚性抓紧但又不损伤纸张封面的阈值区间内；
• 抽取阶段沿垂直书架平面的法向平缓后移，主臂在侧向（Y 轴）和俯仰角上开启柔顺阻抗控制，容忍微小的法向对齐偏差，避免因刚性强拔损坏书籍书脊。

3. 微倾角接触微搜索与狭缝滑入

将整理后的书籍插回缝隙是接触动力学要求最高的环节。由于缝隙两侧存在书页毛刺与封面凸起，正对直推极易引发机械自锁。我们采用“倾角切入—阻抗微搜索—滑顺推进”的三步策略：

• 倾角预入：控制书籍底角以约 12 至 15 度的微小俯仰角先接触缝隙下沿，使接触面积从面接触退化为点线接触，显著降低初始切入阻力；
• 阻抗力控与微幅顺应：在末端建立柔顺阻抗控制模型，设定插入主推进方向为低刚度虚拟顺应，在检测到推力突增时主动减速，同时在航向角与侧向施加微小角位移扰动（微幅自适应抖动搜索），引导书籍顺着缝隙天然导轨滑入；
• 全行程入架：当六维力传感器显示推入阻力回归至基线、位移传感器确认书籍完全齐平书架外边缘时，释放夹爪平稳撤回。

面向货架的对象中心世界模型

书架整理通常涉及数十本物理实体的状态演变。传统的稠密三维体素网格（Voxel Grid）在内存占用与跨视角更新效率上难以支撑长周期推理，而纯隐式表征又缺乏离散物体的物理交互句柄。为此，我们研发了面向书架的对象中心世界模型（Object-Centric World Model）。

1. 结构化对象节点与拓扑属性

世界模型将感知到的物理世界维护为一张显式的几何-语义异质图谱。图中的每个节点对应一本独立实体书籍或货架隔板单元，携带完备的物理状态参数：

• 几何与空间属性：三维有向包围盒（3D OBB）、书脊局部坐标系在全局地图下的后验位姿均值与协方差矩阵 $\Sigma_p$；
• 语义与身份特征：书脊光学字符识别（OCR）文本、条形码/RFID 唯一数字标识、外观全局特征嵌入向量；
• 拓扑与接触关系：左邻节点 ID、右邻节点 ID、当前支撑面、垂直倾斜角度 $\phi_{\text{tilt}}$ 以及相邻紧实度指数。

2. 跨视角跨时序图谱数据关联

随着移动底盘在通道中穿行，不同视角采集到的点云与 RGB 观测不断汇入。世界模型通过匈牙利匹配算法，将新检测到的书脊轮廓与历史图谱节点进行联合代价配准。匹配代价综合了空间几何欧氏距离、外观嵌入相似度与拓扑相对邻接顺序。当同一本书被重新观测到时，卡尔曼滤波框架递归更新其 3D 位姿估计与不确定性椭球；若发现原本记录的架位出现几何留白，模型自动标记为“空置插槽（Vacant Slot）”，供后续回放规划调用。

3. 反事实模拟与动作前瞻评估（Counterfactual Simulation）

世界模型的核心优势在于支持 AI Agent 在物理动作发出前进行虚拟前瞻模拟。当 Agent 规划出某个底盘停靠点与抓取顺序时，世界模型在内部推演动作可能产生的状态演变：

• 评估在当前位姿下，机械臂包络体是否会侵入邻近立柱的凸包；
• 评估抽出该书后，根据相邻节点的倾角与重力分布，剩余书堆是否满足无支护条件下的静力平衡条件；
• 如果反事实模拟显示倾覆风险指标超过安全阈值，Agent 自动否决单臂直接抽取方案，转为生成带有“辅助臂前置侧向抵靠”的双臂协同行为树。

AI Agent 闭环任务编排与自愈机制

作为 AI Agent 方向的负责人，我构建了上层语言推理与底层机器人动作执行之间的统一语义转译框架，将复杂的整理任务组织为行为树（BehaviorTree.CPP）与严格的物理技能契约。

1. 结构化任务规划与严格模式校验

当用户输入非结构化的高层指令（如“请将归还车中的三本计算机科学类书籍归架到 B 区第二层书架，并整理歪斜书堆”）时：

• 机载 agent_planner 结合场景图谱的当前符号状态（包含底盘当前所在区域、各货架空槽分布、归还车书籍清单），通过本地语言模型进行任务分层剖析，生成有向无环的任务调度逻辑；
• 输出结果被限定在包含 NavigateToArea, DockAtShelf, ScanSpines, StabilizeCluster, ExtractBook, VerifyHolding, InsertBook, RetractArm 等标准技能库集合内；
• 确定性模式校验器（Schema Validator）介入审查：检查每一个技能调用的参数合法性、目标实体 ID 是否真实存在、坐标系变换是否存在数值奇异。通过校验后，任务被实例化为行为树控制节点。

2. 显式前置与后置物理检验契约

为杜绝开环假设带来的级联失效，每一个原子技能节点都绑定了严密的物理感知断言（Verification Contracts）：

• 以 ExtractBook 为例，其前置条件严格要求辅助臂已建立稳定的侧向力平衡，且主臂夹爪已对齐书脊轴线；
• 动作执行后，系统并不仅凭轨迹执行器返回“Success”即认定完成，而是同时读取机械臂末端安装的微型红外对射光电传感器与夹爪位移编码器：若红外光路被遮断且夹爪开度处于书本厚度合理公差区间，后置条件方才判定通过；若检测到空夹或光电未触发，技能直接返回 FAILURE 状态，阻止流程盲目向下一环节推进。

3. 多层级有界自愈状态机（Bounded Recovery）

在密集狭缝插入与抽取中，由于纸张边缘褶皱或定位残余误差，机械卡阻是不可避免的物理偶发事件。我们设计了有界自愈机制（Bounded Recovery Logic）：

• 局部微回退重试：当插入阻抗力超过预设安全极值（例如推力超过 25 N）且位移未到位时，系统判定发生硬卡滞。行为树自动挂起主干流程，触发局部自愈恢复子树：主臂沿原路回退 20 毫米，末端在偏航角方向反向微调 1.5 度，重新执行微幅顺应插入；
• 有限重试与安全退避：若连续执行三次局部自愈仍无法顺利完全入架，系统终止尝试，执行安全退出动作：平稳抽出该书籍，回转并平放在机载置物托盘上，将当前插槽在世界模型中标记为“几何受阻”，记录详细的错误日志与传感器时间序列，随后继续调度执行下一本书籍的整理，避免单点阻塞瘫痪整个自动化流程。

递进式里程碑规划与预期指标

为了系统验证整套技术方案在真实硬件上的鲁棒性，项目规划了由易到难的四个演进里程碑（M1 至 M4）：

• M1：会议室桌面清理闭环。在开放平整环境中验证自然语言任务生成、底盘可达性停靠、单臂与双臂基础抓放及机载托盘管理，打通全流程软硬件通信链路；
• M2：图书馆书架密集整理与归架。引入狭窄通道导航、高密度书籍挤压提取、毫米级窄缝柔顺插入、辅助臂协同防倾倒与卡阻自愈，验证高难度物理交互的稳定性；
• M3：高价值物品与药剂核验转运。在密集操作基础上叠加封闭容器开启、细径瓶管轻柔夹持、条形码/电子标签精确识读以及全程时空审计溯源，满足严苛的确定性容错需求；
• M4：完全未知布局长程自主泛化。在从未建图与建模的多房间复杂场景下，验证对象中心世界模型的跨区域自主探索、未知实体拓扑推理与长时序持续任务稳定性。

针对 M2 阶段密集书架整理的仿真预演与指标规划，项目设定了明确的预期目标与性能基准：

• 在书本间隙低至 1.5 至 3.0 毫米的密集排列场景下，预期双臂协同书籍抽取成功率超 92%，有效防范相邻书堆连带倾覆；
• 底盘停靠优化预期在 88% 以上的首发停靠中将机械臂置于高灵巧操作包络内，大幅压缩二次伺服微调窗口；
• 针对插入偶发性卡滞，规划的有界自愈机制预期自主恢复超 80% 的摩擦机械自锁状态，最小化人工介入频次。

工程反思与技术积累

这项工作为移动双臂操作提供了一个具体的实践切片。从最初面对密集书架时频繁遭遇的机械卡死与视觉遮挡，到最终构建起软硬件分层的稳健闭环，我们沉淀了若干关键的工程经验：

1. 不可替代的确定性控制底座：当前具身智能领域对端到端大模型寄予厚望，但在毫米级缝隙插拔与刚性接触场景中，端到端策略输出的高频抖动极易引发硬件损伤。将大模型与语义推理约束在任务规划与反事实世界模型层面，让经典的阻抗力控、逆运动学求解与确定性行为树牢牢把控底层执行，是现阶段保障真实物理部署安全的实用工程范式。
2. 移动性与操作性必须一体化考量：移动机械臂不是简单的“底盘加上手臂”。把底盘停靠当成孤立的导航终点是早期频繁出现奇异点与视线遮挡的根源。从末端任务的几何与视觉需求反向推导底盘的最佳停靠流形，将移动性直接转化为操作工作空间的有效延伸，是解决狭窄空间移动操作的核心解法。
3. 真实世界的确定性来自对失败的严密防御：在长时序物理操作中，指望每一个原子动作永远 100% 成功是不切实际的。系统的韧性不在于从不出错，而在于能否在动作执行后通过多模态物理传感如实检验结果，并在发生偏差时具备局部有界自愈与优雅退避的能力。这套架构积累的经验，为密集仓储货架运维、实验室半结构化操作与移动双臂具身机器人的工程化落地提供了长期的技术支撑。