面向激光SLAM的自主导航方法

Autonomous navigation method for laser SLAM

摘要

复杂环境下的移动机器人自主导航高度依赖于环境地图，由于地图构建的精度依赖于系统优化算法和配套传感器性能，而路径规划又需协同全局规划与局部规划两个核心环节。因此，以同步定位与地图构建（Simultaneous Localization and Mapping，SLAM）理论为基础，搭建了一套移动机器人实验平台，提出了一种基于激光SLAM的自主导航方法，并针对路径规划中的局部规划模块，设计了时间最优与图优化相结合的优化策略。为验证系统性能，开展了仿真实验和实体实验，重点对机器人传感器校准精度、环境地图构建精度及局部路径规划合理性等关键指标进行测试。结果表明，所设计的移动机器人能够精准地完成地图构建与路径规划任务，为后续机器人在林区作业场景中的应用提供了理论支撑。

Abstract

The autonomous navigation of mobile robots in complex environments relied heavily on environmental maps， the accuracy of map construction depended on system optimization algorithms and the performance of supporting sensors， the path planning also needed to coordinate two core links：global planning and local planning. Therefore， based on the theory of Simultaneous Localization and Mapping （SLAM）， an experimental platform for mobile robots was built， an autonomous navigation method based on laser SLAM was proposed， and an optimization strategy combining time optimality and graph optimization was designed for the local planning module in path planning. To verify the system performance， the simulation experiments and physical experiments were conducted， and the key indicators such as the calibration accuracy of robot sensors， the accuracy of environmental map construction， and the rationality of local path planning were tested with emphasis. The results showed that the designed mobile robot could accurately complete the tasks of map construction and path planning， which provided a theoretical basis for the subsequent application of robots in forest operation scenarios.

Graphical abstract

关键词

激光SLAM / 机器人操作系统 / 路径规划 / 自主导航 / 同步定位与地图构建

Key words

laser SLAM / robot operating system / path planning / autonomous navigation / simultaneous localization and mapping

0 引言

近年来，随着科学技术与人工智能的高速发展，移动机器人在各领域发挥着愈发关键的作用，其中具备自主作业能力的移动机器人已广泛应用于火灾巡检、信息采集、救援救灾和物资运输等场景^［1-5］。在移动机器人研究领域，美国研发的PR2^［6］移动机器人配备相机、激光雷达与机械臂，可自主完成运动、充电和开关门等操作；本田公司设计的人形机器人实现了行走、跳跃和爬楼梯等复杂动作^［7］。SCHIFFER团队^［8］借助激光雷达技术，成功实现机器人的局部避障功能。本田公司后续进一步研发的类人形双足机器人，采用双足结构替代传统轮式底盘，以适配更多应用场景^［9］；SPOTMINI^［10］机器人则通过融合视觉传感器与低成本感知设备，为机器人相关功能的实现提供多维度数据支撑。国内研究同样成果显著，由华中科技大学研发的HRMRP机器人与复旦大学研发的RoboWay R1机器人均搭载了GPS和激光雷达等传感器，可实现无人化自主配送任务^［11］。

多数机器人的自主导航与定位高度依赖GPS辅助，该方式仅在GPS信号良好的室外开阔环境中表现优异，在有遮挡物（如楼宇、树木）的场景下难以满足机器人对导航定位精度与稳定性的需求。针对这一技术难点，以激光雷达为核心感知传感器，融合惯性测量单元（IMU）与里程计数据，本文提出一种不依赖GPS的自主导航方案，不仅可在室内场景下实现机器人自主导航，同时适用于室外有遮挡的复杂场景。

本研究在Linux操作系统环境下，基于机器人操作系统（Robot Operating System，ROS） 的分布式处理框架，搭建了轮式移动机器人实验平台；通过激光雷达数据与IMU、轮式里程计数据的多源融合，构建了机器人周围环境的高精度地图；同时，通过对路径规划算法的优化与验证，最终实现了机器人的全流程自主导航。实验设置了仿真与模拟两类测试场景，结果表明所设计的移动机器人能够精准地完成地图构建与路径规划任务，可为复杂场景下机器人自主作业提供可靠技术方案。

1 机器人平台的整体设计

