机器人操作系统（ROS）课件10.4 机器人SLAM及自主导航

机器人操作系统（ROS）机器人操作系统（ROS）机器人SLAM及自主导航10.4机器人操作系统（ROS）10.4机器人SLAM及自主导航

根据机器人所使用的传感器不同，可以将SLAM（SimultaneousLocalizationandMapping，同步定位与建图）分为激光SLAM和视觉SLAM。激光SLAM是使用激光雷达进行环境感知进而定位建图；视觉SLAM通过视觉传感器进行定位建图。本章我们将使用单线激光雷达实现环境地图的构建，并在已构建完成的地图中实现自主导航。自主导航机器人操作系统（ROS）10.4.1使用gmapping构建地图gmapping原理gmapping算法是基于RBPF(Rao-BlackwellisedParticleFilter)的激光2DSLAM算法。该算法将机器人的里程计位姿信息和激光雷达的点云数据进行结合。RBPFSLAM的核心是通过激光雷达感知环境信息，并在RBPF的基础上引入了改进的建议分布（Proposaldistribution）和自适应重采样技术，在估计粒子分布时，同时考虑里程计位姿信息和最新的激光雷达观测值，从而一定程度上减少了粒子数目和计算量，保证了建图的准确性，有效改善了RBPF的粒子耗散和计算量大的劣势。gmapping算法流程图机器人操作系统（ROS）10.4.1使用gmapping构建地图gmapping原理当节点获取到激光雷达和里程计的数据时，将最新时刻获取的激光雷达观测数据与之前构建的点云地图扫描匹配，确定当前机器人的位置。同时，算法会根据匹配的程度来计算得分，若得分在设定值的范围内，则匹配成功，采用改进的建议分布（即观测模型）进行粒子采样。若失败，则粒子采样使用运动模型x_t^((i))∼p(x_t∣x_(t-1),u_t)，其中x_t、x_(t-1)、u_t分别是当前时刻的机器人位姿，上一时刻机器人位姿，当前时刻里程计信息。通过当前时刻里程计数据和上一时刻机器人位姿推算当前时刻位姿并计算粒子权值。根据机器人当前位置和已构建的地图，通过计算来构建下一时刻地图。改进前的RBPF使用运动学模型作为粒子采样的建议分布，由于方差较大，只有少数粒子符合真实分布，因此必须进行重采样来使粒子数符合实际分布。改进后的建议分布在运动学模型的基础上根据观测值和上一时刻地图信息对采样的粒子进行加权，选用权重大的粒子进而更新地图，改进建议分布如下式所示：机器人操作系统（ROS）10.4.1使用gmapping构建地图gmapping原理

