GeoFlow-SLAM：面向动态足式机器人的鲁棒紧耦合RGBD惯性里程计融合SLAM

论文信息：Tingyang Xiao、Xiaolin Zhou、Liu Liu、Wei Sui、Wei Feng、Jiaxiong Qiu、Xinjie Wang、Zhizhong Su，2025年3月18日，IROS 2025。论文链接：https://arxiv.org/abs/2503.14247。开源代码：https://github.com/HorizonRobotics/GeoFlowSlam，状态为已发布。

一句话总结：GeoFlow-SLAM把双流光流、足式里程计、GICP和深度到地图约束放入同一个因子图，使剧烈运动与弱纹理环境中的足式机器人定位仍能保持稳定、准确和实时。

1. 研究背景：为什么普通视觉SLAM在足式机器人上容易崩

RGB-D相机一次采集同时获得颜色和深度，通常被认为比单目视觉更适合机器人SLAM。问题在于，足式机器人不是平稳移动的平台。步态中的加减速、姿态切换和机身高频振动会让图像出现运动模糊，相邻帧之间的视差也可能突然变大。ORB特征在纹理丰富的静态场景中可靠，但在这种快速运动里，传统投影匹配很容易把上一帧特征投影到错误邻域，导致内点数量骤减。

第二个困难是位姿初始化。视觉SLAM通常依靠恒速模型或IMU预积分给出当前帧的初始位姿。对足式机器人而言，每个步态周期内的速度变化和机体形态变化会带来非平滑激励，IMU预积分可能在短时间后偏离真实运动。若初值错了，后续光流搜索窗口、深度数据关联和局部优化都会沿着错误方向收敛。

第三个困难来自弱纹理场景。办公楼、走廊和结构化室内空间常见大块白墙、重复纹理和缺少角点的表面。视觉观测不足时，仅靠视觉重投影约束没有足够几何信息把位姿拉回正确位置。深度图本身仍然记录了平面与结构，只是直接做完整点云到地图配准的计算代价很高。

因此，GeoFlow-SLAM的设计目标不是简单叠加更多传感器，而是让三类互补线索在同一优化框架内互相补位：光流解决快速运动下的特征对应，足式里程计和GICP解决初值，深度平面约束解决弱纹理下的几何支撑。

2. 系统概览：一个面向退化场景的流水线

系统入口包括RGB图像、深度图、IMU数据和足式里程计。跟踪线程先根据可用数据预测当前位姿，再通过双流光流建立特征对应，随后构建视觉重投影、IMU预积分、GICP相对位姿和深度到地图四类因子。关键帧局部优化同时估计窗口内的相机位姿和地图点。为了避免计算量失控，深度到地图约束只在有效视觉观测不足时加入对应帧节点。

从系统结构看，最重要的变化是“初值—匹配—优化”三者不再各自独立。光流使用IMU、足式里程计、PnP或GICP给出的先验位姿；优化结果又反过来修正地图和局部几何，为下一帧光流提供更可靠的投影位置。这种闭环使方法在观测缺失时不会迅速漂移。

3. 双流光流：让地图点先解释当前图像

第一路是三维到二维光流。设上一帧特征点对应的地图点为$P(X,Y,Z)$，初始位姿为$T_{wk}^{c}$，则地图点可投影到当前帧：

相机的投影模型写成：

得到投影点后，算法并不直接把投影当成匹配，而是以投影点为起点在当前帧中运行光流。这样搜索被限制在几何合理的位置附近，避免在大位移下盲目寻找描述子最近邻。

第二路处理上一帧中没有合格地图点对应的特征。这些点先在上一帧与当前帧之间做光流跟踪，再与第一路结果合并。作者把两路称为GeoFlow：一路由局部地图和位姿先验引导，另一路保留短期图像运动线索。前者的优点是尺度明确、受外观变化影响小；后者的优点是不依赖已有地图点，能补充新观测。

这张设计的价值在于错误传播路径被切断。若IMU或里程计给出略偏的位姿，几何投影仍可能落在真实特征附近；若某些点没有地图对应，第二路光流仍能维持短时连续性。二者合并后，后端有更充分的观测，优化也更不容易落入错误局部极小。

4. 位姿初始化：足式里程计优先，PnP和GICP兜底

论文明确指出，足式里程计可用时优先使用它预测位姿，以避免腿式机器人IMU误差造成的错误初值。IMU在短时间内精度高、计算简单，因此也可作为优先预测来源。若二者不可用或失效，系统先基于成功跟踪的特征计算本质矩阵，恢复帧间运动；由于该步骤缺少尺度，再利用深度做PnP得到带尺度的初始位姿。若PnP失败，则使用GICP细化初值；全部失败时才退回恒速模型。

