移动机器人的ROS导航堆栈技术手册

ROS 导航堆栈对于移动机器人从一个地方移动到另一个地方是强大的可靠。

导航堆栈的工作是通过处理来自测距、传感器和环境图的数据来产生让机器人执行的安全路径。

最大限度地提高此导航堆栈的性能需要对参数进行一些精细的调整，这项工作并不像看起来那么简单。

如果对概念和推理了解不清楚，可能会随意尝试，这将浪费大量的时间。

本文旨在引导读者正确微调导航参数。当在设置一些关键参数的时候，需要知道“如何”和“为什么”。

本指南假定读者有已经设置了导航堆栈并准备优化。这也是我总结在 ROS 导航堆栈的一个过程。

正文

1 速度和加速度

本节涉及同步驱动机器人。动力性（即速度和机器人的加速度）对于包括动态窗口方法（DWA）和定时弹性带（TEB）在内的本地规划器来说是必不可少的。

在 ROS 导航堆栈，本地规划器接收测距消息（“odom”主题）并输出速度命令（“cmd_vel”主题）来控制机器人的运动。

最大/最小速度和加速度是移动基站的两个基本参数正确设置它们对于最佳的本地计划行为非常有帮助。

在 ROS 中导航，我们需要知道平移和旋转的速度和加速度。

1.1 获得最大速度

通常可以参考你的移动基站的手册。例如，SCITOS G5 的最大速度是 1.4m/s。

在 ROS 中，你可以订阅 odom 主题来获取当前的测距信息。

如果你可以手动控制你的机器人（如通过操纵杆），你可以尝试向前运行，直到达到恒定速度，然后输出里程计数据。

平移速度（m/s）是机器人在直线移动时的速度。它的最大值就是上边我们获得的最大值。

旋转速度（rad/s）等效于角速度，它的最大值是机器人旋转到位时的速度。

为了获得最大角速度，我们可以通过控制操纵杆来让机器人旋转 360°，直到它的角速度达到恒定值。

为了安全起见，我们倾向于设定最大平移速度和旋转速度低于它们的实际值。

1.2 获得最大加速度

如果手册没有说明，有很多种方法可以获得移动基站的最大加速度。

在 ROS 中，我们可以输出带有时间戳的里程计数据，然后看机器人达到恒定的最大平移速度（ti）需要多长时间，然后使用来自里程计数据（nav_msgs/Odometry message）来计算这个过程的加速度。多做几次实验求平均值。

使用 tt、tr 分别来表征从静态到达最大平移速度和最大旋转速度的时间。

最大平移加速度约等于最大平移速度除以时间 tt，同样的，最大旋转加速度约等于最大旋转速度除以旋转时间 tr。

1.3 设置最小值

设置最小速度的方法与上述不同。对于最小平移速度，我们将它设置为一个大的负值，因为这将可以使机器人陷入困境中时可以后退，即使大多数情况下它都是前进的。

对于最小的旋转速度，如果参数允许，我们将其设置为负值，以便机器人可以在任意方向旋转。要注意，DWA 本地规划器采用的是机器人最小旋转速度的绝对值。

1.4 x、y 方向速度

x 方向的速度是指平行于机器人直线运动方向的速度，这与移动速度相同。

y 方向的速度垂直于直线运动，它被称之为“冲击速度”。对于非整体机器人（如差速轮机器人），y 速度应该设置为零。

2 全局路径规划

2.1 全局路径规划方法选择

为了使用导航包中的 move_base 节点，我们需要有全局规划器和局部规划器。在导航包中有三种全局规划器：carrot 规划器、navfn 规划器和 global 规划器。

2.1.1 carrot 规划器

这是最简单的一种规划器。它检查给定的目标点是否是障碍物，如果是，则通过沿机器人和目标点的向量来选择靠近原始目标的替代目标。

最终他将给局部规划器或内部控制器一个有效的目标点。因此，这种规划器没有任何全局路径规划。

如果你需要机器人靠近给定的目标，即使目标不可到达的情况下，这种方法是有效的。但在复杂的室内环境中，这种方法不是很实用。

