This paper presents the full dynamics and control of arbitrary number of quadrotor unmanned aerial vehicles (UAVs) transporting a rigid body. The rigid body is connected to the quadrotors via flexible cables where each flexible cable is modeled as a system of arbitrary number of serially connected links. It is shown that a coordinate-free form of equations of motion can be derived for the complete model without any simplicity assumptions that commonly appear in other literature, according to Lagrangian mechanics on a manifold. A geometric nonlinear controller is presented to transport the rigid body to a fixed desired position while aligning all of the links along the vertical direction. A rigorous mathematical stability proof is given and the desirable features of the proposed controller are illustrated by numerical examples and experimental results.

# Stabilization of a Rigid Body Payload With Multiple Cooperative Quadrotors OPEN ACCESS

**Farhad A. Goodarzi**

**Taeyoung Lee**

Contributed by the Dynamic Systems Division of ASME for publication in the JOURNAL OF DYNAMIC SYSTEMS, MEASUREMENT, AND CONTROL. Manuscript received November 6, 2015; final manuscript received June 7, 2016; published online August 10, 2016. Assoc. Editor: Yongchun Fang.

*J. Dyn. Sys., Meas., Control*138(12), 121001 (Aug 10, 2016) (17 pages) Paper No: DS-15-1557; doi: 10.1115/1.4033945 History: Received November 06, 2015; Revised June 07, 2016

Quadrotor UAVs are being considered for various missions such as Mars surface exploration, search and rescue, and particularly payload transportation. There are various applications for aerial load transportation such as usage in construction, military operations, emergency response, or delivering packages. Load transportation with UAVs can be performed using a cable or by grasping the payload [1,2]. There are several limitations for grasping a payload with UAVs such as in situations where the landing area is inaccessible, or, it transporting a heavy/bulky object by multiple quadrotors (Fig. 1).

In most of the prior works, the dynamics of aerial transportation has been simplified due to the inherent dynamic complexities. For example, it is assumed that the dynamics of the payload is considered completely decoupled from quadrotors, and the effects of the payload and the cable are regarded as arbitrary external forces and moments exerted to the quadrotors [8–10], thereby making it challenging to suppress the swinging motion of the payload actively, particularly for agile aerial transportations.

Recently, the coupled dynamics of the payload or cable has been explicitly incorporated into control system design [11]. In particular, a complete model of a quadrotor transporting a payload modeled as a point mass, connected via a flexible cable is presented, where the cable is modeled as serially connected links to represent the deformation of the cable [12,13]. In these studies, the payload simplified and considered as a point mass without the attitude and the moment of inertia. In another study, multiple quadrotors transporting a rigid body payload have been studied [14], but it is assumed that the cables connecting the rigid body payload and quadrotors are always taut. These assumptions and simplifications in the dynamics of the system reduce the stability of the controlled system, particularly in rapid and aggressive load transportation where the motion of the cable and payload is excited nontrivially.

The other critical issue in designing controllers for quadrotors is that they are mostly based on local coordinates. Some aggressive maneuvers are demonstrated at Ref. [15] based on Euler angles. However, they involve complicated expressions for trigonometric functions, and they exhibit singularities in representing quadrotor attitudes, thereby restricting their ability to achieve complex rotational maneuvers significantly. A quaternion-based feedback controller for attitude stabilization was shown in Ref. [16]. By considering the Coriolis and gyroscopic torques explicitly, this controller guarantees exponential stability. Quaternions do not have singularities but, as the three-sphere double-covers the special orthogonal group, one attitude may be represented by two antipodal points on the three-sphere. This ambiguity should be carefully resolved in quaternion-based attitude control systems, otherwise they may exhibit unwinding, where a rigid body unnecessarily rotates through a large angle even if the initial attitude error is small [17]. To avoid these, an additional mechanism to lift attitude onto the unit-quaternion space is introduced [18].