机器人操作系统（ROS）10.4.1使用gmapping构建地图PLICP模拟里程计mapping是一个依赖里程计的算法，除了激光扫描数据，也需要有里程计信息(Odometry)输入作为运算的前提，比如轮式里程计，视觉里程计等。laser_scan_matcher功能包是基于ROS的增量式激光扫描配准工具，可以通过扫描连续的两帧sensor_msgs/LaserScan消息之间匹配完成位姿估计，并将估计完成的位姿信息以geometry_msgs/Pose2D类型的话题进行发布。在仅有激光雷达传感器的情况下，该功能包可以作为单独的里程计估计器来使用。laser_scan_matcher功能包的核心是PLICP（点对线迭代最近点）扫描匹配算法。PLICP算法流程和ICP流程基本一样，不同之处在于ICP是找最近邻的一点，以点到点之间的距离作为误差，而PLICP是找到最近邻的两点，两点连线，是以点到线的距离作为误差，因此PLICP的匹配误差比ICP的匹配误差要小的多。机器人操作系统（ROS）安装步骤首先，运行如下命令，安装本次实验相关依赖库：$sudoapt-getinstalllibsdl1.2-dev$sudoaptinstalllibsdl-image1.2-devROS中已经集成了gmapping相关功能包的二进制文件，可以使用如下命令进行安装：$sudoapt-getinstallros-melodic-slam-gmapping下面通过源码安装scan_tools功能包，作用是使用激光数据为gmapping提供里程计。首先，进入工作空间的src文件夹下：$cd~/catkin_ws/src/使用如下命令克隆源码：$gitclone/ccny-ros-pkg/scan_tools.git10.4.1使用gmapping构建地图机器人操作系统（ROS）源码下载完成后，运行如下命令进行编译，也可将本书提供的功能包（scan_tools）拷贝至工作空间并进行编译：$cd~/catkin_ws/$catkin_make本次实验使用的激光雷达为镭神LS01B，这是一款价格低廉的二维雷达，能够实现在25米范围内360度的二维平面扫描。将本书配套代码中的激光雷达驱动功能包（ls01b_v2）复制到当前工作空间的src文件夹下，并进行编译。LS01B激光雷达安装步骤10.4.1使用gmapping构建地图机器人操作系统（ROS）如果在编译过程中出现无法找到csm功能包的错误时，运行如下命令安装csm功能包，并重新编译：$sudoapt-getinstallros-melodic-csm无法找到csm功能包10.4.1使用gmapping构建地图机器人操作系统（ROS）编译成功后，修改scan_tools/laser_scan_matcher/demo文件夹中的demo_gmapping.launch文件，其中<paramname=“serial_port”value=“/dev/ttyUSB0”/>，ttyUSB0为激光雷达端口号，读者根据实际情况修改，修改后的文件如下（部分）：<launch>##setupleishenlidar#################<nodename="ls01b_v2"pkg="ls01b_v2"type="ls01b_v2"output="screen"><paramname="scan_topic"value="scan"/><paramname="frame_id"value="laser_link"/><paramname="serial_port"value="/dev/ttyUSB0"/>……10.4.1使用gmapping构建地图机器人操作系统（ROS）配置参数讲解map_udpate_interval：每次更新地图的时间间隔（数据类型：float，默认值：5.0，单位：秒）。该值越小，节点将更频繁的更新地图，代价是计算负荷变大。maxUrange：激光的最大可用范围。光束被裁剪为该值（数据类型：float，默认值：80.0，单位：米）。sigma：扫描匹配过程中cell的标准差（数据类型：float，默认值：0.05）。kernelSize：扫描匹配过程的搜索窗口大小（数据类型：int，默认值：1）。lstep和astep分别是扫描匹配的初始距离步长和扫描匹配的初始角度步长（数据类型：float，默认值：0.05）。iterations：扫描匹配器的迭代次数（数据类型：int，默认值：5）。lsigma：扫描匹配过程中单个激光扫描束的标准差（数据类型：float,，默认值:0.075）。ogain：似然估计时使用的增益，用于平滑重采样效果（数据类型：默认值:：3.0）。lskip：每个n+1次扫描进行一次扫描匹配，取值为0时表示每次扫描之后都进行一次匹配(数据类型：int,默认值：0)。srr：位置的噪声项（数据类型：float，默认值：0.1）。srt：方位角的噪声项（数据类型：float，默认值：0.2）。str：位置到方位角的协方差项（数据类型：float，默认值：0.1）。stt：方位角到位置的协方差项（数据类型：float，默认值：0.2）。10.4.1使用gmapping构建地图机器人操作系统（ROS）linearUpdate：只有当机器人至少运动了linearUpdate的距离之后才进行一次新的测量（数据类型：float,默认值:1.0）angularUpdate：只有当机器人至少转动了angularUpdate的角度之后才进行一次新的测量（数据类型：float,默认值:0.5）temporalUpdate：如果上次扫描处理的时间早于更新时间（秒），则处理扫描。小于零的值将关闭基于时间的更新(数据类型：float,默认值：-1.0)。resampleThreshold：粒子重采样的阈值。只有当评价粒子相似度的指标Neff小于该阈值时才进行重采样，所以降低该值意味着提高重采样的频率(数据类型：float,默认值：0.5)。particles：滤波器中的粒子数，粒子数越多，定位精度越高，计算代价越大（数据类型：int，默认值：30）。xmin、ymin、xmax和ymax分别是初始地图大小中X的最小值（数据类型：float，默认值：-100.0）、Y的最小值（数据类型：float，默认值：-100.0）、X的最大值（数据类型：float，默认值：100.0）以及Y的最大值（数据类型：float，默认值：100.0）。delta：地图的分辨率（数据类型：float，默认值：0.05）。llsamplerange：似然估计的距离采样范围(数据类型：float，default:0.01)。llsamplestep似然估计的距离采样步长（数据类型：float,默认值：0.01）。lasamplerange：似然估计的旋转采样范围。（数据类型：float,默认值：0.005）。lasamplestep：似然估计的旋转采样步长（数据类型：float,默认值：0.005）。occ_thresh：占用概率阈值（数据类型：float，默认值：default:0.25）。10.4.1使用gmapping构建地图机器人操作系统（ROS）实验测试首先，将激光雷达接入工控机，通过以下命令查看设备是否正常接入：$ls/dev若出现ttyUSB*（*代表0-9中的某个数，本节中激光雷达的设备是dev/ttyUSB0），则激光雷达驱动成功。通过以下命令更改相应串口权限，允许串口进行数据读写：$sudochmod777/dev/ttyUSB0运行如下命令启动SLAM节点，并移动实验小车进行地图构建，实验结果如图。$roslaunchlaser_scan_matcherdemo_gmapping.launch实验结果10.4.1使用gmapping构建地图机器人操作系统（ROS）在工作空间目录下创建一个map文件夹，通过如下命令保存当前构建的地图，保存后的地图如图所示：$rosrunmap_servermap_saver-f~/catkin_ws/map/mymap保存地图若未安装map_server，可运行如下命令安装功能包：$sudo

