21、机器人SLAM与导航技术详解

机器人SLAM与导航技术详解

1. 使用保存的Bag文件进行SLAM

若不使用TurtleBot3和LDS传感器进行SLAM，需要下载记录好的Bag文件，可使用以下命令：

$ wget https://raw.githubusercontent.com/ROBOTIS-GIT/turtlebot3/master/turtlebot3_slam/bag/TB3_WAFFLE_SLAM.bag

后续步骤与常规SLAM操作类似，但需将 rosbag 命令选项从 save 改为 play ，具体操作如下：

$ roscore
$ export TURTLEBOT3_MODEL=waffle
$ roslaunch turtlebot3_bringup turtlebot3_remote.launch
$ export TURTLEBOT3_MODEL=waffle
$ rosrun rviz rviz -d `rospack find turtlebot3_slam`/rviz/turtlebot3_slam.rviz
$ roscd turtlebot3_slam/bag
$ rosbag play ./TB3_WAFFLE_SLAM.bag
$ rosrun map_server map_saver -f ~/map

2. SLAM应用

此部分聚焦于ROS中用于SLAM的软件包，主要涉及TurtleBot3元软件包、 slam_gmapping 元软件包中的 gmapping 软件包以及导航元软件包中的 map_server 软件包。

2.1 地图

机器人使用的地图需数字化以便理解和计算信息。在ROS社区，常用二维占用栅格地图（OGM）。在该地图中，白色代表机器人可移动的自由区域，黑色代表不可移动的占用区域，灰色代表未知区域。

地图区域由0 - 255的灰度值表示，通过贝叶斯定理的后验概率计算占用概率 occ ，公式为 occ = (255 - color_avg) / 255.0 。若图像为24位， color_avg = (一个单元格的灰度值 / 0xFFFFFF x 255) 。 occ 越接近1，占用可能性越高；越接近0，占用可能性越低。

当占用概率以ROS消息（nav_msgs/OccupancyGrid）发布时，会重新定义为整数[0 - 100]，接近0为自由区域，100为占用区域，-1为未知区域。

在ROS中，实际地图存储为 *.pgm 文件， *.yaml 文件包含地图信息，例如：

image: map.pgm 
resolution: 0.050000 
origin: [-10.000000, -10.000000, 0.000000] 
negate: 0 
occupied_thresh: 0.65 
free_thresh: 0.196 

这意味着每个像素对应5cm，原点代表地图的起始位置，若占用概率超过阈值，像素显示为黑色占用区域，否则为白色自由区域。

2.2 SLAM所需信息

创建地图时，SLAM需要以下两类信息：
- 距离值 ：以机器人为测量中心，获取与特定物体的距离，如LDS和深度相机扫描的XY平面距离数据。
- 位姿值 ：即安装在机器人上的传感器的位姿信息，依赖于机器人的里程计信息。

在ROS中，LDS测量的距离称为 scan ，位姿信息受相对坐标影响，称为 tf （变换）。基于这两个信息运行SLAM，可创建所需地图。

2.3 SLAM流程

为使用SLAM创建地图，除 turtlebot3_core 节点外，还创建了 turtlebot3_slam 软件包。该软件包无源文件，但执行启动文件时会启动SLAM所需的软件包，流程如下：

graph LR
    A[sensor_node] --> B[turtlebot3_slam_gmapping]
    C[turtlebot3_teleop] --> D[turtlebot3_core]
    D --> B
    B --> E[map_saver]

• sensor_node（如turtlebot3_lds） ：运行LDS传感器，将SLAM所需的 scan 信息发送到 slam_gmapping 节点。
• turtlebot3_teleop（如turtlebot3_teleop_keyboard） ：接收键盘输入，控制机器人，将平移和旋转速度命令发送到 turtlebot3_core 节点。
• turtlebot3_core ：接收速度命令，移动机器人，发布机器人的测量和估计位姿 odom ，并将其转换为相对坐标以 tf 形式发布。
• turtlebot3_slam_gmapping ：根据距离传感器的 scan 信息和传感器位姿的 tf 信息创建地图。
• map_saver ：在 map_server 软件包中，创建 map.pgm 文件和 map.yaml 文件。

2.4 坐标变换（TF）

SLAM使用的距离值和测量该值的位姿可通过传感器节点获取距离值，通过计算传感器位置获取位姿值。传感器安装在机器人固定位置，其位姿随机器人位姿变化，这在ROS中称为相对坐标变换 tf 。可使用以下命令可视化相对坐标树结构：