Recently, the dynamics of a quadrotor UAV is globally expressed on the special Euclidean group, $SE(3)$, and nonlinear control systems are developed to track outputs of several flight modes [19]. Several aggressive maneuvers of a quadrotor UAV are demonstrated based on a hybrid control architecture, and a nonlinear robust control system is also considered in Refs. [20,21]. As they are directly developed on the special orthogonal group, complexities, singularities, and ambiguities associated with minimal attitude representations or quaternions are completely avoided. The proposed control system is particularly useful for rapid and safe payload transportation in complex terrain, where the position of the payload should be controlled concurrently while suppressing the deformation of the cables.

Comparing with the prior work of the authors in Refs. [22–24] and other existing studies, this paper is the first study considering a complete model which includes a rigid body payload with attitude, arbitrary number of quadrotors, and flexible cables. Also, this study presents a control system to stabilize the rigid body at desired position. Geometric nonlinear controllers are developed for the presented model. More explicitly, we show that the rigid body payload is asymptotically transported into a desired location, while aligning all of the links along the vertical direction corresponding to a hanging equilibrium.

In short, new contributions and the unique features of the dynamics model and control system proposed in this paper compared with other studies are as follows: (i) it is developed for the full dynamic model of arbitrary number of multiple quadrotor UAVs on $SE(3)$ transporting a rigid body connected via flexible cables, including the coupling effects between the translational dynamics and the rotational dynamics on a nonlinear manifold, (ii) the control systems are developed directly on the nonlinear configuration manifold in a coordinate-free fashion. Thus, singularities of local parameterization are completely avoided to generate agile maneuvers in a uniform way, (iii) a rigorous Lyapunov analysis is presented to establish stability properties without any timescale separation assumption or singular perturbation, (iv) an integral control term is proposed to guarantee asymptotical convergence of tracking error variables in the presence of unstructured uncertainties in both rotational and translational dynamics, and (v) the proposed algorithm is validated with experiments for payload transportation with multiple cooperative quadrotor UAVs. A rigorous and complete mathematical analysis for multiple quadrotor UAVs transporting a payload on $SE(3)$ with experimental validations for payload transportation maneuvers is unprecedented.

Consider a rigid body with the mass $m0\u2208\mathbb{R}$ and the moment of inertia $J0\u2208\mathbb{R}3\xd73$, being transported with *n* arbitrary number of quadrotors as shown in Fig. 2. The location of the mass center of the rigid body is denoted by $x0\u2208\mathbb{R}3$, and its attitude is given by $R0\u2208SO(3)$, where the special orthogonal group is given by $SO(3)={R\u2208\mathbb{R}3\xd73\u2223RTR=I,det(R)=1}$. We choose an inertial frame ${e1,e2,e3}$ and body-fixed frame ${b1,b2,b3}$ attached to the payload. We also consider a body-fixed frame attached to the *i*th quadrotor ${b1i,b2i,b3i}$. In the inertial frame, the third axes $e3$ points downward along the gravity and the other axes are chosen to form an orthonormal frame.

The mass and the symmetric moment of inertia of the *i*th quadrotor are denoted by $mi\u2208\mathbb{R}$ and $Ji\u2208\mathbb{R}3\xd73$, respectively. The cable connecting each quadrotor to the rigid body is modeled as an arbitrary numbers of links for each quadrotor with varying masses, *m _{ij}*, and lengths,

*l*. The direction of the

_{ij}*j*th link of the

*i*th quadrotor, measured outward from the quadrotor toward the payload is defined by the unit vector $qij\u2208S2$, where $S2={q\u2208\mathbb{R}3|\Vert q\Vert =1}$, where the mass and length of that link is denoted with

*m*and

_{ij}*l*respectively. The number of links in the cable connected to the

_{ij}_{,}*i*th quadrotor is defined as

*n*. The configuration manifold for this system is given by $SO(3)\xd7\mathbb{R}3\xd7(SO(3)n)\xd7(S2)\Sigma i=1nni$.

