ROS实践（四）机器人建图及导航

一、概念

        机器人导航是指机器人在环境中自主地从一个地点移动到另一个地点的过程。这个过程涉及到多个关键技术，包括定位、路径规划、避障等。机器人导航通常包括以下几个重要部分。

1. 定位

定位是机器人确定自己在环境中的位置的过程。常用的定位方法包括：

（1）GPS定位：适用于室外环境，利用卫星信号确定机器人的位置。

（2）里程计（Odometry）：根据轮子的转动来估算机器人的位置，但容易受到误差累积的影响。

（3）激光雷达（Lidar）/传感器定位：利用激光扫描仪或其他传感器获取环境数据，进行位置修正。常用的方法有：①粒子滤波（Particle Filter）：常用于基于激光雷达的定位。扩展②卡尔曼滤波（EKF）：用于融合多个传感器数据。

（4）视觉定位（SLAM）：通过相机和其他传感器（如激光雷达）进行同时定位与地图构建。

2. 地图构建

        地图构建（Mapping）是机器人在运动过程中逐步建立和更新其所处环境的地图。常见的地图构建方法包括：

（1）SLAM（Simultaneous Localization and Mapping）：结合定位和地图构建，是一种实时的在线建图技术。

（2）网格地图（Grid Map）：环境划分为一系列的栅格，机器人通过传感器的输入来判断哪些区域是障碍物，哪些是自由的。

3. 路径规划

        路径规划是机器人从起点到目标点之间寻找一条最佳路径的过程。路径规划可以分为以下几种：

（1）全局路径规划

        全局路径规划是在已知整个环境地图的情况下进行的路径规划。它的目标是找到一条从起点到终点的最优路径，这条路径通常是在不考虑动态障碍物的情况下计算得出的。全局路径规划常用的算法包括：

（1）全局路径规划

• A*算法：
          A算法是一种启发式搜索算法，它结合了最佳优先搜索的高效性和Dijkstra算法的完备性。A算法使用启发式函数来估计从当前节点到目标节点的代价，从而指导搜索过程。这个启发式函数通常基于欧几里得距离或曼哈顿距离等简单度量。A*算法广泛应用于静态障碍物环境中的路径规划，因为它能够高效地找到最短路径。

• Dijkstra算法：
         Dijkstra算法是一种基于图的路径搜索算法，它适用于所有边的权重为正数的图。该算法从起点开始，逐步扩展最短路径树，直到包含终点。在每一步中，它选择具有最小累积权重的边来扩展树。虽然Dijkstra算法不直接考虑目标节点的启发式信息，但它保证了找到从起点到图中所有其他节点的最短路径。在全局路径规划中，Dijkstra算法可以用于在权重图中找到最短路径，其中权重可能代表距离、时间或其他相关成本。

（2）局部路径规划

        局部路径规划是在全局路径的基础上，考虑动态障碍物和环境变化进行的路径规划。它的目标是生成一条能够安全避开动态障碍物并沿全局路径前进的局部路径。常见的局部规划方法包括：

• 动态窗口法（DWA）：
          动态窗口法是一种基于采样的局部路径规划算法，它考虑了机器人的动力学约束（如速度和加速度限制）。DWA算法在速度空间中采样多个速度组合，并模拟这些速度组合在未来一段时间内的轨迹。然后，它根据这些轨迹的成本函数（通常考虑与障碍物的距离、与全局路径的偏差等因素）来选择最优速度。DWA算法能够实时响应动态环境的变化，并生成平滑且安全的局部路径。

• 虚拟力场（VFH）方法：
         虚拟力场方法是一种基于势场的局部路径规划算法。它将环境表示为一个由吸引力和排斥力组成的力场。目标点产生吸引力，而障碍物产生排斥力。机器人根据这些力的合力来决定其运动方向。VFH方法的一个变种是基于当前环境的危险度图来生成避障路径。危险度图根据障碍物的位置、速度和形状等信息来评估环境中每个点的危险程度。机器人然后根据危险度图来选择安全的路径。

        需要注意的是，局部路径规划算法通常与全局路径规划算法结合使用。全局路径规划提供了一条大致的路径方向，而局部路径规划则负责在这条路径上安全地避开动态障碍物。这种结合使用的方法能够充分利用两种规划算法的优点，从而提高机器人的导航性能和安全性。

