增强型视觉SLAM与轮式移动机器人自主导航路径规划

摘要

本研究提出了一种通过优化视觉SLAM和路径规划算法来提升轮式移动机器人（WMR）自主室内导航性能的方法。该方法融合了多传感器信息，增强了特征提取能力，并应用轨迹优化技术，从而实现了更精确的定位、更有效的障碍物规避以及更平滑的运动路径，在真实环境和模拟环境中均表现出优越的性能。

摘要

本研究聚焦于轮式移动机器人自主导航中使用的关键技术，如路径规划优化、系统集成以及视觉同步定位与建图（SLAM）技术的进展。为解决传统视觉里程计因特征点重复或分布不均而导致的定位问题，提出了一种改进方法。该方法结合了高效透视n点（EPNP）特征匹配、迭代最近点（ICP）位姿优化以及基于四叉树的特征点管理策略。实验结果表明，所提出的方法显著提高了定位的精度与稳定性。为提升环境表达的完整性与细节水平，同时缓解传统SLAM系统生成的点云地图中常见的稀疏性问题，开发了一种基于RGB-D数据的稠密点云重建技术。为进一步提升路径质量与计算效率，提出了一种改进的快速探索随机树（RRT）方法，该方法融合了自适应步长管理、目标偏向策略以及基于B样条的路径平滑技术。此外，通过集成定时弹性带（TEB）算法，实现了在动态环境中的实时局部避障。在基于机器人操作系统（ROS）的实验平台上实现后，全面的实际测试验证了所提出方案在效率、鲁棒性及实际应用性方面的有效性。

引言

机器人技术的潜力和应用模式正在经历快速变革，这主要得益于人工智能技术的进步。近年来，视觉同步定位与建图（Visual SLAM）及其扩展形式——视觉惯性导航系统（VINS）在鲁棒性和定位精度方面取得了显著进展¹。为了提高在低纹理和光照条件差等挑战性环境下的初始化可靠性，Campos 等人提出了 ORB-SLAM3，该方法引入了多地图系统，并改进了视觉与视觉-惯性系统的初始化过程²。为在复杂场景中实现更优的特征匹配，DeTone 等人开发了 SuperPoint，一种自监督的兴趣点检测与描述方法³；而 Sarlin 等人则提出了 SuperGlue，这是一种基于图神经网络的特征匹配器，能够应对恶劣的视觉条件⁴。在稠密三维重建方面，Dai 等人提出了 BundleFusion，这是一种实时全局一致的三维重建系统，通过在线表面重新融合技术来处理大规模环境及闭环问题⁵。

在路径规划领域，快速扩展随机树（Rapidly-exploring Random Trees, RRT）及其变体仍被广泛用于机器人运动规划。基础RRT算法最初由LaValle提出，作为一种新的路径规划工具，为求解复杂的高维问题提供了一种高效的基于采样的方法⁶。Karaman和Frazzoli对此进行了重大改进，提出了RRT*算法，该算法在运动规划中具备渐近最优性保证⁷。在这些核心算法的基础上，现代研究聚焦于将基于采样的方法与其他技术相结合的混合方法。例如，Rösmann等人开发了时间弹性带（Timed Elastic Band, TEB）方法，可实现局部最优轨迹生成，并已广泛与全局规划器集成⁸。类似地，Fox等人提出的动态窗口法（Dynamic Window Approach, DWA）为动态环境中的局部避障提供了有效手段⁹。

在局部规划与语义感知层面，Chen 等人提出了一种面向微型飞行器（MAVs）的语义感知型信息路径规划策略，显著提升了目标探索过程中的搜索效率与安全性¹⁰。Kabiri 等人将5G到达时间（ToA）测量数据集成至VINS框架中，实现了全局与局部SLAM的融合，有效提高了在GNSS覆盖受限环境下的定位精度¹¹。为实现高频实时建图，Xu 等人开发了FAST-LIO2方法，这是一种紧耦合的激光雷达-惯性里程计方法，能够生成精确且稠密的三维地图¹²。针对复杂环境中的路径规划，Gammell 等人提出了一种改进的RRT*方法，该方法引入双向树生长与自适应采样机制，显著提升了动态环境中路径质量与搜索效率¹³。此外，在狭窄通道场景下，Coleman 等人提出了一种基于采样的运动规划方法，采用可变概率采样策略，提高了规划成功率与计算效率¹⁴。

