简介

        本文基于ROS2 jazzy 和 Gazebo harmonic创建了一个可移动的小车机器人仿真环境，利用激光雷达对周边环境进行扫描并建图，使用SLAM建图、导航。

        项目连接：

        https://github.com/HulinCal/ros2_autopilot_robot.git

        预览：

1 机器人模型描述文件

        文件位置：src/qxwy_description/urdf/qxwy/base.urdf.xacro

    <xacro:macro name="base_xacro" params="length radius">
        <link name="base_footprint"/>

        <link name="base_link">
            <visual>
                <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
                <geometry>
                    <cylinder radius="${radius}" length="${length}"/>
                </geometry>
                <material name="white">
                    <color rgba="1.0 1.0 1.0 1.0"/>
                </material>
            </visual>

            <collision>
                <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
                <geometry>
                    <cylinder radius="${radius}" length="${length}"/>
                </geometry>
                <material name="white">
                    <color rgba="1.0 1.0 1.0 1.0"/>
                </material>
            </collision>

            <xacro:cylinder_inertial mass="1.0" radius="${radius}" height="${length}">
                 <origin xyz="0 0 0" rpy="0 0 0"/>
            </xacro:cylinder_inertial>
        </link>
        
        <joint name="joint_name" type="fixed">
            <origin xyz="0.0 0.0 ${length/2 + 0.032 - 0.001}" rpy="0.0 0.0 0.0"/>
            <parent link="base_footprint"/>
            <child link="base_link"/>
        </joint>
    </xacro:macro>

  <xacro:macro name="base_xacro" params="length radius">：

        -把机器人本体定义为一个宏，方便后面引用，名称为base_xacro，两个输入参数：length和radius。在qxwy-vehicle‌/src/qxwy_description/urdf/qxwy/qxwy.urdf.xacro文件中引用base_xacro。

<link name="base_footprint"/>：

        - base_footprint 是一个 虚拟 link （无几何形状），作为机器人的地面接触点和运动学基准
        - 在 URDF 中， base_link 通过固定关节连接到 base_footprint ，Z轴偏移为 ${length/2 +         0.032 - 0.001} （即机器人底盘中心到地面的高度）
        - 在 Gazebo 仿真启动时，通过静态变换发布器设置 base_footprint 在 odom 坐标系中的初始位置为 (0, 0, 0) ，姿态为 (0, 0, 0)。（位置：qxwy-vehicle‌/src/qxwy_description/launch/gazebo_sim.launch.py）

<link name="base_link">：

        基座部件定义。

        外观：<visual>包含初始位置<origin>、形状<geometry>、材质<metarial>，材质只定义了颜色<color>;

        碰撞属性：<collition>，碰撞属性的内容与外观的一致;

        惯性属性：<inertial>,这个使用了宏定义，位置：qxwy-vehicle‌/src/qxwy_description/urdf/qxwy/common_inertial.xacro。

<joint name="joint_name" type="fixed">：

        基座base_xarco和虚拟关机base_footprint的关节，连接方式为固定（fixed），父为base_footprint，<origin>定义的是base_xacro相对于base_footprint的相对位置：base_xacro的x,y方向不变，升高大概半个轮子的高度（让轮子在地面）。

        机器人本体会在qxwy-vehicle‌/src/qxwy_description/urdf/qxwy/qxwy.urdf.xacro文件中引入。

 其他部件描述文件：

        万向轮：qxwy_description/urdf/qxwy/actuator/caster.urdf.xacro

        轮子：qxwy_description/urdf/qxwy/actuator/wheel.urdf.xacro

        摄像头：qxwy_description/urdf/qxwy/sensor/camera.urdf.xacro

        惯性测量单元：qxwy_description/urdf/qxwy/sensor/imu.urdf.xacro

        激光雷达：qxwy_description/urdf/qxwy/sensor/laser.urdf.xacro

        这些部件将会在qxwy-vehicle‌/src/qxwy_description/urdf/qxwy/qxwy.urdf.xacro文件中与机器人基座一同导入。

        模型在gazebo中显示如下：

        

2 使用gazebo的传感器