二、常用工具和传感器

激光雷达（LiDAR）：用于生成环境的高精度地图。

RGBD相机：能够提供深度信息的相机，用于地图构建和障碍物检测。

超声波传感器：常用于短距离的障碍物检测。

IMU（惯性测量单元）：用于机器人姿态估计。

轮式里程计（Odometry）：通过机器人轮子的转动估算机器人的位置和姿态。

 三、相关功能包

1. 导航（robot_navigation）

   robot_navigation是一个基于ROS（Robot Operating System）的导航堆栈，设计用于处理复杂的室内和室外环境。robot_navigation是一个由第三方团队开发并开源的导航堆栈项目。它基于ROS系统构建，提供了全面的导航功能，并支持多种算法和应用场景。

        该项目的目的是简化机器人系统的集成，通过提供一套全面的导航工具包，使得开发者可以专注于特定的应用需求而不是基础架构的构建。

        下面这些组件虽然都是相对独立的功能包，但它们在ROS的导航堆栈中紧密协作，共同实现机器人的自主导航功能。robot_navigation并不是一个具体的软件包名，而是可能指代整个导航堆栈或相关软件包的集合。在ROS的官方文档和社区中，这些组件通常被归类为导航堆栈的一部分。相关功能包如下：

（1）move_base

• 定义：move_base是ROS导航栈中的核心节点，负责全局路径规划和局部路径规划。
• 功能：它根据目标位置和当前地图信息生成一条从起点到终点的路径。在机器人行进过程中，move_base还会通过避障算法动态调整路径，以确保机器人能够顺利到达目标位置。
• 组件：move_base包括全局规划器、全局代价地图、局部规划器、局部代价地图及恢复行为等组件。
• 交互方式：move_base通过action API与客户端交互，并订阅和发布主题来实现机器人导航功能。

（2）amcl

• 定义：amcl（Adaptive Monte Carlo Localization）是ROS中的一个软件包，提供节点用于在静态地图中对机器人进行定位。
• 功能：该节点通过粒子滤波算法结合激光雷达数据和地图信息，估算机器人在环境中的位置。
• 应用场景：在已知地图中，机器人可以使用amcl进行定位，并根据当前位置进行路径规划。amcl节点高度可配置，可以根据实际需求调整参数。

（3）csotmap_2d

• 定义：costmap_2d是ROS中的一个功能包，用于生成代价地图。
• 功能：代价地图表示环境中每个位置的“成本”，每个区域会根据障碍物的距离、机器人行驶的安全性等因素分配不同的代价。
• 重要性：costmap_2d对于路径规划和避障非常重要，它帮助move_base生成可行路径，并确保机器人在行进过程中能够避开障碍物。

（4）map_server

• 定义：map_server是ROS中的一个功能包，用于加载和保存地图。
• 功能：在启动时，map_server可以加载一个地图（如.pgm格式的地图），并通过ROS话题和服务提供地图数据。
• 命令：使用rosrun map_server map_server map_file.yaml命令可以启动map_server节点，其中map_file.yaml是描述地图的YAML文件。而rosrun map_server map_saver -f map命令用于保存当前地图数据到文件中。

（5）base_local_planner

• 定义：base_local_planner是ROS中的一个软件包，用于实现局部路径规划。
• 功能：在给定的全局路径基础上，base_local_planner负责计算如何局部避障并规划实际的运动轨迹。
• 实现方式：它提供了一组接口和实现，可以让机器人在给定地图和当前位置的情况下，规划并执行一条安全、平滑的轨迹。

2. gmapping建图功能包

  gmapping 是一种基于粒子滤波算法的 SLAM工具，它可以帮助机器人在未知环境中创建地图的同时定位自己。

        具体来说，gmapping 是 ROS 中实现激光SLAM的一个软件包，它结合了激光扫描器数据和机器人运动信息，实时生成并更新地图。常用在需要通过激光雷达进行建图的机器人系统中，尤其适用于移动机器人、无人车等场景。

  gmapping 包含了完成 SLAM所需的所有核心算法，它会自动利用激光扫描数据和机器人的运动信息来进行地图构建和位置估算。只要你正确配置了相关参数和话题，就可以通过启动 gmapping 节点来实现SLAM功能，而不必从头实现复杂的 SLAM 算法。

