关于ROS的一些建图的基础主要学习了gmapping建图的机制以及相关知识。其实我感觉学习的并不是很深入，所以本次作为一个基础性总结，没什么太大的参考价值

TF 简介

TF（Transform Library）是 ROS 中用于管理坐标系变换的核心工具，用于跟踪多个坐标系之间的相对位置和姿态关系。它通过树状结构维护坐标系间的层级关系，支持静态和动态变换的发布与查询。

TF 核心功能

坐标系管理
TF 维护一个全局坐标系树，每个节点代表一个坐标系，父子关系表示坐标系间的变换（如 base_link 到 laser）。
变换查询
通过 lookupTransform() 查询任意两个坐标系间的变换（平移和旋转），支持时间戳同步和历史数据查询。
数据类型
使用 tf::Transform 和 geometry_msgs/TransformStamped 存储变换数据，包含平移（x, y, z）和旋转（四元数 x, y, z, w）。

TF 基本操作

发布变换
静态变换（如固定传感器位置）通过 static_transform_publisher 发布：

rosrun tf static_transform_publisher x y z yaw pitch roll frame_id child_frame_id period_ms

动态变换需编写节点调用 tf::TransformBroadcaster：

tf::Transform transform;
transform.setOrigin(tf::Vector3(1.0, 0.0, 0.0));
transform.setRotation(tf::Quaternion(0, 0, 0, 1));
broadcaster.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "parent_frame", "child_frame"));

监听变换
通过 tf::TransformListener 查询坐标变换：

tf::StampedTransform transform;
listener.lookupTransform("target_frame", "source_frame", ros::Time(0), transform);

TF 工具

命令行工具

• view_frames：生成坐标系树 PDF 可视化文件。
• tf_echo：实时打印两个坐标系间的变换。

rosrun tf tf_echo source_frame target_frame

• tf_monitor：监控坐标系间变换的频率和延迟。

常见问题

时间同步
查询变换时需处理时间戳问题。若直接使用 ros::Time(0) 获取最新数据，可能因未收到数据而报错。建议使用 waitForTransform() 等待数据就绪：

listener.waitForTransform("target_frame", "source_frame", ros::Time(0), ros::Duration(3.0));

geometry_msgs/TransformStamped 概述

geometry_msgs/TransformStamped 是 ROS（Robot Operating System）中用于表示带时间戳的坐标变换（Transform）的消息类型。它属于 geometry_msgs 包，常用             于 tf2 系统中，描述两个坐标系（如 parent_frame 和 child_frame）之间的空间关系（平移和旋转）。

消息结构

消息包含以下字段：

• std_msgs/Header header

  • uint32 seq：消息序列号（通常忽略）。
  • time stamp：时间戳，表示变换的时间点。
  • string frame_id：父坐标系名称（parent_frame）。

• string child_frame_id
  子坐标系名称（child_frame）。

• geometry_msgs/Transform transform

  • geometry_msgs/Vector3 translation：平移分量（x, y, z）。
  • geometry_msgs/Quaternion rotation：旋转分量（四元数形式，x, y, z, w）。

示例代码

以下是一个创建并发布 TransformStamped 消息的 Python 示例（使用 rospy 和 tf2_ros）：

import rospy
import tf2_ros
from geometry_msgs.msg import TransformStamped
from tf.transformations import quaternion_from_euler

rospy.init_node('transform_publisher')

# 创建 TransformStamped 对象
transform = TransformStamped()
transform.header.stamp = rospy.Time.now()
transform.header.frame_id = "world"  # 父坐标系
transform.child_frame_id = "robot"  # 子坐标系

# 设置平移（x, y, z）
transform.transform.translation.x = 1.0
transform.transform.translation.y = 0.5
transform.transform.translation.z = 0.0

# 设置旋转（四元数）
(transform.transform.rotation.x, 
 transform.transform.rotation.y, 
 transform.transform.rotation.z, 
 transform.transform.rotation.w) = quaternion_from_euler(0, 0, 1.57)  # 绕 z 轴旋转 90 度

