地平线最新开源：腿足里程计融合GeoFlow-SLAM系统

文章：GeoFlow-SLAM: A Robust Tightly-Coupled RGBD-Inertial and Legged Odometry Fusion SLAM for Dynamic Legged Robotics

作者：Tingyang Xiao , Xiaolin Zhou , Liu Liu , Wei Sui , Wei Feng , Jiaxiong Qiu , Xinjie Wang , and Zhizhong Su

编辑：点云PCL

欢迎各位加入知识星球，获取PDF论文，欢迎转发朋友圈。文章仅做学术分享，如有侵权联系删文。

从技术角度观察行业发展，努力跟上时代的步伐。公众号致力于点云处理，SLAM，三维视觉，具身智能，自动驾驶等领域相关内容的干货分享，欢迎各位加入，有兴趣的可联系dianyunpcl@163.com。文章未申请原创，未经过本人允许请勿转载，有意转载联系微信920177957。

摘要

本文提出GeoFlow-SLAM—一种面向面临剧烈高频运动的腿式机器人、鲁棒且高效的紧耦合RGBD-惯性-腿足里程计融合SLAM系统。通过融合几何一致性、腿足里程计约束与双流光流（GeoFlow），本方法解决了三大关键挑战：快速运动中的特征匹配与位姿初始化失效，以及无纹理场景中的视觉特征缺失问题。具体而言：在快速运动场景中，通过融合先验地图点和位姿的双流光流显著提升了特征匹配能力；针对腿式机器人的快速运动与IMU差，提出结合IMU/腿足里程计、帧间PnP（Perspective-n-Point）和GICP（广义迭代最近点）的鲁棒位姿初始化方法；此外首次引入深度-地图与GICP几何约束紧耦合的新型优化框架，显著提升了长期无纹理环境下的鲁棒性与精度。所提算法在自采集腿式机器人数据集及开源数据集上均达到业界最优（SOTA）水平。为促进技术发展，数据集与代码将开源：https://github.com/HorizonRobotics/GeoFlowSlam。

主要贡献

RGBD相机能够同步采集视野内所有区域的RGB信息与深度信息，无论该区域是否存在纹理特征。这一特性使其成为机器人领域的常用传感器，尤其在视觉同步定位与建图（SLAM）等应用中。对于腿式机器人，其运动过程中因步态周期引发的速度与姿态变化常导致快速运动抖动，进而造成图像模糊与IMU畸变。此外这类机器人通常在无纹理室内环境运行，这些因素为定位系统带来全新挑战。然而现有基于RGB/RGBD的SLAM方案难以有效应对：

• 快速运动抖动引发的图像模糊会阻碍特征匹配
• 当未使用IMU或腿式机器人特殊运动特性导致IMU畸变时，位姿初始化在高速运动中易失效
• 长期无纹理环境中，深度信息仅用于特征点深度估计，缺乏提升系统精度与鲁棒性的补充约束

针对上述问题，本文提出GeoFlow-SLAM系统，核心创新如下：

1. 提出融合地图点与位姿信息的双流光流跟踪机制，显著增强快速运动场景下的特征匹配能力

2. 开发基于IMU/腿足里程计、PnP（透视n点）与GICP（广义迭代最近点）的位姿初始化方法，解决高速运动位姿初始化失效问题

3. 首次建立深度转地图与GICP几何一致性紧耦合优化框架，提升无纹理环境下的长期鲁棒性与精度

4. 在开源及自采腿式机器人数据集上实现业界最优（SOTA）性能，相关数据集与代码将开源以推动技术发展。

主要内容

A.概述

所提出的 GeoFlow-SLAM 系统由三个部分组成：特征提取与跟踪、IMU/腿式/PnP/GICP 里程计，以及视觉-深度-IMU 融合建图，如图 1 所示。

* 特征提取与跟踪

首先从深度点云中提取深度平面特征，并将其用于 GICP 算法以获得初始位姿估计。随后，从图像数据中提取定向 FAST 和旋转 BRIEF（ORB）特征。接着利用双流光流跟踪和透视 n 点（PnP）算法进行特征跟踪。IMU 原始测量数据和腿式编码器数据经过预积分处理，为 IMU/腿式里程计提供基础。

* IMU/腿式/PnP/GICP 里程计

当腿式和 IMU 里程计均可用时，它们作为主要手段用于位姿预测。在缺少这两者的情况下，通过求解连续帧间的 PnP 问题来确定机器人的初始位姿。来自 IMU/腿式里程计或 PnP 的初始位姿进一步通过 GICP 算法进行优化。在完成位姿初始化与特征跟踪后，通过深度到地图的配准以及视觉地图点的重投影来生成观测残差。

