[Note] ROS2 自主探索建图

tinker2025-11-01 13:16

介绍

探索是指当机器人处于一个完全未知或部分已知环境中，通过一定的方法，在合理的时间内，尽可能多的获得周围环境的完整信息和自身的精确定位，以便于实现机器人在该环境中的导航，并实现后续工作任务。探索是移动机器人实现自主的关键功能，是移动机器人的一项重要任务，也是一个重要的研究领域。在许多潜在的应用中，建筑物、洞穴、隧道和矿山内的搜索操作有时是极其危险的活动。使用自主机器人在复杂环境中执行这些任务，降低了人类执行这些任务的风险。

扫地机器人应该是目前生活中的比较常见的应用场景，它需要能够在完全陌生的环境下，自主移动探索建图，之后才能基于建图进行打扫。

扫地机器人自主探索建图

  • 模块划分：

    • 小车：负责搭载雷达数据和里程计
    • SLAM： 负责定位与建图
    • navigation: 负责导航到探索点，并规避路障
    • exploration: 负责生成边界和新的探索点

  • 基本流程： SLAM 基于雷达和里程计计算位姿, 通过 exploration 边界探索算法，生成边界，以及规划下一个探索位置，使用 navigation 移动到探索位置， SLAM 同时生成 map.

探索建图环境

zhuanlan.zhihu.com/p/698612599

www.waveshare.net/wiki/%E5%BB...

基于小鱼两驱车搭建自主探索建图环境，提供 SLAM 定位与建图，以及 navigation2 的导航栈。在此基础上再配合自主探索算法进行建图。

搭建小车

juejin.cn/post/752861... 中记录了小车的搭建过程，配置好小车雷达和主控驱动。

小鱼两驱车关键技术拆分成小车驱动版，MicroROS, 激光雷达驱动，ROS2 TF 坐标关系

MicroROS

Micro-ROS 为在 micro-controller 机器人项目中提供一些关键功能，方便接入 ROS2 体系中。小车主空驱动通过连接 Micro-ROS 服务，接入上位机，提供 odom 里程计和接收移动指令。

Micro-ROS 为 microcontrollers (MCU) 提供了 nodes, publish/subscribe, client/service, node graph, lifecycle 等核心能力。 micro-ROS 的 client API 是 C 语言接口，基于标准的 ROS 2 Client Support Library (rcl) 和扩展与功能函数(rclc)。

并且 rcl+rclc 的组合是针对 MCUs 进行了优化。在初始化之后，无需再动态分配内存。rclc package 为嵌入式工程师提供了更高级的执行机制和调度模式。

  • 下载源码
bash 复制代码 
git clone http://github.com/micro-ROS/micro-ROS-Agent.git -b humble
git clone http://github.com/micro-ROS/micro_ros_msgs.git -b humble
  • 构建 micro-ros-agent
vbnet 复制代码 
colcon build --packages-select micro_ros_msgs --cmake-clean-cache
colcon build --packages-select micro_ros_agent --cmake-clean-cache

[ 12%] Performing download step (git clone) for 'xrceagent'       
Cloning into 'xrceagent'...                              
HEAD is now at 57d0862 Release v2.4.2

如果 clone xrceagent 失败后，一直报下面的错误，可以通过把 build 和 install 目录下的 micro_ros_agent 目录删除后重新 build

ini 复制代码 
Starting >>> micro_ros_agent                                    
--- stderr: micro_ros_agent                                  
gmake[3]: *** No rule to make target 'install'.  Stop.            
gmake[2]: *** [CMakeFiles/xrceagent.dir/build.make:104: agent/src/xrceagent-stamp/xrceagent-install] Error 2                                  
gmake[1]: *** [CMakeFiles/Makefile2:85: CMakeFiles/xrceagent.dir/all] Error 2 
gmake: *** [Makefile:91: all] Error 2
编写 bringup.launch.py 文件中配置 MicroROS 启动信息
ini 复制代码 
    microros_agent = launch_ros.actions.Node(
        package='micro_ros_agent',
        executable='micro_ros_agent',
        arguments=['udp4','--port','8888'],
        output='screen'
    )
  1. 小车驱动版负责里程计计算，并通过 Micro ROS 发布 /odom 话题。
  2. MicroROS 在上位机中运行部分，负责接收小车的 /odom 话题数据，并接入到 ROS2 系统。

小车坐标系转换 TF

无论是在二维空间还是在三维空间，我们想要描述一个物体的位置和姿态第一步就是确定一个参考坐标系，物体的位置和姿态描述我们都是以这个坐标系作为参考的。ROS 社区制定了一个标准 REP-105

REP-105 是一个名为 "Coordinate Frames for Mobile Platforms" （移动平台的坐标系框架）的 ROS Enhancement Proposal （REP）。该提案由 Wim Meeussen 于 2010年10月27日 创建，并处于活动状态。本文将介绍 REP-105 的内容，包括其摘要、动机、规范、坐标系、坐标系之间的关系等 github.com/ros-infrast...

简而言之： REP-105 规定至少必须为机器人构造一个包含map -> odom -base_link -> [sensorframes] 的完整 的TF树

首先是 URDF base_link -> laser_link

ROS2 TF 基于 urdf 定义通过 robot_state_publisher 发布 base_link, base_footprint, laser_link 静态坐标关系。

雷达与小车是刚性连接，可以使用小车的 urdf 文件定义相应的 TF 坐标系转换。

如下，给 base_link 与 laser_link 定义一个 fixed joint 的刚性连接。

