Geometric Adaptive Control of Quadrotor UAVs Transporting a Cable-Suspended Rigid Body

Transcription

1 Geometric Adaptive Control of Quadrotor UAVs Transporting a Cable-Suspended Rigid Body Taeyoung Lee Abstract This paper is focused on tracking control for a rigid body payload that is connected to an arbitrary number of quadrotor unmanned aerial vehicles via rigid links. An intrinsic form of the equations of motion is derived on the nonlinear configuration manifold and a geometric adaptive controller is constructed such that the payload asymptotically follows a given desired trajectory for its position and attitude in the presence of uncertainties. The unique feature is that the coupled dynamics between the rigid body payload links and quadrotors are explicitly incorporated into control system design and stability analysis. These are developed in a coordinate-free fashion to avoid singularities and complexities that are associated with local parameterizations. The desirable features of the proposed control system are illustrated by a numerical example. I. INTRODUCTION Aerial transport of payloads by towed cables is common in emergency response industrial operations and military missions. Examples of aerial transportation range from search and rescue missions where individuals are lifted from dangerous situations to the delivery of heavy equipment to the top of a tall building that is difficult to reach by other means. Transportation of a cable-suspended load has been studied traditionally for human-operated helicopters. Recently small unmanned aerial vehicles or quadrotors are also considered for load transportation and deployments This is to utilize the high thrust-to-weight ratio of quadrotors or multi-rotor aerial vehicles in autonomous aerial load transportation. However these are based on simplified dynamics models. For example the effects of the payload are considered as additional force and torque exerted to quadrotors instead of considering the dynamic coupling between the payload and the quadrotor and a pre-computed trajectory that minimizes swing motion of the payload is followed instead of actively controlling the motion of payload and cable 4. As such these may not be suitable for agile load transportation where the motion of cable and payload should be actively suppressed online. Recently geometric nonlinear control systems are developed for the complete dynamic model of multiple quadrotors transporting a common payload cooperatively 6. It is also generalized for a quadrotor with a payload connected by flexible cable that is modeled as a serially-connected links to incorporate the effects of deforming cables 7. However in these results it is assumed that the payload is modeled Taeyoung Lee Mechanical and Aerospace Engineering George Washington University Washington DC 5 tylee@gwu.edu This research has been supported in part by NSF under the grant CMMI- 43 (transferred from 955) CMMI-3358 and CNS by a point mass. Such assumption is quite restrictive for practical cases where the size of the payload is comparable to the quadrotors and the length of cables. Also the effects of modeling errors or unknown disturbances are not considered in the control system design and therefore it is impractical to implement those results in experimental settings. The objective of this paper is to construct a control system for an arbitrary number of quadrotors connected to a rigid body payload via rigid links with explicit consideration on uncertainties. This is challenging in the sense that dynamically coupled quadrotors should cooperate safely to transport a rigid body. This is in contrast to the existing results on formation control of decoupled multi-agent systems. In this paper a coordinate-free form of the equations of motion is derived according to Lagrange mechanics on a nonlinear manifold and a geometric adaptive control system is designed such that the rigid body payload asymptotically follows a given desired trajectory of both the payload position and attitude. The unique property of the proposed control system is that the nontrivial coupling effects between the dynamics of payload cable and multiple quadrotors are explicitly incorporated into control system design without any simplifying assumption. Compared with the preliminary work in 8 this paper adopts nonlinear adaptive controls and finite-time stability such that the tracking errors asymptotically converge to zero in the presence of uncertainties. Another distinct feature is that the equations of motion and the control systems are developed directly on the nonlinear configuration manifold intrinsically. Therefore singularities of local parameterization are completely avoided to generate agile maneuvers of the payload in a uniform way. In short the proposed control system is particularly useful for rapid and safe payload transportation in complex terrain where the position and attitude of the payload should be controlled concurrently in a fast manner to avoid collision with obstacles. Most of the existing control systems of aerial load transportation suffer from limited agility as they are based on reactive assumptions that ignore important dynamic characteristics of aerial load transportation. The proposed control system explicitly integrates the comprehensive dynamic characteristics to achieve extreme maneuverability in aerial load transportation. To the author s best knowledge adaptive tracking controls of a cable-suspended rigid body have not been studied as mathematically rigorously as presented in this paper. This paper is organized as follows. A dynamic model is presented and the problem is formulated at Section II. Control systems are constructed at Sections III and IV which are