2.1 激光雷达

        在qxwy-vehicle‌/src/qxwy_description/urdf/qxwy/sensor/laser.urdf.xacro文件进行配置，内容如下：

        <gazebo reference="laser_link">
            <material>Gazebo/Black</material>
            <sensor name='gpu_lidar' type='gpu_lidar'>
                <always_on>true</always_on>
                <topic>scan</topic>
                <update_rate>20</update_rate>
                <visualize>true</visualize> 
                <lidar>
                    <scan>
                        <horizontal>
                            <samples>720</samples>
                            <resolution>1</resolution>
                            <min_angle>-3.14159</min_angle>
                            <max_angle>3.14159</max_angle>
                        </horizontal>

                    <vertical>
                        <samples>16</samples>
                        <resolution>1</resolution>
                        <min_angle>-0.261799</min_angle>
                        <max_angle>0.261799</max_angle>
                    </vertical>
                    </scan>

                    <range>
                        <min>0.12</min>
                        <max>8.0</max>
                        <resolution>0.01</resolution>
                    </range>

                    <noise>
                        <type>gaussian</type>
                        <mean>0.0</mean>
                        <stddev>0.01</stddev>
                    </noise>
                </lidar>     
            </sensor>
        </gazebo>

基础的关联和外观

        <gazebo reference="laser_link">
            <material>Gazebo/Black</material>

    ‌        reference="laser_link"‌：指定该传感器附着在名为 laser_link 的连杆上。传感器的坐标系原点将与该 Link 的原点重合。
    ‌        <material>Gazebo/Black</material>‌：设置该 Link 在 Gazebo 可视化界面中的材质颜色为黑色。这仅影响视觉显示，不影响传感器数据。

传感器核心属性
    <sensor name='gpu_lidar' type='gpu_lidar'>
        <always_on>true</always_on>
        <topic>scan</topic>
        <update_rate>20</update_rate>
        <visualize>true</visualize> 

    ‌        type='gpu_lidar'‌：关键标识。声明这是一个基于 GPU 加速的激光雷达。它比传统的 ray 类型更适合高密度、多线束的 3D 激光雷达仿真。
    ‌        <always_on>true</always_on>‌：即使没有 ROS/GZ 节点订阅数据，传感器也持续运行。设为 false 可在无人订阅时节省算力。
    ‌        <topic>scan</topic>‌：定义 Gazebo 内部发布数据的话题名称为 /scan。
    ‌        <update_rate>20</update_rate>‌：数据更新频率为 ‌20 Hz‌。
    ‌        <visualize>true</visualize>‌：在 Gazebo 图形界面中实时渲染激光点云或扫描线。调试完成后建议设为 false 以提升仿真帧率。

 激光扫描几何参数 (Lidar Scan)

        这是定义激光雷达“如何扫描”的核心部分，决定了它是单线还是多线，以及覆盖范围。
        <lidar>
            <scan>
                <horizontal>
                    <samples>720</samples>
                    <resolution>1</resolution>
                    <min_angle>-3.14159</min_angle>
                    <max_angle>3.14159</max_angle>
                </horizontal>
                <vertical>
                    <samples>16</samples>
                    <resolution>1</resolution>
                    <min_angle>-0.261799</min_angle>
                    <max_angle>0.261799</max_angle>
                </vertical>
            </scan>

    ‌水平方向 (Horizontal)‌：
        ‌samples=720‌：水平一圈采集 ‌720 个点‌。
        ‌min/max_angle‌：−π−π 到 +π+π（即 -180° 到 +180°），表示水平方向全覆盖（360°）。
        ‌角分辨率‌：360∘/720=0.5∘360∘/720=0.5∘。

    ‌垂直方向 (Vertical)‌：
        ‌samples=16‌：垂直方向有 ‌16 线‌（16 beams）。这表明它是一个 ‌16 线 3D 激光雷达‌（类似 Velodyne VLP-16）。
        ‌min/max_angle‌：−0.261799 rad≈−15∘−0.261799 rad≈−15∘， +0.261799 rad≈+15∘+0.261799 rad≈+15∘。垂直视场角总共约 30°。
        ‌垂直分辨率‌：30∘/16≈1.875∘30∘/16≈1.875∘。