xml 复制代码 
<!-- base link -->
  <link name="base_link">
  <visual>
    <origin xyz="0 0 0.0" rpy="0 0 0" />
    <geometry>
      <cylinder length="0.12" radius="0.10" />
    </geometry>
    <material name="blue">
      <color rgba="0.1 0.1 1.0 0.5" />
    </material>
  </visual>
  </link>
  
 <!-- laser link -->
  <link name="laser_link">
  <visual>
    <origin xyz="0 0 0" rpy="0 0 0" />
    <geometry>
      <cylinder length="0.02" radius="0.02" />
    </geometry>
    <material name="black">
      <color rgba="0.0 0.0 0.0 0.5" />
    </material>
  </visual>
  </link>
  <joint name="laser_joint" type="fixed">
    <parent link="base_link" />
    <child link="laser_link" />
    <origin xyz="0 0 0.075" rpy="0 0 0" />
  </joint>

urdf2tf.launch.py 中使用 robot_state_publisher 和 joint_state_publisher 两个节点发布 /tf 和 /tf_static topic ，将 urdf 中定义的 tf 坐标系转换关系发布，则可以生成 base_link -> laser_link 的 tf 数据。

ini 复制代码 
# 状态发布节点
    robot_state_publisher_node = launch_ros.actions.Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        parameters=[{'robot_description': robot_description}]
    )
    # 关节状态发布节点
    joint_state_publisher_node = launch_ros.actions.Node(
        package='joint_state_publisher',
        executable='joint_state_publisher',
    )
其次是里程计 odom -> base_link

ROS2 odom2tf node 订阅 /odom 话题，生成 odom 与 base_link 的坐标关系发布到动态 TF

odom 里程计的数据来源一般是IMU、激光雷达或者车轮编码器。这里使用的小鱼小车的主控中实现了基于 车轮编码器的里程计 。但 TF 数据需要在上位机中通过订阅 odom topic 的数据，并发表到 TF topic 中，所以编写一个 odom2TF 的转换 Node

ini 复制代码 
class OdomTopic2TF : public rclcpp::Node {
public:
  OdomTopic2TF(std::string name) : Node(name) {
    // 创建 odom 话题订阅者，使用传感器数据的 Qos
    odom_subscribe_ = this->create_subscription<nav_msgs::msg::Odometry>(
        "odom", rclcpp::SensorDataQoS(),
        std::bind(&OdomTopic2TF::odom_callback_, this, std::placeholders::_1));
    // 创建一个tf2_ros::TransformBroadcaster用于广播坐标变换
    tf_broadcaster_ = std::make_unique<tf2_ros::TransformBroadcaster>(this);
  }

private:
  rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_subscribe_;
  std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
  // 回调函数，处理接收到的odom消息，并发布tf
  void odom_callback_(const nav_msgs::msg::Odometry::SharedPtr msg) {
    geometry_msgs::msg::TransformStamped transform;
    transform.header = msg->header; // 使用消息的时间戳和框架ID
    transform.child_frame_id = msg->child_frame_id;
    transform.transform.translation.x = msg->pose.pose.position.x;
    transform.transform.translation.y = msg->pose.pose.position.y;
    transform.transform.translation.z = msg->pose.pose.position.z;
    transform.transform.rotation.x = msg->pose.pose.orientation.x;
    transform.transform.rotation.y = msg->pose.pose.orientation.y;
    transform.transform.rotation.z = msg->pose.pose.orientation.z;
    transform.transform.rotation.w = msg->pose.pose.orientation.w;
    // 广播坐标变换信息
    tf_broadcaster_->sendTransform(transform);
  };
};
最后是 map -> odom

  • 建图时可由 SLAM 定位与建图模块产生 map -> odom 的 TF 数据

  • 导航时可由 Nav2项目提供的 amcl 是一种基于粒子过滤器的自适应蒙特卡罗定位技术，用于静态地图的定位，产生 map -> odom 的 TF 数据时

编写 bringup.launch.py

在 bringup.launch.py 文件增加 urdf2tf 和 odom2tf 两个 TF 数据相关节点

ini 复制代码 
    urdf2tf = launch.actions.IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
                [fishbot_bringup_dir, '/launch', '/urdf2tf.launch.py']),
    )

    odom2tf = launch_ros.actions.Node(
        package='fishbot_bringup',
        executable='odom2tf',
        output='screen'
    )

雷达数据

雷达板烧录固件，这个固件的作用就是将串口转成无线 TCP, 其中配置的 server------ip 则是数据接受端的 IP

clone ros_serial2wifi 仓库，该仓库负责接送雷达固件发送的雷达数据，并生成一个虚拟串口，将雷达数据写入到这个虚拟串口。所以雷达版与 ros_serial2wifi 要求接入同一个 wifi，确保网络连通。

bash 复制代码 
cd <your_path>/fishbot_ws/src
git clone https://github.com/fishros/ros_serial2wifi.git

clone ydlidar_ros2 仓库，该仓库是雷达驱动代码，使用 CYdLidar 从串口读取雷达数据，通过 ros2 把读取到的雷达数据发布到 "/scan" topic

bash 复制代码 
cd <your_path>/fishbot_ws/src
git clone https://github.com/fishros/ydlidar_ros2 -b  v1.0.0/fishbot

在编译前修改配置文件 ydlidar_ros2/params/ydlidar.yaml 中的参数 port 串口端口号到这个虚拟串口，一般是 /tmp/laserport, 然后编译构建。

确定雷达连接上，然后启动雷达驱动就可以了。

yaml 复制代码 
ydlidar_node:
  ros__parameters:
    port: /tmp/laserport
编写 bringup.launch.py 增加雷达启动 node
ini 复制代码 
    ros_serail2wifi =  launch_ros.actions.Node(
        package='ros_serail2wifi',
        executable='tcp_server',
        parameters=[{'serial_port': '/tmp/laserport'}],
        output='screen'
    )

    ydlidar = launch.actions.IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            [ydlidar_ros2_dir, '/launch', '/ydlidar_launch.py']),
    )