2 e 3 e e m i J i R i SO(3) ρ i q i S x R 3 m J R SO(3) Fig.. Dynamics model: n quadrotors are connect to a rigid body m via massless links l i. The configuration manifold is R 3 SO(3) (S SO(3)) n. followed by a numerical example at Section V. II. PROBLEM FORMULATION Consider n quadrotor UAVs that are connected to a payload that is modeled as a rigid body via massless links (see Figure ). Throughout this paper the variables related to the payload is denoted by the subscript and the variables for the i-th quadrotor are denoted by the subscript i which is assumed to be an element of I = { n} if not specified. We choose an inertial reference frame { e e e 3 } and body-fixed frames { b j b j b j3 } for j n as follows. For the inertial frame the third axis e 3 points downward along the gravity and the other axes are chosen to form an orthonormal frame. The origin of the j-th body-fixed frame is located at the center of mass of the payload for j = and at the mass center the quadrotor for j n. The third body-fixed axis b i3 is normal to the plane defined by the centers of rotors and it points downward. The location of the mass center of the payload is denoted by x R 3 and its attitude is given by R SO(3) where the special orthogonal group is defined by SO(3) = {R R 3 3 R T R = I detr = }. Let ρ i R 3 be the point on the payload where the i-th link is attached and it is represented with respect to the zeroth body-fixed frame. The other end of the link is attached to the mass center of the i-th quadrotor. The direction of the link from the mass center of the i-th quadrotor toward the payload is defined by the unit-vector q i S where S = {q R 3 q = } and the length of the i-th link is denoted by l i R. Let x i R 3 be the location of the mass center of the i- th quadrotor with respect to the inertial frame. As the link is assumed to be rigid we have x i = x + R ρ i l i q i. The attitude of the i-th quadrotor is defined by R i SO(3) which represents the linear transformation of the representation of a vector from the i-th body-fixed frame to the inertial frame. In summary the configuration of the presented system is described by the position x and the attitude R of the payload the direction q i of the links and the attitudes R i of the quadrotors. The corresponding configuration manifold of this system is R 3 SO(3) (S SO(3)) n. l i The mass and the inertia matrix of the payload are denoted by m R and J R 3 3 respectively. The dynamic model of each quadrotor is identical to 9. The mass and the inertia matrix of the i-th quadrotor are denoted by m i R and J i R 3 3 respectively. The i-th quadrotor can generates a thrust f i R i e 3 R 3 with respect to the inertial frame where f i R is the total thrust magnitude and e 3 = T R 3. It also generates a moment M i R 3 with respect to its bodyfixed frame. The control input of this system corresponds to {f i M i } i n. Disturbances are modeled as follows. Let x R R 3 be arbitrary but fixed disturbance force and moment acting on the mass center of the payload respectively. The disturbance force and moment exerted on the i-th quadrotor are denoted by xi Ri R 3 respectively. The disturbance forces are represented with respect to the inertial frame and the disturbance moments are represented with respect to the corresponding body-fixed frame. Throughout this paper the -norm of a matrix A is denoted by A and its maximum eigenvalue and minimum eigenvalues are denoted by λ M A and λ m A respectively. The standard dot product is denoted by x y = x T y for any x y R 3. A. Equations of Motion The kinematic equations for the payload quadrotors and links are given by q i = ω i q i = ˆω i q i () Ṙ = R ˆΩ Ṙ i = R i ˆΩi () where ω i R 3 is the angular velocity of the i-th link satisfying q i ω i = and Ω and Ω i R 3 are the angular velocities of the payload and the i-th quadrotor expressed with respect to its body-fixed frame respectively. The hat map ˆ : R 3 so(3) is defined by the condition that ˆxy = x y for all x y R 3 and the inverse of the hat map is denoted by the vee map : so(3) R 3 where so(3) denotes the set of 3 3 skew-symmetric matrices i.e. so(3) = {S R 3 3 S T = S} and it corresponds to the Lie algebra of SO(3). Several properties of the hat map are summarized as follows : ˆxy = x y = y x = ŷx (3) traˆx = trˆx(a A T ) = x T (A A T ) (4) ˆxA + A T ˆx = ({tra I 3 3 A} x) (5) RˆxR T = (Rx) (6) for any x y R 3 A R 3 3 and R SO(3). We derive equations of motion according to Lagrangian mechanics. The velocity of the i-th quadrotor is given by ẋ i = ẋ + Ṙρ i l i q i. The kinetic energy of the system is composed of the translational kinetic energy and the rotational kinetic energy of the payload and quadrotors: T = m ẋ + Ω J Ω

3 + m i ẋ + Ṙρ i l i q i + Ω i J i Ω i. (7) The gravitational potential energy is given by U = m ge 3 x m i ge 3 (x + R ρ i l i q i ) (8) where the unit-vector e 3 points downward along the gravitational acceleration as shown at Fig.. The corresponding Lagrangian of the system is L = T U. Coordinate-free form of Lagrangian mechanics on the twosphere S and the special orthogonal group SO(3) for various multibody systems has been studied in. The key idea is representing the infinitesimal variation of q i S in terms of the exponential map: δq i = d dɛ exp(ɛˆξ i )q i = ξ i q i (9) ɛ= for a vector ξ i R 3 with ξ i q i =. Similarly the variation of R i is given by δr i = d dɛ R i exp(ɛˆη i )q i = R iˆη i () ɛ= for η i R 3. By using these expressions the equations of motion can be obtained from Hamilton s variational principle as follows (see Appendix A for more detailed derivations). M q (ẍ ge 3 ) m i q i qi T R ˆρ i Ω = x + + u i + x i m i l i ω i q i m i q i qi T R ˆΩ ρ i () + (J m i ˆρ i R T q i qi T R ˆρ i ) Ω m i ˆρ i R T q i qi T (ẍ ge 3 ) + ˆΩ J Ω = R ˆρ i R T (u i + x i m i l i ω i q i m i q i qi T R ˆΩ ρ i ) ω i = l i ˆq i (ẍ ge 3 R ˆρ i Ω + R ˆΩ ρ i ) () ˆq i (u i + x m i l i ) i (3) J i Ωi + Ω i J i Ω i = M i + Ri (4) where M q = m y I + n m iq i q T i R 3 3 which is symmetric positive-definite for any q i. The vector u i R 3 represents the control force at the i-th quadrotor i.e. u i = f i R i e 3. The vectors u i and u i R 3 denote the orthogonal projection of u i along q i and the orthogonal projection of u i to the plane normal to q i respectively i.e. u i = q iq T i u i (5) u i = ˆq i u i = (I q i q T i )u i. (6) Therefore u i = u i +u i. Throughout this paper the subscripts and of a vector denote the component of the vector that is parallel to q i and the other component of the vector that is perpendicular to q i. Similarly the disturbance force at the i-th quadrotor is decomposed as B. Tracking Problem x i = q i q T i xi (7) x i = ˆq i xi = (I q i q T i ) xi. (8) Define a fixed matrix P R 6 3n as I3 3 I P = 3 3. (9) ˆρ ˆρ n Recall that ρ i describe the point on the payload where the i-th link is attached. Assume the links are attached to the payload such that rankp 6. () This is to guarantee that there exist enough degrees of freedom in control inputs for both the translational motion and the rotational maneuver of the payload. The assumption () requires that the number of quadrotor is at least three i.e. n 3 since when n = the above matrix P has a non-empty null space spanned by (ρ ρ ) T (ρ ρ ) T T. This follows from the fact that it is impossible to generate any moment to the payload along the direction of ρ ρ when n =. It is also assumed that the bounds of the disturbance forces and moments are available i.e. for a known positive constant B δ R we have max{ x R T x T x n T Ri } < B δ. () Suppose that the desired trajectories for the position and the attitude of the payload are given as smooth functions of time namely x d (t) R 3 and R d (t) SO(3). From the attitude kinematics equation we have Ṙ d (t) = R d (t)ˆω d (t) where Ω d (t) R 3 corresponds to the desired angular velocity of the payload. It is assumed that the velocity and the acceleration of the desired trajectories are bounded by known constants. We wish to design a control input of each quadrotor {f i M i } i n such that the tracking errors asymptotically converge to zero along the solution of the controlled dynamics. III. CONTROL SYSTEM DESIGN FOR SIMPLIFIED DYNAMIC MODEL In this section we consider a simplified dynamic model where the attitude dynamics of each quadrotor is ignored and we design a control input by assuming that the thrust at each quadrotor namely u i can be arbitrarily chosen. It corresponds to the case where every quadrotor is replaced by a fully actuated aerial vehicle that can generates a thrust along any direction arbitrarily. The effects of the attitude dynamics of quadrotors will be incorporated in the next section.

