本系列文章是 2024 春季学期北航计算机学院本科生课程《软件工程》（嵌入式方向）的实验部分报告，不包含团队大作业项目内容与相关细节

任务3-机器人激光雷达建图与导航功能实现

实验目的

  • 学会使用启智机器人的激光雷达建图与导航功能

实验任务

  • 使用启智机器人的激光雷达完成对仿真实验场景的建图
  • 在建好的地图上进行导航功能

实验内容

使用启智机器人的激光雷达完成对仿真实验场景的建图

本项目在任务 2 的基础上继续开发。

SLAM 功能：即时定位与地图构建。

将连续的多个位置激光雷达扫描到的障碍物轮廓拼合在一起，就能形成一个比较完整的平面地图。这个地图是一个二维平面上的地图，其反映的是在激光雷达的扫描面上，整个环境里的障碍物轮廓和分布情况。在构建地图的过程中，还可以根据障碍物轮廓的重合关系，反推出机器人所走过的这几个位置之间的相互关系以及机器人在地图中所处的位置，这就同时完成了地图构建和机器人的自身实时定位这两项功能。

ROS 支持多种 SLAM 算法，常用的是 Hector SLAM 和 Gmapping，前者仅依靠激光雷达就能工作，后者需要结合电机码盘里程计等信息，建图的稳定性要更高。

Gmapping 建图中：roslaunch wpr_simulation wpb_gmapping.launch

image-20240315233445391

image-20240315233200931

GMapping 建图完毕：

image-20240315233305406

image-20240315233525578

在原有的 gmapping launch 文件基础上创建使用 Hector SLAM 的 launch 文件

<launch>

  <!-- 载入 机器人 和 RoboCup@Home 的仿真场景 -->
  <include file="$(find wpr_simulation)/launch/wpb_stage_robocup.launch"/>

  <!-- Hector SLAM -->
  <node pkg="hector_mapping" type="hector_mapping" name="hector_mapping">
    <param name="tf_map_scanmatch_transform_frame_name" value="base_footprint" />
  </node>

  <!-- Rviz -->
  <arg name="rvizconfig" default="$(find wpr_simulation)/rviz/slam.rviz" />
  <node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />

  <!-- 手柄控制 -->
  <node respawn="true" pkg="joy" type="joy_node" name="joy_node" >
    <param name="dev" type="string" value="/dev/input/js0" />
    <param name="deadzone" value="0.12" />
  </node>
  <param name="axis_linear" value="1" type="int"/>
  <param name="axis_angular" value="0" type="int"/>
  <param name="scale_linear" value="0.5" type="double"/>
  <param name="scale_angular" value="1" type="double"/>
  <node pkg="wpr_simulation" type="teleop_js_node" name="teleop_js_node"/>

</launch>

Hector 建图完毕：roslaunch wpr_simulation wpb_hector_mapping.launch

image-20240315234417410

Gmapping&Hector比较

两种 SLAM 的建图特点如下：

  1. 建图效果比较：显然 hector 的清晰度更高，对细节的处理更好
image-20240315234927342 image-20240315235001611
  1. 分辨率：Hector 地图分辨率更高
  • Gmapping 保存的地图的分辨率为0.050米/像素（Received a 4000 X 4000 map @ 0.050 m/pix）
  • Hector 保存的地图的分辨率为0.025米/像素（Received a 1024 X 1024 map @ 0.025 m/pix）
  1. 执行速度：Gmapping 建图速度快于 Hector，Hector 只能在小车低速运行时才能采样到充足的数据，Gmapping 允许的速度范围更大

在建好地图的上进行导航功能

文件分析

下一步执行的是模拟导航操作，需要执行的 launch 文件为 wpb_navigation.launch，其内容如下：

