基于ROS平台的导航机器人局部路径规划的研究与优化

摘  要：智能机器人的基础功能之一就是自主导航，包括了地图构建和路径规划。基于开源操作系统ROS的机器人，运用启发式搜索A*算法进行全局路径规划。对机器人局部路径规划进行了集中研究，采用DWA算法进行局部实时路径规划，加入基于弹性时间带碰撞TEB算法，优化机器人位置和时间间隔变量，通过加权多目标化获取最优的路径规划能力。实验结果证实了TEB算法的规划效果明显优于DWA算法，为移动机器人自动导航功能的优化提供了思路。

关键词：ROS;导航机器人;TEB算法;DWA算法;路径规划优化

中图分类号：TP242       文献标识码：A文章编号：2096-4706（2022）05-0144-05

Research and Optimization of Local Path Planning of Navigation Robot Based on ROS Platform

GUO Jiaying

（College of Electronic Science and Engineering， Jilin University， Changchun  130061， China）

Abstract： One of the basic functions of smart robots is autonomous navigation， including map construction and path planning. The robot based on ROS， the open source operating system， uses the heuristic search A * algorithm for global path planning. The paper researches centrally the local path planning of the robot， uses the DWA algorithm for local real-time path planning， adds the TEB algorithm baesd on the elastic time-band collision， optimizes robot positions and time interval variables， and obtains the optimal path planning capability by weighted multi-object. The experimental results confirmes that the planning effect of the TEB algorithm is significantly better than the DWA algorithm， and it provides ideas for the optimization of mobile robot automatic navigation function.

Keywords： ROS; navigation robot; TEB algorithm; DWA algorithm; path planning optimization

0  引  言

從机器人软件系统（Robot Operating System， ROS）在2007年问世，到现在已经经历了14年，凭借着较低的技术壁垒和便捷的传播因素，ROS现已成为全球主流的机器人操作系统。通过ROS这个平台，不断地有创新的知识和技术运用到机器人中，加快了人工智能的研发。而自动导航作为机器人基础的功能之一，广泛运用于各个应用层面，因此必须对其进行不断地加强和完善。路径规划，包涵全局和局部路径规划，作为其关键性技术之一，直接决定了机器人任务的完成度和损耗值。

近年来，来自不同行业的研究人员对移动机器人的导航算法研究从未停止。在基于ROS的机器人局部移动规划中，曾广泛使用Trajectory Planner，后又逐渐发现了相同条件下样本采样空间更少的动态窗口（Dynamic Windows Approach，DWA）算法。因此本文选择从DWA算法入手，通过数学逻辑推理和ROS机器人的仿真及实物实景下的测试结果，研究其对机器人的实际控制效果，以及能否找到局部移动规划算法的更优解。

1  自主导航

智能机器人的基础功能之一就是自主导航，包括了地图构建、路径规划和自主移动，在机器人实际运用中是必不可少的部分。本文所用机器人使用LS01D 360°二维扫描测距激光雷达采集周围距离信息，并采用SLAM算法在ROS机器人操作系统中构建实时地图，搭载了用来获得整体位姿和加速度信息的IMU姿态传感器，通过51单片机作为下位机的控制器，进行编程和总线操作，控制直流无刷电机和舵机，从而实现对机器人方向和速度方面的控制。采用自适应蒙特卡洛定位 AMCL 实现定位功能，使用A*算法进行整体路径规划，从而实现地图构建、路径规划和自主移动功能。

1.1  ROS操作系统

自主导航程序需要基于开源的机器人操作系统ROS。它提供了操作系统应有的服务，包含硬件抽象，底层设备控制，常用函数的完成，进程间消息传递，以及包管理。它也提供用于获取、编译、编写、跨计算机运行代码所需的工具和库函数。ROS作为一个开源的软件系统，在某些方面ROS为机器人软件开发者提供了一个包含一系列工具、库和约定的灵活框架，相当于一种“机器人框架（robot frameworks）”。此外，ROS是一个分布式的进程框架，将每个工作进程视为一个节点，并使用节点管理器进行统一管理。同时，ROS提供了一套消息传递机制，计算机视觉和语音识别等功能会带来的实时计算压力，可以由此消息传递机制进行分散，使机器人能够克服遇到的挑战。因此，所有的工程都可以通过ROS的基础工具集成在一起。

1.2  SLAM建图原理

SLAM（simultaneous localization and mapping），也被叫作CML（Concurrent Mapping and Localization），同步定位与地图创建，或并行映射与定位。换言之，机器人被放置于未知环境中的未知位置，让机器人在移动时逐渐绘制周围障碍物和环境的地图，并实时获取机器人本身此时的位置信息。所谓完整的地图（a consistent map）指的是无障碍地走到房间可达到的每个角落。

