SLAM算法（ROS入门）

SLAM 建图​ 机器人研究的问题包含许许多多的领域我们常见的几个研究的问题包括建图(Mapping)、定位(Localization)和路径规划Path Planning如果机器人带有机械臂那么运动规划Motion Planning也是重要的一个环节。而同步定位与建图SLAM问题位于定位和建图的交集部分。​ SLAM 希望机器人从未知环境的未知地点出发在运动过程中通过重复观测得到的地图特征比如墙角柱子等定位自身位置和姿态再根据自身位置增量式的构建地图从而达到同时定位和地图构建的目的。运用SLAM技术我们可以将一个机器人放到未知环境中的未知位置然后机器人可以一边移动一边创建环境的地图当地图创建好之后我们就可以控制机器人更加准确移动到地图中的任意角落啦地图​ ROS 中的地图很好理解就是一张普通的灰度图像通常为pgm 格式。这张图像上的黑色像素表示障碍物白色像素表示可行区域灰色是未探索的区域。如下图所示​ 在 SLAM 建图的过程中你可以在 RViz 里看到一张地图被逐渐建立起来的过程类似于一块块拼图被拼接成一张完整的地图。这张地图对于我们定位、路径规划都是不可缺少的信息。事实上地图在 ROS 中是以 Topic 的形式维护和呈现的这个 Topic 名称就叫做/map它的消息类型是nav_msgs/OccupancyGrid。锁存发布方式​ 由于/map中实际上存储的是一张图片为了减少不必要的开销这个 Topic 往往采用锁存latched的方式来发布。什么是锁存其实就是地图如果没有更新就维持着上次发布的内容不变此时如果有新的订阅者订阅消息这时只会收到一个/map的消息也就是上次发布的消息只有地图更新了比如 SLAM 又建出来新的地图这时/map才会发布新的内容。 锁存器的作用就是将发布者最后一次发布的消息保存下来然后把它自动发送给后来的订阅者。这种方式非常适合变动较慢、相对固定的数据例如地图然后只发布一次相比于同样的消息不定的发布锁存的方式既可以减少通信中对带宽的占用也可以减少消息资源维护的开销。地图类型​ 然后我们来看一下地图的 OccupancyGrid 类型是如何定义的你可以通过rosmsg show nav_msgs/OccupancyGrid来查看消息或者直接rosed nav_msgs OccupancyGrid.msg来查看 srv 文件。std_msgs/Header header #消息的报头 uint32 seq time stamp string frame_id #地图消息绑定在TF的哪个frame上一般为map nav_msgs/MapMetaData info #地图相关信息 time map_load_time #加载时间 float32 resolution #分辨率 单位m/pixel uint32 width #宽 单位pixel uint32 height #高 单位pixel geometry_msgs/Pose origin #原点 geometry_msgs/Point position float64 x float64 y float64 z geometry_msgs/Quaternion orientation float64 x float64 y float64 z float64 w int8[] data #地图具体信息​ 这个 srv 文件定义了/map 话题的数据结构包含了三个主要的部分: header, info 和 data。header 是消息的报头保存了序号、时间戳、frame 等通用信息info 是地图的配置信息它反映了地图的属性data 是真正存储这张地图数据的部分它是一个可变长数组int8后面加了[]你可以理解为一个类似于 vector 的容器它存储的内容有 width*height 个 int8 型的数据, 也就是这张地图上每个像素。GmappingGmapping SLAM 软件包​ Gmapping 算法是目前基于激光雷达和里程计方案里面比较可靠和成熟的一个算法是 ROS 中曾经最常用、最经典的 2D SLAM 算法之一至今仍有广泛应用。GMapping 基于Rao-Blackwellized 粒子滤波 (RBPF)算法。它将 SLAM 问题分解为两个部分定位问题通过粒子滤波器来估计机器人的位姿位置和姿态。每个粒子都代表一个可能的机器人轨迹。建图问题在每个粒子给定的位姿下构建一个独立的栅格地图。​算法流程是通过里程计数据进行运动预测更新粒子的位姿然后使用激光雷达的观测数据来计算每个粒子的权重即这个位姿与观测数据的匹配程度最后根据权重进行重采样淘汰掉权重低的粒子复制权重高的粒子。最终输出的地图是根据最高权重粒子或加权平均的轨迹构建的。直接用 apt 二进制安装sudoapt-getinstallros-$ROS_DISTRO-gmappingGmapping SLAM 计算图​ Gmapping 是应用最为广泛的 2D slam 方法主要是利用RBPFRao-Blackwellized Particle Filters方法所以需要了解粒子滤波的方法利用统计特性描述物理表达式下的结果。gmapping 的作用是根据激光雷达和里程计Odometry的信息对环境地图进行构建并且对自身状态进行估计。因此它得输入应当包括激光雷达和里程计的数据而输出应当有自身位置和地图。 下面我们从计算图消息的流向的角度来看看 gmapping 算法的实际运行中的结构输入/tf以及/tf_static 坐标变换类型为第一代的tf/tfMessage或第二代的tf2_msgs/TFMessage其中一定得提供的有两个 tf一个是base_frame与laser_frame之间的 tf即机器人底盘和激光雷达之间的变换一个是base_frame与odom_frame之间的 tf即底盘和里程计原点之间的坐标变换。odom_frame可以理解为里程计原点所在的坐标系。/scan: 激光雷达数据类型为sensor_msgs/LaserScan​/scan很好理解Gmapping SLAM 所必须的激光雷达数据而/tf是一个比较容易忽视的细节。尽管/tf这个 Topic 听起来很简单但它维护了整个 ROS 三维世界里的转换关系而slam_gmapping要从中读取的数据是base_frame与laser_frame之间的 tf, 只有这样才能够把周围障碍物变换到机器人坐标系下更重要的是base_frame与odom_frame之间的 tf这个 tf 反映了里程计电机的光电码盘、视觉里程计、IMU的监测数据也就是机器人里程计测得走了多少距离它会把这段变换发布到odom_frame和laser_frame之间。因此slam_gmapping会从/tf中获得机器人里程计的数据。输出/tf 主要是输出map_frame和odom_frame之间的变换/slam_gmapping/entropystd_msgs/Float64类型反映了机器人位姿估计的分散程度/mapslam_gmapping建立的地图/map_metadata 地图的相关信息输出的/tf里又一个很重要的信息就是map_frame和odom_frame之间的变换这其实就是对机器人的定位。通过连通map_frame和odom_frame这样map_frame与base_frame甚至与laser_frame都连通了。这样便实现了机器人在地图上的定位。同时输出的 Topic 里还有/map在上一节我们介绍了地图的类型在 SLAM 场景中地图是作为 SLAM 的结果被不断地更新和发布。里程计误差及修正目前 ROS 中常用的里程计广义上包括车轮上的光电码盘、惯性导航元件IMU、视觉里程计你可以只用其中的一个作为 odom也可以选择多个进行数据融合融合结果作为 odom。通常来说实际 ROS 项目中的里程计会发布两个 Topic/odom: 类型为nav_msgs/Odometry反映里程计估测的机器人位置、方向、线速度、角速度信息。/tf: 主要是输出odom_frame和base_frame之间的 tf。这段 tf 反映了机器人的位置和方向变换数值与/odom中的相同。​ 由于以上三种里程计都是对机器人的位姿进行估计存在着累计误差因此当运动时间较长时odom_frame和base_frame之间变换的真实值与估计值的误差会越来越大。你可能会想能否用激光雷达数据来修正odom_frame和base_frame的 tf。事实上 gmapping 不是这么做的里程计估计的是多少odom_frame和base_frame的 tf 就显示多少永远不会去修正这段 tf。gmapping 的做法是把里程计误差的修正发布到map_frame和odom_frame之间的 tf 上也就是把误差补偿在了地图坐标系和里程计原点坐标系之间。通过这种方式来修正定位。这样map_frame和base_frame甚至和laser_frame之间就连通了实现了机器人在地图上的定位。参数​ 但由于 gmapping 算法中需要设置的参数很多这种启动单个节点的效率很低。所以往往我们会把 gmapping 的启动写到 launch 文件中同时把 gmapping 需要的一些参数也提前设置好写进 launch 文件或 yaml 文件。launch !-- 定义参数激光扫描话题、基础坐标系、里程计坐标系 -- arg namescan_topic defaultscan / arg namebase_frame defaultbase_footprint/ arg nameodom_frame defaultodom_combined/ !-- 启动gmapping SLAM节点 -- node pkggmapping typeslam_gmapping nameslam_gmapping outputscreen !-- 基础坐标系机器人坐标系 -- param namebase_frame value$(arg base_frame)/ !-- 里程计坐标系 -- param nameodom_frame value$(arg odom_frame)/ !-- 地图更新间隔秒 -- param namemap_update_interval value0.2/ !-- 传感器参数 -- !-- 有效测距最大值米超过此值的测量将被忽略 -- param namemaxUrange value11/ !-- 传感器最大物理量程米 -- param namemaxRange value12/ !-- 扫描匹配优化参数 -- !-- 高斯分布的标准差影响扫描点匹配权重 -- param namesigma value0.05/ !-- 相关核的大小像素 -- param namekernelSize value1/ !-- 平移优化步长 -- param namelstep value0.05/ !-- 旋转优化步长弧度 -- param nameastep value0.05/ !-- 优化迭代次数 -- param nameiterations value5/ !-- 似然计算的标准差 -- param namelsigma value0.075/ !-- 障碍物信息增益 -- param nameogain value3.0/ !-- 激光扫描降采样率每n个点取1个 -- param namelskip value1/ !-- 扫描匹配最小得分阈值低于此值视为匹配失败 -- param nameminimumScore value30/ !-- 里程计运动模型噪声参数 -- !-- 平移噪声引起的平移误差 -- param namesrr value0.01/ !-- 旋转噪声引起的平移误差 -- param namesrt value0.02/ !-- 平移噪声引起的旋转误差 -- param namestr value0.01/ !-- 旋转噪声引起的旋转误差 -- param namestt value0.02/ !-- 更新触发条件 -- !-- 最小平移距离米触发地图更新 -- param namelinearUpdate value0.05/ !-- 最小旋转角度弧度触发地图更新 -- param nameangularUpdate value0.1/ !-- 时间更新秒即使未达到运动阈值也更新 -- param nametemporalUpdate value1/ !-- 粒子滤波参数 -- !-- 重采样阈值粒子权重方差阈值 -- param nameresampleThreshold value0.5/ !-- 使用的粒子数量 -- param nameparticles value8/ !-- 地图边界米 -- param namexmin value-10.0/ param nameymin value-10.0/ param namexmax value10.0/ param nameymax value10.0/ !-- 地图分辨率米/像素 -- param namedelta value0.05/ !-- 似然场采样参数 -- !-- 平移采样的范围 -- param namellsamplerange value0.01/ !-- 平移采样的步长 -- param namellsamplestep value0.01/ !-- 旋转采样的范围弧度 -- param namelasamplerange value0.005/ !-- 旋转采样的步长弧度 -- param namelasamplestep value0.005/ !-- 重映射扫描话题 -- remap fromscan to$(arg scan_topic)/ /node /launch总结优点开源经典作为 ROS 早期标配社区支持好文档丰富稳定可靠。对长廊友好在特征较少的长直走廊等环境下表现相对稳健。计算资源适中相比现代的图优化算法其计算负担在可接受范围内。缺点强依赖里程计里程计的精度直接影响建图质量。如果里程计漂移严重或打滑地图会迅速恶化。粒子退化长时间运行后粒子多样性会减少粒子退化可能导致定位失败无法恢复。无回环检测没有显式的闭环检测和校正机制。当机器人回到初始区域时地图可能会出现无法闭合的错位。不适合大场景随着地图尺寸和粒子数量的增加内存和计算消耗会急剧增长因此不适合构建大型环境地图。适用场景中小型、结构化的室内环境。拥有较精确轮式里程计的移动机器人。教学、入门和快速原型验证。KartoKarto SLAM 软件包​ Karto SLAM 是基于图优化的方法Karto SLAM 将机器人的位姿作为图的节点 (Node)将位姿之间的相对运动约束如里程计数据和激光雷达扫描匹配结果作为图的边 (Edge)。前端 (Frontend)通过扫描匹配 (Scan-Matching) 将当前的激光扫描数据与局部地图对齐计算出机器人两帧之间的相对位姿变换这构成了一个边。后端 (Backend)当机器人回到一个曾经访问过的地方时算法会检测到回环 (Loop Closure)。这会在图中增加一个额外的约束边连接当前节点和历史节点。最后通过稀疏位姿调整 (Sparse Pose Adjustment, SPA)等非线性最小二乘法来优化整个图使所有约束的误差最小化。这个过程会校正整个轨迹和地图消除累积误差。使用 apt 二进制安装sudo apt-get install ros-$ROS_DISTRO-slam-kartoKarto SLAM 计算图Karto SLAM 和 Gmapping SLAM 在工作方式上非常类似如下图所示输入的 Topic 同样是/tf和/scan其中/tf里要连通odom_frame与base_frame还有laser_frame。这里和 Gmapping 完全一样。唯一不同的地方是输出slam_karto 的输出少相比 slam_gmapping 了一个位姿估计的分散程度。参数launch !-- 启动Karto SLAM算法节点 -- node pkgslam_karto typeslam_karto nameslam_karto outputscreen !-- 重映射激光扫描话题 此处将默认的scan话题映射到当前使用的扫描话题 实际使用中可按需修改为实际的激光话题名 -- remap fromscan toscan/ !-- 里程计坐标系名称 该坐标系用于接收里程计位姿信息 必须与机器人发布的里程计坐标系一致 -- param nameodom_frame valueodom_combined/ !-- 地图更新间隔秒 控制地图发布和保存的频率 时间间隔越长计算负载越低但地图更新延迟越高 -- param namemap_update_interval value1/ !-- 地图分辨率米/像素 决定地图的精细程度 - 值越小地图越精细常用0.01-0.05 - 值越大计算效率越高 - 0.0252.5cm/像素高精度地图 -- param nameresolution value0.025/ /node /launch优缺点​ ROS 版本的 KartoSLAM其中采用的稀疏点调整the Spare Pose Adjustment(SPA)与扫描匹配和闭环检测相关。landmark 越多内存需求越大然而图优化方式相比其他方法在大环境下制图优势更大在某些情况下 Karto SLAM 更有效因为他仅包含点的图(robot pose)求得位置后再求 map。Karto 最重要的点就是引入了后端优化与回环检测。在 Karto 之前诞生的激光 SLAM 如 GMappingHector都是没有后端优化与回环检测的。优点高精度由于有后端优化和回环检测Karto 生成的地图全局一致性好精度较高。资源效率相比于其他图优化方法Karto 在节点和边的管理上做了优化资源消耗相对较低。缺点依赖里程计虽然是图优化但其前端的初始位姿估计仍然依赖于里程计糟糕的里程计会影响扫描匹配的效果和回环检测的成功率。回环检测较弱其回环检测机制不如 Cartographer 强大在特征相似的区域可能发生误判。适用场景中大型室内环境。需要较高地图精度的应用。希望在资源消耗和地图质量之间取得平衡的场景。HectorHector SLAM 软件包​ Hector SLAM 的核心是纯扫描匹配 (Scan-Matching)。它利用高斯牛顿法 (Gauss-Newton) 将当前的激光扫描帧直接与已经构建的地图进行匹配从而计算出机器人的位姿变换。 这个过程非常类似于图像配准它假设机器人运动是平滑的并且激光雷达的扫描频率足够高使得两帧之间的位移很小。它通过对占据栅格地图进行数值求导来获得梯度信息从而高效地找到最优匹配。​ Hector 对传感器要求较高需具备高更新频率且测量噪音小的激光扫描仪。其最大特点是无需依赖里程计所以在不平坦区域实现建图的空中无人机及地面小车具有运用的可行性利用已经获得的地图对激光束点阵进行优化估计激光点在地图的表示和占据网络的概率。获得激光点集映射到已有地图的刚体变换为避免局部最小而非全局最优出现地图使用多分辨率。使用 apt 二进制安装sudo apt install ros-noetic-hector-slamHector SLAM 计算图​ Hector SLAM 算法不同于前面两种算法Hector 只需要激光雷达数据而不需要里程计数据。这种算法比较适合手持式的激光雷达并且对激光雷达的扫描频率有一定要求。​ Hector 算法的效果不如 Gmapping、Karto因为它仅用到激光雷达信息。这样建图与定位的依据就不如多传感器结合的效果好。但 Hector 适合手持移动或者本身就没有里程计的机器人使用。Hector 的计算图如下所示位于中心的节点叫作hector_mapping它的输入和其他 SLAM 框架类似都包括了/tf和/scan另外 Hector 还订阅一个/syscommandTopic这是一个字符串型的 Topic当接收到reset消息时地图和机器人的位置都会初始化到最初最初的位置。在输出的 Topic 方面hector 多了一个/poseupdate和/slam_out_pose, 前者是具有协方差的机器人位姿估计后者是没有协方差的位姿估计。不需要里程计人抱着小车走也可以建图。效果不如 gmapping适合因条 件限制没有里程计的机器人。 建图时转向速度尽量缓慢建议 0.1rad/s 以下参数launch !-- Arguments -- arg nameodom_frame defaultodom_combined/ arg namebase_frame defaultbase_footprint/ arg namescan_subscriber_queue_size default5/ arg namescan_topic defaultscan/ arg namemap_size default2048/ arg namepub_map_odom_transform defaulttrue/ arg nametf_map_scanmatch_transform_frame_name defaultscanmatcher_frame/ arg namesimulation default false/ param name/use_sim_time value$(arg simulation) / !-- Hector mapping -- node pkghector_mapping typehector_mapping namehector_mapping outputscreen !-- Frame names -- param namemap_frame valuemap / param nameodom_frame value$(arg odom_frame) / param namebase_frame value$(arg base_frame) / !-- Tf use -- param nameuse_tf_scan_transformation valuetrue/ param nameuse_tf_pose_start_estimate valuefalse/ param namepub_map_scanmatch_transform valuetrue / param namepub_map_odom_transform value$(arg pub_map_odom_transform)/ param nametf_map_scanmatch_transform_frame_name value$(arg tf_map_scanmatch_transform_frame_name) / !-- Map size / start point -- param namemap_resolution value0.050/ param namemap_size value$(arg map_size)/ param namemap_start_x value0.5/ param namemap_start_y value0.5 / param namemap_multi_res_levels value2 / !-- Map update parameters -- param nameupdate_factor_free value0.4/ param nameupdate_factor_occupied value0.9 / param namemap_update_distance_thresh value0.1/ param namemap_update_angle_thresh value0.04 / param namemap_pub_period value2 / param namelaser_z_min_value value -0.1 / param namelaser_z_max_value value 0.1 / param namelaser_min_dist value0.12 / param namelaser_max_dist value3.5 / !-- Advertising config -- param nameadvertise_map_service valuetrue/ param namescan_subscriber_queue_size value$(arg scan_subscriber_queue_size)/ param namescan_topic value$(arg scan_topic)/ !-- Debug parameters -- !-- param nameoutput_timing valuefalse/ param namepub_drawings valuetrue/ param namepub_debug_output valuetrue/ -- /node /launch特点优点不依赖里程计这是其最大优势。非常适合手持设备、无人机 (UAV)、或者在不平坦地面上容易打滑的机器人。低延迟算法计算速度快实时性好。更新频率高可以处理高频率的激光雷达数据。**缺点 **依赖高频高精度激光雷达算法性能严重依赖激光雷达的扫描频率和精度。低频率如 10Hz或噪声大的激光雷达会导致匹配失败。对运动敏感剧烈的旋转或快速的平移会导致扫描匹配失败因为这破坏了“两帧间位移很小”的假设。对低特征环境敏感在长廊、空旷大厅等缺乏几何特征的环境中扫描匹配会退化导致定位漂移或错误。无回环检测与 GMapping 类似它没有后端优化和回环修正。适用场景配备高频激光雷达如 20Hz-40Hz的设备。手持激光扫描仪、无人机、背包式建图系统。地面平滑、机器人运动平稳的场景。CartographerCartographer 软件包Cartographer 是由 Google 开源的、目前业界领先的 2D 和 3D SLAM 库以其高精度和强大的鲁棒性著称。Cartographer 是一种基于图优化的 SLAM 方法但它引入了子图 (Submap)的概念来管理和优化地图。局部 SLAM (Frontend)将一系列连续的激光扫描数据插入到一个不断增长的子图中。这个过程通过扫描匹配来精确定位每一帧扫描在子图中的位置。当一个子图构建完成后它就被视为一个固定的、内部一致的“小地图”不再改变。全局 SLAM (Backend)在后端所有完成的子图被当作优化的基本单位。算法会不断地在新的扫描数据和所有历史子图之间进行回环检测。一旦检测到回环就在位姿图中增加一个约束。最后一个后台线程会定期运行优化器调整所有子图的位姿从而修正整个地图的累积误差。由于noetic版本没有集成的二进制版安装包我们需要通过源码安装安装依赖工具sudo apt-get update sudo apt-get install -y python3-wstool python3-rosdep ninja-build stow创建工作空间注意必须主目录下mkdir cartographer_ws cd cartographer_ws wstool init src wstool merge -t src https://raw.githubusercontent.com/cartographer-project/cartographer_ros/master/cartographer_ros.rosinstall wstool update -t src安装依赖并下载cartographer相关功能包,此处推荐使用国内用户用的rosdepcsudo pip install rosdepc sudo rosdepc init rosdepc update rosdep install --from-paths src --ignore-src --rosdistro${ROS_DISTRO} -y安装abseil cpp库src/cartographer/scripts/install_abseil.sh若与已装库冲突需要卸载之前的库可选sudo apt-get remove ros-${ROS_DISTRO}-abseil-cpp构建安装catkin_make_isolated --install --use-ninja source ~/cartographer_ws/devel_isolated/setup.bashCartographer 计算图​ 整个流程是cartographer_node接收雷达和里程计数据来计算机器人位姿和子图然后cartographer_occupancy_grid_node将这些子图拼接成一个完整的、可用于导航的栅格地图。核心 SLAM 节点cartographer_node输入左侧/scan接收激光雷达发布的雷达扫描信息。这是 Cartographer 进行扫描匹配和生成子图的主要数据源。/odom接收里程计数据。这个数据通常由机器人底盘或轮式编码器发布它提供了机器人的局部运动信息可以大大提高 SLAM 算法的准确性和效率。输出右侧/tf发布两个关键的坐标变换TFmap到odom的 TF 坐标系这是 Cartographer SLAM 算法的核心输出。它通过全局优化来修正里程计的累积漂移将本地里程计坐标系与全局地图坐标系对齐。odom到base_footprint的 TF 坐标系这张图中的odom到base_footprint的 TF 变换是由 Cartographer提供的但这只在一种特殊配置下provide_odom_frametrue发生。在之前的配置下这个变换是由robot_pose_ekf节点发布的在外面使用Cartographer 时需要关闭robot_pose_ekf节点其 tf 发布频率 200HZ 大于 robot_pose_ekf 的 20HZrobot_pose_ekf 发布的 tf 会被覆盖。/submap_list发布地图数据。这些数据以子图submaps的形式发布每一个子图都代表了机器人某个区域的局部地图。这个话题通常用于可视化例如在 RViz 中显示正在构建的地图。地图生成节点cartographer_occupancy_grid_node输入左侧/submap_list订阅由cartographer_node发布的子图数据。输出右侧/map发布最终的栅格地图nav_msgs/OccupancyGrid格式。这是机器人导航和路径规划通常需要的地图格式包含了地图的尺寸、分辨率以及每个栅格的占用状态空闲、占用、未知。参数Cartographer的配置采用lua文件具体配置可参考官方文档Cartographer ROS Integration — Cartographer ROS documentation-- Copyright 2016 The Cartographer Authors -- -- Licensed under the Apache License, Version 2.0 (the License); -- you may not use this file except in compliance with the License. -- You may obtain a copy of the License at -- -- http://www.apache.org/licenses/LICENSE-2.0 -- -- Unless required by applicable law or agreed to in writing, software -- distributed under the License is distributed on an AS IS BASIS, -- WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -- See the License for the specific language governing permissions and -- limitations under the License. include map_builder.lua -- 包含地图构建器配置 include trajectory_builder.lua -- 包含轨迹构建器配置 options { -- 核心组件配置 map_builder MAP_BUILDER, -- 地图构建器实例 trajectory_builder TRAJECTORY_BUILDER, -- 轨迹构建器实例 -- 坐标系设置 map_frame map, -- 地图坐标系名称 tracking_frame base_footprint, -- 传感器数据对齐的坐标系机器人基座 published_frame base_footprint, -- 发布位姿的坐标系 odom_frame odom_combined, -- 里程计坐标系名称 provide_odom_frame true, -- 是否发布map到odom的tf变换 publish_frame_projected_to_2d false, -- 是否将3D位姿投影到2D -- 传感器使用配置 use_odometry true, -- 是否使用里程计数据 use_nav_sat false, -- 是否使用GPS数据 use_landmarks false, -- 是否使用路标数据 -- 激光雷达配置 num_laser_scans 1, -- 使用的单线激光话题数量 num_multi_echo_laser_scans 0, -- 使用的多回波激光话题数量 num_subdivisions_per_laser_scan 1, -- 每条激光扫描细分的次数 num_point_clouds 0, -- 使用的点云话题数量 -- 系统参数 lookup_transform_timeout_sec 1.0, -- TF查找超时时间(秒) submap_publish_period_sec 0.3, -- 子图发布周期(秒) pose_publish_period_sec 5e-3, -- 位姿发布周期(秒)(200Hz) trajectory_publish_period_sec 30e-3, -- 轨迹发布周期(秒)(~33Hz) -- 传感器采样率 rangefinder_sampling_ratio 1., -- 测距仪(激光)采样率(1100%) odometry_sampling_ratio 1., -- 里程计采样率 fixed_frame_pose_sampling_ratio 1., -- 固定坐标系位姿采样率 imu_sampling_ratio 1., -- IMU采样率 landmarks_sampling_ratio 1., -- 路标采样率 } -- 2D建图配置 MAP_BUILDER.use_trajectory_builder_2d true -- 启用2D轨迹构建器 -- 2D轨迹构建器参数 TRAJECTORY_BUILDER_2D.submaps.num_range_data 35 -- 每个子图包含的扫描次数 TRAJECTORY_BUILDER_2D.min_range 0.3 -- 最小有效测距(m)过滤近距离噪声 TRAJECTORY_BUILDER_2D.max_range 8. -- 最大有效测距(m)过滤远距离噪声 TRAJECTORY_BUILDER_2D.missing_data_ray_length 1. -- 无效数据射线长度(m) TRAJECTORY_BUILDER_2D.use_imu_data false -- 是否使用IMU数据2D建图通常禁用 TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching true -- 启用在线相关扫描匹配 -- 实时相关扫描匹配器参数 TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.linear_search_window 0.1 -- 线性搜索窗口(m) TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.translation_delta_cost_weight 10. -- 平移变化代价权重 TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.rotation_delta_cost_weight 1e-1 -- 旋转变化代价权重 -- 位姿图优化参数 POSE_GRAPH.optimization_problem.huber_scale 1e2 -- Huber损失函数比例因子鲁棒优化 POSE_GRAPH.optimize_every_n_nodes 35 -- 每N个节点执行一次全局优化 POSE_GRAPH.constraint_builder.min_score 0.65 -- 约束匹配最小得分阈值(0-1) return options特点优点极高的精度和鲁棒性子图的设计和强大的后端回环检测能力使其能够构建大规模、高精度的地图并且全局一致性非常好。传感器融合可以很好地融合多种传感器数据如里程计、IMU 和激光雷达。IMU 数据对于减少旋转误差、改善扫描匹配效果至关重要。支持 2D 和 3D一套框架同时支持 2D 和 3D 建图。不强依赖里程计虽然推荐使用里程计和 IMU但在只有激光雷达的情况下也能工作其性能远超 Hector SLAM。缺点计算资源消耗大它是四种算法中资源消耗最大的对 CPU 和内存的要求都比较高尤其是在大场景中。配置复杂参数众多需要根据不同的传感器和场景进行仔细调优才能达到最佳效果。非实时闭环地图的最终闭环校正是在后台异步完成的可能不会立即看到地图“闭合”的效果。适用场景任何对地图精度有高要求的场景尤其是大型、复杂的环境如商场、办公楼、仓库。需要长时间、稳定运行的商业机器人产品。传感器配置丰富的平台尤其是有 IMU 的平台。地图的保存与使用​ 我们建图所得的地图数据是保存在内存中的当节点关闭时数据也会被一并释放我们需要将栅格地图序列化到的磁盘以持久化存储后期还要通过反序列化读取磁盘的地图数据再执行后续操作。在ROS中地图数据的序列化与反序列化可以通过map_server功能包实现。map_server简介map_server是一个和地图相关的功能包它可以将已知地图发布出来供导航和其他功能使用也可以保存SLAM建立的地图。要让map_server发布/map需要输入给它两个文件:地图文件通常为pgm格式地图的描述文件通常为yaml格式map_server功能包中提供了两个节点: map_saver 和 map_server前者用于将栅格地图保存到磁盘后者读取磁盘的栅格地图并以服务的方式提供出去。map_server安装命令如下:sudo apt install ros-ROS版本-map-server地图保存节点(map_saver)订阅的topic:map(nav_msgs/OccupancyGrid)订阅此话题用于生成地图文件。地图保存的语法比较简单编写一个launch文件内容如下:launchnodepkgmap_servertypemap_savernamemap_saver1args-f /home/GGB/catkin_ws/src/robot_navigation/map/mapoutputscreen/node/launchSLAM建图完毕后不能结束建图节点执行该launch文件即可保存地图。image: Software_Museum.pgm #指定地图文件 resolution: 0.050000 #地图的分辨率 单位为m/pixel origin: [-25.000000, -25.000000, 0.000000] #地图的原点 negate: 0 #0代表 白色为空闲 黑色为占据 occupied_thresh: 0.65 #当占据的概率大于0.65认为被占据 free_thresh: 0.196 #当占据的概率小于0.196认为无障碍其中占据的概率 occ (255-color_avg)/255.0 color_avg为RGB三个通道的平均值。使用地图服务(map_server)发布的话题发布地图元数据map_metadatanav_msgs / MapMetaData地图数据mapnav_msgs / OccupancyGrid服务static_map: 用于请求和响应当前的静态地图。参数地图坐标系〜frame_id字符串默认值“map”通过 map_server 的 map_server 节点可以读取栅格地图数据编写 launch 文件如下:launch arg namemap_file default$(find robot_navigation)/map/map.yaml/ node pkgmap_server namemap_server typemap_server args$(arg map_file) param nameframe_id valuemap/ /node /launchsh: 0.65 #当占据的概率大于0.65认为被占据free_thresh: 0.196 #当占据的概率小于0.196认为无障碍其中占据的概率 occ (255-color_avg)/255.0 color_avg为RGB三个通道的平均值。 ### 使用地图服务(map_server) **发布的话题** - 发布地图元数据map_metadatanav_msgs / MapMetaData - 地图数据mapnav_msgs / OccupancyGrid **服务** - static_map: 用于请求和响应当前的静态地图。 **参数** - 地图坐标系〜frame_id字符串默认值“map” 通过 map_server 的 map_server 节点可以读取栅格地图数据编写 launch 文件如下: