移动机器人激光SLAM导航（三）：Hector SLAM 篇
参考引用
- Hector_Mapping ROS-Wiki
- 从零开始搭二维激光SLAM
- 机器人工匠阿杰
- wpr_simulation
移动机器人激光SLAM导航（文章链接汇总）
1. 基于滤波器的 SLAM 问题
1.1 什么是 SLAM
-
什么是SLAM
-
SLAM 就是为了构建地图用的，这个地图可以保存下来，用于后续的定位及导航避障中，也有一些 SLAM 作为里程计在使用，始终提供估计的位姿，目前主流 SLAM 的结构分为前端里程计，后端优化，回环检测三个大模块
- 前端里程计：始终累加位姿，作为里程计使用
- 后端优化：使用图的结构模型，优化整体位姿，减小前端里程计产生的累计误差
- 回环检测：可以提供一个更强烈的图结构的约束，能够更好的减小累计误差
1.2 SLAM 问题的数学表述
- 运动方程（提供对状态 x x x 的先验 x k − 1 x_{k-1} xk−1，正向推理）
- 其中 x x x 位姿， k − 1 / k k-1/k k−1/k 时刻， u u u 控制器输入/运动测量， w w w 噪声
x k = f ( x k − 1 , u k , w k ) x_k=f(x_{k-1}, u_k, w_k) xk=f(xk−1,uk,wk)
- 观测方程（提供对状态 x x x 的后验 x k x_k xk，由果溯因）
- 其中 z z z 传感器读数， y y y 路标， j j j 路标编号， v v v 噪声
z k , j = h ( y j , x k , v k , j ) z_{k,j}=h(y_j, x_k, v_{k,j}) zk,j=h(yj,xk,vk,j)
- 已知量：上一时刻位姿 x k − 1 x_{k-1} xk−1，控制器输入/运动测量 u k u_k uk，当前时刻对路标 j j j 的观测 z k , j z_{k,j} zk,j
- 估计量：当前时刻机器人位姿 x k x_k xk（定位），路标 j j j 的位置 y j y_j yj（建图）
1.3 SLAM 概率模型
-
SLAM（Simultaneous Localization and Mapping）：给定传感器数据情况下，同时进行机器人位姿和地图估计
- 得到一个精确的位姿需要与地图进行匹配
- 得到一个良好的地图需要有精确的位姿
-
SLAM 条件联合概率分布
- 1 : t 1:t 1:t 表示从起始到 t 时刻
- z 1 : t z_{1:t} z1:t 表示传感器观测数据（如 /scan）
- u 1 : t u_{1:t} u1:t 表示里程计测量数据（如 /odom）
- m m m 表示地图， x 1 : t x_{1:t} x1:t 表示机器人轨迹/位姿估计（定位）
p ( x 1 : t , m ∣ z 1 : t , u 1 : t − 1 ) p(x_{1:t},m|z_{1:t},u_{1:{t-1}}) p(x1:t,m∣z1:t,u1:t−1)
2. Hector_Mapping
2.1 简介
-
Hector_Mapping 是一种无需里程计数据的 SLAM 方法，利用激光雷达获得二维姿态估计，虽然没有回环检测功能，但对于大多真实场景，它是较准确的，该系统已用于无人地面机器人/车辆、手持测绘设备、四旋翼无人机
-
硬件要求
- 需要高精度的激光扫描仪（SICK、hokuyo 等），扫描周围环境时，节点会使用 TF 变换，因此无需将雷达固定，并且不需要里程计数据
2.2 话题节点
- 订阅的话题（Topic）
- scan (sensor_msgs/LaserScan)：订阅 2D 激光雷达扫描数据
- syscommand (std_msgs/String)：如果字符串等于 “reset”，则地图和机器人姿态将重置为初始状态
- 发布的话题（Topic）
- map_metadata (nav_msgs/MapMetaData)：发布 Meta 地图数据（存储地图描述信息）
- map (nav_msgs/OccupancyGrid)：发布占据栅格地图数据
- slam_out_pose (geometry_msgs/PoseStamped)：原始的机器人位姿（无协方差）
- poseupdate (geometry_msgs/PoseWithCovarianceStamped)：校正后的机器人位姿（具有不确定性的高斯估计）
- Service
- dynamic_map (nav_msgs/GetMap)：获取地图数据
- reset_map (std_srvs/Trigger)：调用这个服务来重置地图，Hector_Mapping 将从头开始一张全新的地图。注意，这不会重新启动机器人的姿势，而是会从上次记录的姿势重新开始
- pause_mapping (std_srvs/SetBool)：调用此服务来停止/开始处理激光扫描
- restart_mapping_with_new_pose (hector_mapping/ResetMapping)：调用此服务来重置地图、机器人的姿势并恢复建图（如果暂停）
2.3 TF 变换
-
必要的 TF 变换
→ base_frame ：通常为固定值，激光雷达坐标系 与 基坐标系 之间的变换，一般由 robot_state_publisher 或 static_transform_publisher 发布
-
发布的 TF 变换
- map → odom：地图坐标系 与 里程计坐标系 之间的变换，估计机器人在地图中的位姿（仅在参数 “pub_map_odom_transform” 为 true 时提供）
-
ROS 中常用坐标系
- map：地图坐标系，也被称为世界坐标系，是静止不动的
- odom：里程计坐标系，相对于 map 来说一般是静止的，有些情况下会变动（定位节点为了修正机器人的位姿从而改变了 map->odom 间的坐标变换）
- base_footprint：位于机器人底盘中心在地面的投影，不提供高度信息，代表机器人的 2D 位姿
- base_link：位于机器人几何中心，与机器人刚性连接，相对于 base_stabilized 坐标系增加了横滚角和俯仰角
- base_stabilized：坐标系添加了机器人相对于 map/odom 层的高度信息（对于没有横滚/俯仰运动的平台，base_stabilized 等价于 base_link）
- laser_link：激光雷达的坐标系，相对于base_link来说是静止的，因为雷达装在机器人上
2.4 建图测试
本小节使用 移动机器人激光SLAM导航（二）：运动控制与传感器篇 中安装的测试环境 wpr_simulation（hector_mapping 已在此环境中通过脚本安装）
- 在 wpr_ws 工作空间新建功能包 slam_pkg
$ cd ~/wpr_ws/src $ catkin_create_pkg slam_pkg roscpp rospy std_msgs $ code . # 在 VSCode 中编辑 - 在 slam_pkg 中新建 launch 文件夹，并在 launch 文件夹中新建 hector.launch 文件
<launch> <include file="$(find wpr_simulation)/launch/wpb_stage_slam.launch" /> <node pkg="hector_mapping" type="hector_mapping" name="hector_mapping" /> <node pkg="rviz" type="rviz" name="rviz" args="-d $(find slam_pkg)/rviz/slam.rviz" /> <node pkg="rqt_robot_steering" type="rqt_robot_steering" name="rqt_robot_steering" /> launch> - 编译并启动 hector.launch 建图
$ cd ~/wpr_ws $ catkin_make $ source devel/setup.bash $ roslaunch slam_pkg hector.launch
2.5 建图参数设置 Parameters
-
~map_update_distance_thresh (double, default: 0.4)
- 地图更新的移动距离阈值 [单位：米]，越小则更新越快
- 每次地图更新后，机器人必须移动超过这个阈值，或满足 map_update_angle_thresh 参数描述的位移角度变化，才会再次更新地图
-
~map_update_angle_thresh (double, default: 0.9)
- 地图更新的旋转角度闯值 [单位：弧度]，越小则更新越快
- 每次地图更新后，机器人必须转动超过这个阈值，并产生超过 map_update_distance_thresh 阈值的位移才会再次更新地图
-
~map_pub_period (double, default: 2.0)
- 地图发布的周期 [单位：秒]
<launch>
<include file="$(find wpr_simulation)/launch/wpb_stage_slam.launch" />
<node pkg="hector_mapping" type="hector_mapping" name="hector_mapping">
<param name="map_update_distance_thresh" value="0.1" />
<param name="map_update_angle_thresh" value="0.1" />
<param name="map_pub_period" value="0.1" />
node>
<node pkg="rviz" type="rviz" name="rviz" args="-d $(find slam_pkg)/rviz/slam.rviz" />
<node pkg="rqt_robot_steering" type="rqt_robot_steering" name="rqt_robot_steering" />
launch>
其他参数请参考 Hector_Mapping ROS-Wiki
- 调参效果对比 wpb_hector_comparison.launch
<launch>
<group ns="slam_1">
<node pkg="hector_mapping" type="hector_mapping" name="hector_mapping_1">
<param name="map_update_distance_thresh" value="0.5"/>
<param name="map_update_angle_thresh" value="0.5" />
<param name="map_pub_period" value="0.2" />
<param name="map_frame" value="slam_1/map" />
<param name="base_frame" value="slam_1/base_footprint" />
<param name="odom_frame" value="slam_1/odom" />
node>
group>
<group ns="slam_2">
<node pkg="hector_mapping" type="hector_mapping" name="hector_mapping_2">
<param name="map_update_distance_thresh" value="0.1"/>
<param name="map_update_angle_thresh" value="0.1" />
<param name="map_pub_period" value="0.2" />
<param name="map_frame" value="slam_2/map" />
<param name="base_frame" value="slam_2/base_footprint" />
<param name="odom_frame" value="slam_2/odom" />
node>
group>
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="world_name" value="$(find wpr_simulation)/worlds/slam_simple.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>
<include file="$(find wpr_simulation)/launch/wpb_slam_template.launch">
<arg name="robot_namespace" value="slam_1" />
<arg name="local_x" value="0" />
<arg name="local_y" value="-0.3" />
<arg name="local_yaw" value="0" />
include>
<include file="$(find wpr_simulation)/launch/wpb_slam_template.launch">
<arg name="robot_namespace" value="slam_2" />
<arg name="local_x" value="0" />
<arg name="local_y" value="0.3" />
<arg name="local_yaw" value="0" />
include>
<node pkg="rqt_robot_steering" type="rqt_robot_steering" name="rqt_robot_steering"/>
<node pkg = "topic_tools" type = "relay" name = "relay_1" args="/cmd_vel /slam_1/cmd_vel" />
<node pkg = "topic_tools" type = "relay" name = "relay_2" args="/cmd_vel /slam_2/cmd_vel" />
launch>
3. TF 和 里程计
3.1 TF 系统
-
地面移动机器人在地图中的位姿描述方式（x, y, yaw）
-
ROS 中通过 TF（TransForm，坐标系变换） 来获取机器人具体的定位/位姿数值
- TF 主要用于描述两个坐标系之间的空间关系
- TF 关系由特定的 ROS 节点以消息包的形式发布到 /tf 话题中去，其他节点通过订阅这个 /tf 话题来查询坐标系
-
案例测试
# 利用 2.4 小节创建的 hector 建图包 $ cd ~/wpr_ws $ source devel/setup.bash $ roslaunch slam_pkg hector.launch
- 查看 /tf 话题类型
$ rostopic type /tf tf2_msgs/TFMessage
-
查看 /tf 话题数值
$ rostopic echo /tf ... --- transforms: - header: seq: 0 stamp: secs: 780 nsecs: 481000000 frame_id: "map" child_frame_id: "scanmatcher_frame" transform: translation: x: 2.27857303619 y: 1.645611763 z: 0.0 rotation: x: 0.0 y: 0.0 z: -0.352547165132 w: 0.935794045908 ... -
查看 TF 关系树
$ rosrun rqt_tf_tree rqt_tf_tree
3.2 里程计
本小节使用 移动机器人激光SLAM导航（二）：运动控制与传感器篇 中安装的测试环境
- hector_mapping 在长直走廊建图
- 由于缺少参照物特征的变化，导致机器人无法估计自己的位移，建图失败
- 解决办法：轮子转过的圈数×轮子周长=走过的距离（轮子里程计算法）
$ cd ~/wpr_ws $ source devel/setup.bash $ roslaunch wpr_simulation wpb_corridor_hector.launch
- GMapping 在长直走廊建图
- 由于 GMapping 自带里程计算法，在里程计的帮助下，激光 SLAM 有效克服了建图过程中位移特征缺失的问题
$ cd ~/wpr_ws $ source devel/setup.bash $ roslaunch wpr_simulation wpb_corridor_gmapping.launch
- 激光雷达和里程计输出的 TF 坐标变换关系
- 激光雷达：map --> base_footprint
- 里程计：odom --> base_footprint
- GMapping 的核心算法：先使用里程计推算机器人的位移，再使用激光雷达点云配准算法来修正里程计误差（如：轮子打滑） map --> odom --> base_footprint
更直观化的里程计演示请查看视频 什么是里程计