yaml 复制代码 
/usr/bin/ld: cannot find /lib64/libm.so.6: No such file or directory 
/usr/bin/ld: cannot find /lib64/libmvec.so.1: No such file or directory 
collect2: error: ld returned 1 exit status                 
gmake[2]: *** [CMakeFiles/ydlidar_node.dir/build.make:295: ydlidar_node] Error
gmake[1]: *** [CMakeFiles/Makefile2:139: CMakeFiles/ydlidar_node.dir/all] Error 2                                                    
gmake[1]: *** Waiting for unfinished jobs....                 
gmake: *** [Makefile:146: all] Error 2                          
---                                                    
Failed   <<< ydlidar [6.45s, exited with code 2]

首先，我们需要确认系统中确实缺失了libm.so库。可以通过以下命令来查找：

css 复制代码 
ldconfig -p | grep libm.so
bash 复制代码 
ldconfig -p | grep libm.so                                     
        libmysofa.so.1 (libc6,x86-64) => /lib/x86_64-linux-gnu/libmysofa.so.1
        libm.so.6 (libc6,x86-64, OS ABI: Linux 3.2.0) => /lib/x86_64-linux-gnu/libm.so.6                                                     
        libm.so.6 (libc6, OS ABI: Linux 3.2.0) => /lib/i386-linux-gnu/libm.so.6

Ubuntu 的 64-bit 库在 /usr/lib/x86_64-linux-gnu，不在 /lib64；
让链接器去那里找（--sysroot=/ 或 -L）就行，或者临时补个软链即可

Ubuntu 默认没有把 /lib64 作为 64-bit 库的搜索路径，而是统一放在
/usr/lib/x86_64-linux-gnu/。

因此 真正的"缺失"并不是文件本身，而是链接器（ld）在 /lib64 里去找它------这通常是下面两种场景造成的：

  • 你正在用 非发行版自带的交叉工具链 （例如 CodeSourcery、Linaro、Yocto SDK ...）
    这些工具链里 ld 的默认 sysroot 是 /lib64:/lib:/usr/lib64:/usr/lib，
    所以它会先去 /lib64 下找 libm.so.6，找不到就报错。
  • 你手动给 ld 传了 -L/lib64 或者把 /lib64 写到了 LIBRARY_PATH / ld.so.conf，
    结果它只认这个目录，而忽略了 Ubuntu 实际存放库的路径。

把工具链的 sysroot 指向 Ubuntu 的根目录 ，让它去 /usr/lib/x86_64-linux-gnu 找库：

ini 复制代码 
# 对本地 gcc/ld 生效
export LIBRARY_PATH=/usr/lib/x86_64-linux-gnu:$LIBRARY_PATH
export LD_LIBRARY_PATH=/usr/lib/x86_64-linux-gnu:$LD_LIBRARY_PATH

# 如果你用交叉工具链
<your>-gcc --sysroot=/  ...
# 或者
<your>-ld -L/usr/lib/x86_64-linux-gnu  ...

colcon build 本身不会调用编译器，它只是把 cmake / ament_cmake 的构建命令分发到各个 ROS package。

真正决定 "去哪里找 libm.so.6" 的是：

  • 每个 package 的 CMakeLists.txt 里有没有显式写死 -L/lib64 之类的路径；
  • 工具链的默认 library search path （ld --verbose | grep SEARCH_DIR 能看到）。

因此要让链接阶段去 /usr/lib/x86_64-linux-gnu 而不是 /lib64，只需要 把该目录塞进 CMake 的链接搜索路径 即可，下面给出 3 种常用办法，按"侵入性"从小到大排序，任选其一。

  1. LIBRARY_PATH 是 gcc/g++ 在 链接阶段 用的搜索路径，不会影响运行时。
    只在当前终端生效，不会弄脏系统。
bash 复制代码 
# 对 GNU 工具链生效
export LIBRARY_PATH=/usr/lib/x86_64-linux-gnu:$LIBRARY_PATH
export LD_LIBRARY_PATH=/usr/lib/x86_64-linux-gnu:$LD_LIBRARY_PATH

# 开始编译
colcon build --symlink-install
  1. 一次性给 colcon 传 CMake 参数
ini 复制代码 
colcon build \
  --cmake-args \
    -DCMAKE_C_FLAGS="-L/usr/lib/x86_64-linux-gnu" \
    -DCMAKE_CXX_FLAGS="-L/usr/lib/x86_64-linux-gnu" \
  --symlink-install
  1. 修改 CMakefile.txt
scss 复制代码 
set(CMAKE_SYSTEM_NAME Linux)
set(CMAKE_SYSTEM_PROCESSOR x86_64)
# 让编译器把"/"当成 sysroot，自然就能找到 /usr/lib/x86_64-linux-gnu
set(CMAKE_SYSROOT /)
perl 复制代码 
# 先清理
colcon build --cmake-clean-cache

colcon build --packages-select <pkg_name> --cmake-clean-cache

# 任选上面一种办法重新 build
colcon build --symlink-install

# 重新编译，并把 stdout+stderr 全部留到文件
colcon build --packages-select <pkg_name> --symlink-install \
             --event-handlers console_direct+ \
             2>&1 | tee build.log
             
# 看具体某个 package 的链接命令
ninja -C build/<pkg_name> -v | grep libm.so
# 应该出现 /usr/lib/x86_64-linux-gnu/libm.so.6 而不再报 /lib64
  1. 激光雷达驱动负责将雷达数据发布到 ROS2 系统中 /scan 话题。

启动小车 bringup.launch.py

bash 复制代码 
source install/setup.bash
ros2 launch fishbot_bringup bringup.launch.py

bringup.launch.py 源码

github.com/fishros/ros... 仓库中包含了上面用到的 bringup.launch.py 和 odom2tf 的源码

