TinyNav：轻量级机器人导航系统的设计与实现

profgong

在机器人自主导航领域，系统的复杂度往往与其功能性成正比。传统的导航系统如ROS Navigation Stack虽然功能强大，但其庞大的代码库和复杂的配置流程给开发者带来了不小的学习成本。TinyNav项目的出现，为这一领域提供了一个全新的思路：用不到2000行的核心代码，实现一个完整的、可用于生产环境的机器人导航系统。

TinyNav（https://github.com/UniflexAI/tinynav）是由Uniflex AI团队开发并维护的开源项目，其设计理念是**"Tiny but Robust"。该系统专为Intel RealSense深度相机和NVIDIA Jetson平台优化，支持多种机器人平台，包括Unitree GO2四足机器人和LeKiwi轮式机器人**。项目采用模块化架构，将导航任务分解为感知、建图、规划三个核心模块，每个模块职责清晰，接口简洁，便于理解和二次开发。

1.1 核心创新点

与传统导航系统相比，TinyNav的创新之处在于其对深度学习技术的深度整合。在特征提取方面，系统使用SuperPoint网络进行关键点检测，相比传统ORB特征具有更强的鲁棒性，能够在光照变化和运动模糊场景下保持稳定的特征检测能力。在特征匹配环节，TinyNav采用LightGlue基于注意力机制的匹配网络，有效处理重复纹理和大视角变化带来的匹配难题，显著提升了匹配准确率。

对于场景识别任务，系统使用DINO v2提取全局描述子用于回环检测，无需预训练视觉词典即可实现高效的场景识别，大幅简化了系统部署流程。在地图表示方面，TinyNav支持**3D高斯散射（3D Gaussian Splatting）**技术，提供直观的地图可视化和编辑能力，使开发者能够直接在三维地图上标注目标点并规划路径。这些先进的神经网络模型显著提升了系统在复杂环境下的鲁棒性。

二、系统架构

TinyNav采用经典的感知-建图-规划（Perception-Mapping-Planning）三层架构，这种设计模式在机器人导航领域已被广泛验证。系统基于ROS2（Robot Operating System 2）构建，利用其发布-订阅机制实现模块间的松耦合通信。

2.1 整体数据流

系统的数据流动遵循以下路径。首先是传感器数据采集，Intel RealSense D435等立体相机持续输出左右目图像和IMU数据。接着感知模块处理对图像进行特征提取、立体匹配，估计相机位姿和深度信息。然后建图模块维护构建全局地图，执行回环检测和位姿图优化。随后规划模块决策基于局部地图和目标点生成可执行的运动轨迹。最后控制指令输出将规划结果转换为机器人可执行的速度命令。

2.2 核心模块职责

2.2.1 感知节点（Perception Node）

感知节点负责处理传感器原始数据，其主要任务体现在多个关键环节。首先是立体视觉里程计估计，该功能通过特征匹配技术计算相机在空间中的运动轨迹，为后续定位提供基础数据。其次是深度图生成，系统使用神经网络进行立体匹配，从双目图像中恢复场景的三维几何信息。第三是关键帧选择，基于运动阈值筛选关键帧，确保只保留对建图有价值的图像帧，避免冗余计算。最后是IMU-视觉融合，结合惯性测量单元数据提高位姿估计精度，特别是在快速运动或光照变化等视觉退化场景下保持稳定性。该模块是系统的基础，其输出质量直接影响后续建图和规划的效果。

2.2.2 建图节点（Map Node）

建图节点维护机器人对环境的全局认知，其核心功能体现在多个技术层面。首先是全局特征提取与匹配，系统使用DINO v2视觉Transformer模型提取场景描述子，实现高效的场景识别和回环检测。其次是局部束调整优化，通过同时优化相机位姿和3D地图点，最小化重投影误差，提升局部地图的几何一致性。第三是回环检测与闭环，当机器人重新访问已知场景时，系统能够识别并利用这一信息消除长期累积的定位误差。最后是全局路径规划，基于A*算法在已构建的地图上规划从当前位置到目标点的最优路径。该模块确保机器人能够在大范围环境中保持定位精度。

2.2.3 规划节点（Planning Node）