设计的机器人平台结构如图1所示，整体采用分层架构，主要包含信息处理层、感知层与驱动层3个核心模块，各模块功能明确且协同工作，共同支撑机器人自主运行。

驱动层承担机器人运动控制功能，具体配置包含2台100 W直流电机、1个8 in气胎驱动轮及2个6 in实心橡胶轮（用于辅助支撑与转向调节）；该层通过1块STM32单片机实现对直流电机的精准驱动控制，包括转速调节、正反转切换等，为机器人提供稳定的运动动力。

感知层作为机器人的“感知器官”，核心作用是采集机器人自身状态数据与周围环境信息，主要搭载三类感知设备：轮式里程计（用于获取机器人位移与速度信息）、激光雷达（用于探测环境障碍物及距离信息）和惯性测量单元（IMU，用于采集机器人姿态角、加速度等运动状态数据），3类设备的数据共同为后续地图构建与导航定位提供基础支撑。

信息处理层是机器人的“核心大脑”，采用工控机作为硬件载体，主要负责运行机器人操作系统（ROS），并完成三大关键任务：一是解析感知层传输的多源数据，实现数据融合与环境感知；二是处理驱动层的控制指令，生成电机驱动信号；三是运行建图、导航算法与人机交互程序，最终实现自主建图、路径规划与实时导航功能。在数据通信方面，底层STM32控制板与工控机之间通过Rosserial协议建立通信连接，确保控制指令与状态数据的实时传输；其中，STM32与IMU的通信采用IIC（Inter‑Integrated Circuit）协议，保障姿态数据的高效采集。激光雷达与摄像头则通过USB接口直接与工控机连接，简化硬件接线，同时提升数据传输稳定性。此外，机器人车身搭载12 in显示器，可实时展示地图构建、导航路径及机器人当前位置。

2 参数校准

惯性测量单元（IMU）与里程计在实际应用中，受制作工艺、数据采集方式差异等因素影响，其输出数据存在一定误差。为获取更准确的感知数据以保障后续导航与建图精度，需要对传感器的误差参数进行系统辨识，进而实现传感器数据的校准与修正。

2.1 IMU离线校准

选用微机电系统惯性测量单元（MEMS‑IMU）完成机器人姿态角的估计任务^［12］。相较于传统惯性测量单元，MEMS‑IMU具备成本低、体积小、重量轻的优势，同时拥有较强的可靠性与抗干扰能力，能够适应移动机器人复杂的作业环境，满足其对姿态角信息实时、稳定采集的需求。

受工艺制造精度限制及电子器件温度漂移等因素影响，MEMS‑IMU在数据输出过程中会产生随机误差。为实现对MEMS‑IMU随机误差的精准分析，Allan方差法成为核心技术手段。该方法于2003年被引入IMU随机误差分析领域，凭借其对时域信号的优异解析能力，在后续研究中得到广泛应用^［13］。其核心原理是将随机误差视为时间序列，通过计算不同时间间隔下的均方差，构建Allan方差曲线，进而精准识别随机误差的来源与特性^［14］。

2.2 IMU在线校准

在真实环境中，移动机器人对IMU的校准方式提出“简便化、高效化和精准化”的需求，通过低成本、易操作的流程快速获取IMU的误差系数，以保障后续位姿估计与导航精度。为此，设计了适用于移动机器人实际应用场景的IMU在线校准方案。该方案对加速度计的校准无需复杂的外部设备，仅需在机器人静止状态下即可完成。操作时，将机器人分别调整至加速度计坐标系的6个正负方向（即沿X轴正方向、X轴负方向、Y轴正方向、Y轴负方向、Z轴正方向、Z轴负方向），并在每个方向保持静止姿态；随后以50 Hz的采样频率采集该姿态下加速度的原始数据，每个方向采集1000个数据样本，通过多组静态数据的统计分析与误差建模，实现加速度计零偏、刻度系数及安装误差的校准。

2.3 线速度校准

所采用的移动机器人为非全向移动三轮结构，便于在林区较狭窄环境中实现灵活转向。在位移计算中，仅考虑X轴方向（即机器人前进/后退方向）的运动，其位移信息主要依靠轮式里程计获取。机器人搭载了AB相输出的500线光电编码器，在STM32单片机控制系统中，将定时器分频设置为0，在软件中实现对AB相信号的四倍频计数处理，即旋转一周可产生2000个脉冲。基于该脉冲计数，计算一段时间内车轮转过的圈数后，即可求得机器人在该时间段内的移动距离。

