{
  "paper_id": "665284f73e74c17703a34e66ff4c8fd9",
  "paper_title": "GeoFlow-SLAM: A Robust Tightly-Coupled RGBD-Inertial and Legged Odometry Fusion SLAM for Dynamic Legged Robotics",
  "paper_authors": [
    "Tingyang Xiao",
    "Xiaolin Zhou",
    "Liu Liu",
    "Wei Sui",
    "Wei Feng",
    "Jiaxiong Qiu",
    "Xinjie Wang",
    "Zhizhong Su"
  ],
  "publication_date": "",
  "publication_venue": "2025 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)",
  "abstract": "This paper presents GeoFlow-SLAM, a robust and effective Tightly-Coupled RGBD-Inertial and Legged Odometry Fusion SLAM for legged robotics undergoing aggressive and high-frequency motions. By integrating geometric consistency, legged odometry constraints, and dual-stream optical flow (GeoFlow), our method addresses three critical challenges: feature matching and pose initialization failures during fast locomotion and visual feature scarcity in texture-less scenes. Specifically, in rapid motion scenarios, feature matching is notably enhanced by leveraging dual-stream optical flow, which combines prior map points and poses. Additionally, we propose a robust pose initialization method for fast locomotion and IMU error in legged robots, integrating IMU/Legged odometry, interframe Perspective-n-Point (PnP), and Generalized Iterative Closest Point (GICP). Furthermore, a novel optimization framework that tightly couples depth-to-map and GICP geometric constraints is first introduced to improve the robustness and accuracy in long-duration, visually texture-less environments. The proposed algorithms achieve state-of-the-art (SOTA) on collected legged robots and open-source datasets. To further promote research and development, the open-source datasets and code will be made publicly available at https://github.com/HorizonRobotics/GeoFlowSlam.",
  "keywords": [
    "SLAM",
    "RGBD-Inertial",
    "Legged Robotics",
    "Dual-Stream Optical Flow",
    "Pose Initialization",
    "GICP",
    "Texture-less Environments",
    "Legged Odometry"
  ],
  "paper_institutions": [],
  "output_language": "zh",
  "provider": "doubao",
  "model": "glm-5.3",
  "prompt_template": "simple",
  "extracted_at": "2026-09-17T09:39:28.746441Z",
  "template_tag": "simple",
  "summary": "### 摘要\n\n针对腿式机器人在剧烈、高频运动下的定位难题——快速运动导致图像模糊与特征匹配失败、IMU受运动周期扰动致使位姿初始化失败、以及长时间无纹理环境中视觉特征稀缺——本文提出GeoFlow-SLAM，一个紧耦合融合RGBD-惯性测量与腿式里程计的SLAM系统。现有RGB-D SLAM（如ORB-SLAM、VINS-RGBD、RTAB-Map）仅将深度用于恢复特征点深度，结构化特征方法（PLD-VINS、PLP-SLAM、S-VIO、STL-SLAM）高度依赖环境且计算昂贵，NeRF/高斯类方法（NICE-SLAM、SplaTAM）无法实时运行，而现有腿式机器人SLAM（Leg-KILO、ViQu-SLAM）依赖昂贵笨重的LiDAR。GeoFlow-SLAM的核心机制包括：(1) 利用先验地图点与位姿辅助的双流光流跟踪——3D-to-2D流将地图点经初始位姿投影到当前帧后进行光流跟踪，2D-to-2D流对无有效地图点的特征直接跟踪并用基础矩阵剔除离群点；(2) 融合IMU/腿式里程计、帧间PnP与GICP的鲁棒位姿初始化，优先使用腿式里程计以规避腿式机器人IMU误差；(3) 首次提出紧耦合深度到地图与GICP几何一致性的优化框架，在因子图中联合优化视觉重投影残差、IMU预积分残差、GICP相对位姿残差与深度到地图点到平面残差（核心公式 r_l(d^{c_i}, X) = (n^w)^T (P' - q_plane^w) = JX + ω），且仅在视觉观测不足时引入深度约束以保证实时性。实验在自采集腿式机器人数据集（Go2机器狗配备D435i相机10-15FPS、BMI055 IMU 200FPS、XT16 LiDAR 10FPS、腿式里程计500FPS；G1人形机器人配备D435i 25-30FPS、Mid360 LiDAR，以增强版LIO-SAM提供真值）以及OpenLORIS、ScanNet、TUM基准上验证——这些数据集覆盖无纹理走廊、低光停车场、重复纹理广场、动态物体等真实挑战，用以检验快速运动与无纹理场景下的鲁棒性。结果表明：Go2数据集Seq1上ATE较VINS-RGBD提升88%、较S-VIO提升93%，且是Seq2-4、Seq6中唯一成功的方法；ScanNet上平均ATE为0.0779米（较ORB-SLAM3的0.2638米提升70%），TUM上ATE与RTE分别达0.0129米与0.0086米（提升62.2%与36.7%）；每帧处理约20-30毫秒实现实时运行，验证了双流光流、鲁棒初始化与紧耦合深度约束三项创新的有效性。未来工作将迈向基于场景图的语义重建，为腿式机器人提供结构化、以对象为中心的表示；实际部署需应对低光照、动态物体（行人、车辆）、重复纹理与IMU剧烈扰动（最大加速度达40 m/s²）等约束。\n\n### 关键要点\n\n- 双流光流跟踪的第一流利用IMU/腿式里程计或PnP、GICP提供的初始位姿，将特征点对应的先验地图点投影到当前帧后执行3D-to-2D光流跟踪，第二流对缺乏有效地图点的特征执行2D-to-2D光流跟踪，确保无深度信息时仍能连续跟踪。\n- 光流跟踪后通过基础矩阵识别并剔除不符合运动约束的离群点，并配合内点掩码机制避免同一区域重复加入跟踪点，一定程度上缓解了Seq2与Seq6中行人、车辆等动态物体的干扰。\n- 位姿初始化策略：优先使用腿式里程计进行位姿预测以规避腿式机器人IMU误差；IMU可用时利用其短时高精度；无IMU时先由本质矩阵恢复帧间位姿再结合深度PnP，失败时用GICP精化，全部失败则退回恒速模型假设。\n- 优化框架在因子图中紧耦合视觉重投影残差、深度到地图点到平面残差、GICP相对位姿残差与IMU预积分残差；当IMU预积分明显偏离腿式里程计或恒速假设（表明积分发散）时，自适应引入腿式里程计约束改进位姿估计。\n- 为保证实时性，仅当有效视觉观测不足时才向帧节点添加深度到地图约束；深度平面特征点通过将点云按垂直角与水平角分割为M×N单元并计算局部平滑度提取，计算高效且适合处理大量点云数据。\n- 实验平台：Go2机器狗配备D435i相机（10-15FPS）、Bosch BMI055 IMU（200FPS）、XT16 LiDAR（10FPS）与500FPS腿式里程计；G1人形机器人配备D435i（25-30FPS）与Mid360 LiDAR；采用增强版LIO-SAM（类似Leg-KILO）提供真值。\n- 自采集6个序列（4个机器狗、2个人形机器人）覆盖室内办公、无纹理走廊、低光反光的地下停车场、重复地面纹理的开放广场等挑战场景，所有序列均存在显著快速运动抖动，机器狗最大加速度达40 m/s²、人形机器人达20 m/s²。\n- Go2数据集结果：Seq1的ATE为0.0947米，较VINS-RGBD（0.8316米）提升88%、较S-VIO（1.3621米）提升93%；Seq2-4与Seq6中本方法是唯一成功运行的（对比方法均失败重置）；人形机器人Seq5上ATE为0.1709米，较ORB-SLAM3提升45%。\n- 运行效率：在13代Intel Core i9-13900H上每帧平均处理约20-30毫秒（各序列20.1-27.6毫秒），实现实时性能，但因引入额外观测，计算效率不可避免地低于ORB-SLAM3。\n- OpenLORIS基准上与VINS-Mono、VINS-RGBD、ORB-SLAM3、S-VIO、VID-SLAM、DDIO-VIO对比，本方法在多数序列上取得最优或极具竞争力的ATE（如Office1-1为0.0577米、Office1-3为0.0291米、Office1-5为0.2000米）。\n- 消融实验（ScanNet 9个序列、TUM 7个序列，RGBD模式无IMU与腿式里程计）：完整配置r+o+i+d在ScanNet上平均ATE为0.0779米（较ORB-SLAM3的0.2638米提升70%），在TUM上ATE为0.0129米、RTE为0.0086米（分别提升62.2%与36.7%），且所有配置均无跟踪失败，而ORB-SLAM3在多个序列（如ScanNet的50、84、181、207、616及TUM的无纹理序列）失败。\n- 极端场景（TUM no_structure_notexture_far/near等无纹理无结构场景）中ATE分别为0.0308米与0.0467米，深度到地图与GICP约束在缺乏视觉观测时有效约束位姿估计防止优化发散；未来工作将聚焦基于场景图的语义重建，为腿式机器人提供结构化、以对象为中心的表示。"
}