4 In the simplified dynamic model given by ()-(3) the dynamics of the payload are affected by the parallel components u i of the thrusts and the dynamics of the links are directly affected by the normal components u i of the thrusts. This structure motivates the following control system design procedure: first the parallel components u i are chosen such that the payload follows the desired position and attitude trajectory while yielding the desired direction of each link namely q id S ; next the normal components u i are designed such that the actual direction of the links q i follows the desired direction q id. A. Design of Parallel Components Let a i R 3 be the acceleration of the point on the payload where the i-th link is attached that is measured relative to the gravitational acceleration: a i = ẍ ge 3 + R ˆΩ ρ i R ˆρ i Ω. () The parallel component of the control input is chosen as u i = µ i + m i l i ω i q i + m i q i q T i a i (3) where µ i R 3 is a virtual control input that is designed later with a constraint that µ i is parallel to q i. Note that the expression of u i is guaranteed to be parallel to q i due to the projection operator q i qi T at the last term of the right-hand side of the above expression. The motivation for the proposed parallel components becomes clear if (3) is substituted into ()-() and rearranged to obtain m (ẍ ge 3 ) = x + (µ i + x i ) (4) J Ω + ˆΩ J Ω = R + ˆρ i R T (µ i + x i ). (5) Therefore considering a free-body diagram of the payload the virtual control input µ i corresponds to the force exerted to the payload by the i-link or the tension of the i-th link in the absence of disturbances. Next we determine the virtual control input µ i. Any control scheme developed for the translational and rotational dynamics of a rigid body can be applied to (4) and (5). Here we consider an adaptive control scheme to handle the effects of the disturbances. As in 3 define position attitude and angular velocity tracking error vectors e x e R e Ω R 3 for the payload as e x = x x d e R = (RT d R R T R d ) e Ω = Ω R T R d Ω d. The desired resultant control force F d R 3 and moment M d R 3 acting on the payload are given in term of these error variables as F d = m ( k x e x kẋ ė x + ẍ d ge 3 ) x x i (6) M d = k R e R k Ω e Ω + (R T R d Ω d ) J R T R d Ω d + J R T R d Ωd R ˆρ i R xi (7) for positive constants k x kẋ k R k Ω R. Here the estimates of x xi R are denoted by x xi R R 3 and the orthogonal projection of xi along q i is denoted by x i = q i qi T xi R 3. Adaptive control laws to update the estimates of disturbances are introduced later at Section III-C. These are the ideal resultant force and moment to achieve the control objectives. One may try to choose the virtual control input µ i by making the expressions in the right-hand sides of (4) and (5) namely i µ i and i ˆρ ir T µ i become identical to F d and M d respectively. But this is not valid in general as each µ i is constrained to be parallel to q i. Instead we choose the desired value of µ i without any constraint such that µ id = F d ˆρ i R T µ id = M d (8) or equivalently using the matrix P defined at (9) R T µ d R T P. = F d. M d R T µ nd From the assumption stated at () there exists at least one solution to the above matrix equation for any F d M d. Here we find the minimum-norm solution given by µ d. µ nd = diagr R P T (PP T ) R T F d M d. (9) The virtual control input µ i is selected as the projection of its desired value µ id along q i µ i = (µ id q i )q i = q i q T i µ id (3) and the desired direction of each link namely q id S is defined as q id = µ i d µ id. (3) It is straightforward to verify that when q i = q id the resultant force and moment acting on the payload become identical to their desired values. Here the extra degrees of freedom in control inputs are used to minimize the magnitude of the desired tension at (9) but they can be applied to other tasks such as controlling the relative configuration of links 6. This is referred to future investigation. B. Design of Normal Components Substituting () into (3) and using (8) the equation of motion for the i-link is given by ω i = l i ˆq i a i m i l i ˆq i (u i + x i ). (3)

