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

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.

Figures (17)

• Figure 1: Method overview: (a) The robot is driven by an SIP-based robust MPC controller running at 20 Hz. (b) State uncertainty sets are modeled as ellipsoids and are updated in a zero-order scheme. The green tube on the right shows the union of the occupied space of the uncertain robot over all discrete time steps. (c) The environment is modeled with sampled points over the obstacle surfaces (black points). For each time step $k$, a subset of critical points is maintained (colored triangles). (d) The uncertainties and the robot-shape parameterization lead to infinitely many constraints, which are locally reduced to one linearized constraint per critical point, based on the lower-level maximizer (colored cross). The algorithm iterates through the steps in (b)--(d).
• Figure 2: Robot shapes considered in this paper.
• Figure 3: Illustration of Algorithm \ref{['alg:nominal-collision-free-trajectory']}. For clarity, only the robot state and the obstacle subset at one time step $k$ are plotted. The closest obstacles are identified among all point obstacles (see the orange dots), and the obstacle subset $\mathcal{O}_{\mathrm{s}, k}^{(j)}$ is updated (see the orange triangles). For each obstacle $p_{\mathrm{o}} \in \mathcal{O}_{\mathrm{s}, k}^{(j)}$, the lower-level optimization problem \ref{['eq:simple-example-lower-level']} is solved, yielding the lower-level maximizer $\gamma_{\mathrm{shp}}^{*}\!\left(x^{(j)}_k;p_{\mathrm{o}}\right)$ and one linearized constraint is imposed, which can be interpreted as a plane (see the brown dash-dotted lines and the brown arrows) to separate the obstacle and the circle corresponding to the maximizer. The procedure is repeated until convergence.
• Figure 4: Four world maps used for numerical evaluation. The dashed lines are the reference paths. The green tube represents the union of the occupied space across all discrete time steps, as given by the uncertainty model in \ref{['eq:robust-coll-constr-separateRotPos']} and evaluated using a converged OCP solution. The resolution of the point obstacles is 0.02 m. See Table \ref{['table:mobile-robot-config-split']} for the number of points in each environment.
• Figure 5: Optimal control of a mobile robot: The percentages of the OCPs that converge within a certain number of iterations. The radius of the padding circle $r_{\mathrm{shp}}$ is varied. The reference trajectories are carefully chosen such that the collision-avoidance constraints are active for different $r_{\mathrm{shp}}$.

Theorems & Definitions (14)

• Definition 1: Nondegenerate local solution
• Lemma 2
• proof
• Remark 3
• Remark 4
• Remark 5
• Remark 6
• Remark 7
• Lemma 8
• proof