根据从编码器和IMU惯性传感器可以获得机器人的测位，通过距离传感器可以计算机器人与障碍物之间的直线距离。随后，导航系统会将机器人的位置、传感器姿态、障碍物信息以及通过SLAM地图获得的占用栅格地图，调用到固定地图（static map），作为占用区域（occupied area）、自由区域（free area）和未知区域（unknown area）。根据上述的四种信息，计算出障碍物存在的区域、预计碰撞障碍物的区域和可以无阻向前移动的区域，这些统称为成本地图（costmap）。成本地图又可以根据导航类型被分为整体成本地图（global_costmap）和局部成本地图（local_costmap）两部分。在全局移动路径规划中以整个区域为对象建立移动计划，其输出结果就是global_costmap。在局部移动路径规划中，local_costmap就是在以机器人为中心的部分限定区域中规划实时的移动路径，或是遇到需要躲避的障碍物时用到的地图。然而，这两种成本图的目的不同，但表示方法却是相同的。成本地图可以用0到255之间的值来表示，数值的含义如表1和图1所示。每个区域的计算取决于成本地图中的配置参数。

机器人在实时定位的过程中，由于对机器人的运动进行过程估计时，所获取的位置信息有可能不精确，从而产生误差。当前的室内机器人用得最多的是导航推测（dead reckoning）。它的缺点是只能估算相对位置，然而，由于它仅用廉价的传感器就能实现，而且研究进展已有较长的时间，因此可以获得一定程度的位置估计值得到一定水平的位置估计值，从而得到了广泛的应用[1]。导航推测技术使用机器人的车轮的旋转量来估计机器人本身的移动量，但车轮的旋转量具有不少的误差。减少误差的方式就是利用IMU传感器等获取惯性信息来补偿位置和方向值、利用测距单元不断地测量周围环境的信息，实时地进行调整更正。

图1  障碍距离与成本地图值的关系

在完成了机器人的同步定位和地图建模之后，就可以开始导航了。导航包括全局路径规划（global planner）和局部路径规划（local planner）。

2  全局路径规划

全局路径规划，它主要指基于 SLAM 这个系统，在二维地图上预先规划好当前一个机器人移动所处的地理位置，即一条全局路径。ROS中间全局路径规划器算法采用的是global_planner，基于A*（A-star）的算法。A*算法，作为常见的路径检索和路径遍历的通用算法，具备了较高的驱动性能和工作精度。它采用启发函数来估计地图上机器人当前的位置到目标位置之间的距离，然后选择最佳方向进行搜索，若失败将选择其他路径继续搜索直到找出最佳路径。作为Dijkstra算法的一个扩展，又借助了基于启发式算法的方式来进行引导，A*算法因此拥有更好更高效的路径搜索结果[2]。

A*算法同样也是求解静态地图最短路径的最有效方法，适用于各种路径规划场景。在每次搜索中，选择估计函数f（n）的最小值来规划最优路径，经典A*算法的估计函数f（n）可表示为：f（n）=g（n）+h（n）

从起点到当前搜索点的实际距离代价的值用g（n）式表示;从当前搜索点到终点的估价求值函数用h（n）式表示。

3  局部路径规划

仅仅是获得了整体规划路径还是远远不够的，实际生活当中可能会出现很多突发的情况，需要将原先规划好的路径进行及时调整，因此，还有另一个更深层次的路径规划模块，即局部路径规划。

3.1  算法初步优化

在以往的ROS局部移动规划中，曾广泛使用Trajectory Planner。在给定机器人的加速度极限的情况下，Trajectory Planner在整个前向模拟周期内从可实现的速度集合中进行采样。而DWA算法在相同前提下的采样点，仅针对一个模拟步骤中的可用速度集合。

这意味着DWA的优点是更少的样本空间和更高的采样效率，但又知DWA不能对常加速度做前向模拟，似乎对于是低加速度的机器人来说，可能Trajectory Rollout更好。但是在实践中，经过多次实验发现两种方法性能基本相當，由此DWA算法更胜一筹。调用dwa_local_planner包即可实现DWA算法的使用，算法流程如图2所示[3]。

图2  DWA算法流程

3.2  DWA算法原理

DWA算法中没有用较为常见的x轴和y轴的位置坐标系，而是用图3中的平移速度v和旋转速度w为轴的速度搜索空间来表示机器人。在这个存在硬件限制的空间中，机器人具有最大允许速度，这被称为动态窗口（Dynamic Window）。

