移动机器人学 课件第六章 移动机器人定位与建图

华南理工大学自动化科学与工程学院《移动机器人学》

华南理工大学自动化科学与工程学院第六章

移动机器人定位与建图

6.移动机器人定位与建图地图构建和定位是移动机器人自主导航的关键基础问题。SLAM技术通过迭代优化机器人位姿和环境地图，解决了两者相互影响的难题。SLAM方法可分为滤波SLAM和图优化SLAM，传感器上分为激光SLAM和视觉SLAM。激光SLAM因其高精度和实时性广泛应用于自动驾驶、AGV和扫地机器人等领域。本章研究SLAM问题的数学描述及基于改进RBPF的SLAM方法的实现原理，并概述栅格地图构建。重点分析基于Gmapping算法的激光雷达建图及ORB-SLAM3视觉算法。6.1SLAM理论基础SLAM核心思想SLAM的核心思想是通过机器人或车辆的传感器数据，实时完成自身位姿估计和环境地图的构建。在未知环境中，SLAM利用传感器检测运动信息和环境特征，通过特征匹配和数据关联不断更新全局地图，从而实现自主定位与导航。这一技术在机器人自主导航、无人驾驶等领域具有重要意义。SLAM系统框图6.1SLAM理论基础SLAM概率模型SLAM的概率模型通过贝叶斯滤波和马尔可夫假设描述机器人在未知环境中的定位和建图不确定性。通过结合运动模型和观测模型，贝叶斯滤波计算状态和地图的后验概率分布，不断更新以减小不确定性。SLAM问题原理图马尔可夫假设简化了计算复杂度，使得SLAM可以实时运行。此外，SLAM还使用因子图和回环检测来提高精度，并结合深度学习增强鲁棒性。6.1SLAM理论基础SLAM概率模型根据概率论原理，SLAM问题可以建模成P

(xt,

M

|Z0:t,

U0:t)机器人当前时刻t下的位姿xt状态转移方程可以描述为xt

=

f

(xt−1,

ut)

+

Vt机器人t时刻的状态转移概率分布如下P

(xt,

M

|xt−1,

U0:t)

=

P

(xt,

M

|xt−1,

ut)

=

F

(xt−1,

ut)

+

Vt6.1SLAM理论基础SLAM概率模型根据机器人传感器测量数据，当前时刻机器人对路标位置的观测方程可以描述为zt

=

h(xt,

M

)

+

Wt基于贝叶斯估计理论，结合机器人的观测方程（6-4）得到如下的观测信息的概率分布：P

(zt|xt,

M

)

=

H(xt,

M

)

+

Wt6.1SLAM理论基础SLAM概率模型通过递归的方式得到SLAM问题的预测公式和更新公式，即机器人联合后验概率密度分布，具体如下：P

(xt,

M

|Z0:t−1,

U0:t)

=

P

(xt,

M

|xt−1,

ut)P