2.1.2 navfn 和 global 规划器

navfn 使用 dijkstra 算法来在起点和终点之间寻找最小代价路线。global 规划器建立了更灵活的替代 navfn 的选择，这些选择包括：

（1）支持 A*算法（2）切换二次近似（3）切换网格路径。navfn 和 global 规划器都是基于这篇论文：

https://cs.stanford.edu/group/manips/publications/pdfs/Brock_1999_ICRA.pdf

2.2 全局路径规划参数

因为 global 规划器是一种更常使用的方法，我们来看它的一些关键参数。

注意：不是所有的参数都能在 ROS 网页上找到,但是你可以通过运行 rosrun rqt_reconfigure rqt_reconfigure 来查看。

我们可以设置这些默认值：allow unknown(true), use dijkstra(true), use quadrati c(true), use grid path(false), old navfn behavior(false) 。

如果我们想在 RVIZ 中查看势力图可以将 visualize_potential 值由 FALSE 设为 TRUE。

除了这些参数外， 还有三个没有列出来的参数会决定全局路径规划的性能。分别cost_factor, neutral_cost, lethal_cost。

事实上，这些参数在 navfn 算法中也提到了。这个开源代码详细的解释了 navfn 如何计算这些代价值。

https://github.com/rosplanning/navigation/blob/indigo-devel/navfn/include/navfn/navfn.h

navfn 代价值的计算方法如下：

cost = COST_NEUTRAL + COST_FACTOR * costmap_cost_value

传入的代价值设置在 0 到 252 的范围，进一步分析：

COST_NEUTRAL 为 50 时，COST_FACTOR 需要约为 0.8，从而确保输入值的分布能使输出值在 50 到 253 间变化。

如果 COST_FACTOR 较高，cost 值将会在障碍物附近有一个峰值，然后规划器将视狭窄走廊的整个宽度为较差，将不会沿着中心规划路径。

实验观察：实验也证实了这些解释。将 cost_factor 设置过高或过低都会降低路径质量。这些路径没有穿过每侧障碍物的中间，也没有相对光滑的曲率。

极端的 COST _NEUTRAL 值也有类似的效果。对于关键的 cost 值，如果设置的过低可能会无法产生任何路径，即使可行路径是明显的。

图 5-10 显示了 COST_FACTOR 和 COS T_NEUTRAL 对全局路径规划的影响。绿色的线是全局规划器产生的全局路径。经过几次实验.

我们发现当 COST_FACTOR=0.55，COST_NEUTRAL=66，cost=253 时，全局路径是很好的。

3 局部路径规划

局部规划器包含 dwa 局部规划器、eband 局部规划器和 teb 局部规划器。

它们使用不同的算法来产生速度指令。通常 dwa 规划器使用的较多，我们将详细讨论这种算法。其它规划器的信息稍后将会提供。

3.1 DWA 本地规划器

3.1.1 DWA 算法

dwa 局部规划器采用动态窗口方法，ROS 维基上提供了这种算法执行过程的介绍：

1.将机器人的控制空间离散化（dx,dy,dtheta）

2. 对于每一个采样速度，从机器人的当前状态执行正向模拟，以预测如果在短时间段内采用采样速度将会发生什么

3. 评估从正向模拟产生的每个轨迹使用包含诸如：障碍物接近度、目标接近度、全局路径接近度和速度等特征的度量，丢弃非法轨迹（与障碍物相撞的轨迹）

4. 选择得分最高的轨迹，将相关联的速度发送给移动基站

5. 清零然后重复以上过程DWA 算法是由 Dieter Fox 的论文：

https://www.ri.cmu.edu/pub_files/pub1/fox_diet er_1997_1/fox_dieter_1997_1.pdf提出的。

根据这篇论文的介绍，DWA 算法的目的是生成一个动作对（v，w），它代表了机器人最佳的圆形轨迹。

DWA 通过在下一个时间间隔内搜索速度空间来达到此目的。

这个空间的速度被限制为可以接受的，这意味着机器人必须能够在到达这些可接受速度所规定的圆形轨迹上的最接近的障碍物之前停止。