本研究通过改进路径规划策略和SLAM前端，解决了轮式移动机器人（WMRs）在室内自主导航中的基本挑战。具体而言，所提出的系统针对典型的结构化室内环境（如实验室和走廊）设计，适用于光照适中且全球导航卫星系统（GNSS）信号受限的条件。该导航系统主要采用立体RGB-D相机、惯性测量单元（IMU）和轮式编码器，所有传感器均配置为采样频率不低于20 Hz。为确保系统运行的可靠性，机器人的最大速度被限制在1.5 m/s以下。主要贡献如下：

开发了一种用于轮式移动机器人（WMRs）的多传感器融合自主导航平台，采用深度相机作为主传感器。为在典型室内环境中实现精确定位和高效避障，该系统融合了轮速里程计和惯性测量单元（IMU）。这些组件之间的协同作用对提升整体导航性能具有关键作用。

将EPnP和ICP算法与基于四叉树的特征提取技术相结合，有助于提升ORB-SLAM2中跟踪模块的性能。这些改进带来了更高的跟踪精度和鲁棒性。

提出了一种强调轨迹优化的新型路径规划方法。该方法基于一种改进的RRT技术，采用目标偏向策略和可调步长，并利用B样条曲线进行轨迹平滑。同时引入TEB算法，以实现动态环境中的避障管理。

通过真实环境测试和仿真验证了该系统的性能。典型的室内环境可实现对地图精度、路径质量和导航性能的定量与定性分析。在鲁棒性、实时处理和轨迹平滑性方面，所提出的方法优于现有解决方案。

方案

1. 硬件平台

1. 准备适用于室内导航的两轮差速驱动移动机器人平台（见图1）。该平台采用两个沿底盘中心线布置的独立驱动轮，并在前后端安装万向 caster 轮，以确保机械平衡性和机动性。
2. 将差速驱动轮安装在底盘的中央纵向轴线上。使用六角螺丝刀将轮轴与电机轮毂对齐并紧固。确保车轮牢固安装，但能自由旋转且无轴向晃动。检查两个车轮是否精确对齐，以保证直线运动和准确的里程计性能。
3. 在底盘两端安装前、后万向 caster 轮，以在转向过程中提供机械支撑。对齐不良可能导致高速转向时出现不稳定或倾斜现象。
4. 在底盘前上方面板安装结构光深度相机。使用可调节支架或粘接式安装件将相机牢固固定。调整其朝向，使视场覆盖机器人前方约 0.3 m 至 3.0 m 的范围。
5. 将红外投影仪和接收模块接入相机外壳，确保所有光学中心正确对齐。调节相机的俯仰角以优化深度感知性能。
6. 使用可调节支架将相机向下倾斜 15°–30°。确保底盘任何部分均不遮挡投射的红外图案。该角度有助于捕捉近场地形特征并避免盲区。
7. 使用可视化软件（如 RViz，版本 1.14.1）验证相机的实时深度输出。启动相机节点并观察深度图像流。将深度相机连接至安装在底盘中央的微控制器单元（MCU）。
   注意：所有连接操作期间务必断开电源。保持线缆整齐并远离运动部件，以防运动过程中发生缠绕。

2. 室内建图中 ORB-SLAM2 的优化

