Trajectory optimization is a powerful tool for robot motion planning and control. State-of-the-art general-purpose nonlinear programming solvers are versatile, handle constraints effectively and provide a high numerical robustness, but they are slow because they do not fully exploit the optimal control problem structure at hand. Existing structure-exploiting solvers are fast, but they often lack techniques to deal with nonlinearity or rely on penalty methods to enforce (equality or inequality) path constraints. This work presents Fatrop: a trajectory optimization solver that is fast and benefits from the salient features of general-purpose nonlinear optimization solvers. The speed-up is mainly achieved through the integration of a specialized linear solver, based on a Riccati recursion that is generalized to also support stagewise equality constraints. To demonstrate the algorithm's potential, it is benchmarked on a set of robot problems that are challenging from a numerical perspective, including problems with a minimum-time objective and no-collision constraints. The solver is shown to solve problems for trajectory generation of a quadrotor, a robot manipulator and a truck-trailer problem in a few tens of milliseconds. The algorithm's C++-code implementation accompanies this work as open source software, released under the GNU Lesser General Public License (LGPL). This software framework may encourage and enable the robotics community to use trajectory optimization in more challenging applications.

翻译：轨迹优化是机器人运动规划与控制的有力工具。现有的通用非线性规划求解器具有通用性强、能有效处理约束且数值鲁棒性高的特点，但由于未能充分利用最优控制问题的特定结构，导致求解速度较慢。而现有利用结构特性的求解器虽快，但常缺乏处理非线性的技术，或依赖惩罚函数法来强制执行（等式或不等式）路径约束。本文提出Fatrop：一种兼具快速性与通用非线性优化求解器核心优势的轨迹优化求解器。其加速能力主要源于集成了一种基于广义Riccati递推的特殊线性求解器，该求解器同时支持逐阶段等式约束。为展示该算法的潜力，我们在多类数值求解难度较高的机器人问题上进行基准测试，包括最小时间目标和无碰撞约束问题。实验表明，该求解器可在数十毫秒内解决四旋翼飞行器、机械臂及卡车-拖车系统的轨迹生成问题。本工作随附的算法C++代码实现以开源软件形式发布，遵循GNU宽通用公共许可证（LGPL）。该软件框架有望激励并支持机器人社区在更具挑战性的应用中运用轨迹优化技术。