图3  DWA算法示意图

3.2.1  机器人的运动建模

在动态窗口算法中，为了将机器人未来局部的轨迹模拟出来，需要知道机器人的运动模型。在这里，我们假设两轮移动机器人的轨迹为圆弧形状，那么一对（v，w）就代表一个圆弧轨迹。它做圆弧运动的半径为：

当旋转速度w不等于0时，机器人坐标为：

3.2.2  速度采样

DWA的第二个核心是如何采样速度：在速度（v，w）的二维空间中有无限多组速度。然而，根据机器人的极限速度限制、部件性能和环境约束，采样速度可以被有效地控制在一定范围内。

3.2.3  评价函数

DWA结合了机器人的动力学特性，在多组速度空间中对速度进行采样，通过采集的数据模拟轨迹，然后根据事先设定的评分规则对模拟轨迹进行评分，评分最高的轨迹作为最优路径。然而，ROS中DWA算法的评估规则与原始DWA不同。此处评分规则综合考虑了路径、目标、方向角三方面。在局部导航过程中，较好的避障性能、方向性能、速度性能三者缺一不可，所以评价函数G（v，w）可以被表示为：

G（v，w）=σ（α·h（v，w）+β·d（v，w）+γ·v（v，w））

式中的h（v，w）为方向角评价函数，当机器人以当前设定的采样速度到达模拟轨迹末端时，方向角评价函数用于评估实际末端朝向和目标之间的角度间隙。如图4所示，如果采用180-θ的方式来评价，则θ越小，评价得分越高。式中d（v，w）为空隙评价函数，表示当前轨迹上的机器人与最近障碍物之间的最短距离。如果在此轨道上不存在任何障碍物，那么将其设置为常量。式中的v（v，w）为速度评价函数用来评价当前轨迹的速度大小。通过设置σ、α、β、γ四个常数可调整权值，从而得到最优解[4]。

图4  方向角评价函数示意图

实际操作中，通常是利用base_local_planner包实现局部路径实时规划的，该包融合了Trajectory Rollout和 DWA算法。具体步骤是，先离散采样机器人的控制空间（dx，dy，dθ），再对于每个采样的速度控制量，以机器人当前的状态为基础，进行向前模拟预测，即如果继续以该速度（v，w）前进一段距离后是否会遇到障碍物等情况。然后结合仿真预测中的障碍物接近目标、接近全局路径和速度等特征，对每条轨迹的预测结果进行评价，丢弃不合适的路径，如有障碍物碰撞的路径。不断重复以上步骤，最后选择得分最高的轨迹，并将其相应的速度控制量发布给相关的移动平台。

3.3  算法再次优化

但若实验环境稍微复杂一些，运用dwa_local_planner包进行的局部实时规划效果不是很理想。DWA算法无法设置转向半径参数，因此，此算法大多时候面向的是差分或者全向机器人，它们的共同点是可以做到原地自旋转，而另一些机器人模型则无法做到自转，比如阿克曼模型。于是选择加入一种较新的局部路径规划器teb_local_planner，它实质上是ROS导航包dwa_local_planner的插件，在线优化了本地轨迹规划器。使用TEB算法时，机器人在运动过程中会调整自己的位姿朝向，使之与目标朝向一致。

3.4  TEB算法原理

TEB（Timed-Elastic-Band，时间弹性带）优化运动轨迹的方式是修正全局路径规划的初始轨迹。该算法主要优化与障碍物的距离、路径的长度和轨迹的运行时间等。改进后的机器人控制系统框图如图5所示。

图5  带TEB算法的机器人控制系统框图

如图6所示，N个带有时间信息的离散姿态构成TEB算法生成的轨迹，然后使用G2O（general graph optimization）算法对这些姿态进行优化，使生成的轨迹路径长度最短、耗时最快、距离障碍物最远，并对速度和加速度进行限制，使轨迹满足机器人运动学要求。

图6  TEB算法中的配置序列和时间间隔

移动机器人的中心点坐标与偏转方向共同决定了其在环境中的位姿。首先定义机器人位姿：

χα=（xi，yi，βi）T∈R2×S1

其中xi和yi、βi分别对应机器人在地图坐标系的位置和姿态朝向，则空间内位姿序列可表示为：

将机器人两个位姿间的时间间隔定义为，记录时间序列：

接着，将位姿和时间序列合并：

B：=（Q，τ）

TEB算法以姿态和时间间隔设置为待优化变量，在动态约束的条件下求解最优路径。这些动态约束包括速度和加速度限制、路径长度、障碍物与机器人之间的距离以及机器人在轨迹上的运行时间等等。随后，通过设置加权多目标函数得到最优路径Q：