1. 准备 ORB-SLAM2 环境。使用标准的 ROS 校准工具对相机（RGB-D）进行校准。配置启动文件以指定相机话题、分辨率（例如 640 x 480）和帧率（例如 30 fps）。通过以下命令启动 SLAM 系统：xtark@tarkbot: $ roslaunch robot_platform slam map.launch slam _methods:=gmapping。在终端中验证实时相机画面及 SLAM 初始化信息。运动开始后应出现关键帧。
2. 修改 ORB-SLAM2 以支持稠密建图。扩展默认的建图模块，增加一个稠密重建线程，用于处理来自关键帧的深度数据。
3. 对于每个选定的关键帧：提取同步的 RGB 图像和深度图像，利用相机内参将深度像素转换为三维点，并使用位姿信息融合跨关键帧的累积点云。将包含多个关键点的区域递归划分为四个象限，直到每个叶节点最多包含一个主导关键点，或区域尺寸小于 10 x 10 像素为止。
4. 使用四叉树增强特征分布（见图 2）。修改 ORB 特征提取模块，引入基于四叉树的空间划分策略。将图像划分为层次化的网格区域，在每个区域内应用 FAST 角点检测，并仅保留每个区域内最显著的特征，以确保空间覆盖均匀。
5. 从每个有效区域中，选择显著性响应最高的候选点作为代表特征。
6. 使用 EPnP 提升位姿估计精度。将默认的位姿估计方法（例如迭代方法）替换为基于 OpenCV 的 solvePnP 实现的高效透视 n 点（EPnP）算法。利用 2D 图像特征及其对应的 3D 地图点求解相机位姿。
7. 部署、可视化并控制机器人。为机器人机载系统分配静态 IP 地址以实现稳定通信（例如 机器人IP：172.20.10.13）。在主机 PC 上打开 RViz（v1.14.1），加载配置文件以可视化机器人的轨迹、稀疏与稠密点云地图、关键帧以及检测到的特征。
8. 使用键盘方向键手动控制机器人在空间中移动以完成建图。确保轨迹线在 RViz 中显示，且相机位姿坐标系实时更新。
   注意：图 3 展示了建图过程中用于手动控制机器人的键盘布局。

使用四叉树算法进行特征点处理

1. 按照以下方法执行 ORB 特征提取。
   1. 使用 OpenCV（版本 4.5.3）从 ROS 图像主题或本地数据集加载输入图像。
   2. 构建具有四个层级的高斯金字塔，将图像划分为均匀的网格单元（每个层级分为 8 × 8 个单元）。在每个单元内，使用阈值为 20 的 FAST 检测器识别局部关键点。
2. 构建基于四叉树的特征优化方法，如下所述。
   1. 对于给定金字塔层级上的每组关键点，构建四叉树结构：以整幅图像作为根节点。递归地将包含多个关键点的区域划分为四个象限，直到每个叶节点最多包含一个主导关键点，或区域尺寸小于 10 × 10 像素为止。
3. 应用特征显著性评估，如下所述。
   1. 使用以下公式评估每个节点内候选关键点的显著性：
           (1)
      其中 I_p 表示局部邻域中心像素的强度值，I_i 表示其 16 个相邻像素的强度值。绝对差值 |I_p - I_i| 衡量中心像素与每个邻域像素之间的局部对比度。对全部 16 个邻域像素求和，可反映中心像素周围的整体局部对比度或纹理强度。
   2. 使用按显著性得分排序的动态优先级队列对所有候选点进行排序。从每个有效区域中选择显著性响应最高的候选点作为代表特征。
4. 优化并验证特征选择
   1. 整合所有金字塔层级上所选的特征，确保图像中空间覆盖均匀。使用与 OpenCV 版本一致的 ORB 描述符提取器存储最终的特征点及其描述符。
   2. 验证特征未集中在图像的少数区域。特征点应表现出均匀的空间分布，以支持鲁棒的跟踪性能。避免在物理机器人系统运动过程中执行图像处理。确保相机图像流稳定，且工作区域已清理无遮挡。

4. 使用EPnP进行姿态估计

1. 通过选择至少四对匹配的三维地图点（在世界坐标系中）及其对应的二维图像关键点，建立二维-三维对应关系。确保这些对应关系是从跟踪线程中获得的有效 ORB 特征匹配中提取的。
2. 使用 EPnP 求解初始位姿。持续进行，直到每个叶节点最多包含一个主导关键点，或区域尺寸小于 10 × 10 像素。使用 OpenCV 的 solvePnP 函数并设置 cv::SOLVEPNP_EPNP 标志来估计相机位姿。

5. 使用ICP进行精细位姿优化

1. 执行如下所述的点云采样。
   1. 对源点云进行下采样，以降低计算负载并去除冗余数据。
   2. 采用均匀采样，确保在所有方向上均匀保留结构特征。根据输入点云的密度和噪声特性，必要时可应用体素网格滤波或随机选择方法。确保滤波后的点云在将总点数减少至少50%的同时，仍保留物体轮廓。