5 Here the normal component of the control input u i is chosen such that q i q id as t. Control systems for the unitvectors on the two-sphere have been studied in 4 5. In this paper we adopt the control system developed in terms of the angular velocity in 5 and we augment it with an adaptive control term to handle the disturbance x i. For the given desired direction of each link its desired angular velocity is obtained from the kinematics equation as ω id = q id q id. Define the direction and the angular velocity tracking error vectors for the i-th link namely e qi e ωi R 3 as e qi = q id q i e ωi = ω i + ˆq i ω id. For positive constants k q k ω R the normal component of the control input is chosen as u i = m i l iˆq i { k q e qi k ω e ωi (q i ω id ) q i ˆq i ω d } m iˆq i a i x i (33) where x i R 3 corresponds to the component of the estimate xi that is projected onto the plane normal to q i i.e. xi = ˆq i xi. Note that the expression of u i is perpendicular to q i by definition. Substituting (33) into (3) and rearranging by the facts that the matrix ˆq i corresponds to the orthogonal projection to the plane normal to q i and ˆq i 3 = ˆq i we obtain ω i = k q e qi k ω e ωi (q i ω id ) q i ˆq i ω d m i l i ˆq i xi (34) where the estimation error is defined as x i = x i x i R 3. In short the control force for the simplified dynamic model is given by C. Design of Adaptive Law u i = u i + u i. (35) The above control inputs require estimates of the uncertainties. They are updated according to the following adaptive laws x = h x (ė x + c x e x ) m (36) R = h R (e Ω + c R e R ) (37) xi = h xi q i q T i { m (ė x + c x e x ) R ˆρ i (e Ω + c R e R )} + h x i m i l i ˆq i (e ωi + c q e qi ) (38) for positive constants c x c R c q R and adaptive gains h x h R h xi R. Note that the adaptive law for xi is composed of two parts: the first part that is parallel to q i determined by the tracking errors of the payload and the second part that is normal to q i defined by the dynamics of the link. The resulting stability properties are summarized as follows. Proposition. Consider the simplified dynamic model defined by ()-(3). For given tracking commands x d R d a control input is designed as (35)-(38). Then there exist the values of controller gains and controller parameters such that the following properties are satisfied. (i) The zero equilibrium of tracking errors (e x ė x e R e Ω e qi e ωi ) and the estimation errors ( x R xi ) is stable in the sense of Lyapunov. (ii) The tracking errors asymptotically coverage to zero. (iii) The estimation errors are uniformly bounded. Proof. See Appendix B At (3) the negative sign appeared to make the tension at each cable positive when q i = q id. Assuming that the tracking errors e x ė x e R e Ω and the variables ẍ d Ω d Ω d obtained from the desired trajectories are sufficiently small this guarantees that quadrotors remain above the payload. If desired the negative sign at (3) can be eliminated to place quadrotors below the payload resulting in a tracking control of an inverted rigid body multi-link pendulum that can be considered as a generalization of a flying spherical inverted spherical pendulum illustrated in 6. The proposed control system guarantees stability in the sense of Lyapunov and asymptotic convergence of tracking errors variables but as the convergence of estimation error is not guaranteed asymptotic stability is not achieved. However in the absence of the disturbances we can achieve stronger exponential stability by eliminating the adaptive parts of the proposed control system via setting x = R = xi = and h x h R h xi = 8. IV. CONTROL SYSTEM DESIGN FOR FULL DYNAMIC MODEL The control system designed at the previous section is based on a simplifying assumption that each quadrotor can generate a thrust along any arbitrary direction instantaneously. However the dynamics of quadrotor is underactuated since the direction of the total thrust is always parallel to its third body-fixed axis while the magnitude of the total thrust can be arbitrarily changed. This can be directly observed from the expression of the total thrust u i = f i R i e 3 where f i is the total thrust magnitude and R i e 3 corresponds to the direction of the third body-fixed axis. Whereas the rotational attitude dynamics is fully actuated by the control moment M i. Based on these observations the attitude of each quadrotor is controlled such that the third body-fixed axis becomes parallel to the direction of the ideal control force u i designed in the previous section within a finite time. More explicitly the desired attitude of each quadrotor is constructed as follows. The desired direction of the third body-fixed axis of the i-th quadrotor namely b 3i S is given by b 3i = u i u i. (39) This provides two-dimensional constraint on the threedimensional desired attitude of each quadrotor and there

6 remains one degree of freedom. To resolve it the desired direction of the first body-fixed axis b i (t) S is introduced as a smooth function of time. Due to the fact that the first bodyfixed axis is normal to the third body-fixed axis it is impossible to follow an arbitrary command b i (t) exactly. Instead its projection onto the plane normal to b 3i is followed and the desired direction of the second body-fixed axis is chosen to constitute an orthonormal frame 9. This corresponds to controlling the additional one dimensional yawing angle of each quadrotor. From these the desired attitude of the i-th quadrotor is given by R ic = (ˆb 3i ) b i (ˆb 3i ) b i ˆb3i b i b ˆb 3i b i 3 i which is guaranteed to be an element of SO(3). The desired angular velocity is obtained from the attitude kinematics equation Ω ic = (Ri T c Ṙ ic ) R 3. In the prior work described in 8 the attitude of each quadrotor is controlled such that the equilibrium R i = R ic becomes exponentially stable and the stability of the combined full dynamic model is achieved via singular perturbation theory 6. However we can not follow such approach in this paper as the presented adaptive control system guarantees asymptotical convergence of the tracking error variables due to the disturbances. Here we design the attitude controller of each quadrotor such that R i becomes equal to R ic within a finite time via finite-time stability theory Define the tracking error vectors e Ri e Ωi R 3 for the attitude and the angular velocity of the i-th quadrotor as e Ri = (RT i c R i R T i R ic ) e Ωi = Ω i R T i R ic Ω ic. The time-derivative of e Ri can be written as 9 ė Ri = (tr R T i R ic I R T i R ic )e Ωi E(R i R ic )e Ωi. (4) For < r < define S : R R 3 R 3 as S(r y) = y r sgn(y ) y r sgn(y ) y 3 r sgn(y 3 ) T where y = y y y 3 T R 3 and sgn( ) denotes the sign function. For positive constants k R l R the terminal sliding surface s i R 3 is designed as s i = e Ωi + k R e Ri + l R S(r e Ri ). (4) We can show that when confined to the surface of s i the tracking errors become zero in a finite time. To reach the sliding surface for positive constants k s l s the control moment is designed as M i = k s s i l s S(r s i ) + Ω i J i Ω i (k R J i + l s rj i diag j e Rij r )E(R i R ci )e Ωi J i (ˆΩ i R T i R ic Ω ic R T i R ic Ωic ). (4) The thrust magnitude is chosen as the length of u i projected on to R i e 3 f i = u i R i e 3 (43) which yields that the thrust of each quadrotor becomes equal to its desired value u i when R i = R ic. t = t = 3.3 t = 6.6 (a) 3D perspective (b) Top view (c) Side view t = 3.3 t = 6.6 t = t = Fig.. Snapshots of controlled maneuver (red:desired trajectory blue:actual trajectory). A short animation illustrating this maneuver is available at w6c. Stability of the corresponding controlled systems for the full dynamic model can be shown by using the fact that the full dynamic model becomes exactly same as the simplified dynamic model within a finite time. Proposition. Consider the full dynamic model defined by ()-(4). For given tracking commands x d R d and the desired direction of the first body-fixed axis b i control inputs for quadrotors are designed as (4) and (43). Then there exists controller parameters such that the tracking error variables (e x ė x e R e Ω e qi e ωi ) asymptotically converge to zero and the estimation errors are uniformly bounded. Proof. See Appendix C. This implies that the payload asymptotically follows any arbitrary desired trajectory both in translations and rotations in the presence of uncertainties. In contrast to the existing results in aerial transportation of a cable suspended load it does not rely on any simplifying assumption that ignores the coupling between payload cable and quadrotors. Also the presented global formulation on the nonlinear configuration manifold avoids singularities and complexities that are inherently associated with local coordinates. As such the presented control system is particularly useful for agile load transportation involving combined translational and rotational maneuvers of the payload in the presence of uncertainties.