apt-get

install

ros-melodic-map-server保存后的地图10.4.1使用gmapping构建地图机器人操作系统（ROS）10.4.2基于地图的定位与自主导航介绍导航框架（1）amclamcl的英文全称是adaptiveMonteCarlolocalization，是在蒙特卡洛定位的基础上，使用自适应的KLD（kullback-leiblerdivergence）方法来更新粒子。蒙特卡洛定位使用粒子滤波的方法进行定位，粒子滤波用通俗的话说就是一开始在地图空间均匀的撒一把粒子，然后通过获取机器人的移动位姿来移动粒子，比如机器人向前移动了一米，所有的粒子也就向前移动一米，不管现在这个粒子的位置对不对，使用每个粒子所处位置模拟一个传感器信息与观察到的传感器信息（一般是激光信息）作对比，从而赋给每个粒子一个权重。之后根据生成的权重来重新生成粒子，权重越高的生成的概率越大。这样的迭代之后，所有的粒子会慢慢地收敛到一起，机器人在地图上的确切位置也就被推算出来了。（2）move_basemove_base功能包的作用是将全局路径规划和局部路径规划结合，从而使机器人完成基于地图的导航任务中的最优路径规划，全局路径规划用于生成地图上机器人的起始点到设置的目标点的路径，局部路径规划用于生成到近距离目标和为了临时躲避障碍物的路径。机器人操作系统（ROS）ROS中的导航框架10.4.2基于地图的定位与自主导航机器人操作系统（ROS）在机器人的导航任务中，首先，机器人需要发布必要的传感器话题（sensortopic），消息类型为sensor_msgs/LaserScan或sensor_msgs/PointCloud，导航目标位置信息（move_base_simle/goal），消息类型为geometry_msgs/PoseStamped。其次，要求机器人发布里程计信息及相应的TF变换。导航功能包用tf功能包来确定机器人在世界坐标系中的位置和相对于静态地图的相关传感器信息，但是tf功能包不提供与机器人速度相关的任何信息，所以导航功能包要求里程计源程序发布一个变换和一个包含速度信息的nav_msgs/Odometry消息。最后，该导航功能包输出控制机器人移动的指令（cmd_vel），并通过geometry_msgs/Twist类型的消息来和底层驱动板通信，从而控制电机运转，使机器人完成相应的移动。ROS中的导航框架10.4.2基于地图的定位与自主导航机器人操作系统（ROS）在导航框架中，机器人的路径规划包括全局路径规划（globalplanner）和局部实时规划（localplanner）。前者根据给定的目标位置进行总体路径规划，后者根据所在位置附近的障碍物进行躲避规划。全局路径规划器使用了A*算法，A*算法是一种高效的路径搜索算法，采用启发函数来估计地图上机器人当前的位置到目标位置之间的距离，并以此选择最优的方向进行搜索，如果失败会选择其他路径继续搜索直到得到最优路径。局部路径实时规划是利用base_local_plann包实现的，该包使用DWA（DynamicWindowapproaches，规划推理和动态窗口）算法，计算机器人每个周期内应该行驶的速度和角度（dx，dy，dthetavelocities）。DWA算法中先离散采样机器人控制空间（dx,dy,dtheta），再对于每个采样速度，从机器人当前的状态，进行模拟预测。ROS中的导航框架10.4.2基于地图的定位与自主导航机器人操作系统（ROS）（3）costmap_2d代价地图（costmap）是机器人收集传感器信息建立和更新的二维或三维地图。在move_base的框架下，costmap_2d为全局路径规划和局部路径规划提供了2D的代价地图。costmap_2d使用的是占用栅格地图，通过多个图层描述环境信息。每个图层描述了一种类型的信息，最终的代价是这些图层叠加的结果。比如说静态地图层(staticmaplayer)描述的是导航的地图信息，障碍物层(obstaclelayer)则记录了环境中的障碍物，膨胀层(inflationlayer)根据用户指定的参数和机器人的尺寸将障碍物的占用栅格区域放大一部分，以防止碰撞。根据导航类型，代价地图又被分成两部分。一个是全局地图（global_costmap），在全局移动路径规划中以整个区域为对象建立移动计划。而另一个被称为局部地图（local_costmap），这是在局部移动路径规划中，在以机器人为中心的部分限定区域中规划移动路径时，或在躲避障碍物时用到的地图。然而，尽管两种地图的目的不同，但表示方法是相同的。costmap用0到255之间的值来表示。简单地说，根据该值可以知道机器人是位于可移动区域还是位于可能与障碍物碰撞的区域。000：机器人可以自由移动的freearea（自由区域）001~127：碰撞概率低的区域128~252：碰撞概率高的区域253~254：碰撞区域255：机器人不能移动的占用区域（occupiedarea）10.4.2基于地图的定位与自主导航机器人操作系统（ROS）障碍距离与costmap值的关系10.4.2基于地图的定位与自主导航机器人操作系统（ROS）10.4.3安装步骤导航框架中包含amcl、move_base等很多功能包，可以通过源码安装，也可以使用如下命令安装：$sudoapt-getinstallros-melodic-navigation用源码安装的方法如下，在工作空间的src文件夹下克隆源码：$gitclone/ros-planning/navigation进入navigation功能包查看版本：$cdnavigation