<launch>

  <!-- We resume the logic in empty_world.launch, changing only the name of the world to be launched -->
  <include file="$(find gazebo_ros)/launch/empty_world.launch">
    <arg name="world_name" value="$(find wpr_simulation)/worlds/robocup_home.world"/>
    <arg name="paused" value="false"/>
    <arg name="use_sim_time" value="true"/>
    <arg name="gui" value="true"/>
    <arg name="recording" value="false"/>
    <arg name="debug" value="false"/>
  </include>

  <!-- Spawn the objects into Gazebo -->
  <node name="bed" pkg="gazebo_ros" type="spawn_model" args="-file $(find wpr_simulation)/models/bed.model -x 5.0 -y -3.9 -z 0 -Y 3.14159 -urdf -model bed" />
  <node name="sofa" pkg="gazebo_ros" type="spawn_model" args="-file $(find wpr_simulation)/models/sofa.model -x -1.0 -y -3.9 -z 0 -Y 1.57 -urdf -model sofa" />
  <node name="tea_table" pkg="gazebo_ros" type="spawn_model" args="-file $(find wpr_simulation)/models/tea_table.model -x -2.1 -y -2.2 -z 0 -Y 1.57 -urdf -model tea_table" />
  <node name="bookshelft" pkg="gazebo_ros" type="spawn_model" args="-file $(find wpr_simulation)/models/bookshelft.model -x 2.0 -y -0.55 -z 0 -Y -1.57 -urdf -model bookshelft" />

  <node name="kitchen_table" pkg="gazebo_ros" type="spawn_model" args="-file $(find wpr_simulation)/models/table.model -x -3.5 -y 3.7 -z 0 -Y 1.57 -urdf -model kitchen_table" /> 

  <node name="cupboard_0" pkg="gazebo_ros" type="spawn_model" args="-file $(find wpr_simulation)/models/cupboard.model -x -2.0 -y 0.7 -z 0 -Y 1.57 -urdf -model cupboard_0" />
  <node name="cupboard_1" pkg="gazebo_ros" type="spawn_model" args="-file $(find wpr_simulation)/models/cupboard.model -x -1.3 -y 3.7 -z 0 -Y -1.57 -urdf -model cupboard_1" /> 

  <node name="dinning_table_0" pkg="gazebo_ros" type="spawn_model" args="-file $(find wpr_simulation)/models/table.model -x 1.5 -y 1.5 -z 0 -Y 1.57 -urdf -model dinning_table_0" />
  <node name="dinning_table_1" pkg="gazebo_ros" type="spawn_model" args="-file $(find wpr_simulation)/models/table.model -x 1.5 -y 2.0 -z 0 -Y 1.57 -urdf -model dinning_table_1" />
  <node name="dinning_table_2" pkg="gazebo_ros" type="spawn_model" args="-file $(find wpr_simulation)/models/table.model -x 2.7 -y 1.5 -z 0 -Y 1.57 -urdf -model dinning_table_2" />
  <node name="dinning_table_3" pkg="gazebo_ros" type="spawn_model" args="-file $(find wpr_simulation)/models/table.model -x 2.7 -y 2.0 -z 0 -Y 1.57 -urdf -model dinning_table_3" />

  <node name="chair_0" pkg="gazebo_ros" type="spawn_model" args="-file $(find wpr_simulation)/models/chair.model -x 1.5 -y 1.2 -z 0 -Y 1.57 -urdf -model chair_0" />
  <node name="chair_1" pkg="gazebo_ros" type="spawn_model" args="-file $(find wpr_simulation)/models/chair.model -x 1.5 -y 2.3 -z 0 -Y -1.57 -urdf -model chair_1" />
  <node name="chair_2" pkg="gazebo_ros" type="spawn_model" args="-file $(find wpr_simulation)/models/chair.model -x 2.7 -y 1.2 -z 0 -Y 1.57 -urdf -model chair_2" />
  <node name="chair_3" pkg="gazebo_ros" type="spawn_model" args="-file $(find wpr_simulation)/models/chair.model -x 2.7 -y 2.3 -z 0 -Y -1.57 -urdf -model chair_3" />

  <!-- Spawn a robot into Gazebo -->
  <node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="-file $(find wpr_simulation)/models/wpb_home.model -urdf -x -6.0 -y -0.5 -model wpb_home" />

  <!-- Run the map server -->
  <node name="map_server" pkg="map_server" type="map_server" args="$(find wpr_simulation)/maps/map_hector.yaml"/>

  <!--- Run AMCL -->
  <include file="$(find wpb_home_tutorials)/nav_lidar/amcl_omni.launch" />

  <!--- Run move base -->
  <node pkg="move_base" type="move_base" respawn="false" name="move_base"  output="screen">
    <rosparam file="$(find wpb_home_tutorials)/nav_lidar/costmap_common_params.yaml" command="load" ns="global_costmap" />
    <rosparam file="$(find wpb_home_tutorials)/nav_lidar/costmap_common_params.yaml" command="load" ns="local_costmap" />
    <rosparam file="$(find wpb_home_tutorials)/nav_lidar/local_costmap_params.yaml" command="load" />
    <rosparam file="$(find wpb_home_tutorials)/nav_lidar/global_costmap_params.yaml" command="load" />
    <rosparam file="$(find wpb_home_tutorials)/nav_lidar/local_planner_params.yaml" command="load" />
    <param name="base_global_planner" value="global_planner/GlobalPlanner" /> 
    <param name="use_dijkstra" value="true"/>
    <param name="base_local_planner" value="wpbh_local_planner/WpbhLocalPlanner" />
    <param name= "controller_frequency" value="10" type="double"/>
  </node>

  <!-- RViz and TF tree -->
  <arg name="model" default="$(find wpb_home_bringup)/urdf/wpb_home.urdf"/>
  <arg name="gui" default="false" />
  <arg name="rvizconfig" default="$(find wpr_simulation)/rviz/nav.rviz" />

  <param name="robot_description" command="$(find xacro)/xacro $(arg model)" />
  <param name="use_gui" value="$(arg gui)"/>

  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"/>
  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" >
    <rosparam command="load" file="$(find wpb_home_bringup)/config/wpb_home.yaml" />
  </node>
  <node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />

</launch>

主要可以分成四部分：

  • 创建 Gazebo 世界并放置物体和 robot
  • 启动地图服务器，加载并广播已经生成的地图
  • 启用 AMCL（自适应蒙特卡洛定位）、本地指向路径生成
  • RViz 节点启动，发布 tf 话题

在这里需要将上一阶段生成的地图 yaml 和 pgm 文件全部移动到地图服务器节点指定的位置 $(find wpr_simulation)/maps/map_hector.yaml，等待地图服务器启动服务时调用；注意不要随便修改地图的名字，yaml 文件中包括其指向的 pgm 文件的名字，需要保持同步，才能加载地图。

实验操作

在 ROS 中，使用 roslaunch wpr_simulation wpb_navigation.launch启动 Navigation 导航系统后，可以在 Rviz 里看到障碍物栅格周围有一圈浅蓝色的安全边界。机器人的路径规划会避开黑色障碍物栅格和淡蓝色的安全边界，在剩余的白色可通行区域里寻找出一条最短路径。

在刚刚进入地图时，可能 Gazebo 和 RViz 显示的机器人位置不同，需要使用 RViz 中的 2D Pos Estimate 操作将机器人移动到地图边界与实际建模相匹配的情形，拖动生成的箭头可以选定机器人指向的方向。

地图匹配后，可以使用 2D Nav Goal 指定机器人的目标地点，类似的也可以拖动选定指向的方向。确定后，路线规划模块则会计算路线并驱动机器人进行移动。

机器人的导航速度可以通过配置文件进行调整，调整的文件和真实机器人移动时使用的文件一致：/src/wpb_home/wpb_home_tutorials/nav_lidar/local_planner_params.yaml

参数名称 说明
max_vel_trans 移动速度的最大值限制，单位为米/秒。
max_vel_rot 转动速度的最大值限制，单位为弧度/秒。
acc_scale_trans 移动速度的比例系数，这个值越大，移动速度越快。（调速主要字段）
acc_scale_rot 转动速度的比例系数，这个值越大，转动速度越快。
goal_dist_tolerance 机器人到达目标点的判定距离，单位为米。
goal_yaw_tolerance 机器人到达目标点的判定角度，单位为弧度。
scan_topic 获取激光雷达数据的主题名称，主要用于导航中的动态避障。

演示视频点这里

软件包（节点）和话题订阅发布情况

使用 rqt_graph 统计得到的节点、话题发布情况如下：

rosgraph

节点分析

cookedbear@ubuntu:~/ros/task1/BUAA-SoftwareEngineering-2024-ROS-Single$ rosnode list
/amcl
/gazebo
/gazebo_gui
/joint_state_publisher
/map_server
/move_base
/robot_state_publisher
/rosout
/rviz
  • /amcl (Adaptive Monte Carlo Localization)：实现了 AMCL 算法，用于机器人在已知地图上的自我定位，配合激光雷达等传感器使用
  • /gazebo：Gazebo 仿真器运行需要的基础节点
  • gazebo_gui：仿真器的GUI节点
  • /joint_state_publisher：发布关于机器人各个关节的位置、速度、力等关节信息
  • /map_server：提供地图数据服务，通过载入地图文件并作为话题发布出去，提供基础的地图信息
  • /move_base：执行路径规划和导航，内部服务通过接受一个目标位置，控制机器人规划并执行到达该位置的最佳路径
  • /robot_state_publisher：读取机器人的 URDF，并发布整个机器人的状态信息
  • /rosout：ROS的日志节点
  • /rviz：RViz 应用运行的基础节点

