ROS机器人自主导航详解

序言

机器人在完成建图后即可在建立好的地图中进行导航，在ROS机器人中，导航使用到Navigation功能包集。Navigation包中最重要的就是Amcl与Move_base两个核心节点，本篇将详细讲解这两个核心节点及其参数。

Navigation导航原理

一. 基本概述

ROS机器人导航功能的实现主要是依靠 navigation 功能包集来完成的，navigation 是 2D 的导航包集，它通过接收里程计数据、tf 坐标变换树以及传感器数据，为移动机器人输出目标位置以及安全速度。

二. 导航逻辑图

三. 功能分析

导航功能的核心是 move_base 节点，它接收来自里程计消息、机器人姿态位置、地图数据等信息，在节点内会进行全局规划以及局部规划。其中局部规划是为了能在导航过程中随时根据环境的改变来改变自己的路径，达到自动避障的效果。

导航功能的实现首先要有的三个因素就是地图、导航的起点、终点目标，并在导航过程中不断根据里程计、激光雷达等传感器数据来确定自己的位置。在 navigation 导航功能中，首先会根据代价地图规划处起点到终点的路线，然后结合里程计信息以及激光雷达的数据判断当前位置并规划处当前位置附近的局部路线以达到避障的效果。最终将局部规划的路线以速度指令的形式输出。

我们来分析一下 navigation 这个 launch 文件：

可以看到启动了三个核心节点

• Map_server：加载地图到地图服务器
• Amcl：基于地图和激光雷达，里程计实现概率定位
• Move_base：导航与路径规划节点

接下来详细讲解一下Amcl与Move_base的功能参数。

Amcl自主定位

一. 基本概述

Amcl 是基于现有地图与激光雷达实现概率自定位的节点，用于确认并输出机器人在当前地图中的位置。

二. Amcl订阅与发布

Amcl 订阅的话题：
/scan激光雷达输出的话题，在进行 amcl 定位时，这个是必不可少需要订阅的话题，因为我们要知道目标周围的环境状态。/tf订阅了各坐标系转换的话题，用于查询各坐标系的转换。/initialpose用于（重新）初始化粒子滤波器的平均值和协方差，简单来理解就是先预估计一下机器人的初始位姿。/amcl/map当在launch文件中设置了use_map_topic 为true 时，amcl 则订阅该话题获取地图，然后使用基于激光来进行定位，当然设置use_map_topic 为 false 时不订阅该话题也是可以的。
Amcl 发布的话题：
/amcl_pose机器人在地图上带有协方差的位姿估计， 这个是话题是整个粒子滤波定位的最终输出结果，该话题输出的位姿信息是根据全局坐标系/map 的坐标转换后的位置。/particlecloud在粒子滤波器维护下的一组粒子位姿估计，可以直接在 rviz 中显示，查看粒子的收敛效果。/tf发布从 odom 坐标系到 map 坐标系的转换，当然该 odom 坐标系可以使用odom_frame_id 参数来重新映射为自定义的坐标系名称。

三. Amcl可调整的参数

滤波器可以设定的参数：
min_particles (int, default: 100)：滤波器中的最少粒子数，值越大定位效果越好，但是相应的会增加主控平台的计算资源消耗。

max_particles (int, default: 5000)：滤波器中最多粒子数，是一个上限值，因为太多的粒子数会导致系统资源消耗过多。

kld_err (double, default: 0.01)：真实分布与估计分布之间的最大误差。

kld_z (double, default: 0.99)：上标准分位数（1-p），其中 p 是估计分布上误差小于 kld_err 的概率，默认 0.99。

update_min_d (double, default: 0.2 meters)：在执行滤波更新前平移运动的距离，默认 0.2m(对于里程计模型有影响，模型中根据运动和地图求最终位姿的似然时丢弃了路径中的相关所有信息，已知的只有最终位姿，为了规避不合理的穿过障碍物后的非零似然，这个值建议不大于机器人半径，否则因更新频率的不同可能产生完全不同的结果)。

update_min_a (double, default: π/6.0 radians)：执行滤波更新前旋转的角度。

resample_interval(int, default: 2)：在重采样前需要滤波更新的次数。

transform_tolerance (double, default: 0.1 seconds)：tf 变换发布推迟的时间，为了说明 tf 变换在未来时间内是可用的。