此外，DWA 将仅考虑动态窗口内的速度，其被定义为给定当前平移和旋转速度和加速度在下一时间间隔内可到达的速度对集合。

现在，我们来看 ROS Wiki 的算法总结。第一步是在动态窗口内的速度空间中的采样速度对（vx,vy,w）。

第二步是消除不可接受的速度（即消除不良轨迹）。

第三步是使用目标函数来评估速度对，输出轨迹得分。第四和第五步很容易了解：采取当前最佳速度选项并重新计算。

这个 DWA 规划器取决于提供障碍物信息的本地成本图。 

因此，调整本地成本图的参数对于 DWA 本地规划的最佳行为至关重要。接下来，我们将参考前向仿真，轨迹评分，成本地图等参数。

3.1.2 DWA 本地规划器：前向模拟

前向模拟是 DWA 算法的第二步。

在这一步中，本地规划器将机器人控制空间中的速度采样，并检查由这些速度样本表示的圆形轨迹，并最终消除不良速度（轨迹与障碍物相交）。

在机器人的一段时间间隔内，每个速度样本由仿真时间控制及仿真。

我们可以将模拟时间视为允许机器人以采样速度移动的时间。

通过实验，我们观察到仿真时间越长，计算负荷越大。此外，当仿真时间变长后，本地路径规划器产生路径的时间也会变长，这是合理的。

这里有一些关于如何调整仿真时间参数的建议。

如何设置仿真时间：如果将仿真时间设置为非常低的值（≤2.0）将导致性能有限，特别是当机器人需要通过狭窄的门口或家具之间的间隙时.

因为没有足够的时间来获得最佳轨迹来通过狭窄的通道。另一方面，由于使用了 DWA 本地规划器，所有的轨迹都是简单的圆弧，如果将仿真时间设置的非常高（≥5.0），将导致长曲线不是非常灵活。

这个问题并不是不可避免的，因为规划器在每个时间间隔后都会积极地重新规划，可以进行小的调整。对于高性能的计算机，4.0 秒的值也是足够的。

除了仿真时间，还有几个参数值得关注。

速度采样：在其它几个参数中，vx_sample，vy_sample 确定在 x，y 方向上取多少平移速度样本。vth_sample 控制旋转速度样本的数量。

样本的数量取决于你的计算能力。在大多数情况下，我们倾向于设置 vth_sample 高于平移速度样本，因为通常旋转比直线前进更复杂。

如果将 y 向最大速度设置为零，则没必要在 y 方向提取速度样本，因为没有可用的样本。我们设置

vx_sample=20,vth_samples=40。

仿真粒度：sim_granularity 是在轨迹上的点之间采取的步长。它意味着要多频繁的检查轨迹上的点（检测它们是否与障碍物相交）。

较低的值意味着高频率，这需要更多的计算能力。对于 turtlebot 机器人来说，0.025 的默认值是足够的。

3.1.3 DWA 本地规划器：轨迹得分

如上所述，DWA 本地规划器最大化目标函数来获得最佳速度对。

在其论文中，目标函数的值依赖于三个组成部分：到目标点的过程、清除障碍物和前进速度。在 R OS 中，目标函数的计算公式如下：

cost = path_distance_bias*(distance(m) to path from the endpoint of the trajectory)+ goal_distance_bias (distance(m) to local goal from the endpoint of the trajectory)+ occdist_scale*(maximu m obstacle cost along the trajectory in obstacle cost (0-254))

目标是获得最小的代价。path_distance_bias 是本地规划器与全局路径保持一致的权重。较大的值将使本地规划器更倾向于跟踪全局路径。

goal_distance_bias 是机器人尝试到达目标点的权重。实验显示增加 goal_distance_bias 值将会使机器人与全局路径的一致性偏低。

occdist_scale 是机器人尝试躲避障碍物的权重，这个值偏大将导致机器人陷入困境。

在 SCITOS G5 上，我们设置 path_distance_bias 为 32，goal_distance_bias 为 20，occdist_sacle 为 0.02,仿真结果良好。