# 发布变换
tf_broadcaster = tf2_ros.TransformBroadcaster()
tf_broadcaster.sendTransform(transform)
rospy.spin()

常见用途

1. 坐标系间变换
   在 tf2 系统中，通过 TransformStamped 描述父子坐标系的相对位置和姿态，用于传感器数据融合或运动规划。

2. 静态变换发布
   使用 tf2_ros.StaticTransformBroadcaster 发布静态变换（如机器人基座与传感器间的固定偏移）。

3. 动态变换更新
   通过实时更新 header.stamp 和变换数据，表示移动物体（如机器人或障碍物）的位置变化。

注意事项

• 时间戳一致性：header.stamp 需与数据时间同步，避免 tf2 缓存过期问题。
• 四元数归一化：旋转四元数需满足归一化条件（x² + y² + z² + w² = 1），否则可能导致计算错误。
• 坐标系命名：frame_id 和 child_frame_id 需遵循 ROS 命名规范（避免特殊字符）。

与其他消息的关系

• geometry_msgs/PoseStamped：带时间戳的位姿（位置 + 方向），但不包含坐标系关系。
• tf2_msgs/TFMessage：包含多个 TransformStamped 的集合，用于传输批量变换数据。

配置 launch 文件启动 gmapping

在 ROS 中，gmapping 是一个常用的 2D SLAM 工具包，用于生成环境地图。以下是一个完整的 launch 文件示例，包含必要的 TF 关系和话题配置，每行代码均附带注释。

<launch>
    <!-- 启动 gmapping 节点 -->
    <node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen">
        <!-- 设置雷达话题名称，默认为 /scan -->
        <param name="scan" type="string" value="/scan" />
        
        <!-- 设置 TF 坐标系关系 -->
        <!-- base_link → odom：odom 是父坐标系 -->
        <param name="odom_frame" type="string" value="odom" />
        <!-- 雷达坐标系 → base_link：base_link 是父坐标系 -->
        <param name="base_frame" type="string" value="base_link" />
        
        <!-- 地图更新频率（Hz） -->
        <param name="map_update_interval" type="double" value="5.0" />
        <!-- 雷达最大有效距离（米） -->
        <param name="maxRange" type="double" value="10.0" />
    </node>

    <!-- 发布静态 TF 变换：雷达坐标系 → base_link -->
    <!-- 假设雷达安装在 base_link 上，偏移量为 (0.1, 0, 0.2) -->
    <node pkg="tf" type="static_transform_publisher" name="laser_to_base"
          args="0.1 0 0.2 0 0 0 base_link laser 100" />
</launch>

关键配置说明

1. TF 坐标系关系

   • odom_frame：odom 是 base_link 的父级，表示里程计坐标系。
   • base_frame：base_link 是雷达坐标系的父级，表示机器人基坐标系。

2. 雷达话题

   • scan 参数指定雷达数据的话题名称（例如 /scan），需与实际发布的话题一致。

3. 静态 TF 发布

   • 使用 static_transform_publisher 发布雷达坐标系到 base_link 的固定变换（假设为 laser 坐标系）。

运行与验证

1. 启动 launch 文件：

   roslaunch your_package gmapping.launch
   

2. 检查 TF 树是否正常：

   rosrun tf view_frames
   

   生成的 frames.pdf 应显示 odom → base_link → laser 的层级关系。

3. 确保雷达数据正常发布：

   rostopic echo /scan
   

常见问题

• TF 错误：若出现 Could not find transform from X to Y，检查静态 TF 发布或 base_frame/odom_frame 名称是否匹配。
• 地图不更新：确认 /scan 数据频率足够（例如 10Hz），并调整 map_update_interval。

通过以上配置，gmapping 将利用雷达数据和 TF 关系构建地图。

节点功能解析

<include file = "$(find wpr_simulation)/launch/wpb_stage_robocup.launch"/>

