arxiv: v1 [cs.sy] 13 Nov 2017

Transcription

1 Continuum Deformation of a Multiple Quadcopter Payload Delivery Team without Inter-Agent Communication Hossein Rastgoftar 1 and Ella M. Atkins 2 University of Michigan, 1320 Beal Avenue, Ann Arbor, MI, USA arxiv: v1 [cs.sy] 13 Nov 2017 This paper proposes continuum deformation as a strategy for controlling the collective motion of a multiple quadcopter system (MQS) carrying a common payload. Continuum deformation allows expansion and contraction of inter-agent distances in a 2 D motion plane to follow desired motions of three team leaders. The remaining quadcopter followers establish the desired continuum deformation only by knowing leaders positions at desired sample time waypoints without the need for inter-agent communication over the intermediate intervals. Each quadcopter applies a linear-quadratic Gaussian (LQG) controller to track the desired trajectory given by the continuum deformation in the presence of disturbance and measurement noise. Results of simulated cooperative aerial payload transport in the presence of uncertainty illustrate application of continuum deformation for coordinated transport through a narrow channel. I. Introduction Cooperative control of multiple unmanned aerial vehicles (UAVs) has been an active area of research over the past two decades. Formation flight[1, 2], air traffic control [3], transportation engineering [4], aerobiological sampling over agricultural lands [5], cooperative manipulation [6] and general team-based surveillance are some applications of cooperative control. A UAV team can 1 Postdoctoral Research Fellow, Aerospace Engineering Department, University of Michigan Ann Arbor, Member AIAA. 2 Professor, Aerospace Engineering Department, University of Michigan Ann Arbor, Associate Fellow AIAA. 1

2 Fig. 1 Structure of the proposed method for aerial payload transport improve mission efficiency, reduce cost, and offer increased resilience to failures including individual UAV loss. Group cooperation also improves fault detection and ability for the team to recover from anomaly conditions [7 9]. Consensus [10, 11], containment control [12, 13] and partial differential equation (PDE) methods [14] offer schemes to coordinate large-scale multi-uav teams. Cooperative payload transport, grasping and manipulation using multiple UAVs are applications that require UAVs to manage total forces and moments applied to external object(s) along with their collective motions. Manipulation can be used to achieve perching or deploy/pickup payloads, or the cooperative team can carry slung loads [15, 16]. Swing-free trajectory tracking is demonstrated by a single quadcopter carrying a payload in [17]. Aerial manipulation using a single quadcopter is studied in [18]. Modeling and control of multiple UAVs deployed for cooperative manipulation are investigated in [4, 6, 18 20], while cooperative grasping of a payload using multiple UAVs is studied in [19, 21]. Adaptive control for a slung load transport mission using a single rotorcraft is studied in [22 24]. Stability of slung load transport carrying by a multiple single-rotor helicopters is investigated in [25], while an inverse kinematics formulation for aerial payload transport is presented in [26]. This paper proposes a novel control strategy (Fig. 1) for cooperative aerial payload transport in the presence of uncertainty. A central guidance and control module manages leader UAV motions 2

3 while follower UAV motions are efficiently regulated through continuum deformation. A linear quadratic Gaussian (LQG) controller [27] combining a linear quadratic controller and a Kalman estimator [28, 29] offers robust trajectory tracking by each quadcopter. The guidance subsystem applies the recently proposed continuum deformation algorithm to achieve robust collective motion of quadcopter team during payload transport [30, 31]. This protocol treats quadcopters as particles of a continuum and allows expansion and contraction of inter-agent distances. For continuum deformation in a plane, i.e., constant-altitude quadcopter motion, a leader-follower model designates three quadcopter leaders at the vertices of a triangle, called the leading triangle. The remaining quadcopters fly inside the leading triangle and are modeled as followers. Application of continuum deformation to payload transport offers the following advantages: 1. Scalability: The total number of quadcopters participating in a payload delivery mission can be chosen sufficiently high that each quadcopter in a multi-quadcopter system (MQS) can carry a modest payload weight. 2. Minimal Inter-Agent Communication: Section V will describe how a desired MQS formation can be achieved without regular inter-agent communication. A follower quadcopter does not require position information of other follow quadcopters to learn the desired continuum deformation. Only leaders positions at waypoint arrival times must communicated across the team, minimizing communication costs and bandwidth requirements. 3. Negotiating obstacle-laden environments: Because quadcopters are treated as particles of a continuum and inter-agent distances can be expanded or contracted via the continuum deformation strategy, the team can navigate constrained environments including passing through a narrow channel. This capability will be demonstrated in simulation below. 4. Collision Avoidance: Because the proposed continuum deformation protocol is guided by leaders at the vertices of a triangle and followers are all inside the leading triangle, collisions with obstacles can be avoided by choosing appropriate trajectories for leaders motions. Due to the nonsingularity of continuum deformation, inter-agent collision can be avoided throughout the mission. 3

