Hydra-Multi

Chang 2023 · 论文

一句话总结 — 首个多机器人空间感知系统:每个机器人在线运行Hydra以构建局部三维场景图,中央站将它们进行对齐、优化并整合为一个全局一致的分层地图。

问题

三维场景图已被证明是一种富有表现力的高层地图表示,但Hydra只能从单个机器人的视角构建场景图——而大型环境对单个机器人来说难以快速覆盖。与此同时,多机器人SLAM系统能生成度量地图,却没有分层语义。要在线构建多机器人场景图,需要解决三个相互耦合的问题:估计机器人坐标系之间的相对变换(且没有初始标定)、在严重的感知歧义下检测机器人间回环,以及协调由不同机器人贡献的重复场景图节点。

方法与架构

前端(控制站)。 每个机器人运行一个本地Hydra实例,并周期性地传输其完整场景图。场景图处理器将这些图累积为一个单一的未优化、未协调的前端图,并对每个节点的参考坐标系进行细致的记录。机器人间回环通过Hydra的分层检测器发现——先自上而下比较描述子(地点→物体→视觉外观),再自下而上进行几何验证,使用RANSAC(视觉关键点)或TEASER++(物体)。

后端:对齐——优化——协调。

初始对齐。 机器人 $A$ 与 $B$ 之间的每个机器人间回环 $(\alpha_{i},\beta_{j})$ 给出 $B$ 坐标系在 $A$ 坐标系下的一个带噪声的估计:

$\widehat{\mathbf{X}}^{A}_{B,ij}=\widehat{\mathbf{X}}^{A}_{\alpha_{i}}\,\widetilde{\mathbf{X}}^{\alpha_{i}}_{\beta_{j}}\big(\widehat{\mathbf{X}}^{B}_{\beta_{j}}\big)^{-1}$

其中 $\widehat{\mathbf{X}}$ 为里程计位姿估计, $\widetilde{\mathbf{X}}^{\alpha_{i}}_{\beta_{j}}$ 为回环检测量测值。这些估计通过带截断最小二乘代价 $\rho$ 的鲁棒位姿平均进行融合:

$\widehat{\mathbf{X}}^{A}_{B}\in\arg\min_{\mathbf{X}\in\mathrm{SE}(3)}\;\sum_{(i,j)\in L_{A,B}}\rho\left(\big\|\mathbf{X}\boxminus\widehat{\mathbf{X}}^{A}_{B,ij}\big\|_{\Sigma}\right)$

在GTSAM中用渐进非凸性(GNC)求解;一旦有 $k=5$ 个内点存活,该机器人即被视为已初始化,变换沿机器人级依赖图的生成树逐步传递。 2. 协调候选提议。 对齐完成后,提出合并候选:重叠的地点对(距离 $\leq 0.01$ 米、半径相近)以及语义标签相同且包围盒重叠的物体对(变换由其网格顶点的ICP得出)。 3. 鲁棒场景图优化。 各机器人位姿图、地点、合并候选物体以及降采样的网格控制点构成一个嵌入式变形图,作为位姿图优化问题求解

$\arg\min_{\mathbf{T}_{1},\ldots,\mathbf{T}_{n}\in\mathrm{SE}(3)}\;\sum_{\mathbf{E}_{ij}}\big\|\mathbf{T}_{i}^{-1}\mathbf{T}_{j}\boxminus\mathbf{E}_{ij}\big\|^{2}_{\mathbf{\Omega}_{ij}}$

其中边 $\mathbf{E}_{ij}$ 包括里程计、回环、网格刚性约束和协调因子;GNC将虚假回环和错误合并识别为外点并加以剔除。 4. 节点协调。 执行GNC判定为内点的合并操作,重新插值网格,重新计算物体质心/包围盒;若提议合并中有效的少于一半,则全部撤销,并从合并后的地点重新分割房间。

异构团队。 只要地图与至少一层兼容,任何机器人都可以贡献信息:基于物体的SLAM机器人贡献物体层;搭载激光雷达、无语义信息的机器人仍可贡献网格和地点——正是这种分层结构使异构地图能够融合。

实验结果

数据集:模拟的uHumans2办公室场景(3个机器人)、真实的SidPac场景(SP,两段约400米的多楼层记录,作为两个机器人,分别从不同楼层出发)以及Simmons场景(SM1/SM2,两台配备RealSense D455和激光雷达的Clearpath Jackal,约500米轨迹,其中感知歧义导致80–90%的回环为外点)。
ATE(米):Hydra-Multi在uH2/SP/SM1/SM2上分别为0.25、3.92、0.99、0.79,而集中式Kimera-Multi为0.59/4.99/2.0/0.87;尽管使用视觉,仍接近基于激光雷达的LAMP 2.0(SM1/SM2上为0.73/0.58)。
场景图精度与经真值坐标系对齐的单机器人Hydra相当,在uH2/SP上甚至因机器人间回环而略有超越。
消融实验(物体”发现百分比”):完整系统在uH2/SM1上分别为92.9%和59.3%,而无初始对齐时为80.8%/23.8%,无协调时为91.1%/37.3%。
两台机器人在约30分钟内重建出完整的一层宿舍楼,而单台机器人覆盖同一区域需约50分钟;前端每次迭代保持在约100毫秒左右,每个机器人的回环和变形图通信流量保持在1MB以下(带宽主要由原始网格/场景图流式传输占用)。

对SLAM的意义

Hydra-Multi是首个在机器人团队间协作构建三维场景图的系统,将Kimera/Hydra这条脉络从单机器人扩展到了舰队级语义建图。其对齐-优化-协调后端表明,用于修正回环的同一套鲁棒机制(基于变形图的GNC)同样可以用来仲裁机器人间的节点合并,而其异构团队实验结果说明了一个架构层面的观点:场景图层级结构本身就是让纯激光雷达机器人和基于视觉的机器人共享同一世界模型的互操作层——这与搜救、仓储以及任何多机器人部署场景都密切相关。

Hydra-Multi

问题

方法与架构

实验结果

对SLAM的意义

相关条目