KRRF: Kinodynamic Rapidly-exploring Random Forest algorithm for multi-goal motion planning
作者: Petr Ježek, Michal Minařík, Vojtěch Vonásek, Robert Pěnička
分类: cs.RO
发布日期: 2025-05-09
期刊: IEEE Robotics and Automation Letters, vol. 9, no. 12, pp. 10724-10731, Dec. 2024
💡 一句话要点
提出KRRF算法，解决运动学约束下多目标点运动规划问题
🎯 匹配领域: 支柱一：机器人控制 (Robot Control)
关键词: 运动规划 多目标点规划 运动学约束 RRT 旅行商问题
📋 核心要点
- 多目标点运动规划问题结合了TSP和运动学约束，求解难度高，现有方法效率较低。
- KRRF算法同时从所有目标点扩展运动学树，并利用其他树作为启发式信息加速搜索。
- 实验结果表明，KRRF在轨迹长度和计算效率上优于现有方法，成本降低1.1-2倍。
📝 摘要（中文）
本文提出了一种名为运动学快速探索随机森林(KRRF)的近似方法，用于解决具有运动学约束的多目标点运动规划问题，该问题旨在找到一条在复杂环境中访问多个目标位置的轨迹，且访问顺序事先未知，目标是最小化轨迹成本。由于该问题结合了旅行商问题(TSP)和运动学运动规划这两个NP-hard问题，因此高效求解一直具有挑战性。KRRF同时从所有目标点向所有其他目标点增长运动学树，并利用其他树作为启发式来加速增长。一旦规划出目标点到目标点的轨迹，其成本就被用于解决TSP以找到目标点的访问顺序。最后，通过沿着TSP序列中目标点到目标点的轨迹引导基于RRT的规划器，来规划满足运动学约束的最终多目标点轨迹。与现有方法相比，KRRF提供了更短的目标点到目标点轨迹和最终多目标点轨迹，成本降低了1.1-2倍，并且在大多数测试用例中计算速度更快。该方法将作为开源库发布。
🔬 方法详解
问题定义：论文旨在解决具有运动学约束的多目标点运动规划问题。该问题需要在已知多个目标点的情况下，找到一条满足机器人运动学约束的、访问所有目标点的最优轨迹，且目标点的访问顺序事先未知。现有方法通常难以在复杂环境中找到高质量的轨迹，并且计算效率较低，难以满足实时性要求。
核心思路：KRRF的核心思路是将多目标点运动规划问题分解为两个子问题：目标点到目标点的轨迹规划和目标点访问顺序的优化。首先，通过同时从所有目标点扩展运动学树，快速找到目标点之间的可行轨迹。然后，利用这些轨迹的成本作为TSP问题的输入，确定最优的目标点访问顺序。最后，沿着TSP确定的顺序，引导RRT规划器生成最终的运动学可行轨迹。
技术框架：KRRF算法主要包含以下几个阶段： 1. 目标点间轨迹规划：同时从每个目标点出发，使用RRT算法向其他目标点扩展运动学树。每个树的扩展都受到其他树的启发，从而加速搜索过程。 2. TSP求解：利用目标点间轨迹的成本，构建TSP问题，并使用现有的TSP求解器（如LKH）找到最优的目标点访问顺序。 3. 最终轨迹生成：沿着TSP确定的目标点访问顺序，使用RRT算法连接目标点间的轨迹，生成满足运动学约束的完整轨迹。
关键创新：KRRF的关键创新在于同时从所有目标点扩展运动学树，并利用其他树作为启发式信息。这种方法能够更有效地探索搜索空间，找到更短的目标点间轨迹。此外，将运动规划问题与TSP问题解耦，使得可以分别优化轨迹的几何形状和目标点的访问顺序。
关键设计：KRRF算法的关键设计包括： 1. 启发式函数：在RRT扩展过程中，使用其他树的最近节点作为启发式信息，引导树向目标点方向生长。 2. TSP求解器：可以使用不同的TSP求解器，如LKH、Concorde等，根据问题的规模和精度要求进行选择。 3. RRT参数：RRT算法的步长、采样策略等参数需要根据具体的机器人运动学模型和环境进行调整。
🖼️ 关键图片
📊 实验亮点
实验结果表明，KRRF算法在多个测试用例中优于现有的方法。与现有方法相比，KRRF能够生成更短的目标点到目标点轨迹和最终多目标点轨迹，成本降低了1.1-2倍。此外，KRRF在大多数测试用例中计算速度更快，表明其具有更高的计算效率。这些结果验证了KRRF算法在解决运动学约束下多目标点运动规划问题上的有效性。
🎯 应用场景
KRRF算法可应用于各种需要多目标点运动规划的场景，例如：物流配送机器人需要在仓库中访问多个货物点；无人机需要在城市中巡检多个目标地点；清洁机器人在房间内清扫多个区域。该算法能够提高机器人的工作效率，降低能源消耗，并增强其在复杂环境中的适应性。未来，KRRF可以进一步扩展到动态环境和多机器人系统。
📄 摘要（原文）
The problem of kinodynamic multi-goal motion planning is to find a trajectory over multiple target locations with an apriori unknown sequence of visits. The objective is to minimize the cost of the trajectory planned in a cluttered environment for a robot with a kinodynamic motion model. This problem has yet to be efficiently solved as it combines two NP-hard problems, the Traveling Salesman Problem~(TSP) and the kinodynamic motion planning problem. We propose a novel approximate method called Kinodynamic Rapidly-exploring Random Forest~(KRRF) to find a collision-free multi-goal trajectory that satisfies the motion constraints of the robot. KRRF simultaneously grows kinodynamic trees from all targets towards all other targets while using the other trees as a heuristic to boost the growth. Once the target-to-target trajectories are planned, their cost is used to solve the TSP to find the sequence of targets. The final multi-goal trajectory satisfying kinodynamic constraints is planned by guiding the RRT-based planner along the target-to-target trajectories in the TSP sequence. Compared with existing approaches, KRRF provides shorter target-to-target trajectories and final multi-goal trajectories with $1.1-2$ times lower costs while being computationally faster in most test cases. The method will be published as an open-source library.