SLAM

juejin.cn/post/755630...

概述

所有的自主机器人都绕不开一个问题，即在陌生环境中，需要知道周边是啥样（建图），需要知道我在哪（定位），于是有了SLAM 课题的研究。SLAM在室内机器人，自动驾驶汽车建图，VR/AR穿戴等领域都有广泛的应用。

SLAM：Simultaneous Localization and Mapping，中文是即时定位与地图构建，所谓的SLAM算法准确说是能实现SLAM功能的算法，而不是某一个具体算法。

SLAM算法根据依赖的传感器不同，可以分为 激光SLAM 和 视觉SLAM ，前者是激光雷达，后者是能提供深度信息的摄像头，如双目摄像头，红外摄像头等。除此之外，SLAM算法通常还依赖里程计提供距离信息，否则地图很难无缝的拼接起来，很容易跑飞。

SLAM 建图服务订阅 /scan 雷达数据，通过 SLAM 算法生成地图数据，并发布到 /map 话题，并生成 map -> odom TF 数据，发布到 /tf

  • Subscribed Topics
    • /scan:
  • Published Topics
    • /map
    • /map_update:
  • TF
    • map -> odom

在 ROS 2 中，提供了很多的 SLAM 功能包，比如 slam_toolbox，cartographer_ros 和 rtabmap_slam 等。针对二维场景，其中 slam_toolbox 开箱即用，上手较为简单，所以使用 slam_toolbox 提供 SLAM 服务。

slam_toolbox

SLAM Toolbox 是一个强大的开源库，专为动态环境中的二维SLAM（同时定位与地图构建）而设计。该项目由 Steve Macenski 在 Simbe Robotics 创立，并在他在三星研究院的期间以及业余时间进行了维护和更新。SLAM Toolbox 不仅提供了常规的移动机器人所需的 2D SLAM 功能，如启动、绘图、保存地图等，还包括了一些高级特性，如地图的连续细化、重映射、以及基于优化的定位模式等。

  • ROS Node：SLAM toolbox 以同步模式运行，生成 ROS Node。此节点订阅激光扫描 scan 和里程计 odom topic，并发布 map -> odom TF 信息和 map。

  • Get odometry and LIDAR data： LIDAR data callback 中将里程计数据生成 pose 和一个次激光扫描 LIDAR data 组合成一个 PosedScan 对象。这些 PosedScan 对象形成一个队列，由算法处理。

  • Process Data： PosedScan 对象的队列用于构建 pose graph；使用 laser scan matching 进行里程计 odom 校正。pose graph 用于计算机位姿，并找到循环闭包。如果发现循环闭合，则优化 pose graph，并更新姿态估计。姿态估计用于计算和发布 map -> odom 变换 TF 数据

  • Mapping：使用与姿势图中每个姿势相关的激光扫描来构建和发布地图。

  • docs: docs.ros.org/en/humble/p...

Subscribed topics
Topic Type Description
/scan sensor_msgs/LaserScan the input scan from your laser to utilize
tf N/A a valid transform from your configured odom_frame to base_frame
Published topics
Topic Type Description
map nav_msgs/OccupancyGrid occupancy grid representation of the pose-graph at map_update_interval frequency
pose geometry_msgs/PoseWithCovarianceStamped pose of the base_frame in the configured map_frame along with the covariance calculated from the scan match
核心功能与AMCL对比

fishros.org.cn/forum/topic...

SLAM Toolbox 的设计目标是提供一个比其他SLAM库（更全面的解决方案。其核心功能包括：

  • 基础2D SLAM：允许用户启动地图构建，保存为PGM文件，同时提供了地图保存等实用工具。
  • 序列化和反序列化：能够在任何时间点加载已保存的位姿图（pose-graph），继续地图的绘制或修改。
  • 终身映射：加载已保存的位姿图，继续在空间中绘图，同时从新加入的扫描中删除多余的信息。
  • 优化基定位模式：在没有先验地图的情况下，通过位姿图进行"激光里程计"模式定位，支持本地闭环。

与AMCL（自适应蒙特卡罗定位方法）相比，SLAM Toolbox 提供了更为动态和灵活的地图处理能力。AMCL主要用于在已知地图中进行定位，而SLAM Toolbox 则允许用户不仅进行定位，还可以继续更新和优化地图。此外，SLAM Toolbox 支持的"终身映射"功能使得它可以在长时间内持续更新地图，这是AMCL所不支持的。

高级应用

除了基础的地图绘制和保存功能，SLAM Toolbox 还提供了几种高级应用：

  • 生命周期管理：在多次会话中对同一区域进行映射，允许用户创建和更新现有地图，并将数据序列化以供其他映射会话使用。
  • 动态本地化：将现有的序列化地图加载到节点中，维护最近扫描的滚动缓冲区，并在缓冲区过期后移除扫描，但不影响底层地图。
安装与运行
  • 安装 slam_toolbox
bash 复制代码 
sudo apt install ros-$ROS_DISTRO-slam-toolbox
  • 运行异步建图服务
go 复制代码 
ros2 launch slam_toolbox online_async_launch.py  use_sim_time:=False
编写 bringup.launch.py 增加 slam_toolbox

要在你的 bringup.launch.py 文件中增加对 slam_toolbox 的 online_sync_launch.py 的启动，可以按照以下方式操作：

python 复制代码 
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from ament_index_python.packages import get_package_share_directory
import os

def generate_launch_description():
    # 获取 slam_toolbox 包的路径
    slam_toolbox_dir = get_package_share_directory('slam_toolbox')

    # 构建 online_sync_launch.py 的完整路径
    online_sync_launch_path = os.path.join(
        slam_toolbox_dir,
        'launch',
        'online_sync_launch.py'
    )

    # 包含并启动 online_sync_launch.py
    slam_toolbox_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(online_sync_launch_path)
    )

    return LaunchDescription([
        slam_toolbox_launch,
        # 你可以在这里添加其他节点或 launch 文件
    ])