$ rosrun rqt_tf_tree rqt_tf_tree

2.5 turtlebot3_slam软件包

turtlebot3_slam.launch 文件内容如下：

<launch>
  <!-- Turtlebot3 -->
  <include file="$(find turtlebot3_bringup)/launch/turtlebot3_remote.launch" />
  <!-- Gmapping -->
  <node pkg="gmapping" type="slam_gmapping" name="turtlebot3_slam_gmapping" output="screen">
    <param name="base_frame" value="base_footprint"/>
    <param name="odom_frame" value="odom"/>
    <param name="map_update_interval" value="2.0"/>
    <param name="maxUrange" value="4.0"/>
    <param name="minimumScore" value="100"/>
    <param name="linearUpdate" value="0.2"/>
    <param name="angularUpdate" value="0.2"/>
    <param name="temporalUpdate" value="0.5"/>
    <param name="delta" value="0.05"/>
    <param name="lskip" value="0"/>
    <param name="particles" value="120"/>
    <param name="sigma" value="0.05"/>
    <param name="kernelSize" value="1"/>
    <param name="lstep" value="0.05"/>
    <param name="astep" value="0.05"/>
    <param name="iterations" value="5"/>
    <param name="lsigma" value="0.075"/>
    <param name="ogain" value="3.0"/>
    <param name="srr" value="0.01"/>
    <param name="srt" value="0.02"/>
    <param name="str" value="0.01"/>
    <param name="stt" value="0.02"/>
    <param name="resampleThreshold" value="0.5"/>
    <param name="xmin" value="-10.0"/>
    <param name="ymin" value="-10.0"/>
    <param name="xmax" value="10.0"/>
    <param name="ymax" value="10.0"/>
    <param name="llsamplerange" value="0.01"/>
    <param name="llsamplestep" value="0.01"/>
    <param name="lasamplerange" value="0.005"/>
    <param name="lasamplestep" value="0.005"/>
  </node>
</launch>

其中， turtlebot3_remote.launch 文件加载用户指定的机器人模型，并执行 robot_state_publisher 节点，发布机器人状态信息。

slam_gmapping 节点设置如下表所示：
| 参数名 | 描述 |
| ---- | ---- |
| base_frame | 附着在移动基座上的框架 |
| odom_frame | 附着在里程计系统上的框架 |
| map_update_interval | 地图更新间隔（秒） |
| maxUrange | 激光传感器的最大使用范围（米） |
| minimumScore | 考虑扫描匹配结果的最小分数 |
| linearUpdate | 处理所需的最小移动距离 |
| angularUpdate | 处理所需的最小旋转角度 |
| temporalUpdate | 若上次扫描时间超过此更新时间，则进行扫描，负值将被忽略 |
| delta | 地图分辨率：距离 / 像素 |
| lskip | 每次扫描跳过的光束数量 |
| particles | 粒子滤波器中的粒子数量 |
| sigma | 激光辅助搜索的标准差 |
| kernelSize | 激光辅助搜索的窗口大小 |
| lstep | 初始搜索步长（平移） |
| astep | 初始搜索步长（旋转） |
| iterations | 扫描匹配迭代次数 |
| lsigma | 用于似然计算的光束的sigma |
| ogain | 评估似然时使用的增益 |
| srr | 里程计误差（平移→平移） |
| srt | 里程计误差（平移→旋转） |
| str | 里程计误差（旋转→平移） |
| stt | 里程计误差（旋转→旋转） |
| resampleThreshold | 重采样阈值 |
| xmin | 初始地图大小（最小x） |
| ymin | 初始地图大小（最小y） |
| xmax | 初始地图大小（最大x） |
| ymax | 初始地图大小（最大y） |
| llsamplerange | 似然的平移采样范围 |
| llsamplestep | 似然的平移采样步长 |
| lasamplerange | 似然的角度采样范围 |
| lasamplestep | 似然的角度采样步长 |

3. SLAM理论

3.1 SLAM概述

SLAM（Simultaneous Localization And Mapping）指的是机器人利用自身搭载的传感器，在探索和绘制未知环境的同时，估计自身的位姿。这是自动驾驶等导航领域的关键技术。

在位姿估计方面，通常会使用编码器和惯性测量单元（IMU）。编码器通过航位推算法，测量驱动轮的旋转量来计算机器人的大致位姿，但这种方法存在较大的估计误差。而惯性传感器测量的惯性信息可以补偿计算位姿时产生的误差。根据不同的需求，也可以仅使用惯性传感器来估计位姿。