服务分析

cookedbear@ubuntu:~/ros/task1/BUAA-SoftwareEngineering-2024-ROS-Single$ rostopic list
/amcl/parameter_descriptions
/amcl/parameter_updates
/amcl_pose
/clicked_point
/clock
/cmd_vel
/diagnostics
/gazebo/link_states
/gazebo/model_states
/gazebo/parameter_descriptions
/gazebo/parameter_updates
/gazebo/set_link_state
/gazebo/set_model_state
/imu/data
/initialpose
/joint_states
/kinect2/hd/camera_info
/kinect2/hd/image_color_rect
/kinect2/hd/image_color_rect/compressed
/kinect2/hd/image_color_rect/compressed/parameter_descriptions
/kinect2/hd/image_color_rect/compressed/parameter_updates
/kinect2/hd/image_color_rect/compressedDepth
/kinect2/hd/image_color_rect/compressedDepth/parameter_descriptions
/kinect2/hd/image_color_rect/compressedDepth/parameter_updates
/kinect2/hd/image_color_rect/theora
/kinect2/hd/image_color_rect/theora/parameter_descriptions
/kinect2/hd/image_color_rect/theora/parameter_updates
/kinect2/hd/parameter_descriptions
/kinect2/hd/parameter_updates
/kinect2/qhd/camera_info
/kinect2/qhd/image_color_rect
/kinect2/qhd/image_color_rect/compressed
/kinect2/qhd/image_color_rect/compressed/parameter_descriptions
/kinect2/qhd/image_color_rect/compressed/parameter_updates
/kinect2/qhd/image_color_rect/compressedDepth
/kinect2/qhd/image_color_rect/compressedDepth/parameter_descriptions
/kinect2/qhd/image_color_rect/compressedDepth/parameter_updates
/kinect2/qhd/image_color_rect/theora
/kinect2/qhd/image_color_rect/theora/parameter_descriptions
/kinect2/qhd/image_color_rect/theora/parameter_updates
/kinect2/qhd/parameter_descriptions
/kinect2/qhd/parameter_updates
/kinect2/qhd/points
/kinect2/sd/depth/camera_info
/kinect2/sd/depth_camera_info
/kinect2/sd/image_depth_rect
/kinect2/sd/image_ir_rect
/kinect2/sd/image_ir_rect/compressed
/kinect2/sd/image_ir_rect/compressed/parameter_descriptions
/kinect2/sd/image_ir_rect/compressed/parameter_updates
/kinect2/sd/image_ir_rect/compressedDepth
/kinect2/sd/image_ir_rect/compressedDepth/parameter_descriptions
/kinect2/sd/image_ir_rect/compressedDepth/parameter_updates
/kinect2/sd/image_ir_rect/theora
/kinect2/sd/image_ir_rect/theora/parameter_descriptions
/kinect2/sd/image_ir_rect/theora/parameter_updates
/kinect2/sd/parameter_descriptions
/kinect2/sd/parameter_updates
/kinect2/sd/points
/map
/map_metadata
/map_updates
/move_base/GlobalPlanner/parameter_descriptions
/move_base/GlobalPlanner/parameter_updates
/move_base/GlobalPlanner/plan
/move_base/GlobalPlanner/potential
/move_base/WpbhLocalPlanner/local_planner_target
/move_base/cancel
/move_base/current_goal
/move_base/feedback
/move_base/global_costmap/costmap
/move_base/global_costmap/costmap_updates
/move_base/global_costmap/footprint
/move_base/global_costmap/inflation_layer/parameter_descriptions
/move_base/global_costmap/inflation_layer/parameter_updates
/move_base/global_costmap/obstacle_layer/parameter_descriptions
/move_base/global_costmap/obstacle_layer/parameter_updates
/move_base/global_costmap/parameter_descriptions
/move_base/global_costmap/parameter_updates
/move_base/global_costmap/static_layer/parameter_descriptions
/move_base/global_costmap/static_layer/parameter_updates
/move_base/goal
/move_base/local_costmap/costmap
/move_base/local_costmap/costmap_updates
/move_base/local_costmap/footprint
/move_base/local_costmap/inflation_layer/parameter_descriptions
/move_base/local_costmap/inflation_layer/parameter_updates
/move_base/local_costmap/obstacle_layer/parameter_descriptions
/move_base/local_costmap/obstacle_layer/parameter_updates
/move_base/local_costmap/parameter_descriptions
/move_base/local_costmap/parameter_updates
/move_base/local_costmap/static_layer/parameter_descriptions
/move_base/local_costmap/static_layer/parameter_updates
/move_base/parameter_descriptions
/move_base/parameter_updates
/move_base/result
/move_base/status
/move_base_simple/goal
/odom
/particlecloud
/rosout
/rosout_agg
/scan
/scan_filtered
/tf
/tf_static
/visualization_marker
/visualization_marker_array
/waypoints_marker
/waypoints_marker_array

