基于GPU加速的全局进化动态规划与局部粒子群优化的去中心化多机器人路径规划方法

《Expert Systems with Applications》：GPU-enabled Decentralized, multi-robot path planning based on global evolutionary dynamic programming and local particle swarm optimization

为了解决动态环境中多机器人路径规划实时性差、易陷入局部最优等问题，本研究提出了一种新颖的GPU加速的混合路径规划框架。该方法结合了全局进化动态规划（EDP）与局部粒子群优化（PSO），通过EDP生成多样化初始路径，并利用并行化多群PSO对靠近机器人的局部路径进行实时优化。在移动机器人平台上的实验验证表明，该方法能以约14 Hz的频率连续生成最优路径，显著提升了路径质量和计算速度，为实现高效、鲁棒的去中心化多机器人导航提供了有效方案。

　　

在搜救、监视、物流等诸多现实应用中，让机器人能在复杂、动态的环境中安全、顺畅地自主穿梭，一直是个充满挑战的“卡脖子”难题。传统的路径规划算法，无论是经典的A^*、Dijkstra，还是启发式的粒子群优化（Particle Swarm Optimization, PSO）、遗传算法，或是新兴的基于学习的方法，似乎都难以“十全十美”。它们或受限于计算效率，或在复杂环境中容易“迷路”陷入局部最优，要么就需要漫长的预训练，难以适应快速变化的环境。特别是当多个机器人需要协同工作时，如何在保证各自高效、安全抵达目标的同时，还能避免互相“撞车”，对算法的实时性、鲁棒性和计算效率提出了近乎苛刻的要求。现有的混合方法试图取长补短，但往往依赖于单一初始路径，多样性不足，依然难逃局部最优的“魔咒”。此外，沉重的计算负荷也常常让算法“跑”不起来，难以满足机器人“边思考边行动”的实时需求。为此，美国中田纳西州立大学的Junlin Ou、Ge Song、Jiawei Guo、Yichuan Cao和Yi Wang等研究人员在《Expert Systems with Applications》上发表了一项创新研究，他们提出并验证了一种基于GPU加速、融合全局进化动态规划（Evolutionary Dynamic Programming, EDP）与局部粒子群优化（PSO）的去中心化多机器人路径规划新方法，为解决上述困境提供了全新的思路。

为了开展这项研究，作者团队构建了一套完整的软硬件实验验证系统。关键技术方法包括：1) 环境感知与定位系统：利用三台顶置鱼眼摄像头实时拍摄环境，通过图像拼接、特征点检测与匹配（ORB算法）以及轮廓提取，实时检测并多边形近似化环境中的障碍物；同时，通过识别机器人上的ArUco标记，实现机器人的实时精确定位，整套系统处理一帧图像的平均耗时约为0.06秒。2) 核心路径规划算法：提出了一种七步走的混合规划框架。首先，基于检测到的障碍物和机器人位置生成能见度图（Visibility Graph）；接着，将路径规划问题建模为马尔可夫决策过程（Markov Decision Process, MDP）；然后，利用GPU并行计算能力，执行创新的进化动态规划（EDP）算法，快速生成多条高质量的全局可行路径；随后，将这些路径按规则插入航点，形成用于局部优化的初始路径集；最后，采用并行化的多群PSO，同时对所有初始路径中靠近机器人的前两个航点坐标进行优化，以极快的速度（约14 Hz）选出最优路径。3) 硬件与计算平台：算法部署在机器人搭载的Nvidia Jetson AGX Orin边缘计算设备上运行，实现了算法的板载、去中心化计算。机器人平台采用TurtleBot 3 Waffle Pi，并通过ROS（Robot Operating System）与上位机（笔记本电脑）进行通信，完成环境信息获取、控制指令下发与可视化。

3.1. 问题陈述

研究旨在动态环境中，为单机器人或多机器人找到从起点到目标点的最优路径。最优性由路径长度、平滑度和安全性等多个标准共同定义。机器人需要在移动中根据当前位置、朝向和实时探测到的障碍物（包括移动障碍物）持续更新其航点序列。研究使用了6张不同障碍物布局的地图进行评估，其中两张地图涉及两个机器人同时规划与避障。

3.2. 方法论