测距范围与精度
            <range>
                <min>0.12</min>
                <max>8.0</max>
                <resolution>0.01</resolution>
            </range>

    ‌<min>0.12</min>‌：最小探测距离为 ‌0.12 米‌。小于此距离的物体可能无法被检测或数据无效。
    ‌<max>8.0</max>‌：最大探测距离为 ‌8.0 米‌。超过此距离返回无穷大或最大值。
    ‌<resolution>0.01</resolution>‌：测距分辨率为 ‌1 厘米‌。

噪声模型
            <noise>
                <type>gaussian</type>
                <mean>0.0</mean>
                <stddev>0.01</stddev>
            </noise>

    ‌type=gaussian‌：使用高斯分布模拟测量噪声。
    ‌stddev=0.01‌：标准差为 ‌0.01 米 (1 cm)‌。这意味着测量值会在真实距离附近波动，模拟真实传感器的误差。如果设为 0，则数据是完美的，不利于测试算法的鲁棒性。

        点击gazebo中右上角的三个点按钮，输入lidar搜索，在visualize lidar的窗口中按刷新按钮，就会显示激光雷达的图像:

2.2 图像传感器

        位置：qxwy-vehicle‌/src/qxwy_description/urdf/qxwy/sensor/camera.urdf.xacro

       <gazebo reference="camera_link">
            <sensor name="depth_camera_name" type="depth_camera">
                <always_on>true</always_on>
                <update_rate>10</update_rate>
                <topic>depth_camera</topic>
                <camera>
                    <horizontal_fov>1.05</horizontal_fov>
                    <image>
                    <width>256</width>
                    <height>256</height>
                    <format>R_FLOAT32</format>
                    </image>
                    <clip>
                    <near>0.1</near>
                    <far>10.0</far>
                    </clip>

                    <depth_camera>
                        <output>depth</output>
                    </depth_camera>
                </camera>
            </sensor>
        </gazebo>

传感器绑定与基础属性
<gazebo reference="camera_link">
    <sensor name="depth_camera_name" type="depth_camera">
        <always_on>true</always_on>
        <update_rate>10</update_rate>
        <topic>depth_camera</topic>

    ‌reference="camera_link"‌：指定该传感器附着在名为 camera_link 的连杆上。传感器的坐标系原点与该 Link 重合。
    ‌type="depth_camera"‌：声明传感器类型为‌深度相机‌。这种类型专门用于生成深度图像（Depth Image），每个像素值代表距离相机的深度（单位：米）。
    ‌<always_on>true</always_on>‌：即使没有节点订阅数据，传感器也保持运行。设为 false 可在无人使用时节省仿真资源。
    ‌<update_rate>10</update_rate>‌：数据更新频率为 ‌10 Hz‌（每秒10帧）。对于深度相机来说，这是一个较低的频率，有助于降低 CPU/GPU 负载。
    ‌<topic>depth_camera</topic>‌：定义 Gazebo 内部发布数据的话题名称为 /depth_camera。

相机光学与图像参数
        <camera>
            <horizontal_fov>1.05</horizontal_fov>
            <image>
                <width>256</width>
                <height>256</height>
                <format>R_FLOAT32</format>
            </image>

    ‌<horizontal_fov>1.05</horizontal_fov>‌：水平视场角（FOV）为 ‌1.05 弧度‌（约 ‌60 度‌）。这决定了相机能看到的水平宽度。
    ‌<width>256</width> & <height>256</height>‌：输出图像的分辨率为 ‌256x256‌ 像素。这是一个非常低的分辨率，通常用于快速原型验证或对计算资源极其敏感的场景。
    ‌<format>R_FLOAT32</format>‌：图像数据格式为 ‌32位浮点数‌。
        这是深度相机的标准格式，每个像素存储一个浮点数值，表示该点到相机的距离（单位：米）。
        如果是普通 RGB 相机，格式通常为 R8G8B8。

裁剪平面 (Clipping Planes)
            <clip>
                <near>0.1</near>
                <far>10.0</far>
            </clip>

    ‌<near>0.1</near>‌：近裁剪面为 ‌0.1 米‌。距离相机小于 10 厘米的物体不会被渲染，深度值可能无效。
    ‌<far>10.0</far>‌：远裁剪面为 ‌10.0 米‌。距离相机超过 10 米的物体不会被渲染，深度值将显示为最大值或无效。
        合理设置这两个值可以优化渲染性能，避免处理过近或过远的无用数据。