4 This article is organized as follows. First, multi-agent system continuum deformation is summarized in Section II. Application to MQS continuum deformation for a cooperative payload delivery mission is described in Section III. The dynamics model of a single quadcopter as well as LQG controller design is described in Section IV, while Section V describes the guidance subsystem and formulates follower quadcopter desired trajectories learned without inter-agent communication. An aerial MQS payload transport case study in which the MQS negotiates a narrow channel is provided in Section VI followed by concluding remarks in Section VII. II. 2 D Continuum Deformation of Multi-Agent Systems Let a continuum deformation in a 3 D motion space be defined by t t 0, r d,i (t) = Q(t)r d,i (t 0 ) + D(t) (1) where Q R 3 3 is the Jacobian matrix, D R 3 1 is a rigid-body displacement vector, and r d,i (t) = x d,i (t)i + y d,i (t)j + z d,i (t)k. (2) is called the desired position of agent i given by a homogeneous deformation. Without loss of generality, a 2 D continuum deformation in the X Y plane is considered in this article, therefore, t t 0, D 3 (t) = 0, Q 1,3 (t) = Q 2,3 (t) = Q 3,1 (t) = Q 3,2 (t) = 0, Q 3,3 (t) = 1. For continuum deformation in a 2 D motion space, three leaders are placed at the vertices of a leading triangle and V L = {1, 2, 3} defines leader index numbers. Additionally, N 3 followers are located inside the leading triangle and V F = {4..., N} defines follower index numbers. Elements of the Jacobian matrix Q and rigid-body displacement vector D can be uniquely related to leaders positions by [30 32] J t = [I 2 L 0 I ] 1 P t (3) where " " is the Kronecker product symbol, I 2 R 2 2 is the identity matrix, and 1 2 R 2 1 is the one-entry vector, [ T J t = Q 1,1 (t) Q 1,2 (t) Q 2,1 (t) Q 2,2 (t) D 1 (t) D 2 (t)] R 6 1, (4) 4