由于服务过多，下面主要按照功能进行分类并解释：

  • 传感器信息：

    • 激光雷达、相机 Kinect：/scan、/kinect2/hd/image_color_rect、/kinect2/qhd/points 等话题，传递从激光雷达和 Kinect 相机接收到的数据

    • 惯性测量单元 IMU：/imu/data 用于传输加速度、旋转速率等

  • 机器人控制：

    • 速度控制：/cmd_vel 接收控制机器人移动的速度命令，实验 2 中也是靠 cmd_vel 将键盘输入的指令传递给机器人的

    • 位置与速度：/odom 发布机器人的位置和速度信息

    • 关节状态：/joint_states 发布机器人关节的状态信息

  • 导航、定位：

    • AMCL：自适应蒙特卡洛定位，如 /amcl_pose 和 /particlecloud，计算使用 AMCL 算法得到的自我定位结果

    • 地图服务器：/map、/map_metadata 和 /map_updates 话题用于传递环境地图及其更新信息。

    • 导航规划：/move_base/*，处理路径规划和导航的任务

    • 时间、坐标：/clock和 /tf_static 等，处理模拟时间和坐标时的变换信息

  • 仿真软件：

    • Gazebo：/gazebo/model_states、/gazebo/link_states 等话题

    • RViz：/visualization_marker、/visualization_marker_array 等话题

  • 参数配置：

    • 查询参数，*/parameter_descriptions，动态获取节点的配置文件包含的参数

实验之后

地图服务器-map_server

map_server 包位于 navigation 中，提供了对外发布地图消息的 map_server 节点以及 保存动态生成的地图的 map_saver 命令行操作

地图格式

通过 map_saver 保存/被 map_server 调用的地图都以特定的文件形式存在：

  • yaml 文件存储地图的元数据、指向的图像名
image: testmap.png
resolution: 0.1
origin: [0.0, 0.0, 0.0]
occupied_thresh: 0.65
free_thresh: 0.196
negate: 0
  • image：包含地图数据的图像文件的路径；可以是绝对路径，也可以相对于 yaml 文件的路径
  • resolution：地图的分辨率，单位为米/像素
  • origin：贴图中左下角像素的二维姿势，如（x，y，yaw），其中 yaw 为逆时针旋转的角度；许多系统目前忽略 yam 字段。
  • occupied_thresh：在图像中占用概率大于该阈值的像素被视为完全被占用。
  • free_thresh：同理，占用概率小于该阈值的像素被认为是完全不受限的。
  • negate：是否应该颠倒地图中黑/白的语义（阈值的解释不受影响）

地图的具体数据以 yaml 文件指向的 pgm 文件为准，它以像素的颜色描述了地图中单元是否可达/被占用；彩色的图像会被平均至灰度图像

map_server

map_server 是一个ROS节点，它从磁盘读取地图并通过 ROS 服务提供给其他服务。

map_server 将地图图像数据中的颜色值转换为三元占用值：空闲（0）、占用（100）和未知（-1），未来还可能会加入更精细的划分方法

启动方式为：rosrun map_server map_server mymap.yaml

map_saver

map_saver 保存地图数据至磁盘，检索地图数据并将其写入 map.pgm 和 map.yaml。使用 -f 选项可以为输出文件提供不同的名称。–occ 和 –free 选项的值介于 0 和 100 之间。

启动方式为：rosrun map_server map_saver -f mymap

导航系统-Navigation

ROS 的 Navigation 系统已经包含了到杭州过程中需要的大部分功能，主要分为两个部分：一个用于定位自身，一个用于控制行进：

  • AMCL：蒙特卡洛自适应定位算法，使用概率理论在地图中对机器人的当前位置进行估计 ，并根据行进过程中的地形对位置进行筛选确定，寻找到范围较小的可信定位位置。（RViz 中机器人身边的绿色箭头就是 AMCL 推断的机器人位置，它会随着机器人运行而收束）
  • Move_Base：移动导航的核心包，将地图、坐标、路径和行为规划器连接，包含导航的大部分在内