recovery_alpha_slow(double, default: 0.0 (disabled))：慢速的平均权重滤波的指数衰减频率，用作决定什么时候通过增加随机位姿来 recover，默认 0（disable），可能 0.001 是一个不错的值。

recovery_alpha_fast(double, default: 0.0 (disabled))：快速的平均权重滤波的指数衰减频率，用作决定什么时候通过增加随机位姿来 recover，默认 0（disable），可能 0.1 是个不错的值。

initial_pose_x (double, default: 0.0 meters)：初始位姿均值（x），用于初始化高斯分布滤波器。（initial_pose_参数决定撒出去的初始位姿粒子集范围中心）。

initial_pose_y(double, default: 0.0 meters)：初始位姿均值（y），用于初始化高斯分布滤波器。（同上）

initial_pose_a (double, default: 0.0 radians)：初始位姿均值（yaw），用于初始化高斯分布滤波器。（粒子朝向）

initial_cov_xx(double, default: 0.50.5 meters)：初始位姿协方差（xx），用于初始化高斯分布滤波器。（initial_cov_参数决定初始粒子集的范围）

initial_cov_yy (double, default: 0.50.5 meters)：初始位姿协方差（yy），用于初始化高斯分布滤波器。（同上）

initial_cov_aa(double, default: (π/12)(π/12) radian)：初始位姿协方差（yawyaw），用于初始化高斯分布滤波器。（粒子朝向的偏差）

gui_publish_rate (double, default: -1.0 Hz)：扫描和路径发布到可视化软件的最大频率，设置参数为-1.0 意为失能此功能，默认-1.0。

save_pose_rate (double, default: 0.5 Hz)：存储上一次估计的位姿和协方差到参数服务器的最大速率。被保存的位姿将会用在连续的运动上来初始化滤波器。-1.0 失能。

use_map_topic (bool, default: false)：当设置为 true 时，AMCL 将会订阅 map 话题，而不是调用服务返回地图。也就是说当设置为 true 时，有另外一个节点实时的发布 map 话题，也就是机器人在实时的进行地图构建，并供给 amcl 话题使用；当设置为 false 时，通过 map server，也就是调用已经构建完成的地图。

first_map_only (bool, default: false)：当设置为 true 时，AMCL 将仅仅使用订阅的第一个地图，而不是每次接收到新的时更新为一个新的地图。

可以设置的所有激光模型参数：

laser_min_range(double, default: -1.0)：最小扫描范围，参数设置为-1.0 时，将会使用激光上报的最小扫描范围。

laser_max_range(double, default: -1.0)：最大扫描范围，参数设置为-1.0 时，将会使用激光上报的最大扫描范围。

laser_max_beams (int, default: 30)：更新滤波器时，每次扫描中多少个等间距的光束被使用（减小计算量，测距扫描中相邻波束往往不是独立的可以减小噪声影响，太小也会造成信息量少定位不准）。

laser_z_hit(double, default: 0.95)：模型的 z_hit 部分的混合权值，默认 0.95(混合权重 1.具有局部测量噪声的正确范围–以测量距离近似真实距离为均值，其后 laser_sigma_hit 为标准偏差的高斯分布的权重)。

laser_z_short (double, default: 0.1)：模型的z_short 部分的混合权值，默认 0.1（混合权重 2.意外对象权重（类似于一元指数关于 y 轴对称 0～测量距离（非最大距离）的部分：–ηλe^(-λz)，其余部分为0，其中 η 为归一化参数，λ 为 laser_lambda_short,z 为 t 时刻的一个独立测量值（一个测距值，测距传感器一次测量通常产生一系列的测量值）），动态的环境，如人或移动物体）。

laser_z_max (double, default: 0.05)：模型的z_max 部分的混合权值，默认 0.05（混合权重 3.测量失败权重（最大距离时为 1，其余为 0），如声呐镜面反射，激光黑色吸光对象或强光下的测量，最典型的是超出最大距离）。

laser_z_rand(double, default: 0.05)：模型的 z_rand 部分的混合权值，默认 0.05（混合权重 4.随机测量权重–均匀分布（1 平均分布到 0～最大测量范围），完全无法解释的测量，如声呐的多次反射，传感器串扰）。