• 作用：加载WPR机器人的仿真环境，包括机器人模型、Stage仿真器和RoboCup比赛场景。
• 扩展说明：
  该指令会启动Gazebo或Stage仿真器，并加载预定义的机器人URDF模型、传感器配置及虚拟环境。若在实体机器人上运行，需替换为实际机器人的启动文件（如robot_bringup.launch），以加载真实的驱动节点和传感器接口。

---

<node name="slam_gmapping" pkg="gmapping" type="slam_gmapping"/>

• 作用：启动Gmapping SLAM算法节点，将激光雷达数据转换为栅格地图。
• 关键参数（未显式设置）：
  • scan：默认订阅/scan话题的激光数据。
  • odom_frame：依赖里程计坐标系（通常为odom）。
  • map_update_interval：控制地图更新频率。
• 扩展说明：
  Gmapping基于粒子滤波算法，适合小范围环境建图。若需更高精度，可在节点中添加参数如delta=0.05（地图分辨率）或maxUrange=10.0（最大有效激光距离）。

---

<node name="rviz" pkg="rviz" type="rviz" args="-d $(find slam_pkg)/rviz/gmapping.rviz"/>

• 作用：启动RViz可视化工具，加载预配置的视图（gmapping.rviz）显示地图和传感器数据。
• 扩展说明：
  RViz配置文件（.rviz）通常已预设显示激光点云、地图、机器人模型和TF坐标系。若需自定义，可通过RViz界面调整并保存新配置。

---

<node name="keyboard_vel_ctrl" pkg="wpr_simulation" type="keyboard_vel_ctrl"/>

• 作用：启动键盘控制节点，通过终端输入控制机器人移动（前后左右）。
• 实现原理：
  该节点发布/cmd_vel话题（Twist消息类型），订阅者（如仿真器或底盘驱动节点）会根据线速度和角速度执行运动。
• 替代方案：
  若需自动化建图，可替换为导航栈的move_base节点，或使用遥操作工具如teleop_twist_keyboard。

---

通用优化建议

1. 参数配置
   在<node>标签内显式定义参数，例如调整Gmapping的粒子数：

   <param name="particles" value="100"/>
   

2. TF坐标系检查
   确保机器人发布的TF树包含base_link→laser→odom的完整链，否则Gmapping无法正确对齐数据。

3. 实体机器人适配
   替换仿真启动文件时，需同步验证激光雷达话题名称（如/scan是否匹配）和里程计来源（如/odom或IMU数据）。

4. 性能监控
   通过rostopic hz /scan检查激光数据频率，若低于10Hz可能需调整传感器配置或SLAM参数。

5. 多传感器融合
   若机器人配备IMU或深度相机，可在Gmapping节点中启用use_sim_time并配置imu_topic以提升建图精度。

gmapping建图相关接口

接口相关参数

• base_frame：默认值 base_link，指定底盘坐标系名称。
• map_frame：默认值 map，指定地图坐标系名称。
• odom_frame：默认值 odom_link，指定里程计坐标系名称。

性能相关参数

• maxRange：激光雷达射线的最大采纳距离，需根据实际环境调整。
• throttle_scans：默认值 1，控制激光雷达数据跳帧处理频率。
• map_update_interval：默认值 5秒，设置地图更新的时间间隔。

算法相关参数

• sigma：默认值 0.05，粒子滤波器的噪声标准差，影响定位精度。
• iterations：默认值 5，粒子滤波器的优化迭代次数。
• occ_thresh：默认值 0.25，占据栅格的阈值参数，决定栅格是否被标记为障碍物。

地图尺寸参数

• xmin：默认值 -100.0米，地图的X轴负向边界。
• xmax：默认值 100.0米，地图的X轴正向边界。
• ymin：默认值 -100.0米，地图的Y轴负向边界。
• ymax：默认值 100.0米，地图的Y轴正向边界。
• delta：默认值 0.05米/格，栅格地图的分辨率。

激光雷达参数

