# Linear Parameter-Varying Model Predictive Control of AUV for Docking Scenarios
## 3 AUV Control System
### Thrust Allocation  
The TA algorithm maps the set of virtual forces and moments \(\tau_{V}(k)\) into the space of actuator set-points \(u_{A}(k)\). The mapping is computed, at the controller sample times, given the instantaneous value of velocities (linear and angular) and orientation of the vehicle, also the orientation of the thrusters and the fins mounted on the vessel's hull. The TA problem is formulated as a quadratic programming optimization problem with equality constraints:  
\[\min_{u}{u_{c}}^{T}Wu_{A}+{u_{c}}^{T}h \tag{26}\]\[s.t.\quad\tau_{R}(t)=G(t)u_{A} \tag{27}\] \[u_{A}{}^{m}<u_{A}<u_{A}{}^{M} \tag{28}\]  
where \(W\) is the weighting matrix on the quadratic term, \(h\) is the linear term, the vector of optimization variables grouping the actuator set-point values (main thruster, rudders and stern-planes, front and rear side-thrusters) is:  
\[u_{A}=[u_{M},\delta_{r1},\delta_{r2},\delta_{s1},\delta_{s2},\ u_{R1},u_{R2},u_{ R3},u_{R4},u_{F1},u_{F2},u_{F3},u_{F4}]^{T} \tag{29}\]  
and \(u_{A}{}^{m}\) and \(u_{A}{}^{M}\) are the vectors of minimum and maximum admissible values of the optimization variables (actuator saturations), respectively. The \(G(t)\) is the matrix representing a combination of optimization variables combining actuator forces and moments according their position on the vessel hull and vehicle's pose:  
\[\tau_{V}=[X_{V}-X_{A},Y_{V}-Y_{A},Z_{V}-Z_{A},K_{V}-K_{A},M_{V}-M_{A},N_{V}-N_ {A}]^{T} \tag{30}\]  
is the vector of reference moment and forces. It is computed considering the difference between virtual forces and moments (\(X_{V},Y_{V},Z_{V},K_{V},M_{V},N_{V}\)) provided externally (from the MPC) and the effort provided by the interaction between actuators and the underwater environment, according to the vehicle's layout and dynamics (\(X_{A},Y_{A},Z_{A},K_{A},M_{A},N_{A}\)). These last terms are given by the resultant of the set of actuators, so that:  
\[X_{A}=0 \tag{31}\]  
\[Y_{A}=\frac{1}{2}Y_{uubr}(-uv)+\frac{1}{2}Y_{urf}ur \tag{32}\]  
\[Z_{A}=\frac{1}{2}Z_{uubs}(-uw)+\frac{1}{2}Y_{urf}uq \tag{33}\]  
\[K_{A}=0 \tag{34}\]  
\[M_{A}=-\frac{1}{2}Y_{urf}(-uw)+\frac{1}{2}M_{uqf}uq \tag{35}\]  
\[s.t.\quad\tau_{R}(t)=G(t)u_{A} \tag{27}\]0  
The matrix \(G\) is a time-varying configuration matrix defined as:  
\[G=\left[ \begin{array}{ccccc}G_{11}&0&0&0&0&G_{s(1)}\\ 0&G_{Y}&G_{Y}&0&0&G_{s(2)}\\ 0&0&0&G_{z}&G_{z}&G_{s(3)}\\ 0&G_{k}&G_{k}&G_{k}&G_{k}&G_{k}\\ 0&0&0&G_{m}&G_{m}&G_{s(5)}\\ 0&G_{n}&G_{n}&0&0&G_{s(6)}\end{array} \right] \tag{37}\]  
with  
\[G_{11}=-0.46u\rho D_{0}^{3}/n_{k-1}+0.66\rho D_{0}^{4}\text{ if }\frac{n_{k-1}}{u}>=0 \tag{38}\]  
\[G_{11}=-0.46\rho D_{0}^{4}\text{ if }\frac{n_{k}-1}{u}<0 \tag{39}\]  
\[G_{Y}=u^{2}0.25Y_{uubr} \tag{40}\]  
\[G_{z}=u^{2}0.25Z_{uubs} \tag{41}\]  
\[G_{k}=K_{uubr}u^{2}0.25 \tag{42}\]  
\[G_{m}=u^{2}0.25Y_{urf} \tag{43}\]  
\[G_{n}=u^{2}0.25N_{uubr} \tag{44}\]\[=\left[ \begin{array}{cccccccc}G_{s}=\left[G_{s(1)},G_{s(2)},G_{c(3)},G_{s(4)},G_{ s(5)},G_{s(6)}\right]^{T}\\ 0&0&0&0&0&0&0\\ -c(\alpha)&-c(\alpha)&c(\alpha)&c(\alpha)&-c(\alpha)&-c(\alpha)&c(\alpha)&c( \alpha)\\ -c(\alpha)&c(\alpha)&c(\alpha)&-c(\alpha)&c(\alpha)&c(\alpha)&c(\alpha)&-c( \alpha)\\ -d&d&-d&d&-d&-d&-d&d\\ D_{CF}(\alpha)&-D_{CF}(\alpha)&-D_{CF}(\alpha)&D_{CF}(\alpha)&-D_{DE}(\alpha) &D_{BC}(\alpha)&D_{BC}(\alpha)&-D_{BC}(\alpha)\\ -D_{CF}(\alpha)&-D_{CF}(\alpha)&D_{CF}(\alpha)&D_{CF}(\alpha)&D_{BC}(\alpha)&D_ {BC}(\alpha)&-D_{BC}(\alpha)&-D_{BC}(\alpha)\end{array} \right] \tag{45}\]  
where \(c(\alpha)=\cos(\alpha)\), and \(\alpha=45\) degrees is the angular position of the thrusters with respect to the body-fixed reference frame. Let \(u_{c}(k)\) represent the actuator set-points computed at the actual iteration of the algorithm, \(u_{c}(k-1)\) the signals provided to the actuators at the previous time step and \(\Delta u_{c}{}^{m}\) and \(\Delta u_{c}{}^{M}\) the minimum and maximum values on the rate of change of the computed signals that define the constraints. These constraints were introduced in the TA problem above.