（1）下载

  gmapping 并不是 ROS 系统自带的核心组件，而是一个额外的 ROS 软件包。通常在使用 ROS 时，gmapping 需要单独安装并配置。

sudo apt-get install ros-<ros_version>-gmapping

其中，<ros_version> 需要替换为你使用的 ROS 版本名称（如 melodic、noetic 等）。

（2）使用

        为了使用gmapping功能包，需要配置相应的参数，如激光雷达的topic、里程计信息、地图分辨率等。这些参数通常通过launch文件或命令行参数进行设置。在运行gmapping节点时，它会订阅指定的传感器数据和里程计信息，并输出构建的地图数据。

四、实验

1. SLAM 建图

（1）配置gmapping（launch文件）

         功能：下面这段代码的主要功能是启动 gmapping 节点，使用激光雷达和机器人自身的运动数据来进行 SLAM（同步定位与地图构建）。具体来说，它通过激光扫描获取环境信息，并将这些信息转化为地图，同时估算机器人在该地图中的位置。通过 SLAM，机器人可以在没有事先地图的情况下自动探索并创建自己的环境地图。

         要完成的工作：我们只需要配置好机器人坐标系和激光数据的相关参数。启动 gmapping 节点。让机器人在环境中移动，gmapping 会实时构建地图并定位机器人，它会帮你自动完成 SLAM 的工作。

        目录： 在功能包的launch目录下编写gmapping_slam.launch文件，如下所示：

  <!-- 启动 gmapping 节点进行SLAM建图 -->
  <node pkg="gmapping" type="slam_gmapping" name="gmapping" output="screen">
    <!-- 设置基本坐标框架 -->
    <param name="base_frame" value="$(arg set_base_frame)"/>
    <!-- 设置里程计坐标框架 -->
    <param name="odom_frame" value="$(arg set_odom_frame)"/>
    <!-- 设置地图坐标框架 -->
    <param name="map_frame"  value="$(arg set_map_frame)"/>
    
    <!-- 设置地图更新的时间间隔（单位：秒） -->
    <param name="map_update_interval" value="2.0"/>
    
    <!-- 设置最大激光扫描距离 -->
    <param name="maxUrange" value="5.0"/>
    
    <!-- 设置激光噪声模型的标准差 -->
    <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="lskip" value="0"/>
    
    <!-- 设置地图匹配的最低得分 -->
    <param name="minimumScore" value="50"/>
    
    <!-- 设置机器人运动噪声的标准差 -->
    <param name="srr" value="0.1"/>
    <param name="srt" value="0.2"/>
    <param name="str" value="0.1"/>
    <param name="stt" value="0.2"/>
    
    <!-- 设置线性和角度更新的阈值 -->
    <param name="linearUpdate" value="1.0"/>
    <param name="angularUpdate" value="0.2"/>
    
    <!-- 设置时间更新的阈值 -->
    <param name="temporalUpdate" value="0.5"/>
    
    <!-- 设置重采样的阈值 -->
    <param name="resampleThreshold" value="0.5"/>
    
    <!-- 设置粒子滤波的粒子数 -->
    <param name="particles" value="100"/>
    
    <!-- 设置地图边界 -->
    <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="delta" value="0.05"/>
    
    <!-- 设置激光采样器的范围和步长 -->
    <param name="llsamplerange" value="0.01"/>
    <param name="llsamplestep" value="0.01"/>
    <param name="lasamplerange" value="0.005"/>
    <param name="lasamplestep" value="0.005"/>
    
    <!-- 将扫描话题重映射为传入的扫描话题名称 -->
    <remap from="scan" to="$(arg scan_topic)"/>
  </node>
</launch>

