Collision-Free Trajectory Optimization in Cluttered Environments Using Sums-of-Squares Programming

In this work, we propose a trajectory optimization approach for robot navigation in cluttered 3D environments. We represent the robot's geometry as a semialgebraic set defined by polynomial inequalities such that robots with general shapes can be suitably characterized. We exploit the collision-free space directly to construct a graph of free regions, search for the reference path, and allocate each waypoint on the trajectory to a specific region. Then, we incorporate a uniform scaling factor for each free region and formulate a Sums-of-Squares (SOS) optimization problem whose optimal solutions reveal the containment relationship between robots and the free space. The SOS optimization problem is further reformulated to a semidefinite program (SDP), and the collision-free constraints are shown to be equivalent to limiting the scaling factor along the entire trajectory. Next, to solve the trajectory optimization problem with the proposed safety constraints, we derive a guiding direction for updating the robot configuration to decrease the minimum scaling factor by calculating the gradient of the Lagrangian at the primal-dual optimum of the SDP. As a result, this seamlessly facilitates the use of gradient-based methods in efficient solving of the trajectory optimization problem. Through a series of simulations and real-world experiments, the proposed trajectory optimization approach is validated in various challenging scenarios, and the results demonstrate its effectiveness in generating collision-free trajectories in dense and intricate environments.