$gitbranch选择melodic版本的navigation：$gitcheckoutmelodic-devel回到工作空间下进行编译：$cdcatkin_ws/

$catkin_make若编译出现如图所示的问题，说明缺少相应功能包，运行如下命令安装相应功能包：$sudoapt-getinstallros-melodic-tf2-sensor-msgs编译报错机器人操作系统（ROS）10.4.4参数配置文件讲解代价地图配置障碍物信息通过两种代价地图储存：一种是global_costmap，用于全局路径规划；一种是local_costmap，用于本地路径规划和实时避障。两种代价地图的正常使用需要三个配置文件，分别是通用配置文件（CommonConfiguration）、全局规划配置文件（GlobalConfiguration）和局部规划配置文件（LocalConfiguration）。1.通用配置文件obstacle_range:2.5#设置地图中检测障碍物的最大范围（m）。raytrace_range:3.0#设置机器人检测自由空间的最大范围（m）。footprint:[[0.165,0.165],[-0.165,0.165],[-0.165,-0.165],[0.165,-0.165]]#设置机器人在地图上的占用面积，以机器人的中心作为原点。若机器人外形为圆形，则设置robot_radius（圆形半径）。这里我们设置机器人外形为矩形。#robot_radius:0.165inflation_radius:0.1#机器人的膨胀参数（m），参数为0.1表示机器人规划的路径应与障碍物保持大于0.1m的安全距离。max_obstacle_height:0.6#障碍物的最大高度（m）。min_obstacle_height:0.0#障碍物的最小高度（m）。observation_sources:scan#代价地图需要关注的传感器信息scan:{data_type:LaserScan,topic:/scan,marking:true,clearing:true,expected_update_rate:0}#分别为传感器的消息类型、话题、是否使用传感器的实时信息来添加或清除代价地图的障碍物信息以及根据传感器实际发布的速率为每个观测源设置预期更新速率，当传感器低于预期速率时，会在终端中发出警告。地图的更新，来源于机器人发布的传感器消息，代价地图储存由传感器获取的障碍物信息。配置文件名为costmap_common_params.yaml，文件内容与解释如下：机器人操作系统（ROS）全局规划配置文件用于全局代价地图参数的配置，配置文件名为global_costmap_params.yaml,文件内容及解释如下：2.全局规划配置文件global_costmap:global_frame:/map#表示全局代价地图在哪个坐标系下运行，这里选择map参考系。robot_base_frame:/base_footprint#表示全局地图参考的机器人坐标系。update_frequency:1.0#设置全局地图信息更新的频率（HZ）。publish_frequency:1.0#设置全局地图信息发布的频率（HZ）。static_map:true#用来决定代价地图是否需要根据map_server提供的地图信息进行初始化，若不需要已知地图或map_server，将该参数设为false。rolling_window:false#用来设置机器人移动过程中是否需要滚动窗口来保持机器人处于中心位置。resolution:0.01#设置地图分辨率（米/格）。transform_tolerance:1.0#TF变换的容忍误差。map_type:costmap#地图类型，代价地图。代价地图配置10.4.4参数配置文件讲解机器人操作系统（ROS）局部规划配置文件用于局部代价地图参数的配置，配置文件名为local_costmap_params.yaml，文件内容及解释如下：3.局部规划配置文件local_costmap:#参数含义与全局规划配置文件中的相同