* 视觉-深度-IMU 融合建图

该模块包含单帧优化、局部关键帧优化和全局关键帧优化。在单帧位姿优化和局部关键帧捆绑调整（BA）模块中，视觉重投影残差、深度到地图的残差以及 IMU/腿式/GICP 位姿约束在因子图框架下紧密耦合。与单帧位姿优化仅关注当前相邻帧的位姿估计不同，局部 BA 会估计局部关键帧及其对应视觉地图点的位姿。当检测到回环时，在全局优化模块中对所有关键帧位姿和视觉点云地图进行进一步优化。

图 1. GeoFlow-SLAM 流程概览。首先利用来自 RGBD、IMU 和腿式编码器的数据，提取深度平面特征和 ORB 特征，并进行 IMU/腿式预积分。初始位姿通过 GICP、PnP 以及 IMU/腿式里程计确定。在该初始位姿的基础上，进一步利用双流光流跟踪 ORB 特征。随后在紧耦合的视觉-深度-IMU 因子图中对单帧和局部关键帧进行优化，从而实现高精度的地图构建与位姿估计。

借助先验地图点与位姿的双流光流跟踪

传统光流通常假设运动较小且连续。然而当发生快速运动时，基于亮度一致性假设的光流估计可能会变得不准确，甚至无法收敛。此外直接在高分辨率图像上计算光流会导致算法收敛速度慢并消耗大量计算资源。为了解决这些问题，我们提出了一种双流光流跟踪方法（如图 2 所示），利用先验地图点（与 ORB 特征对应的地图点）和位姿信息，在快速运动时增强特征跟踪的鲁棒性。这种方法有效缓解了由于相机快速运动导致像素大位移而引发的特征跟踪失败的常见问题。

图 2. 基于先验地图点与位姿的双流光流处理流程。首先将与先验地图点对应的特征点进行重投影，生成重投影点（蓝色点所示）。随后在当前帧上对这些重投影点执行光流跟踪。接着对前一帧中缺少合格先验地图点的特征点（黄色点和粉色点所示）进一步进行光流跟踪。

1) 3D-2D 光流跟踪

在第一条流中，使用来自 IMU/腿式里程计或 PnP、GICP（里程计模块中）的初始位姿，将特征点 p(x,y)对应的地图点 P(X,Y,Z)投影到当前帧 p′(x,y)上，其中 F(P)表示内参投影。然后在投影后的像素 p′(x,y)上执行基于当前帧的 3D-2D 光流跟踪。Tck−1表示时间 k−1时刻的相机位姿。通过利用精确的运动先验与地图点信息，可以在快速运动中实现更高的匹配精度。

此外该方法还会自动过滤掉动态不一致等不稳定特征。

2) 2D-2D 光流跟踪

尽管第一阶段有明显优势，但 ORB 特征往往缺少有效的对应地图点，特别是在三角化失败的情况下。此外不准确的初始位姿也会显著降低 3D-2D 特征跟踪的成功率。为此引入了第二条流，在原始特征点上执行 2D-2D 光流跟踪，即使在缺乏深度信息的情况下也能保证连续跟踪。为了进一步提高鲁棒性，计算基础矩阵来识别并剔除外点，并采用内点掩膜机制，防止在同一区域中不必要地增加跟踪点。这种组合方法有效减弱了外点的影响，保证了跟踪过程的可靠性。如图 3 所示，当 ORB-SLAM3 无法有效跟踪特征点时，我们的双流光流跟踪算法依然能够成功跟踪地面、门闩等特征，即使在快速运动导致的运动模糊或大面积白墙等无纹理环境等恶劣条件下依然表现出色。

图 3. 基于双重光流的特征跟踪结果。左图为 ORB-SLAM3 的结果，右图为我们的方法。绿色点表示特征点，红色点表示重投影到图像上的点。

B. PnP 与 GICP 里程计

