PWTO: A Heuristic Approach for Trajectory Optimization in Complex Terrains

📄 arXiv: 2407.02745v1 📥 PDF

作者: Yilin Cai, Zhongqiang Ren

分类: cs.RO

发布日期: 2024-07-03

🔗 代码/项目: GITHUB

💡 一句话要点

PWTO：一种复杂地形下机器人轨迹优化的启发式方法

🎯 匹配领域: 支柱一：机器人控制 (Robot Control)

关键词: 轨迹优化 复杂地形 机器人导航 图搜索 多目标优化

📋 核心要点

  1. 复杂地形下的机器人轨迹规划面临非线性优化难题，易陷入局部最优解，现有方法难以兼顾全局探索和局部优化。
  2. PWTO方法结合图搜索的全局性和轨迹优化的精确性，利用多目标图搜索生成候选路径，并温启动局部轨迹优化。
  3. 实验结果表明，PWTO显著降低了轨迹成本，优于基线方法，并在Gazebo仿真中验证了其在轮式和四足机器人上的有效性。

📝 摘要（中文）

本文研究了机器人在复杂地形中导航的轨迹规划问题，该问题常见于自主采矿车辆和行星探测车等应用中。问题的目标是为机器人找到一条低成本且动力学可行的轨迹。由于复杂地形通常导致非线性优化问题存在许多局部最小值，因此该问题具有挑战性。为了解决这一挑战，我们提出了一种名为Pareto最优温启动轨迹优化（PWTO）的方法，该方法试图结合图搜索和轨迹优化这两种截然不同的规划方法的优点。PWTO首先利用机器人简化的动力学模型创建一个状态格，并利用多目标图搜索方法获得一组路径。然后，每条路径都被用于温启动局部轨迹优化过程，以便探索不同的局部最小值，从而找到全局低成本的解决方案。在我们的测试中，PWTO计算出的解决方案成本通常低于基线成本的一半。此外，我们在Gazebo仿真中验证了PWTO生成的轨迹在复杂地形中对轮式和四足机器人的适用性。本文的代码已开源，可在https://github.com/rap-lab-org/public_pwto找到。

🔬 方法详解

问题定义：论文旨在解决复杂地形下机器人轨迹规划问题，目标是找到一条低成本且动力学可行的轨迹。现有方法，如纯粹的轨迹优化，容易陷入局部最优解，而图搜索方法则难以保证轨迹的动力学可行性和精确性。因此，需要一种能够兼顾全局探索和局部优化的方法。

核心思路：PWTO的核心思路是将图搜索和轨迹优化相结合。首先利用图搜索快速生成一组候选路径，这些路径覆盖了不同的地形区域，从而实现全局探索。然后，使用这些路径作为初始猜测（温启动）来运行局部轨迹优化，从而在每个候选路径附近寻找更优的局部解。通过这种方式，PWTO能够有效地探索不同的局部最小值，并找到全局低成本的解决方案。

技术框架：PWTO的整体框架包含以下几个主要阶段：1) 状态格构建：使用机器人简化的动力学模型构建状态格，用于图搜索。2) 多目标图搜索：利用多目标图搜索算法在状态格中搜索一组Pareto最优路径，这些路径在不同的优化目标（如路径长度、地形难度等）之间进行权衡。3) 温启动轨迹优化：将每条Pareto最优路径作为初始猜测，运行局部轨迹优化算法，例如iLQR或SQP，以寻找更优的局部解。4) 选择最优轨迹：从所有优化后的轨迹中选择成本最低的轨迹作为最终结果。

关键创新：PWTO的关键创新在于将多目标图搜索与温启动轨迹优化相结合。传统方法通常只使用单一的图搜索结果或随机初始猜测来启动轨迹优化，而PWTO利用多目标图搜索生成一组多样化的候选路径，从而提高了找到全局最优解的可能性。此外，使用温启动策略可以加速轨迹优化的收敛速度，并避免陷入较差的局部最小值。

关键设计：在状态格构建阶段，需要选择合适的离散化步长，以保证状态格的覆盖范围和搜索效率。在多目标图搜索阶段，需要定义合适的优化目标和权重，以生成多样化的Pareto最优路径。在温启动轨迹优化阶段，需要选择合适的优化算法和参数，以保证轨迹优化的收敛性和稳定性。此外，还需要设计合适的成本函数，以衡量轨迹的质量，例如路径长度、能量消耗、地形难度等。

🖼️ 关键图片

fig_0
fig_1
fig_2

📊 实验亮点

实验结果表明，PWTO方法在复杂地形下的轨迹规划中表现出色。与基线方法相比，PWTO计算出的解决方案成本通常低于基线成本的一半。此外，PWTO生成的轨迹在Gazebo仿真中得到了验证，证明了其在轮式和四足机器人上的有效性。这些结果表明，PWTO是一种高效且实用的复杂地形轨迹规划方法。

🎯 应用场景

PWTO方法适用于各种需要在复杂地形中进行自主导航的机器人应用，例如自主采矿车辆、行星探测车、搜索救援机器人等。该方法可以提高机器人在复杂环境中的导航效率和安全性，降低运营成本，并扩展机器人的应用范围。未来，PWTO可以进一步扩展到多机器人协同导航、动态环境下的轨迹规划等更复杂的场景。

📄 摘要（原文）

This paper considers a trajectory planning problem for a robot navigating complex terrains, which arises in applications ranging from autonomous mining vehicles to planetary rovers. The problem seeks to find a low-cost dynamically feasible trajectory for the robot. The problem is challenging as it requires solving a non-linear optimization problem that often has many local minima due to the complex terrain. To address the challenge, we propose a method called Pareto-optimal Warm-started Trajectory Optimization (PWTO) that attempts to combine the benefits of graph search and trajectory optimization, two very different approaches to planning. PWTO first creates a state lattice using simplified dynamics of the robot and leverages a multi-objective graph search method to obtain a set of paths. Each of the paths is then used to warm-start a local trajectory optimization process, so that different local minima are explored to find a globally low-cost solution. In our tests, the solution cost computed by PWTO is often less than half of the costs computed by the baselines. In addition, we verify the trajectories generated by PWTO in Gazebo simulation in complex terrains with both wheeled and quadruped robots. The code of this paper is open sourced and can be found at https://github.com/rap-lab-org/public_pwto.