采用在线校准模式，通过脚本控制机器人运动轨迹，实时可视化调整速度参数。当实际距离与标准距离的误差收敛至1 cm时，校准完成。实验中采用1 m作为标准距离参考值，以机器人的轮中心为运动参考点，沿直线匀速移动，停止后立即测量实际位移，最终完成校准。共采集20组实验数据，分别为校准前10组、校准后10组，并分析每组数据的误差值。结果显示，校准前机器人位移平均误差为29.58 cm，校准后平均误差降至0.68 cm，位移控制误差显著降低。

2.4 角速度校准

采用与线速度类似的校准方法，通过调整实际旋转角度与额定旋转角度的比例系数，实现对旋转运动精度的校准优化。结果表明，校准前的平均绝对误差达22.39°，偏差较大；经校准后，平均绝对误差降至2.98°，误差显著降低。

3 实验分析

3.1 仿真建图与分析

为验证自主导航方案的可行性，本实验基于ROS系统，结合Gazebo三维物理仿真环境与Rviz三维可视化工具，搭建了移动机器人本体模型与模拟机器人作业场景的周边环境^［15］。采用长方形车体结构，尺寸设定为长60 cm、宽45 cm，包括2个驱动轮和1个万向轮。构建尺寸为长18 m、宽10 m的开放式场景，在场景内布置桌子、行人（动态障碍物）和柜子（静态障碍物）等元素。该仿真环境的俯视图如图2所示。

仿真机器人的驱动控制基于ROS中的Arbotix功能包实现，可稳定支持差速驱动控制及PID速度调节等核心需求。机器人各结构部件的连接关系如图3所示，各节点通过连轴器完成机械关联；同时，各部件的局部坐标系可借助ROS中的TF（Transform）坐标变换机制实现实时转换。此外，机器人车体上方搭载的二维激光雷达，为仿真场景下的自主导航提供关键环境感知能力。

在搭建的仿真环境中（图4），基于Gmapping算法实现激光SLAM地图构建。作为滤波框架下的2D激光SLAM方案，Gmapping算法适用于低特征密度、小范围场景的建图任务，其核心技术依托RBPF（Rao‑Blackwell粒子滤波）^［16］，并通过scan_match （扫描匹配）策略进行优化，可快速收敛至局部最优解，有效减少粒子耗散，提升建图效率；同时，该算法对激光雷达的采样频率要求较低，仅需依赖激光雷达数据与轮式里程计数据即可完成建图。不过，Gmapping算法存在固有局限性：算法结构未集成回环检测模块，无法消除长时间建图过程中累积的位姿误差，其受限于对轮式里程计的依赖，在无人机或不平坦地形场景中难以适用^［17-18］。

3.2 路径规划

移动机器人的运动控制模式主要分成两类：一类是固定路径跟随模式，另一类是自主路径探索模式。自主路径探索模式要求初始条件下仅获取自身当前坐标与终点目标坐标，机器人需自主在起点与终点之间探索可行路线。该模式对机器人的性能要求较高，不仅要判断路径的通行可行性，还需在众多备选路径中筛选出最优方案。

针对单一的全局路径规划算法在时间效率与路径合理性上的不足，提出了全局路径规划与局部路径规划相结合的分层路径规划算法。在全局路径规划层面，采用 A^* 算法^［19］完成起点到终点的宏观路径规划；在局部路径规划层面，则在全局路径规划的基础上，对机器人当前所处局部区域的轨迹进行精细化处理，实现路径的动态调整与最优管理。在局部路径规划中，当起点与终点位置固定，两点间的路径因障碍物等因素发生改变时，将路径变化的条件作为约束项，通过因子图对整个优化过程与约束关系进行建模表述，如图5所示。

图5中正方形表示待优化的变量，三角形表示约束关系，f_v 表示速度，f_a 表示加速度。在map坐标系中，

T={\left\{x_i\right\}}_{i=\mathrm{0,1},\dots ,n},n\in \mathbf{N}，（1）