提出的混合路径规划算法流程包含七个关键步骤：1) 基于实时视觉感知生成环境与障碍物图；2) 构建包含起点、目标点及扩展后障碍物顶点的能见度图；3) 基于能见度图将路径规划问题表述为MDP；4) 利用GPU并行化的EDP算法求解MDP，生成多条可行的全局路径；5) 根据预设规则为这些可行路径插入航点，形成用于PSO优化的初始路径集，每条路径仅优化其前两个航点（共4个设计变量）；6) 采用并行化的多群PSO同时对所有初始路径进行局部优化，每个PSO种群独立运行在GPU线程上；7) 从所有优化后的路径中，根据适应度函数值选择全局最优路径。该方法结合了EDP的全局多样性与PSO的局部精细搜索能力，并通过将计算密集的EDP和PSO步骤部署在GPU上，实现了高效的实时计算。

3.3. 障碍物检测

障碍物检测通过三台顶置摄像头的图像拼接实现。使用FAST进行关键点检测，BRIEF进行特征描述，结合为ORB算法以提高对图像旋转的鲁棒性。通过计算汉明距离匹配特征并使用RANSAC估计单应性矩阵，实现图像的对齐与融合。拼接后的图像经灰度化、阈值处理后，使用OpenCV检测轮廓，并用Douglas–Peucker算法将轮廓近似为多边形，从而得到以顶点集合表示的障碍物模型。

3.4. 马尔可夫决策过程（MDP）

将路径规划环境建模为一个元组，其中S是机器人位置（x, y）和朝向（θ）定义的所有可能状态集合。目标是最大化从起始状态开始的状态价值。通过公式(1)表达这一目标，其中奖励r_s→g综合考虑了从起始状态到目标状态的路径长度和平滑度。

3.5. 进化动态规划（EDP）

为了解决直接计算MDP状态价值的困难，研究提出了EDP方法。它将整体MDP问题分解为大量可并行求解的子问题（计算每个状态的价值v(s_i)）。每个状态价值的更新依赖于其后续状态的价值（公式(2)），其中奖励r_i→j由路径长度和平滑度构成（公式(3)）。在每次迭代中，所有状态价值的计算被分配到独立的GPU线程上并行执行。状态价值的更新从目标状态开始，逐步向起点传播，直至所有状态价值收敛。收敛后，机器人从起点开始，根据最大状态价值选择下一个状态，即可回溯出从起点到目标点的多条可行路径。论文通过一个简单示例和表格详细说明了EDP迭代更新状态价值并最终收敛、回溯出路径的过程。

3.6. 局部粒子群优化（PSO）

PSO用于对由EDP生成的初始路径进行局部精细化优化。优化变量仅限于路径上靠近机器人的前两个航点的x和y坐标。粒子位置和速度的更新遵循标准PSO公式（公式(4)和(5)）。适应度函数F（公式(6)）是优化的核心，它包含了四个关键评价指标：1) 路径长度L（公式(7)），即所有路径段欧氏距离之和；2) 路径平滑度Φ（公式(8)），定义为路径上所有转弯角（包括起点和终点的朝向与路径段的夹角）之和，其中起点和终点的转向角计算考虑了机器人当前朝向φ和目标朝向α（公式(9)和(10)）；3) 路径安全性惩罚P（公式(11)），对于不可行路径或第三个线段过长的路径施加巨大惩罚（10000），对于可行路径则计算其路径安全值PS；4) 路径安全值PS（公式(12)），通过计算路径前两个线段与环境中所有障碍物所有边之间的最短距离d_i,j,k，并根据距离与安全阈值R（本研究设为0.4米）的关系给予惩罚（公式(13)），距离越近惩罚越大。优化目标是最小化适应度函数F，从而得到长度短、转弯平滑、远离障碍物的优质路径。

4. 实验设置

实验系统由三部分组成：1) 移动机器人平台：TurtleBot 3 Waffle Pi，其计算核心升级为Nvidia Jetson AGX Orin。2) 室内机器人定位系统：通过顶置摄像头识别机器人上的ArUco标记，估计其位置和朝向。3) 障碍物检测系统：与定位系统硬件共享，实时处理图像以检测和定位障碍物。笔记本电脑（CPU处理）负责运行视觉处理算法（定位与检测），并通过Wi-Fi将机器人位姿和障碍物信息发送给机器人。机器人板载的Jetson AGX Orin运行提出的混合路径规划算法，生成最优路径，并通过ROS中的PID控制器驱动机器人跟踪路径。系统实现了感知-规划-控制的闭环。

