# A Hybrid Control Approach for the Swing Free Transportation of a Double Pendulum with a Quadrotor
## 3 Control System  
### MPC-PD Control Configuration  
The control configuration we use in the quadrotor and double pendulum system is shown in Figure 3. For generalization purposes, variables that are not used in the actual experiment are also included, such as \(Y\) axis or \(\psi\) angle. For reproducibility purposes we have published our code in ([[http://www.ehu.eus/ccwintroo/uploads/3/36/MPC-double-pendulum.zip](http://www.ehu.eus/ccwintroo/uploads/3/36/MPC-double-pendulum.zip)]([http://www.ehu.eus/ccwintroo/uploads/3/36/MPC-double-pendulum.zip](http://www.ehu.eus/ccwintroo/uploads/3/36/MPC-double-pendulum.zip)) accessed on 7 June 2021). * The PD controller computes the quadrotor torques and lift in Equation (4). It consists of four decoupled Single Input Single Output (SISO) PD circuits, each in charge of one of the four degrees of freedom (\(z\), \(\phi\), \(\theta\) and \(\psi\)). The PD equation is a discrete time controller, where the error derivative is obtained with the division of two consequent error in time and the chosen time increment. The main equation of the PD control is shown next: \[u(t)=K_{P}e(t)+K_{D}\frac{de(t)}{dt},\] (12) where \(e(t)\) is the difference between the desired value of each degree of freedom (denoted by subindex \(d\)) and the actual value in each discrete moment. \(K_{P}\) and \(K_{D}\) are the gain of the proportional and derivative terms, respectively. * The MPC block is in charge of computing the desired attitude angles minimizing the payload swing (\(z_{d}\), \(\phi_{d}\), \(\theta_{d}\) and \(\psi_{d}\)). MPC is a class of advanced process control methods [46] which computes the optimal control parameters of the system in finite window of time, denoted the control horizon, and then applies the first step in this control horizon. The process goes on moving the control horizon one step ahead. The algorithm minimizes a cost function, consisting of the error between the output and desired tracking state, subjected to some constraints. Our MPC controller is designed over a discrete-time state space linear time-invariant (LTI) system, the linearization of the double pendulum dynamic system described in Section 2.3. We apply a Kalmann filter for the prediction of the states in the control horizon, thus we have a Robust MPC, that has shown promising results in highly nonlinear quadrotor related applications [47; 48]. Our cost function will be the minimization of the difference between the quadrotor position and the desired trajectory and the minimization of the control inputs, thus resulting in Equation (13):  
\[J=(x_{N}-x_{N}^{*})^{T}P_{m}(x_{N}-x_{N}^{*})+\sum\limits_{i=0}^{N-1}[(x_{i}-x_ {i}^{*})^{T}Q(x_{i}-x_{i}^{*})+u_{i}^{T}Ru_{i}], \tag{13}\]  
where the control horizon is denoted by \(N\). The terminal state and its desired state are denoted by \(x_{N}\) and \(x_{N}^{*}\). \(P_{m}\) is the weighting matrix for the motor states. Finally, the reference state is denoted by \(x_{i}^{*}\). The minimization function is subjected to the linearized dynamics of the double pendulum and the quadrotor:  
\[x_{k+1}=Ax_{k}+B_{k}u_{k}\enspace, \tag{14}\]  
with state and control constraints:  
\[x_{i}\in X;u_{i}\in U. \tag{15}\]  
The system matrices \(A\in\mathbb{R}^{8\times 8}\), \(B\in\mathbb{R}^{8\times 3}\) and \(C\in\mathbb{R}^{6\times 8}\), and are not reproduced here due to their large size. The control input constraints are defined as follows  
\[-10\leq\theta_{ref}\leq 1, \tag{16}\]  
\[-10\leq\phi_{ref}\leq 10, \tag{17}\]  
\[u_{min}\leq u_{k}\leq u_{max}, \tag{18}\]  
Finally, \(Q\in\mathbb{R}^{8\times 8}\) and \(R\in\mathbb{R}^{3\times 3}\) are the weighting matrices and their values are set as follows:  
\[Q=diag[(\quad 50\quad 50\quad 100\quad 100\quad 1\quad 1\quad 10\quad 10)], \tag{19}\]  
\[R=diag[(\quad 10^{4}\quad 10^{4})]\enspace. \tag{20}\]  
Robust MPC Using Kalmann Filter for State Prediction  
A Kalmann filter is used for the prediction of the system state within the control horizont in MPC.