# GeoFlow-SLAM：面向动态足式机器人的一种鲁棒的紧耦合RGBD-惯性与足式里程计融合SLAM算法 肖廷阳 $^{1}$、周晓林 $^{1}$、刘柳 $^{1}$、隋伟 $^{2}$、冯伟 $^{1}$、邱佳雄 $^{1}$、王新杰 $^{1}$、苏志忠 $^{1}$ 摘要—本文提出GeoFlow-SLAM，这是一种针对进行剧烈高频运动的足式机器人所设计的鲁棒且高效的紧耦合RGBD-惯性与足式里程计融合SLAM算法。通过整合几何一致性、足式里程计约束以及双流光流（GeoFlow）机制，该方法解决了三大关键难题：快速运动过程中的特征匹配与位姿初始化失败问题，以及无纹理场景中视觉特征匮乏的问题。具体而言，在快速运动场景下，通过利用结合先验地图点与位姿信息的双流光流技术，特征匹配能力得到显著提升。此外，我们针对快速运动及足式机器人中的惯性测量单元（IMU）误差问题，提出了一种鲁棒的位姿初始化方法，该方法融合了IMU/足式里程计数据、帧间透视n点（PnP）求解以及广义迭代最近点（GICP）算法。更进一步，我们首次引入了一种将深度图匹配与GICP几何约束紧密耦合的新型优化框架，以提升系统在长时间、无视觉纹理环境下的鲁棒性与精度。所提算法在采集的足式机器人数据及开源数据集上均取得了业界领先的性能。为了进一步推动相关研究与开发，开源数据集与代码将于公开发布。 ## I. 引言 RGB-D相机能够捕捉RGB信息，同时还能为其视野内所有区域提供深度信息，无论这些区域是否有纹理 [^1]。这一特性使得RGBD相机在机器人领域广受欢迎，尤其在视觉同步定位与建图（SLAM）等应用中。 针对上述问题，我们提出了GeoFlow-SLAM，其主要贡献如下： 1) 提出了一种融合地图点与位姿信息的双流光流跟踪技术，可显著增强特征匹配能力，尤其是在快速运动场景下。 对于足式机器人而言，由于每个运动周期中速度与姿态的变化，常会出现快速运动抖动现象，进而导致图像模糊与IMU数据失真。此外，这类机器人通常在无纹理的室内环境中作业，这给足式机器人的定位带来了新的挑战。然而，基于RGB/RGBD的SLAM算法 [^2]、[^3]、[^4]、[^5]、[^6]、[^7] 无法有效应对这些问题。具体来说，快速运动抖动会引发图像模糊，进而增加特征匹配的难度。当未使用IMU，或因足式机器人独特的运动特性导致IMU数据失真时 [^8]、[^9]，机器人在快速运动中往往无法准确初始化位姿。此外，在无纹理环境下长时间作业时，深度信息仅用于估算特征点的深度，缺乏能够提升系统整体精度与鲁棒性的额外约束。为了解决这些问题， 2) 针对快速运动过程中的位姿初始化失败问题，我们提出了一种基于惯性测量单元/足式机器人里程计、透视 n 点问题（PnP）以及广义迭代最近点（GICP）的位姿初始化方法。 3) 我们首次提出了一种全新的优化框架，该框架将深度图与GICP几何一致性紧密结合起来，从而提升了算法的鲁棒性和精度，尤其是在无纹理环境中表现更为突出。 4) 我们的算法在开源数据集以及我们利用足式机器人采集的数据集上均取得了最先进的性能。为了推动后续的研究与发展，我们将发布关于足式机器人的开源数据集和代码。 ### 二、相关工作 Kinect-Fusion [^10]是一种基于RGB-D摄像头的实时3D重建系统，它从深度图像中生成点云，并通过迭代最近点（ICP）算法估计相机位姿。Kintinuous [^11]在Kinect-Fusion的基础上增加了闭环检测功能，并利用GPU加速进行位姿估计。Elastic-Fusion [^12]则利用RGB颜色的一致性来估计相机位姿，同时采用基于深度的ICP算法进行位姿估计。不过，这些系统主要聚焦于3D重建而非位姿估计。 ORBSLAM [^5]、VINS-RGBD [^7]和RTAB-Map [^13]是基于特征点的RGB-D SLAM系统，其中深度信息主要用于恢复特征点的深度，并未提供额外的观测约束。在基于ORB-SLAM的RGB-D SLAM系统中，特征跟踪主要依赖特征点的深度和位姿先验信息；而在基于VINS的系统中，光流跟踪并不利用深度和位姿先验信息来辅助跟踪。在快速运动的场景下，特征点极易出现跟踪失败的情况。 DS-SLAM [^6]、RSO-SLAM [^14] 和 ViQu-SLAM [^8] 利用深度信息来辅助识别和去除 --- 环境中的动态特征点，从而提高定位精度。基于 VINS 和 ORBSLAM 框架，出现了一些用于结构化环境的 RGB-D SLAM 系统，例如 PLD-VINS [^15]、PLP-SLAM [^16]、S-VIO [^17]、STL-SLAM [^18] 和 VID-SLAM [^19]。虽然结构特征的提取在一定程度上增强了定位精度和鲁棒性，但这些方法高度依赖环境。在非结构化环境中，它们的鲁棒性会显著下降。此外，额外的结构特征提取计算成本高昂且耗时。 基于神经辐射场和高斯模型的RGB-D SLAM系统，如NICE-SLAM [^20]和SplaTAM [^21]，专注于三维场景几何与纹理的密集重建，但通常无法实现实时性能。在缺乏视觉观测的情况下，不依赖环境特征的惯性测量单元（IMU）能够提供连续的位姿输出与预积分约束，因此被广泛应用于各类RGB-D SLAM算法中（例如VINS-RGBD、ORBSLAM3、VID-SLAM、DDIO-Mapping [^22]、S-VIO等）。然而，由于IMU零偏噪声的存在以及深度约束的缺失，在视觉特征约束长期缺失的环境中，位姿发散仍不可避免，从而影响系统的精度与鲁棒性。R3LIVE [^23]和FASTLIVO [^24]、[^25]系列算法将IMU、视觉与激光雷达观测紧密耦合，但它们并不适用于RGB-D相机。 现有的RGB-D-惯性SLAM系统往往无法有效追踪特征并充分利用深度信息，导致在非结构化环境中鲁棒性较差。此外，足式机器人在运动过程中的速度与形态变化会干扰IMU，使得传统的视觉-运动融合方法不再适用 [^8]。 在足式机器人SLAM领域，MIT Cheetah2 [^26]和ANYmal [^27]采用立体相机、RGB-D相机或激光雷达的组合来进行定位并构建以机器人为中心的地图。基于LIOSAM [^28]，Leg-KILO [^9]提出了一种利用流形上误差状态卡尔曼滤波器（ESKF）的新型腿部里程计，将腿部运动学与IMU紧密耦合。ViQu-SLAM则解决了动态环境中移动物体带来的挑战，实现了足式机器人的腿部里程计（LO）与视觉-惯性里程计（VIO）的融合。不过，上述足式机器人相关方法均依赖成本高昂且体积庞大的激光雷达传感器，且未能充分应对现实世界的挑战。具体而言，这些系统在无纹理环境中以及快速运动时会出现特征追踪失败，进而导致定位精度下降。 ### 三、系统概述 所提出的GeoFlow-SLAM系统由三个模块组成：特征提取与追踪、IMU/腿部/PnP/GICP里程计，以及视觉-深度-IMU融合建图，具体如图1所示。 ## 1) 特征提取与追踪 首先从深度点云中提取深度平面特征，并将其用于GICP算法以获得初始位姿估计。随后从图像数据中提取定向FAST与旋转BRIEF（ORB）特征。接着采用双流光流追踪与透视n点（PnP）算法进行特征追踪。IMU的原始测量数据与腿部编码器数据会被预先积分，从而为IMU/腿部里程计奠定基础。 ## 2) IMU/腿部/PnP/GICP里程计 当腿部里程计与IMU数据均可用时，它们会被主要用于位姿预测；若两者不可用时，则通过连续帧间的PnP问题来确定机器人的初始位姿。来自IMU/腿部或PnP的初始位姿会进一步通过GICP算法进行优化。完成位姿初始化与特征追踪后，会利用深度到地图的配准以及视觉地图点的重投影来生成观测残差。 ## 3) 视觉-深度-IMU融合建图 该模块包含单帧优化、局部关键帧优化以及全局关键帧优化。在单帧位姿优化和局部关键帧束调整（BA）模块中，视觉重投影残差、深度到地图残差以及IMU/腿部运动/GICP位姿约束在因子图框架内被紧密耦合在一起。与仅专注于当前相邻帧位姿估计的单帧位姿优化不同，局部BA会估算局部关键帧的位姿以及对应的视觉地图点。一旦检测到回环闭合，全局优化模块就会进一步对整体关键帧位姿和视觉点云地图进行优化。 ![Image](images/5dcdf8547f102355bb9ce9520761441df3664576c93a130e763661336e28fb8b.jpg)
Figure 1. Overview of Our GeoFlow-SLAM Pipeline. Initially, utilizing data from RGBD, IMU, and Legged Encoders, we perform the extraction of depth plane features, and ORB features, and carry out IMU/legged pre-integration. The initial pose is determined using GICP, PnP, and IMU/Legged Odometry. Building upon this preliminary pose, Dual-Stream Optical Flow is subsequently utilized to track ORB features. Subsequently, single-frame and local keyframes are optimized within a tightly coupled Vision-Depth-IMU factor graph, enabling accurate map construction and pose estimation.
--- ### 四、方法 ### A. 基于先验地图点和位姿辅助的双流光流跟踪 传统光流通常基于运动微小且连续这一假设。然而，在快速运动场景下，基于亮度恒定假设的光流估计可能会变得不准确，甚至无法收敛。此外，直接在高分辨率图像上计算光流会导致算法收敛缓慢，并消耗大量计算资源。为解决这些挑战，我们提出了一种双流光流跟踪方法，如图2所示。该方法利用先验地图点（ORB特征对应的地图点）和位姿，来增强快速运动时的特征跟踪鲁棒性。这种方法有效缓解了因相机快速运动导致像素发生大幅位移而引发的常见特征跟踪失败问题。 ![Image](images/1653aad7976f3ec3bb6fd1fed59a59994eab3e75eb2333eb058db65310e6852b.jpg)
Figure 2. The prior map points and pose-aided dual-stream optical flow pipeline. Initially, the feature points corresponding to prior map points are subjected to reprojection, generating the reprojected points (depicted as blue points). Subsequently, optical flow is conducted on these reprojected points based on the current frame. Next, optical flow tracking is further applied to the feature points in the previous frame that lack qualified prior map points (represented as yellow and pink points).
## 1) 3D到2D的光流跟踪 在第一条流中，我们利用IMU/腿部里程计或PnP、里程计模块中的GICP所提供的初始位姿，将特征点$\boldsymbol{P}(X, Y, Z)$对应的地图点$p(x, y)$投影到当前帧$p'(x, y)$中。 $$ p^{\prime}(x,y)=F(T_{w}^{c_{k-1}}P) $$ $$ F(\boldsymbol{P})=\begin{pmatrix}f_{X}\frac{x}{z}+c_{x}\\ f_{Y}\frac{y}{z}+c_{y}\end{pmatrix} $$ 其中$F(\mathbf{P})$表示内部投影参数。接着，基于当前帧，对投影后的像素$p'(x,y)$进行3D到2D的光流跟踪。$T_{w}^{ck-1}$表示时间$k-1$下的相机位姿。通过利用特征点的精确运动和地图点先验信息，我们在快速运动期间实现了更高的匹配精度。此外，该方法还能自动过滤掉不稳定的特征，比如动态不一致的特征。 ## 2) 2D到2D的光流跟踪 尽管第一阶段具有诸多优势，但ORB特征往往缺乏有效的对应地图点，尤其是在三角测量失败的情况下。此外，不准确的初始位姿也会大幅降低3D到2D特征跟踪的成功率。为应对这些挑战，我们引入了第二条流，对原始特征点采用2D到2D的光流跟踪。这一额外步骤确保了即使在没有深度信息的情况下也能持续跟踪。为了进一步提升鲁棒性，系统会计算基础矩阵来识别并剔除异常值，同时采用内点掩码机制防止同一区域内的跟踪点被不必要地纳入。这种组合方法有效减轻了异常值的影响，确保了更可靠的跟踪过程。 如图3所示，当ORB-SLAM3无法跟踪到有效的特征点时，我们的双流光流跟踪算法仍能成功跟踪地面、门闩等特征，即便是在快速移动导致的运动模糊或纹理较少的环境（如大面积的白色墙面）中也是如此。 ![Image](images/0d827dcda3d8470f9eda43429ae95515e81f79cca4f8cd7fab8be59a0f00ef4f.jpg)
Figure 3. Feature track: stream optical flow. The left image is the ORD green points denote feature points onto the
![Image](images/acb8ee45b4ebd91448858a8fd0ee648dd33824d4a1a96664e453e643dd866b3a.jpg)
Figure 4. Factor graph framework of GeoFlow-SLAM in local keyframe optimization. The visual map point $P_1 - P_n$ and pose $x_1 - x_n$ in a window are jointly estimated by integrating visual reprojection residual, depth to map residual, GICP residual, and IMU pre-integration residual. To enhance computational efficiency, we only integrate depth-to-map constraints when valid visual observations are insufficient.
--- ### B. 足式机器人、PnP和GICP里程计 在无纹理环境中，无法获取准确且鲁棒的初始位姿会削弱光流跟踪的效果，也会降低视觉重投影及深度到地图数据关联的精度，最终引发系统发散。此外，对于足式机器人而言，不同运动模式周期内的速度变化与形态改变会干扰机器人的IMU [^8]、[^9]，导致积分过程中的初始值出现偏差，最终引发系统发散。为了获取准确且鲁棒的初始位姿，我们开发了一种融合足式里程计、PnP和GICP的初始位姿估计方法。 首先，如果有足式里程计数据可用，会优先将其用于位姿预测，以避免足式机器人因IMU误差导致的初始位姿预测错误。当IMU可用时，鉴于其在短时间内的高精度特性以及相对简单的计算量，我们会优先利用IMU进行初始位姿预测。在无需使用IMU的场景下，我们会先根据成功追踪到的特征点计算本质矩阵，从而恢复帧间位姿；不过该方法无法提供尺度信息，因此我们需要进一步基于深度数据开展PnP计算，以获得更精确的初始位姿。如果PnP失败，则通过GICP进一步优化初始位姿。若上述所有方法均失效，则基于恒速模型假设来估计初始位姿。 为了提升计算效率，我们使用small_gicp [^29]来进行GICP计算。同时，连续帧间的基于GICP的相对位姿约束会被纳入因子图优化框架中，以进一步提升精度。 ### C. 视觉、深度与IMU的紧耦合 在无纹理环境中，严重依赖RGB信息的多数RGBD SLAM系统往往难以实现鲁棒且精确的定位。而深度信息能够提供额外的结构细节，对系统发展大有裨益。因此，提升RGBD SLAM的性能、进一步整合深度观测数据至关重要。如图4所示，我们构建了如下因子图来优化局部窗口内关键帧及视觉特征点的位姿。该因子图包含视觉重投影残差因子、深度到地图的点面残差因子、GICP相对位姿残差因子以及IMU预积分因子。关键帧的深度平面特征点会被拼接成局部地图。对于当前关键帧中的每个平面特征点，将其投影到局部地图后，会对最近的k个点云进行平面拟合，进而得到点面残差。随后利用GICP对帧间位姿施加约束。具体而言，如果IMU预积分结果与足式里程计或恒速假设存在显著偏差，说明出现了IMU积分发散现象，此时我们会自适应引入足式里程计约束来优化位姿估计。 优化完成后，我们利用优化后的相机位姿来重建更精确的局部深度图。为确保系统的实时性能，仅在有效视觉观测不足时，才会向帧节点添加深度到地图的约束。这种方法既能保证系统的精度，又能维持其实时性。 对于单帧优化，我们也借鉴ORB-SLAM的做法，在相邻帧之间构建约束，包括IMU预积分、GICP相对位姿、视觉重投影以及深度到地图的约束。 $$ r_{1}=min_{X}\sum_{(i,j)\in F}\rho\big(\|r_{v}(p^{c_{i}},X)\|_{\Sigma_{v}}^{2}\big) $$ $$ r_{2}=min_{X}\sum_{i\in F}\rho\left(\left\|r_{imu}\big(z_{b_{i}b_{i+1}},X\big)\right\|^{2}_{\Sigma_{b_{i}b_{i}+1}}\right) $$ $$ r_{3}=m i n_{\boldsymbol{X}}\sum_{(i,j)\in F}\rho\left(\left\|r_{i c p}\left(\boldsymbol{I}_{c_{j}}^{c_{i}},\boldsymbol{X}\right)\right\|_{\boldsymbol{\Sigma}_{i c p}}^{2}\right) $$ $$ r_{4}=m i n_{\boldsymbol{X}}\sum_{(i,j)\in F}\rho\big(\|r_{l}(d^{c_{i}},\boldsymbol{X})\|_{\Sigma_{l}}^{2}\big) $$ $r_1$、$r_2$ 表示视觉重投影与IMU预积分残差因子；$r_3$、$r_4$ 分别对应GICP相对位姿以及深度到地图的点面残差。$p^{c_i}$ 代表第i帧的视觉观测值；$z_{b_i b_{i+1}}$ 表示预积分后的观测值；$I_{c_j}^{c_i}$ 代表第i帧和第j帧之间的GICP相对位姿；$d^{c_i}$ 定义为第i帧的深度到地图观测值。$\rho$ 是一种用于提升优化步骤稳定性、减轻异常值影响的核心函数。在局部BA中，X表示关键帧的IMU状态x以及视觉地图点P；而在单帧优化中，它仅表示相邻帧的IMU状态。 ## 1）从深度图中提取平面特征点 与SSL-SLAM [^30]类似，我们为了节省计算资源而从点云中提取平面特征点，因为固态激光雷达生成的点云数据量庞大，直接进行深度到地图配准会耗费大量计算资源。最初，通过计算每个点$\alpha_i$的垂直角度$\theta_i$和水平角度$p_i = (x_i, y_i, z_i)$，将点云分割为二维矩阵。 $$ \alpha_{i}=\arctan\left(\frac{y_{i}}{x_{i}}\right) $$ $$ \theta_{i}=\arctan\left(\frac{z_{i}}{x_{i}}\right) $$ 随后，利用这些角度对检测范围进行等份划分，将点云分割成$M \times N$个单元格。在每个单元格内，我们计算所有点的几何中心并定义局部平滑度以提取平面特征。平滑度数值越低，说明该特征越接近平面。这种方法在计算上更高效，也更适合处理固态激光雷达产生的大量点云数据。 ## 2）深度到地图的残差 通过将采样的平面点云与最近的子图进行匹配来生成深度残差。在局部建图线程中，会识别出与当前帧在时空上距离较近的关键帧。随后，利用这些关键帧的位姿及其对应的平面特征点来构建子图。 受LIOSAM的启发，构建了从平面点到局部地图的点面残差$r_{l}$： --- $$ r_{l}(d^{c_{i}},X)=(n^{w})^{T}\big(P^{\prime}-q_{p l a n e}^{w}\big)=J X+\omega $$ $$\boldsymbol{J}=\frac{\partial\boldsymbol{r}_{l}}{\partial\boldsymbol{X}}=\frac{\partial\boldsymbol{r}_{l}}{\partial\boldsymbol{P}^{\prime}}\frac{\partial\boldsymbol{P}^{\prime}}{\partial\boldsymbol{X}}=(\boldsymbol{n}^{w})^{T}[\boldsymbol{I},-{\boldsymbol{P}^{\prime}}^{\wedge}]$$ $$ P^{\prime}=T_{c}^{w}p_{p l a n e}^{c} $$ 其中，$\boldsymbol{n}^{w}$ 表示世界坐标系下平面的法向量，$q_{plane}^{w}$ 表示世界坐标系下平面的中心点，$p_{plane}^{c}$ 表示相机坐标系下的平面特征点，$T_{c}^{w}$ 表示从相机坐标系到世界坐标系的位姿变换矩阵。$\omega$ 是点面观测噪声。 ### V. 实验 ### A. 在Go2机器狗和OpenLORIS基准测试集上对GeoFlow-SLAM的评估 为了在实际场景中评估我们的方法，我们在Unitree公司生产的Go2四足机器狗和G1人形机器人上进行了验证，如图5所示。该机器狗配备了D435i相机（帧率约10-15 FPS）、D435i内置的Bosch BMI055 IMU（帧率200 FPS）、XT16激光雷达（帧率10 FPS）以及腿式里程计系统（帧率500 FPS）。G1人形机器人则配备了D435i相机（帧率约25-30 FPS）、Mid360激光雷达（帧率10 FPS）以及腿式里程计（帧率500 FPS）。我们采用针对腿式机器人的改进版LIOSAM（类似于Leg-KILO）来提供真实值。LIO-SAM和腿式里程计的轨迹会被对齐，以此大致估算IMU与腿式里程计之间的外部参数。 ![Image](images/081e612d47bd93d15f73a86369286132af5f60c3a61ffbd75cd8ef7c6b38c77a.jpg)
Figure 5. The Go2 legged robot dog (left) and the G1 Humanoid Robot (right).
我们为足式机器人共收集了六组测试序列，其中四组来自机器狗，两组来自人形机器人，如图6所示。在Seq1中，机器人走进了室内办公室；Seq2记录了机器人在极具挑战性的无纹理走廊环境中的探索过程；Seq3展示的是地下停车场，其特点在于光线较暗且存在反射现象。为了进一步验证我们的算法在户外场景中的有效性，我们专门从开放式广场区域采集了数据集，该区域的地面纹理重复性较强，给定位带来很大挑战，如Seq4所示。Seq5是在室内办公室记录的，而Seq6则是在户外记录的。 需要注意的是，所有序列在机器人运动过程中均出现了明显的快速运动抖动现象，从而导致图像模糊以及IMU数据失真。 ![Image](images/30374e715544146917692d2493089decf2ea8b6142c02f71b6c71058375f4357.jpg)
Figure 6. The Real-world challenge environments. Seq1-4 on Go2-legged robot dog and Seq5-6 on G1 humanoid robot
我们将GeoFlow-SLAM与VINS系列算法（VINS-RGBD (RGBD+IMU)和S-VIO (RGBD+IMU)）以及ORB-SLAM3进行了对比，因为它们属于VSLAM领域最重要的研究成果。绝对轨迹误差（ATE）结果列于表I中。
TABLE I. ATE(M) EVALUATION RESULTS ON Go2 D435I DATASET.
VINS-RGBDS-VIOORBSLAM3Ours
Seq10.83161.3621\0.0947
Seq2\\\0.1369
Seq3\\\0.6009
Seq4\\\0.4458
Seq50.32230.71210.29630.1709
Seq6\\\0.6235
TABLE II. RUNNING TIME(AVERAGE TIME PER-FRAME IN MS) ON 13TH GEN INTEL(R) CORE(TM) 19-13900H.
Seq1Seq2Seq3Seq4Seq5Seq6
Ours24.926.527.626.320.127.1
ORBSLAM3\\\\9.011.6
在Seq1中，我们的算法在定位精度上取得了显著提升，相比VINS-RGBD提升了88%，相比S-VIO提升了93%。此外，在Seq2、Seq3、Seq4和Seq6数据集上，我们的算法是唯一成功运行的算法，而其他方法均出现运行失败（需不断重置）的情况。在人形机器人数据集Seq5中，与ORB-SLAM相比，我们的算法定位精度提升了45%，且GeoFlow-SLAM的性能最佳。就运行时间而言，如表II所示，我们的方法达到了实时处理效率，每帧处理时间约为20-30毫秒。不过，由于引入了额外的观测数据，其计算效率相较于ORB-SLAM3难免会有所降低。在Seq6中，由于ORB-SLAM3在序列中途出现跟踪失败，因此仅采用成功跟踪的部分来计算运行时间。 此外，如图7所示，我们的方法与参考轨迹的一致性最高，端到端误差也最小。相比之下，VINS-RGBD和S-VIO的轨迹则存在跳变和漂移现象。图8展示了GeoFlow-SLAM在Seq2和Seq4这两组较长的室内外数据集上的深度映射结果。 --- ![Image](images/ca5d03d552873f9c5ea79c7f87b48f5d1ebf32e88603bb9cc9785fb8cc05fc82.jpg)
Figure 7. Estimated trajectory (colored) and ground truth (black) in the G1 Seq5 dataset.
![Image](images/cea3261c1ae3e99e8b3d2866452e299682cb4a2fbee0721a5bbc42e306ff0554.jpg)
Figure 8. Our global plane points feature map in Seq2 and Seq4.
这种卓越的性能主要归功于三个因素： 鲁棒的姿态初始化：由于足式机器人的运动特性，IMU数据经常会出现误差。如图9所示，Seq1-6中的原始IMU数据严重失真，机器狗的最大加速度可达40 m/s $^{2}$，人形机器人则达到20 m/s $^{2}$。 ![Image](images/00ee809ea1ed5d4655dabb49902a2c6179f0e8fbce8425f8d7d1f8ba2a6cd550.jpg)
Figure 9. The raw IMU acceleration of our legged robot dog in Seq1-4 and humanoid robot in Seq5-6.
VINS-RGBD和S-VIO在视觉和深度信息不可用时，会利用IMU进行预积分和姿态预测。这种失真可能会导致优化过程发散。与其他严重依赖IMU进行初始姿态估计的方法不同，即便没有IMU或IMU数据失真，通过Legged、PnP和GICP里程计仍能实现机器人姿态的精确初始化。 增强型双流光流跟踪：足式机器人独特的运动特性往往会导致图像模糊，这会严重影响特征跟踪的鲁棒性。不过，我们采用的双流光流方法能够利用先前的地图点和位姿信息，即便当前图像模糊也能实现有效的特征跟踪。通过投影先前的地图点，我们便能在此类条件下继续有效跟踪特征点。此外，增加视觉特征点的数量——尤其是在低纹理环境中——能大幅提升系统的精度。 需要注意的是，Seq2和Seq6数据集中包含行人、汽车等动态物体。在完成光流跟踪后，我们的算法会利用基础矩阵来过滤掉不符合运动约束的异常值。如图10所示，这一过程在一定程度上减轻了动态物体对算法的影响。 ![Image](images/feec342d4e352c3afe4ce0b6dd71ce6b652a1e154e1c744ae5f441eadf98f1cc.jpg)
Figure 10. Feature points tracking result based on dual-stream optical flow in Seq2 with pedestrians. The green points denote feature points, while the red points indicate the reprojected points onto the image.
紧密耦合的深度约束：在无纹理环境中，视觉特征点较为稀疏，导致位姿估计难度较大。通过将深度到地图约束与GICP约束相结合，我们的方法既提升了精度，又增强了鲁棒性。例如，在Seq2这一低纹理走廊场景中，视觉数据有限，但地面和墙壁等几何特征能提供宝贵的约束条件，从而提升鲁棒性。 实验验证采用了OpenLoris数据集，该数据集由轮式机器人在住宅、办公等各类室内环境中采集而成。这些环境存在纹理对比度低、动态物体多、光照不足等诸多挑战。实验结果会与VINS-Mono（单目+IMU）、VINS-RGBD（RGB-D+IMU）、ORB-SLAM3（RGB-D+IMU）、S-VIO（RGB-D+IMU）、VID-SLAM（RGB-D+IMU）以及DDIO-VIO（RGB-D+IMU）等几种成熟的VIO算法进行系统对比。各VIO方法的ATE值列于表III中。结果表明，通过引入光流、深度信息这类额外约束，我们的方法在大部分OpenLoris数据集上均展现出更强的鲁棒性和精度。 ---
TABLE III. EVALUATION RESULT ON OPENLOROS DATASET. THE BEST RESULT IS HIGHLIGHTED IN BOLD AND SYMBOL \ INDICATES FAILURE.
VINS-MonoVINS-RGBDORBSLAM3S-VIOVID-SLAMDDIO-VIOGeoFlow-SLAM
Home1-11.07600.45200.92900.40200.38200.40600.4575
Home1-20.54200.3790\0.31600.36200.34700.3775
Office1-10.23100.10100.06800.10100.06700.07800.0577
Office1-20.28900.12400.12300.09900.10800.07000.0700
Office1-30.15500.1540\0.15100.02200.10400.0291
Office1-40.39200.18600.25300.16400.12900.14400.1554
Office1-50.23800.2390\0.21200.20900.21400.2000
### B. 消融研究 为了评估所提算法的有效性，我们使用ScanNet数据集中的9个序列（50、59、84、106、169、181、207、580、616）以及TUM数据集中的7个序列（long_office_house、nostr_tex_far、nostr_tex_near、str_notex_far、str_notex_near、str_tex_far、str_tex_near），采用算法的不同配置开展了消融研究。研究在未使用IMU和足式里程计的情况下，于RGBD模式下对多种测试场景下的性能进行了全面评估。本研究采用的配置分类如下：r+o代表仅利用光流的版本，r+i表示仅采用GICP的版本，r+d指仅依赖深度到地图约束的版本，而r+o+i+d则表示启用所有功能的完整版本。如表IV所示，与RGB-D模式下的ORB-SLAM3相比，引入双流光流、GICP和深度到地图约束后，精度有了显著提升。 在ScanNet数据集上，r+o+i+d配置的平均ATE为0.0779米，相比ATE达到0.2638米的ORB-SLAM3，其精度提升了70%。值得注意的是，ORB-SLAM3在序列50、84、181、207和616中无法持续跟踪。相比之下，我们的算法配置（r+o、r+i、r+d以及r+o+i+d）在所有序列中均未出现任何跟踪失败情况。同样，在TUM数据集上，我们的所有配置（r+o、r+i、r+d以及r+o+i+d）都能成功跟踪全部序列而无故障，而ORB-SLAM3则在无纹理且无结构的场景——即nostr_tex_far、str_notex_far和str_notex_near中无法跟踪。此外，我们的所有配置在定位精度方面均有不同程度的提升。具体而言，r+o+i+d配置的ATE和相对轨迹误差（RTE）分别为0.0129米和0.0086米，相较于ORB-SLAM3分别提升了62.2%和36.7%。 从上述结果可以看出，引入双流光流、GICP初始化以及深度到地图约束能够有效提升系统的精度和鲁棒性。整合了所有这些约束的配置$(r+o+i+d)$表现最为出色。
TABLE IV. AVERAGE ATE AND RTE COMPARISON ON THE SCANNET AND TUM DATASETS. THE BEST RESULT IS HIGHLIGHTED IN BOLD
orbslam3r+or+ir+dr+o+i+d
ScanNetATE0.26380.24040.10550.07750.0779
RTE0.01010.00950.01000.00940.0090
TUMATE0.03410.01420.01560.01550.0129
RTE0.01360.00890.01060.01000.0086
我们进一步将所提算法与最先进的RGBD-SLAM方法STL-SLAM[^18]进行了对比分析。表V中的结果显示，我们的方法取得了更高的精度。
TABLE V. STL-SLAM, ORBSLAM3 COMPARISION OF ATE (M) ON TUM. {} INDICATE FAILURE
STL-SLAMORBSLAM3GeoFlow-SLAM
fr1/xyz0.01000.01050.0101
fr1/desk0.02800.01640.0162
fr3/cabinet0.0190\0.0500
fr3/large_cabinet0.0620\0.0601
fr3/str_notex_far0.0330\0.0116
fr3/str_notex_near0.0130\0.0126
fr3/str_tex_far0.01400.01020.0089
fr3/str_tex_near0.01100.00930.0094
### C. 极端困难场景实验 由于集成了深度到地图约束和双流光流跟踪机制，我们的算法在纹理稀少且结构复杂程度低的环境中表现出强大的鲁棒性。为了进一步验证该方法的有效性，我们在TUM数据集中的no_structure_notexture_far和nostructure_notexture_near这两个极具挑战性的场景下进行了额外测试。这些场景以缺乏纹理和结构为特征。结果表明，我们的算法在无纹理环境中具有极高的韧性和精确度。 在纹理稀疏且模式重复的情况下，传统的光流跟踪方法往往难以奏效。然而，我们的算法利用精确的位姿先验来减少错误的特征点匹配。即便在没有视觉观测的情况下，深度到地图的点面约束以及GICP相对位姿约束也能有效约束位姿估计，从而避免优化过程发散。nostr_notex_far和nostr_tex_near数据集的ATE指标分别为0.0308米和0.0467米。如图11所示，在nostr_notex_far场景中，我们的SLAM系统仍能构建全局平面特征图。 ![Image](images/0392b8cb2a90450a55ee05799a5561fb2e08d3c437186212db6986aa39a5664f.jpg)
Figure 11. Our global plane feature map in Nostr_notex_far.The left is the original image and the right is our global plane feature point cloud.
--- ### VI. 结论 在本研究中，我们提出了GeoFlow-SLAM——一种面向足式机器人的新型SLAM系统，它在特征点匹配、位姿初始化以及整体系统的鲁棒性和精度方面均取得了显著提升。具体而言，我们提出了一种将双流光流跟踪与先验地图点和位姿信息相融合的方法，从而增强了快速运动场景下的特征跟踪能力。此外，我们开发了一种结合IMU/足式里程计、帧间PnP和GICP的位姿初始化方法，即便没有IMU数据也能确保可靠的位姿估计。更进一步，通过紧密耦合几何GICP约束，我们的方法在视觉纹理稀少的环境中性能得到了显著改善。GeoFlow-SLAM在多个基准数据集上均展现了出色的综合性能。为了推动后续研究，我们将公开相关代码和足式机器人数据集。 在超越几何SLAM的基础上，我们未来的工作将聚焦于基于场景图的语义重建，从而为足式机器人提供结构化的、以物体为中心的表征。 ## 参考文献 [^1]: S. Zhang, L. Zheng, and W. Tao, 'Survey and Evaluation of RGB-D SLAM', IEEE Access, vol. 9, pp. 21367–21387, 2021, doi:10.1109/ACCESS.2021.3053188. [^2]: D. Caruso, J. Engel, and D. Cremers, ‘Large-scale direct SLAM for omnidirectional cameras’, in 2015 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Sep. 2015, pp. 141–148. doi: 10.1109/IROS.2015.7353366. [^3]: R. Mur-Artal and J. D. Tardos, 'ORB-SLAM2: an Open-Source SLAM System for Monocular, Stereo and RGB-D Cameras', IEEE Trans. Robot., vol. 33, no. 5, pp. 1255–1262, Oct. 2017, doi: 10.1109/TRO.2017.2705103. [^4]: T. Qin, S. Cao, J. Pan, and S. Shen, ‘A General Optimization-based Framework for Global Pose Estimation with Multiple Sensors’, Jan. 11, 2019, arXiv:1901.03642. doi: 10.48550/arXiv.1901.03642. [^5]: C. Campos, R. Elvira, J. J. G. Rodriguez, J. M. M. Montiel, and J. D. Tardós, ‘ORB-SLAM3: An Accurate Open-Source Library for Visual, Visual-Inertial, and Multimap SLAM’, IEEE Trans. Robot., vol. 37, no. 6, pp. 1874–1890, Dec. 2021, doi: 10.1109/TRO.2021.3075644. [^6]: C. Yu et al., 'DS-SLAM: A Semantic Visual SLAM towards Dynamic Environments', in 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Oct. 2018, pp. 1168–1174. doi: 10.1109/IROS.2018.8593691. [^7]: Z. Shan, R. Li, and S. Schwertfeger, 'RGBD-Inertial Trajectory Estimation and Mapping for Ground Robots', Sensors, vol. 19, no. 10, p. 2251, May 2019, doi: 10.3390/s19102251. [^8]: C. Li, Y. Zhang, Z. Yu, X. Liu, and Q. Shi, ‘A Robust Visual SLAM System for Small-Scale Quadruped Robots in Dynamic Environments’, in 2024 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Oct. 2024, pp. 321–326. doi:10.1109/IROS58592.2024.10802163. [^9]: G. Ou, D. Li, and H. Li, 'Leg-KILO: Robust Kinematic-Inertial-Lidar Odometry for Dynamic Legged Robots', IEEE Robot. Autom. Lett., vol. 9, no. 10, pp. 8194–8201, Oct. 2024, doi: 10.1109/LRA.2024.3440730. [^10]: R. A. Newcombe et al., ‘KinectFusion: Real-time dense surface mapping and tracking’, in 2011 10th IEEE International Symposium on Mixed and Augmented Reality, Oct. 2011, pp. 127–136. doi:10.1109/ISMAR.2011.6092378. [^11]: T. Whelan, J. McDonald, M. Kaess, M. Fallon, H. Johannsson, and J. J. Leonard, 'Kintinuous: Spatially Extended KinectFusion'. [^12]: T. Whelan, S. Leutenegger, R. Salas Moreno, B. Glocker, and A. Davison, 'ElasticFusion: Dense SLAM Without A Pose Graph', in 《机器人：科学与系统XI》，机器人：科学与系统基金会，2015年7月。doi: 10.15607/RSS.2015.XI.001。 [^13]: M. Labbé and F. Michaud, 'RTAB-Map as an Open-Source Lidar and Visual SLAM Library for Large-Scale and Long-Term Online Operation', J. Field Robot., vol. 36, no. 2, pp. 416–446, Mar. 2019, doi: 10.1002/rob.21831. [^14]: L. Qin, C. Wu, Z. Chen, X. Kong, Z. Lv, and Z. Zhao, ‘RSO-SLAM: A Robust Semantic Visual SLAM With Optical Flow in Complex Dynamic Environments’, IEEE Trans. Intell. Transp. Syst., vol. 25, no. 10, pp. 14669–14684, Oct. 2024, doi: 10.1109/TITS.2024.3402241. [^15]: Y. Zhu, R. Jin, T. Lou, and L. Zhao, 'PLD-VINS: RGBD visual-inertial SLAM with point and line features', Aerosp. Sci. Technol., vol. 119, p. 107185, Dec. 2021, doi: 10.1016/j.ast.2021.107185. [^16]: F. Shu, J. Wang, A. Pagani, and D. Stricker, 'Structure PLP-SLAM: Efficient Sparse Mapping and Localization using Point, Line and Plane for Monocular, RGB-D and Stereo Cameras', in 2023 IEEE International Conference on Robotics and Automation (ICRA), May 2023, pp. 2105–2112. doi: 10.1109/ICRA48891.2023.10160452. [^17]: P. Gu and Z. Meng, 'S-VIO: Exploiting Structural Constraints for RGB-D Visual Inertial Odometry', IEEE Robot. Autom. Lett., vol. 8, no. 6, pp. 3542–3549, Jun. 2023, doi: 10.1109/LRA.2023.3270033. [^18]: J. Dong, M. Lu, C. Chen, F. Deng, and J. Chen, 'STL-SLAM: A Structured-Constrained RGB-D SLAM Approach to Texture-Limited Environments', in 2024 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Oct. 2024, pp. 10850–10855. doi: 10.1109/IROS58592.2024.10801465. [^19]: C. Zhang, S. Gu, X. Li, J. Deng, and S. Jin, 'VID-SLAM: A New Visual Inertial SLAM Algorithm Coupling an RGB-D Camera and IMU Based on Adaptive Point and Line Features', IEEE Sens. J., vol. 24, no. 24, pp. 41548–41562, Dec. 2024, doi: 10.1109/JSEN.2024.3486556. [^20]: Z. Zhu et al., ‘NICE-SLAM: Neural Implicit Scalable Encoding for SLAM’, Apr. 21, 2022, arXiv:2112.12130. doi:10.48550/arXiv.2112.12130. [^21]: N. Keetha et al., 'SplaTAM: Splat, Track & Map 3D Gaussians for Dense RGB-D SLAM', Dec. 04, 2023, arXiv:2312.02126. doi:10.48550/arXiv.2312.02126. [^22]: X. Jiang et al., 'DDIO-Mapping: A Fast and Robust Visual-Inertial Odometry for Low-Texture Environment Challenge', IEEE Trans. Ind. Inform., vol. 20, no. 3, pp. 4418–4428, Mar. 2024, doi: 10.1109/TII.2023.3323680. [^23]: J. Lin and F. Zhang, 'R3LIVE: A Robust, Real-time, RGB-colored, LiDAR-Inertial-Visual tightly-coupled state Estimation and mapping package', Sep. 10, 2021, arXiv:2109.07982. doi:10.48550/arXiv.2109.07982. [^24]: C. Zheng, Q. Zhu, W. Xu, X. Liu, Q. Guo, and F. Zhang, 'FAST-LIVO: Fast and Tightly-coupled Sparse-Direct LiDAR-Inertial-Visual Odometry', in 2022 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Oct. 2022, pp. 4003–4009. doi: 10.1109/IROS47612.2022.9981107. [^25]: C. Zheng et al., 'FAST-LIVO2: Fast, Direct LiDAR–Inertial–Visual Odometry', IEEE Trans. Robot., vol. 41, pp. 326–346, 2025, doi:10.1109/TRO.2024.3502198. [^26]: D. J. Hyun, S. Seok, J. Lee, and S. Kim, 'High speed trot-running: Implementation of a hierarchical controller using proprioceptive impedance control on the MIT Cheetah', Int. J. Robot. Res., vol. 33, no. 11, pp. 1417–1445, Sep. 2014, doi: 10.1177/0278364914532150. [^27]: M. Hutter et al., ‘ANYmal - a highly mobile and dynamic quadrupedal robot’, in 2016 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Oct. 2016, pp. 38–44. doi:10.1109/IROS.2016.7758092. [^28]: T. Shan, B. Englot, D. Meyers, W. Wang, C. Ratti, and D. Rus, 'LIO-SAM: Tightly-coupled Lidar Inertial Odometry via Smoothing and Mapping', in 2020 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Oct. 2020, pp. 5135–5142. doi:10.1109/IROS45743.2020.9341176. [^29]: K. Koide, ‘small_gicp: Efficient and parallel algorithms for pointcloud registration’, J. Open Source Softw., vol. 9, no. 100, p. 6948, Aug. 2024, doi: 10.21105/joss.06948. [^30]: H. Wang, C. Wang, and L. Xie, ‘Lightweight 3-D Localization and Mapping for Solid-State LiDAR’, IEEE Robot. Autom. Lett., vol. 6, no. 2, pp. 1801–1807, Apr. 2021, doi: 10.1109/LRA.2021.3060392.