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）发布。该软件框架有望激励并推动机器人学界将轨迹优化应用于更具挑战性的场景。