Semi-Infinite Programming for Collision-Avoidance in Optimal and Model Predictive Control

TL;DR

This paper introduces a semi-infinite programming approach for collision avoidance in optimal and model predictive control, efficiently handling infinite constraints from environment points and uncertainties, demonstrated on real robots and in simulation.

Contribution

It develops a novel semi-infinite programming method combining local reduction and active-set techniques for collision avoidance under uncertainty in real-time control.

Findings

Efficient collision avoidance with real-time control at 20Hz.

Robust handling of translational and rotational uncertainties.

Successful real-world robot navigation in tight spaces.

Abstract

This paper presents a novel approach for collision avoidance in optimal and model predictive control, in which the environment is represented by a large number of points and the robot as a union of padded polygons. The conditions that none of the points shall collide with the robot can be written in terms of an infinite number of constraints per obstacle point. We show that the resulting semi-infinite programming (SIP) optimal control problem (OCP) can be efficiently tackled through a combination of two methods: local reduction and an external active-set method. Specifically, this involves iteratively identifying the closest point obstacles, determining the lower-level distance minimizer among all feasible robot shape parameters, and solving the upper-level finitely-constrained subproblems. In addition, this paper addresses robust collision avoidance in the presence of ellipsoidal state uncertainties. Enforcing constraint satisfaction over all possible uncertainty realizations extends the dimension of constraint infiniteness. The infinitely many constraints arising from translational uncertainty are handled by local reduction together with the robot shape parameterization, while rotational uncertainty is addressed via a backoff reformulation. A controller implemented based on the proposed method is demonstrated on a real-world robot running at 20Hz, enabling fast and collision-free navigation in tight spaces. An application to 3D collision avoidance is also demonstrated in simulation.