3.1.4 DWA 本地规划器：其他参数

目标距离公差（Goal distance tolerance）：

这些参数很容易理解，以下是他们在 ROS 维基上的描述：yaw_goal_tolerance（double,默认值：0.05），实现目标时，偏航/旋转中控制器弧度公差

xy_goal_tolerance（double,，默认值：0.10），实现目标时，在 x y 方向的距离公差

latch_xy_goal_tolerance（bool，默认：false）如果目标公差被锁定，即使在目标公差之前结束，如果机器人到达目标 xy 位置，它会简单旋转到位

振荡复位（Oscilation reset）：在通过门口的情况下，机器人可能会来回振荡，是因为本地规划器正在产生通过两个相反方向的路径。

如果机器人保持振荡，导航堆栈将让机器人尝试恢复行为。

oscillation_reset_dist（double，默认值：0.05）在振荡标志复位之前，机器人以米为单位行走多远

4 代价地图参数

如上所述，代价地图参数对于本地规划器（不仅仅是 DWA）是至关重要的。

在 RO S 中，代价地图由静态地图层、障碍物图层和膨胀层组成。

静态地图层直接给导航堆栈提供静态 SLAM 地图解释。障碍物图层包含 2D 障碍物和 3D 障碍物（体素层）。

膨胀层是将障碍物膨胀来计算每个 2D 代价地图单元的代价。

此外，有全局代价地图，也有局部代价地图。全局代价地图是通过膨胀导航堆栈上的地图障碍物来实现的。

局部代价地图是通过将机器人传感器检测到的障碍物膨胀产生的.

4.1 足迹

足迹是移动基站的轮廓。在 ROS 中，它由二维数组表示[x0,y0] ; [x1,y1] ; [x2,y2]……不需要重复第一个坐标。

该占位面积将用于计算内切圆和外接圆的半径，用于以适合此机器人的方式对障碍物进行膨胀。为了安全起见，我们通常将足迹稍大于机器人的实际轮廓。

要确定机器人的占地面积，最直接的方法是参考机器人的图纸。 

此外，您可以手动拍摄其基座顶视图。然后使用 CAD 软件（如 Solidworks）适当缩放图像，并将鼠标移动到基座轮廓上并读取其坐标。 

坐标的起点应该是机器人的中心。或者，您可以将机器人移动到一张大纸上，然后绘制基座的轮廓。

然后选择一些顶点并使用标尺来确定它们的坐标。

4.2 膨胀

膨胀层由代价值为 0-255 的单元组成。每个单元可能会被占据、无障碍或未知三种情况。下图介绍了膨胀值的计算方法。

inflation_radius 和 cost_scaling_factor 是决定膨胀的主要参数。

inflation_radius 控制零成本点距离障碍物有多远。

cost_scaling_factor 与单元的代价值成反比，设置高值将使衰减更陡峭。

Pronobis 博士建议，最佳的代价图衰减曲线是具有相对较低斜率的曲线，以便最佳路径尽可能远离每侧的障碍物。

优点是机器人可以在障碍物中间移动。如图14和图15所示，具有相同的起点和目标，当代价图曲线陡峭时，机器人往往靠近障碍物。

在图14中，膨胀半径=0.55，代价比例因子=5.0；在图15中，膨胀半径=1.75，代价比例因子=2.58

根据衰变曲线图，我们要设定这两个参数值，使得膨胀半径几乎覆盖走廊，代价值的衰减中等，这意味着要降低代价比例因子 cost_scaling_factor 的值

4.3 代价地图精度 costmap resolution

本参数可以分别设置本地代价地图和全局代价地图，它们影响计算负荷和路径规划能力。

在低分别率（≥0.05）的情况下，障碍物区域可能重叠，导致本地规划器无法找到可用路径。

对于全局代价地图精度，只要保持与提供给导航堆栈的地图的分辨率相同即可。

如果有足够的计算能力，可以查看激光扫描仪的分辨率，因为当使用 gmapping 建图时.

如果激光扫描仪的分辨率低于所需的地图分辨率，则会有很多小的“未知点”，因为激光扫描仪不能覆盖该区域，如图 16 所示。