(xt−1,

M

|Z0:t−1,

U0:t−1)dxt−1预测公式：更新公式：6.2基于改进RBPF粒子滤波的SLAM算法Rao­Blackwellized粒子滤波算法Rao-Blackwellized粒子滤波（RBPF）算法结合了Rao-Blackwellized估计与粒子滤波，适用于高维非线性非高斯状态估计问题。通过条件独立性假设将高维状态空间分解为低维子空间，每个子空间使用粒子滤波独立估计，从而降低计算复杂性并提高估计精度。RBPF通过迭代采样和权重更新，逐步逼近真实状态分布，避免粒子退化。相比传统粒子滤波，RBPF算法在精度和效率上具有优势，适用于复杂系统，但需要根据具体问题调整算法参数以优化性能。6.2基于改进RBPF粒子滤波的SLAM算法Rao­Blackwellized粒子滤波算法基于RBPF的SLAM（RBPF-SLAM）问题实际上是计算机器人位姿和地图的联合概率分布的问题，通过RBPF分解为对机器人位姿的后验概率分布和地图概率分布，最后通过两者乘积获得。联合后验概率分布如下：6.2基于改进RBPF粒子滤波的SLAM算法Rao­Blackwellized粒子滤波算法RBPF算法的5个步骤总结如下：粒子初始化：根据机器人的运动模型，通过运动状态转移概率估计生成N个独立粒子，并为每个粒子分配初始权重。重要性采用：根据建议分布函数将上一时刻的粒子代入当前时刻，生成新的粒子。重要性权重更新：更新每个粒子的权重，权重反映了粒子与真实后验分布的匹配程度，通过观测数据（如激光雷达）计算得到。重采样：为了避免粒子退化，重采样步骤根据粒子权重选择权重大的粒子，丢弃权重小的粒子，所有粒子权重重设为1/N。更新地图：根据机器人的位姿和观测数据，更新地图中的路标信息。6.2基于改进RBPF粒子滤波的SLAM算法基于改进RBPF的Gmapping算法传统Gmapping算法：通过粒子滤波（ParticleFilter,PF）估计机器人的位姿，并结合激光雷达数据构建环境地图。粒子滤波：基于大量的粒子来近似机器人的状态分布。局限性：需要大量粒子来保证精度，计算量大、实时性差。改进RBPF的Gmapping算法：通过引入RBPF方法将状态空间分解为可观测部分和不可观测部分。可观测部分：通过最大似然估计进行精确计算。不可观测部分：使用粒子滤波进行概率估计。改进点：精确计算与概率估计相结合，减少粒子数量，提升算法的实时性和计算效率。通过优化粒子权重计算和引入自适应重采样，降低粒子退化问题。6.2基于改进RBPF粒子滤波的SLAM算法基于改进RBPF的Gmapping计算方法状态空间分解RBPF方法：通过对机器人状态空间进行分解，减少粒子滤波的计算复杂度。可观测状态：通过最大似然估计精确计算。不可观测状态：使用粒子滤波估算。目标：减少粒子数量，提高计算效率。2.粒子权重计算最大似然估计（MLE）：用于计算可观测部分的最优估计。粒子滤波：对不可观测状态进行概率估计。权重更新公式：6.2基于改进RBPF粒子滤波的SLAM算法基于改进RBPF的Gmapping计算方法

6.3基于激光雷达的Gmapping算法室内地图构建实现基于激光雷达的Gmapping算法原理激光雷达数据采集：激光雷达通过发射激光束并测量反射时间，获取周围物体的距离数据，形成点云，揭示环境的结构与布局。点云数据处理与地图对齐：Gmapping算法对点云数据进行滤波和降噪，并使用扫描匹配技术将当前激光扫描与已有地图对齐。机器人状态估计与地图更新：通过粒子滤波技术，Gmapping算法不断估计机器人位置并更新地图，使位置和地图信息融合。6.3基于激光雷达的Gmapping算法室内地图构建实现Gmapping算法的地图构建与应用粒子滤波与地图构建：在Gmapping算法中，粒子滤波不仅用于估计机器人位置，还用于构建环境的二维栅格地图，每个粒子携带一个局部地图，经过多次迭代更新，最终生成全局一致的地图。栅格地图表示与环境适应：该地图中的栅格表示环境信息，颜色或数值反映占据概率或高度信息，算法能够适应动态环境并处理噪声。回环检测与地图优化：Gmapping算法通过回环检测纠正误差，提升地图的准确性，实验验证表明，改进RBPF的Gmapping算法能够在复杂环境中有效实现机器人定位与建图。6.3基于激光雷达的Gmapping算法室内地图构建实现SLAM实验环境与Gmapping应用

Gmapping实验设置与场景：实验环境:本节实验在6×8m²的室内实验室进行，实验场景如图6-3所示。激光雷达的扫描角度为270°，并通过远程PC端监控和控制移动机器人的运动与建图过程。Gmapping实验场景6.3基于激光雷达的Gmapping算法室内地图构建实现SLAM实验环境与Gmapping应用