在无纹理环境中，无法获得准确且鲁棒的初始位姿会削弱光流跟踪的效果，并降低视觉重投影与深度到地图数据关联的精度，最终导致系统发散。此外对于腿式机器人来说，在每个运动模式周期中，速度变化和形态变化可能会干扰机器人的 IMU，从而在积分过程中产生错误的初始值，最终引发系统发散。为了获得准确且鲁棒的初始位姿，提出了一种融合腿式里程计（Legged Odom）、PnP 和 GICP 的初始位姿估计方法。首先如果腿式里程计数据可用，则优先使用它进行位姿预测，以避免因腿式机器人 IMU 误差导致的错误初始位姿预测。当 IMU 可用时，考虑到其在短时间内的高精度和计算量相对较小，优先使用 IMU 进行初始位姿预测。在未使用 IMU 的情况下，首先基于成功跟踪的特征点计算本质矩阵（Essential Matrix），以恢复帧间位姿。然而该方法无法提供尺度信息，因此进一步基于深度执行 PnP 以获得更精确的初始位姿。如果 PnP 失败，则使用 GICP 进一步优化初始位姿。若以上方法均失败，则基于恒速模型假设估计初始位姿。为提高计算效率，在 GICP 计算中采用 small_gicp。同时将基于 GICP 的连续帧相对位姿约束引入因子图优化框架，以进一步提升精度。

紧耦合的视觉、深度与 IMU

在无纹理环境中，大多数高度依赖 RGB 信息的 RGBD SLAM 系统在实现鲁棒且精确的定位方面面临挑战。而深度信息能够提供额外的结构细节，对系统性能具有重要帮助。因此，提升 RGBD SLAM 对深度观测的融合能力至关重要。如图 4 所示构建了如下因子图，在局部窗口内优化关键帧位姿与视觉特征点。该因子图由视觉重投影残差因子、深度到地图的点-平面残差因子、GICP 相对位姿残差因子以及 IMU 预积分因子组成。关键帧的深度平面特征点会拼接形成局部地图。对于当前关键帧的每个平面特征点，将其投影到局部地图后，对其 k 近邻点云进行平面拟合，从而得到点-平面残差。随后利用 GICP 约束帧间位姿。具体而言，当 IMU 预积分与腿式里程计或恒速模型假设存在较大偏差（表明 IMU 积分出现发散）时，我们会自适应地引入腿式里程计约束，以改善位姿估计。

图 4. GeoFlow-SLAM 在局部关键帧优化中的因子图框架。

优化完成后，我们使用优化后的相机位姿重建更精确的局部深度地图。为了保证系统实时性，当有效视觉观测不足时，才会向帧节点添加深度到地图的约束，从而兼顾精度与实时性能。单帧优化也遵循 ORB-SLAM 的做法，通过构建相邻帧之间的约束（包括 IMU 预积分、GICP 相对位姿、视觉重投影和深度到地图约束）来保持位姿估计的稳定性。

1) 从深度中提取平面特征点

类似于 SSL-SLAM，我们从点云中提取平面特征点以节省计算资源，因为直接进行深度到地图的配准在固态 LiDAR 产生的大量点云数据下计算量非常大。首先，通过计算每个点的垂直角和水平角，将点云分割为二维矩阵。随后，根据这些角度将检测范围均匀划分为多个单元格。在每个单元格中，计算所有点的几何中心，并根据局部平滑度提取平面特征。平滑度值越低表示越平坦的平面特征。这种方法计算效率高，适用于处理固态 LiDAR 产生的大量点云数据。

2) 深度到地图残差

深度残差通过将采样的平面点云与最近的子地图匹配生成。在局部建图线程中，会识别与当前帧在时空上接近的关键帧，并利用这些关键帧及其对应的平面特征点构建子地图。借鉴 LIOSAM，构建点到平面的残差，将当前帧的平面特征点与局部地图进行匹配，以优化位姿估计和地图精度。

平面残差考虑了世界坐标系下的平面法向量、平面中心点以及相机坐标系中的平面特征点，同时使用相机到世界坐标系的位姿变换矩阵进行转换，从而实现深度点云与地图的精确对齐。

实验结果

A. GeoFlow-SLAM 在 Go2 机器人狗和 OpenLORIS 基准测试中的评估

为了验证方法在真实场景下的表现，在 Unitree 生产的 Go2 四足机器人狗和 G1 人形机器人上进行了测试。Go2 机器人狗搭载 D435i 摄像头（约 10-15 FPS）、Bosch BMI055 IMU（200 FPS）、XT16 LiDAR（10 FPS）以及腿式里程计（500 FPS）。G1 人形机器人搭载 D435i 摄像头（25-30 FPS）、Mid360 LiDAR（10 FPS）及腿式里程计（500 FPS）。使用增强版 LIOSAM（类似 Leg-KILO）提供地面真值，并通过 LIO-SAM 与腿式里程计的轨迹对齐近似计算 IMU 与腿式里程计的外参。

图 5. Go2 四足机器人狗（左）与 G1 人形机器人（右）

共收集了六条四足机器人测试序列、四条机器人狗序列以及两条人形机器人序列。测试环境包括室内办公室、低纹理走廊、地下停车场以及户外广场，序列中存在快速运动抖动，导致图像模糊和 IMU 扭曲。

