We present a semi-infinite program (SIP) solver for trajectory optimizations of general articulated robots. These problems are more challenging than standard Nonlinear Program (NLP) by involving an infinite number of non-convex, collision constraints. Prior SIP solvers based on constraint sampling cannot guarantee the satisfaction of all constraints. Instead, our method uses a conservative bound on articulated body motions to ensure the solution feasibility throughout the optimization procedure. We further use subdivision to adaptively reduce the error in conservative motion estimation. Combined, we prove that our SIP solver guarantees feasibility while approaches the critical point of SIP problems up to arbitrary user-provided precision. We have verified our method on a row of trajectory optimization problems involving industrial robot arms and UAVs, where our method can generate collision-free, locally optimal trajectories within a couple minutes.

翻译：我们提出了一种适用于通用关节机器人轨迹优化的半无限规划求解器。这类问题因涉及无限个非凸碰撞约束而比标准非线性规划更具挑战性。现有基于约束采样的半无限规划求解器无法保证满足所有约束。相反，我们的方法采用保守的关节体运动界来确保优化过程中解的可行性。我们进一步利用细分技术自适应减小保守运动估计的误差。综合而言，我们证明所提出的半无限规划求解器能保证解可行，同时可逼近至任意用户指定精度的半无限规划问题的临界点。我们已在涉及工业机械臂和无人机的一系列轨迹优化问题上验证了该方法，该方法能在数分钟内生成无碰撞的局部最优轨迹。