Robust optimal motion planning approach to cooperative grasping and transporting using multiple UAVs based on SDRE

Abstract

In this paper, we consider the problem of controlling a team of Quadrotors that cooperatively grasp and transport a common payload in three dimensions in the presence of external disturbances and parametric uncertainties such as wind field effects. The main contribution of this work is to propose a cooperative control algorithm based on a decentralized strategy. This algorithm consists of two main parts: first calculating the control vectors for each Quadrotor using Moore–Penrose theory and second combining these control vectors with individual control vectors, which are obtained from a closed-loop non-linear robust optimal controller. In this regard, a robust optimal sliding mode controller (ROSMC), which incorporates the state-dependent Riccati equation (SDRE) method with sliding mode control (SMC) technique, is designed. It also has the capability of maximum dynamic load carrying capacity (DLCC) to increase the carrying capacity and the efficiency of the group of Quadrotors. The proposed method inherits the advantages of both approaches including robustness against model uncertainties and high flexibility in designing the control parameters to provide an optimal solution for the non-linear dynamic of the system. The control algorithm is based on the Lyapunov technique, which is able to provide the stability of the end-effecter during tracking of the desired trajectory with acceptable precision. Finally, the simulation results demonstrate the effectiveness of the control strategy for the cooperative Quadrotors to grasp and transport a common payload in various manoeuvres.

Keywords

Cooperative Quadrotors dynamic load carrying capacity SDRE sliding mode control

Introduction

Undoubtedly, carrying and displacing objects is one of the most important tasks of robots in robotic science. The cooperative control of multiple vehicle systems poses significant theoretical and practical challenges. Cooperative control problems for multiple vehicle systems can also be categorized as formation control problems with applications to mobile robots, unmanned aerial vehicles (UAVs), autonomous underwater vehicles, cooperative transport, cooperative role assignment, cooperative timing and cooperative search. In this paper, we seek to develop an algorithm for cooperation between flying robots for carrying an object. The controller is designed to move the object by two or more Quadrotors.

To create mechanisms for interaction between object and cooperative Quadrotors, some research utilized the structure of a leader follower, which has unique benefits. For example, in Korayem et al. (2014) and Desai et al. (1998, 2001) a feedback linearization controller, in Das et al. (2002) a sliding mode controller and in Sanchez and Fierro (2003) a controller based on the position of the robot using a high gain are designed based on the structure of a leader follower system, which led to the desired goals of the moving robots. It should be noted that the structure of the leader follower controllers based on feedback linearization is mostly used, because of their ability to compensate for uncertainties associated with the acceleration of the leader robot motion (Orqueda and Fierro, 2006). However, this method has several disadvantages for cooperative strategy. For example, the position of the leader must be notified to the followers at each moment, and if the leader’s position signal is not issued for any reason, it is possible that the formation disappears quickly. On the other hand, there is always a delay from the moment of receiving the signal of the leader’s position to the moment when the changes are applied to the status of followers, and this leads to a high complexity in the elimination of disturbances in this method.

Another form of cooperation control strategy is the centralized control method that controls small robotic systems, which has better results than the method of the leader follower approach. As the Quadrotors are categorized as large-scale systems, using this type of controller alone to establish the necessary cooperation in the performance of these flying unmanned robots may not be useful. One of the disadvantages of this controller is increasing levels of freedom (Tarn et al., 1986). However, much effort is focused on the development of methods based on the reduced model to solve this problem in robotic systems (Liu et al., 2007; Yoshikawa and Zheng, 1997).

There has been much research in cooperative multi-robot controller design based on decentralized control methods. A cooperative control framework for vehicle formations has been proposed in Fax and Murray (2004). In Cheng et al. (2008), the authors describe the problem of the cooperation approach by a team of ground robots for a unique solution to the motion of the object and robots motions under quasi-static assumptions based on a decentralized controller. The problem of aerial manipulation using cables based on decentralized control in transporting a payload has been studied in Fink et al. (2011) and Michael et al. (2011). The focus of their works is on finding robot configurations and ensuring static equilibrium of the payload at a desired position while respecting the constraints on the tension.

In Michael et al. (2011), the authors proposed aerial grabbing based on a cooperative scheme for load transportation with a simplified model and control, and Fink et al. (2011) proposed planning of these tasks. Also, in Bernard et al. (2011), cooperative aerial towing has been studied in particular. Furthermore, it can be shown that the advantages such as decreased number of sensors and speeding up the performance of decentralized multi-agent systems are quite evident (Nathan et al., 2011). The bilinear matrix inequalities, as a special form of linear matrix inequalities, is an effective way of presenting optimal solutions for decentralized multi-agent non-linear systems (Nian, 2005; Wang et al., 2012). In all these cases, decentralized control methods are considered to solve a substantial proportion of the problems in cooperative strategy directly or indirectly.

Hence, in this research we design new cooperative decentralized control laws for a group of UAVs in a formation where each vehicle is controlled individually. This enables the manipulation of a payload in three dimensions. This cooperative control algorithm is comprised of two parts. In the first part, using Moore–Penrose theory, control fundamental vectors that guarantee the optimal performance of the coupled system are determined and, in the second part, individual control vectors that guarantee the robustness of the coupled system against uncertainties and disturbances are designed for each Quadrotor. In order to have a soft touch between the object being carried and the cooperative Quadrotors, a robust optimal sliding mode controller (ROSMC) based on the state-dependent Riccati equation (SDRE) is employed to control all of the Quadrotors in cooperative system. Finally, by combining these parts the control forces and torques of the cooperative team of Quadrotors are determined.

The SDRE technique has emerged as a design method, which provides a systematic and effective design of non-linear controllers in many fields such as aerospace (Massari and Zamaro, 2014), robotics (Korayem et al., 2014), motors (Do et al., 2014), magnitude torque attitude control of a satellite (Abdelrahman et al., 2011), flexible cable transporter system (Zhang et al., 2005) and in this paper we are to apply this technique to control cooperative UAVs as a part of our novel method. The idea of SDRE approach is to provide an optimal solution for a non-linear dynamic system similar to the linear quadratic regulator (LQR) control (Cloutier, 1997). It provides high flexibility in components design and proper functionality of the system in its real time implementation. The design flexibility, principle selection of design matrices and finding solutions in the presence of control and state constraints are its advantages. Because of these excellent features, SDRE is a suitable approach for designing individual control vectors in the proposed cooperative control strategy of this research in order to provide asymptotic stability of the closed-loop coupled system.

Another item focused on in this paper is the maximum dynamic load carrying capacity (DLCC) by a group of Quadrotors. The carrying capacity and the efficiency of the robot end-effector may be limited by the amount of deviation. It is clear that increasing the stimulus cannot be a good solution because it leads to higher power consumption and increment of the size and weight of the driver. For example, in Korayem et al. (1992), DLCC has been used as a criterion for determining the load carrying capacity in design of the robot arm. Also in Korayem et al. (1993) and Sidi et al. (1998), the maximum DLCC of the robot is determined in the usual way and two-arm robot with flexible joints is implemented. In this study, the maximum load neighbourhood robot configurations have been investigated, and some techniques to increase the carrying capacity of the cooperative system have been developed. In order to apply this algorithm, two important factors should be considered: first, the maximum torques that can be generated by motors and, second, the maximum acceptable bounds of errors within which the end-effector is permitted to move (Korayem et al., 2007). The required constraints can be easily satisfied by the aid of our proposed iterative algorithm in this paper.

A sliding mode control (SMC) algorithm as a robust control method has also been discussed for identification of the maximum DLCC, in the presence of disturbance and system uncertainties (Korayem and Pilechian, 2005). Furthermore, in Nian (2005) and Wang and Ravani (1988) the authors described DLCC for a two-link flexible manipulator dynamic optimal path planning. It should be noted that individual UAVs are fundamentally limited in their ability to manipulate and transport objects of any significant size. We address this limitation in this paper and consider the problem for cooperative Quadrotors.

In this research, we extract the system equations in state–space form and apply the combination of proposed ROSMC with an optimal cooperative algorithm to achieve a novel robust optimal controller, which successfully rejects the external disturbances for cooperative Quadrotors such as wind effect. Merits of using this approach range from simple implementation and flexible parameter learning to design criteria customization. The simulation results demonstrate the effectiveness and superior performance, and high DLCC of the proposed control strategy for the cooperative Quadrotors to grasp and transport a common payload in various manoeuvres.

The remainder of this paper approaches the development of controllers for cooperative Quadrotors and transportation as follows: at first the development of a model for a single Quadrotor is discussed. Then we describe the modelling of a team of Quadrotors rigidly attached to a payload. This section also establishes a distinction on different modalities and information of what is known as aerial grasp manipulation. We propose cooperative control algorithm laws defined with respect to the payload that stabilizes the payload along three-dimensional trajectories based on a SDRE incorporated with SMC technique. We review and analyse experimental study with teams of Quadrotors cooperatively grasping, stabilizing and transporting payloads. This includes point-to-point path planning and trajectory tracking. Maximum allowable load is computed by considering the limiting factors of the robot in each case by definition of DLCC, and finally we conclude the paper with some future improvement suggestions.

Dynamic modelling and coordinate reference frames for single Quadrotors

According to Figure 1 (Zeghlache, 2013), the Quadrotors can fly in all directions and have no limit on manoeuvre limit on manoeuvre. Two main reference frames are considered in order to analyse the dynamic model frame: $E = {E_{x}, E_{y}, E_{z}}$ and the body-fixed reference frame such as: $B = {B_{x}, B_{y}, B_{z}}$ . In this nomenclature, ξ $= {[x, y, z]}^{T}$ and $η = {[φ, θ, Ψ]}^{T}$ are the position vector and angle vector in reference E. These three Euler angels are named roll angle $(\frac{- π}{2} < φ < \frac{π}{2})$ , pitch angle $(\frac{- π}{2} < θ < \frac{π}{2})$ and yaw angle $(- π < ψ < π)$ . $F_{i} (i = 1, 2, 3, 4)$ is the thrust forced produced by four rotors.

Figure 1.

Configuration, inertial and body fixed frame of a Quadrotor.

The equations describing the altitude and the attitude motions of a Quadrotor helicopter are basically the same as those describing a rotating rigid body with six degrees of freedom. The three-axis rotational dynamic of rigid body in body-fixed reference frame is given by

I \overset{\cdot}{ω} + ω \times (I ω) = τ + ω_{τ}

(1)

where $I = diag {I_{x}, I_{y}, I_{z}}$ is the diagonal inertia matrix, $ω = {[p q r]}^{T}$ is the angular velocity vector, $τ = {[τ_{φ} τ_{θ} τ_{Ψ}]}^{T}$ is the vector of actuator torques and $ω_{τ} = {[ω_{φ} ω_{θ} ω_{Ψ}]}^{T}$ is the vector of external disturbance torques. The rigid body rotational kinematics equations are given by:

\overset{\cdot}{η} = R_{B} ω

(2)

