[论文解读] Newton methods for k-order Markov Constrained Motion Problems

Marc Toussaint|arXiv (Cornell University)|Jul 1, 2014

Computational Geometry and Mesh Generation被引用 50

一句话总结

本文提出KOMO框架，一种针对k阶马尔可夫约束运动问题的机器人运动优化方法，采用牛顿类方法。该方法将轨迹优化建模为带约束的最小二乘问题，通过专门设计的矩阵打包方式，有效利用雅可比矩阵的带状结构，实现高斯-牛顿法、增广拉格朗日法和对数障碍法的高效应用，从而在具有运动学与动力学约束的复杂机器人路径规划中实现高计算效率。

ABSTRACT

This is a documentation of a framework for robot motion optimization that aims to draw on classical constrained optimization methods. With one exception the underlying algorithms are classical ones: Gauss-Newton (with adaptive step size and damping), Augmented Lagrangian, log-barrier, etc. The exception is a novel any-time version of the Augmented Lagrangian. The contribution of this framework is to frame motion optimization problems in a way that makes the application of these methods efficient, especially by defining a very general class of robot motion problems while at the same time introducing abstractions that directly reflect the API of the source code.

研究动机与目标

• 开发一种通用且高效的机器人运动优化框架，支持复杂的运动学与动力学约束。
• 使经典约束优化方法（如高斯-牛顿法、增广拉格朗日法和对数障碍法）能够高效应用于高维轨迹优化问题。
• 通过结构化矩阵表示，在保持计算效率的同时，将运动规划问题抽象为语义化、任务级接口。
• 通过清晰的API将问题描述与优化求解器解耦，实现从语义任务到数学优化形式的映射。
• 通过k阶马尔可夫公式支持更高阶导数（如速度、加速度、加加速度）及其约束。

提出的方法

• 将运动优化建模为在k阶状态元组（x_{t-k}, ..., x_t）上的带约束最小二乘问题，支持对动力学和任务代价的灵活建模。
• 直接在配置空间中表示轨迹，而非相空间，从而简化优化公式。
• 引入行偏移矩阵打包表示法，仅存储全局雅可比矩阵J每行的(k+1)n个非零元素，充分利用其带状结构。
• 采用专用算法高效计算J^T J和J^T x，从而在高斯-牛顿法中实现快速海森矩阵近似。
• 采用一种新颖的“随时可用”版本增广拉格朗日法，提升约束优化中的收敛鲁棒性与稳定性。
• 将运动学引擎抽象化，实现运动问题描述与优化求解器的解耦，支持通过自定义任务映射和约束类型进行模块化扩展。

实验结果

研究问题

• RQ1 如何高效地将经典牛顿类优化方法应用于高维、k阶马尔可夫机器人运动规划问题？
• RQ2 轨迹优化中雅可比矩阵的何种结构特性可被利用以实现计算效率的提升？
• RQ3 如何在保持效率与表达力的前提下，将高层语义化任务接口与底层优化求解器解耦？
• RQ4 统一框架能否通过同一优化流程同时支持不等式约束（如碰撞、关节限位）和等式约束（如目标点到达）？
• RQ5 何种设计抽象可实现对新任务映射、运动学引擎和优化算法在运动规划中的可扩展支持？

主要发现

• 通过利用雅可比矩阵的带状结构，该框架将存储与计算成本降低至O((k+1)nT)，实现高计算效率，其中T为时间步数。
• 行偏移矩阵打包表示法可高效计算J^T J和J^T x，这对高斯-牛顿法及其他牛顿类方法至关重要。
• 新颖的“随时可用”增广拉格朗日法提升了收敛鲁棒性，并支持在约束优化中实现增量式进展。
• 运动学引擎的抽象化支持模块化集成自定义任务映射，同时支持碰撞避免与任务空间控制。
• 该框架通过k=2和k=3公式成功建模了更高阶导数（如加速度、加加速度），实现了对转矩和加加速度的最小化。
• 实验评估表明，该框架在无需大量调参的情况下，能高效求解复杂的运动规划问题，包括碰撞避免与末端执行器精确定位。