2. 通过从目标点云构建KD树，实现高效的最近邻搜索，从而匹配对应点。对于下采样后的源点云中的每个点，利用KD树在目标点云中查找其最近点。确保点匹配的准确性，因为此步骤对配准性能具有关键影响。
3. 估算最优变换，具体如下所述。
   1. 利用匹配的点对计算刚体变换矩阵，包括旋转和平移部分。
   2. 通过奇异值分解（SVD）对互协方差矩阵进行分解，最小化均方误差（MSE），从而计算匹配点对之间的最优刚性变换，直接得到旋转矩阵，随后基于旋转后的质心计算平移向量。
4. 将计算得到的变换应用于源点云，并更新所有点的坐标。迭代重复点匹配和变换估算过程。持续迭代，直至配准误差低于预设阈值，或达到最大迭代次数为止。

6. 密集点云地图构建

1. 构建稠密的三维点云地图，以实现对室内环境的精确且详细的表示。请遵循以下步骤（见图4）。
2. 从关键帧中提取RGB和深度数据。根据视觉丰富度和空间覆盖范围选择关键帧。从每个选定的关键帧中，提取RGB图像以及来自RGB-D传感器的对应对齐深度图。
3. 将图像像素转换为三维相机坐标。对于每个有效的深度像素，使用相机内参将二维像素投影到三维空间中。该过程生成相机坐标系下的三维坐标。
4. 将相机坐标变换至世界坐标系。从ORB-SLAM2中获取每个关键帧的优化后相机位姿。利用相机位姿将三维相机坐标变换至世界坐标系，使所有点云对齐于统一的全局参考系中。
5. 生成着色的三维点。为每个变换后的三维点，分配其在原始图像中对应的RGB值。由此得到一个包含几何结构与外观信息的彩色点云。
6. 合并所有关键帧的点云。将所有已变换并着色的点云累积成一个统一的全局点云地图。使用各关键帧对应的相机位姿确保正确对齐。
7. 使用PCL注册并优化最终地图。利用点云库（Point Cloud Library, PCL）对最终地图进行优化。应用滤波以去除噪声，并进行下采样以提高效率。如有必要，执行全局配准（例如使用ICP算法）以进一步优化点云之间的对齐（见图5）。
   注意：如图6所示，在稠密建图初始化阶段，由于观测数据有限，初始点云对齐可能出现短暂的错位，但随着更多视角的加入，对齐结果会迅速收敛。通过控制机器人遍历环境，可获得完整的三维模型。

7. 从VSLAM生成的点云生成占据栅格地图

1. 对全局稠密点云进行下采样。采用体素分辨率为 0.05 m 的体素网格滤波方法，以减少冗余并确定栅格构建的空间分辨率。
2. 将三维点投影到二维占据栅格中。将所有三维点投影到水平（x-y）平面上。将空间离散化为均匀的栅格单元，每个单元在真实世界中代表一个 0.05 m × 0.05 m 的正方形区域。
3. 估计占据概率。利用逆向传感器模型，根据点云密度和模拟光线投射结果计算每个栅格单元的占据概率。
   1. 将占据概率阈值设为 0.65，自由概率阈值设为 0.35。将概率值介于两者之间的栅格单元分类为未知区域。
4. 应用障碍物膨胀。通过应用半径为 0.2 m 的圆形核对占据区域进行膨胀，以考虑机器人的安全距离和避障余量。
5. 导出占据栅格地图。将生成的占据栅格地图以便携式灰度图（Portable GrayMap）格式保存，并附带相应的 m.yaml 元数据文件，以确保与基于 ROS 的导航系统兼容。

8. 改进的全局路径规划策略（基于RRT算法）

1. 初始化路径树。将机器人的起始位置设为树的根节点。在构型（状态）空间中随机采样点以探索新区域。
2. 确定最近的已有节点。对于每个新采样的随机点，计算其到所有已有节点的欧几里得距离。选择距离最小的节点作为最近节点，作为扩展的基础。
3. 向随机采样点生成新节点。从最近节点指向采样点创建一个方向单位向量。沿此方向移动一个固定步长（初始阶段）以形成新节点，并将其连接到树中。
4. 用自适应机制替代固定步长。不再使用恒定步长，而是根据局部障碍物密度动态调整步长。在开阔环境中使用较大步长以加速树的扩展；在杂乱或狭窄区域减小步长，以提高控制精度和避障能力。
5. 按以下方式实时计算自适应步长：
   1. 利用传感器数据（例如激光雷达或深度相机）估计当前区域周围的障碍物密度。
   2. 若检测到的障碍物数量较少，则略微增大步长；若障碍物密集，则按比例减小步长，以插入更多中间节点，确保安全通过。