例如，Hokuyo URG-04LX-UG01 激光扫描仪的分辨率是 0.01mm，因此扫描分辨率≤0.01 的地图将需要机器人旋转几次才能清除未知的点。我们发现 0.02 的精度就够用了。

4.4 障碍物层和体素层

这两层负责标注代价图上的障碍，他们可以被称为障碍层。根据 ROS 维基，障碍物层跟踪二维的，体素层跟踪三维的。

障碍物是根据机器人传感器的数据进行标记（检测）或清除（删除），其中需要订阅代价图的主题。

在 ROS 执行中，体素层从障碍物层继承，并且都是通过使用激光雷达发布的 Point Cloud 或 PointCloud2 类型的消息来获取障碍物信息。

此外，体素层需要深度传感器，如 Microsoft Kinect 或华硕 Xtion，3D 障碍物最终会被膨胀为二维代价图。

体素层如何工作：体素是空间中具有一定相对位置的 3D 立方体（类似于 3D 像素）。它可以用于与附近体积的数据或属性相关联。

例如，它的位置是够是一个障碍。与体素与深度相机相关的 3D 重建已经有很多研究了。

体素网格是一个 ROS 包，它提供了一个高效的三维体素网格数据结构的实现，它存储三种状态的体素：标记、自由、未知。

体素网格占据了代价地图区域内的体积。在每次更新体素边界期间，体素层根据传感器的数据来标记或去除体素网格中的一些体素。

它还执行光线跟踪，接下来会讨论。请注意，在更新时，不会重新创建体素网格，而仅仅在更改本地代价图的大小时才更新。

为什么要在障碍物层或体素层光线跟踪：光线跟踪为人所知是因为用于渲染逼真的 3D 图形，所以可能会困惑为什么被用于处理障碍物。

一个重要的原因是可以通过传感器来检测不同类型的障碍物。理论上，我们还可以知道障碍物是刚性的还是柔性的。

通过以上的理解，我们来研究一下障碍物层的一些参数，这些参数对于所有的传感器都是适用的。

max_obstacle_height：插入代价图中的障碍物的最大高度。该参数设置为稍高于机器人的高度，对于体素层，这基本上是体素网格的高度。

obstacle_range：障碍物距离机器人的最大距离，障碍物以米为单位掺入代价地图，并可以在每个传感器的基础上进行覆盖

raytrace_range：用于使用传感器数据在地图中扫描出障碍物，以米为单位，可以在每个传感器的基础上进行覆盖下面的这些参数仅适用于体素层

origin_z：地图的 Z 轴原点，以米为单位

z_resolution：地图 Z 轴精度

z_voxels：每个垂直列中的体素数，网格的高度是 Z 轴分辨率*Z 轴体素数

unknown_threshold：被认为是“已知”的列中允许的未知单元的数量

mark_threshold：在被认为是“自由”的列中允许的标记单元的最大数量

实验观察：实验进一步阐明了体素层数的影响。我们使用华硕 Xtion Pro 作为我们的深度传感器。

我们发现 Xtion 的位置很重要，它决定了“盲区的范围”，即深度传感器看不到的区域。

此外，当障碍物出现在 Xtion 范围内时，表示障碍物的体素会更新（标记或清除）。

否则，一些体素的信息仍保持不变，在代价地图中的膨胀信息也会保留。此外，Z 轴分辨率灰顶 Z 轴体素的密度。如果值很高，体素层会很密集。

如果值太低（例如 0.01），所有的体素将被放在一起，将不会获得有效的代价图信息。

如果将 Z 轴分辨率设置为较高的值，意图是更好地获得障碍物，因此需要增加 Z 轴体素数（该参数控制每个垂直列中的体素数）。

如果列中的体素数太多但分辨率不够也是没用的，因为每个垂直列的高度都有限制。图 18-20 显示了不同体素参数设置之间的比较。

5 AMCL

AMCL 是处理机器人定位的 ROS 包。它是自适应蒙特卡罗定位的缩写（AdaptiveMonte Carlo Localization），也被称为部分滤波定位器。