• maxUrange：激光雷达射线的最大有效距离，需与传感器性能匹配。
• lskip：默认值 0，控制激光雷达扫描的跳线处理。
• throttle_scans：默认值 1，与性能参数中的跳帧处理一致。

地图更新触发条件

• linearUpdate：默认值 1.0米，移动距离超过此阈值时触发地图更新。
• angularUpdate：默认值 0.5弧度，旋转角度超过此阈值时触发地图更新。

粒子滤波器参数

• particles：默认值 30，滤波器使用的粒子数量，影响计算负载和精度。
• resampleThreshold：默认值 0.5，粒子重采样阈值，控制重采样频率。

保存地图使用map_server

执行以下命令保存当前地图到指定文件（默认保存为map.pgm和map.yaml）：

rosrun map_server map_saver -f <mapname> map:=/your/costmap/topic

可选参数说明： --occ <threshold_occupied> 设置占用阈值（默认65） --free <threshold_free> 设置空闲阈值（默认25）

加载已保存的地图

使用以下命令加载之前保存的地图（需提供YAML配置文件）：

rosrun map_server map_server <mymap.yaml>

启动RViz可视化工具

运行以下命令启动RViz进行地图可视化：

rosrun rviz rviz

注意：实际使用时需将<mapname>和<mymap.yaml>替换为实际文件名。未指定map:=参数时默认使用/map话题。保存的PGM文件为地图图像，YAML文件包含地图元数据。

move_base导航节点

move_base是ROS中实现全局路径规划和局部避障的核心节点，负责处理目标点请求并生成可行的运动轨迹。需要配置base_global_planner（如Navfn或Global Planner）和base_local_planner（如TEB或DWA）。参数文件通常包括costmap_common_params.yaml、global_costmap_params.yaml和local_costmap_params.yaml，需根据机器人尺寸和传感器特性调整膨胀半径、更新频率等。

map_server地图服务节点

加载已构建的栅格地图（通过gmapping生成后保存为.pgm和.yaml文件），启动命令为：

rosrun map_server map_server /path/to/map.yaml

地图数据通过/map话题发布，供move_base和amcl使用。需确保地图分辨率与坐标系（如map到odom的TF关系）正确。

激光雷达传感器节点

激光雷达（如Hokuyo或RPLIDAR）发布/scan话题，数据类型为sensor_msgs/LaserScan。需在costmap配置中指定话题名称及传感器范围：

obstacle_layer:  
  topics: ["/scan"]  
  min_obstacle_height: 0.0  
  max_obstacle_height: 2.0  

里程计节点

编码器数据通过/odom话题发布，类型为nav_msgs/Odometry。需确保TF树中包含odom到base_link的变换，通常由机器人底盘驱动节点或robot_state_publisher完成。在move_base中启用use_odom_twist以融合里程计信息。

TF传感器位置节点

TF树维护各坐标系间的动态关系，关键变换包括：

• map → odom（由amcl更新）
• odom → base_link（由里程计提供）
• base_link → laser（传感器外参）
  使用static_transform_publisher或URDF文件定义静态变换。

amcl定位节点

自适应蒙特卡洛定位（AMCL）通过粒子滤波融合激光与里程计数据，输出map到odom的变换。配置参数如粒子数量、初始位姿噪声：

amcl:  
  min_particles: 100  
  max_particles: 500  
  initial_pose_x: 0.0  
  initial_pose_y: 0.0  

指定导航目标点

通过RViz的2D Nav Goal工具或程序发布geometry_msgs/PoseStamped到/move_base_simple/goal话题。目标点需在map坐标系下，示例代码：

goal = PoseStamped()  
goal.header.frame_id = "map"  
goal.pose.position.x = 3.0  
goal.pose.position.y = 1.0  
goal.pose.orientation.w = 1.0  
pub.publish(goal)  

想要启动仿真需要环境配置，如果没有配置优先对仿真环境进行配置。

