参考引用
- 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 变换
- <scan frame> → 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> <!-- 载入机器人和 SLAM 仿真环境 --> <!-- 如果使用实体机器人,则将下行代码替换为启动激光雷达和底盘控制的 launch 文件即可 --> <include file="$(find wpr_simulation)/launch/wpb_stage_slam.launch" /> <!-- Hector SLAM 节点 --> <node pkg="hector_mapping" type="hector_mapping" name="hector_mapping" /> <!-- rviz 显示 --> <!-- 其中 args 是运行该 launch 文件并在 rviz 中添加相关选项后保存的配置文件 --> <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" />
<!-- Hector SLAM 节点 -->
<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>
<!-- 第一个 Hector_Mapping 建图节点 -->
<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>
<!-- 第二个 Hector_Mapping 建图节点 -->
<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>
<!-- **************************** 分割线 **************************** -->
<!-- 载入 SLAM 的仿真场景 -->
<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>
<!-- 载入 1号机器人 -->
<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>
<!-- 载入 2号机器人 -->
<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
更直观化的里程计演示请查看视频 什么是里程计