_{i}The *i*th quadrotor can generate a thrust force of $\u2212fiRie3\u2208\mathbb{R}3$ with respect to the inertial frame, where $fi\u2208\mathbb{R}$ is the total thrust magnitude of the *i*th quadrotor. It also generates a moment $Mi\u2208\mathbb{R}3$ with respect to its body-fixed frame. Also, we define $\Delta xi$ and $\Delta Ri\u2208\mathbb{R}3$ as fixed disturbances applied to the *i*th quadrotor's translational and rotational dynamics, respectively. It is also assumed that an upper bound of the infinite norm of the uncertainty is known

Throughout this paper, $\lambda m(A)$ and $\lambda M(A)$ denote the minimum eigenvalue and the maximum eigenvalue of a square matrix *A*, respectively, and *λ _{m}* and

*λ*are the minimum eigenvalue and the maximum eigenvalue of the inertia matrix

_{M}*J*, i.e., $\lambda m=\lambda m(J)$ and $\lambda M=\lambda M(J)$. The two-norm of a matrix

*A*is denoted by $\Vert A\Vert $. The standard dot product is denoted by $x\xb7y=xTy$ for any $x,y\u2208\mathbb{R}3$.

The kinematics equations for the links, payload, and quadrotors are given by

where $\omega ij\u2208\mathbb{R}3$ is the angular velocity of the *j*th link in the *i*th cable satisfying $qij\xb7\omega ij=0$. Also, $\Omega 0\u2208\mathbb{R}3$ is the angular velocity of the payload and $\Omega i\u2208\mathbb{R}3$ is the angular velocity of the *i*th quadrotor, expressed with respect to the corresponding body-fixed frame. The *hat map*$\xb7\u0302:\mathbb{R}3\u2192SO(3)$ is defined by the condition that $x\u0302y=x\xd7y$ for all $x,y\u2208\mathbb{R}3$. More explicitly, for a vector $a=[a1,a2,a3]T\u2208\mathbb{R}3$, the matrix $a\u0302$ is given by

This identifies the Lie algebra $SO(3)$ with $\mathbb{R}3$ using the vector cross product in $\mathbb{R}3$. The inverse of the hat map is denoted by the *vee* map, $\u2228:SO(3)\u2192\mathbb{R}3$.

The position of the *i*th quadrotor is given by

where $\rho i\u2208\mathbb{R}3$ is the vector from the center of mass of the rigid body to the point that *i*th cable is connected to the rigid body. Similarly, the position of the *j*th link in the cable connecting the *i*th quadrotor to the rigid body is given by

We derive equations of motion according to Lagrangian mechanics. Total kinetic energy of the system is given by

The gravitational potential energy is given by

where it is assumed that the unit vector *e*_{3} points downward along the gravitational acceleration as shown at Fig. 2. The corresponding Lagrangian of the system is $L=T\u2212V$.

Coordinate-free form of Lagrangian mechanics on the two-sphere $S2$ and the special orthogonal group $SO(3)$ for various multibody systems has been studied in Refs. [25,26]. The key idea is representing the infinitesimal variation of $Ri\u2208SO(3)$ in terms of the exponential map

for $\eta i\u2208\mathbb{R}3$. The corresponding variation of the angular velocity is given by $\delta \Omega i=\eta \u02d9i+\Omega i\xd7\eta i$. Similarly, the infinitesimal variation of $qij\u2208S2$ is given by

for $\xi ij\u2208\mathbb{R}3$ satisfying $\xi ij\xb7qij=0$. Using these, we obtain the following Euler–Lagrange equations.

Proposition 1. *The equations of motion for the proposed payload transportation system are as follows:*

*Here, the total mass M _{T} of the system and the mass of the ith quadrotor and its flexible cable M_{iT} are defined as*

*and the constants related to the mass of links are given as*

*The equations of motion can be rearranged in a matrix form as follows:*

*where the state vector*$X\u2208\mathbb{R}DX$* with*$DX=6+3\u2211i=1nni$* is given by*