如果你需要传递参数（如 params_file 或 use_sim_time），可以这样做：

scss 复制代码 
slam_toolbox_launch = IncludeLaunchDescription(
    PythonLaunchDescriptionSource(online_sync_launch_path),
    launch_arguments={
        'use_sim_time': 'true',
        'slam_params_file': '/path/to/your/slam_params.yaml'
    }.items()
)

配置文件介绍：

  • mapper_params_online_async.yaml: 该文件包含异步在线建图的参数，如传感器数据处理频率、地图分辨率等。
  • mapper_params_online_sync.yaml: 该文件包含同步在线建图的参数，如传感器数据处理频率、地图分辨率等。
  • mapper_params_localization.yaml: 该文件包含定位的参数，如初始位置估计、定位精度要求等。
  • serialization_flags.yaml: 该文件包含序列化工具的参数，如地图文件路径、序列化模式等。

Nav2具有下列工具：

  • 加载、提供和存储地图的工具（地图服务器Map Server）
  • 在地图上定位机器人的工具 (AMCL)
  • 避开障碍物从A点移动到B点的路径规划工具（Nav2 Planner）
  • 跟随路径过程中控制机器人的工具（Nav2 Controller）
  • 将传感器数据转换为机器人世界中的成本地图表达的工具（Nav2 Costmap 2D）
  • 使用行为树构建复杂机器人行为的工具（Nav2 行为树和BT Navigator）
  • 发生故障时计算恢复行为的工具（Nav2 Recoveries）
  • 跟随顺序航点的工具（Nav2 Waypoint Follower）
  • 管理服务器生命周期的工具和看门狗(Nav2 Lifecycle Manager)
  • 启用用户自定义算法和行为的插件（Nav2 Core）

在Navigation2中，各个功能的组织则有所不同。全局路径规划功能由各个全局路径规划的插件实现，这些插件被加载到Planer Server中。可以通过相应的action向Planer Server请求全局规划的路径。

同理，局部路径规划器也是以插件的形式实现的。它们有统一的接口，可以被加载到Controller Server中。当需要执行某条路径的时候，只需要通过相应的action将要执行的路径发送给Controller Server即可。

Recovery Server也是以相同的逻辑来工作的。

Planer Server 、 Controller Server 和 Recovery Server分别实现了各自的功能，即全局路径规划，局部路径规划和恢复操作。但它们的功能是独立的，互不干扰的。而要实现一个完整的导航功能需要各个模块的配合。BT Navigator Server就是那个组装乐高积木的人。

每一个功能就是一棵行为树。这棵树上的节点将与Planer Server 、 Controller Server 和 Recovery Server通信，把各个server的功能组合起来。比如单点导航功能，行为树上的相应节点会向Planer Server 请求一条规划到目标点的路径。然后另一个树上的节点与Controller Server通信，将规划好的路径发给Controller Server去执行。当然这里只是简单说明一下流程，实际使用时可以组合更多的功能。

当BT Navigator Server中实现了单点导航的功能，便可顺理成章的实现多点导航。Waypoint Follower通过连续向BT Navigator Server请求单点导航功能即可实现多点导航。当之前请求的单点导航完成后，再发送下一个单点导航请求。直到所有的点都已下发。

www.behaviortree.dev/docs/intro

上面框图外围的map、Sensor Data和Tf则是共有数据。各个功能模块都可以获取到，按需取用即可。

可以简单分为一大三小四个服务。

一大： BT Navigator Server 导航行为树服务，通过这个大的服务来进行下面三个小服务组织和调用。

三小：

  • Planner Server，规划服务器，其任务是计算完成一些目标函数的路径。根据所选的命名法和算法，该路径也可以称为路线。说白了就是在地图上找路。
  • Controller Server，控制服务器，在ROS 1中也被称为局部规划器，是我们跟随全局计算路径或完成局部任务的方法。说白了就是根据找的路控制机器人走路。
  • Recovery Server，恢复服务器，恢复器是容错系统的支柱。恢复器的目标是处理系统的未知状况或故障状况并自主处理这些状况。说白了就是机器人可能出现意外的时候想办法让其正常，比如走着走着掉坑如何爬出来。 通过规划路径、控制机器人沿着路径运动、遇到问题自主恢复三者进行不断切换完成机器人的自主导航

官方文档：

navigation.ros.org/getting_sta...

Navigation2官方github：

其中的issue讨论挺值得看的

github.com/ros-plannin...

Navigation2 对应的论文：

arxiv.org/pdf/2003.00...

Navigation 2系列教程：

zhuanlan.zhihu.com/p/384099348

目前Navigation2提供的插件：

navigation.ros.org/plugins/ind...

Environmental Representation

环境表示是机器人感知其周围环境的方式。它还作为各类算法和数据源的中央定位点，将信息整合至统一空间。该空间随后被控制器、规划器和恢复系统用于安全高效地完成任务计算。

当前的环境表示是一个 costmap。costmap 是一个规则的二维网格，每个单元格包含状态 unknown, free, occupied, or inflated cost。该 costmap 随后会被搜索以计算全局路径规划，或进行采样以计算局部控制指令。

各种 costmap layers 通过插件库实现，用于将信息缓冲至 costmap 中。这些信息包括激光雷达、雷达、声纳、深度图像等传感器数据。在将传感器数据输入 costmap layers 前进行处理可能更为明智，但具体操作取决于开发者。

