  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. To address the robot navigation
task in obstacle-dense environments, we exploit the free space directly to
construct a sequence of free regions, 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 that renders the containment relationship between the robot and the
free space computationally tractable. 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. In this context, the robot at a specific configuration is
tailored to stay within the free region. Next, to solve the trajectory
optimization problem with the proposed safety constraints (which are implicitly
dependent on the robot configurations), we derive the analytical solution to
the gradient of the minimum scaling factor with respect to the robot
configuration. 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 populated with
obstacles.