这个顺序体现了一个实用判断：足式机器人的运动学里程计虽然不是全局真值，但在步态周期内通常比受振动影响的IMU更稳定；GICP则适合在视觉退化时利用几何结构补位。源码中`Tracking.cc`的`TrackWithMotionModelICP()`实现了这一逻辑：先按IMU或光流预测状态，再在足式里程计可用时调用`PredictStateOdom()`，随后按配置调用`PredictStateICP()`，最后执行`SearchByProjectionWithOF()`。

5. 局部因子图：四类残差共同约束状态

局部关键帧优化中，GeoFlow-SLAM联合估计窗口内位姿$x_1,\ldots,x_n$与地图点$P_1,\ldots,P_n$。论文将目标写成四个鲁棒残差项：

$$r_2=\min_{X}\sum_{i\in F}\rho\left(r_{imu}^T\left(z_{b_i b_{i+1}}^c,X\right)\Sigma_{b_i b_{i+1}}^{-1}r_{imu}\left(z_{b_i b_{i+1}}^c,X\right)\right)$$

$$r_3=\min_{X}\sum_{(i,j)\in F}\rho\left(r_{gicp}^T\left(I_{j i}^c,X\right)\Sigma_{gicp}^{-1}r_{gicp}\left(I_{j i}^c,X\right)\right)$$

$$r_4=\min_{X}\sum_{(i,j)\in F}\rho\left(\|r_l(d_i^c,X)\|_{\Sigma_l}^2\right)$$

其中$r_1$是视觉重投影残差，$r_2$是IMU预积分残差，$r_3$是GICP相对位姿残差，$r_4$是深度到地图残差。各协方差矩阵表达对应观测的不确定性，$\rho$为鲁棒核，用于抑制错误匹配和点云外点。局部BA中$X$包含IMU状态和地图点；单帧优化中只优化相邻帧IMU状态。

6. 深度平面特征与点面残差

为了降低直接深度配准的计算量，系统先从深度点云中提取平面特征。论文采用类似SSL-SLAM的做法，计算每个点的垂直角$\alpha_i$与水平角$\theta_i$：

$$\theta_i=\arctan\left(\frac{y_i}{x_i}\right)$$

角度区间把检测范围划分为$M\times N$个单元；每个单元内求几何中心并计算局部平滑度，平滑度越低越可能属于平面。之后，关键帧的深度平面点被拼接成局部地图。对当前关键帧中的平面点，先投影到局部地图并对k近邻拟合平面，形成点面约束。

点面残差定义为：

其雅可比矩阵为：

且

这里$n^w$是世界系平面法向量，$q_{plane}^w$是平面中心，$p_{plane}^c$是相机系平面点，$\omega$是观测噪声。直观地说，该残差不要求点完全落在某个地图点上，而要求其落在已有平面上，因此天然适合墙、地面、桌面等结构。

7. 与开源实现的对应关系

开源仓库并非只提供论文插图，也包含可直接核查的核心实现。`src/Tracking.cc`中`TrackWithMotionModelICP()`按IMU、光流、足式里程计和GICP的顺序形成当前位姿先验，并在`UseOpticalFlow`开启时调用`SearchByProjectionWithOF()`；当匹配数低于25时，它会补充新特征并把距离阈值减半，再执行一次更宽窗口的光流搜索。

`src/RegistrationGICP.cc`把GICP封装为`RegisterPointClouds()`，设置4线程、0.02米降采样分辨率和0.1米最大对应距离，然后调用`small_gicp`的`align()`。`src/Optimizer.cc`在局部关键帧优化中调用`RegisterPointClouds()`得到相邻关键帧GICP相对位姿，并把它转换成Sim3边加入因子图；对匹配内点不超过75的关键帧，`GenerateLidarEdge()`生成点面边，信息权重取100，并使用Huber鲁棒核。这些实现与论文公式和流程一一对应。

8. 实验设置与Go2/G1结果

作者在Unitree Go2四足机器人和G1人形机器人上验证。Go2配置D435i相机约10–15 FPS、BMI055 IMU约200 FPS、XT16 LiDAR约10 FPS、足式里程计约500 FPS。G1配置D435i约25–30 FPS、Mid360 LiDAR约10 FPS、足式里程计约500 FPS。真值由面向足式机器人的增强LIOSAM提供，并利用LIO-SAM与足式里程计轨迹估计IMU与足式里程计之间的外参。

下表汇总Go2 D435i数据集上的ATE，单位为米。斜杠表示跟踪失败。

Seq1中GeoFlow-SLAM相对VINS-RGBD提升约88%，相对S-VIO提升约93%。更重要的是Seq2、Seq3、Seq4和Seq6上其他基线大多失败，而本方法仍完成跟踪。作者报告的平均每帧耗时在i9-13900H上为20.1–27.6毫秒，说明性能提升并未以完全牺牲实时性为代价。

9. OpenLORIS结果