6. 迭代扩展过程。继续使用自适应步长进行采样、最近节点搜索和新节点生成。
7. 应用B样条曲线进行平滑处理。将RRT路径中的折线段替换为连续的B样条曲线以提高平滑性。在原始RRT路径上选择控制点，通常位于转弯点或关键路径点处。按顺序连接这些控制点构建控制多边形。
8. 生成B样条曲线。使用标准B样条公式¹⁵：
        (2)
   该公式用于B样条曲线中，其中最终曲线 C(u) 是控制点的加权组合。权重由B样条基函数 N_i,k (u) 确定，确保曲线平滑并遵循控制点定义的整体形状。
9. 将曲线次数设为3（三次），以保证连续性（光滑的一阶和二阶导数）。使用在 PyCharm 2024.3 中编写的路径规划模块。

9. 采用改进型TEB的局部轨迹优化

1. 引入如下所述的最短距离约束。
   1. 为缓解这些缺点，将最短距离约束整合到 TEB 框架中。
   2. 将该约束定义为机器人当前位置 S_t 与轨迹上未来位姿 S_i+n 之间的欧几里得距离：
           (3)
      该约束通过鼓励路径保持在全局路径走廊边缘附近，惩罚低效的偏离行为，从而提升规划质量与安全性。
2. 将约束项融入 TEB 代价函数中，通过修改原始 TEB 优化图，将距离约束作为额外边加入。调整总代价函数，增加对 f_os 的加权项，以平衡路径的平滑性、可行性与能量效率。
3. 将约束项融入 TEB 代价函数中。在优化过程中，求解使总代价最小的轨迹点，总代价包括速度、加速度、避障距离以及新增的最短距离项。利用 TEB 内置的求解器，在 N 个时间区间上迭代优化轨迹。在考虑该约束的前提下优化路径（见 图 7）。

结果

改进的 ORB-SLAM2 评估
特征提取实验
为评估 RGB-D 深度相机在实际场景中的有效性，开展了一项特征点提取实验。该测试采用两种不同的背景环境设计，每种环境在物体颜色和亮度上有所变化，以模拟真实世界中的视觉复杂性。

将所提出的改进提取方法与传统的基准方法应用于相同的测试条件。记录并分析提取的特征点数量及其空间一致性，作为主要性能指标。

如表1所示，提取的特征点的数量和空间一致性被记录并分析为主要性能指标。改进后的方法使识别出的特征点比例提高了25.95%，特征点分布的均匀性提升了24.36%。实验结果如图8所示，展示了两种方法的对比性能。值得注意的是，在光照和颜色条件变化的情况下，改进后的方法获得了更多的特征点，并表现出更强的稳定性。

完成时间说明：特征提取为实时执行。在2.4 GHz四...

讨论

本研究聚焦于轮式移动机器人自主室内导航系统的两项关键技术：视觉同步定位与建图（SLAM）^16,17和路径规划¹⁸。SLAM 模块提出一种基于四叉树的分层选择方法，以校正 ORB-SLAM2 中特征点分布不均的问题。为提高所生成地图的精度，采用了异步稠密建图方法。实验结果表明，所提出的方法显著提升了定位稳定性与建图精度，特征点提取准确率提高了 25.95%。

为克服传统快速扩展随机树（RRT）方法在路径规划中收敛速度慢、路径平滑性不足的问题¹⁹，提出了一种融合目标偏差与自适应步长的新型采样技术。该方法还采用三次B样条函数进行轨迹优化，并利用时间弹性带（TEB）技术实现动态环境下的避障。与传统RRT方法相比，实验结果表明，所提出的方法有效提升了轨迹质量与规划效率，运行时间缩短了8.15%，节点数量减少了85.70%，路径长度降低了12.11%。

材料

本文使用的材料清单

姓名	公司	目录编号	评论
Astra Pro Plus 3D相机	CRBBEC	无	3D相机
TARKBOT-R20-TWD	无	无	ROS机器人