laser_sigma_hit(double, default: 0.2 meters)：被用在模型的z_hit 部分的高斯模型的标准差，默认0.2m。

laser_lambda_short (double, default: 0.1)：模型 z_short 部分的指数衰减参数，默认 0.1（根据ηλe^(-λz)，λ 越大随距离增大意外对象概率衰减越快）。

laser_likelihood_max_dist(double, default: 2.0 meters)：地图上做障碍物膨胀的最大距离，用作likelihood_field 模（likelihood_field_range_finder_model 只描述了最近障碍物的距离，（目前理解应该是在这个距离内的障碍物膨胀处理,但是算法里又没有提到膨胀，不明确是什么意思）.这里算法用到上面的laser_sigma_hit。似然域计算测量概率的算法是将 t 时刻的各个测量（舍去达到最大测量范围的测量值） 的概率相乘，单个测量概率：Zh * prob(dist,σ) +avg，Zh 为 laser_z_hit,avg 为均匀分布概率，dist 最近障碍物的距离，prob 为 0 为中心标准方差为 σ（laser_sigma_hit）的高斯分布的距离概率。

laser_model_type(string, default: “likelihood_field”)：激光模型类型定义，可以是 beam, likehood_field, likehood_field_prob（和 likehood_field 一样但是融合了beamskip 特征–官网的注释），默认是"likehood_field" 。

可以设置的里程计模型参数：

odom_model_type (string, default: “diff”)：odom 模型定义，可以是"diff", “omni”, “diff-corrected”, “omni-corrected”，后面两个是对老版本里程计模型的矫正，相应的里程计参数需要做一定的减小。

odom_alpha1 (double, default: 0.2)：指定由机器人运动部分的旋转分量估计的里程计旋转的期望噪声，默认 0.2（旋转存在旋转噪声）。

odom_alpha2 (double, default: 0.2)：机器人运动部分的平移分量估计的里程计旋转的期望噪声，默认 0.2（旋转中可能出现平移噪声）。

odom_alpha3(double, default: 0.2)：机器人运动部分的平移分量估计的里程计平移的期望噪声，如(double, default: 0.2)：机器人运动部分的平移分量估计的里程计平移的期望噪声，如果你自认为自己机器人的里程计信息比较准确那么就可以将该值设置的很小。

odom_alpha4 (double, default: 0.2)：机器人运动部分的旋转分量估计的里程计平移的期望噪声，你设置的这 4 个 alpha 值越大说明里程计的误差越大。

odom_alpha5 (double, default: 0.2)：平移相关的噪声参数（仅用于模型是"omni"的情况，就是当你的机器人是全向移动时才需要设置该参数，否则就设置其为 0.0）

odom_frame_id (string, default: “odom”)：里程计默认使用的坐标系。

base_frame_id(string, default: “base_link”)：机器人的基坐标系。

global_frame_id(string, default: “map”)：由定位系统发布的坐标系名称。

tf_broadcast (bool, default: true)：设置为 false 阻止 amcl 发布全局坐标系和里程计坐标系之间的 tf变换。

另外必须要注意的是，amcl 发布的 tf 变换是 map 到 odom 的变换，用于修正轮子打滑等问题导致的odom 不准确。

Move_base功能使用

一. 基本概述

在机器人进行路径规划时，我们需要明白规划算法是依靠什么在地图上来计算出来一条路径的。依靠的是 SLAM建图算法（例如gmapping）扫描构建的一张环境全局地图，但是仅仅依靠一张原始的全局地图是不行的。因为这张地图是静态的，无法随时来更新地图上的障碍物信息。在现实环境中，总会有各种无法预料到的新障碍物出现在当前地图中，或者旧的障碍物现在已经从环境地图中被移除掉了，那么我们就需要来随时更新这张地图。同时由于默认的地图是一张黑白灰三色地图，即只会标出障碍物区域、自由移动区域和未被探索区域。机器人在这样的地图中进行路径规划，会导致规划的路径不够安全，因为我们的机器人在移动时需要与障碍物之间保持一定的安全缓冲距离，这样机器人在当前地图中移动时就更安全了。

二. Costmap代价地图