global_frame:/odomrobot_base_frame:/base_footprintupdate_frequency:3.0publish_frequency:1.0static_map:falserolling_window:truewidth:6.0#设置代价地图的长（米）。

height:6.0#设置代价地图的高（米）。

resolution:0.01transform_tolerance:1.0map_type:costmap代价地图配置10.4.4参数配置文件讲解机器人操作系统（ROS）局部规划器（base_local_planner）配置局部规划器的作用是，根据已经规划好的全局路径计算发布给机器人的速度控制指令。配置文件名称为base_local_planner_params.yaml，文件内容与解释如下：controller_frequency:3.0#设置向底盘控制移动话题cmd_vel发送命令的频率。recovery_behavior_enabled:false#是否启用move_base修复机制来清理出空间。clearing_rotation_allowed:false#决定做清理空间操作时候，机器人是否会采用原地旋转。TrajectoryPlannerROS:max_vel_x:0.3#机器人的最大线速度，单位是m/s。

min_vel_x:0.05#机器人的最小线速度，单位是m/s。

max_vel_y:0.0#差速机器人的零位控制。

min_vel_y:0.0#差速机器人的零位控制。

min_in_place_vel_theta:0.5#机器人最小的原地旋转速度，单位是弧度/秒。

escape_vel:-0.1#机器人逃离时的速度，单位是m/s。这个值必须是负数，这样机器人才能反向移动。

acc_lim_x:2.5#在x方向上的最大线加速度。

acc_lim_y:0.0#在y方向上的最大线加速度。

acc_lim_theta:3.2#在z方向上的最大旋转角。10.4.4参数配置文件讲解机器人操作系统（ROS）holonomic_robot:false#全方向驱动机器人设置为true，其他设为false。

yaw_goal_tolerance:0.1#最大距离目标方向的误差（单位为弧度）。

xy_goal_tolerance:0.1#最多距离目标位置的误差（单位为米）。

latch_xy_goal_tolerance:false#一般为false，若设置为true，则当进入xy_goal_tolerance范围内后会设置一个锁，此后即使在旋转调整yaw的过程中跳出xy_goal_tolerance，也不会进行xy上的调整。

pdist_scale:0.9#path的权重，权重越大越靠近全局路径。

gdist_scale:0.6#goal的权重，权重越大越靠近全局目标。

meter_scoring:true#计算系数时统一参数的单位为米，一般都是true，false时单位为cells。

heading_lookahead:0.325#对不同的旋转角，最多向前看几米。

heading_scoring:false#通过机器人航向计算距离还是通过路径计算距离，false为通过路径计算。

heading_scoring_timestep:0.8#对不同的轨迹，每次前向仿真时间步长。

occdist_scale:0.1#权衡机器人以多大的权重躲避障碍物，该值过大会导致机器人陷入困境。

oscillation_reset_dist:0.05#表示机器人运动多远距离才会重置振荡标记。局部规划器（base_local_planner）配置10.4.4参数配置文件讲解机器人操作系统（ROS）publish_cost_grid_pc:false#将代价值进行可视化显示,如果设置为true，那么就会在~/cost_cloud话题上发sensor_msgs/PointCloud2类型消息。

prune_plan:true#机器人前进是是否清除身后1m外的轨迹。

sim_time:1.0#前向模拟轨迹的时间，单位为s。

sim_granularity:0.025#轨迹点之间的步长，越短频率越高，要求的计算机的性能要好。

angular_sim_granularity:0.025#给定角度轨迹的弧长。

vx_samples:8#x方向速度的样本数。

vy_samples:0#差速轮机器人无Y方向速度，取0。

vtheta_samples:20#角速度的样本数。

dwa:true#是否选择DWA算法，false的话会选择TrajectoryRollout。局部规划器（base_local_planner）配置10.4.4参数配置文件讲解机器人操作系统（ROS