Mixed-Integer MPC for Closed-Loop Motion Planning and Flight Control of a Laboratory Helicopter

Abstract

This work addresses the problem of planning and tracking collision-free trajectories with a laboratory helicopter restrained to three degrees of freedom (DOF) using model predictive control (MPC) with mixed-integer programming (MIP) encoding. We propose a multirate structure that divides the system dynamics into slower and faster modes. The MPC-MIP is then employed to directly control only the slower portion, allowing it to operate in a closed loop in spite of its relatively time-demanding optimizations. Experimental results show that the proposed control structure allows the computation of globally optimal trajectories and controls with the required frequency, resulting in the successful maneuvering of the helicopter toward a target in an obstacle-filled environment.

Appendices

Appendix A Dynamics Models

The following nonlinear model, proposed in Lopes et al. (2006) and enhanced in Maia (2008), is considered

$$\begin{aligned} \dot{x}_1&= x_2,\\ \dot{x}_2&= \lambda _{16} \left\{ \lambda _1 \left( V_f^2 - V_b^2 \right) + \lambda _2 \left( V_f - V_b \right) - \nu _2 x_2 \right\} , \\ \dot{x}_3&= x_4,\\ \dot{x}_4&= x_6^2 \left\{ \lambda _3 \sin (2x_3) + \lambda _4 \cos (2x_3) \right\} \\&\quad + \left\{ \lambda _7 \left( V_f^2 + V_b^2 \right) + \lambda _8 \left( V_f + V_b \right) \right\} \cos (x_1),\\&\quad + \ \lambda _6 \cos (x_3) + \lambda _5 \sin (x_3)\\ \dot{x}_5&= x_6,\\ \dot{x}_6&= \left\{ \lambda _{13} + \lambda _{14}\sin (2x_3) + \lambda _{15} \cos (2x_3) \right\} ^{-1} \cdot \{ \nu _1 \\&\quad -\nu _3 x_6 + \left[ \lambda _9 \left( V_f^2 + V_b^2 \right) + \lambda _{10} (V_f + V_b) \right] \sin (x_1) \\&\quad + x_4x_6 \left[ \lambda _{11}\sin (2x_3) + \lambda _{12} \cos (2x_3) \right] \} \end{aligned}$$

where $x_1,x_3,x_5 \in \mathbb {R}$ represent the pitch, elevation, and travel angles, respectively; $x_2, x_4, x_6 \in \mathbb {R}$ are their respective time derivatives; $\lambda _i \in \mathbb {R},\ i=\{1,\dots ,16\}$ and $\nu _1, \nu _2, \nu _3 \in \mathbb {R}$ are model parameters. The system is linearized around the equilibrium point $x_{1,eq}=x_{5,eq}=0^\circ$, $x_{3,eq}=27^\circ$, $x_{2,eq}=x_{4,eq}=x_{6,eq}=0^\circ /s$, $V_{f,eq} = V_{b,eq}= 2.97$ V, which corresponds to a position where the arm of the helicopter is parallel to its supporting table (Afonso & Galvão, 2010). The following state space model, as in (1) and (2), is obtained

$$\begin{aligned}&\varvec{\Gamma } = \begin{bmatrix} 0 &{} 1 &{} 0 &{} 0 &{} 0 &{} 0\\ 0 &{} -0.753 &{} 0 &{} 0 &{}0 &{} 0 \\ 0 &{} 0 &{} 0 &{} 1 &{} 0 &{} 0\\ 0 &{} 0 &{} -1.0389 &{} 0 &{} 0 &{} 0\\ 0 &{} 0 &{} 0 &{} 0 &{} 0 &{} 1\\ -1.3426 &{} 0 &{} 0 &{} 0 &{} 0 &{} 0.4377 \end{bmatrix},\\&\varvec{\Theta } = \begin{bmatrix} 0 &{} 0\\ 2.966 &{} -2.966\\ 0 &{} 0\\ 0.4165 &{} 0.4165\\ 0 &{} 0\\ 0 &{} 0 \end{bmatrix}, \ \varvec{\Lambda } = \begin{bmatrix} 0 &{} 0 &{} 1 &{} 0 &{} 0 &{} 0\\ 0 &{} 0 &{} 0 &{} 0 &{} 1 &{} 0\\ \end{bmatrix}. \end{aligned}$$

Appendix B Frequency Experiment Data

Table 1 provides all data collected during the frequency experiment.

Table 1 Frequency and corresponding magnitude and phase measured during the experiments

Appendix C Collision Avoidance Constraints

This appendix provides details on the implementation of (20) and the corner-cutting constraint on a MIP model. We start by presenting the halfspace representation of the polyhedral obstacles, $\forall c \in \{1,\dots ,n_o\}$,

$$\begin{aligned} \mathcal {O}_c = \{ \textbf{r} \in \mathbb {R}^2\ \vert \ \textbf{P}_c\textbf{r} \le \textbf{q}_c \}, \end{aligned}$$

(C1)

where $\textbf{P}_c \in \mathbb {R}^{n_{s,c} \times 2}$, $\textbf{q}_c \in \mathbb {R}^{n_{s,c}}$ with $n_{s,c}$ being the number of sides of the polyhedron representing the c-th obstacle. Then, the collision avoidance constraints are encoded using the “Big-M” method as, $\forall \ell \in \{0,\dots ,N_\textrm{max} \}$, $\forall c \in \{1,\dots ,n_o\}$,

$$\begin{aligned}&\textbf{P}_c \textbf{y}_e(\ell \vert k) \ge \textbf{q}_c - ( \textbf{1}_{n_{s,c}} - \textbf{b}^\textrm{obs}_c(\ell \vert k) )M \nonumber \\&\quad + \textbf{1}_{n_{s,c}}\sum _{z=0}^{\ell -1}b^{hor}(z \vert k)M + \textbf{1}_{n_{s,c}}\epsilon \end{aligned}$$

(C2)

$$\begin{aligned}&\sum _{v=0}^{n_{s,c}} b^\textrm{obs}_{c,v}(\ell \vert k) \ge 1 \end{aligned}$$

(C3)

where $\epsilon >0$ and with $\textbf{b}_c^\textrm{obs} = [b^\textrm{obs}_{c,1},b^\textrm{obs}_{c,2},\dots , b^\textrm{obs}_{c,n_{s,c}}]^\top$ and $b^{hor} \in \{0,1\}$ being the obstacle and horizon binary variables, respectively. The reader is referred to Afonso et al. (2020) for further details. The corner-cutting constraint is similar, $\forall \ell \in \{0,\dots ,N_\textrm{max}-1 \}$, $\forall c \in \{1,\dots ,n_o\}$

$$\begin{aligned}&\textbf{P}_c \textbf{y}_e(\ell +1 \vert k) \le \textbf{q}_c + ( \textbf{1}_{n_{s,c}} - \textbf{b}^\textrm{obs}_c(\ell \vert k) )M \nonumber \\&\quad + \textbf{1}_{n_{s,c}}\sum _{z=0}^{\ell -1}b^{hor}(z \vert k)M + \textbf{1}_{n_{s,c}}\epsilon , \end{aligned}$$

(C4)

with the main distinction being the implementation of the collision avoidance constraint for the position at time step $\textbf{y}_e(\ell +1\vert k)$ using the binary from the previous time step $\textbf{b}^\textrm{obs}(\ell \vert k)$. More details are presented in Richards and Turnbull (2015).