[论文解读] Improved Path Planning by Tightly Combining Lattice-based Path Planning and Numerical Optimal Control

Kristoffer Bergman, Oskar Ljungqvist+1 more|arXiv (Cornell University)|Mar 19, 2019

Robotic Path Planning Algorithms参考文献 26被引用 4

一句话总结

本文提出了一种紧密耦合的两阶段路径规划方法，结合基于格栅的采样与数值最优控制，以实现局部最优且动力学可行的轨迹。通过将格栅规划器的组合优化解作为数值最优控制求解器的热启动——使用相同的动力学模型和目标函数——该方法相比以往的初始化策略，显著提升了收敛速度、数值可靠性与解的质量。

ABSTRACT

This paper presents a unified optimization-based path planning approach to efficiently compute locally optimal solutions to advanced path planning problems. The approach is motivated by first showing that a lattice-based path planner can be cast and analyzed as a bilevel optimization problem. This information is then used to tightly integrate a lattice-based path planner and numerical optimal control in a novel way. The lattice-based path planner is applied to the problem in a first step using a discretized search space, where system dynamics and objective function are chosen to coincide with those used in a second numerical optimal control step. As a consequence, the lattice planner provides the numerical optimal control step with a resolution optimal solution to the problem, which is highly suitable as a warm-start to the second step. This novel tight combination of a sampling-based path planner and numerical optimal control makes, in a structured way, benefit of the former method's ability to solve combinatorial parts of the problem and the latter method's ability to obtain locally optimal solutions not constrained to a discretized search space. Compared to previously presented combinations of sampling-based path planners and optimization, the proposed approach is shown in several path planning experiments to provide significant improvements in terms of computation time, numerical reliability, and objective function value.

研究动机与目标

• 解决在复杂环境中为非完整约束车辆计算局部最优、动力学可行路径的挑战。
• 克服传统基于采样的规划器因离散化搜索空间而导致次优解的局限性。
• 通过提供高质量、可行且与目标函数对齐的热启动，提升数值最优控制的收敛性能。
• 通过双层优化公式统一基于格栅的规划与数值最优控制，揭示格栅近似解的次优性。
• 证明在规划与优化阶段使用相同目标函数可提升收敛性与解的质量。

提出的方法

• 将路径规划问题建模为双层优化问题，以分析原始最优控制问题与其基于格栅的近似解之间的关系。
• 使用具有离散化搜索空间的基于格栅的路径规划器，并采用与后续数值最优控制步骤中相同的车辆动力学模型和目标函数。
• 通过图搜索生成分辨率最优解，其运动基元基于与最优控制阶段相同的系统模型和代价函数。
• 应用数值最优控制（NOC）对格栅解进行精细化处理，优化连续轨迹参数，同时保持组合结构（如路径拓扑）。
• 通过在两个阶段中对齐系统动力学、控制约束和目标函数，实现紧密耦合，以确保NOC求解器的可靠且快速收敛。
• 在NOC阶段使用格栅解作为热启动，利用其可行性与近似最优性，加速收敛并避免陷入局部极小值。

实验结果

研究问题

• RQ1 双层优化公式能否有效分析并量化基于格栅的路径规划的次优性？
• RQ2 在NOC步骤中使用与格栅规划器相同动力学和目标函数时，是否能提升最优控制的收敛性与解的质量？
• RQ3 在格栅规划器与NOC阶段对齐目标函数，对计算时间与数值可靠性有何影响？
• RQ4 与传统初始化策略（如几何或运动学模型）相比，该方法在解的质量与收敛性方面表现如何？
• RQ5 采样规划与数值优化的紧密耦合是否能减少总规划时间，同时提升目标函数值？

主要发现

• 与使用几何或运动学初始化相比，该方法将总计算时间减少了最多40%，当两个阶段使用相同目标函数时，收敛速度最快。
• 在装卸站点场景中，该方法在270个测试用例中实现了100%的收敛率，显著优于其他初始化策略。
• 当格栅规划器使用与NOC步骤相同的目标函数（γu = γl = γi = 10）时，平均改进时间为9.4秒，成功率达到100%，目标函数值为190。
• 若格栅阶段使用不同目标函数（如γu = 0），总时间增至14.5秒，目标函数值升至212，表明初始化质量差且收敛更慢。
• 在装卸站点场景中，使用基于动态模型的初始化（P_dyn）时，方法的改进率达到100%，而使用几何初始化时仅为43.7%。
• 在最终NOC步骤中，解的代价（J）从几何初始化的236降至运动学初始化的164，最终也保持在164（动态初始化），表明初始化质量越高，优化效果越一致。