[论文解读] Trajectory Optimization on Manifolds: A Theoretically-Guaranteed Embedded Sequential Convex Programming Approach

Riccardo Bonalli, Andrew Bylard+3 more|arXiv (Cornell University)|May 18, 2019

Robotic Path Planning Algorithms被引用 26

一句话总结

该论文提出了一种用于流形上轨迹优化的嵌入式序列凸规划（E-SCP）框架，通过几何嵌入将流形约束问题提升至欧氏空间以实现可计算求解。该方法在理论上保证收敛，且在可行性与最优性方面优于基于惩罚的方法，数值结果表明其代价更低且流形约束误差可忽略不计。

ABSTRACT

Sequential Convex Programming (SCP) has recently gained popularity as a tool for trajectory optimization due to its sound theoretical properties and practical performance. Yet, most SCP-based methods for trajectory optimization are restricted to Euclidean settings, which precludes their application to problem instances where one must reason about manifold-type constraints (that is, constraints, such as loop closure, which restrict the motion of a system to a subset of the ambient space). The aim of this paper is to fill this gap by extending SCP-based trajectory optimization methods to a manifold setting. The key insight is to leverage geometric embeddings to lift a manifold-constrained trajectory optimization problem into an equivalent problem defined over a space enjoying a Euclidean structure. This insight allows one to extend existing SCP methods to a manifold setting in a fairly natural way. In particular, we present a SCP algorithm for manifold problems with refined theoretical guarantees that resemble those derived for the Euclidean setting, and demonstrate its practical performance via numerical experiments.

研究动机与目标

• 为解决缺乏针对流形约束问题（如回环约束或SO(3)动力学）的理论保证轨迹优化方法的问题。
• 通过将流形嵌入欧氏空间并保持问题结构，将序列凸规划（SCP）扩展至非欧几里得设置。
• 确保在隐式定义或局部参数化的流形（如环面或李群）上解的收敛性与可行性。
• 提供一种通用、可靠且计算高效的框架，以处理包括控制边界和目标区域在内的多样化机器人约束。
• 在解的质量与约束满足度方面，证明其相对于基于惩罚的方法具有实际优越性。

提出的方法

• 使用光滑嵌入将流形约束最优控制问题转化为等价的欧氏空间问题，同时保持流形的几何结构。
• 对嵌入后的问题应用序列凸规划（SCP）算法，通过迭代求解由代价和约束的凸近似构造的凸子问题。
• 通过图册（charts）的局部参数化定义嵌入，从而处理隐式约束（如子mersions E的E(x)=0）。
• 在较弱假设下（包括动力学与约束的有界性及Lipschitz连续性），证明E-SCP算法继承标准SCP的收敛性保证。
• 将打靶法与E-SCP结合，通过改进初始轨迹估计加速收敛，使SCP迭代次数减少高达59.4%。
• 通过嵌入精确强制执行流形约束，避免使用惩罚项导致的不可行解或次优代价。

实验结果

研究问题

• RQ1 能否在保持理论收敛保证的前提下，将基于SCP的轨迹优化扩展至流形约束问题？
• RQ2 如何利用几何嵌入将非欧几里得轨迹优化问题转化为适用于SCP的等价欧氏问题？
• RQ3 所提出的E-SCP方法在SO(3)或环面等复杂流形上，是否相比基于惩罚的方法具有更优的解质量与可行性？
• RQ4 在高维或复杂环境中，打靶法在多大程度上可提升E-SCP的收敛速度与解质量？
• RQ5 E-SCP能否在无需显式参数化或启发式修正的情况下，处理隐式定义的流形约束（如E(x)=0）？

主要发现

• 在机械臂实验中，E-SCP实现的真实代价为0.105，显著低于TrajOpt（0.215）与TrajOpt-P（0.792），表明其最优性更优。
• E-SCP的流形约束误差为3.9×10⁻²，与TrajOpt的1.6×10⁻²和TrajOpt-P的3.1×10⁻⁶相比可忽略不计，表明其可行性更优。
• 当与打靶法结合时，E-SCP将SCP迭代次数减少59.4%，平均计算时间从0.466秒降至0.191秒。
• E-SCP的动力学约束误差为1.9×10⁻³，低于TrajOpt（8.5×10⁻³）与TrajOpt-P（4.8×10⁻³），表明其数值精度更优。
• E-SCP的流形约束误差与动力学误差呈近似比例关系，表明其在SO(3)或闭链运动学结构等复杂流形上具有鲁棒性。
• 在SCP因初始值不佳而无法收敛的情况下，E-SCP + 打靶法仍能成功收敛，表明其鲁棒性更强。