*and matrix*$N\u2208\mathbb{R}DX\xd7DX$* is defined as*

*where the submatrices are defined as*

*and the submatrix*$Nqqi\u2208\mathbb{R}3ni\xd73ni$* is given by*

*The*$P\u2208\mathbb{R}DX$* matrix is*

*and submatrices of**P** matrix are also defined as*

These equations are derived directly on a nonlinear manifold. The dynamics of the payload, flexible cables, and quadrotors are considered explicitly, and they avoid singularities and complexities associated to local coordinates.

Let $x0d\u2208\mathbb{R}3$ be the desired position of the payload. The desired attitude of the payload is considered as $R0d=I3\xd73$, and the desired direction of links is aligned along the vertical direction. The corresponding location of the *i*th quadrotor at this desired configuration is given by

We wish to design control forces *f _{i}* and control moments

*M*of quadrotors such that this desired configuration becomes asymptotically stable.

_{i}Control forces for each quadrotor are given by $\u2212fiRie3$ for the given equations of motion (12)–(15). As such, the quadrotor dynamics is underactuated. The total thrust magnitude of each quadrotor can be arbitrary chosen, but the direction of the thrust vector is always along the third body-fixed axis, represented by *R _{i}e*

_{3}. But, the rotational attitude dynamics of the quadrotors are fully actuated, and they are not affected by the translational dynamics of the quadrotors or the dynamics of links.

Based on these observations, in this section, we simplify the model by replacing the $\u2212fiRie3$ term by a fictitious control input $ui\u2208\mathbb{R}3$, and design an expression for *u _{i}* to asymptotically stabilize the desired equilibrium. In other words, we assume that the attitude of the quadrotor can be instantaneously changed. Also, $\Delta xi$ are ignored in the simplified dynamic model. The effects of the attitude dynamics are incorporated in Sec. 4.

The control system for the simplified dynamic model is developed based on the linearized equations of motion. At the desired equilibrium, the position and the attitude of the payload are given by $x0d$ and $R0d=I3$, respectively. Also, we have $qijd=e3$ and $Rid=I3$. In this equilibrium configuration, the control input for the *i*th quadrotor is

and the variation of the attitude of the payload is defined as

for $\eta 0\u2208\mathbb{R}3$. The variation of *q _{ij}* can be written as

where $\xi ij\u2208\mathbb{R}3$ with $\xi ij\xb7e3=0$. The variation of *ω _{ij}* is given by $\delta \omega ij\u2208\mathbb{R}3$ with $\delta \omega ij\xb7e3=0$. Therefore, the third element of each of

*ξ*and $\delta \omega ij$ for any equilibrium configuration is zero, and they are omitted in the following linearized equations. The state vector of the linearized equation is composed of $CT\xi ij\u2208\mathbb{R}2$, where $C=[e1,\u205fe2]\u2208\mathbb{R}3\xd72$. The variation of the control input $\delta ui\u2208\mathbb{R}3\xd71$, is given as $\delta ui=ui\u2212uid$.

_{ij}Proposition 2. *The linearized equations of the simplified dynamic model are given by*

*where*$g(x,x\u02d9)$* corresponds to the higher-order term and the state vector*$x\u2208\mathbb{R}Dx$* with*$Dx=6+2\u2211i=1nni$* is given by*

*and*$\delta u=[\delta u1T,\u205f\delta u2T,\u205f\cdots ,\u205f\delta unT]T\u2208\mathbb{R}3n\xd71$*. The matrix*$M\u2208\mathbb{R}Dx\xd7Dx$* are defined as*

*where the submatrices are defined as*

*and the submatrix*$Mqqi\u2208\mathbb{R}2ni\xd72ni$* is given by*

*The matrix*$G\u2208\mathbb{R}Dx\xd7Dx$* is defined as*

*where*$G\Omega 0\Omega 0=\u2211i=1n(m0/n)g\rho \u0302ie\u03023$* and the submatrices*$Gi\u2208\mathbb{R}2ni\xd72ni$* are*