7 V. NUMERICAL EXAMPLE We consider a numerical example where three quadrotors (n = 3) transport a rectangular box along a figure-eight curve which is a special case of Lissajous figure shaped like the symbol. More explicitly the mass of the payload is m =.5 kg and its length width and height are. m.8 m and. m respectively. Throughout this section all of the units are in kg m and s if not specified. Mass properties of three quadrotors are identical and they are given by m i =.755 J i = diag The length of cable is l i = m and they are attached to the following points of the payload. ρ =.5. T ρ =.5.4. T ρ 3 =.5.4. T. In other words the first link is attached to the center of the top front edge and the remaining two links are attached to the vertices of the top rear edge (see Figure ). The desired trajectory of the payload is chosen as x d (t) =. sin(.πt) 4. cos(.πt).5 T. The desired attitude of the payload is chosen such that its first axis is tangent to the desired path and the third axis is parallel to the direction of gravity it is given by R d (t) = ẋd ẋ d Initial conditions are chosen as ê 3ẋ d ê 3ẋ d e 3. x () = 4.8 T v () = 3 q i () = e 3 ω i () = 3 R i () = I 3 3 Ω i () = 3. The uncertainties are specified as x = 3.5 T R =.5..5 T xi =.5..3 T Ri =..3.7 T. The corresponding simulation results are presented at Figures and 3. Figure illustrates the desired trajectory that is shaped like a figure-eight curve around two obstacles represented by cones and the actual maneuver of the payload and quadrotors. Figure 3 shows tracking errors for the position and the attitude of the payload tracking errors for the link directions and the attitude of quadrotors as well as tension and control inputs. These illustrate excellent tracking performances of the proposed control system. VI. CONCLUSIONS This paper presents a geometric control system for an arbitrary number of quadrotors that transport a cable-suspended rigid body. It is guaranteed that the payload asymptotically follows a desired position trajectory and a desired attitude trajectory concurrently in the presence of uncertainties. The main contribution is developing a geometric nonlinear adaptive control system that incorporate the nontrivial coupling between the payload links and quadrotors in an intrinsic fashion t (sec) (a) Position of payload (x :blue x d :red) t (sec) (c) Link direction error Ψ qi = q i q id t (sec) (e) Tension at links t (sec) (b) Attitude tracking error of payload Ψ = R R d t (sec) (d) Attitude tracking error of quadrotors Ψ i = R i R id fi Mi t (sec) (f) Control input for quadrotors f i M i Fig. 3. Simulation results for tracking errors and control inputs. (for figures (c)-(f): i = :blue i = :green i = 3:red) Rigorous stability proof of a nonlinear control system for such comprehensive dynamic model of cooperative aerial load transportation has been unprecedented. Future works include motion planning and control of the formation of the quadrotors relative to the payload to avoid collisions with obstacles places near the desired trajectory of the payload. A. Lagrangian Mechanics APPENDIX a) Derivatives of Lagrangian: Here we develop the equations of motion for the Lagrangian given by (7) and (8). The derivatives of the Lagrangian are given by Dẋ L = m T ẋ + m i (R ˆΩ ρ i l i q i ) (44) D qi L = m i (li q i l i ẋ l i R ˆΩ ρ i ) (45) D Ω L = J Ω + m i ˆρ i R T (ẋ l i q i ) (46)

