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 in an effective way 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 works 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 use 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）。该软件框架有望鼓励并推动机器人领域在更具挑战性的应用中使用轨迹优化。