*The matrix*$B\u2208\mathbb{R}Dx\xd73n$* is given by*

We present the following proportional-derivative-type control system for the linearized dynamics:

for controller gains $Kxi,Kx\u02d9i\u2208\mathbb{R}3\xd7Dx$. Provided that Eq. (28) is controllable, we can choose the combined controller gains $Kx=[Kx1T,\u2009\u2026\u205fKxnT]T,\u2009Kx\u02d9=[Kx\u02d91T,\u2009\u2026Kx\u02d9nT]T\u2208\mathbb{R}3n\xd7Dx$ such that the equilibrium is asymptotically stable for the linearized equations [27]. Then, the equilibrium becomes asymptotically stable for the nonlinear Euler–Lagrange equation. The controlled linearized system can be written as

where $z1=[x,x\u02d9]T\u2208\mathbb{R}2Dx$ and

We choose $Kx$ and $Kx\u02d9$ such that $A\u2208\mathbb{R}2Dx\xd72Dx$ is Hurwitz. Then for any positive definite matrix $Q\u2208\mathbb{R}2Dx\xd72Dx$, there exist a positive definite and symmetric matrix $P\u2208\mathbb{R}2Dx\xd72Dx$ such that $ATP+PA=\u2212Q$ according to Ref. [27, Theorem 3.6].

The control system designed at Sec. 3 is based on a simplifying assumption that each quadrotor can generate a thrust along any direction. In the full dynamic model, the direction of the thrust for each quadrotor is parallel to its third body-fixed axis always. In this section, the attitude of each quadrotor is controlled such that the third body-fixed axis becomes parallel to the direction of the ideal control force designed in Sec. 3. Also in the full dynamics model, we consider the $\Delta xi$ in the control design and introduce a new integral term to eliminate the disturbances and uncertainties. The central idea is that the attitude *R _{i}* of the quadrotor is controlled such that its total thrust direction $\u2212Rie3$, corresponding to the third body-fixed axis, asymptotically follows the direction of the fictitious control input

*u*. By choosing the total thrust magnitude properly, we can guarantee local asymptotical stability for the full dynamic model.

_{i}Let $Ai\u2208\mathbb{R}3$ be the ideal total thrust of the *i*th quadrotor that locally asymptotically stabilize the desired equilibrium, defined as

where $fid\u2208\mathbb{R}$ and $uid\u2208\mathbb{R}3$ are the total thrust and control input of each quadrotor at its equilibrium, respectively, and the following integral term $ex\u2208\mathbb{R}Dx$ is added to eliminate the effect of disturbance $\Delta xi$ in the full dynamic model:

where $Kz=[kzI3,kzI3,kz1I3\xd72,\u2026kznI3\xd72]\u2208\mathbb{R}3\xd7Dx$ is an integral gain. For a positive constant $\sigma \u2208\mathbb{R}$, a saturation function $sat\sigma :\mathbb{R}\u2192[\u2212\sigma ,\sigma ]$ is introduced as

If the input is a vector $y\u2208\mathbb{R}n$, then the above saturation function is applied element by element to define a saturation function $sat\sigma (y):\mathbb{R}n\u2192[\u2212\sigma ,\sigma ]n$ for a vector.

From the desired direction of the third body-fixed axis of the *i*th quadrotor, namely, $b3ci\u2208S2$, is given by

This provides a two-dimensional constraint on the three-dimensional desired attitude of each quadrotor, such that there remains one degree-of-freedom. To resolve it, the desired direction of the first body-fixed axis $b1i(t)\u2208S2$ is introduced as a smooth function of time. Due to the fact that the first body-fixed axis is normal to the third body-fixed axis, it is impossible to follow an arbitrary command $b1i(t)$ exactly. Instead, its projection onto the plane normal to $b3ci$ is followed, and the desired direction of the second body-fixed axis is chosen to constitute an orthonormal frame [23]. More explicitly, the desired attitude of the *i*th quadrotor is given by