可通过创建成本图层，利用摄像头或深度传感器检测并追踪场景中的障碍物以实现避障。此外，还可创建图层根据特定规则或启发式方法算法化地改变底层成本图。最后，这些图层可用于将实时数据缓冲至二维或三维世界以进行二值化障碍物标记。

在机器人导航的时候，机器人在运动过程中可能会出现新的障碍物，也有可能发现原始地图中某一块的障碍物消失了，可通过创建 costmap layer，利用摄像头或深度传感器检测并追踪场景中的障碍物以实现避障。此外，还可创建图层根据特定规则或启发式方法算法化地改变底层 costmap。最后，这些 layers 可用于将实时数据缓冲至二维或三维世界以进行二值化障碍物标记。所以在机器人导航过程中维护的地图是一个动态的地图，根据更新频率方式和用途不同，可以分为下面两种。

  1. 全局代价地图 （Global Costmap） 全局代价地图主要用于全局的路径规划器 Planner Server 中。

通常包含的 costmap layers 有：

  • Static Map Layer：静态地图层，通常都是SLAM建立完成的静态地图。
  • Obstacle Map Layer：障碍地图层，用于动态的记录传感器感知到的障碍物信息。
  • Inflation Layer：膨胀层，在以上两层地图上进行膨胀（向外扩张），以避免机器人的外壳会撞上障碍物。
  1. 局部代价地图（Local Costmap） 局部代价地图主要用于局部的路径规划器 Controller Server 中。

通常包含的图层有：

  • Obstacle Map Layer：障碍地图层，用于动态的记录传感器感知到的障碍物信息。
  • Inflation Layer：膨胀层，在障碍地图层上进行膨胀（向外扩张），以避免机器人的外壳会撞上障碍物。

State Estimation

docs.nav2.org/concepts/in...

nav2 没有必要一定使用激光雷达来实现导航，也可以使用其他传感器来进行，但是无论是使用激光雷达或者是其他传感器，都必须要遵循这个标准 REP 105，根据ROS社区的项目标准 REP 105 ，导航的项目里面需要提供主要两个坐标转换是 map -> odom 和 odom -> base_link。

Global Positioning: Localization and SLAM map -> odom

Nav2 默认定位模块是AMCL，在已知地图中定位

  • 激光扫描：/scan (sensor_msgs/LaserScan) (由激光雷达发布)
  • TF: 将 odom -> base_link
  • 初始姿势：/initialpose (geometry_msgs/PoseWithCovarianceStamped)
  • 地图：/map（nav_msgs/OccupancyGrid）

上列数据算法经过计算之后，就会建立起 map 和 odom 之 间的 TF 关系，可以利用静态地图作 Scan-To-Map 来提高定位精度.

Nav2 也支持使用 slam_toolbox 作为默认的SLAM算法，用于定位和生成静态地图。

Odometry odom -> base_linker

上面小车坐标系转换 TF 中通过 ROS2 odom2tf node 订阅的小车主控里程计发布的 odom topic 数据生成了 odom -> base_linker 的 TF 数据。

里程计系统的作用是提供 odom -> base_link TF 数据。测距可以来自许多来源，包括激光雷达、雷达、车轮编码器、VIO和IMU。里程计的目标是基于机器人运动提供平滑连续的局部坐标。全局定位系统将相对于全局坐标的转换进行更新，以考虑里程漂移。

上面小鱼小车使用的 ROS2 odom2tf node 较为简单，只使用了车轮编码器 odom 的数据计算 TF 数据。

robot_localization 通常用于融合多种数据计算里程计。它将接收N个不同类型的传感器，并为TF和一个主题提供连续平滑的里程计。典型的移动机器人设置可能会以这种方式融合来自车轮编码器、IMU和视觉的里程计。

平滑的输出可用于精确运动的导航推算，并在全局位置更新之间准确更新机器人的位置。

docs.nav2.org/tutorials/d...

因为要实现自主探索建图，所以也需要 Nav2 运行在地图动态更新的场景下，所以 Nav2 会基于 SLAM 提供的动态地图 /map 运行，所以不加载静态 map ，不启动 AMCL。

启动 Navigation2（不指定 map 文件）

go 复制代码 
ros2 launch nav2_bringup navigation_launch.py use_sim_time:=True

注意：不要加 map:=xxx.yaml 参数，否则 Nav2 会尝试加载静态地图。

启动 RViz2 并设置初始位姿

复制代码 
rviz2
  • 使用 2D Pose Estimate 设置机器人初始位置。
  • 使用 2D Nav Goal 设置导航目标。

注意事项：

  • 确保 use_sim_time 参数与你的系统一致（仿真中设为 True）。
  • 确保 slam_toolbox 的参数文件中启用了地图发布。

Launch.py 编写

ini 复制代码 
import os
import launch
import launch_ros
from ament_index_python.packages import get_package_share_directory
from launch.launch_description_sources import PythonLaunchDescriptionSource


def generate_launch_description():
    # 获取与拼接默认路径
    fishbot_navigation2_dir = get_package_share_directory(
        'fishbot_navigation2')
    nav2_bringup_dir = get_package_share_directory('nav2_bringup')
    rviz_config_dir = os.path.join(
        nav2_bringup_dir, 'rviz', 'nav2_default_view.rviz')
    
    # 创建 Launch 配置
    use_sim_time = launch.substitutions.LaunchConfiguration(
        'use_sim_time', default='false')
    nav2_param_path = launch.substitutions.LaunchConfiguration(
        'params_file', default=os.path.join(fishbot_navigation2_dir, 'config', 'nav2_params.yaml'))

    return launch.LaunchDescription([
        # 声明新的 Launch 参数
        launch.actions.DeclareLaunchArgument('use_sim_time', default_value=use_sim_time,
                                             description='Use simulation (Gazebo) clock if true'),
        launch.actions.DeclareLaunchArgument('params_file', default_value=nav2_param_path,
                                             description='Full path to param file to load'),

        launch.actions.IncludeLaunchDescription(
            # PythonLaunchDescriptionSource(
                # [nav2_bringup_dir, '/launch', '/bringup_launch.py']),
            PythonLaunchDescriptionSource(
                [nav2_bringup_dir, '/launch', '/navigation_launch.py']),
            # 使用 Launch 参数替换原有参数
            launch_arguments={
                'use_sim_time': use_sim_time,
                'params_file': nav2_param_path}.items(),
        ),
        launch_ros.actions.Node(
            package='rviz2',
            executable='rviz2',
            name='rviz2',
            arguments=['-d', rviz_config_dir],
            parameters=[{'use_sim_time': use_sim_time}],
            output='screen'),
    ])