where $η = [φ, θ, ψ]$ is Euler angles and $R_{B}$ is the rotation matrix given by of vectors from the body fixed frame to the inertial frame is given by:

R_{B} = [\begin{matrix} C_{Ψ} C_{θ} & S_{Ψ} C_{θ} & - S_{θ} \\ C_{Ψ} S_{θ} S_{φ} - S_{Ψ} C_{φ} & S_{Ψ} S_{θ} S_{φ} - S_{Ψ} C_{φ} & C_{θ} S_{φ} \\ C_{Ψ} S_{θ} C_{φ} + S_{Ψ} S_{φ} & C_{Ψ} S_{θ} C_{φ} + S_{Ψ} S_{φ} & C_{θ} S_{φ} \end{matrix}]

(3)

The translational dynamic model of rigid body in the body-fixed reference frame is given by

m \overset{\cdot}{V} + ω \times (mV) = F + ω_{F}

(4)

where m is the mass of Quadrotor, $F = {[F_{x} F_{y} F_{z}]}^{T}$ is the vector of external forces, $V = {[V_{x} V_{y} V_{z}]}^{T}$ is the Quadrotor linear velocity and $ω_{F} = {[ω_{x} ω_{y} ω_{z}]}^{T}$ is the vector of external disturbance forces. The rigid body translational kinematics equations are given by

\overset{\cdot}{ξ} = R_{B} V

(5)

where $ξ = [x, y, z]$ is the vector of translational positions in inertial coordinate frame. During the Quadrotor navigation with moderate velocity, the roll and pitch angles remain near zero degrees to allow approximation of matrix $Ω_{B}$ with the identity matrix, thus the vector of derivation of the Euler angles $η$ can be approximated by the body axis angular velocity $ω$ . Under these assumptions, the dynamic model of the Quadrotor, Equations (1–5) can be reduced to a more appropriate form for control system design. The full Quadrotor dynamic model with the $x, y, z$ motions as outcome of a pitch, roll and yaw rotation is:

{\begin{matrix} \overset{\cdot\cdot}{x} = (\cos φ \sin θ \cos ψ + \sin φ \sin ψ) \frac{l}{m} U_{1} + ω_{x} \\ \overset{\cdot\cdot}{y} = (\cos φ \sin θ \sin ψ - \sin φ \sin ψ) \frac{l}{m} U_{1} + ω_{y} \\ \overset{\cdot\cdot}{z} = - g + (\cos φ \cos θ) \frac{l}{m} U_{1} + ω_{z} \\ \overset{\cdot\cdot}{φ} = \overset{\cdot}{θ} \overset{\cdot}{ψ} (\frac{Iy - Iz}{Ix}) - \frac{jr}{Ix} \overset{\cdot}{θ} Ω + \frac{l}{I_{x}} U_{2} + ω_{φ} \\ \overset{\cdot\cdot}{θ} = \overset{\cdot}{φ} \overset{\cdot}{ψ} (\frac{Iz - Ix}{Iy}) - \frac{jr}{Ix} \overset{\cdot}{φ} Ω + \frac{l}{y} U_{3} + ω_{θ} \\ \overset{\cdot\cdot}{ψ} = \overset{\cdot}{φ} \overset{\cdot}{θ} (\frac{Ix - Iy}{Iz}) + \frac{l}{Iz} U_{4} + ω_{ψ} \end{matrix}

(6)

where $U_{1} = F_{z}, U_{2} = τ_{φ}, U_{3} = τ_{θ}, U_{4} = τ_{Ψ}$ . The input $U_{1}$ is related to the total thrust, whereas the inputs $U_{2}, U_{3}, U_{4}$ are related to the rotations of the Quadrotor and $ω_{x}, ω_{y}, ω_{z}, ω_{φ}, ω_{θ}, ω_{Ψ}$ are external disturbances, jr denotes the inertia of the Z axis and $Ω$ is the overall residual propeller angular speed that is given by:

{\begin{matrix} U_{1} = b (Ω_{1}^{2} + Ω_{2}^{2} + Ω_{3}^{2} + Ω_{4}^{2}) \\ U_{2} = lb (Ω_{4}^{2} - Ω_{2}^{2}) \\ U_{3} = lb (Ω_{3}^{2} - Ω_{1}^{2}) \\ U_{4} = d (Ω_{2}^{2} - Ω_{1}^{2} + Ω_{4}^{2} - Ω_{3}^{2}) \\ Ω = - Ω_{1} + Ω_{2} - Ω_{3} + Ω_{4} \end{matrix}

(7)

In this equation, d is the thrust coefficient and b is the drag coefficient. Equation (7) can be rearranged in terms of the rotational speed vector,

[\begin{matrix} Ω_{1}^{2} \\ Ω_{2}^{2} \\ \begin{matrix} Ω_{3}^{2} \\ Ω_{4}^{2} \end{matrix} \end{matrix}] = [\begin{matrix} \frac{1}{4 b} & - \frac{1}{2 bl} & \begin{matrix} 0 & - \frac{1}{4 b} \end{matrix} \\ \frac{1}{4 b} & 0 & \begin{matrix} \frac{1}{2 bl} & - \frac{1}{4 d} \end{matrix} \\ \begin{matrix} \frac{1}{4 b} \\ \frac{1}{4 b} \end{matrix} & \begin{matrix} \frac{1}{2 bl} \\ 0 \end{matrix} & \begin{matrix} \begin{matrix} 0 & - \frac{1}{4 d} \end{matrix} \\ \begin{matrix} - \frac{1}{2 bl} & - \frac{1}{4 d} \end{matrix} \end{matrix} \end{matrix}] [\begin{matrix} U_{1} \\ U_{2} \\ \begin{matrix} U_{3} \\ U_{4} \end{matrix} \end{matrix}]

(8)

Therefore, the desired motor speed can be computed to be sent to the motor controllers. The model (6) can be rewritten in a state-space form $\overset{\cdot}{X} = f (X, U)$ by introducing $X = [x_{1}, \dots \dots, x_{12}]$ , which is the state vector of the system

{\begin{matrix} x_{1} = φ, x_{2} = {\overset{\cdot}{x}}_{1} = \overset{\cdot}{φ}, x_{3} = θ, x_{4} = {\overset{\cdot}{x}}_{3} = \overset{\cdot}{θ}, x_{5} = ψ, x_{6} = {\overset{\cdot}{x}}_{5} = \overset{\cdot}{ψ} \\ x_{7} = x, x_{8} = {\overset{\cdot}{x}}_{7} = \overset{\cdot}{x}, x_{9} = y, x_{10} = {\overset{\cdot}{x}}_{9} = \overset{\cdot}{y}, x_{11} = z, x_{12} = {\overset{\cdot}{x}}_{11} = \overset{\cdot}{z} \end{matrix}

(9)

Lastly, according to the Quadrotor rigid-body structure and its entirely symmetrical model, the state–space model of the translational and the rotational subsystems based on the Newton–Euler method are defined as follows

f (X, U) = (\begin{matrix} x_{2} \\ x_{4} x_{6} a_{1} + x_{4} x_{2} Ω + b_{1} U_{2} + ω_{φ} \\ x_{4} \\ x_{2} x_{6} a_{3} + x_{2} a_{4} Ω + b_{2} U_{3} + ω_{θ} \\ x_{6} \\ x_{4} x_{2} a_{5} + b_{3} U_{4} + ω_{ψ} \\ x_{8} \\ u_{x} \frac{1}{m} U_{1} + ω_{x} \\ x_{10} \\ u_{y} \frac{1}{m} U_{1} + ω_{y} \\ x_{12} \\ - g + (\cos x_{1} \cos x_{3}) \frac{1}{m} U_{1} + ω_{z} \end{matrix})

(10)

where

\begin{matrix} u_{x} = (\cos x_{1} \sin x_{3} \cos x_{5} + \sin x_{1} \sin x_{5}), \\ u_{y} = (\cos x_{1} \sin x_{3} \sin x_{5} - \sin x_{1} \sin x_{5}) \\ a_{1} = (I_{y} - I_{z}) / I_{x}, a_{2} = - j_{r} / I_{x}, a_{3} = (I_{z} - I_{x}) / I_{y}, a_{4} = j_{r} / I_{y} \\ a_{5} = (I_{x} - I_{y}) / I_{z}, b_{1} = l / I_{x}, b_{2} = l / I_{y}, b_{3} = l / I_{z} \end{matrix}

(11)

The control variables $u_{x}$ and $u_{y}$ can be considered virtual commands, which rotate the thrust vector in such a way that the desired translation motion is achieved; on the other hand, the translations depend on the angles.

Dynamic model of coupled system

To solve the problem using the Euler equations, kinematics system must be extracted. Figure 2 (Mellinger et al., 2013) shows a team of Quadrotors manipulate a payload in three dimensions. In this scheme, four Quadrotors are used for lifting a cross configuration object. The coordinate systems including the world frame, W, and body frame, B, as well as the free body diagram for the Quadrotor are shown in Figure 2. In general, we assume the group of n UAVs are connected through a fixed interaction to the object coordinate frame B in the centre of mass and relative coordinates. The body frame axes are chosen as the principal axes of the entire system and each Quadrotor has an individual body frame $Q_{i}$ , where $i = {1, 2, . . ., n}$ and n is the number of UAVs in the group. We use x–y–z Euler angles to define the roll, pitch and yaw angles ( $φ$ , $θ, ψ$ ).

Figure 2.

Scheme of payload being transported by four Quadrotors.

The rotation matrix from B to W is given by $R_{B}$ . Centre of mass of the ith Quadrotor axis is considered $(x_{Qi}, y_{Qi}, z_{Qi})$ . In the following, we will make simplifying assumptions for the n Quadrotors:

The payload has a cross configuration structure, completely homogeneous, and the centre of mass of the object is intended as the centre of mass of the coupled system.

The mass of the payload is sufficiently small that n Quadrotors are able to lift the object.

The payload does not flip during manipulation.

The motion equation for each of Quadrotors is as follows:

m_{i} {\overset{\cdot\cdot}{z}}_{Qi} = - m_{i} g + F_{Bi}

(12)

In this equation, Quadrotor $m_{i}$ is considered mass to move along the vertical Z, g is gravity and ${F_{B}}_{i}$ is the lift force generated by each Quadrotor. Newton’s second law to the object is as follow:

m_{i} {\overset{\cdot\cdot}{Z}}_{QB} = - m_{L} g

(13)

In this equation, $m_{L}$ is the mass of the object. However, for payload transport, it is necessary that the robots be placed in an identical frame and adapted to improve the tracking performance. As all the forces are used for lifting and moving toward the positive Z direction, the equation of motion for the coupled system may be easily written as:

m \overset{\cdot\cdot}{Z} = - mg + F_{B}

(14)

In Equation (14), $F_{B}$ is lift force of all Quadrotors and m is sum of the Quadrotors and object’s mass. The acceleration of the centre of mass is simply:

m \overset{\cdot\cdot}{r} = [\begin{matrix} 0 \\ 0 \\ - mg \end{matrix}] + R_{B} [\begin{matrix} 0 \\ 0 \\ F_{B} \end{matrix}]

(15)

In Equation (15),r denoted position vector to the centre of mass of the ith Quadrotor. The angular velocity of the object in the object coordinate system frame B in the frame W is denoted by p, q, r:

[\begin{matrix} p \\ q \\ r \end{matrix}] = [\begin{matrix} \cos θ & 0 & - \cos φ \sin θ \\ 0 & 1 & \sin φ \\ \sin θ & 0 & \cos φ \cos θ \end{matrix}] [\begin{matrix} \overset{\cdot}{\emptyset} \\ \overset{\cdot}{θ} \\ \overset{\cdot}{ψ} \end{matrix}]

(16)

As the motor dynamics are fast compared with the rigid body dynamics and the aerodynamics, we will assume that rotor speeds can be instantly achieved during the controller development. These rotor forces can be rewritten as a total force from each Quadrotor $F_{q, i}$ as well as moments about each of the Quadrotor’s body frame axes (Mellinger et al., 2013):

[\begin{matrix} {F_{q}}_{i} \\ {M_{xq}}_{i} \\ {M_{yq}}_{i} \\ {M_{zq}}_{i} \end{matrix}] = [\begin{matrix} 1 & 1 & 1 & 1 \\ 0 & L & 0 & - L \\ - L & 0 & L & 0 \\ \frac{λ_{M}}{λ_{F}} & - \frac{λ_{M}}{λ_{F}} & \frac{λ_{M}}{λ_{F}} & - \frac{λ_{M}}{λ_{F}} \end{matrix}] [\begin{matrix} F_{i, 1} \\ F_{i, 2} \\ F_{i, 3} \\ F_{i, 4} \end{matrix}]

(17)

where L is the distance from the axis of rotation of the rotors to the centre of the Quadrotor. Also $λ_{F} \approx 6.11 \times 10^{- 8} N / rp m^{2}$ and $λ_{M} \approx 1.5 \times 10^{- 9} Nm / rp m^{2}$ are steady-state constants, determined by matching the performance of the simulation to the real system (Mellinger et al., 2013). Therefore, the equation of motion for the coupled system is obtained as follows:

[\begin{matrix} F_{B} \\ {M_{x}}_{B} \\ {M_{y}}_{B} \\ {M_{z}}_{B} \end{matrix}] = \sum_{i} [\begin{matrix} 1 & 0 & 0 & 0 \\ y_{i} & \cos ψ_{i} & - \sin ψ_{i} & 0 \\ - x_{i} & \sin ψ_{i} & \cos ψ_{i} & 0 \\ 0 & 0 & 0 & 1 \end{matrix}] [\begin{matrix} F_{q, i} \\ {M_{x}}_{q, i} \\ {M_{y}}_{q, i} \\ {M_{z}}_{q, i} \end{matrix}]

(18)

where control input $F_{B}$ is the net body force and ${M_{x}}_{B}, {M_{y}}_{B}$ , ${M_{z}}_{B}$ are the body moments, in the $Z_{Qi}$ direction. Finally, the angular accelerations determined by the Euler equations are written as follows:

I = [\begin{matrix} \overset{\cdot}{p} \\ \overset{\cdot}{q} \\ \overset{\cdot}{r} \end{matrix}] + [\begin{matrix} p \\ q \\ r \end{matrix}] \times I [\begin{matrix} p \\ q \\ r \end{matrix}] = [\begin{matrix} M_{xB} \\ M_{yB} \\ M_{zB} \end{matrix}]

(19)

The cooperative control algorithm

The development of the cooperative guidance controller for the case of perfect state knowledge of the chasers and target UAV is described in this section based on Mellinger et al. (2013). This control strategy is a decentralized control algorithm and includes three parts. In the first part of this algorithm, the control fundamental vectors for n Quadrotors that define a pseudo-inverse matrix of the mathematical theory of optimal control using the Moore–Penrose are determined. The second part of this strategy is composed of determining individual control vectors by ROSMC. Finally, in the last part, the cooperative control algorithm can be extracted by combining the individual control vectors and the control fundamental vectors.

Control fundamental vectors of cooperative control strategy

After determining the sum of the force vectors of the coupled system, the linear system (18), four equations with four unknowns 4n can be described as:

{[F_{B}, M_{xB}, M_{yB}, M_{zB}]}^{T} = χ u

(20)

where $χ \in R^{4 \times 4 n}$ is a constant vector containing the states of translational and rotational subsystems, $u \in R^{4 n}$ also includes four control inputs vectors for each n Quadrotor:

u = {[F_{Bq, 1}, M_{xBq 1}, M_{yBq 1}, M_{zBq 1} \dots, F_{Bq, n}, M_{xBqn}, M_{yBqn}, M_{zBqn}]}^{T}

(21)

Therefore, these linear systems can be generalized to n Quadrotors. For this purpose, optimal control vectors are selected to deliver the system to the desired values, which can be minimized as subject to the following cost function (Mellinger et al., 2013):

u^{*} = \arg \min {J | {[(F_{B_{qn}}^{des}, M_{x B_{qn}}^{des}, M_{y B_{qn}}^{des}, M_{z B_{qn}}^{des}]}^{T} = χ u}

(22)

where:

J = \sum_{i} υ_{Fi} F_{Bq, i}^{2} + υ_{Mxi} M_{xBq, i}^{2} + υ_{Myi} M_{yBq, i}^{2} + υ_{Mzi} M_{zBq, i}^{2}

(23)

In this equation, $[υ_{Fi}, υ_{Mxi}, υ_{Myi}, υ_{Mzi}]$ are the weights of each of the control inputs. As the Equation (18) is a linear equation, a good choice to treat the point-wise minimization of the function J is using the mathematical theory of Moore–Penrose inverse. For this purpose, define $Γ \in R^{4 \times 4 n}$ on which we have:

J = ‖ Γ u ‖_{2}^{2}

(24)

Γ = dig [\sqrt{υ_{F 1}}, \sqrt{υ_{Mx 1}}, \sqrt{υ_{My 1}}, \sqrt{υ_{Mz 1}}, \dots . \sqrt{υ_{Fn}}, \sqrt{υ_{Mxn}}, \sqrt{υ_{Myn}}, \sqrt{υ_{Mzn}}]

(25)

To consider the features of Moore–Penrose inverse theory, the optimal control vector is obtained as follows:

\begin{matrix} u^{*} = Γ^{- 1} {(χ Γ^{- 1})}^{+} {[(F_{B_{qn}}^{des}, M_{x B_{qn}}^{des}, M_{y B_{qn}}^{des}, M_{z B_{qn}}^{des}]}^{T} \\ = Γ^{- 2} χ^{T} {(χ Γ^{- 2} χ^{T})}^{- 1} {[(F_{B_{qn}}^{des}, M_{x B_{qn}}^{des}, M_{y B_{qn}}^{des}, M_{z B_{qn}}^{des}]}^{T} \end{matrix}

(26)

In Equation (26), the ‘+’ sign indicates the method of Moore–Penrose. In this regard, the columns of the matrix $Γ^{- 1} {(χ Γ^{- 1})}^{+}$ are the control fundamental vectors $[u_{F}, u_{Mx}, u_{My}, u_{Mz}]$ . Thus, the optimal control input vector can be rewritten as follows:

u^{*} = [u_{F}, u_{Mx}, u_{My}, u_{Mz}] {[(F_{B_{qn}}^{des}, M_{x B_{qn}}^{des}, M_{y B_{qn}}^{des}, M_{z B_{qn}}^{des}]}^{T}

(27)

The more open design of control algorithms, considering the special case in which all n Quadrotors are identical and axially symmetric, can be assumed a similar move to roll and pitch angle. Therefore, the weight of control fundamental vectors $υ_{Fi} = υ_{F}, υ_{Mxi} = υ_{Myi} = υ_{Mxi}$ are considered. Thus, in the Equation (26), $χ Γ^{- 2} χ^{T}$ can be written as:

χ Γ^{- 2} χ^{T} = [\begin{matrix} \frac{n}{υ_{F}} & \frac{\sum yi}{υ_{F}} & - \frac{\sum y_{i}}{υ_{F}} & 0 \\ \frac{\sum y_{i}}{υ_{F}} & \frac{\sum y_{i}^{2}}{υ_{F}} + \frac{n}{υ_{Mxy}} & - \frac{\sum x_{i} y_{i}}{υ_{F}} & 0 \\ - \frac{\sum x_{i}}{υ_{F}} & - \frac{\sum x_{i} y_{i}}{υ_{F}} & - \frac{\sum x_{i}^{2}}{υ_{F}} + \frac{n}{υ_{Mxy}} & 0 \\ 0 & 0 & 0 & \frac{n}{υ_{Mz}} \end{matrix}]

(28)

Remark 1. In this research, it can be assumed that the position of the cooperative Quadrotors dominate the position of the object, that is, the Quadrotors are heavier than the object being carried. As a result, the X and Y coordinates of the centroid of the object and the centroid of the Quadrotors coincide according to Figure 2. The following two assumptions are considered in designing the control algorithm:

\sum x_{i} = \sum y_{i} = 0

(29)

\sum x_{i} y_{i} = 0

(30)

Thus, according to the mentioned conditions in Remark 1 for the coupled system, all Quadrotors have a common role in lifting the object off the ground and equal partnership. As a result, the body force F and yaw torque produced by each of them is almost equal. Thus, the control fundamental vectors consist of the total force F and yaw moment of the coupled system for n Quadrotors is as follows,

\begin{matrix} U_{F_{qn}} = \frac{1}{n} {[1, 0, 0, 0, \dots, 1, 0, 0, 0]}^{T} \\ U_{M z_{qn} =} \frac{1}{n} {[0, 0, 0, 1, \dots \dots, 0, 0, 0, 1]}^{T} \end{matrix}

(31)

Also, two fundamental control vectors that include pitch and roll moment generated for the coupled system structure are obtained as follows (Mellinger et al., 2013):

\begin{matrix} U_{M x_{qn}} = \frac{1}{\frac{υ_{Mxy}}{υ_{F}} \sum y_{i}^{2} + n} \\ {[\frac{υ_{Mxy}}{υ_{F}} y_{q 1}, C ψ_{q 1}, S ψ_{q 1}, 0, \dots, \frac{υ_{Mxy}}{υ_{F}} y_{qn}, C ψ_{qn}, S ψ_{qn}, 0]}^{T} \\ U_{M y_{qn}} = \frac{1}{\frac{υ_{Mxy}}{υ_{F}} \sum x_{i}^{2} + n} \\ {[- \frac{υ_{Mxy}}{υ_{F}} x_{q 1}, - S ψ q_{1}, C ψ_{q 1}, 0, \dots, - \frac{υ_{Mxy}}{υ_{F}} x_{qn}, - S ψ_{qn}, C ψ_{qn}, 0]}^{T} \end{matrix}

(32)

It is evident that the weighting parameters are dependent on control vectors and reflect a trade-off between these factors. Also, it is important to notice to the increment of the force produced by each of n Quadrotors will lead to decrement individual body moments generated by them, according to the factor $\frac{υ_{Mxy}}{υ_{F}}$ . On the other hand, it allows the user to switch between individual forces and moments generated by each Quadrotors produced in the body roll and pitch angles. Finally, calculating the four fundamental control vectors, the cooperative control vectors for n collaboration Quadrotors equation is obtained as follows:

U_{coop | Q_{n}} = U_{Fqn} F_{Bqn} + U_{M x_{qn}} M_{x B_{qn}}^{des} + U_{M y_{qn}} M_{y B_{qn}}^{des} + U_{M z_{qn}} M_{z B_{qn}}^{des}

(33)

Individual control vectors of cooperative control strategy

In this subsection, to complete the cooperative control algorithm, the target is determined from individual control vectors $(F_{B_{qn}}^{des}, M_{x B_{qn}}^{des}, M_{y B_{qn}}^{des}, M_{z B_{qn}}^{des})$ using a non-linear ROSMC approach. To control the attitude of the body, we use $M_{x B_{qn}}^{des}$ to control roll angle, $M_{y B_{qn}}^{des}$ to control pitch angle, $M_{z B_{qn}}^{des}$ to control yaw angle and $F_{B_{qn}}^{des}$ to control position along z. Hence we will design these individual control vectors of the cooperative Quadrotors based on SDRE strategies in order to generate an optimal solution for this cooperative controller scheme.

Sliding mode control of Quadrotors

Sliding mode is one of the robust controlling methods widely used in various non-linear systems with external disturbance and indefinite states. SMC is developed by two stages: first a suitable switching surface is selected and defined based on tracking errors. Second, a Lyapunov function is opted with needed conditions. In sliding mode, the dynamic of state variables is stated by extracting input equations (Bouadi, 2007). The SMC to Quadrotor state variables dynamic is presented by establishing the statement for control input. The choice of the sliding surfaces is based upon the synthesized tracking errors as follows:

{\begin{matrix} S_{φ} = e_{2} + α_{1} e_{1}, S_{θ} = e_{4} + α_{2} e_{3}, S_{Ψ} = e_{6} + α_{3} e_{5} \\ S_{x} = e_{8} + α_{4} e_{7}, S_{y} = e_{10} + α_{5} e_{9}, S_{z} = e_{12} + α_{6} e_{11} \end{matrix}

(34)

Such that $α_{i} > 0$ and ${\begin{matrix} e_{i} = x_{id} - x_{i} \\ e_{i + 1} = {\overset{\cdot}{x}}_{id} - x_{i + 1} \end{matrix}$ , $i \in [1, 2, . ., 12] .$ Finally, the sliding surfaces are chosen as follows:

{\begin{matrix} S_{φ} = {\overset{\cdot}{x}}_{1 d} - x_{2} + a_{1} (x_{1 d} - x_{1}) \\ S_{θ} = {\overset{\cdot}{x}}_{3 d} - x_{4} + a_{2} (x_{3 d} - x_{3}) \\ S_{Ψ} = {\overset{\cdot}{x}}_{5 d} - x_{6} + a_{3} (x_{5 d} - x_{5}) \\ S_{x} = {\overset{\cdot}{x}}_{7 d} - x_{8} + a_{4} (x_{7 d} - x_{7}) \\ S_{y} = {\overset{\cdot}{x}}_{9 d} - x_{10} + a_{5} (x_{9 d} - x_{9}) \\ S_{z} = {\overset{\cdot}{x}}_{11 d} - x_{12} + a_{6} (x_{11 d} - x_{11}) \end{matrix}

(35)

To synthesize a stabilizing control law by sliding mode, the necessary sliding condition $(S \overset{\cdot}{S} < 0)$ must be verified. A Lyapunov function can be written as $V = \sum \frac{1}{2} S^{T} S$ , which one can describe as (Runcharoon and Srichatrapimuk, 2013):

V = \frac{1}{2} (S_{X}^{T} S_{X} + S_{Y}^{T} S_{Y} + S_{Z}^{T} S_{Z} + S_{φ}^{T} S_{φ} + S_{θ}^{T} S_{θ} + S_{Ψ}^{T} S_{Ψ})

(36)

Assuming here that $V_{s φ} = \frac{1}{2} s_{φ}^{2}$ then, the necessary sliding condition is checked and Lyapunov stability is guaranteed. The chosen law for the attractive surface is the time derivative of $V_{s_{φ}}$ satisfying $s_{φ} {\overset{\cdot}{s}}_{φ} < 0$

\begin{matrix} {\overset{\cdot}{s}}_{φ} = k_{1} sign (s_{φ}) = {\overset{\cdot}{e}}_{2} + α_{1} {\overset{\cdot}{e}}_{1} = {\overset{\cdot\cdot}{x}}_{1 d} - {\overset{\cdot}{x}}_{2} + α_{1} ({\overset{\cdot}{x}}_{1 d} - x_{2}) \\ - x_{4} x_{6} α_{1} - x_{4} a_{2} Ω - b_{1} U_{2} - ω_{φ} + {\overset{\cdot\cdot}{φ}}_{d} + α_{1} ({\overset{\cdot}{φ}}_{d} - x_{2}) \end{matrix}

(37)

Same steps are followed to extract ${\overset{\cdot}{s}}_{θ} {\overset{\cdot}{s}}_{ψ} {\overset{\cdot}{s}}_{x} {\overset{\cdot}{s}}_{y} {\overset{\cdot}{s}}_{z}$ :

\begin{matrix} {\begin{matrix} {\overset{\cdot}{s}}_{θ} = k_{2} sign (s_{θ}) = {\overset{\cdot}{e}}_{4} + α_{2} {\overset{\cdot}{e}}_{3} = {\overset{\cdot\cdot}{x}}_{3 d} - {\overset{\cdot}{x}}_{4} + α_{2} ({\overset{\cdot}{x}}_{3 d} - x_{4}) \\ = - x_{2} x_{6} a_{3} - x_{2} a_{4} Ω - b_{2} U_{3} - ω_{θ} + {\overset{\cdot\cdot}{θ}}_{d} + α_{2} ({\overset{\cdot}{θ}}_{d} - x_{4}) \end{matrix} \\ {\begin{matrix} {\overset{\cdot}{s}}_{Ψ} = k_{3} sign (s_{ψ}) = {\overset{\cdot}{e}}_{6} + α_{3} {\overset{\cdot}{e}}_{5} = {\overset{\cdot\cdot}{x}}_{5 d} - {\overset{\cdot}{x}}_{6} + α_{2} ({\overset{\cdot}{x}}_{5 d} - x_{6}) \\ = - x_{4} x_{2} a_{5} - b_{3} U_{4} + {\overset{\cdot\cdot}{ψ}}_{d} + α_{3} ({\overset{\cdot}{ψ}}_{d} - x_{6}) \end{matrix} \\ {\begin{matrix} {\overset{\cdot}{s}}_{X} = k_{4} sign (s_{x}) = {\overset{\cdot}{e}}_{8} + α_{4} {\overset{\cdot}{e}}_{7} = {\overset{\cdot\cdot}{x}}_{7 d} - {\overset{\cdot}{x}}_{8} + α_{8} ({\overset{\cdot}{x}}_{7 d} - x_{8}) \\ = - ω_{x} - \frac{U_{x} U_{1}}{m} + {\overset{\cdot\cdot}{x}}_{d} + α_{4} ({\overset{\cdot}{x}}_{d} - x_{8}) \end{matrix} \\ {\begin{matrix} {\overset{\cdot}{s}}_{Y} = k_{5} sign (s_{y}) = {\overset{\cdot}{e}}_{10} + α_{5} {\overset{\cdot}{e}}_{9} = {\overset{\cdot\cdot}{x}}_{9 d} - {\overset{\cdot}{x}}_{10} + α_{5} ({\overset{\cdot}{x}}_{9 d} - x_{10}) \\ = - ω_{y} - \frac{U_{y} U_{1}}{m} + {\overset{\cdot\cdot}{y}}_{d} + α_{5} ({\overset{\cdot}{y}}_{d} - x_{10}) \end{matrix} \\ {\begin{matrix} {\overset{\cdot}{s}}_{Z} = k_{6} sign (s_{z}) = {\overset{\cdot}{e}}_{12} + α_{6} {\overset{\cdot}{e}}_{11} = {\overset{\cdot\cdot}{x}}_{11 d} - {\overset{\cdot}{x}}_{12} + α_{6} ({\overset{\cdot}{x}}_{11 d} - x_{12}) \\ = g - \frac{(\cos x_{1} \cos x_{3})}{m} U_{1} - ω_{z} + {\overset{\cdot\cdot}{z}}_{d} + α_{6} ({\overset{\cdot}{z}}_{d} - x_{12}) \end{matrix} \end{matrix}

(38)

Remark 2. Note that signum function which appears in second order sliding mode controllers and filters can cause chattering phenomena, or high-frequency oscillations of control variables. This problem can be avoided by replacing discontinuous signum function with appropriate continuous approximation, like; sign(ξ)≈tanh(Kξ)

Therefore, according to Equation (36), the time derivative of final Lyapunov function $V (s)$ is:

\begin{array}{l} \dot{V} = (S_{φ}^{T} {\dot{S}}_{φ} + S_{θ}^{T} {\dot{S}}_{θ} + S_{Ψ}^{T} {\dot{S}}_{Ψ} + S_{X}^{T} {\dot{S}}_{X} + S_{y}^{T} {\dot{S}}_{y} + S_{z}^{T} {\dot{S}}_{z}) = (s_{X} {- ω_{x} - \frac{U_{x} U_{1}}{m} + {\overset{\cdot\cdot}{x}}_{d} + α_{4} ({\dot{x}}_{7 d} - x_{8})} + \\ s_{Y} {- ω_{y} - \frac{U_{y} U_{1}}{m} + {\overset{\cdot\cdot}{y}}_{d} + α_{5} ({\dot{x}}_{9 d} - x_{10})} + s_{Z} {g - \frac{(c o s x_{1} c o s x_{3})}{m} U_{1} - ω_{z} + {\overset{\cdot\cdot}{z}}_{d} + α_{6} ({\dot{z}}_{d} - x_{12})} + \\ s_{φ} {- x_{4} x_{6} a_{1} - x_{4} a_{2} Ω - b_{1} U_{2} - ω_{φ} {\overset{\cdot\cdot}{φ}}_{d} + α_{1} ({\dot{φ}}_{d} - x_{2})} + s_{θ} {- x_{2} x_{6} a_{3} - x_{2} a_{4} Ω - b_{2} U_{3} - ω_{θ} + {\overset{\cdot\cdot}{θ}}_{d} + α_{2} ({\dot{θ}}_{d} - x_{4})} + \\ s_{ψ} {- x_{4} x_{2} a_{5} - b_{3} U_{4} - ω_{Ψ} + {\overset{\cdot\cdot}{ψ}}_{d} + α_{3} ({\dot{ψ}}_{d} - x_{6})}) \end{array}

(39)

The sufficient condition for the stability of the system is given by

\overset{\cdot}{V} (s) = s \overset{\cdot}{s} < - κ . | s |

(40)

The time derivative of $V (s)$ satisfies $s \overset{\cdot}{s} < 0$ , where κ is the positive value $(κ > 0)$ . Finally using Equation (39), the input control vectors extracted from sliding mode approach can be expressed as follows:

{\begin{matrix} U_{1} = \frac{m}{(\cos x_{1} \cos x_{3})} (- k_{6} sign (s_{Z}) + g - ω_{z} + {\overset{\cdot\cdot}{z}}_{d} + α_{6} ({\overset{\cdot}{z}}_{d} - x_{12})) \\ U_{2} = \frac{1}{b_{1}} (- k_{1} sign (s_{φ}) - x_{4} x_{6} a_{1} - x_{4} a_{2} Ω - ω_{φ} + {\overset{\cdot\cdot}{φ}}_{d} + α_{1} e_{2}) \\ U_{3} = \frac{1}{b_{2}} (- k_{2} sign (s_{θ}) - x_{2} x_{6} a_{3} - x_{2} a_{4} Ω + ω_{θ} + {\overset{\cdot\cdot}{θ}}_{d} + α_{2} e_{4}) \\ U_{4} = \frac{1}{b_{3}} (- k_{3} sign (s_{ψ}) - x_{4} x_{2} a_{5} - ω_{Ψ} + {\overset{\cdot\cdot}{Ψ}}_{d} + α_{3} ({\overset{\cdot}{ψ}}_{d} - x_{6})) \\ U_{x} = \frac{m}{U_{1}} (- k_{4} sign (s_{X}) - ω_{x} + {\overset{\cdot\cdot}{x}}_{d} + α_{4} e_{8}) \\ U_{y} = \frac{m}{U_{1}} (- k_{5} sign (s_{Y}) - ω_{y} + {\overset{\cdot\cdot}{y}}_{d} + α_{5} e_{10}) \end{matrix}

(41)

Problem formulation of robust optimal sliding mode controller design of Quadrotors

SDRE control, initially introduced by Cloutier (1997), was inspired by LQR design. Using the SDRE method, a non-linear control method is applied so that all properties of the linear LQR control can be exploited. Consider an uncertain non-linear system described by:

\overset{\cdot}{x} (t) = f (x) x (t) + g (x) u (t) + δ (x, t)

(42)

where $x (t) \in R^{n}$ is the state vector, $u (t) \in R^{m}$ is the control vector, $f (x) \in c^{q}$ and $g (x) \in c^{q}$ with $q \geq 1$ are non-linear function of x, respectively. $δ (x, t)$ is an indefinite function denoting any changes in external disturbance and unmolded non-linear dynamics. For the system (42), let $u = u_{con}$ , where $u_{con}$ minimizes the following cost function:

J = \frac{1}{2} \int_{0}^{\infty} [x^{T} (t) Q (x) x (t) + u_{con}^{T} (t) R (x) u_{con} (t)] dt

(43)

and $Q (x) \in c^{q}, R (x) \in c^{q}$ (for $q > 1$ ) are the weighting matrices of non-linear functions of x, respectively. Also, Q(x) is semi-positive definite and R(x) is considered positive definite. Lastly, we choose the control law to eliminate uncertainties of system (42) in the form of:

u = u_{con} + u_{dis}

(44)

where $u_{con}$ is the continuous part of the control law, which optimizes and stabilizes the system; $u_{dis}$ is the discontinuous part, which provides necessary conditions for eliminating uncertainties of the system. We design $u_{con}$ and $u_{dis}$ so that we achieve our ultimate goal of establishing an ROSMC to ensure the Quadrotor system has accurate optimal performance as well as robustness against wind field effects. The ROSMC contains two steps: first we design the optimal control $u_{con}$ to optimize nominal system (44) and then earn the discontinuous control $u_{dis}$ for some specific uncertainty of the system (42). To design the optimal part of control law $u_{con}$ , the SDRE method is used and SMC approach is applied to design $u_{dis}$ . Therefore, using this control scheme, we not only improve the system performance, but also make it robust against some external disturbance. In general, the SDRE method necessitates solving algebraic Riccati equations (AREs) at any moment for every state, which therefore also requires the control inputs be recalculated for every state. Clearly, this method has complex calculations, which make it unsuitable for online applications, but Menon et al. (2002) developed some real-time techniques that can be applied on an embedded micro-controller. Also, stability of the SDRE technique was expressed in Willard and Randal (2002) and Zhang et al. (2005). In the next section, we describe how to use the SDRE to solve the optimal equation for a non-linear system.

Optimal control design based on SDRE for Quadrotors

Consider the problem of non-linear optimal regulation of indefinite and continuous time system (44). The goal is to minimize unlimited performance time by quadratic state vector and quadratic input vector of Equation (43). To explain the SDRE approach, it is important to be familiarized with the extended notion of linearization, called the SDC (state-dependent coefficient), which is a key concept in this method. Extended linearization SDC is a process during which the non-linear system is turned into a pseudo-linear structure consisting of SDC matrices. Assuming $f (0) = 0$ , where f is a derivative function, there will invariably be a continuous non-linear matrix (x):

f (x) = A (x) x

(45)

The choice of the matrix $A (x)$ is obtained by factorization, and the input matrix is state-dependent instead of being constant. Also assuming, $g (x) = B (x)$ , the extended linearization of system (42) is thus converted to the following form:

\overset{\cdot}{x} (t) = A (x) x (t) + B (x) u (t) + δ (x, t)

(46)

which has a linear structure with matrixes $A (x)$ and $B (x)$ . Here, the SDRE method requires that the pair ${A (x), B (x)}$ is point wise controllable. Also, we assume there exists an unknown continuous function of appropriate dimensions $\tilde{δ} (x, t)$ , such that

δ (x, t) = B \tilde{δ} (x, t)

(47)

So the dynamic system (42) can be rewritten as

\overset{\cdot}{x} (t) = f (x) x (t) + g (x) u (t) + \tilde{δ} (x, t)

(48)

It is also assumed that $‖ \tilde{δ} (x, t) ‖ \leq γ_{0} + γ_{1} ‖ x (t) ‖$ , where $γ_{0}$ and $γ_{1}$ are positive constants. By setting $\tilde{δ} (x, t) = 0$ , the system (42) will be described as:

\overset{\cdot}{x} (t) = A (x) x + B (x) u

(49)

Inspired by the LQR problem, which is described by ARE, SDRE feedback control provides a similar guideline for the problem of optimal non-linear regulator. SDRE state feedback control is formed as follows:

u (x) = - R^{- 1} (x) B^{T} (x) P (x) x (t)

(50)

where $P (x)$ is a unique, symmetric and positive matrix to any point from x. This control law is achieved by solving algebraic SDRE equation:

A^{T} (x) P (x) + P (x) A (x) + Q (x) - P (x) B (x) R^{- 1} (x) B^{T} (x) P (x) = 0

(51)

Note, also that $P (x)$ is formed when the pair ${A (x), B (x)}$ is as a controllable point for x values and the pair ${A (x), C (x)}$ is as an observable point for x values, where $C^{T} (x) C (x) = Q (x)$ . It can be seen that the solution of SDRE for the problem of linear indefinite time regulator (43) and (44) is a generalization of invariable LQR with indefinite time in which all matrix coefficients depend on the state. SDC matrices are assumed constant and the control input is calculated by solving the problem of LQR optimal control. This approach is a powerful alternative for methods such as solving T-point boundary value problems or Hamilton–Jacobi–Bellman. It is not necessary for solving derivative equations and one important advantage of this method is that it is simple and efficient.

Remark 3. For a scalar system, parameterized SDC or extended linearization is unique to any $x \neq 0 (a (x) = \frac{f (x)}{x})$ , but for multivariate system, there are innumerable of SDC formations. As presentation of SDC is not unique, there is a flexibility that helps to improve the system performance or bring about reconciliation between optimization, stability, robustness and elimination of disturbance. By changing $Q (x)$ and $R (x)$ , it is also possible to bring reconciliation between control and immediate convergence of errors.

Remark 4: Real-time implementation. As mentioned before, SDRE is a full state feedback control requiring all states. Therefore, a state observer should be employed to estimate the states. In real-time implementation, states are sampled at any moment of time $t_{k}$ for a continuous time system, and then a continuous time control signal is calculated and applied to the system. There is virtually no need for system discretion. The most important part of implementation of SDRE is to obtain the steady-state response of the ARE definite time $t_{k}$ . However, these equations should be solved by numerical and repetitive methods, which are time-consuming. To solve this equation, numerical methods such as the Schur complement method and Hamiltonian Matrix have been used. For some systems, though we can use a symbolic software package to obtain a closed solution, the sampling interval $δ_{k} = t_{k + 1} - t_{k}$ is an important practical issue of keeping SDRE stable during implementation.

As shown, any change in $P (x)$ , $A (x)$ and $B (x)$ matrixes of any sampling interval should be trivial relative to $Q (x)$ changes. By selecting larger values for entries of $Q (x)$ , one can improve the stability of the system, although this leads to an excessive control effort. Another point is that for systems with high non-linearity, the limited sampling rate does not allow for increasing it excessively. As a result, reducing sampling rate to a certain limit may cause instability.

Design of robust optimal sliding surface

To establish the sliding mode optimal control law, considering the uncertain system (42), the sliding surface is chosen as follows:

\begin{matrix} s_{opt} (t, x (t)) = G (x) [x (t) - x (0)] - G (x) \\ \int_{0}^{t} [A (x) x - B (x) R^{- 1} B^{T} (x) P (x)] x (τ) d τ \end{matrix}

(52)

where $G (x) \in R^{m \times n}$ and ${G (x), B (x)}$ is non-singular. Equation (51) clearly shows that $s_{opt} (0, x (0)) = 0$ when $t = 0$ , so the system always starts at the predefined sliding surface. By deriving Equation (42), we will have

\begin{matrix} {\overset{\cdot}{s}}_{opt} = G (x) [\overset{\cdot}{x} (t) - A (x) x (t) + B (x) R {(x)}^{- 1} B {(x)}^{T} P (x)] \\ = G (x) [A (x) x (t) + B (x) u (t) - A (x) x (t)] \\ + G (x) B (x) R {(x)}^{- 1} B {(x)}^{T} P (x) x (t)] \end{matrix}

(53)

Now in conditions of ${\overset{\cdot}{S}}_{opt} = 0$ , the equivalent control law for system (42) is obtained as below:

u_{eq} = {[G (x) B (x)]}^{- 1} [G (x) δ (x, t) + G (x) B (x) R^{- 1} B^{T} (x) P (x) x (τ)]

(54)

Finally, substituting (54) into (42), the sliding mode dynamics become

\begin{matrix} \overset{\cdot}{x} (τ) = A (x) x (τ) + B (x) u + δ = A (x) x (τ) \\ + B (x) {[G (x) B (x)]}^{- 1} \\ [G (x) δ + G (x) B (x) R^{- 1} B^{T} (x) P (x) x (τ)] + δ \\ = A (x) x (τ) - B (x) R^{- 1} B^{T} (x) P (x) x (τ) \\ - B (x) {[G (x) B (x)]}^{- 1} G (x) δ + δ \\ = A (x) x (τ) - B (x) R^{- 1} B^{T} (x) P (x) x (τ) + B (x) \tilde{δ} \\ - B (x) {[G (x) B (x)]}^{- 1} G (x) B (x) \overset{\lor}{δ} a \\ = [A (x) - B (x) R^{- 1} B^{T} (x) P (x)] x (τ) \end{matrix}

(55)

Comparing (55) with (46) shows the sliding mode dynamics of uncertain system (42) and the optimal dynamics of the nominal system (46) have exactly the same equation. Thus, according to above sliding surface, the defined control law will be robust against disturbance and, at the same time, guarantees optimal accurate performance of the controlled system (42). So we call $s_{opt} (t, x (t))$ , Equation (52), a robust optimal sliding surface.

Design of robust optimal sliding mode control law

After the sliding surface is chosen, the next step is designing the SMC law. Based on the Lyapunov function $V = \frac{1}{2} S_{opt}^{T} S_{opt}$ , we develop a control law in which switching variables approach the sliding surfaces in a limited time and remain there steadily. A sufficient condition for this is $\overset{\cdot}{V} = S_{opt}^{T} {\overset{\cdot}{S}}_{opt} < 0$ . The proposed SMC law of system (42) is then as follows:

\begin{matrix} u = u_{con} + u_{dis} \\ u_{con} = - R^{- 1} (x) B^{T} (x) P (x) x (τ) \end{matrix}

(56)

u_{d i s} = {[G (x) B (x)]}^{- 1} [k + γ_{0} ‖ G (x) B (x) ‖ + γ_{1} ‖ G (x) B (x) ‖ ‖ x (τ) ‖] s g n (s_{o p t)}

In which $κ ≻ 0$ is an appropriate constant, and

sign (s_{opt}) = {[sign (s_{1}), \dots . sign (s_{m})]}^{T}

(57)

The control law can impel the system trajectories to reach the sliding surface in finite time and maintain it thereafter. However, by considering the chosen Lyapunov function, prove the exponential stability of it:

\begin{matrix} \overset{\cdot}{V} = s_{opt}^{T} {\overset{\cdot}{s}}_{opt} = s_{opt}^{T} (G (x) [\overset{\cdot}{x} (τ) - A (x) x (τ)}) \\ + s_{opt}^{T} G (x) B (x) R {(x)}^{- 1} B {(x)}^{T} P (x) x (τ) = s_{opt}^{T} G (x) \\ [A (x) x (t) + B (x) u (t) + B \tilde{δ} (x, t) - A (x) x (t)] \\ + s_{opt}^{T} G (x) B (x) R {(x)}^{- 1} B {(x)}^{T} P (x) x (τ) \\ \overset{\cdot}{V} = s_{opt}^{T} G (x) [B (x) u (t) + B \tilde{δ} (x, t)] \\ + s_{opt}^{T} G (x) B (x) R {(x)}^{- 1} B {(x)}^{T} P (x) x (τ) = s_{opt}^{T} G (x) \\ [- B (x) [G (x) B (x)]^{- 1} \\ [k + γ_{0} ‖ G (x) B (x) ‖ + γ_{1} ‖ G (x) B (x) ‖ ‖ x ‖] sgn (s_{opt}) \\ + B \tilde{δ} (x, t) \\ V = - k ‖ s_{opt} ‖_{1} + s_{opt}^{T} G (x) B \tilde{δ} (x, t) \\ - [γ_{0} ‖ G (x) B'' (x) ‖ + γ_{1} ‖ G (x) B (x) ‖ ‖ x ‖] {s_{opt}}_{1} \\ \overset{\cdot}{V} \leq - k ‖ s_{opt} ‖_{1} + ‖ G (x) B (x) ‖ ‖ s_{opt} ‖ ‖ \tilde{δ} (x, t) ‖ \\ - [γ_{0} ‖ G (x) B (x) ‖ + γ_{1} ‖ G (x) B (x) ‖ ‖ x ‖] {s_{opt}}_{1} \end{matrix}

(58)

\begin{matrix} \overset{\cdot}{V} \leq - k ‖ s_{opt} ‖_{1} + ‖ G (x) B (x) ‖ ‖ s_{opt} ‖ ‖ (γ_{1} + γ_{1}) ‖ ‖ x ‖) \\ - [γ_{0} ‖ G (x) B (x) ‖ + γ_{1} ‖ G (x) B (x) ‖ ‖ x ‖] ‖ s_{opt} ‖_{1} \\ \overset{\cdot}{V} \leq - k ‖ s_{opt} ‖_{1} + ‖ G (x) B (x) ‖ ‖ s_{opt} ‖ ‖ (γ_{0} + γ_{1}) ‖ ‖ x ‖) \\ - [‖ s_{opt} ‖ - {‖ s_{opt} ‖}_{1}] \end{matrix}

where $‖ S_{opt} ‖_{1}$ represents 1-norm, and because $‖ S_{opt} ‖_{1} \geq ‖ S_{opt} ‖$ , we can show that

\overset{\cdot}{V} = s_{opt}^{T} {\overset{\cdot}{s}}_{opt} \leq k ‖ s_{opt} ‖ < 0 for ‖ s_{opt} ‖ \neq 0

(59)

Now according to Equations (34) and (52), we choose the optimal switching surface variables as follows:

s_{opt} = {[s_{φ} s_{θ} s_{Ψ} s_{x} s_{y} s_{z}]}^{T}

(60)

After selecting switching variables, the only remaining passive variable is G(x), assuming:

\begin{matrix} sw = {[s w_{1} s w_{2} s w_{3} \dots s w_{12}]}^{T} = {[x (t) - x (0)]}^{T} \\ - \int_{0}^{t} [A (x) x - B (x) R^{- 1} B^{T} (x) P (x)] x (τ) d τ \end{matrix}

(61)

We will have

G (x) . sw = s_{opt}

(62)

Equation (61) shows that $G (x)$ is not unique and should be applied to the non-singularity condition of ${G (x), B (x)}$ . So G(x) is chosen as follows:

\begin{matrix} G_{qn} (x) = \\ (\begin{matrix} \begin{matrix} 0 & \begin{matrix} | \frac{S_{φ} / d_{1}}{s w_{2}} | & 0 & \begin{matrix} 0 & 0 & \begin{matrix} 0 & 0 & \begin{matrix} 0 & 0 & \begin{matrix} 0 & 0 & 0 \end{matrix} \end{matrix} \end{matrix} \end{matrix} \end{matrix} \end{matrix} \\ \begin{matrix} \begin{matrix} 0 & 0 & \begin{matrix} 0 & | \frac{S_{φ} / d_{2}}{s w_{4}} | & \begin{matrix} 0 & 0 & \begin{matrix} 0 & 0 & 0 \end{matrix} \end{matrix} \end{matrix} \end{matrix} & 0 & \begin{matrix} 0 & 0 \end{matrix} \end{matrix} \\ \begin{matrix} 0 & 0 & \begin{matrix} 0 & 0 & \begin{matrix} 0 & | \frac{S_{φ} / d_{3}}{s w_{6}} | & \begin{matrix} 0 & 0 & \begin{matrix} 0 & 0 & \begin{matrix} 0 & 0 \end{matrix} \end{matrix} \end{matrix} \end{matrix} \end{matrix} \end{matrix} \\ \begin{matrix} 0 & 0 & \begin{matrix} 0 & 0 & \begin{matrix} 0 & 0 & \begin{matrix} 0 & | \frac{S_{φ} / d_{4}}{s w_{8}} | & \begin{matrix} 0 & \begin{matrix} 0 & \begin{matrix} 0 & 0 \end{matrix} \end{matrix} \end{matrix} \end{matrix} \end{matrix} \end{matrix} \end{matrix} \\ \begin{matrix} 0 & 0 & \begin{matrix} 0 & 0 & \begin{matrix} 0 & 0 & \begin{matrix} 0 & 0 & \begin{matrix} 0 & | \frac{S_{φ} / d_{5}}{s w_{10}} | & \begin{matrix} 0 & 0 \end{matrix} \end{matrix} \end{matrix} \end{matrix} \end{matrix} \end{matrix} \\ \begin{matrix} 0 & 0 & \begin{matrix} 0 & 0 & \begin{matrix} 0 & 0 & \begin{matrix} 0 & 0 & \begin{matrix} 0 & \begin{matrix} 0 & 0 & | \frac{S_{φ} / d_{6}}{s w_{12}} | \end{matrix} \end{matrix} \end{matrix} \end{matrix} \end{matrix} \end{matrix} \end{matrix}) \end{matrix}

(63)

where $d_{1}, \dots ., d_{6}$ are arbitrary constants, which should not be zero. $A (x)$ and $(x)$ , developed by SDRE, are selected by regarding Equations (10), (43) and (46):

\begin{matrix} A_{qn} (x) = (\begin{matrix} 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & a_{2} Ω & 0 & x_{4} a_{1} & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & a_{2} Ω & 0 & 0 & 0 & x_{4} a_{1} & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & x_{6} a_{5} & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 \\ \frac{- g}{x_{1}} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \end{matrix}), B_{qn} (x) = (\begin{matrix} 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & b_{1} & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & b_{2} & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & b_{3} & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 \\ \frac{\cos x_{1} \cos x_{3}}{m} & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & \frac{1}{m} U_{1} & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & \frac{1}{m} U_{1} \end{matrix}) \end{matrix}

(64)

Also weight matrixes $Q (x)$ and $R (x)$ are considered following diagonal matrixes:

Q_{qn} (x) = (\begin{matrix} 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & \frac{10}{| x_{1} |} & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 10^{- 3} & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & \frac{10}{| x_{3} |} & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 10^{- 3} & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & \frac{10}{| x_{5} |} & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 10^{- 3} \end{matrix})

(65)

R_{q n} (x) = (\begin{matrix} 10^{- 3} & 0 & 0 & 0 & 0 & 0 \\ 0 & 1 + 0.01 | x_{7}^{2} | & 0 & 0 & 0 & 0 \\ 0 & 0 & 10^{- 3} & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 + 0.01 | x_{9}^{2} | & 0 & 0 \\ 0 & 0 & 0 & 0 & 10^{- 3} & 0 \\ 0 & 0 & 0 & 0 & 0 & 1 + 0.01 | x_{11}^{2} | \end{matrix})

Note that the selected forms are not unique and can be set as different types regarding different conditions of stability. It is important to remember that these matrices were obtained in a trial and error process. Undoubtedly, considering the flexibility in choosing $Q_{qn} (x)$ and $R_{qn} (x)$ , the system can be controlled precisely by SDRE.

Individual control vectors of cooperative control strategy

In this section, the individual control vectors are designed. These vectors are generated by the ROSMC method, which has been explained in the previous subsections, and are represented by parameters $[U_{1}, U_{2}, U_{3}, U_{4}]$ , which are the equivalent of $(F_{B_{qn}}^{des}, M_{x B_{qn}}^{des}, M_{y B_{qn}}^{des}, M_{z B_{qn}}^{des})$ . Thus the individual control vectors are derived as follows:

\begin{matrix} U_{des - coo p_{qn}} = \\ (\begin{matrix} (- {(G_{qn} B_{qn})}^{- 1} (\begin{matrix} η + γ 0 ‖ G_{qn} B_{qn} ‖ \\ + γ_{1} (‖ G_{qn} B_{qn} ‖ (X_{qn}) \end{matrix})) \times \\ [\tanh (S φ_{qn} / ε), \tanh (S θ_{qn} / ε), \tanh (S ψ_{qn} / ε), \tanh (S y_{qn} / ε), \tanh (S z_{qn} / ε)] \end{matrix}) \end{matrix}

(66)

\begin{matrix} [F_{B_{qn}}^{des}, M_{x B_{qn}}^{des}, M_{y B_{qn}}^{des}, M_{z B_{qn}}^{des}] \\ = - G_{qn} (\begin{matrix} Xqn + \\ {[\begin{matrix} 0 & 0 & 0 & 0 & 0 & 0 \\ - x_{des} & 0 & - y_{des} & 0 & - z_{des} & 0 \end{matrix}]}^{T} \end{matrix}) + U_{des - coo p_{qn}} \end{matrix}

(67)

In these equations, n represents the number of Quadrotors in the coupled system, and Xqn represents the states vector. Virtual control vectors $U_{x}$ and $U_{y}$ are coupled to the vector control system to control the position of the transitional subsystem and are in line with the axis of the x and y:

\begin{matrix} U_{x} = \frac{m}{F_{B_{qn}}^{des}} (- k_{5} sign (s_{x}) + {\overset{\cdot\cdot}{x}}_{d} + α_{1} e_{2}), \\ U_{y} = \frac{m}{F_{B_{qn}}^{des}} (- k_{6} sign (s_{y}) + {\overset{\cdot\cdot}{y}}_{d} + α_{2} e_{4}) \end{matrix}

(68)

Cooperative control vectors

By combining the control fundamental vectors in the Equations (31) and (32) with individual control vectors Equations (67) and (68), the cooperative control vectors for each independently designed Quadrotor can be described as follows:

\begin{matrix} U_{coop | Q_{1}} = U_{Fq 1} F_{Bq 1} + U_{M x_{q 1}} M_{x B_{q 1}}^{des} + U_{M y_{q 1}} M_{y B_{q 1}}^{des} + U_{M z_{q 1}} M_{z B_{q 1}}^{des} \\ U_{coop | Q_{2}} = U_{Fq 2} F_{Bq 2} + U_{M x_{q 2}} M_{x B_{q 2}}^{des} + U_{M y_{q 2}} M_{y B_{q 2}}^{des} + U_{M z_{q 2}} M_{z B_{q 2}}^{des} \\ U_{coop | Q_{3}} = U_{Fq 3} F_{Bq 3} + U_{M x_{q 3}} M_{x B_{q 3}}^{des} + U_{M y_{q 3}} M_{y B_{q 3}}^{des} + U_{M z_{q 3}} M_{z B_{q 3}}^{des} \\ U_{coop | Q_{4}} = U_{Fq 4} F_{Bq 4} + U_{M x_{q 4}} M_{x B_{q 4}}^{des} + U_{M y_{q 4}} M_{y B_{q 4}}^{des} + U_{M z_{4}} M_{z B_{q 4}}^{des} \end{matrix}

(69)

To test the robustness of the controller against external disturbance we chose wind field effect as a destructive factor, which is shown in Figure 3. The wind velocity component $ω (t)$ is expressed with respect to an inertial coordinate frame $x, y, z$ . It has been selected as a periodic signal velocity vector in three directions $ω (t)$ as follows:

ω (t) = {[ω_{x} (t), ω_{y} (t), ω_{z} (t)]}^{T} = 0.1 \times [\begin{matrix} 1.5 + 2.5 \cos (4 t) \\ 2.5 + 1.5 \sin (4 t) \\ 1.5 + 2.5 \sin (4 t) \end{matrix}] \frac{m}{s}

(70)

Figure 3.

Artificial wind gust generating setup.

Remark 5. The genetic algorithm is a well-known method for solving problems to which no satisfactory, obvious solution exists. The mechanisms of GAs are surprisingly simple, involving nothing more complex than string copying and partial string swapping. GAs start with a population of strings and thereafter generate successive populations using the following three basic operations: reproduction, crossover and mutation. The most important feature of this method is the way to transform the system output to the cost function. This cost function is same as in Equation (43). The GAs operator values are decided after numerous experiments. These operator values are considered: the population size is set as 200, the crossover operator is considered Scatter and its value is 0.8, the mutation operator is set as 0.1 and the type of probability is chosen as Gaussian. Also Q and R are formed as:

R = diag [I_{12 \times 12}], Q = diag [I_{12 \times 12}]

(71)

The parameters of proposed controller $α_{1}, \dots ., α_{6}$ , $k_{1}, \dots ., k_{6}$ and $d_{1}, \dots ., d_{6}$ , obtained by GAs, have been presented in Table 1.

Table 1.

Controller parameters.

Item	Value	Item	Value	Item	Value
$α_{1}$	0.0034	$d_{1}$	$10^{- 8}$	$K_{1}$	8.05
$α_{2}$	0.0035	$d_{2}$	$10^{- 8}$	$K_{2}$	13.6
$α_{3}$	0.0018	$d_{3}$	$10^{- 10}$	$K_{3}$	1.89
$α_{4}$	0.0132	$d_{4}$	$10^{- 6}$	$K_{4}$	3.92
$α_{5}$	0.0203	$d_{5}$	$10^{- 2}$	$K_{5}$	7.53
$α_{6}$	0.0156	$d_{6}$	$10^{- 2}$	$K_{6}$	11.98

Simulation results

We now report on the numerical simulation we have carried out to accredit the efficiency of the proposed control scheme. The parameters for simulation of a sample cooperative Quadrotors model are set as (Wang et al., 2014),

\begin{matrix} m = 1.77 kg, d = 0.225 m, I_{x} = 2.16 \times 10^{- 3} kg . m^{2}, \\ I_{y} = 2.16 \times 10^{- 3} kg . m^{2}, I_{z} = 0.33 \times 10^{- 3} kg . m^{2}, \\ g = 9.81 \frac{m}{s^{2}}, J_{r} = 3.357 \times 10^{- 5} kg . m^{2}, \\ b = 2.98 \times 10^{- 6}, l = 0.42 m, \frac{υ_{Mxy}}{υ_{F}} = \frac{1}{400} \end{matrix}

To compare the effectiveness of proposed cooperative controller on system performance and the reduced tracking errors two cases are considered, the first one is grasping, that is, immobilize the object from different initial conditions of each Quadrotor to point-to-point path planning. The second simulation considers grasp and then manipulation for separately described predefined trajectory tracking. In both cases, terminal stability is considered to reach and touch the object at exactly the same time for all Quadrotors.

Point-to-point path planning

The initial conditions of Euler angles of each Quadrotor are set as $[φ, θ, ψ] = {[0.2, 0.4, 0.6]}^{T}$ , and the initial conditions for each of their position are explained in Table 2.

Table 2.

Quadrotors’ initial conditions.

Quadrotor 1	Quadrotor 2	Quadrotor 3	Quadrotor 4
$x_{0} = 0$	$x_{0} = 2.5$	$x_{0} = 2.5$	$x_{0} = 0$
$y_{0} = 0$	$y_{0} = 0$	$y_{0} = 2.5$	$y_{0} = 2.5$
$z_{0} = 0$	$z_{0} = 0$	$z_{0} = 0$	$z_{0} = 0$

For the first simulation, the desired point configuration is $[x, y, z] = {[2, 2, 2]}^{T}$ . The effect of wind is considered an external disturbance formulated in Equation (70), which enters the system. It is considered with respect to the projection path, and is shown in Figure 3. The payload on the end-effector is considered 2.5 kg, whereas the controller is designed for 1.77 kg, which is the mass of end-effector, in order to examine the efficiency of the controller in neutralizing the effect of payload uncertainty. The position and attitude angle response of the system in the presence of wind field are shown in Figures 4 and 5. For the attitude angle, the controller stabilized it at 0 rad in a short time. The top view trajectory tracking and the trajectory of Quadrotors in 3D space are shown in Figures 6 and 7. It is obvious that the system performs well and tracks the desired trajectory in three dimensions.

Figure 4.

Tracking simulation results of desired trajectories of angle (φ, θ, ψ).

Figure 5.

Tracking simulation results of desired trajectories of position (x,y,z).

Figure 6.

Top view trajectory tracking for the Cross configuration.

Figure 7.

Global trajectory of the Quadrotor position.

Predefined trajectory tracking

In order to investigate the efficiency of the proposed method, in this section a simulation is performed for a case in which the end-effector must track a predefined trajectory. The desired trajectory is a spiral, which is described by the function of time according Table 3. Thus, unlike the earlier part of the navigation path, the important thing for this part of the simulation is that it is desirable to follow the trajectory by the Quadrator sets that are defined in the Table 3.

Table 3.

Quadrotors’ desired trajectories.

Quadrotor 1	Quadrotor 2	Quadrotor 3	Quadrotor 4
$x_{1 d} = ρ + \frac{1}{2}$	$x_{2 d} = - ρ + 2$	$x_{3 d} = - ρ + 2$	$x_{4 d} = ρ + \frac{1}{2}$
$y_{1 d} = ρ + \frac{1}{2}$	$y_{2 d} = ρ + 2$	$y_{3 d} = - ρ + 2$	$y_{4 d} = - ρ + 2$
$z_{1 d} = ρ + \frac{1}{2}$	$z_{2 d} = ρ + 2$	$z_{3 d} = ρ + \frac{1}{2}$	$z_{4 d} = ρ + \frac{1}{2}$

With $ρ = 0.5 \tanh (4 t - 3.4)$ .

The convergence of the position trajectories stand for the synchronization of all Quadrotors, according to the task, as can be seen in Figures 8 and 9, respectively. The trajectory of Quadrotors in 3D space is shown in Figure 10. It shows that the system performs well and tracks to the desired trajectory in three dimensions in predefined trajectory tracking, whereas the position error is given in Figure 11, where it can be appreciated that the performance of the control scheme proposed is quite satisfactory, especially in the x- and y-axes. Even if the errors do not converge to zero, they oscillate in a bounded neighbourhood of the origin. Also, Figure 12 shows the stability rotor speed response of a Quadrotor during hovering. The results indicate that the Quadrotors lift up and put down the object, and a stable grasp has been achieved.

Figure 8.

Cooperative Quadrotor exponential position tracking.

Figure 9.

Cooperative Quadrotor exponential position tracking.

Figure 10.

Global trajectory of the Quadrotors position.

Figure 11.

Error tracking of position (x,y,z).

Figure 12.

Control inputs of Quadrotors.

Maximum dynamic load carrying capacity

As is clear from the title, this approach is based on the increase in mass of the final payload to achieve saturation engines and detect the optimal trajectory. In previous work, when determining the maximum load carried by the robots, some constraints affecting the performance of the robot are assumed. Torque and angular speed of rotors and precision linear motion track are important factors in this study. In such a situation, the controller must satisfy the physical constraints of the system to meet the constraints of trajectory tracking. To assess the optimality of these options, the DLCC is used. The DLCC algorithm is the maximum amount of time during the movement with which end-effectors can be carried to an acceptable accuracy. DLCC definition is based on the amount of torque generated by the rotors and the range of allowable errors of the system. Hence, in general, taking into account the limitations of operator error in the implementation of this algorithm, torque can be considered calculated by this system, which is preset in one direction. In this regard, the first limitation is the lower limit of the torque generated by each motor. This constrain is described as follows (Korayem et al., 2014):

U_{\max} = U_{s} - (\frac{U_{s}}{ω_{s}}) ω, U_{\min} = - U_{s} - (\frac{U_{s}}{ω_{s}}) ω

(72)

where $U_{s}$ is the engine stall torque, $ω_{s}$ is angular velocity in free circulation (angular speed of the rotor at no charge) and $ω$ is the angular speed of the motor. Thus, calculating the output torque at each point and considering the maximum and minimum torque condition in Equation (72), the maximum DLCC for the cooperative Quadrotors is obtained. In this process, the Quadrotors are tracking a predefined trajectory with significant precision. The second constraint is defining the boundaries of error for the location parameters of the system. This means that these parameters should track a predefined trajectory with good precision and show an acceptable performance in terms of tracking error. In simulation of this algorithm, the parameters are considered ω_s=286 rad/s and U_s=0.075 rad/s. The first condition limit DLCC moments in the sliding mode controller for a predetermined sinusoidal trajectory tracking algorithm for cooperative Quadrotor are shown in Figure 13. The DLCC for this coupled system was determined to be 15.4 kg. As shown in Figure 13, the rotor torques of the four Quadrotors have a bandwidth limit condition. In addition, with this DLCC, according to Figure 10, the coupled system in both the rotational and translational subsystems has a good performance with high accuracy.

Figure 13.

Saturated motor torque.

Conclusion and future works

We addressed the problem of controlling multiple Quadrotors robots that cooperatively manipulate and transport a payload in three dimensions. An optimal decentralized control algorithm based on ROSMC using the SDRE approach has been proposed for a group of cooperative Quadrotors. We approach to the problem by first developing a model for a single Quadrotor and then extending it to a team of Quadrotors rigidly attached to a payload. We propose individual robot control laws defined with respect to the payload that stabilize the payload along three-dimensional trajectories. This method provides an efficient and systematic procedure to solve a non-linear closed-loop robust control problem. In order to check the optimality of the employed method, the DLCC of the robot was obtained with taking into account of the limits of motor torques and accuracy.

To express the efficiency and optimality of the proposed method, simulation results and computed DLCC were derived for trajectory tracking movement. It is obvious from the simulation results that using our method led to better performance as well as higher DLCC in the presence of external disturbances and parametric uncertainties such as wind field effects. As future work, one may also use the load sharing approach between multiple agents in order to improve the design performance gained in this paper.

Footnotes

Conflict of interest

The authors declare that there are no conflicts of interest.

Funding

This research received no specific grant from any funding agency in the public, commercial or not-for-profit sectors. The authors certify that they have no affiliations with or involvement in any organization or entity with any financial interest or non-financial interest in the subject matter or materials discussed in this manuscript.

References

Abdelrahman

Chang

Park

(2011) Magnetic torque attitude control of a satellite using the state-dependent Riccati equation technique. International Journal of Non-Linear Mechanics 46(5): 758–771.

Bernard

Kondak

Maza

et al . (2011) Autonomous transportation and deployment with aerial robots for search and rescue missions. Journal of Field Robotics 28(6): 914–931.

Bouadi

Bouchoucha

Tadjine

(2007) Sliding Mode Control Based on Backstepping Approach for an UAV Type Quadrotor. World Academy of Science, Engineering and Technology, pp. 22–27.

Cheng

Fink

Kim

et al . (2008) Cooperative towing with multiple robots. In Proceedings of the International Workshop on the Algorithmic Foundations of Robotics, Guanajuato, Mexico.

Cloutier

(1997) State-dependent Riccati equation techniques: an overview. Proceedings of the American Control Conference, Albuquerque, NM, pp. 932–936.

Das

Fierro

Kumar

et al . (2002) A vision-based formation control framework. IEEE Transactions on Robotics and Automation 18(5): 813–825.

Desai

Ostrowski

Kumar

(1998) Controlling formations of multiple mobile robots. In: Proceedings of IEEE International Conference on Robotics and Automation, Leuven, pp. 2864–2869.

Desai

Ostrowski

Kumar

(2001) Modeling and control of formations of nonholonomic mobile robots. IEEE Transactions on Robotics and Automation 17: 905–908.

Kwak

Choi

et al . (2014) Suboptimal control scheme design for interior permanent-magnet synchronous motors: an SDRE-based approach. IEEE Transaction on Power Electronics 29(6): 3020–3031.

10.

Fax

Murray

(2004) Information flow and cooperative control of vehicle formations. IEEE Transactions on Automatic Control 49(9): 1465–1476.

11.

Fink

Michael

Kim

et al . (2010) Planning and control for cooperative manipulation and transportation with aerial robots. International Journal of Robotics Research 30(3): 324–334.

12.

Korayem

Pilechian

(2005) Maximum dynamic load carrying capacity in flexible joint robots using sliding mode control. International Congress on Manufacturing Engineering, Tehran, Iran.

13.

Korayem

Firoozi

Heidari

(2007) Dynamic load carrying capacity of mobile-base flexible link manipulators: feedback linearization approach. IEEE International Conference on Robotics, Sanya, pp. 2172–2177.

14.

Korayem

Yao

Basu

(1992) Dynamic load carrying capacity for a multi-link flexible arm. Control and Robotics Automation, 79–83.

15.

Korayem

Yao

Basu

(1993) Maximum allowable load of flexible manipulator for a given dynamic trajectory. International Journal of Robotics and Computer-Integrated Manufacturing 10(4): 302–309.

16.

Korayem

Zehfroosh

Tourajizadeh

et al . (2014) Optimal motion planning of non-linear dynamic systems in the presence of obstacles and moving boundaries using SDRE: application on cable-suspended robot. Nonlinear Dynamics 76: 1423–1441.

17.

Liu

Tan

Liu

(2007) Robust leader-follower formation control of mobile robots based on a second order kinematics model. Acta Automatica Sinica 33: 947–955.

18.

Massari

Zamaro

(2014) Application of SDRE technique to orbital and attitude control of spacecraft formation flying. Acta Astronautica 94: 409–420.

19.

Mellinger

Shomin

Michael

et al . (2013) Cooperative grasping and transport using multiple Quadrotors. Distributed Autonomous Robotic Systems. Springer Tracts in Advanced Robotics, Vol. 83, pp. 545–588.

20.

Menon

Lam

Crawford

et al . (2002) Real-time computational methods for SDRE nonlinear control of missiles. In: Proceedings of the American Control Conference, Anchorage, AK, pp. 232–237.

21.

Michael

Fink

Kumar

(2011) Cooperative manipulation and transportation with aerial robots. Autonomous Robots 30: 73–86.

22.

Nathan

Almurib

HAF

Kumar

(2011) A review of autonomous multi-agent Quad-rotor control techniques and applications. In: 4th International Conference on Mechatronics (ICOM), Kuala Lumpur, Malaysia, pp. 343–356.

23.

Nian

(2005) Suboptimal strategics of linear quadratic closed-loop differential games: a BMI approach. Acta Automatic Sinica 31(2): 216–222.

24.

Orqueda

OAA

Fierro

(2006) Robust vision-based nonlinear formation control. In: Proceedings of the American Control Conference, Minneapolis, MN, pp. 14–16.

25.

Runcharoon

Srichatrapimuk

(2013) Sliding mode control of Quadrotor. In: International Conference of Technological Advances in Electrical, Electronics and Computer Engineering, pp. 552–556.

26.

Sanchez

Fierro

(2003) Sliding mode control for robot formations. In: Proceedings of the IEEE International Symposium on Intelligent Control, Houston, TX, pp. 438–443.

27.

Sidi

EVO

Sicard

Massicotte

et al . (1998) Adaptive high precision control for a FJ with friction and parameter uncertainties using neural. Electrical and Computer Engineering, Waterloo, Ontario, pp. 325–328.

28.

Tarn

Bejczy

Yun

(1986) Coordinated control of two robot arms. In: Proceedings of the IEEE International Conference Robotics and Automation, Vol. 3, pp. 1193–1202.

29.

Wang

Nian

Wang

(2012) Cooperative control of discrete-time linear multi-agent systems with fixed information structure. Journal of Control Theory 10(3): 403–409.

30.

Wang

Zhu

(2014) Adaptive sliding mode control for Quadrotor aerial robot with I type configuration. International Journal of Automation and Control Engineering (IJACE) 3(1): 20–26.

31.

Wang

Ravani

(1988) Dynamic load carrying capacity of mechanical manipulators. Transactions of ASME, Journal of Dynamic Systems, Measurement and Control 110: 46–61.

32.

Willard

Randal

(2002) Ensuring stability of state-dependent Riccati equation controllers via satisficing. In: Proceedings of the 41st IEEE Conference on Decision and Control, Las Vegas, NV, pp. 2645–2650.

33.

Yoshikawa

Zheng

(1990) Coordinated dynamic hybrid position/force control for multiple robot manipulators handling one constrained object. In: Proceedings of IEEE International Conference on Robotics and Automation, Cincinnati, OH, Vol. 2, pp. 1178–1183.

34.

Zeghlache

(2013) Sliding mode control strategy for a 6 DOF Quadrotor helicopter. Journal of Electrical Engineering 13: 1–6.

35.

Zhang

Agrawal

Hemanshu

et al . (2005) Optimal control using state dependent Riccati equation (SDRE) for a flexible cabletransporter system with arbitrarily varying lengths. In: Proceedings of the 2005 IEEE Conference on Control Applications, Toronto, Canada, pp. 1063–1068.