规划节点负责生成机器人的运动轨迹，其实现基于动态窗口法（Dynamic Window Approach, DWA），能够在满足机器人动力学约束的前提下，实时规避障碍物并向目标点移动。核心功能体现在多个处理环节。首先是局部占据栅格地图构建，使用光线投射算法将深度图转换为二维栅格表示，标记自由空间和障碍物区域。其次是ESDF距离场计算，为每个栅格计算到最近障碍物的欧几里得距离，为障碍物规避提供安全距离信息，使机器人能够保持安全裕度。第三是候选轨迹生成与评分，在动力学约束下生成多条候选轨迹，综合考虑目标朝向、障碍物距离、速度等多个因素进行评分。最后是最优轨迹选择，从候选集中选出得分最高的轨迹并输出相应的速度命令，驱动机器人执行导航任务。

三、视觉里程计与深度估计

感知模块是TinyNav系统的眼睛，负责从原始传感器数据中提取机器人的位姿和环境的几何信息。该模块的核心是立体视觉里程计（Stereo Visual Odometry），通过连续帧间的特征匹配和3D-2D对应关系求解相机运动。

3.1 深度学习驱动的特征提取

传统的视觉SLAM系统多采用手工设计的特征描述子，如ORB、SIFT等。TinyNav则采用了基于深度学习的特征提取方案，具体使用SuperPoint网络进行关键点检测和描述子提取。

3.1.1 SuperPoint特征提取器

SuperPoint是一个自监督学习的特征提取器，其优势在于多个方面。首先是光照鲁棒性，对光照变化具有更强的适应能力。其次是运动模糊抗性，能够处理快速运动导致的图像模糊。第三是重复性高，在不同视角下能够检测到相同的特征点。最后是描述子区分度强，特征描述子具有更好的区分能力。

在TinyNav的实现中，SuperPoint模型被转换为TensorRT格式以实现高效推理。系统在Jetson Orin平台上可以达到30Hz以上的特征提取速度，满足实时导航的需求。

profgong

3.2 特征匹配与位姿估计

在获得当前帧和参考帧的特征点后，系统使用LightGlue进行特征匹配。LightGlue是一个基于注意力机制的特征匹配网络，相比传统的最近邻匹配方法，其能够更好地处理重复纹理和大视角变化的场景。

3.2.1 LightGlue匹配网络

LightGlue的核心优势体现在多个方面。首先是自适应匹配，根据图像内容动态调整匹配策略。其次是上下文感知，利用全局信息辅助局部匹配。第三是高效推理，相比SuperGlue速度提升3-5倍。最后是鲁棒性强，对光照、视角变化具有更强的适应能力。

3.2.2 关键帧选择策略

关键帧选择策略对于视觉里程计的精度至关重要。TinyNav采用基于运动阈值的关键帧选择策略：

这种策略确保关键帧之间有足够的基线，从而提高三角化的精度，同时避免冗余帧的处理开销。

3.3 IMU-视觉融合

TinyNav引入了IMU-视觉融合机制，显著提升了俯仰角估计的精度。系统使用GTSAM（Georgia Tech Smoothing and Mapping）库实现因子图优化，将IMU预积分约束和视觉观测约束统一在一个优化框架中。

IMU预积分的实现基于GTSAM的PreintegratedCombinedMeasurements类，该类能够高效地处理IMU测量并计算预积分量。

每个关键帧不仅存储图像和位姿信息，还保存了IMU的预积分结果和偏置估计。这种设计使得系统能够在视觉跟踪失败时，利用IMU信息进行短时预测，提高系统的鲁棒性。

profgong

建图模块负责维护机器人对环境的全局认知，其核心任务是在大范围环境中保持定位精度，并通过回环检测消除累积误差。TinyNav的建图模块采用了基于特征的SLAM方法，结合深度学习技术实现高效的场景识别和位姿优化。

4.1 全局特征提取与场景识别

传统的回环检测方法多基于词袋模型（Bag of Words），需要预先训练视觉词典。TinyNav采用了更先进的方案：使用DINO v2（Vision Transformer）提取全局图像描述子。DINO v2是Meta AI开发的自监督视觉模型，其提取的特征具有强大的语义表达能力，能够识别不同视角下的同一场景。

在TinyNav的实现中，每个关键帧都会提取一个全局描述子，用于后续的相似度计算

这个函数实现了标准的位姿图优化流程，通过最小化所有边约束的误差，得到全局一致的轨迹估计。