8 D Ωi L = J i Ω i (47) D x L = m T ge 3 (48) D qi L = m i l i ge 3 (49) where J = J n m i ˆρ i. The variation of a rotation matrix is represented by δr j = R j ˆη j for η j R 3. Using this the derivative of the Lagrangian with respect to R j can be written as D R L δr = m i R ˆη ˆΩ ρ i (ẋ i l i q i ) + m i ge 3 R ˆη ρ i = m i { ˆΩ ρ i R T (ẋ l i q i ) + g ˆρ i R T e 3 } η d R L η (5) where d R L (R 3 ) R 3 is referred to as left-trivialized derivatives. Substituting δr j = R j ˆη j into the attitude kinematic equations () and rearranging the variation of the angular velocity can be written as δω j = η j + Ω j η j. For the variation model of q i given at (9) we have δq i = ξ i q i and ξ i = ξ i q i + ξ i q i. b) Lagrange-d Alembert Principle: Let G = t f t L dt be the action integral. Using the above equations the infinitesimal variation of the action integral can be written as tf δg = Dẋ L δẋ + D x L δx t + D Ω L ( η + Ω η ) + d R L η + D qi L ( ξ i q i + ξ i q i ) + D qi L (ξ i q i ) + D Ωi L ( η i + Ω i η i ) dt. The total control thrust at the i-th quadrotor with respect to the inertial frame is denoted by u i = f i R i e 3 R 3 and the total control moment at the i-th quadrotor is defined as M i R 3. There exist the disturbances x R for the payload and the disturbances xi Ri for the i-th quadrotor. The corresponding virtual work can be written as tf δw = (u i + xi ) {δx + R ˆη ρ i l i ξ i q i } + t (M i + Ri ) η i + x δx + R η dt. According to Lagrange-d Alembert principle we have δg = δw for any variation of trajectories with fixed end points. By using the integration by parts and rearranging we obtain the following Euler-Lagrange equations: d dt D ẋ L D x L = x + (u i + xi ) d dt D Ω L + Ω D Ω L d R L = R + We have µ id = q id qi T d µ id from (3). Using this the error term ˆρ i R T (u i + xi ) can be written in terms of e qi as Y x = n (qi T m d µ id ){(qi T q id )q i q id } ˆq i d dt D q i L ˆq i D qi L = l iˆq i (u i + xi ) d dt D Ω i L + Ω i D Ωi L = M i + Ri. Substituting (44)-(5) into these and rearranging by the fact that q i = ˆq i ω i ω i q i and ˆq i q i = ˆq i ω i = ω i the equations of motion can be rewritten as m T ẍ + m i ( R ˆρ i Ω + l iˆq i ω i ) + +m i l i ω i q i = m T ge 3 + x + J Ω + m i R ˆΩ ρ i (u i + xi ) (5) m i ˆρ i R T (ẍ + l iˆq i ω i + l i ω i q i ) + ˆΩ J Ω = R + ˆρ i R T (u i + xi + m i ge 3 ) (5) m i l i ω i m iˆq i ẍ + m iˆq i R ˆρ i Ω m iˆq i R ˆΩ ρ i = ˆq i (u i + xi + m i ge 3 ) (53) J i Ωi + Ω i J i Ω i = M i + Ri (54) where m T = m + n m i R 3 and J = J n m i ˆρ i R3 3. Next we substitute (53) into (5) and (5) to eliminate the dependency of ω i in the expressions for ẍ and Ω. Using the fact that I + ˆq i = q i q T i for any q i S and ˆΩ ˆρ i Ω = ˆρ i ˆΩ ρ i for any Ω ρ i R 3 we obtain () and () after rearrangements and simplifications. It is straightforward to see that (53) is equivalent to (3). B. Proof of Proposition c) Error Dynamics: From () and (3) the dynamics of the position tracking error is given by m ë x = m (ge 3 ẍ d ) + x + q i qi T µ id + x i. From (8) and (6) this can be rearranged as ë x = ge 3 ẍ d + m F d + Y x + m ( x + = k x e x kẋ ė x + m ( x + x i ) x i ) + Y x (55) where x i = x i x i R 3 corresponds to the parallel component of the estimation error. The last term Y x R 3 represents the error caused by the difference between q i and q id and it is given by Y x = n (q i qi T I)µ id. m

9 = n (qi T m d µ id )ˆq i e qi. Using (9) an upper bound of Y x can be obtained as Y x n µ id e qi m γ( F d + M d ) e qi where γ =. From (6) and (7) this can be m λmpp T further bounded by Y x {β(k x e x + kẋ ė x ) + γ(k R e R + k Ω e Ω ) + B} e qi (56) where β = m γ and the constant B is determined by the given desired trajectories of the payload and (6) which defines the domain D of the error variables that the presented stability proof is considered. Throughout the remaining parts of the proof any bound that can be obtained from x d R d or (6) is denoted by B for simplicity. In short the position tracking error dynamics of the payload can be written as (55) where the error term is bounded by (56). Similarly we find the attitude tracking error dynamics for the payload as follows. Using () (7) and (3) the timederivative of J e Ω can be written as J ė Ω = (J e Ω + d) e Ω k R e R k Ω e Ω + R + ˆρ i R T x i + Y R (57) where d = (J trj I)R T R d Ω d R 3 3 that is bounded and R R 3 denotes the estimation error given by R = R ˆ R. The error term in the attitude dynamics of the payload namely Y R R 3 is given by Y R = ˆρ i R T (q i qi T I)µ id = ˆρ i R T (qi T d µ id )ˆq i e qi. Similar with (56) an upper bound of Y R can be obtained as Y R {δ i (k x e x + kẋ ė x ) + σ i (k R e R + k Ω e Ω ) + B} e qi (58) ˆρ i where δ i = m σ λmpp T i = δi m R. Next from (34) the time-derivative of the angular velocity error projected on to the plane normal to q i is given as ˆq i ė ωi = ω + (q ω d ) q + ˆq ω d = k q e qi k ω e ωi m i l i ˆq i xi. (59) In summary the error dynamics of the simplified dynamic model are given by (55) (57) and (59). d) Stability Proof: Define an attitude configuration error function Ψ R for the payload as Ψ R = tr I R T d R which is positive-definite about R = R d and Ψ R = e R e Ω 9 3. We also introduce a configuration error function Ψ qi for each link that is positive-definite about q i = q id as Ψ qi = q i q id. For positive constants e xmax ψ R ψ qi B δ R consider the following open domain containing the zero equilibrium of tracking error variables: D = {(e x ė x e R e Ω e qi e ωi x R xi ) (R 3 ) 4 (R 3 R 3 ) n (R 3 ) R 3n e x < e xmax Ψ R < ψ R < Ψ qi < ψ qi < x < B δ R < B δ xi < B δ }. (6) In this domain we have e R = Ψ R ( Ψ R ) ψr ( ψ R ) α < and e qi = Ψ qi ( Ψ qi ) ψqi ( ψ qi ) α i <. It is assumed that ψ qi is sufficiently small such that nα i β <. We can show that the configuration error functions are quadratic with respect to the error vectors in the sense that e R Ψ R e R ψ R e q i Ψ qi e qi ψ qi where the upper bounds are satisfied only in the domain D. Define V = ė x + k x e x + c x e x ė x + e Ω J Ω + k R Ψ R + c R e R J e Ω + e ω i + k q Ψ qi + c q e qi e ωi where c x c R c q are positive constants. This is composed of tracking error variables only and we define another function for the estimation errors of the adaptive laws as V a = h x x + h R R + h xi xi. The Lyapunov function for the complete simplified dynamic model is chosen as V = V + V a. Let z x = e x ė x T z R = e R e Ω T z qi = e qi e ωi T R. The first part of the Lyapunov function V satisfies zx T P x z x + zr T P R z R + zq T i P qi z qi V z T x P x z x + z T R P R z R + zq T i P qi z qi