目标 方法
不使用静态地图 不加载 .yaml 地图文件
使用动态地图 启动 SLAM 节点（如 slam_toolbox）
同时建图与导航 启动 SLAM + Nav2，并设置初始位姿

如果你使用的是特定平台（如 Isaac Sim、TurtleBot3、Nova Carter 等），它们通常提供了封装好的 launch 文件，支持 slam:=True 参数来一键启动动态地图导航。

robot_localization

robot_localization是基于卡尔曼滤波在ROS系统上比较成熟、应用比较广泛的一个机器人动态定位软件包。 robot_localization软件包中使用的定位算法并不是最时新最优秀的，但是它具备几个不可替代的优势：

  • 它有专门的逻辑融合GPS定位信息，可以支持户外定位
  • 它能够融合多种传感器数据，支持3D空间定位
  • 与ROS系统的集成由来已久，深得人心，普及率挺好。

robot_localization实现了几个机器人状态估计（State Estimation)节点 。每个节点(Node，ROS术语，一个Node是一个独立的进程空间，具备自己的上下文与生命周期)都是非线性状态估计器的一种实现，用于在3D空间中移动的机器人。它包括两个状态估计节点ekf_localization_node和ukf_localization_node。另外，robot_localization提供navsat_transform_node，它实现对GPS数据的集成。

robot_localization具备如下的一些特征：

  • 经过泛化并通用的状态估计软件包。
  • 融合任意数量的输入数据源 。节点不限制传感器的数量。例如，如果您的机器人具有多个IMU或里程计信息，则robot_localization中的状态估计节点可以支持所有传感器。
  • 支持多种ROS消息类型 。robot_localization中的所有状态估计节点都可以接收的消息类型包括：
  • 自定义 每个传感器的输入。如果给定的传感器消息包含您不希望包含在状态估计中的数据，则robot_localization中的状态估计节点允许您排除该数据。
  • 持续估计 。robot_localization中的每个状态估计节点在收到一次测量结果后便开始估算机器人的状态。如果传感器数据中有间歇（即很长一段时间，没有收到任何数据），则滤波器将继续通过内部运动模型来估算机器人的状态。 这个对于规则性运动具备很好的维持性，这个也符合机器人、无人机等短时间内的运动预期。

robot_localization使用15维向量来表示机器人的运动状态：

每3个数据一组，分别表示：

  • x-y-z坐标系的坐标（机器人位置）、
  • 绕x/y/z轴的角度（机器人方向）、
  • 沿x/y/z轴的速度、
  • 绕x/y/z轴的角速度、
  • 沿x/y/z轴的加速度。

安装

  • 直接安装 ROS2 包
swift 复制代码 
sudo apt-get install ros-$ROS_DISTRO-robot-localization
  • 源码安装
bash 复制代码 
mkdir -p cra-ros-pkg/src
cd cra-ros-pkg/src
git clone --depth 1 --branch $ROS_DISTRO-devel https://github.com/cra-ros-pkg/robot_localization 
cd ..
colcon build
source install/setup.bash

小鱼小车 robot_localization

fishros.org.cn/forum/topic...

fishros.org.cn/forum/topic...

配置 bringup.launch.py 文件，增加 robot_localization 的 ekf_node

ini 复制代码 
robot_localization_param_path = launch.substitutions.LaunchConfiguration(
        'params_file', default=os.path.join(fishbot_bringup_dir, 'config', 'ekf_filter_node.yaml'))
    robot_localization_node = launch_ros.actions.Node(
        package='robot_localization',
        executable='ekf_node',
        name='ekf_filter_node',
        output='screen',
        parameters=[
            robot_localization_param_path, 
            {'use_sim_time': use_sim_time}
        ]
    )

ekf_filter_node.yaml

yaml 复制代码 
### ekf config file ###
ekf_filter_node:
    ros__parameters:
        debug: true
        frequency: 30.0
        two_d_mode: false
        publish_acceleration: false
        publish_tf: true
        map_frame: map              # 如果未指定，默认为"map"
        odom_frame: odom            # 如果未指定，默认为"odom"
        base_link_frame: base_footprint  # 如果未指定，默认为"base_link"
        world_frame: odom           # 如果未指定，默认为odom_frame的值

        odom0: /odom
        odom0_config: [true,  true,  true,
                       false, false, false,
                       false, false, false,
                       false, false, true,
                       false, false, false]

        imu0: /imu
        imu0_config: [false, false, false,
                      true,  true,  true,
                      false, false, false,
                      false, false, false,
                      false, false, false]

m-explore-ros2

支持 ROS2 的用于多机器人自主探索 m-explore。目前已在Eloquent、Dashing、Foxy和Galactic发行版上进行了测试。

frontier_exploration:

frontier_exploration 是基于边界的自主探索算法，边界定义为已知空间和未知空间之间的区域（Frontiers are regions on the boundary between open space and unexplored space），通过BFS（广度优先搜索） 算法从机器人当前位置搜索边界。当节点运行时，机器人会贪婪地探索它的环境，直到找不到边界，它的移动命令会发送给 Nav2 NavigateToPose Action 进行移动。