代码中做了以下几件事：

        ①设置坐标系：定义了机器人、里程计（机器人位置数据）、和地图之间的坐标关系。

        ②配置 SLAM 参数：包括地图更新频率、激光扫描最大范围、噪声标准差、地图分辨率等。这些参数影响到地图的精度、更新速度以及机器人对环境的感知。

        ③设置激光传感器的行为：通过设置激光扫描的步长、增益、采样范围等来调整扫描的精度。

        ④重映射扫描话题：将接收到的激光数据从指定的 scan_topic 输入到 slam_gmapping，用于创建地图。

（2）启动机器人仿真（含机器人以及传感器）

ROS实践（三）xacro文件基础（urdf扩展）第4章节内容。https://blog.csdn.net/qq_48361010/article/details/146174461?spm=1001.2014.3001.5501详情查看上文。

（3）运行gmapping节点

        使用代码：roslaunch my_package gmapping_slam.launch simulation:=true ，这里会发布一个/map的话题，用于后面rviz来订阅这个话题显示地图。

  （4）启动rviz可视化工具

使用命令：rviz启动。启动完成后需要添加以下组件。

（5）保存地图文件

        启动rviz仿真后，通过：rosrun teleop_twist_keyboard teleop_twist_keyboard.py遥控节点来使得机器人运动去扫描建图。

        完成建图以后使用命令：roscd my_package/map/ && rosrun map_server map_saver -f my_map，来保存当前地图为my_map，并保存到map文件夹中。保存后退出所有进程即可。

 2. 导航

编写launch文件，如下所示：

<launch>
  <!-- Arguments部分定义了可以在启动文件外部传递的参数，也可以在文件内部设置默认值 -->
  
  <!-- map_file参数指定了地图文件的路径，这里默认使用my_package包下的map文件夹中的my_map.yaml文件 -->
  <arg name="map_file" default="$(find my_package)/map/my_map.yaml"/>
  
  <!-- simulation参数用于指示是否在模拟环境中运行，默认为false，表示真实环境 -->
  <arg name="simulation" default="false"/>
  
  <!-- planner参数指定路径规划算法，默认使用DWA（动态窗口法），另一个选项是TEB（时序弹性带） -->
  <arg name="planner" default="dwa" doc="opt: dwa, teb"/>
  
  <!-- move_forward_only参数用于限制机器人只能前进，不能后退或旋转，默认为false -->
  <arg name="move_forward_only" default="false"/>
  
  <!-- use_dijkstra参数指定路径搜索算法，默认为true，表示使用Dijkstra算法，否则可能使用其他算法如A* -->
  <arg name="use_dijkstra" default="true"/>
  
  <!-- Map server部分启动地图服务器节点，用于加载和提供地图数据 -->
  <node pkg="map_server" name="map_server" type="map_server" args="$(arg map_file)">
    <!-- frame_id参数指定地图的参考坐标系，这里设置为"map" -->
    <param name="frame_id" value="map"/>
  </node>
  
  <!-- AMCL部分启动自适应蒙特卡洛定位节点，用于根据传感器数据和地图估计机器人的位置 -->
  <node pkg="amcl" type="amcl" name="amcl" output="screen">
    <!-- 加载AMCL的参数配置，参数文件路径依赖于环境变量BASE_TYPE的值 -->
    <rosparam file="$(find robot_navigation)/param/$(env BASE_TYPE)/amcl_params.yaml" command="load" />
    <!-- 初始位置参数，用于设置机器人启动时的估计位置（x, y, theta） -->
    <param name="initial_pose_x" value="0.0"/>
    <param name="initial_pose_y" value="0.0"/>
    <param name="initial_pose_a" value="0.0"/>
  </node>
  
  <!-- move_base部分通过包含另一个launch文件来启动移动基础节点，该节点负责路径规划和导航控制 -->
  <include file="$(find robot_navigation)/launch/move_base.launch">
    <!-- 传递之前定义的参数给move_base.launch文件 -->
    <arg name="planner" value="$(arg planner)"/>
    <arg name="simulation" value="$(arg simulation)"/>
    <arg name="move_forward_only" value="$(arg move_forward_only)"/>
    <arg name="use_dijkstra" value="$(arg use_dijkstra)"/>
  </include>
</launch>