10 where the matrices P x P R P qi P x P R P qi R are given by P x = kx c x c x P R = kr c R λ c R λ λ kq c q c q P qi = P x = P R = P qi = kx c x c x kr ψ R c R λ kq ψ qi c R λ λ c q c q where λ = λ m J and λ = λ M J. If the constants c x c R c q are sufficiently small all of the above matrices are positive-definite. As the second part of the Lyapunov function V a is already given as a quadratic form it is straightforward to see that the complete Lyapunov function V is positive-definite and decrescent. The time-derivative of the Lyapunov function along the error dynamics (55) (57) and (59) is given by V = (kẋ c x ) ė x c x k x e x c x kẋ e x ė x + (c x e x + ė x ) Y x k Ω e Ω + c R ė R J e Ω c R k R e R + c R e R ((J e Ω + d) e Ω k Ω e Ω ) + (e Ω + c R e R ) Y R + (k ω c q ) e ωi c q k q e qi c q k ω e qi e ωi + m (ė x + c x e x ) ( x + + (e Ω + c R e R ) ( R + x i ) ˆρ i R T x i ) ( n ) ˆq i (e ωi + c q e qi ) m i l xi i R R h R x x h x h xi ( x i xi + x i xi ) (6) where the last term has been obtained using the facts that x i xi = x i xi =. Substituting the adaptive laws given by (36) (37) and (38) into (6) the expressions at the last four lines of (6) that are dependent of estimation errors vanish. An upper bound of the remaining expressions of V can be obtained as follows. Since e R ė R e Ω and d B V (kẋ c x ) ė x c x k x e x c x kẋ e x ė x + (c x e x + ė x ) Y x (k Ω c R λ) e Ω c R k R e R + c R (k Ω + B) e R e Ω + (e Ω + c R e R ) Y R + (k ω c q ) e ωi c q k q e qi c q k ω e qi e ωi. (6) From (56) an upper bound of the fourth term of the right-hand side is given by (c x e x + ė x ) Y x α i β(c x k x e x + c x kẋ e x ė x + kẋ ė x ) + {c x B e x + (βk x e xmax + B) ė x } e qi + α i γ(c x e x + ė x )(k R e R + k Ω e Ω ). (63) Similarly using (58) (c R e R + e Ω ) Y R α i σ i (c R k R e R + c R k Ω e R e Ω + k Ω e Ω ) + {c R B e R + (α σ i k R + B) e Ω } e qi + α i δ i (c R e R + e Ω )(k x e x + kẋ ė x ). (64) Substituting these into (6) and rearranging V zi T W i z i (65) where z i = z x z R z qi T R 3 and the matrix W i R 3 3 is defined as λ m W xi W xr i W xq i W i = W xr i λ m W Ri W Rq i (66) W xq i W Rq i λ m W qi where the sub-matrices are given by W xi = c x k x ( nα i β) cxkẋ ( + nα i β) n cxkẋ ( + nα i β) kẋ ( nα i β) c x W Ri = c R k R ( nα i σ i ) c R (k Ω + B + nα i σ i ) n c R (k Ω + B + nα i σ i ) k Ω ( nα i σ i ) c R λ c W qi = q k q cqkω cqkω k ω c q γcx k W xri = α R + δ i c R k x γc x k Ω + δ i k x i γk R + δ i c R kẋ γk Ω + δ i kẋ W xqi = c x B βk x e xmax + B W xri = c R B α σ i k R + B If the constants c x c R c q that are independent of the control input are sufficiently small the matrices W xi W Ri W qi are positive-definite. Also if the error in the direction of the link is sufficiently small relative to the desired trajectory we can choose the controller gains such that the matrix W i is positive-definite which follows that the zero equilibrium of tracking errors is stable in the sense of Lyapunov and all of the tracking error variables z i and the estimation error variables are uniformly bounded i.e. e x ė x e R e Ω e qi e ωi x R xi L. These also imply that e x ė x e R e Ω e qi e ωi L from (65) and that ė x ë x ė R ė Ω ė qi ė ωi L. According to Barbalat s lemma all of the tracking error variables e x ė x e R e Ω e qi e ωi and their time-derivatives asymptotically converge to zero..