一、环境配置
进入wpr_simulation/sceipts/中下载依赖./install_for_noetic.sh
启动roslaunch wpr_simulation wpb_stage_robocup.launch
启动roslaunch nav_pkg nav.launch
会有报错，主要是缺少wpb_home,这个必须得去GitHub上搜，gitee上没有
下载玩之后放在src中编译一下，在编译的时候会出现缺少sound-play的问题
通过sudo apt_get install --reinstall ros-noetic-sound-play下载好之后就可以编译成功
编译成功之后就可以启动了

全局规划器与变量名称对照

Navfn
变量名称：navfn/NavfnROS
功能：包含Dijkstra与A*算法，适用于基于代价地图的全局路径规划。

Global Planner
变量名称：global_planner/GlobalPlanner
功能：同样包含Dijkstra与A*算法，提供更灵活的配置选项，支持自定义启发式函数。

Carrot Planner
变量名称：carrot_planner/CarrotPlanner
功能：适用于目标点附近无可行路径时的局部调整，生成临时目标点（“胡萝卜”）引导机器人。

---

自定义路径规划实现方法

1. 规划器接口继承
需继承nav_core::BaseGlobalPlanner类，并实现以下核心方法：

• initialize()：初始化规划器（如加载参数、代价地图）。
• makePlan()：生成路径（输入起点、终点，输出路径点列表）。

2. 算法选择示例

• RRT（快速随机树）：适合高维空间或复杂障碍。
• Theta*：基于A*的任意角度路径优化。
• 自定义混合算法：结合Dijkstra的可靠性与RRT的探索性。

3. ROS集成步骤

• 在package.xml中添加依赖：

  <depend>nav_core</depend>
  <depend>costmap_2d</depend>
  

• 在CMakeLists.txt中编译为插件库：

  add_library(my_planner SHARED src/my_planner.cpp)
  target_link_libraries(my_planner ${catkin_LIBRARIES})
  

4. 注册插件
创建planner_plugins.xml文件：

<library path="lib/libmy_planner">
  <class name="my_planner/MyPlanner" type="my_planner::MyPlanner" base_class_type="nav_core::BaseGlobalPlanner"/>
</library>

并在package.xml中导出：

<export>
  <nav_core plugin="${prefix}/planner_plugins.xml"/>
</export>

5. 调用自定义规划器
在启动文件中指定规划器类型：

<param name="base_global_planner" value="my_planner/MyPlanner"/>

---

算法关键公式（以A*为例）

启发式函数（曼哈顿距离）： $$ h(n) = |x_{goal} - x_n| + |y_{goal} - y_n| $$

总代价计算： $$ f(n) = g(n) + h(n) $$ 其中g(n)为起点到当前节点的实际代价，h(n)为启发式估计代价。

AMCL定位算法概述

自适应蒙特卡洛定位（AMCL）是一种基于粒子滤波的机器人定位算法，适用于已知环境中的位姿估计。它通过融合里程计和激光雷达数据，动态调整粒子数量和分布，实现高精度定位与自我纠正。

核心原理

AMCL通过粒子集表示机器人位姿的概率分布。每个粒子包含位置（x, y）和朝向（θ）信息。算法分为三个主要阶段：

预测阶段
利用里程计运动模型更新粒子位姿，反映机器人运动后的可能分布。运动噪声通过高斯分布建模： $$ x_t = x_{t-1} + \Delta x \cdot \cos\theta - \Delta y \cdot \sin\theta + \mathcal{N}(0, \sigma_x) $$

权重更新阶段
将激光雷达观测数据与地图匹配，计算每个粒子的似然值。采用传感器模型计算权重： $$ w_i = p(z_t | x_t^{(i)}, m) $$

重采样阶段
根据权重进行重要性重采样，低权重粒子被淘汰，高权重粒子被复制。自适应机制动态调整粒子数量： $$ N_{eff} = \frac{1}{\sum w_i^2} $$

实现步骤

初始化
在未知位置时均匀分布粒子，已知初值时在初值附近高斯分布。

运动更新
根据里程计数据应用运动模型，扩散粒子分布以反映不确定性。

