Logo
ScienceStack
Table of Contents
Fetching ...
Sign In

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

TL;DR

The paper addresses collision-free trajectory optimization for robots with general semialgebraic geometries in cluttered 3D spaces. It decomposes free space into polytopic regions and uses SOS programming to compute a minimum scaling factor that certifies containment of the robot within scaled free regions, with the gradient of this factor enabling gradient-based optimization. By embedding these implicit safety constraints into an AL-iLQR framework, the method delivers fast, reliable convergence to collision-free trajectories, demonstrated across diverse shapes, mazes, and a real quadruped. The approach provides geometry-aware safety guarantees without excessive obstacle dilation, offering practical impact for safe autonomy in dense environments. The codebase is available online and the method shows strong potential for real-time planning on resource-constrained platforms.

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. 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. Our code is available at: https://github.com/lyl00/minimum_scaling_free_region

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

TL;DR

The paper addresses collision-free trajectory optimization for robots with general semialgebraic geometries in cluttered 3D spaces. It decomposes free space into polytopic regions and uses SOS programming to compute a minimum scaling factor that certifies containment of the robot within scaled free regions, with the gradient of this factor enabling gradient-based optimization. By embedding these implicit safety constraints into an AL-iLQR framework, the method delivers fast, reliable convergence to collision-free trajectories, demonstrated across diverse shapes, mazes, and a real quadruped. The approach provides geometry-aware safety guarantees without excessive obstacle dilation, offering practical impact for safe autonomy in dense environments. The codebase is available online and the method shows strong potential for real-time planning on resource-constrained platforms.

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. 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. Our code is available at: https://github.com/lyl00/minimum_scaling_free_region
Paper Structure (16 sections, 1 theorem, 17 equations, 6 figures, 2 tables, 1 algorithm)

This paper contains 16 sections, 1 theorem, 17 equations, 6 figures, 2 tables, 1 algorithm.

Table of Contents

  1. Introduction
  2. Related Works
  3. PROBLEM STATEMENT
  4. Methodology
  5. Waypoints Allocation on Graph of Free Regions
  6. Safety Constraints Formulation with SOS Programming
  7. Set Representation with Polynomial Functions
  8. Certified Containment Relationship with Minimum Scaling
  9. Collision-Free Trajectory Optimization with Implicit Safety Constraints
  10. Results
  11. Simulations
  12. Algorithm Validation for Various Robot Geometries
  13. Traversing in Mazes
  14. Comparisons
  15. Real-World Experiment
  16. ...and 1 more sections

Key Result

Theorem 1

putinar1993positive Suppose $^b\mathcal{W}_B$ is ArchimedeanThe set $^b\mathcal{W}_B$ is said to be Archimedean if there exists $\rho\in \hbox{Qmod}[f]$ such that the set given by $\rho(x)\ge 0$ is compact. Note that if $^b\mathcal{W}_B$ is compact, then one can always choose a sufficiently large nu

Figures (6)

  • Figure 1: Illustration of the waypoints allocation process on the constructed graph of free regions $\mathcal{G}$. (a) The free space $\mathcal{F}$ is decomposed into series of overlapping polytopic regions $\mathcal{Q}$ (the light green polytopic regions). (b) A sequence of the free regions connecting $\boldsymbol{q}_s$ (the red square) and $\boldsymbol{q}_g$ (the green star) with minimal path cost is then searched and stored in $\mathbb{P}^{*}_{v}$ (emphasized in dark green). (c) Waypoints are allocated to specific free regions along the path connecting the edge between each pair of regions in $\mathbb{P}^{*}_{v}$. (d) Extra regions are generated at the geometrical center of particular overlapped area and the waypoints in each appended region is reallocated for smooth transition.
  • Figure 2: Geometrical interpretation of the proposed minimum scaling problem. We aim to find the minimum scaling factor $\alpha$ for the polytopic free region ${}^q\mathcal{Q}_{\tau}$ such that the robots $A_\textit{1}$ ($\alpha\leq1$) and $A_\textit{2}$ ($\alpha \geq 1$) are contained in the scaled region.
  • Figure 3: Visualization of the optimized trajectories for robots with different geometries. The robots are commanded to move into the free region (shown in grey) from random start configurations by enforcing our derived safety constraints at the terminal time stamp.
  • Figure 4: Performance of our proposed trajectory optimization framework in maze traversing tasks. (a-c) Three maze scenarios with different degrees of complexity. The start and goal positions are denoted by red and green nodes, respectively. (d-f) Visualization of the collision-free trajectories generated in the graph of free regions $\mathcal{G}$ with keyframes highlighted.
  • Figure 5: Comparsion results in a 5 m$\times$10 m cluttered environment. A 0.2 m$\times$ 0.4 m polytopic robot is commanded to move from initial (red node) to goal configuration (green node). (a) Visualization of the search process of Kinodynamic RRT*. It fails to find a feasible trajectory through the narrow passages (highlighted) within tractable time (100 s). (b-c) Optimized trajectories of TrajOpt and CBF-Polytopes. They fail to push the robot away from all the obstacles simultaneously within the optimization horizon, and the collision areas are highlighted. (d-e) Visualization of the generated free regions and the planned trajectories of Safe-Corridor and our proposed method. Both approaches generate safe trajectories by decomposing free space. However, Safe-Corridor searches for paths and extracts free regions based on obstacle information that is dilated by the robot's maximum side size. This method fails to find the nearest traversable path. Additionally, as highlighted by the gaps between the generated free regions and the obstacles in (d), the free regions do not cover enough of the available free space, resulting in a conservative trajectory.
  • ...and 1 more figures

Theorems & Definitions (1)

  • Theorem 1