costmap 简单来说就是为了在这张地图上进行各种加工，方便我们后面进行路径规划而存在的，通过使用 costmap_2d 这个软件包来实现的，该软件包在原始地图上实现了两张新的地图。一个是 local_costmap， 另外一个就是 global_costmap，根据名字大家就可以知道了，两张 costmap 一个是为局部路径规划准备的，一个是为全局路径规划准备的。无论是 local_costmap 还是 global_costmap，都可以配置多个图层， 包括下面几种：

• Static Map Layer：静态地图层，基本上不变的地图层，通常都是 SLAM 建立完成的静态地图。

• Obstacle Map Layer：障碍地图层，用于动态的记录传感器感知到的障碍物信息。

• Inflation Layer：膨胀层，在以上两层地图上进行膨胀（向外扩张），以避免机器人的撞上障碍物。

• Other Layers：你还可以通过插件的形式自己实现 costmap，目前已有 Social Costmap Layer、Range Sensor Layer 等开源插件。

在启动 move_base 节点时， 首先会加载 costmap_common_params.yaml 到 global_costmap 和local_costmap 两个命名空间中，因为该配置文件是一个通用的代价地图配置参数，即 local_costmap 和 global_costmap 都需要配置的参数。然后 local_costmap_params.yaml 是专门为了局部代价地图配置的参数，global_costmap_params.yaml 是专门为全局代价地图配置的参数。

三. costmap_common_params.yaml

costmap_common_params.yaml 有以下参数：

robot_radius：设置机器人的半径，单位是米。如果机器人是圆形的，就可以直接设置该参数。如果机器人不是圆形的那就需要使用 footprint 这个参数，该参数是一个列表，其中的每一个坐标代表机器人上的一点，设置机器人的中心为[0,0]，根据机器人不同的形状，找到机器人各凸出的坐标点即可，具体可参考下图来设置：

obstacle_layer:配置障碍物图层

enabled:是否启用该层

combination_method:只能设置为0或1，用来更新地图上的代价值，一般设置为 1;

track_unknown_space:如果设置为 false，那么地图上代价值就只分为致命碰撞和自由区域两种，如果设置为 true，那么就分为致命碰撞，自由区域和未知区域三种。意思是说假如该参数设置为true 的话，就意味着地图上的未知区域也会被认为是可以自由移动的区域，这样在进行全局路径规划时，可以把一些未探索的未知区域也来参与到路径规划，如果你需要这样的话就将该参数设置为 false。不过一般情况未探索的区域不应该当作可以自由移动的区域，因此一般将该参数设置为 true;

obstacle_range:设置机器人检测障碍物的最大范围，意思是说超过该范围的障碍物，并不进行检测，只有靠近到该范围内才把该障碍物当作影响路径规划和移动的障碍物;

raytrace_range:在机器人移动过程中，实时清除代价地图上的障碍物的最大范围，更新可自由移动的空间数据。假如设置该值为 3 米，那么就意味着在 3 米内的障碍物，本来开始时是有的， 但是本次检测却没有了，那么就需要在代价地图上来更新，将旧障碍物的空间标记为可以自由移动的空间。

observation_sources:设置导航中所使用的传感器，这里可以用逗号形式来区分开很多个传感器， 例如激光雷达，碰撞传感器，超声波传感器等，我们这里只设置了激光雷达;

laser_scan_sensor:添加的激光雷达传感器

sensor_frame:激光雷达传感器的坐标系名称;

data_type:激光雷达数据类型;

topic:该激光雷达发布的话题名;

marking:是否可以使用该传感器来标记障碍物;

clearing:是否可以使用该传感器来清除障碍物标记为自由空间;

inflation_layer:膨胀层，用于在障碍物外标记一层危险区域，在路径规划时需要避开该危险区域

enabled:是否启用该层;

cost_scaling_factor:膨胀过程中应用到代价值的比例因子，代价地图中到实际障碍物距离在内切圆半径到膨胀半径之间的所有 cell 可以使用如下公式来计算膨胀代价：

exp(-1.0cost_scaling_factor(distance_from_obstacle-inscribed_radius))* (costmap_2d::INSCRIBED_INFLATED_OBSTACLE-1)

公式中 costmap_2d::INSCRIBED_INFLATED_OBSTACLE 目前指定为 254，注意： 由于在公式中cost_scaling_factor 被乘了一个负数，所以增大比例因子反而会降低代价。