深度相机特定配置
            <depth_camera>
                <output>depth</output>
            </depth_camera>

    ‌<depth_camera>‌：这是深度相机特有的配置块。
    ‌<output>depth</output>‌：指定输出类型为‌纯深度图‌。
      有些深度相机传感器支持同时输出 RGB 和 Depth（即 RGB-D 相机），此时可能需要配置 <output>rgb_depth</output> 或使用 rgbd_camera 类型。

        在gazebo中查看图像传感器结果：

        点击右上角三个点的按钮，搜索image,选择相应话题即可：

2.3 与ROS2的桥接

        在ROS2 JAZZY中gazebo中的数据需要配合 ros_gz_bridge 使用才能将数据发布到 ROS 2 话题中。有两种方式实现桥接：

 a.在启动时指定转接话题及参数

        代码位置：qxwy-vehicle‌/src/qxwy_description/launch/gazebo_sim.launch.py

        注意：深度相机只能用ros_gz_image包

  camera_bridge = Node(   
        package='ros_gz_bridge',
        executable='parameter_bridge',
        arguments=[
            '/camera@sensor_msgs/msg/Image@gz.msgs.Image',
            '/camera/camera_info@sensor_msgs/msg/CameraInfo@gz.msgs.CameraInfo',
            '/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock',
            '/scan@sensor_msgs/msg/LaserScan@gz.msgs.LaserScan' # 激光雷达
        ],
        output='screen'
    )

    # 深度相机
    depth_camera_bridge  = Node(
        package='ros_gz_image',
        executable='image_bridge',
        arguments=['/depth_camera'],
        output='screen',
        parameters=[{"use_sim_time": True}], 
    )

  b.使用yaml指定转接内容

        这种方式比较容易理解和维护。

        3D激光雷达的yaml文件示例：

    bridges:
      - ros_topic_name: "/points"  # ROS端话题名
        gz_topic_name: "/scan"     # 对应上面配置的 <topic>scan</topic>
        ros_type_name: "sensor_msgs/msg/PointCloud2" 
        gz_type_name: "gz.msgs.PointCloudPacked"
        direction: GZ_TO_ROS

        深度相机专用配置yaml文件示例

        针对 Gazebo Harmonic 中的深度相机（depth_camera 或 `rgbd_camera），通常需要桥接‌深度图像‌和‌点云数据‌。

# bridge.yaml
bridges:
  # 1. 桥接深度图像 (Depth Image)
  - ros_topic_name: "/camera/depth_image"
    gz_topic_name: "/depth_camera"  # 注意：需与SDF/URDF中<topic>标签一致
    ros_type_name: "sensor_msgs/msg/Image"
    gz_type_name: "gz.msgs.Image"
    direction: GZ_TO_ROS

  # 2. 桥接点云数据 (Point Cloud)
  - ros_topic_name: "/camera/points"
    gz_topic_name: "/depth_camera/points" # Gazebo通常自动发布子话题 /points
    ros_type_name: "sensor_msgs/msg/PointCloud2"
    gz_type_name: "gz.msgs.PointCloudPacked"
    direction: GZ_TO_ROS
    
  # 3. (可选) 如果使用 RGBD相机，还需桥接彩色图像
  - ros_topic_name: "/camera/image_raw"
    gz_topic_name: "/depth_camera/image"
    ros_type_name: "sensor_msgs/msg/Image"
    gz_type_name: "gz.msgs.Image"
    direction: GZ_TO_ROS
    
  # 4. (可选) 桥接相机信息 (Camera Info)，用于去畸变等
  - ros_topic_name: "/camera/camera_info"
    gz_topic_name: "/depth_camera/camera_info"
    ros_type_name: "sensor_msgs/msg/CameraInfo"
    gz_type_name: "gz.msgs.CameraInfo"
    direction: GZ_TO_ROS

        在 Python Launch 文件中添加桥接节点，使其随仿真一起启动，示例：

from launch import LaunchDescription
from launch_ros.actions import Node
import os

def generate_launch_description():
    # 假设 bridge.yaml 放在当前包的 config 目录下
    pkg_share = '/path/to/your/ros2_package' 
    bridge_config = os.path.join(pkg_share, 'config', 'bridge.yaml')

    bridge_node = Node(
        package='ros_gz_bridge',  #深度相机的需要换成ros_gz_image
        executable='parameter_bridge',#深度相机的需要换成image_bridge
        arguments=['--config-file', bridge_config],
        output='screen'
    )

    return LaunchDescription([
        bridge_node
    ])

3 两轮差速控制

3.1 Gazebo Sim与 ros2_control框架集成

        代码位置：qxwy-vehicle‌/src/qxwy_description/urdf/qxwy/qxwy.ros2_control.xacro

        ros2 jazzy版本的Gazebo控制器使用方法变化比较大。如果你使用的是旧版 Gazebo Classic (Gazebo 11)，则需要使用 gazebo_ros2_control 插件和不同的库文件名

        <ros2_control name="QxwyGazeboSystem" type="system">
            <hardware>
            <!-- 新版 Gazebo 必须用这个插件！ -->
            <plugin>gz_ros2_control/GazeboSimSystem</plugin>
            </hardware>

            <joint name="left_wheel_joint">
                <command_interface name="velocity">
                    <param name="min">-1</param>
                    <param name="max">1</param>
                </command_interface>

                <command_interface name="effort">
                    <param name="min">-0.1</param>
                    <param name="max">0.1</param>
                </command_interface>
                <state_interface name="effort" />
                <state_interface name="velocity" />
                <state_interface name="position" />
            </joint>

            <joint name="right_wheel_joint">
                <command_interface name="velocity">
                    <param name="min">-1</param>
                    <param name="max">1</param>
                </command_interface>

                <command_interface name="effort">
                    <param name="min">-0.1</param>
                    <param name="max">0.1</param>
                </command_interface>
                <state_interface name="effort" />
                <state_interface name="velocity" />
                <state_interface name="position" />
            </joint>
        </ros2_control>

        <gazebo>
            <plugin filename="/opt/ros/jazzy/lib/libgz_ros2_control-system.so" name="gz_ros2_control::GazeboSimROS2ControlPlugin">
                <robot_param_name>robot_description</robot_param_name>
                <robot_param_node>robot_state_publisher</robot_param_node>
                <parameters>$(find qxwy_description)/config/qxwy_ros2_controller.yaml</parameters>
                <ros>
                    <remapping>/qxwy_diff_drive_controller/cmd_vel:=cmd_vel</remapping>
                    <remapping>/qxwy_diff_drive_controller/odom:=odom</remapping>
                </ros>
            </plugin>
        </gazebo>

<ros2_control> 标签：硬件抽象层定义

        定义机器人的“虚拟硬件”接口，告诉 ros2_control 如何与仿真中的关节交互。       name="QxwyGazeboSystem" & type="system"‌:定义了一个名为 QxwyGazeboSystem的系统级硬件组件，这个名字可以自己取。type="system" 表示这是一个完整的硬件系统（包含多个关节和传感器），这是最常用的类型。

     <plugin>gz_ros2_control/GazeboSimSystem</plugin>‌:
        ‌关键点‌：指定使用 gz_ros2_control 提供的仿真硬件后端。该插件负责读取 Gazebo 中的关节状态（位置、速度、力矩），并将 ros2_control 发出的控制命令施加到 Gazebo 模型的关节上。

     关节接口定义 (left_wheel_joint & right_wheel_joint)‌:
        ‌Command Interfaces (命令接口)‌：控制器可以发送指令给这些接口。
            velocity：速度控制接口。限制范围 -1 到 1 (单位通常是 rad/s)。
            effort：力矩/电流控制接口。限制范围 -0.1 到 0.1 (单位通常是 N·m 或 A)。
            注意：同时暴露速度和力矩接口允许使用混合控制器或更高级的控制策略，但通常差速控制器主要使用 velocity。
        ‌State Interfaces (状态接口)‌：控制器可以从这些接口读取反馈数据。
            effort：当前关节受力/力矩。
            velocity：当前关节角速度。
            position：当前关节角度。
        作用：这些接口使得 PID 控制器或其他算法能够获取实时反馈并计算输出。

<gazebo> 标签：插件加载与配置

        这部分告诉 Gazebo 仿真器加载 gz_ros2_control 插件，并配置其与 ROS 2 的通信。如果是真实的场景，这部分设置方式将由硬件厂商提供。

    ‌<plugin filename="..." name="...">‌:
        filename: 指向动态库文件 libgz_ros2_control-system.so。路径 /opt/ros/jazzy/lib/... 表明你正在使用 ‌ROS 2 Jazzy‌ 版本（自己搜索下这个插件在什么地方，然后替换路径）。
        name: 插件类名 gz_ros2_control::GazeboSimROS2ControlPlugin。这是 Gazebo Sim 识别并加载该插件的关键标识。

        filename和name是固定的名称，不要更改。

    ‌<robot_param_name>robot_description</robot_param_name>‌:
        指定从哪个 ROS 参数中获取机器人的 URDF/Xacro描述。默认为 robot_description。

    ‌<robot_param_node>robot_state_publisher</robot_param_node>‌:
        指定哪个节点发布了上述参数。通常是 robot_state_publisher 节点。插件会从这个节点读取模型描述以初始化硬件接口。

    ‌<parameters>$(find qxwy_description)/config/qxwy_ros2_controller.yaml</parameters>‌:
        ‌核心配置‌：指向控制器管理器（Controller Manager）的配置文件。
        该 YAML 文件定义了具体加载哪些控制器（如 diff_drive_controller、joint_state_broadcaster）、它们的类型、更新频率以及参数（如轮距、半径等）。
        $(find ...) 是 ROS 的资源查找语法，确保路径正确。

    ‌<ros> 话题重映射 (Remapping)‌:
    <remapping>/qxwy_diff_drive_controller/cmd_vel:=cmd_vel</remapping>
    <remapping>/qxwy_diff_drive_controller/odom:=odom</remapping>

        ‌目的‌：简化话题名称，使其符合常规习惯。
        ‌默认行为‌：ros2_control 的控制器通常会发布/订阅带有控制器命名空间的话题，例如 /qxwy_diff_drive_controller/cmd_vel。
        ‌重映射后‌：
            外部节点只需向 /cmd_vel 发送速度指令，插件会自动将其转发给控制器。
            外部节点只需订阅 /odom 即可获取里程计数据，无需关心内部命名空间。
        优势：提高了代码的通用性，使得导航栈（Nav2）等标准模块可以直接使用标准话题名，无需修改配置。

3.2 启动控制器

        控制器需要启动，文件位置：

        qxwy-vehicle‌/src/qxwy_description/launch/gazebo_sim.launch.py

    # 加载 joint_state_broadcaster 控制器
    action_load_joint_state_controller = launch.actions.ExecuteProcess(
        cmd=['ros2', 'control', 'load_controller', 'qxwy_joint_state_broadcaster', '--set-state', 'active'],
        output='screen'
    )

    # 加载 diff_drive_controller 控制器
    action_load_diff_drive_controller = launch.actions.ExecuteProcess(
        cmd=['ros2', 'control', 'load_controller', 'qxwy_diff_drive_controller', '--set-state', 'active'],
        output='screen'
    )

4 SLAM建图

 4.1 slam_toolbox简介

         简介

                slam_toolbox 是 ROS 2 官方主推、工业级的2D 激光 SLAM 工具，替代了 ROS1 的 gmapping，深度集成 Navigation2，主打稳定、高效、可长期建图。

       核心功能：在线建图、离线定位、地图保存 / 加载、多会话建图、终身地图更新。

       特点：基于图优化（Pose-Graph），支持同步 / 异步模式；依赖外部里程计（轮速 + IMU 融合）；用Ceres Solver做后端优化，精度高、内存可控。

        适用场景：室内仓库、写字楼、家庭等大规模 2D 环境，支持 24 小时连续建图  

        核心原理（图优化 SLAM）

                整体是前端扫描匹配 + 后端位姿图优化 + 闭环检测的经典三层架构。

        前端：Scan-to-Map 匹配

                输入：激光雷达（LaserScan）+ 里程计（Odometry/TF）ROS。

                流程：每帧激光数据与当前地图做ICP / 多分辨率匹配，得到当前帧的精确位姿；

                            把 “位姿 + 激光” 打包成PosedScan，加入位姿图作为节点（Vertex）ROS。

                作用：实时修正里程计漂移，输出连续轨迹与局部地

         后端：位姿图（Pose Graph）优化

                图结构：

                        节点：机器人关键帧位姿 (x,y,θ)。

                        边（约束）：

                                里程计边：连续节点间的运动约束。

                                 激光匹配边：节点与地图的匹配约束。

                                闭环边：检测到回环时添加的强约束。

                优化器：用Ceres Solver做增量式稀疏位姿调整（SPA），最小化所有约束的残差，

                               得到全局一致的轨迹与地图。

                策略：局部高频优化（保证实时）+ 全局低频优化（保证精度）。

        闭环检测（Loop Closure）

                目的：解决长期漂移，让地图 “首尾闭合”ROS。

                方法：用FPFH 特征 + KD-Tree 检索 + RANSAC+ICP 验证，识别 revisited 区域；

                            成功则添加闭环约束边，触发全局优化。

        地图生成与管理

                栅格地图：用优化后的位姿把所有激光扫描融合成占据栅格地图（.pgm+.yaml）。

                终身映射：动态增删节点，支持地图更新、合并与热启动。

4.2 slam_toolbox的使用

        安装slam_toolbox:

sudo apt install ros-$ROS_DISTRO-slam-toolbox

        启动slam_toolbox:

ros2 launch slam_toolbox online_async_launch.py

        启动slam_toolbox后，slam_toolbox会发布/map话题。

        gazebo和rviz显示如下：

        启动键盘控制节点，控制小车运行：

   ros2 run teleop_twist_keyboard teleop_twist_keyboard --ros-args -p stamped:=true

        这个命令会让 teleop_twist_keyboard 直接发布 TwistStamped 消息（而不是常见的Twist消息），与控制器的期望类型匹配，从而实现对小车的控制。

4.3 存储已建好的图

        先安装navigation2:

sudo apt install ros-$ROS_DISTRO-nav2-map-server

        在qxwy_navigation2包中新建一个maps文件夹，在终端进入maps文件夹，运行以下命令：

ros2 run nav2_map_server map_saver_cli -f room

        运行成功后，将会在maps目录下生成room.pgm和room.yaml,把room改成自己的名字。

room.pgm是图片，不同值表示不同的环境状况，一般是三值图。room.yaml图片的描述性文件，例如图片的比例，原点等。

        

5 导航仿真

        先启动小车仿真，因为需要TF来确定小车位置：

ros2 launch qxwy_description gazebo_sim.launch.py

        然后启动导航：

ros2 launch qxwy_navigation2 navigation2.launch.py

    启动后将会看到带地图的rviz，因为要初始化导航位置，所以左边会有错误提示。你需要告诉机器人它在地图上的初始位置。有两种方式：

        第一 通过RViz设置（推荐）：
           在RViz中找到工具栏上的 "2D Pose Estimate" 工具（绿色的定位图标），点击地图上机器人所在的位置（通常在房间的某个角落）。
        第二 通过命令行设置（）：

ros2 topic pub -1 /initialpose geometry_msgs/msg/PoseWithCovarianceStamped "{header: {stamp: {sec: 0, nanosec: 0}, frame_id: 'map'}, pose: {pose: {position: {x: 0.0, y: 0.0, z: 0.0}, orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0}}, covariance: [0.25, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.25, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.06853892326654787]}"

        设置初始位置后的图片：

  

        其中的Navigation和Location的状态必须是active,否则就是启动失败。

        设置目标点，模拟导航：

        在RViz中找到工具栏上的 "Nav2 Goal" 工具，在地图上任意白色部分（已探索没有障碍物部分)点击，小车就会自动导航到目标点。

        

6 语音播放及多点自动巡航

      先启动小车仿真，因为需要TF来确定小车位置：

ros2 launch qxwy_description gazebo_sim.launch.py

        然后启动导航界面：

ros2 launch qxwy_navigation2 navigation2.launch.py

       最后运行：

 ros2 launch autopilot_robot autopilot.launch.py

   运行结果为，先会听到一段提示初始化的声音，然后进行多点循环导航。到了巡航点进行拍照及保持图片到本地。