which is guaranteed to be an element of $SO(3)$. The desired angular velocity is obtained from the attitude kinematics equation, $\Omega ic=(RicTR\u02d9ic)\u2228\u2208\mathbb{R}3$.

Define the tracking error vectors for the attitude and the angular velocity of the *i*th quadrotor as

and a configuration error function on $SO(3)$ as follows:

The thrust magnitude is chosen as the length of *u _{i}*, projected on to $\u2212Rie3$, and the control moment is chosen as a tracking controller on $SO(3)$

where $kRi,k\Omega i$, and *k _{I}* are positive constants and the following integral term is introduced to eliminate the effect of fixed disturbance $\Delta Ri$:

where *c*_{2} is a positive constant. Stability of the corresponding controlled systems for the full dynamic model can be studied by showing the error due to the discrepancy between the desired direction $b3ci$ and the actual direction $Rie3$.

Proposition 3. *Consider control inputs f _{i}, M_{i} defined in Eqs.*(41)

*and*(42)

*. There exist controller parameters and gains such that, (i) the zero equilibrium of tracking error is stable in the sense of Lyapunov; (ii) the tracking errors*$eRi,\u2009e\Omega i$

*,*

*x**,*$x\u02d9$

*asymptotically converge to zero as t → ∞; and (iii) the integral terms*$eIi$

*and*$ex$

*are uniformly bounded*.

By utilizing geometric control systems for quadrotor, we show that the hanging equilibrium of the links can be asymptotically stabilized while translating the payload to a desired position and attitude. The control systems proposed explicitly consider the coupling effects between the cable/load dynamics and the quadrotor dynamics. We presented a rigorous Lyapunov stability analysis to establish stability properties without any timescale separation assumptions or singular perturbation, and a new nonlinear integral control term is designed to guarantee robustness against unstructured uncertainties in both rotational and translational dynamics.

We demonstrate the desirable properties of the proposed control system with numerical examples. Two cases are presented. At the first case, a payload is transported to a desired position from the ground. The second case considers stabilization of a payload with large initial attitude errors.

Consider four quadrotors (*n* = 4) connected via flexible cables to a rigid body payload. Initial conditions are chosen as

The desired position of the payload is chosen as

The mass properties of quadrotors are chosen as

and the attitude control gains of each quadrotor are defined with the following expressions:

where considering $\omega nR=8$ and $\zeta R=0.7$, the attitude control gains are given by $kRi=0.6724$ and $k\Omega i=0.1177$ for all quadrotors and $kI=kz=0.01$. The linear controller gains which are $Kxi$ and $Kx\u02d9i$ are calculated using the linear quadratic regulator method which each has a dimension of 12 × 46. The payload is a box with mass $m0=0.5\u2009kg$, and its length, width, and height are 0.6, 0.8, and 0.2 m, respectively. Each cable connecting the rigid body to the *i*th quadrotor is considered to be *n _{i}* = 5 rigid links. All the links have the same mass of

*m*= 0.01 kg and length of

_{ij}*l*= 0.15 m. Each cable is attached to the following points of the payload:

_{ij}In the second case, we consider large initial errors for the attitude of the payload and quadrotors. Initially, the rigid body is tilted about its *b*_{1} axis by 30 deg, and the initial direction of the links is chosen such that two cables are curved along the horizontal direction. The initial conditions are given by

where *R _{x}*(30 deg) denotes the rotation about the first axis by 30 deg. The initial attitude of quadrotors is chosen as

The mass properties of quadrotors and cables are chosen as previous example. The payload mass is *m* = 1.0 kg, and its length, width, and height are 1.0, 1.2, and 0.2 m, respectively. Each cable is attached to the following points of the payload:

Figure 5 illustrates the tracking errors, and the total thrust of each quadrotor. Snapshots of the controlled maneuvers are also illustrated at Fig. 6. It is shown that the proposed controller is able to stabilize the payload and cables at their desired configuration even from the large initial attitude errors.