inflation_radius:膨胀半径，膨胀层会把障碍物代价膨胀直到该半径为止，一般将该值设置为机器人底盘的直径大小。如果机器人经常撞到障碍物就需要增大该值，若经常无法通过狭窄地方就减小该值。

Static_layer:静态地图层，即 SLAM 中构建的地图层 。

四. global_costmap_params.yaml

global_costmap_params.yaml有以下参数：

参数含义如下：
global_costmap_params.yaml 参数含义如下：

global_frame:全局代价地图需要在哪个坐标系下运行;

robot_base_frame:在全局代价地图中机器人本体的基坐标系，就是机器人上的根坐标系。通过global_frame 和 robot_base_frame 就可以计算两个坐标系之间的变换，得知机器人在全局坐标系中的坐标了。

update_frequency:全局代价地图更新频率，一般全局代价地图更新频率设置的比较小;

static_map:配置是否使用 map_server 提供的地图来初始化，一般全局地图都是静态的，需要设置为true;

rolling_window:是否在机器人移动过程中需要滚动窗口，始终保持机器人在当前窗口中心位置;

transform_tolerance:坐标系间的转换可以忍受的最大延时;

plugins:在 global_costmap 中使用下面三个插件来融合三个不同图层，分别是 static_layer、obstacle_layer 和 inflation_layer，合成一个 master_layer 来进行全局路径规划。

五.local_costmap_params.yaml

local_costmap_params.yaml 有以下参数：

local_costmap_params.yaml 参数含义如下：

global_frame:在局部代价地图中的全局坐标系，一般需要设置为 odom

robot_base_frame:机器人本体的基坐标系;

update_frequency:局部代价地图的更新频率;

publish_frequency:局部代价地图的发布频率;

static_map:局部代价地图一般不设置为静态地图，因为需要检测是否在机器人附近有新增的动态障碍物;

rolling_window:使用滚动窗口，始终保持机器人在当前局部地图的中心位置;

width:滚动窗口的宽度，单位是米;

height:滚动窗口的高度，单位是米;

resolution:地图的分辨率，该分辨率可以从加载的地图相对应的配置文件中获取到;

transform_tolerance:局部代价地图中的坐标系之间转换的最大可忍受延时;

plugins:在局部代价地图中，不需要静态地图层，因为我们使用滚动窗口来不断的扫描障碍物，所以就需要融合两层地图（inflation_layer 和 obstacle_layer）即可，融合后的地图用于进行局部路径规划;

move_base 是导航功能的核心，在导航过程中会根据起点、目标点以及地图信息规划处全局路线，但有了全局路线还不够，为了避免与一些移动的障碍物发生碰撞比如行人，还需要对所处的一个局部环境进行局部路径规划，规划的路径要尽可能的服从全局路径，只是为了暂时性的避开障碍物。若配置路径规划的参数就要配置 move_base 相关的参数，在 move_base 中有多种路径规划器算法可选，我们需要告诉 move_base 路径规划器使用哪种算法。

六.发布的话题

以上程序修改完参数后输入 rostopic list 可以查看当前存在的话题，可以看到有很多 planner 和 costmap：

七.其他

move_base 是导航功能的核心，在导航过程中会根据起点、目标点以及地图信息规划处全局路线，但有了全局路线还不够，为了避免与一些移动的障碍物发生碰撞比如行人，还需要对所处的一个局部环境进行局部路径规划，规划的路径要尽可能的服从全局路径，只是为了暂时性的避开障碍物。若配置路径规划的参数就要配置 move_base 相关的参数，在 move_base 中有多种路径规划器算法可选，我们需要告诉 move_base 路径规划器使用哪种算法。

全局路径的规划插件包括：

• navfn:ROS 中比较旧的代码实现了 dijkstra 和 A*全局规划算法。
• global_planner:重新实现了 Dijkstra 和 A*全局规划算法,可以看作 navfn 的改进版。
• parrot_planner:一种简单的算法实现全局路径规划算法。

局部路径的规划插件包括：

• base_local_planner:实现了 Trajectory Rollout 和 DWA 两种局部规划算法。
• dwa_local_planner:实现了 DWA 局部规划算法，可以看作是 base_local_planner 的改进版本。
• teb_local_planner:应用较好的局部规划算法，路威机器人默认使用的此算法。