观测更新
使用激光雷达数据计算每个粒子与地图的匹配度，更新权重。

自适应重采样
当有效粒子数低于阈值时触发重采样，避免粒子退化。

参数调优

• min_particles/max_particles：控制粒子数量范围
• kld_err：KL距离误差阈值
• laser_model：选择光束模型或似然场模型

ROS中的AMCL实现

<node pkg="amcl" type="amcl" name="amcl">
  <param name="min_particles" value="100"/>
  <param name="max_particles" value="5000"/>
  <param name="kld_err" value="0.01"/>
  <param name="laser_model_type" value="likelihood_field"/>
</node>

典型问题与解决方案

定位丢失
增大初始粒子数，检查传感器与地图的匹配度。

粒子收敛过慢
调整运动噪声参数（odom_alpha1-4），优化激光模型参数。

计算负载高
降低最大粒子数，使用更高效的传感器模型。

性能优化技巧

• 预处理地图以提高匹配速度
• 使用多线程处理粒子更新
• 在低动态环境中减小重采样频率
• 融合IMU数据改善运动预测

该算法在动态环境中表现优异，但需注意参数与环境特性的匹配。实际部署时应进行充分的仿真测试和现场调参。

代价地图概述

代价地图（Costmap）是ROS导航堆栈中的核心组件，用于表示环境的可通行性。分为全局代价地图（Global Costmap）和局部代价地图（Local Costmap），分别用于全局路径规划和局部避障。

全局代价地图（Global Costmap）

• 存放位置：通常基于静态地图（如map_server加载的PGM/YAML文件）生成，存储在/map话题中。
• 用途：为A*等全局规划算法（如navfn或global_planner）提供环境信息，生成长期路径。
• 关键参数：

  global_costmap:
    global_frame: map
    robot_base_frame: base_link
    update_frequency: 1.0
    static_layer: true
    obstacle_layer: true
    inflation_layer: true
    inflation_radius: 0.5
  

局部代价地图（Local Costmap）

• 存放位置：基于传感器（如激光雷达）实时数据动态更新，存储在/local_costmap话题中。
• 用途：为局部规划器（如DWA或TEB）提供即时障碍物信息，用于动态避障。
• 关键参数：

  local_costmap:
    global_frame: odom
    robot_base_frame: base_link
    update_frequency: 5.0
    rolling_window: true
    width: 6.0
    height: 6.0
    resolution: 0.05
    obstacle_layer: true
    inflation_layer: true
  

A*算法与代价地图的关系

• 数据来源：A*算法读取全局代价地图的栅格数据，计算从起点到目标点的最优路径。
• 实现方式：在ROS中，A*通过global_planner包实现，其代价函数由costmap_2d提供的栅格值决定。

代价地图参数分类

1. 通用参数

   • global_frame：参考坐标系（全局为map，局部为odom）。
   • robot_base_frame：机器人基座坐标系。
   • resolution：地图分辨率（米/像素）。

2. 层级参数

   • static_layer：是否加载静态地图。
   • obstacle_layer：是否启用障碍物检测。
   • inflation_layer：是否膨胀障碍物（设置inflation_radius）。

3. 更新参数

   • update_frequency：地图更新频率（Hz）。
   • rolling_window（局部）：是否滑动窗口跟踪机器人位置。

代码示例

以下为代价地图参数的典型YAML配置片段：

global_costmap:
  plugins:
    - {name: static_layer, type: "costmap_2d::StaticLayer"}
    - {name: obstacle_layer, type: "costmap_2d::ObstacleLayer"}
    - {name: inflation_layer, type: "costmap_2d::InflationLayer"}
  inflation_radius: 0.55

注意事项

• 局部代价地图需更高的更新频率（通常≥5Hz）以应对动态障碍物。
• 全局代价地图的inflation_radius需与机器人半径匹配，避免路径过于贴近障碍物。
• A*算法的启发式权重可通过global_planner的use_dijkstra和use_grid_path参数调整。