\tau ={\left\{\mathrm{\Delta }{T}_i\right\}}_{i=\mathrm{0,1},\dots ,n-\mathrm{1}}，（2）

B=\left(T,\tau \right)，（3）

f\left(B\right)=\sum _a{r}_k{f}_k\left(B\right)，（4）

B^{\mathrm{*}}=\mathrm{a}\mathrm{r}\mathrm{g}\underset{B}{\mathrm{m}\mathrm{i}\mathrm{n}}f\left(B\right)，（5）

式中：T表示位姿的集合，τ表示时间的集合，B为T与τ的笛卡儿积，f（B）为多个约束的目标函数，

f_k （B）为各个目标函数的权重，B^*表示最优结果。

在整个优化框架中采用G2o^［20］库进行路径的求解。具体而言，将位姿参数与运动时间间隔作为优化模型的顶点，将加速度、速度等运动学约束参数作为连接顶点的边，构建完整的因子图优化模型；采用列文伯格-马夸尔特（Levenberg-Marquardt）算法 ^［21］ 对该非线性最小二乘问题进行求解。

在仿真环境中对算法的运行过程与结果进行展示，相关结果如图6与图7所示。

图6为仿真环境地图的俯视图，呈现了场景边界与障碍物分布；图7为路径规划的可视化结果，采用差速驱动机器人模型，并搭载激光雷达传感器完成实时障碍检测。图7中的绿色线段代表由 A^* 算法规划的全局路径，而在绿色全局路径周边分布的红色短线，则是经过局部路径规划优化后生成的实际行驶路线。同时，黑色障碍物轮廓上的红色线段为激光雷达采集的环境扫描数据。对比全局路径与局部路径可发现，局部路径的合理性更优，在追踪全局路径整体方向的同时，借助自身构建的局部代价地图得到局部最优线路，在能够达到既定目标的同时，满足运动时间的最优。

3.3 建图算法对比

在真实移动机器人平台上，结合Hector和Karto两种主流激光SLAM算法对同一环境开展实时地图创建实验，并对比分析两种算法的性能差异与适用场景^［22］。实验采用单线激光雷达，扫描频率为5.5 Hz，扫描范围360°。通过Hector算法构建的实际环境地图如图8所示。该算法不依赖里程计信息，因此在无人机或不平坦地形等场景中具有较好的适用性。但在地图构建过程中高度依赖激光雷达采集的深度信息，且对激光雷达的更新频率要求较高。在扫描匹配阶段，将激光点云的帧间匹配问题转化为目标函数的极值求解问题，以高斯-牛顿法迭代求解函数极值。

通过Karto算法^［23］构建的实际环境地图如图9所示。该算法依赖于里程计与激光雷达数据的融合，采用图优化框架对整个建图过程进行全局优化，其算法结构如图10所示。

在图的构建中，机器人的位姿顶点为

{\mathit{p}}_i=[{{\mathit{t}}_i}^{\mathrm{T}},{\theta }_i{]}^{\mathrm{T}}=[x_i,y_i,{\theta }_i{]}^{\mathrm{T}}，（15）

式中： p_i 为机器人第i帧的位姿顶点，为3×1的列向量； t_i =［x_i，y_i ］^T为世界坐标系下的二维平移向量；θ_i 为机器人的航向角。

位姿边的相对位姿预测函数为