OpenLORIS覆盖家庭与办公室等动态室内环境。论文比较了VINS-Mono、VINS-RGBD、ORB-SLAM3、S-VIO、VID-SLAM、DDIO-VIO和GeoFlow-SLAM。下表选择其中的Office序列与Home1-1，单位仍为米。

结果并非每项都第一：Home1-1上S-VIO为0.4020米，优于GeoFlow-SLAM的0.4575米；Office1-4上S-VIO为0.1640米，略优于0.1554米。但在多数Office序列上GeoFlow-SLAM取得最低或接近最低误差，且在关键序列上表现出更稳定的完成率。这个细节值得强调：对于足式机器人，失败与成功的差距往往比两个成功轨迹之间的几厘米差距更重要。

10. 消融实验：每个模块到底贡献什么

作者在ScanNet和TUM上比较四种配置：r+o只加光流，r+i只加GICP，r+d只加深度到地图约束，r+o+i+d为完整版本。ScanNet选择50、59、84、106、169、181、207、580、616九个序列；TUM选择长办公室和多个弱纹理/弱结构序列。平均结果如下：

ScanNet上r+o+i+d平均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的str_notex_far、str_notex_near、nostr_tex_far失败；四种GeoFlow配置均完成全部序列。

消融还揭示出模块互补性。ScanNet中r+d单独达到0.0775米，略优于完整版的0.0779米，说明在结构化室内场景中深度平面约束已经很强；TUM中r+o单独就把ATE从0.0341降到0.0142米，说明光流对快速运动下的视觉匹配贡献显著。完整版本的意义在于跨场景稳定性，而不是在每个数据集上机械压低单项数字。

11. 极端弱纹理场景与STL-SLAM对比

在TUM的no_structure_notexture_far与nostructure_notexture_near两个极端场景中，GeoFlow-SLAM的ATE分别为0.0308米和0.0467米。作者认为，重复纹理和稀疏特征会让传统光流误配，而GeoFlow使用更准确的位姿先验限制搜索；视觉观测不足时，深度点面约束和GICP相对位姿约束继续约束优化，防止发散。

与STL-SLAM的比较显示，GeoFlow-SLAM在多数TUM序列上更低，但并非全部。fr1/xyz上STL-SLAM为0.0100米，GeoFlow-SLAM为0.0101米；fr3/str_notex_near上ORB-SLAM3为0.0126米，GeoFlow-SLAM为0.0126米；fr3/str_tex_near上ORB-SLAM3为0.0093米，GeoFlow-SLAM为0.0094米。差异很小，而在fr3/cabinet与fr3/large_cabinet上ORB-SLAM3失败，GeoFlow-SLAM分别为0.0500米和0.0601米。这个对比说明方法主要收益来自鲁棒完成，而不是在所有常规序列上均占优。

12. 局限性

第一，实验平台仍依赖较高算力。虽然论文给出的20–27.6毫秒结果在i9-13900H上接近实时，但这不等于低功耗嵌入式平台可直接复现。GICP、平面提取和点面边生成都有额外开销，部署到机载小算力设备时还需要进一步剪裁。

第二，几何约束假设局部地图中存在可靠平面或结构。若环境既弱纹理、又弱结构，深度到地图约束的可用性下降；论文展示了极端弱纹理场景，但对完全缺乏几何结构的场景仍需更谨慎评估。

第三，论文的真值由增强LIOSAM给出，并利用轨迹对齐估计外参。虽然设置合理，但真值和外参质量仍可能影响绝对误差解读，尤其在小尺度改进的比较中。

第四，OpenLORIS和TUM并非全部指标领先。某些成功序列上，S-VIO、STL-SLAM或ORB-SLAM3仍有更低ATE。GeoFlow-SLAM的优势是足式剧烈运动与退化观测下的完成率，而不是无条件替代所有RGB-D或VIO方法。

13. 方法流程图

这张流程图概括了GeoFlow-SLAM区别于普通视觉SLAM的关键分支：深度几何约束不是常开模块，而是由视觉观测充分性触发的退化补救。

14. 总结

GeoFlow-SLAM的贡献可以概括为三点：第一，把地图点和位姿先验引入光流，使快速运动下的特征匹配有几何边界；第二，构建足式里程计、PnP与GICP级联的初值策略，降低IMU误差对腿式机器人的影响；第三，在局部因子图中紧耦合视觉重投影、IMU预积分、GICP相对位姿和深度点面残差，让弱纹理场景仍有几何约束。

从工程角度看，这篇论文的价值不只是提出新方程，而是把每个失败环节的补救路径写得清楚：初值从哪里来、搜索窗口如何受控、视觉不足时几何约束何时介入、优化后如何回馈下一帧。开源实现也能验证这些设计确实落地，便于机器人系统开发者移植和改造。

金句：视觉退化时，机器人真正需要的不是更多像素，而是仍然成立的几何约束。