B*=argBminf（B）

为考虑各种约束后，将f（B）用作表示目标函数，fk为各个目标函数的权值。目标函数在经过G2O算法求解后，可最终得出最优结果B*，即为局部路径点序列[5，6]。

4  实验

4.1  仿真实验

DWA路径规划仿真结果如图7所示，仿真系统会生成一条淡灰色轨迹的全局路径和一条深黑色轨迹的局部路径线。在局部代价地图中，机器人通过行进途中不断获取与周围障碍物的距离，在全局路径规划下实时调整局部路径规划。

图7  DWA仿真

當机器人接近障碍物时可能发生碰撞，调整权重后的局部规划结果不再同全局路径线重合，机器人由此不与障碍物发生碰撞。

在配置好teb_local_planner中运动轨迹参数、运动速度和旋转角速度阈值等等和权重参数后，开始进行TEB路径规划仿真实验。仿真结果如图8和图9所示，浅色线条为全局规划路径，带若干箭头的深色轨迹代表局部路径，其中一个箭头代表一个路径点及其朝向。图9中机器人到达障碍物附件，TEB算法会在机器人前进的同时根据全局路径规划不断调整局部规划的轨迹。

图8  TEB仿真一

图9  TEB仿真二

4.2  实物环境实验

利用基于ROS的机器人在实验室进行避开障碍物路径规划算法的比较实验。选择A*算法作为全局路径规划算法，分别使用DWA算法和TEB算法作为局部路径规划算法[7]，在障碍物位置、个数不变的情况下，设置相同的起始点和目的点进行重复实验25次，途中不碰到障碍物且到达指定地点即视为成功到达，结果如表2所示。

实验结果表明在TEB算法的局部路径规划下，机器人在绝大多数情况下能够穿过障碍物最终到达指定目标点且不存在碰撞或卡死现象，整体效果优于DWA算法。综合考虑运动中需要躲避障碍物、行驶时效、动力学约束、路径的平滑性等因素，TEB算法在复杂的环境中具有较为优秀的实验结果。

4.3  TEB算法缺陷与改进

使用TEB进行局部路径规划时，如果在起始点处，机器人的定位朝向不是由起始点指向目标点，那么在启动出发前，机器人会采取倒车旋转指令来把朝向修正到正确位置。但是，在一些场合比如狭窄空间内，倒车和旋转角度是无法实现的。因此，若机器人构造符合两轮差速的运动模型，可设计程序先让底盘在原地旋转至正确朝向，然后再进行局部路径规划等一系列操作，得到最优路径。同样的，到达终点时也有类似的问题。解决方法是在设置参数的时候将角度容差值设为360度，等到达目标点之后再由IMU获取实际角度数据，与理想目标点方向作差得出差值向量，再在原地旋转该值即可。

5  结  论

对于局部路径实时规划，DWA算法以它评价函数多元化、采集样本高效化的优势，目前被广泛运用于机器人自动导航。但是通过实验证明它仍存在着前瞻性不足、动态避障效果差等缺点。于是本文介绍了一种较新的TEB算法，引入了一个新型局部路径规划器teb_local_planner。综合考虑运动中避障、轨迹执行时间、动力学约束等因素，TEB算法适用于更为复杂的环境。

参考文献：

[1] 表允晳，赵汉哲，郑黎蝹，林泰勋.ROS机器人编程（从基本概念到机器人应用程序编程实战） [M].首尔：ROBOTIS Co.，Ltd.，2017：318-338.

[2] 李峥.基于ROS平台的导航机器人路径规划研究 [J].现代信息科技，2020，4（14）：163-165+168.

[3] 温淑慧，问泽藤，刘鑫，等.基于ROS的移动机器人自主建图与路径规划 [J].沈阳工业大学学报，2022，44（1）：90-94.

[4] 孙小飞.基于ROS的自动分拣机器人系统设计与实验研究 [D].重庆：重庆大学，2020.

[5] 高佳佳.基于全局地图的移动机器人路径规划研究 [D].西安：西安工业大学，2019.

[6] 郭柱梁.基于激光雷达的SLAM和动态避障机器人研制 [D].黑龙江：黑龙江大学，2021.

[7] 刘建娟，薛礼啟，张会娟，等.融合改进A*与DWA算法的机器人动态路径规划 [J].计算机工程与应用，2021，57（15）：73-81.

作者简介：过佳颖（2001—），女，汉族，江苏无锡人，本科在读，研究方向：電子信息科学与技术。