5. 边缘计算与实现

详细阐述了算法在CPU/GPU异构平台上的实现流程。图像采集、机器人定位、障碍物检测等步骤在笔记本电脑CPU上执行。获得的机器人位姿和障碍物信息通过Wi-Fi发送至机器人的Jetson AGX Orin。随后，障碍物信息从Orin的CPU传输至GPU。在GPU上并行执行能见度图计算、MDP建模、EDP求解（状态价值更新）以及并行多群PSO优化等计算密集型任务。CPU负责协调流程，如确定初始路径数量，并将最终从GPU得到的多条优化路径中选择全局最优路径。整个流程在机器人移动过程中持续实时运行。

6. 超参数优化

为确定PSO的最佳超参数（惯性权重ω、认知系数c₁、社会系数c₂），研究在前四张地图上进行了大量测试。结果表明，社会系数c₂对算法性能影响最显著。当c₂= 0时性能最差，粒子间缺乏信息共享；c₂在1附近时，平均适应度值最低（性能最好），标准偏差也较小，表明算法稳定；c₂过大则导致粒子过早聚集，易陷入局部最优。惯性权重ω和认知系数c₁的影响相对较小。综合四张地图的表现，研究最终选定的PSO超参数为：ω = 0.5, c₁= 0.5, c₂= 1.0。

7. 实验与对比分析

在六张不同复杂度（包括单/双机器人场景）的地图上，将所提出的EDP-PSO方法与多种基准算法进行了全面对比，包括标准PSO、遗传算法（GA）、蚁群优化（ACO）、A^*算法以及两种混合算法（APSO: A^*-PSO；IRRT^*-PSO）。评价指标包括路径长度、平滑度、安全性、计算时间和成功率。结果表明：

• •
  路径质量：在绝大多数地图上，EDP-PSO规划出的路径在长度、平滑度和安全性上均优于或与其他最佳算法持平。
• •
  计算速度：得益于GPU的并行加速，EDP-PSO的计算时间显著低于其他启发式算法（GA, ACO, PSO），与A^*算法相当，但路径质量更优。在Jetson AGX Orin上，算法单次规划周期平均耗时约0.07秒，即可达到约14 Hz的规划频率。
• •
  成功率与鲁棒性：在复杂地图和动态障碍物场景下，EDP-PSO的成功率（找到可行路径的比例）达到100%，而其他一些算法（如标准PSO）在某些地图上成功率较低。EDP提供的多样化初始路径有效增强了PSO的全局探索能力，避免了局部最优。
• •
  多机器人验证：在双机器人同时运行的地图（Map 5和Map 6）上，EDP-PSO成功为每个机器人规划出无碰撞路径，并完成了点到点导航任务，验证了其去中心化多机器人路径规划的有效性。

8. 结论

本研究成功提出并验证了一种基于GPU加速的、融合全局进化动态规划（EDP）与局部粒子群优化（PSO）的混合路径规划算法，用于动态环境中的去中心化多机器人导航。该研究的主要贡献与意义在于：

第一，框架创新：创造性地将生成多样化全局初始路径的EDP与进行局部精细化搜索的PSO相结合，构建了一个高效的混合规划框架。使用多条而非单条初始路径初始化PSO，显著增强了种群多样性，提升了算法摆脱局部最优的能力。

第二，性能卓越：通过并行化的多群PSO设计与GPU的全面赋能，算法在保证高质量路径（短、平滑、安全）的同时，实现了远超传统启发式算法的计算速度，满足了移动机器人实时规划的需求（约14 Hz）。

第三，系统验证：研究不仅进行了仿真对比，更构建了集成实时视觉感知、定位、板载边缘计算与机器人控制的完整实验系统，在实际移动机器人（TurtleBot 3 Waffle Pi）上验证了算法的可行性与有效性。实验证明了该算法在静态和动态环境中，对于单机器人和多机器人场景，均能实现高效、鲁棒的导航。

综上所述，这项工作为解决动态环境下实时、鲁棒的多机器人路径规划问题提供了一种高效实用的解决方案，其软硬件协同的设计思路与显著的性能提升，对自主移动机器人、智能仓储、无人驾驶等领域的实际应用具有重要的借鉴和推动作用。