5 [ T P t = x d,1 (t) x d,2 (t) x d,3 (t) y d,1 (t) y d,2 (t) y d,3 (t)] R 6 1, (5) and x d,1 (t 0 ) y d,1 (t 0 ) L 0 = x d,2 (t 0 ) y d,2 (t 0 ) R 3 2. (6) x d,3 (t 0 ) y d,3 (t 0 ) Note that the matrix [I 2 L 0 I ] is nonsingular if leader positions satisfy the following rank condition [30 32]: t t 0, ] [r d,2 r d,1 r d,3 r d,1 = 2. (7) The continuum deformation defined in Eq. (2) is called homogeneous deformation because the Jacobian matrix Q is only time-varying but is not spatially-varying. Invariant Parameters of a Homogeneous Deformation: Under a homogeneous deformation, the position of an agent i can be expressed as the linear combination of leaders positions, where α i,k is unique and obtained from r d,i (t) = 3 α i,k r k (t) (8) k=1 x d,1 (t 0 ) x d,2 (t 0 ) x d,3 (t 0 ) y d,1 (t 0 ) y d,2 (t 0 ) y d,3 (t 0 ) α i,1 α i,2 α i,3 x d,i (t 0 ) = y d,i (t 0 ). (9) 1 Parameters α i,1, α i,2, α i,3 remain unchanged at any time t t 0, if r d,i satisfies Eq. (2) ( i V ). Note that 4 α i,k = 1 (10) k=1 and α i,k > 0 when agent i is inside the leading triangle. III. Problem Statement Consider an MQS consisting of N vehicles moving collectively in a 3 D motion space. The MQS is deployed for payload aerial transport and it offers the ability of negotiating a narrow channel 5

6 or other spatially-constrained environment. This paper proposes the architecture shown in Fig. 1 for guidance and control of the cooperative MQS. The guidance system applies principles of continuum mechanics [33] to assign desired waypoints at certain sampling times given constraints of the mission. Note that desired waypoints provided by the guidance system are defined by a homogeneous deformation. The desired homogeneous deformation is formulated based on positions of three leader quadcopters at the vertices of the leading triangle as described above. The remaining quadcopters are followers acquiring the desired homogeneous deformation only by knowing leader waypoints at known sample times t 0, t w,1,..., t w,m, t f. Section V describes how the MQS desired formation given by a homogeneous deformation can be learned by followers without communication during intermediate time intervals (t 0, t w,1 ), (t w,1, t w,2 ),..., (t w,m 1, t w,m ), (t w,m, t f ). The control system of an individual quadcopter uses a 6 degree of freedom (DOF) motion model. For control of each quadcopter, the 12 th order quadcopter dynamics model is linearized around the desired state. Given the desired trajectory of quadcopter i and the tension force exerted by a connecting cable, the desired state of quadcopter i, X d,i, is specified as described in Section IV C 1. An LQG controller offers robust tracking of the desired state X d,i given disturbances and measurement noise. The subsequent case study shows how an MQS consisting of 6 quadcopters can cooperatively carrying a payload from an open area through a narrow channel. IV. Quadcopter Dynamics Model A. Kinematics 1. Quadcopter Centroid Velocity and Acceleration The centroid position of quadcopter i V is expressed with respect to an inertial frame attached to the ground (Earth): r i = x i I + y i J + z i K (11) Therefore, velocity and acceleration of quadcopter i V is given by ṙ i = ẋ i I + ẏ i J + ż i K (12) r i = ẍ i I + ÿ i J + z i K (13) 6

7 2. Quadcopter Centroid Angular Velocity and Acceleration Let φ i, θ i, ψ i be the "roll", "pitch", and "yaw" angles of quadcopter i V, then angular velocity of quadcopter i is assigned by using the "3-2-1" standard Quadcopter angular velocity is given by Ω i = p i i b,i + q i j b,i + r i k b,i = ψ i k 1,i + θ i j 2,i + φ i i b,i, (14) where i 1 j 1 k 1 i b j b k b cos ψ i sin ψ i 0 I = sin ψ i cos ψ i 0 J, K = 0 cos φ i sin φ i 0 sin φ i cos φ i i 2 j 2 k 2. i 2 j 2 k 2 cos θ i 0 sin θ i = sin θ i 0 cos θ i i 1 j 1 k 1, Quadcopter angular velocity given in Eq. (14) can be rewritten in the following component-wise form: p i φ i q i = W 321,i θ i r i ψ i (15) where 1 0 sin θ i W 321,i = 0 cos φ i cos θ i sin φ i. (16) 0 sin φ i cos θ i cos φ i The angular acceleration of quadcopter i is given by Ω i = ṗ i i b,i + q i j b,i + ṙ i k b,i (17) B. Quadcopter Equations of Motion By applying Newton s second law, the quadcopter equations of motion become F Aero,i + F Cord,i T i k b,i m i gk = m i r i M i = I Ω i + Ω i I i Ω i, (18) 7

8 where m i is the mass of quadcopter i, g = 9.81m/s 2 is the gravitational acceleration, F Cord,i is the tension force in the cord connecting quadcopter i to the payload, and T i is the total thrust generated by quadcopter i s rotors, I xx,i 0 0 A x,i ẋ I = 0 I yy,i 0, F Aero,i = A y,i ẏ, M i = 0 0 I zz,i A z,i ẋ τ φ,i τ θ,i τ ψ,i, F Cord,i = A x,i, A y,i, A z,i represent aerodynamics parameters. Expressing k b,i with respect to I, J, and K, P x,i P y,i P z,i, k b,i = sin φ i j 2,i + cos φ i k 2,i = sin φ i j 1,i + cos φ i (sin θ i i 1,i + cos θ i k 1,i ) = sin φ i ( sin ψ i I + cos ψ i J) + cos φ i sin θ i (cos ψ i I + sin ψ i J) + cos φ i sin θ i K, =I (cos φ i sin θ i cos ψ i + sin φ i sin ψ i ) + J (cos φ i sin θ i sin ψ i sin φ i cos ψ i ) + K cos θ i cos φ i equations of motion of quadcopter i can be rewritten in the following form: (19) Ẋ i = f i (X i ) + g i (X i )U i (20) where [ ] T X i = x i y i z i φ i θ i ψ i u i v i w i p i q i r i R 12 1, [ ] T U i = T i τ φ,i τ θ,i τ ψ,i R 4 1, 8

9 u i v i w i sin φ i sin θ i p i + q i cos θ i q i cos φ i r i sin φ i sin φ i cos φ i q i + r i cos θ f i = i cos θ i P x,i A x,i u i m i P y,i A y,i v i m i P z,i A z,i w i m i I yy,i I zz,i q i r i I xx,i I zz,i I xx,i r i p i I yy,i I xx,i I yy,i p i q i I zz,i + r cos φ i sin θ i cos θ i , g i U i = T i. (cos ψ i sin θ i cos φ i + sin ψ i sin φ i ) m i T i (sin ψ i sin θ i cos φ i cos ψ i sin φ i ) m i T i (cos θ i cos φ i ) m i τ φ,i I xx,i τ θ,i I yy,i τ ψ,i I zz,i C. Quadcopter LQG Control Law An LQG controller offers robust tracking of the desired state X d,i for each quadcopter i in the presence of uncertainty. Section IV C 1 formulates the desired state and input given a desired trajectory r d,i assigned by continuum deformation. An LQG controller is then designed in Section IV C 2 to track the desired position defined by the continuum deformation in the presence of uncertainty. 1. Assigning Desired State X d,i and Desired Input U d,i The x d,i, y d,i, z d,i, u d,i, v d,i, and w d,i of desired state, [ ] T X d,i = x d,i y d,i z d,i φ d,i θ d,i ψ d,i u d,i v d,i w d,i p d,i q d,i r d,i R 12 1, are obtained from the guidance system by applying the proposed continuum deformation protocol as described below in Section V. The following procedure is used ito assign X d,i and U d,i = [T d,i τ φ,d,i τ θ,d,i τ ψ,d,i ] T R 4 1 : 1. Assigning ψ d,i : Given the desired trajectory r d,i = x d,i (t)i + y d,i (t)j + z d,i (t)k, ṙ d,i = ẋ d,i (t)i + ẏ d,i (t)j + ż d,i (t)k, and r d,i = ẍ d,i (t)i + ÿ d,i (t)j + z d,i (t)k the desired yaw angle 9

10 ψ d,i (t) is obtained. 2. Assigning T d,i, φ d,i and θ d,i : Substituting ẋ i (t), ẏ i (t), ż i (t), ψ i (t) in Eq. (19) by ẋ d,i (t), ẏ d,i (t), ż d,i (t), ψ d,i (t), T d,i (desired tension in the connecting rope) and k b,d,i (t) (desired orientation of k b,i (t)), are obtained as follows: T d,i = F Aero,d,i + F Cord,i m i gk m i r d,i (21) k b,d,i = F Aero,d,i + F Cord,i m i gk m i r d,i T d,i. (22) From k b,d,i obtained per Eq. (22), k b,d,i = b x,i b y,i b z,i, φ d,i (t) and θ d,i (t) are obtained as follows: φ d,i = arcsin (b x,i cos ψ d,i b y,i sin ψ d,i ) ( ). (23) θ d,i = tan 1 bx,i sin ψ d,i + b y,i sin ψ d,i b z,i 3. Assigning p d,i, q d,i, and r d,i : Given φ d,i, θ d,i, and ψ d,i at discrete times t k and t k 1, φd,i, θ d,i, and ψ d,i are numerically assigned: φ d,i (t k ) = φ d,i(t k ) φ d,i (t k 1 ) t k t k 1 θ d,i (t k ) = θ d,i(t k ) θ d,i (t k 1 ) t k t k 1 ψ d,i (t k ) = ψ d,i(t k ) ψ d,i (t k 1 ) t k t k 1 φ d,i, θ d,i, and ψ d,i are then related to p d,i, q d,i, and r d,i by applying the relation (15), where φ i, θ i, and ψ i in W 321 are replaced by φ d,i, θ d,i, and ψ d,i, respectively. (24) 4. Assigning τ φ,d,i, τ θ,d,i and τ ψ,d,i : By knowing p d,i, q d,i, and r d,i at discrete times t k 1 and t k, ṗ d,i, q d,i, and ṙ d,i are numerically obtained from ṗ d,i (t k ) = p d,i(t k ) p d,i (t k 1 ) t k t k 1 q d,i (t k ) = q d,i(t k ) q d,i (t k 1 ) t k t k 1 ṙ d,i (t k ) = r d,i(t k ) r d,i (t k 1 ) t k t k (25)

11 Then, τ φ,d,i, τ θ,d,i, and τ ψ,d,i are assigned from Eq. (18). 2. LQG Controller To find control U i at t [t k 1, t k ], we use a linearized model of the quadcopter dynamics, δx i,k =A i,k δx i,k + B i,k δu i,k + d i,k δy i,k =C i,k δx i,k + n i,k (26) where C i,k R is the identity matrix, d i,k R 12 1 is a zero-mean distarbance, n i,k R 12 1 is a zero-mean noise, and Note that A i,k = X d,i (t k ) B i,k = U d,i (t k ) [ ] f i (X d,i (t k )) + g i (X d,i (t k ))U d,i (t k ) [ ]. (27) f i (X d,i (t k )) + g i (X d,i (t k ))U d,i (t k ) X i (t) = X d,i (t k 1 ) + δx i,k (t t k 1 ) U i (t) = U i (t k 1 ) + δu i,k (t t k 1 ) (28) are the actual control state and input of the quadcopter dynamics (20). Given X i (t k 1 ), δx k,i,0 = X i (t k 1 ) X d,i (t k 1 ) (29) is considered as the initial state of the quadcopter i at the time t k 1 LQG Controller: As shown in Fig. 2, LQG design integrates a linear quadratic (LQ) controller and a Kalman estimator to the control system of the the quadcopter i V F. The Kalman filter dynamics δ ˆXi = A i,k δ ˆX i + B i,k δu i,k + L i,k (δy i,k ) (30) where L i,k = P i,k C i,k T R i,k 1 (31) is called a Kalman gain and obtained from the Riccati equation [34], 0 =A i,k P i,k + P i,k A i,k T P i,k C i,k T R i,k 1 C i,k P i,k + Q i,k Q i,k =E(d i,k d i,k T ) (32) R i,k =E(n i,k n i,k T ) 11

12 Fig. 2 Structure of the LQG controller of the quadcopter i integrating a linear quadratic controller and a Kalman estimator Also, the control is chosen as U i,k = K i,k δ ˆX i,k (33) where K i,k = H i,k 1 B i,k T S i,k (34) is obtained by solving algebraic Riccati equation: 0 =S i,k A i,k + A i,k T S i,k S i,k B i,k H i,k 1 B i,k T S i,k + E i,k H i,k R 4 4 > 0. (35) E i,k R Note that H i,k = E(n i,k n i,k T ) and E i,k = E(d i,k d i,k T ) where an E( ) denotes the expected value. V. Desired Trajectories of Quadcopters in a Continuum Deformation Suppose every follower quadcopter i ( i V F ) knows its own position and leaders positions at known sampling time t w,k 1 and t w,k. Then, every follower quadcopter i can set up its own desired trajectory without communication with other followers by applying the following steps: 12

13 1. α i Calculation: Assign parameters α i,1 (t w,k 1 ), α i,2 (t w,k 1 ), and α i,3 (t w,k 1 ) by using the Eq. (9) (In Eq. (9) t 0 should be replaced by t w,k 1.) 2. Desired Trajectory Calculation: Set the desired trajectory of the follower i V F at t [t w,k 1, t w,k 1 ] given leaders initial and final positions: i V F, r d,i (t) = 3 [ ] t tw,k 1 α i,k (r k (t w,k ) r k (t w,k 1 )) + r k (t w,k 1 ) t w,k t w,k 1 k=1 (36). Note that the desired velocity of the follower i V F is constant: } 3 u d,i k=1 α (xk (t w,k ) x k (t w,k 1 )) i,k{ t w,k t w,k 1 } i V F, v d,i = 3 k=1 α (yk (t w,k ) y k (t w,k 1 )) i,k{. (37) t w,k t w,k 1 } v 3 d,i k=1 i,k{ α (zk (t w,k ) z k (t w,k 1 )) t w,k t w,k 1 VI. Case Study: Cooperative Payload Delivery through a Narrow Channel This section considers an MQS consisting of 20 quadcopters, three leaders and seventeen followers. The MQS negotiates a narrow channel while it carries a payload. Leader and follower quadcopters all have the same properties as listed in Table 1 [35]. A. Desired Continuum Deformation Leaders are located at (x d,1 (t 0 ), y d,1 (t 0 ), z d,1 (t 0 )) = ( 20, 20, 50), (x d,2 (t 0 ), y d,2 (t 0 ), z d,2 (t 0 )) = (20, 18, 50), and (x d,3 (t 0 ), y d,3 (t 0 ), z d,3 (t 0 )) = (0, 20, 50) at t 0 = 0s. Leaders choose (x d,1 (t f ), y d,1 (t f ), z d,1 (t f )) = ( 15, 0, 50), (x d,2 (t 0 ), y d,2 (t 0 ), z d,2 (t 0 )) = (15, 10, 50), and (x d,3 (t 0 ), y d,3 (t 0 ), z d,3 (t 0 )) = (0, 35, 50) as their desired positions at t f = 20s. The desired trajectories of the leaders are defined by 5 20 Leader 1, r 1,d (t) = 20 t (38) 13

14 0 0 Leader 2, r 2,d (t) = 15 t Leader 3, r 3,d (t) = 28 t (39) (40) Table 1 Quadcopter Simulation Parameters [35] Parameter Value g 9.81 m/s 2 m kg I xx kg m 2 I yy kg m 2 I zz kg m 2 A x A y A z 0.25 kg/s 0.25 kg/s 0.25 kg/s Fig. 3 (a) shows a top view of leaders paths in the plane Z = 50m. Given leaders positions at t [0, 20], elements of the Jacobian Q and rigid-body translation vector D of the desired continuum deformation are obtained from Eq. (3) as depicted in Fig. 11 versus time. As shown, Q 1,3 (t) = Q 3,1 (t) = Q 2,3 (t) = Q 3,2 (t) = 0 and Q 3,3 (t) = 1 ( t [0, 20]) imply continuum deformation of the MQS in a plane normal to the Z axis. Additionally, Q(0) = I 3 and D(0) = 0 R 3 1. Each quadcopter learns the desired continuum deformation without communicating during t [0, 20]. Each follower i V F only needs to know the leaders initial and final positions as well as its own initial positions. Given initial position of follower i as well as leaders initial positions (See Fig. 11 (b).), parameter α i,k (i V F = {4, 5, 6}, k V L = {1, 2, 3}) is assigned from Eq. (9). Parameters α i,k as well as quadcopter initial positions are listed in Table 2. Knowing parameters α i,1, α i,2, α i,3, as well as leaders initial and final desired positions, follower i can assign its own 14

15 (a) (b) Fig. 3 (a) Top view of leader quadcopter paths in the plane Z = 50; (b) Inter-agent communication among quadcopters with quadcopters depicted in their initial positions. (a) Elements of Q (b) Elements of D Fig. 4 Elements of Q and D versus time desired trajectory according to Eq. (37). B. Desired Quadcopter States Dynamics of the Payload: By applying Newton s second law the payload dynamics is given by m p ẍ p,i ÿ p,i z p,i = 1 [ N ] F Cord,i + F Aero,p m p gk m p i=1 (41) 15

16 Table 2 Quadcopter positions at t = t 0; parameters α i,k (i V F, k V L) i x i(t 0) y i(t 0) z i(t 0) α i,1 α i,2 α i, where N is the total number of cables (quadcopters), m p is the payload mass, a p = ẍ p,i I + ÿ p,i J + z p,i K is the payload acceleration, and F Cord,i is the force in cable i connecting the payload to quadcopter i. It is assumed that cable i acts as a spring, therefore, F Cord,i = k i (l i (t) l 0,i ) x i x p l i y i y p l i z i z p l i (42) 16

17 where l i = (x i x p ) 2 + (y i y p ) 2 + (z i z p ) 2 is the current length of cable i, k i is the stiffness of cable i, and l 0,i = (x i (0) x p (0)) 2 + (y i (0) y p (0)) 2 + (z i (0) z p (0)) 2 is the free length of cable i. Notice that the force in cable i (F Cord,i ) is in the direction of the line segment connecting the payload to quadcopter i. Aerodynamic force exerted on the payload is given by C p,x ẋ p F Aero,p = C p,y ẏ p + δf Aero,p (43) C p,z ż p where δf Aero,p is a zero-mean random vector with covariance matrix E(δF Aero,p δf T Aero,p) = Because leaders trajectories are linear with respect to time, follower quadcopter desired trajectories given by the homogeneous deformation are also linear with respect to time (see Eq. (8).). To specify the payload acceleration used in Eq. (IV C 2), x i, y i, and z i in Eqs. (41) and (42) are substituted by x i (t) = x d,i (t) + δx i (t) y i (t) = y d,i (t) + δy i (t) (44) z i (t) = z d,i (t) + δz i (t) where x d,i (t), y d,i (t), and z d,i (t) are obtained from Eq. (8) and δr i (t) = δx i (t)i + δy i (t)j + δz i (t)k is a zero-mean random vector with covariance E(δr i δr T i ) = Desired Tension Force F Cord,i : To assign matrices A i,k and B i,k (see Eq. (IV C 2)), one must determine tension force F Cord,i exerted on quadcopter i. Given the ith quadcopter position r i and payload position r p, n i = r i r p r i r p (45) 17

18 assigns the direction of cable i. By applying Newton s second law, we can write N N F i = T i n i = m p (gk + r p ) (46) i=1 i=1 Note that r p = ẍ p,i I + ÿ p,i J + z p,i K is obtained from Eq. (41), therefore, the right hand side of Eq. (46) as well as the unit vector n p = m p(gk + r p ) m p (gk + r p ) (47) are known. By writing Eq. (46) component wise, we obtain three equations with twenty unknowns f 1,..., f N. This is an over-determined problem that can be solved by modeling cables as parallel springs. We assume all cables have the same stiffness k i = k, thus, f 1 d 1 = = f N d N (48) where d i = d i n i is the axial displacement of the cable i. Payload is displaced in the direction of the vector n p. Define d p = d p n p as the payload displacement, d i = is substituted in Eq. (50). Thus, Eq. (50) can be rewritten as follows: d p n i.n p (49) f 1 n 1.n p = = f N n N.n p. (50) Eq. (46) can be converted to the following with a dot product operation: N f i n i.n p = m p (gk + r p ). (51) i=1 Eqs. (50) and (51) assigning f 1, f 2,..., f N can then be written as follows: n 1 n p n 2 n p n 2 n p n 3 n p f = n 19 n p n N n p f 19 0 n 1 n p n 2 n p n 3 n p... n 19 n p n N n p m p (gk + r p ) f 1 f N (52) Payload motion simulation parameters are given in Table 3. In Fig. 5 components of the tension force in cables 10 and 14 are shown versus time. 18

19 (a) (b) (c) Fig. 5 (a)x components of the tensions generated in cables 10 and 14; (b)y components of the tensions generated in cables 10 and 14; (c) z components of the tensions generated in cables 10 and 14 (a) (b) (c) Fig. 6 (a) Desired thrusts T d,10 and T d,10 versus time; (b) Desired roll angles of quadcopters 10 and 14; (c) Desired pitch angles of quadcopters 10 and 14 Desired Thrust T d,i : Desired thrusts of the quadcopters are computed by using Eq. (21). Fig. 6(a) depicts desired thrusts T d,10 and T d,14 versus time. Desired Euler Angles : The desired path of quadcopter i V is a straight line, so ψ d,i (t) = 0 (t [0, 2]). Desired Euler angles of quadcopters 10 and 14 are calculated from Eq. (23) and shown versus time in Fig. 6(b) and (c). C. LQG Controller Because desired velocities u d,i, v d,i, w d,i are constant, Euler angles φ d,i, θ d,i, ψ d,i and their derivatives are small, and dynamics of quadcopter i can be approximated by linear time invariant (LTI) dynamics with A i,k = A i and A i,k = B i. Given the desired quadcopter states at t = 10s, matrices 19

20 Table 3 Payload Motion Simulation Parameters Parameter Value m p 10 k i 100kg/s 2 N 20 C p,x C p,y C p,z 4kg/s 4kg/s 4kg/s A 1,..., A 6 and B 1,..., B 6 are computed from Eq. (27). We consider t = t k t k 1 = 1, therefore, the initial state δx k,i,0 defined by Eq. (29) is updated at t k 1 = 1,..., 19 from k, i V, H i,k = E i,k = to simulate quadcopter collective motion. 20

21 (a) (b) (c) Fig. 7 (a) Components of quadcopter 10 actual position; (b) Components of quadcopter 10 position estimated by the Kalman filter; (c) Components of quadcopter 10 desired position as given by continuum deformation. D. Results Fig. 7 shows x 10, y 10, and z 10 (components of actual position of the quadcopter 10), ˆx 10, ŷ 10, and ẑ 10 (Kalman filter estimation), and x d,10, y d,10, and z d,10 versus time. Actual and estimated Euler angles of quadcopter 10 are shown versus time in Fig. 8. Fig. 9 shows payload position components (x p (t), y p (t), and z p (t)) versus time. Tensions in the connecting cables are also plotted versus time in Fig. 10. The MQS formation and the payload at different sample times are shown 21

22 (a) (b) Fig. 8 (a) Actual Euler angles of Quadcopter 10; (b) Euler angles of Quadcopter 10 estimated by the Kalman Filter. Fig. 9 Components of the payload position versus time. in Fig. 11. Fig. 7(b) shows that the Kalman filter of quadcopter 10 successfully estimates the actual position r 10 (t) = x 10 (t)î + y 10(t)Ĵ + z 10(t) ˆK of quadcopter 10. Fig. 7(c) shows that the controller is also able to accurately track the desired quadcopter 10 trajectory. This situation is similar for all other quadcopters. 22

23 Fig. 10 Tension forces in the connecting cables. The covariance matrix is defined as Q Actual,10 = 2000 i=1 [ X 10 (0.01(i 1)) X d,10 (0.01(i 1)) ][ X(0.01(i 1)) X d (0.01(i 1)) It is observed that eigenvalues of the symmetric matrix Q Actual are all positive or zero. Additionally, 2000 deviation of the quadcopter 10 from the desired position r 10,HT (t) is characterized by Q pos,10 = 2000 i=1 The simulation results show that x 10 (0.01(i 1)) x d,10 (0.01(i 1)) x 10 (0.01(i 1)) x d,10 (0.01(i 1)) y 10 (0.01(i 1)) y d,10 (0.01(i 1)) y 10 (0.01(i 1)) y d,10 (0.01(i 1)) z 10 (0.01(i 1)) z d,10 (0.01(i 1)) z 10 (0.01(i 1)) z d,10 (0.01(i 1)) 2000 ] T. (53) T (54) Q pos,10 = (55)

24 (a) t = 0s (b) t = 2.5s (c) t = 5s (d) t = 7.5s (e) t = 10s (f) t = 12.5s (g) t = 15s (h) t = 17.5s (i) t = 20s Fig. 11 Payload and MQS configurations at Different Sample Times. Expressing Q pos,10 in its spectural decomposition form, T Q pos,10 = , (56) it is concluded that the variance of the greatest deviation from the desired position is pointed to the direction [ ] T. Note that [ ] T is along an axis that is almost parallel to the unit vector K. Also, variances of the greatest deviation in the x y deformation plane are not large. 24

25 Furthermore, deviation of actual Euler angles (φ 10 (t), θ 10 (t), ψ 10 (t)) from the desired Euler angles (φ d,10 (t), θ d,10 (t), ψ d,10 (t)) is obtained from Q Euler,10 = (57) where Q Euler,10 = 2000 i=1 φ 10 (0.01(i 1)) φ d,10 (0.01(i 1)) φ 10 (0.01(i 1)) φ d,10 (0.01(i 1)) θ 10 (0.01(i 1)) θ d,10 (0.01(i 1)) θ 10 (0.01(i 1)) θ d,10 (0.01(i 1)) ψ 10 (0.01(i 1)) ψ d,10 (0.01(i 1)) ψ 10 (0.01(i 1)) ψ d,10 (0.01(i 1)) Notice that λ 1 (Q Euler,10 ) = , λ 2 (Q Euler,10 ) = , and λ 3 (Q Euler,10 ) = , thus, deviations of the actual Euler angles from their desired values are negligible. Considering the results of Eigen-analysis, it is concluded that the LQG controller can be successfully applied in a payload delivery mission when the payload is carried by large number of UAVs T. (58) VII. Conclusion The paper applies the recently-developed continuum deformation algorithm to an application in which multiple quadcopters carry a single parcel cooperatively. Path planning and motion control algorithms are proposed and validated in simulation. This paper described how a desired continuum deformation can be acquired by follower quadcopters only by knowing leaders positions at certain sample times without any further communication, minimizing communication costs. Because continuum deformation of an MQS is scalable, a large MQS team can cooperatively carry a heavy payload without the need for a heavy-lift quadcopter design. Because continuum deformation allows expansion and contraction of inter-agent distances, it also provides the ability for the team to pass through a narrow channel without collision as demonstrated in a twenty-quadcopter case study. The quadcopter LQG controller provides robust tracking of the desired trajectory assigned by continuum deformation despite disturbance inputs. 25

26 Acknowledgement This work was supported in part under Office of Naval Research (ONR) grant N References [1] Proud, A. W., Pachter, M., and DâĂŹAzzo, J. J., Close formation flight control, in Proceedings of the AIAA Guidance, Navigation and Control Conference,, 1999, pp [2] Fax, J. A. and Murray, R. M., Information flow and cooperative control of vehicle formations, IEEE transactions on automatic control, Vol. 49, No. 9, 2004, pp [3] Paterno, F., Santoro, C., and Tahmassebi, S., Formal models for cooperative tasks: concepts and an application for en-route air traffic control, in Design, Specification and Verification of Interactive SystemsâĂŹ 98, Springer, pp , [4] Michael, N., Fink, J., and Kumar, V., Cooperative manipulation and transportation with aerial robots, Autonomous Robots, Vol. 30, No. 1, 2011, pp [5] Schmale Iii, D. G., Dingus, B. R., and Reinholtz, C., Development and application of an autonomous unmanned aerial vehicle for precise aerobiological sampling above agricultural fields, Journal of Field Robotics, Vol. 25, No. 3, 2008, pp [6] Michael, N., Kim, S., Fink, J., and Kumar, V., Kinematics and statics of cooperative multi-robot aerial manipulation with cables, in ASME 2009 International Design Engineering Technical Conferences and Computers and Information in Engineering Conference, American Society of Mechanical Engineers, 2009, pp [7] Bahceci, E., Soysal, O., and Sahin, E., A review: Pattern formation and adaptation in multi-robot systems, Robotics Institute, Carnegie Mellon University, Pittsburgh, PA, Tech. Rep. CMU-RI-TR [8] Bazoula, A., Djouadi, M., and Maaref, H., Formation control of multi-robots via fuzzy logic technique, International Journal of Computers, Communications & Control, Vol. 3, No. 3. [9] Murray, R. M., Recent research in cooperative control of multivehicle systems, Journal of Dynamic Systems, Measurement, and Control, Vol. 129, No. 5, 2007, pp [10] Olfati-Saber, R., Fax, J. A., and Murray, R. M., Consensus and cooperation in networked multi-agent systems, Proceedings of the IEEE, Vol. 95, No. 1, 2007, pp [11] Ren, W., Distributed leaderless consensus algorithms for networked Euler Lagrange systems, International Journal of Control, Vol. 82, No. 11, 2009, pp [12] Cao, Y., Stuart, D., Ren, W., and Meng, Z., Distributed containment control for multiple autonomous 26

27 vehicles with double-integrator dynamics: algorithms and experiments, IEEE Transactions on Control Systems Technology, Vol. 19, No. 4, 2011, pp [13] Li, J., Ren, W., and Xu, S., Distributed containment control with multiple dynamic leaders for doubleintegrator dynamics using only position measurements, IEEE Transactions on Automatic Control, Vol. 57, No. 6, 2012, pp [14] Frihauf, P. and Krstic, M., Leader-enabled deployment onto planar curves: A PDE-based approach, IEEE Transactions on Automatic Control, Vol. 56, No. 8, 2011, pp [15] Bernard, M., Kondak, K., Maza, I., and Ollero, A., Autonomous transportation and deployment with aerial robots for search and rescue missions, Journal of Field Robotics, Vol. 28, No. 6, 2011, pp [16] Bernard, M., Kondak, K., and Hommel, G., A slung load transportation system based on small size helicopters, in Autonomous Systems Self-Organization, Management, and Control, Springer, pp , [17] Palunko, I., Fierro, R., and Cruz, P., Trajectory generation for swing-free maneuvers of a quadrotor with suspended payload: A dynamic programming approach, in Robotics and Automation (ICRA), 2012 IEEE International Conference on, IEEE, 2012, pp [18] Kim, S., Choi, S., and Kim, H. J., Aerial manipulation using a quadrotor with a two dof robotic arm, in 2013 IEEE/RSJ International Conference on Intelligent Robots and Systems, IEEE, 2013, pp [19] Mellinger, D., Lindsey, Q., Shomin, M., and Kumar, V., Design, modeling, estimation and control for aerial grasping and manipulation, in 2011 IEEE/RSJ International Conference on Intelligent Robots and Systems, IEEE, 2011, pp [20] Sreenath, K. and Kumar, V., Dynamics, control and planning for cooperative manipulation of payloads suspended by cables from multiple quadrotor robots, rn, Vol. 1, No. r2, 2013, p. r3. [21] Mellinger, D., Shomin, M., Michael, N., and Kumar, V., Cooperative grasping and transport using multiple quadrotors, in Distributed autonomous robotic systems, Springer, pp , [22] Bisgaard, M., la Cour-Harbo, A., and Bendtsen, J. D., Adaptive control system for autonomous helicopter slung load operations, Control Engineering Practice, Vol. 18, No. 7, 2010, pp [23] Potter, J., Singhose, W., and Costelloy, M., Reducing swing of model helicopter sling load using input shaping, in th IEEE International Conference on Control and Automation (ICCA), IEEE, 2011, pp [24] Dai, S., Lee, T., and Bernstein, D. S., Adaptive control of a quadrotor UAV transporting a cable- 27

28 suspended load with unknown mass, in 53rd IEEE Conference on Decision and Control, IEEE, 2014, pp [25] Pounds, P. E., Bersak, D. R., and Dollar, A. M., Grasping from the air: Hovering capture and load stability, in Robotics and Automation (ICRA), 2011 IEEE International Conference on, IEEE, 2011, pp [26] Jiang, Q. and Kumar, V., The inverse kinematics of cooperative transport with multiple aerial robots, IEEE Transactions on Robotics, Vol. 29, No. 1, 2013, pp [27] Boyd, S. P. and Barratt, C. H., Linear controller design: limits of performance, Prentice Hall Englewood Cliffs, NJ, [28] Kalman, R. E., A new approach to linear filtering and prediction problems, Journal of basic Engineering, Vol. 82, No. 1, 1960, pp [29] Brown, R. G. and Hwang, P. Y., Introduction to random signals and applied Kalman filtering: with MATLAB exercises and solutions, Introduction to random signals and applied Kalman filtering: with MATLAB exercises and solutions, by Brown, Robert Grover.; Hwang, Patrick YC New York: Wiley, c1997., Vol. 1. [30] Rastgoftar, H., Continuum Deformation of Multi-Agent Systems, Birkhäuser, [31] Rastgoftar, H., Kwatny, H. G., and Atkins, E. M., Asymptotic Tracking and Robustness of MAS Transitions Under a New Communication Topology,. [32] Rastgoftar, H. and Atkins, E. M., Continuum Deformation of Multi-Agent Systems Under Directed Communication Topologies, Journal of Dynamic Systems, Measurement, and Control, Vol. 139, No. 1, 2017, p [33] Lai, W. M., Rubin, D. H., Rubin, D., and Krempl, E., Introduction to continuum mechanics, Butterworth-Heinemann, [34] Bittanti, S., Laub, A. J., and Willems, J. C., The Riccati Equation, Springer Science & Business Media, [35] Luukkonen, T., Modelling and control of quadcopter, Independent research project in applied mathematics, Espoo. 28