Navigation 需要用户输入两个信息，而这些信息在实验中也都有输入，所以能够实现自助的导航操作：

  • 导航所处的全局地图：通过 SLAM 生成
  • 导航移动的终点：在 RViz 界面中设置

Move_Base

5-启智ROS_实验指导书_Python版本_1

  • map_server 在主题 /map 里提供全局地图信息，``global_costmap从主题里获得全局地图信息，再加上从sensor sources` 获得激光雷达和点云等信息，融合成一个全局的栅格代价地图。
  • 全局的栅格代价地图交给 global_planner 进行全局路径规划，再结合外部节点发送到主题 /move_base_simple/goal 里的移动终点得出紫色的全局移动路径。
  • local_costmap 从 global_costmap 中截取机器人周围一定距离内的地图，结合传感器信息生成一个局部的代价地图。
  • local_planner 从 global_planner 获得全局规划路径，结合从 local_costmap 获得的局部代价地图以及 AMCL 提供的机器人位置信息，计算出机器人当前应该执行的速度，发送到主题 /cmd_vel 里，驱动机器人沿着全局路径进行移动。

recovery_behaviors.png

AMCL

AMCL 是一个二维运动机器人的概率定位系统。它实现了自适应（或 KLD 采样）蒙特卡洛定位方法，该方法使用粒子滤波器根据一直地图跟踪机器人的姿态。

ROS 的 AMCL 算法实现参考了以下算法：sample_motion_model_odometry、beam_range_finder_model、likelihood_field_range_finder_mode、Augmented_MCL和KLD_Sampling_MCL。

AMCL 节点接受激光雷达产生的地图、激光扫描的输入，输出机器人的预期姿态；启动时，AMCL 会根据提供的参数初始化其粒子过滤器。该节点仅适用于激光扫描和激光地图，它可以扩展到与其他传感器数据一起工作。

AMCL 具有众多参数设置，详情可见官方 wiki

AMCL&odom位姿误差修正

  • odom：即 odometry 里程计，是 ROS 使用的基本定位系统。里程计定位基于死亡推算（Dead Reckoning）方法，它通过测量机器人的相对运动来估计机器人的位移。这种方法的优点是简单、反应快速。然而，由于每次位移估计都会累积一些误差，随着时间的推移，这些误差会累积起来，导致定位的漂移，这是里程计定位的一个主要缺点。

  • 与 odometry 相比，AMCL 的特点是能够处理定位的不确定性；但更新频率低，实际应用时甚至会给发布的消息打上未来的时间戳，让其他组件暂时使用这些数据直至下次计算结束（amcl：我算完了，在我算出下次转换关系之前，你们先将就用这个）。AMCL 的低频率作为位姿修正是可以接受的，但并不能取代 odom 实现具体移动的计算。

  • 但更常见的，odom 指的是里程计的坐标系，在小车启动底盘时建立，充当局部参照系时用处更大，但不适合作为全局参照 frame，因为它和机器人的姿态相对位置是随时可变的（死亡推算产生的漂移）

  • map_frame（map）可以视为全局坐标系，和 odom 坐标系同时启动且初始时重合，显然 map 和 odom 会在运动过程中产生偏移，这时就需要发布一个相对坐标系的偏移量来修正 odom 的误差（也就是 AMCL 的工作）

    • AMCL 在实际使用时，会先获取机器人的世界坐标，经过一系列变换发布一个 map → odom 的 frame 系变换

amcl_localization.png

  1. 得到base_link在世界坐标系global_map的坐标变换tmp_tf （即base_link在global_map下的坐标）
  2. 那么tmp_tf.inverse()则表示世界坐标系global_map到base_link的坐标变换（即global_map在base_link下的坐标）
  3. 由于base_link到odom坐标系的坐标变换是可以直接知道的（即base_link在odom下的坐标）
  4. 因此，使用tf.transformPose可以获得global_map到odom的变换tmp1_tf，（即global_map原点在odom坐标系下的坐标)
  5. 最后，对tmp1_tf求逆，得到odom–>map的变换，（即odom在global_map坐标系下的坐标）
  6. 发布odom–>map即可实现修正

参考内容：