Gmapping实验设置与场景：实验配置:本实验使用ROS平台的Gmapping功能包，通过调节参数实现实验过程。启动文件运行后，底盘驱动、激光雷达、IMU、Gmapping以及远程控制节点将开始工作，完成机器人定位与地图构建。6.3基于激光雷达的Gmapping算法室内地图构建实现SLAM实验环境与Gmapping应用

Gmapping节点与坐标关系：节点关系:如图6-4所示，IMU与STM32发布惯导信息（/imu/data）与编码器信息（/raw_vel），底盘节点提供里程计信息。Gmapping订阅底盘里程计与激光雷达信息，进行机器人位姿估计与地图构建。远程端通过/my_cmd_vel发送控制信息。Gmapping运行节点图6.3基于激光雷达的Gmapping算法室内地图构建实现Gmapping地图构建重要参数配置:在启动建图前，Gmapping需要配置多个参数，其中最重要的调节参数为：particles:设置粒子数量，本节实验中设置为50，以保证运算效率和定位精度。minimumScore:设置扫描匹配的分数阈值，本节设置为100，以判断是否采用改进建议分布。启动过程:启动Gmapping功能包、机器人底层Bringup、远程PC端的teleop_keyboard功能包和RVIZ可视化界面。通过远程控制机器人进行SLAM建图，完成实验场景的地图构建。6.3基于激光雷达的Gmapping算法室内地图构建实现Gmapping地图构建实验结果:经过调节，建图精度满足室内导航需求。图6-6展示了栅格地图，墙壁和角落无明显扭曲，实验室内物体位置与真实场景吻合，成功实现了定位导航。Gmapping地图构建过程6.4Catorgrapher算法Cartographer算法概述什么是Cartographer?由Google提出的实时同时定位与地图构建（SLAM）算法。支持2D和3D激光雷达数据输入，跨平台使用，广泛应用于机器人、自动驾驶和增强现实等领域。通过图优化方法进行精准的定位与地图构建，实时生成高分辨率的地图。核心特点实时性:能够在运行时生成环境地图，实时显示更新的地图信息。高精度:通过优化算法和回环检测机制，确保定位精度和地图构建的准确性。子地图方法:将地图构建分为多个子地图，逐步拼接成完整地图，降低误差积累。工作原理：机器人通过传感器数据（如激光雷达、IMU等）收集环境信息。连续扫描数据转化为子地图，通过扫描匹配算法与已建地图进行对齐，确保机器人定位精度。6.4Catorgrapher算法子图构建与扫描匹配子图构建：Cartographer使用子图的方式构建地图。每次收集的扫描数据都会被划分成多个子图，通过扫描匹配算法将它们拼接成完整地图。扫描匹配与优化：激光雷达扫描转换到子图坐标系，通过最小化误差对齐扫描。使用Ceres求解器进行非线性最小二乘优化，保证精度。从激光雷达坐标系到子图坐标系的刚体变换公式：6.4Catorgrapher算法网格点与命中/未命中网格示意图

网格点和对应的像素示意图命中与未命中网格示意图6.5ORB­SLAM3算法ORB-SLAM3算法概述ORB-SLAM3简介：ORB-SLAM3是一种视觉和视觉惯性SLAM系统，能够处理单目、立体和RGB-D相机，并支持基于针孔和鱼眼镜头模型的视觉、视觉–惯性和多地图SLAM。它通过最大后验估计（MAP）方法进行优化，精度是以往方法的2至10倍。ORB-SLAM3的关键特点：多地图系统：ORB-SLAM3可以处理多会话的地图并支持地图合并。高鲁棒性与准确性：即使在长时间视觉信息不佳的情况下，也能持续稳定地运行。短期、中期、长期数据关联：ORB-SLAM3能够在建图区域实现零漂移，提高了数据关联精度。6.5ORB­SLAM3算法ORB-SLAM3的工作原理与关键技术最大后验估计与光束平差：ORB-SLAM3使用最大后验估计（MAP），类似光束平差（BundleAdjustment,BA）在视