m-explore 是一个基于ROS（Robot Operating System）的开源包集合，旨在为多机器人环境下的自主探索和地图合并提供强大的工具。这个项目包含两个主要的包：explore_lite 和 multirobot_map_merge，分别处理机器人探索任务和多机器人地图整合问题。

github: gitcode.com/gh_mirrors/...

explore_lite

该包提供了轻量级的单机器人探索算法，能够使机器人在未知环境中进行有效的自我导航，逐步覆盖整个区域，避免重复探索。它利用了ROS中的SLAM（Simultaneous Localization And Mapping）技术来构建和更新地图，并通过智能路径规划确保高效探索。

multirobot_map_merge

当有多台机器人共同工作时，multirobot_map_merge 包能将各个机器人的局部地图无缝地融合成全局地图。这依赖于先进的数据同步策略和精确的位置估计，使得多机器人系统可以协同作业，共同构建出更完整、准确的世界模型。

rrt_exploration

rrt_exploration 是 ROS 的功能包，基于 Rapidly-Exploring Random Tree RRT, 快速探索随机树是一种基于随机采样的路基规划算法，对高维状态空间的路径规划非常高效, 算法实现移动机器人的自主探索能力。使用栅格地图格式，整体架构包含 5 个不同功能的 Node 组成。

  • Global Frontier Detector Node: 采用占用网格并在其中找到边界点（即勘探目标）。它发布检测到的点，以便过滤器节点可以处理。在多机器人配置中，只打算运行此节点的单个实例。 如果需要，运行全局边界检测器的额外实例可以提高边界点检测的速度。

  • Local Frontier Detector Node: 此节点类似于global_rrt_frontier_detector。然而，它的工作方式不同，因为这里的树每次检测到边界点时都会不断重置。该节点旨在与global_rrt_frontier_detector节点一起运行，它负责快速检测机器人附近的边界点。 在多机器人配置中，每个机器人运行local_rrt_frontier_detector的一个实例。因此，对于一个由3个机器人组成的团队，将有4个节点用于检测边境点；3个局部探测器和1个全局探测器。如果需要，运行本地边界检测器的额外实例可以提高边界点检测的速度。 所有探测器将发布同一主题上探测到的前沿点（"/detected_points"）。

  • OpenCV-based frontier detector Node: 该节点是另一个边界检测器，但它不是基于RRT的。此节点使用OpenCV工具检测边界点。它旨在单独运行，在多机器人配置中，只应运行一个实例（运行此节点的其他实例没有任何区别）。 最初，该节点是为了与基于RRT的边界检测器进行比较而实现的。沿着RRT检测器（本地和全局）运行此节点可以提高蛙跳点检测的速度。 注意：您可以运行任何类型和任何数量的检测器，所有检测器都将发布在过滤器节点（将在下一节中解释）订阅的同一主题上。另一方面，过滤器将把过滤后的forntier点传递给分配器，以命令机器人探索这些点。

  • Filter Node: 过滤器节点从所有检测器接收检测到的边界点，过滤这些点，并将其传递给分配器节点以命令机器人。过滤包括删除旧的和无效的点，也包括对冗余点的识别。

  • Assigner Node: 该节点接收目标探索目标，即过滤节点发布的过滤边界点，并相应地命令机器人。分配器节点通过move_base_node命令机器人。这就是为什么你在机器人上打开了导航堆栈。

rrt_exploration 依赖的环境：机器人导航堆栈的 move_base_node 节点必须正常运行。rrt_exploration 生成目标探索目标点，每个机器人必须能够接收这些点并使用导航堆栈朝驱动机器人它们移动。此外，每个机器人都必须有一个全局和局部 costmap

rrt-explore(ROS2)

参考

相关推荐
JustTest6 小时前
Mac mini初始安装软件记录
程序员
SimonKing6 小时前
轻量级富文本编辑器Quill，保姆级教程，5分钟快速上手
java·后端·程序员
文心快码BaiduComate1 天前
Comate搭载Kimi K2.6，长程13h！
前端·后端·程序员
图图玩ai1 天前
SSH 命令管理工具怎么选？从命令收藏到批量执行一次讲清
linux·nginx·docker·ai·程序员·ssh·可视化·gmssh·批量命令执行
SamDeepThinking1 天前
程序员懂业务，到底要懂到什么程度
后端·程序员·团队管理
盖世英雄酱581361 天前
java技术博主停更3个月了？？？
程序员
DyLatte1 天前
我做了个AI项目后才发现：会做事的人，正在输给会讲故事的人
前端·后端·程序员
SimonKing1 天前
别让你的代码裸奔！Spring Boot混淆全攻略（附配置）
java·后端·程序员
前端双越老师1 天前
为什么我现在不安装 Hermes Agent
程序员·agent
怕浪猫2 天前
程序员越想转型AI，越不要只盯着技术
程序员
热门推荐
012026年4月技术前沿：AI大模型爆发、智能体革命与量子安全新纪元02GitHub 镜像站点032026年4月AI大事件深度解读：大模型竞争进入“深水区“042026 年 AI 编程助手全面对比评测：Cursor vs Copilot vs Claude Code vs GitHub Copilot Free05近期有什么ai的新消息，新动态？ 2026.4月06AI Weekly | 2026年4月第二周 · GitHub热门项目与AI发展趋势深度解析07Claude Code Windows 兼容性问题：指定版本 2.1.112 可解决08从限购到畅通：GLM-5.1 Coding Plan接入攻略09UBUNTU Claude Code 报错 claude native binary not installed10免费！不限量！用opencode接入英伟达（NVIDIA）大模型，轻松打造你的 AI 编程助手