For Stanford’s AA203 Optimal Learning-based Control, I completed the following project regarding Contingency MPC for Quadrotor Collision Avoidance. The full final report can be found here.

Contigency Model Predicitive Control (CMPC) uses at least two parallel prediction horizons within the standard MPC problem. The different prediction horizons account for different contingency plans; however, the first control input of each plan is constrained to be same.

Consider the rudimentary scenario where a controller must plan for a nominal scenario and one contingency scenario. Let \(n\) designate the nominal plan, and let \(c\) designate an emergency contingency plan. As with the typical MPC nomenclature, let \(j_t(\cdot,\cdot)\) denote the stage cost at time \(t\); \(x_t\) the state at time \(t\); \(u_t\), the control input at time \(t\), \(f(\cdot, \cdot)\), the one-step dynamics function; \(\mathcal{X}\), the permissible state-space set; \(\mathcal{U}\), the permissible control set; and \(x_0\), the current estimate of the state. The equations below provide the CMPC formulation. Note the last equation, which constrains the control input of both scenarios to be equal.

\[\begin{align} \label{CMPC formulation} J^*(x_0) = &\min_{u_{1:N}, x_{1:N+1}} \sum_{t=0}^{N} j_t^n (x_t^n, u_t^n) + j_t^c (x_t^c, u_t^c) \\ \text{s.t} \quad & x_{t+1}^n = f^n(x_t^n, u_t^n);\ x_{t+1}^c = f^c(x_t^c, u_t^c) \\ & x_t^n, x_t^c \in \mathcal{X};\ x_t^n \in \mathcal{X}^n;\ x_t^c \in \mathcal{X}^c \\ & u_t^n, u_t^c \in \mathcal{U} \\ & x_0^n = x_0^c = x_0 \\ & u_0^n = u_0^c \label{CMPC formulation end} \end{align}\]

Constant-Velocity Intercept Contingency

Consider the following scenario. A test quadrotor moves from \((0,0,0)\) to \((2,2,0)\) while the other quadrotor moves from \((2,2,0)\) to \((0,0,0)\). Without an avoidance strategy, the two will collide. The following videos compare the performance of two controllers: (1, left) an MPC controller that assumes the other quadrotor will maintain a constant velocity through the MPC horizon and (2, right) a CMPC controller with two contingency horizons: (A) one assuming no other quadrotor present and (B) one assuming the other quadrotor will maintain a constant velocity through the MPC horizon.

In the second scenario, the other quadrotor diverts to \((0,0,-1)\) at time \(t=1\). Without an avoidance strategy, the two will not collide.

The CMPC controller continges the presence of the other quadrotor, as opposed to assuming the presence of the other quadrotor; hence, we expect to observe better control performance both when the other quadrotor follows a collision-course and when it diverts. In both scenarios, the CMPC controller provides a less-perturbed trajectory for both scenarios. Moreover, the CMPC controller causes the test quadrotor to arrive at its goal state faster, as illustrated by the evenly-temporally-spaced dots.

In these tests, we claim that CMPC exploits two cost-reducing measures to provide the superior performance observed. First, CMPC defers acting on the less-ideal (cost increasing) contingency until the last possible moment it can still safely be managed. The difference between the nominal and collision avoidance trajectories encodes uncertainty about whether the other drone will divert. This uncertainty enables the controller to greedily act on the more fortuitous contingency (that it will divert), until it is no longer safe to do so. This is observed as a delay in the diversion of the CMPC controlled quadrotor from the direct path to its goal. Secondly, because CMPC expressively incorporates the nominal trajectory, the controller is ready to exploit the nominal case when the contingency does not happen.

Ballistic Adversarial quadrotor Contingency

Consider the following scenario, where the other quadrotor is potentially ballistic. The test quadrotor attempts to maintain the position \((1,1,0)\) and the adversary quadrotor moves from \((2,0,0)\) to \((0,0,0)\). We compare the performance of two controllers for the test quadrotor: (1, left) an MPC controller that assumes the adversary quadrotor is attempting to collide and (2, right) a CMPC controller with two contingency horizons: (A) one assuming no other quadrotor present and (B) one that assumes an adversarial quadrotor is attempting to collide with the test quadrotor.

Now with the other quadrotor attempting to collide with the test quadrotor.

The CMPC controller continges on a ballistic adversarial quadrotor, as opposed to assuming a ballistic adversarial quadrotor; hence, we expect better performance of the CMPC controller in the presence of both a non-ballistic and a ballistic adversary. The CMPC controller outperforms MPC as it keeps the test quadrotor closer to the goal, even in the presence of a ballistic quadroor. Moreover, the CMPC trajectory’s average deviation from the goal is smaller and the test quadrotor manages to cross its goal state multiple times.

In this test, the CMPC controller is immediately ready to exploit the nominal case cost if the contingency does not occur; hence, the observed performance increase when the other quadrotor is not ballistic. Moreover, the CMPC controller performs better in the ballistic scenario given the extra emphasis on following the nominal scenario.

Comparing the Obstacle Behavior Modeling

The follwing videos shows how the CMPC controller accounts for the two obstacle behaviors modelled. In this experiment, the test quadrotor attempts to maintain a position of \((1,0,0)\). The other quadrotor moves from \((2,0,0)\) to \((0,0,0)\). With a constant velocity contingency (left), the test quadrotor returns to a stationary position after moving itself out of the path of the other quadrotor. With a ballistic contingency (right), the quadrotor returns to its original position, but does not consider a stationary hover safe, so it completes loops that contain its original position. In this looping trajectory, the test quadrotor maintains a safe state from which it can react if the other quadrotor becomes ballistic.