11 C. Proof of Proposition We first show that the attitude of the i-th quadrotor becomes exactly equal to its desired value within a finite time i.e. R i (t) = R ic (t) for any t T for some T >. This is achieved by finite-time stability theory 7. This proof is composed of two parts: (i) s i (t) = for any t > T s for some T s < ; (ii) when the state is confined to the surface defined by s i = we have e Ri (t) = e Ωi (t) = for any t > T R for some T R <. From now on we drop the subscript i for simplicity as the subsequent development is identical for all quadrotors. From 9 the error dynamics for e Ω is given by Jė Ω = Ω Ω + M + R + J(ˆΩR T R c Ω c R T R c Ωc ). Substituting (4) s Jė Ω = k s s l s S(r s) + R B δ s (k R J + l s rjdiag j e Rj r )E(R R c )e Ω. (67) Let a Lyapunov function be W = s Js. From (4) and (4) its time-derivative is given by Ẇ = s {Jė Ω + (k R J + l s rjdiag j e Rj r )E(R R c )e Ω }. Substituting (67) and (4) and using () it reduces to Ẇ = s { k s s l s S(r s) + R s s B δ} k s s l s n j= k s s l s s r+ s j r+ + B δ s B δ s where the last inequality is obtained from the fact that x α n x i α for any x = x... x n T and < α < 9 Lemma. Therefore Ẇ ɛ W ɛ W (r+)/ where ɛ = ks λ M J and ɛ = l s ( λ M J )(r+)/. This implies that s(t) = for any t T s where the settling time T s satisfies T s ɛ ( r) ln ɛ W() ( r)/ + ɛ ɛ according to 8 Remark. Next consider the second part of the proof when s =. Let a configuration error function for the attitude of a quadrotor be Ψ R = tr I R T c R. Consider a domain give by D R = {(R Ω) SO(3) R 3 Ψ R < ψ R < }. It has been shown that the following inequality is satisfied in the domain e R Ψ R e R. (68) ψ R Therefore it is positive-definite about e R =. The timederivative of Ψ R is given by Ψ R = e R e Ω. Therefore when s = we have Ψ R = k R e R l R Substituting (68) we obtain n j= e Rj r+ k R e R l R e R r+ Ψ R ɛ 3 Ψ R ɛ 4 Ψ (r+)/ R where ɛ 3 = k R l ψ R and ɛ 4 = R ( ψ R. This implies that ) (r+)/ e R (t) = e Ω (t) = for any t T R where the settling time T R satisfies T R ɛ 3 ( r) ln ɛ 3Ψ R () ( r)/ + ɛ 4 ɛ 4. In summary whenever t T max{t s T R } it is guaranteed that R i (t) = R ic (t) for the i-th quadrotor. Next we consider the reduced system which corresponds to the dynamics of the payload and the rotational dynamics of the links when R i = R ic. From (43) and (39) the control force of quadrotors when R i = R ic is given by f i R i e 3 = (u i R ci e 3 )R ci e 3 = (u i u i u i ) u i u i = u i. Therefore the reduced system is given by the controlled dynamics of the simplified model. If the controller gains k R l R k s l s are selected large such that T is sufficiently small the solution stays inside of the domain D given at (6) during t < T. After t T the controlled system corresponds to the controlled system of the simplified dynamic model and from Proposition the tracking errors asymptotically coverage to zero and the estimation error are uniformly bounded. REFERENCES L. Cicolani G. Kanning and R. Synnestvedt Simulation of the dynamics of helicopter slung load systems Journal of the American Helicopter Society vol. 4 no. 4 pp M. Bernard Generic slung load transportation system using small size helicopters in Proceedings of the International Conference on Robotics and Automation 9 pp I. Palunko P. Cruz and R. Fierro Agile load transportation IEEE Robotics and Automation Magazine vol. 9 no. 3 pp N. Michael J. Fink and V. Kumar Cooperative manipulation and transportation with aerial robots Autonomous Robots vol. 3 pp I. Maza K. Kondak M. Bernard and A. Ollero Multi-UAV cooperation and control for load transportation and deployment Journal of Intelligent and Robotic Systems vol. 57 pp T. Lee K. Sreenath and V. Kumar Geometric control of cooperating multiple quadrotor UAVs with a suspended load in Proceedings of the IEEE Conference on Decision and Control Florence Italy Dec. 3 pp F. Goodarzi D. Lee and T. Lee Geometric stabilization of a quadrotor UAV with a payload connected by flexible cable in Proceedings of the American Control Conference June 4 pp T. Lee Geometric control of multiple quadrotor UAVs transporting a cable-suspended rigid body in Proceedings of the IEEE Conference on Decision and Control 4 accepted. 9 T. Lee M. Leok and N. McClamroch Geometric tracking control of a quadrotor aerial vehicle on SE(3) in Proceedings of the IEEE Conference on Decision and Control Atlanta GA Dec. pp (Acceptance Rate: 6%).

12 M. Shuster Survey of attitude representations Journal of the Astronautical Sciences vol. 4 pp T. Lee Computational geometric mechanics and control of rigid bodies Ph.D. dissertation University of Michigan 8. T. Lee M. Leok and N. McClamroch Lagrangian mechanics and variational integrators on two-spheres International Journal for Numerical Methods in Engineering vol. 79 no. 9 pp F. Goodarzi D. Lee and T. Lee Geometric nonlinear PID control of a quadrotor UAV on SE(3) in Proceedings of the European Control Conference Zurich July 3 pp F. Bullo and A. Lewis Geometric control of mechanical systems ser. Texts in Applied Mathematics. New York: Springer-Verlag 5 vol. 49 modeling analysis and design for simple mechanical control systems. 5 T. Wu Spacecraft relative attitude formation tracking on SO(3) based on line-of-sight measurements Master s thesis The George Washington University. 6 H. Khalil Nonlinear Systems nd Edition Ed. Prentice Hall S. Bhat and D. Bernstein Finite-time stability of continuous autonomous systems SIAM Journal of Control and Optimization vol. 38 no. 3 pp S. Yu X. Yu B. Shirinzadeh and Z. Man Continuous finite-time control for robotic manipulators with terminal slideing mode Automatica vol. 4 pp S. Wu G. Radice Y. Gao and Z. Sun Quaternion-based finite time control for spacecraft attitude tracking Acta Astronautica vol. 69 pp P. Ioannou and J. Sung Robust Adaptive Control. Prentice Hall 995.