陈劲宇, 王 坤, 王 硕, 樊世杰, 麻琦昌, 李冬梅, 王红波
(1. 吉林大学 电子科学与工程学院, 长春 130012；
2. 中国工商银行吉林省分行 金融科技部, 长春 130061)
近年来, 随着科学技术的进步, 移动机器人被广泛应用, 并在无人驾驶、 智能物料运输等领域取得了长足发展。为了能在复杂环境中正常运行, 机器人需要配备自主导航系统。由于路径规划是自主导航的关键技术之一, 如何在人流密集区域中高效地进行路径规划成为急需解决的问题。
目前人们已对路径规划问题进行了大量研究。全局路径规划算法有Dijkstra算法[1]、 蚁群算法[2]及A*[3]算法等; 局部路径规划算法有动态窗口法[4]和人工势场法[5]等。Hart等[6]于1968年发表了标准A*算法, 该算法运用启发信息指引搜索方向, 搜索时间短、 效率高, 被广泛应用于移动机器人的路径规划, 但其规划路径拐点多、 不平滑, 不利于机器人的运行[7]。为改进该算法, Lin等[8]在A*算法的启发函数中引入了父节点信息, 减少了Openlist中的节点数量, 缩短了搜索时间。Yu等[9]将A*算法与人工势场法相结合, 解决了人工势场法出现陷入局部极小点以至于无法找到最佳点的问题, 得到了较好的路径融合算法。李文刚等[10]对A*算法的局部目标点进行迭代更新, 结合动态窗口法, 得到了水面无人艇的最佳搜索路径。传统的路径规划算法不能很好地解决移动机器人的自主避障问题, 为解决这个问题, Fiorini等[11]于1998年首次提出速度障碍法。该算法将动态环境映射到机器人的速度空间中, 为机器人的动态避障提出了新的策略。许文瑶等[12]应用基于速度障碍法的动态避障方法, 解决了水下机器人动态运动特性不好及控制时滞性差的问题。
应用动态窗口法改进的A*算法, 机器人在动态环境下的路径规划能力可得到较大改善。然而, 将其应用于Turtlebot3移动机器人上, 实验结果表明, 在人流密集的情况下, 机器人的运动能力不够理想。因此, 笔者利用速度障碍法改进A*算法, 以提高其在动态障碍物较多的情况下避障能力差的问题。
1.1 局部路径规划算法----动态窗口法(DWA)
动态窗口方法(DWA：
Dynamic Window Approach)在机器人可达的速度空间内搜索最优的控制速度[13], 从而使机器人的硬件特性融入到该算法中。动态窗口法在速度空间(v,w)中对速度进行多次采样, 模拟机器人下一个时间节点内的运行路径。在得到多组速度后, 算法将对模拟运行路径进行评价, 选取最佳轨迹所对应的速度输出到机器人。使用的步骤分为4步。
1) 设置初始参数, 机器人的初始姿态(x,y,θ,v,w), 目标点, 障碍物的地点。其中x为机器人相对于世界坐标系的横坐标；
y为机器人相对于世界坐标系的纵坐标；
θ为目标点方向与速度方向的角度差；
v为机器人速度；
w为机器人角速度。
2) 计算速度指令。根据机器人当前姿态和机器人的物理结构参数计算一个矩形动态窗口。
3) 在动态窗口中均匀采集多个点, 随后分别计算每个采集点的机器人姿态, 得到机器人的3个指标和总目标函数, 并将目标函数最小的点对应的速度输出到机器人。
4) 根据3)输出的速度计算机器人的新姿态, 将新姿态更新为目前姿态。如果此时机器人没有行进到目标点, 则返回步骤2)重新计算；
如果已经到达目标点, 则算法结束运行。机器人的运动路线对应的一系列位置即为DWA算法计算出的路径。如图1所示。
评价函数如下
G(v,w)=σ(αh(v,ω)+βd(v,ω)+γv(v,ω))
(1)
其中h(v,w)为方位角评价函数, 用于评价机器人在当前设置的采样速度下行进到模拟轨迹终点时的朝向和目标角度差距；
d(v,w)为机器人在设定路径运动时与最近障碍物之间的最小距离, 没有障碍物则定义最小距离为一个常量；
v(v,w)用于评价当前轨迹的速度大小；
α为方向角系数；
β为距离系数；
γ为速度系数。
完成上述步骤后, 进行数据归一化。
最后将3个归一化结果相加即可得到最终结果。其中n为采样的所有轨迹,i为待评价的当前轨迹。
1.2 局部路径规划算法----速度障碍法
速度障碍法(VO：
Velocity Obstacle)是一种通过速度信息直接预测变化环境中的碰撞的算法[11]。其利用了速度障碍的概念, 将动态环境映射到机器人的速度空间中。速度障碍法计算在给定时间范围内, 机器人与障碍物会在将来某个时刻发生碰撞的速度的一阶近似值。利用这种表示, 可简单地通过选择VO区域外的速度作为避障策略。
速度障碍法的具体步骤如下。
1) 将机器人和障碍物(见图2a)分别抽象。首先将机器人的形状大小映射到障碍物的构造空间, 此时, 机器人抽象成一个点, 障碍物的抽象为一个圆且半径增加了RA。
VO将机器人的绝对速度分成了将发生碰撞和不会碰撞两个区域, 只要机器人选择VO以外的速度, 就不会发生碰撞。其中表示机器人的速度空间如图2d所示, 对多障碍物的避障, 可将其看成多个独立VO的联合
图2 速度障碍法的步骤Fig.2 The steps of velocity obstacle
其中m为障碍物个数。
1.3 即时定位与地图构建----SLAM-GMapping
同步定位与地图构建是自主移动式机器人采用的一种技术[14]。其不仅可构建地图, 还可定位机器人。工程师们使用地图信息执行路径规划和避障等任务。笔者使用GMapping作为地图构建算法。
图3 估计的位姿信息概率Fig.3 Estimated probability of posture information
SLAM-GMapping是基于RBpf粒子滤波算法, 即定位和建图过程分离的算法。GMapping算法有效利用了车轮里程计信息, 使其对激光雷达频率要求降低。GMapping算法通过观测数据z1:t和控制数据u1:t, 以推测位姿和联合概率密度分布p(x1:t,m|z1:t,u1:t-1)。
为估计位姿, RBpf算法通过使用重要性重采样算法作为粒子滤波器, 用以估计每个时刻机器人的位姿。RBpf算法包括4部分：
采样、 计算权重、 重采样和地图估计。
机器人的定位信息如图3所示。使用条件概率将联合概率密度分布进行展开
p(x1:t,m|z1:t,u1:t-1)=p(m|x1:t,z1:t)p(x1:t|z1:t,u1:t-1)
(6)
式(6)含义为通过上一时刻根据已有的位姿信息, 推断出这一时刻的位姿信息, 然后开始对地图进行构建。
1.4 全局路径规划算法----A*算法
A*算法是移动机器人常用的全局路径规划算法[15], 此算法是在Dijkstra算法的基础上改进而成, 有着较好的性能和准确度。A*算法计算每个节点的优先度公式如下
f(n)=g(n)+h(n)
(7)
其中f(n)为节点n的综合优先级。当选择下一个要遍历的节点时, 需要选取综合优先级最高的节点。g(n)为节点n距离起点的代价。h(n)为节点n距离终点的预计代价, 这即是A*算法的启发式函数。
A*算法在运算过程中, 每次从队列中选取f(n)值最小(优先级最高)的节点作为下一个待遍历的节点。
启发函数使用曼哈顿距离。曼哈顿距离指在欧几里得空间的固定直角坐标系上两点所形成的线段对轴产生的投影的距离总和。公式如下
H(n)=|X(g)-X(n)|+|Y(g)-Y(n)|
(8)
其中H(n)为当前节点的曼哈顿距离；
X(n),Y(n)为当前节点的X坐标和Y坐标；
X(g),Y(g)为目标节点的X坐标和Y坐标。
1.5 VO混合路径规划算法
笔者将VO算法和A*算法结合使用作为VO混合路径规划算法。其流程如图4所示。
图4 混合算法流程图Fig.4 Hybrid algorithm flow chart
VO混合路径规划算法实现流程如下：
首先完成栅格地图的加载, 然后使用A*全局路径规划算法获取子节点序列, 之后判断子目标节点是否被障碍物阻挡, 即动态障碍物是否移动到A*算法生成的路径点上, 如果机器人被障碍物阻挡, 则使用VO算法进行局部路径规划, 否则判断子目标节点是否为全局目标点。局部路径规划完成后, 判断子目标节点是否为全局目标节点, 如是则完成路径规划, 否则更新当前子目标节点, 重复操作。
在Gazebo中搭建完成仿真环境模型后, 需要使用SLAM(Simultoneous Localization and Mapping)技术进行地图构建。最方便且成熟的GMapping算法被采用绘制栅格地图, 并使用Rviz软件获取并保存生成的栅格地图。实现构图的步骤如下：
1) 编写launch执行文件, 用于配置GMapping算法的参数列表；
2) 启动仿真测试环境的launch文件；
3) 启动使用GMapping算法的launch文件；
4) 启动keyboard_teleop节点, 并使机器人订阅/cmd_vel主题, 实现对机器人速度、 方向控制；
5) 启动rviz软件, 并订阅/LaserScan,/RobotModel,/map节点, 监测机器人完成地图构建, 如图5所示；
图5 Turtlebot3构建地图的过程Fig.5 The process of building a map in Turtlebot3
6) 构建地图完成后, 使用map_server命令保存地图文件。
在地图建立完毕后, 将VO算法与A*算法结合使用作为混合路径规划算法, 检测在不同运动条件的障碍物下, 自主导航的移动机器人避障能力, 并与DWA算法进行对比研究。将使用ROS(Robot Operating System)中的两个核心包, 分别是move_base和amcl。move_base用于最优路径的规划, amcl用于实现机器人的位姿信息估计。实现规划的流程如下。
1) 确定Turtlebot3移动机器人各项配置参数, 并编写两份配置文件：
① 在局部路径规划配置文件中声明使用VO算法, 在全局路径规划配置文件中声明使用A*算法；
② 在局部路径规划配置文件中声明使用DWA算法, 在全局路径规划配置文件中声明使用A*算法。
2) 修改world文件。修改velocity节点使障碍物在两坐标点间反复运动。
3) 编写launch文件。在gazebo中完成地图服务的启动和地图仿真环境的加载, 并启动move_base和amcl二进制文件, 发布/odom与/map之间的TF(Transform Listener)变换。
4) 启动自主导航的launch文件, 并将pgm地图文件作为参数输入。
5) 启动Rviz软件, 添加/map、/Path、/RobotModel等参数以完成可视化。通过使用2D Pose Estimate功能对机器人位姿信息估计, 之后使用2D Nav Goal功能设置目标地点, 指导机器人运动。
6) 编译并运行C++时间记录程序, 使移动机器人在起点和终点之间不断反复运动, 并输出路程长度和到达终点时间等数据。
实验将DWA算法和VO混合算法分别作为路径规划算法, 并最终检测机器人从起点到达终点所需要的时间, 重复10次实验后取均值作为最终机器人达到终点时间并分析出结果。
3.1 建 图
实验使用键盘控制Turtlebot3机器人移动, 基于GMapping算法进行地图绘制, 如图6所示。实验结果表明, 在地图较小的情况下, 使用GMapping算法绘制地图速度较快, 结果具有较高的精准度。但随着地图增大, 地图绘制中出现的错误也会增加。
3.2 VO导航
打开rviz软件, 加载Turtlebot3机器人模型, 设置目标地点, 在可视化列表中加载Global Map、 Local Map、 Path与Amcl Particles, 并订阅/move_base主题, 结果如图7所示。
图6 使用rviz画出的地图 图7 Rviz导航画面Fig.6 Use the map drawn by rviz Fig.7 Rviz navigation screen
机器人周围的深色区域为局部地图， 最外围浅色的区域为全局地图。箭头为目标地点和机器人最终的矢量信息， 机器人周围的粒子， 表示采用AMCL(Adaptive Monte Carlo Localization)自适应蒙特卡洛定位， 并使用粒子滤波器跟踪已知的地图中机器人位姿。机器人周围的白点为雷达扫描出的障碍物信息。细线为机器人使用局部路径规划算法规划得到的路径， 在不存在障碍物情况下， 二者仅具有细微的差别。但当实际地形出现地图中不存在的障碍物时， 二者将会具有较大差距。
3.3 对照试验
实验人为规定起点和终点, 指定机器人的初始方向后, 机器人将自主的由起点运动至终点, 并且C++程序计算机器人从起点到终点的时间差, 重复实验, 最终使用SPSS(Statistical Product Service Solutions)绘制实验结果。实验设置机器人运动的最大速度Vrmax=0.22 m/s, 静态障碍物速度V=0 m/s, 动态障碍物的最大速度Vbmax=-0.124 m/s, 且匀速直线运动。
第1组实验, 设置3个静态障碍物(见图8), 机器人运动过程中会使用自身的雷达传感器显示出周围障碍物的信息, 即使障碍物在地图中并不存在。统计10次机器人运动到终点的时间, 结果如图9所示。
图8 在gazebo中添加障碍物 图9 SPSS绘制第1组实验结果Fig.8 Add obstacles to gazebo Fig.9 SPSS plots the results of the first set of experiments
由于机器人每次计算出的全局代价地图不完全相同, 并且每次进行局部代价地图评估存在差别, 因此到达终点的时间不尽相同。
采用DWA算法和VO混合路径规划算法到达目标地点的时间并不相同, 两者在进行避障决策时存在一定的差异。DWA算法, 在速度空间中搜索适当的平移速度和旋转速度指令, 给出速度指令的避障规划, 基本只涉及约束条件, 并不涉及障碍物的运动情况, 所以路径的选取十分迅速。VO混合路径规划算法的路径判定过程, 涉及机器人和障碍物之间的坐标变换, 在每个时刻对周围障碍物的速度矢量进行预测, 计算强度相对较大。即使障碍物处于静止状态, VO混合路径规划算法也会对其进行预测和处理。因此采用VO混合路径规划算法在执行避障策略时所需要的时间较多。第2组实验将其中两个静态障碍物替换为动态障碍物, 测量结果如图11所示。
图10 Rviz显示一个障碍物的相对运动轨迹 图11 SPSS绘制第2组实验结果Fig.10 Rviz shows the relative trajectory of an obstacle Fig.11 SPSS plots the results of the second set of experiments
分析结果表明, 机器人分别采用VO混合路径规划算法和DWA算法, 到达终点的时间差Tinter随着动态障碍物数量的增加而减小, 但运动至终点的时间均随之增加。其次, 使用DWA算法的机器人到达终点时间, 波动较大, 而使用VO混合路径规划算法的机器人到达终点的时间, 波动较小, 到达终点的时间较稳定。
基于上述实验, 提出如下猜想, DWA算法并不具备“预测”障碍物运动的能力。首先, 确定DWA算法的评价函数(见式(1))中并不涉及任何有关障碍物速度的关系矩阵。其次, 假如雷达传感器两次检测时间Tint均过长, 将导致DWA算法无法及时对地图数据进行更新, 使机器人与障碍物发生短暂碰撞的机率增大。碰撞结果具有随机性, 加剧机器人到达终点时间曲线的波动。对VO混合路径规划算法(见式(5)), 涉及到障碍物的速度信息, 因而具备转化障碍物速度关系矩阵的能力, 在处理运动障碍物中具有更大的优势, 同时降低碰撞发生的概率。
图12, 图13分别给出DWA算法和VO混合路径规划算法在遇到动态障碍物时的避障轨迹, 其中曲线表示机器人轨迹图。
图12 DWA算法避障轨迹 图13 VO混合路径规划算法的避障轨迹Fig.12 Obstacle avoidance trajectory for dynamic window method Fig.13 Obstacle avoidance trajectory for speed barrier method
如图12, 图13所示, 使用DWA算法的机器人在避障过程中需要沿障碍物运动的垂直方向移动一段距离才能完成避障。因此, 认为DWA算法避障策略依然是将动态障碍物转化为静态的方式进行处理。当雷达扫描频率增加, 或障碍物移动速度减小时, 机器人与障碍物发生碰撞的几率将会下降。当发生碰撞几率Pcrash VO混合路径规划算法会保存上一次测量的障碍物速度信息, 并假设目标障碍物保持恒定速度沿直线运动。因为VO混合路径规划算法存在这种“预测”行为, 所以并不会仅依靠雷达探测器的扫描频率。即使扫描频率减小, 机器人与障碍物发生碰撞的几率仍较小。设碰撞几率表达式为 Pcrash(t)=Pcrash(0)+λTint (9) 其中λ为碰撞因子, 由雷达传感器的精确度决定； 图14 SPSS绘制第3组实验结果Fig.14 SPSS plots the results of the third set of experiments 第3组实验, 目的是检验第2组实验的猜想正确与否。将目标障碍物全部替换为动态障碍物, 同时把障碍物的速度增加至Vbmax=-0.254 m/s, 此时|Vbmax|>|Vrmax|。若DWA算法时间曲线和VO混合路径规划算法时间曲线波动进一步加大, 则说明DWA算法“预测”障碍物轨迹能力较弱, 或不具备“预测”能力, 同时障碍物的速度大小也会影响到DWA算法规划轨迹的准确度。 测量结果如图14所示。两种算法的时间曲线对比表明： 碍物的移动速度将影响DWA算法的精确度。 基于ROS仿真环境, 笔者搭建了机器人模型、 构建了环境模型并绘制了环境地图, 同时进行了基于DWA算法和VO混合算法的路径规划对比研究。实验选取了3组不同运动状态的障碍物, 统计分析了寻路时间, 并对DWA算法曲线的波动做出了合理的猜想与验证。最终, 基于实验数据发现, 当静态障碍物占据障碍物的主体时, 移动机器人应用DWA算法相比于VO混合路径规划算法到达目标地点所需要的时间更短。在动态障碍物多, 障碍物移动速度快以及雷达扫描频率低的情况, 使用VO混合路径规划算法的寻路时间较短, 且时间波动曲线更平缓。原因是障碍物的移动速度影响DWA算法的避障精确度, 并且VO混合路径规划算法较DWA算法, 能显著降低碰撞发生的概率。通过该实验可以为实际情况下移动机器人使用A*算法结合VO算法的混合路径规划的选择提供一个可靠的思路和方向。 参考文献： [1]DIJKSTRAE W. A Note on Two Problems in Connexionwith Graphs [J]. Numerische Mathematik, 1959, 1(1): 269-271. [2]THI THOA MAC A B, COSMIN COPOT A C , DUC TRUNG TRAN B, et al. A Hierarchical Global Path Planning Approach for Mobile Robots Based on Multi-Objective Particle Swarm Optimization [J]. Applied Soft Computing, 2017, 59: 68-76. [3]付丽霞, 任玉洁, 张勇, 等. 基于改进平滑A*算法的移动机器人路径规划 [J]. 计算机仿真, 2020, 37(8): 271-276. FU Lixia, REN Yujie, ZHANG Yong, et al. Path Planning of Mobile Robot Based on Improved Smoothing A*Algorithms [J]. Computer Simulation, 2020, 37(8): 271-276. [4]DROGE GREG. Dual-Mode Dynamic Window Approach to Robot Navigation with Convergence Guarantees [J]. Journal of Control and Decision, 2021, 8(2): 77-88. [5]YANG W L, WU P, ZHOU X Q, et al. Improved Artificial Potential Field and Dynamic Window Method for Amphibious Robot Fish Path Planning [J]. Applied Sciences, 2021, 12(5): 14-19. [6]HART P E, NILSSON N J, RAPHAEL B. A Formal Basis for the Heuristic Determination of Minimum Cost Paths [J]. IEEE Transactions on Systems Science & Cybernetics, 1968, 4(2): 100-107. [7]詹京吴, 黄宜庆. 融合安全A*算法与DWA法的机器人动态避障研究 [J/OL]. 计算机工程. [2022-01-23]. https:∥kns.cnki.net/kcms/detail/detail.aspx?dbcode=CAPJ&dbname=CAPJLAST&filename=JSJC20211103004&uniplatform=NZKPT&v=g9BjGJf5ZLUKRACC91AL1G0lWG0h4NXNA1-Jfg_y5se98o7aaSiYqigHkBCa-Kt. ZHAN Jingwu, HUANG Yiqing. Research on Dynamic Obstacle Avoidance of Robot by Combing Safety A*Algorithm and Dynamic Window Approach [J/OL]. Computer Engineering. [2022-01-23]. https:∥kns.cnki.net/kcms/detail/detail.aspx?dbcode=CAPJ&dbname=CAPJLAST&filename=JSJC20211103004&uniplatform=NZKPT&v=g9BjGJf5ZLUKRACC91AL1G0lWG0h4NXNA1-fg_y5se98o7aaSiYqigHkBCa-Kt. [8]LIN M, YUAN K, SHI C, et al. Path Planning of Mobile Robot Based on Improved A*Algorithm [C]∥Proceedings of the 2017 29th Chinese Control and Decision Conference. Piscataway, NJ: IEEE, 2017: 3570-3576. [9]YU J B, DENG W, ZHAOZ Y, et al. A Hybrid Path Planning Method for an Unmanned Cruise Ship in Water Quality Sampling [J]. IEEE Access, 2019, 7: 87127-87140. [10]李文刚, 汪流江, 方德翔, 等. 联合A*与动态窗口法的路径规划算法 [J]. 系统工程与电子技术, 2021, 43(12): 3694-3702. LI Wengang, WANG Liujiang, FANG Dexiang, et al. Path Planning Algorithm Combining A*with DWA [J]. Systems Engineering and Electronic Technology, 2021, 43(12): 3694-3702. [11]FIORINI P, SHILLER Z. Motion Planning in Dynamic Environments Using Velocity Obstacles [J]. International Journal of Robotics Research, 1998, 17(7): 760-772. [12]许文瑶, 贺继林. 基于改进速度障碍法的水下机器人动态避障 [J]. 电光与控制, 2021, 28(12): 86-90. XU Wenyao, HE Jilin. Dynamic Obstacle Avoidance for ROV Based on Improved Velocity Obstacle Method [J]. Electro Optic and Control, 2021, 28(12): 86-90. [13]刘建娟, 薛礼啟, 张会娟, 等. 融合改进A*与DWA算法的机器人动态路径规划 [J]. 计算机工程与应用, 2021, 57(15): 73-81. LIU Jianjuan, XUE Liqi, ZHANG Huijuan, et al. Dynamic Path Programming of Robots Based on Fusion Improvement A*and DWA Algorithms [J]. Computer Engineering and Applications, 2021, 57(15): 73-81. [14]GRISETTI G, STACHNISS C, BURGARD W. Improved Techniques for Grid Mapping with Rao-Blackwellized Particle Filters [J]. IEEE Transactions on Robotics, 2007, 23(1): 34-46. [15]杨明亮, 李宁. 改进A*算法的移动机器人路径规划 [J/OL]. 机械科学与技术: 1-7 [2022-04-26]. https:∥kns.cnki.net/kcms/detail/detail.aspx?dbcode=CJFD&dbname=CJFDLAST2022&filename=JXKX202205018&uniplatform=NZKPT&v=0qNUz6ECP02sRXDXHKhGPikttAENUxcSlUatnCW6RR4OB6HKW05ccjOyE7IXxcLl. YANG Mingliang, LI Ning. Path Planning of Mobile Robots with Improved A*Algorithm [J/OL]. Mechanical Science and Technology: 1-7 [2022-04-26]. https:∥kns.cnki.net/kcms/detail/detail.aspx?dbcode=CJFD&dbname=CJFDLAST2022&filename=JXKX202205018&uniplatform=NZKPT&v=0qNUz6ECP02sRXDXHKhGPikttAENUxcSlUatnCW6RR4OB6HKW05ccjOyE7IXxcLl.
Tint为雷达传感器两次扫描的时间间隔。随着Tint的增加, 发生碰撞几率Pcrash(t)也会随之增加。
DWA算法时间曲线波动加大, 而VO混合路径规划算法时间曲线依然较为平坦, 且VO混合路径规划算法到达时间领先于DWA算法, 同时表明在具有3个动态障碍物的动态环境下, VO混合路径规划算法时间曲线抗干扰能力较强, 更适合在环境多变的场合下应用。本组实验结果验证了第2组实验中猜想的正确性----DWA算法并不具备“预测”的能力和障