利用距离传感器或相机获取的周围环境信息，还可以进一步修正估计的位姿。常见的位姿估计算法包括卡尔曼滤波器、马尔可夫定位、使用粒子滤波器的蒙特卡罗定位等。

在地图绘制时，常使用超声波传感器、光探测器、无线电探测器、激光测距仪和红外扫描仪等距离传感器。此外，相机也可用于测量距离，如立体相机，还有基于普通相机的视觉SLAM。另外，也有通过附着标记物来识别环境的方法，例如在天花板上设置标记物，让相机进行区分。近年来，深度相机（如RealSense、Kinect、Xtion等）被广泛应用，它们能提取与距离传感器精度相当的距离信息。

3.2 各种定位方法

位姿估计是机器人领域的重要研究方向，至今仍在积极探索。准确估计机器人的位姿有助于完成SLAM等基于位姿的地图构建任务，但实际应用中存在传感器观测信息不确定等问题，且需要保证实时性。为解决这些问题，研究出了多种定位估计方法，下面介绍卡尔曼滤波器和粒子滤波器这两种常见方法。

• 卡尔曼滤波器
  卡尔曼滤波器由鲁道夫·E·卡尔曼博士开发，曾用于美国国家航空航天局（NASA）的阿波罗项目。它是一种递归滤波器，用于跟踪带有噪声的线性系统中物体的状态。该滤波器基于贝叶斯概率，通过模型从先前状态预测当前状态，然后利用预测值与实际测量值之间的误差进行更新，以提高估计的准确性。其基本概念如下：

graph LR
    A[先前状态] --> B[预测当前状态]
    B --> C[与实际测量值对比]
    C --> D[更新状态估计]
    D --> E[输出准确状态]

然而，卡尔曼滤波器仅适用于线性系统。由于大多数机器人和传感器是非线性系统，因此扩展卡尔曼滤波器（EKF）被广泛使用。此外，还有无迹卡尔曼滤波器（UKF）等改进版本，它们在精度和速度方面有所提升，相关研究仍在进行中。卡尔曼滤波器还常与其他算法结合使用，如与粒子滤波器结合的Rao - Blackwelled粒子滤波器（RBPF）。

• 粒子滤波器
  粒子滤波器是目标跟踪中最常用的算法，蒙特卡罗定位就是其典型应用。与卡尔曼滤波器不同，粒子滤波器适用于非线性系统。在实际应用中，由于机器人和传感器多为非线性，粒子滤波器常用于位姿估计。

粒子滤波器假设输入信息包含误差，在SLAM中，利用机器人的里程计值和距离传感器的测量值来估计机器人的当前位姿。该方法将不确定的位姿用一组称为样本的粒子来描述，通过机器人的运动模型和概率将粒子移动到新的估计位置和方向，根据实际测量值计算每个粒子的权重，逐步减少噪声以估计精确位姿。对于移动机器人，每个粒子表示为 particle = pose (x, y, i), weight ，其中 x 、 y 、 i 表示机器人的估计位置和方向， weight 表示粒子的权重。

粒子滤波器的工作流程包括以下5个步骤，除步骤1的初始化外，步骤2 - 5会重复执行以估计机器人的位姿：
1. 初始化 ：由于机器人的初始位姿未知，将 N 个粒子随机分布在可能的位姿范围内，每个初始粒子的权重为 1/N ，粒子权重总和为1。 N 通常根据经验确定，一般为几百个。若初始位置已知，则将粒子放置在机器人附近。
2. 预测 ：根据描述机器人运动的系统模型，结合里程计信息和噪声，移动每个粒子到新的估计位置和方向。
3. 更新 ：根据测量的传感器信息，计算每个粒子的概率，并更新其权重值。
4. 位姿估计 ：利用所有粒子的位置、方向和权重，计算平均权重、中值和最大权重值，以估计机器人的位姿。
5. 重采样 ：移除权重较小的粒子，创建继承权重较大粒子位姿信息的新粒子，保持粒子数量 N 不变。

当样本数量足够时，粒子滤波器的位姿估计可能比改进后的卡尔曼滤波器（如EKF或UKF）更准确；但样本数量不足时，估计结果可能不准确。基于Rao - Blackwelled粒子滤波器的SLAM在实际应用中也有广泛的研究和应用。