\mathit{c}\left({\mathit{p}}_i,{\mathit{p}}_j\right)=\left[\begin{array}{c}{\mathit{R}}_i^{\mathrm{T}}\left({\mathit{t}}_j-{\mathit{t}}_i\right)\\ {\theta }_j-{\theta }_i\end{array}\right]，(16）

式中： c ( p_i, p_j )为预测得到的 p_i 到 p_j 的相对位姿，为3×1的列向量； R_i 为对应航向角θ_i 的2×2旋转矩阵。

位姿边的误差定义为

{\mathit{e}}_{ij}={\mathit{z}}_{ij}-\mathit{c}\left({\mathit{p}}_i,{\mathit{p}}_j\right)，(17)

式中： z_ij 为 p_i 与 p_j 之间的相对位姿测量值，为3×1的列向量； e_ij 是图优化的最小化目标。

3.4 模拟林区建图与分析

为模拟林区作业场景的环境特征，在室内走廊空间中构建了类林区测试场景：在走廊内随机布置7棵形态、高度各异的仿真树木，以还原林区中树木分布的随机性与多样性。

采用Karto激光SLAM算法开展地图构建实验，结果如图11所示。其中，红色框标注的区域即为地图中识别出的实际树木位置，共标注7处，与场景内实际树木数量完全一致。结果也表明，移动机器人不仅能精准定位树木的空间坐标，还能完整构建出包含树木障碍的周边环境地图。

在进行误差评价时，首先从场景图中选取14处特征测量点，各点具体位置如图12所示，并按比例尺计算图上各标注点的理论距离。随后，在实际场景中定位图中标注的位置，使用卷尺测量各标注点对应的实际距离。两次测量所得的理论值、实际值及其绝对误差如表1所示，统计得到的绝对误差平均值为1.18 cm。

4 结束语

本文搭建了轮式移动机器人平台，分析其运动学特性，提出传感器在线校准方法，完成IMU、线速度与角速度校准。在仿真环境中，构建机器人模型与三维场景，实现Gmapping地图构建，提出全局与局部相结合的路径规划算法。在真实场景中，分别基于Hector与Karto激光SLAM算法完成地图构建，从原理与实验结果两方面对比二者差异。实验结果表明，设计的机器人能够有效完成建图与导航任务。受成本约束，选用简易雷达设备，其数据采集分辨率、测距范围及抗干扰能力有限。未来将优化移动机构设计，增加多传感器融合方案，弥补单一传感器的感知盲区，提升复杂环境下的导航精度。

参考文献

[1] 高明镜， 郭杭. Xtion/IMU组合的机器人室内定位方法［J］. 测绘科学， 2020， 45（6）： 62‑66， 79.

[2] GAO Mingjing， GUO Hang. Robot indoor positioning method based on Xtion/IMU combination［J］. Science of Surveying and Mapping， 2020， 45（6）： 62‑66， 79.

[3] 史裕问. 面向室内动态环境的移动服务机器人及其导航方法研究［D］. 哈尔滨： 哈尔滨工业大学， 2022.

[4] SHI Yuwen. Research on mobile service robot and its navigation method for indoor dynamic environment［D］. Harbin： Harbin Institute of Technology， 2022.

[5] 季宇寒， 李寒， 张漫， 等. 基于激光雷达的巡检机器人导航系统研究［J］. 农业机械学报， 2018， 49（2）： 14‑21.

[6] JI Yuhan， LI Han， ZHANG Man， et al. Navigation system for inspection robot based on LiDAR［J］. Transactions of the Chinese Society for Agricultural Machinery， 2018， 49（2）： 14‑21.

[7] 李森， 马小飞， 冯春勇， 等. 室内自主巡检消防灭火机器人设计［J］. 消防科学与技术， 2025， 44（2）： 181‑189.

[8] LI Sen， MA Xiaofei， FENG Chunyong， et al. Design of an autonomous indoor inspection and firefighting robot［J］. Fire Science and Technology， 2025， 44（2）： 181‑189.

[9] 赵铖钥， 马伟， 苏道毕力格， 等. 丘陵地区柑橘园采收搬运机器人系统的研制［J］. 江苏大学学报（自然科学版）， 2024， 45（2）： 167‑172.

[10] ZHAO Chengyao， MA Wei， SUDAO Bilige， et al. Development of harvesting and handling robot system for hilly citrus orchard［J］. Journal of Jiangsu University （Natural Science Edition）， 2024， 45（2）： 167‑172.

[11] BOHREN J， RUSU R B， GIL JONES E， et al. Towards autonomous robotic butlers： Lessons learned with the PR2［C］//2011 IEEE International Conference on Robotics and Automation， Shanghai， 2011： 5568‑5575.

[12] HIRAI K. The honda humanoid robot： Development and future perspective［J］. Industrial Robot， 1999， 26（4）： 260‑266.

[13] SCHIFFER S， FERREIN A， LAKEMEYER G. Caesar： An intelligent domestic service robot［J］. Intelligent Service Robotics， 2012， 5（4）： 259‑273.

[14] SAKAGAMI Y， WATANABE R， AOYAMA C， et al. The intelligent ASIMO： System overview and integration［C］//IEEE/RSJ International Conference on Intelligent Robots and Systems， Lausanne， 2002： 2478‑2483.

[15] 刘京运. 从Big Dog到Spot Mini： 波士顿动力四足机器人进化史［J］. 机器人产业， 2018（2）： 109‑116.

[16] LIU Jingyun. From Big Dog to Spot Mini： The evolutionary history of Boston dynamics^，s quadruped robots［J］. Robot Industry， 2018（2）： 109‑116.

[17] 胡春旭， 熊枭， 任慰， 等. 基于嵌入式系统的室内移动机器人定位与导航［J］. 华中科技大学学报（自然科学版）， 2013， 41（）： 254‑257， 266.

[18] HU Chunxu， XIONG Xiao， REN Wei， et al. Localization and navigation for indoor mobile robot on embedded system［J］. Journal of Huazhong University of Science and Technology （Natural Science Edition）， 2013， 41（S1）： 254‑257， 266.

[19] 赵思浩， 陆明泉， 冯振明. MEMS惯性器件误差系数的Allan方差分析方法［J］. 中国科学： 物理学 力学 天文学， 2010， 40（5）： 672‑675.

[20] ZHAO Sihao， LU Mingquan， FENG Zhenming. Allan variance analysis on error coefficients of MEMS inertial components［J］. Scientia Sinica （Physica， Mechanica & Astronomica）， 2010， 40（5）： 672‑675.

[21] 王慧， 郭成虎. 极端环境下光电编码器误差补偿方法［J］. 传感器与微系统， 2019， 38（6）： 23‑26， 30.

[22] WANG Hui， GUO Chenghu. Error compensation method of photoelectric encoder in extreme environment［J］. Transducer and Microsystem Technologies， 2019， 38（6）： 23‑26， 30.

[23] VACCARO R J， ZAKI A S. Statistical modeling of rate gyros［J］. IEEE Transactions on Instrumentation and Measurement， 2012， 61（3）： 673‑684.

[24] KOHLBRECHER S， VON STRYK O， MEYER J， et al. A flexible and scalable SLAM system with full 3D motion estimation［C］//2011 IEEE International Symposium on Safety， Security， and Rescue Robotics， Kyoto， 2011： 155‑160.

[25] ALBORZI Y， JALAL B S， NAJAFI E. ROS-based SLAM and navigation for a gazebo-simulated autonomous quadrotor［C］//2020 21st International Conference on Research and Education in Mechatronics （REM）， Cracow， 2020： 1‑5.

[26] HESS W， KOHLER D， RAPP H， et al. Real‑time loop closure in 2D LIDAR SLAM［C］//2016 IEEE International Conference on Robotics and Automation （ICRA）， Stockholm， 2016： 1271‑1278.

[27] GRISETTI G， STACHNISS C， BURGARD W. Improved techniques for grid mapping with Rao-Blackwellized particle filters［J］. IEEE Transactions on Robotics， 2007， 23（1）： 34‑46.

[28] NANNICINI G， DELLING D， LIBERTI L， et al. Bidirectional A^* search for time-dependent fast paths［C］//Experimental Algorithms， Cape Cod， 2008： 334‑346.

[29] KÜMMERLE R， GRISETTI G， STRASDAT H， et al. G2o： A general framework for graph optimization［C］//2011 IEEE International Conference on Robotics and Automation， Shanghai， 2011： 3607‑3613.

[30] WANG Xiyuan， DING Hai， DI Yingjie. Cross-coupling matrix reconfiguration using the levenberg‑marquardt algorithm on orthogonal groups［J］. IEEE Microwave and Wireless Technology Letters， 2025， 35（4）： 380‑383.

[31] HORNUNG A， OßWALD S， MAIER D， et al. Monte carlo localization for humanoid robot navigation in complex indoor environments［J］. International Journal of Humanoid Robotics， 2014， 11（2）： 1441002.

[32] OLSON E B. Real-time correlative scan matching［C］//2009 IEEE International Conference on Robotics and Automation， Kobe， 2009： 4387‑4393.

基金资助

国家自然科学基金项目(32071680)

广东省基础与应用基础研究基金项目(2023A1515110776)

湖北省教育厅科学技术研究项目(D20241702)

河南省高等学校重点科研项目(26B520044)