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 Lesser General Public License(LGPL)下发布。这个软件框架可能鼓励和使机器人学界在更具挑战性的应用中使用轨迹优化。