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

Yulin Li, Chunxin Zheng, Kai Chen, Yusen Xie, Xindong Tang, Michael Yu Wang, Jun Ma*

*Corresponding author for this work

Research output: Contribution to journalJournal articlepeer-review

Abstract

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.

Original languageEnglish
Pages (from-to)11026-11033
Number of pages8
JournalIEEE Robotics and Automation Letters
Volume9
Issue number12
DOIs
Publication statusPublished - Dec 2024

Scopus Subject Areas

  • Control and Systems Engineering
  • Biomedical Engineering
  • Human-Computer Interaction
  • Mechanical Engineering
  • Computer Vision and Pattern Recognition
  • Computer Science Applications
  • Control and Optimization
  • Artificial Intelligence

User-Defined Keywords

  • Collision avoidance
  • constrained motion planning
  • optimization and optimal control

Fingerprint

Dive into the research topics of 'Collision-Free Trajectory Optimization in Cluttered Environments Using Sums-of-Squares Programming'. Together they form a unique fingerprint.

Cite this