GeoFlow-SLAM 与 VINS 系列（VINS-RGBD、S-VIO）及 ORB-SLAM3 进行了比较。结果显示，在 Seq1 中，GeoFlow-SLAM 的定位精度较 VINS-RGBD 提升 88%，较 S-VIO 提升 93%。在 Seq2、Seq3、Seq4 和 Seq6 中，只有 GeoFlow-SLAM 成功跟踪，而其他方法均失败。在人形机器人 Seq5 中，相较于 ORB-SLAM3 提升 45%，表现最佳。运行时间方面，每帧处理约 20-30ms，虽然加入额外观测影响了效率，但仍保持实时性。

图 6. 真实环境挑战场景。Seq1-4 为 Go2 四足机器人狗测试场景，Seq5-6 为 G1 人形机器人测试场景。

GeoFlow-SLAM 优势主要来自三方面：

• 鲁棒的位姿初始化：腿式机器人运动特性导致 IMU 数据常出现误差，而通过腿式里程计、PnP 和 GICP 结合，即使在 IMU 扭曲或缺失情况下也能实现准确初始化。
• 增强的双流光流跟踪：利用先验地图点和位姿信息，即使图像模糊也能有效跟踪特征点，提高低纹理环境下的精度，并通过基础矩阵滤除动态物体产生的异常点。
• 紧耦合深度约束：在低纹理环境中，通过深度到地图和 GICP 约束提高位姿估计的精度和鲁棒性，例如低纹理走廊中，地面和墙壁提供了重要几何约束。

在 OpenLORIS 数据集上，GeoFlow-SLAM 与多种 VIO 算法（VINS-Mono、VINS-RGBD、ORB-SLAM3、S-VIO、VID-SLAM、DDIO-VIO）进行了系统比较，结果显示通过加入光流和深度约束，系统在大部分场景中表现出更高的鲁棒性和精度。

B. 消融实验

使用 ScanNet（9 条序列）和 TUM（7 条序列）数据集，对算法不同配置进行评估，包括仅光流（r+o）、仅 GICP（r+i）、仅深度到地图（r+d）以及全功能版本（r+o+i+d）。结果显示，加入双流光流、GICP 和深度约束后，相较于 RGB-D 模式下的 ORB-SLAM3，精度显著提升，且所有序列均未出现跟踪失败。ScanNet 数据集 r+o+i+d 平均 ATE 为 0.0779m，相较 ORB-SLAM3 提升 70%。TUM 数据集也显示全配置成功跟踪所有序列，并在 ATE 与 RTE 上分别提升 62.2% 和 36.7%。此外，与最先进的 RGB-D SLAM 方法 STLSLAM 比较，GeoFlow-SLAM 同样表现出更高精度。

C. 极难场景实验

在 TUM 数据集中的低纹理、低结构复杂度场景（nostr_notex_far 和 nostr_notex_near），GeoFlow-SLAM 展示了极强的鲁棒性和精度。传统光流在这些环境中容易失败，而我们的算法通过精确位姿先验、深度到地图点-平面约束及 GICP 约束，有效防止优化发散，即使视觉观测缺失也能保持稳健。nostr_notex_far 与 nostr_tex_near 的 ATE 分别为 0.0308m 和 0.0467m，且仍能构建全局平面特征地图。

图 11. 在 Nostr_notex_far 场景下的全局平面特征地图。左图为原始图像，右图为我们的全局平面特征点云。

总结

本研究提出了 GeoFlow-SLAM，一种针对四足机器人设计的新型 SLAM 系统，在特征点匹配、位姿初始化以及整体系统的鲁棒性和精度方面取得了显著提升。具体而言引入了一种方法，将双重光流跟踪与先验地图点和位姿信息相结合，从而在快速运动场景下增强特征跟踪能力。此外开发了一种结合 IMU/四足里程计、帧间 PnP 和 GICP 的位姿初始化方法，即使在缺少 IMU 数据的情况下，也能保证可靠的位姿估计。进一步地，通过紧耦合几何 GICP 约束，该方法在视觉纹理稀少的环境中显著提高了性能。GeoFlow-SLAM 在多个基准数据集上实现了最先进的性能。

相关阅读：2024年度历史文章大汇总

特别提醒：有意购买智元灵犀系列和宇树系列产品的朋友，以及需要人形机器人二次开发的朋友，后台发送“人形机器人”获取我的联系方式哦，也欢迎交流。

以上内容如有错误请留言评论，欢迎指正交流。如有侵权，请联系删除