这种定位方法的原理如下：每个样本存储表示机器人姿态的位置和方向数据。

粒子是随机抽样的，当机器人移动时，粒子根据他们的状态记忆机器人的动作，采用递归贝叶斯估计进行重采样。稍后将提供 AMCL 参数调整的更多讨论。

请参考 ROS 维基 http://wiki.ros.org/amcl 了解更多信息。关于原始算法的细节，可以参考 Chapter 8 of Probabilistic Robot ics, by Thrun, Burgard, and Fox.

6 恢复行为

机器人导航的一个讨厌的事情就是机器人可能会卡住。幸运的是，导航堆栈具有内置的恢复行为。

即使如此，有时机器人会耗尽所有可用的恢复行为后保持静止。因此，我们需要一个更强大的解决方案。

恢复行为的类型：ROS 导航包有两种恢复行为，分别是清除代价地图恢复和旋转恢复。

清除代价地图恢复是将本地代价地图还原成全局代价地图的状态。旋转恢复是通过旋转 360°来恢复。

解救机器人：有时由于旋转故障，旋转恢复将无法执行。在这一点上，机器人可能会放弃，因为它已经尝试了所有的恢复行为。

在大多数试验中，我们观察到，当机器人放弃时，实际上有很多方法可以解救机器人。

为了避免放弃，我们使用 SMAC H 来连续尝试不同的恢复行为，通过其他额外的行为，例如设置非常接近机器人的临时目标，并返回到以前访问过得姿态（即退出）。

这些方法可以显著提高机器人的耐久性，并且从以前观察到的无望空间中解救出来。

参数：ROS 恢复行为的参数一般设为默认值。

为了清除代价地图恢复，你可以设置一个相对较高的模拟时间 sim_time，这意味着轨迹很长，你可能需要考虑增加 res et_distance 参数的值，这样可以消除本地代价地图上更大的区域，并且有更好的机会寻找一条路径。

7 动态重新配置

关于 ROS 导航最灵活的方面之一是动态重新配置，因为不同的参数设置可能对某些情况（如机器人靠近目标点时）更有帮助。

然后，没有必要进行大量的动态重新配置。

示例：我们在实验中观察到的一种情况是机器人常常区域脱离全局路径，即使没有必要这么做。

因此，我们增加路径距离偏差 path_distance_bias。

由于大的路径距离偏差值会使机器人遵循全局路径，但实际上由于公差不会最终到达目标点，我们需要找到一种方法来使机器人毫不犹豫的到达目标点。

因此我们选择动态减少路径距离偏差，以便在机器人靠近目标点时有一个小的值。毕竟，做更多的实验是为了找到解决问题的方法。

8 问题

8.1 陷入困境

在使用 ROS 导航的时候，这个问题经常出现，无论是在仿真还是实际中，机器人都可能陷入困境然后放弃目标

8.2 不同方向的不同速度

在导航堆栈中我们观察到一些奇怪的行为。当目标点设置在相对于 TF 原点的-x 方向时，dwa 局部规划器规划不稳定（局部规划路径跳跃），而且机器人的移动速度非常慢。

但是当把目标设置在+x 方向时，dwa 局部规划器就比较稳定了，并且移动速度很快。

8.3 实际和仿真

实际与仿真是有区别的。在现实情况中，障碍物有各种各样的形状。

例如，在实验室中有一个垂直的柜子，防止门闭上，由于太细，机器人有时无法检测到然后撞击上去。而且实际中也会有更多复杂的人类活动。

8.4 前后矛盾

使用 ROS 导航堆栈可能会出现不一致的行为。

例如进门时，在不同时间本地代价地图会一次又一次的生成，这可能会影响路径规划，特别是在分辨率较低的时候。

另外，机器人没有内存，它不记得上次从门进入房间，所以每次尝试进门都需要重新开始。

因此，如果没有与以前相同的进门角度，机器人可能会卡住并放弃目标。翻译自 ROS Navigation Tuning Guide（导航调试指南）

编辑：黄飞