- Email: [email protected]
Cooperative Aerial Load Transport with Force Control
Proceedings, 2018 IFAC Workshop on Networked & Autonomous Air & Space Systems Proceedings, 2018 IFAC Workshop on Networked Proceedings, 2018Santa IFAC Fe, Workshop Networked & & Autonomous Autonomous Air & 13-15, Space 2018. Systems June NM USAonAvailable online at www.sciencedirect.com Air & Space Systems Air & 13-15, Space 2018. Systems Proceedings, 2018Santa IFAC Fe, Workshop June NM USAon Networked & Autonomous June 13-15, 2018. Santa June Santa Fe, Fe, NM NM USA USA Air & 13-15, Space 2018. Systems June 13-15, 2018. Santa Fe, NM USA
ScienceDirect
IFAC PapersOnLine 51-12 (2018) 38–43
Cooperative Cooperative Cooperative Cooperative Cooperative
Aerial Load Transport Aerial Load Transport Aerial Load Aerial Load Transport Transport Force Control Force Control Aerial Load Transport Force Control Force Control ∗ ∗ ´ Acosta ∗∗ Sandesh Thapa Bai J.A. ForceHeControl ´ Acosta ∗∗ Sandesh Thapa ∗∗ He Bai ∗∗ J.A. ∗∗
with with with with with
´ Sandesh A. Acosta ∗∗ ´ Acosta Sandesh Thapa Thapa ∗ He He Bai Bai ∗ J. J.A. ∗ ∗ ∗ ∗∗ School of Mechanical and Aerospace Engineering, Oklahoma State ´ Sandesh Thapa He BaiEngineering, J.A. Acosta ∗ Mechanical and Aerospace Oklahoma ∗ School of Stillwater, University, (email: [email protected], ∗ School of MechanicalOK, and USA. Aerospace Engineering, Oklahoma State State
School of Stillwater, MechanicalOK, and USA. Aerospace Engineering, Oklahoma State University, (email: [email protected], [email protected]). ∗ University, USA. (email: [email protected], School of Stillwater, MechanicalOK, and Aerospace Engineering, Oklahoma State University, Stillwater, OK, USA. (email: [email protected], [email protected]). ∗∗ Ingenier´ıa de USA. Sistemas y Autom´ atica, Universidad de [email protected]). ∗∗ Depto. de University, Stillwater, OK, (email: [email protected], [email protected]). Depto. de Ingenier´ ıa de Sistemas y Autom´ a tica, Universidad de ∗∗ Sevilla, de losıa Descubrimientos s.n., a Sevilla, Spain. ∗∗ Depto. de de tica, de Depto.Camino de Ingenier´ Ingenier´ ı[email protected]). de Sistemas Sistemas yy Autom´ Autom´ a41092 tica, Universidad Universidad de Sevilla, Camino de los Descubrimientos s.n., 41092 Sevilla, Spain. (email: [email protected]). ∗∗ Sevilla, Camino de los Descubrimientos s.n., 41092 Sevilla, Spain. Depto.Camino de Ingenier´ de Sistemas y Autom´ tica, Universidad de Sevilla, de losıa(email: Descubrimientos s.n., a41092 Sevilla, Spain. [email protected]). [email protected]). Sevilla, Camino de los (email: Descubrimientos s.n., 41092 Sevilla, Spain. (email: [email protected]). [email protected]). (AM) collaboratively transporting a Abstract: We consider a group of (email: aerial manipulators Abstract: We consider a group of aerial manipulators (AM) collaboratively transporting flexible payload. Each AM is a combination of an Unmanned Aerial Vehicle (UAV) with a two-aaa Abstract: We a of (AM) collaboratively transporting Abstract: We consider consider a isgroup group of aerial aerial manipulators manipulators (AM) collaboratively transporting flexible payload. Each AM a combination of an Unmanned Aerial Vehicle (UAV) with aa twodegree-of-freedom robotic manipulator (RM) attached to it. Contact forces between the agents flexible payload. payload. Each AM AMa is isgroup a combination combination of an an Unmanned Unmanned Aerial Vehicle (UAV) (UAV) with two-a Abstract: We consider of aerial manipulators (AM) collaboratively transporting flexible Each a of Aerial Vehicle with a twodegree-of-freedom robotic manipulator (RM) attached to it. Contact forces between the agents (AMs) and the payload are modeled as(RM) the of gradient ofto nonlinear potentials that describe the degree-of-freedom robotic manipulator attached it. Contact forces between the agents flexible payload. Each AM is a combination an Unmanned Aerial Vehicle (UAV) with a twodegree-of-freedom robotic manipulator (RM) attached to it. Contact forces between the agents (AMs) and the payload are modeled as the gradient of nonlinear potentials that describe the deformation of the payload. We develop an adaptive decentralized control law for transporting (AMs) and the payload are modeled as the gradient of nonlinear potentials that describe the degree-of-freedom robotic manipulator attacheddecentralized it. Contact forceslaw between the agents (AMs) and the payload are modeled as(RM) theadaptive gradient oftononlinear potentials that describe the deformation of the payload. We develop an control for transporting adeformation payload with an unknown mass without explicit communication between the agents. The of the payload. We develop an adaptive decentralized control law for transporting (AMs) andwith the payload are We modeled as an theadaptive gradient decentralized of nonlinear potentials describe the deformation of the payload. develop control lawthat for transporting a payload an unknown mass without explicit communication between the agents. The algorithm that allWe thedevelop agents converge to decentralized a desired velocity and the a with an unknown mass without explicit communication between the agents. The deformation of the an adaptive control law forcontact transporting a payload payload guarantees with an payload. unknown mass without explicit communication between the agents.forces The algorithm guarantees that all the agents converge to a desired velocity and the contact forces are regulated. of the estimates of the unknown mass from all the converge to algorithm guarantees that all the agents converge to aa desired velocity and the forces a payload withThe an sum unknown without explicit between thecontact agents. The algorithm guarantees that all mass the agents converge to communication desired velocity and agents the contact forces are regulated. The sum of the estimates of the unknown mass from all the agents converge to the true mass. Using the inverse kinematics of the AM, we implement the developed algorithm are regulated. The sum of the estimates of the unknown mass from all the agents converge to algorithm guarantees that all the agents of converge to a desired velocity and the contact forces are regulated. The sum ofinverse the estimates the unknown mass from allthe the agents converge to the true mass. Using the kinematics of the AM, we implement developed algorithm at the kinematic level for the AMs and demonstrate its effectiveness in simulations. the true mass. Using the inverse kinematics of the AM, we implement the developed algorithm are regulated. The sum of the estimates of the unknown mass from all the agents converge to thethe true mass. Using the inverse kinematics of the AM, we implementinthe developed algorithm at kinematic level for the AMs and demonstrate its effectiveness simulations. at the kinematic level for the AMs and demonstrate its effectiveness in simulations. the true mass. Using the inverse kinematics of the AM, we implement the developed algorithm at the kinematic level for the AMs and demonstrate its effectiveness in simulations. © 2018, IFAC (International Federation of Automatic Control) Hosting by Elsevier Ltd. All rights reserved. Keywords: Cooperative control, aerial loadeffectiveness transport, force control, multi-agent at the kinematic level for the AMs andmanipulators, demonstrate its in simulations. Keywords: Cooperative control, aerial manipulators, load transport, force control, multi-agent system. Keywords: Keywords: Cooperative Cooperative control, control, aerial aerial manipulators, manipulators, load load transport, transport, force force control, control, multi-agent multi-agent system. system. Keywords: Cooperative control, aerial manipulators, load transport, force control, multi-agent system. 1. INTRODUCTION mass. Specifically, we develop decentralized control and system. 1. INTRODUCTION INTRODUCTION mass. Specifically, we develop develop decentralized control and estimation algorithms that guarantees force control regulation, 1. mass. we decentralized and 1. INTRODUCTION mass. Specifically, Specifically, we develop decentralized control and estimation algorithms that guarantees force regulation, velocity convergence convergence offorce the control estimate of algorithms that guarantees regulation, INTRODUCTION mass. Specifically, weand develop decentralized and estimation algorithms that guarantees force regulation, Aerial robots are 1. popular research platforms because of estimation velocity convergence and convergence of the estimate of the mass of the payload without explicit communication velocity convergence and convergence of the estimate of Aerial robots are popular research platforms because of estimation algorithms that guarantees force regulation, velocity convergence and without convergence of the estimate of their superior mobility in three dimensional Euclidean Aerial robots are popular research platforms because of the mass of the payload explicit communication Aerialsuperior robots are popular platforms because of the between the Theand algorithms are implemented at the of the without explicit communication their mobility in research three dimensional dimensional Euclidean velocity convergence of the estimate of the mass massconvergence of AMs. the payload payload without explicit communication space. Equipped with manipulators, aerial manipulators their mobility in three Euclidean between the AMs. Thethe algorithms are implemented implemented at the Aerial robots are popular platforms because of between their superior superior mobility in research three dimensional Euclidean kinematic level using inverse kinematics of the AMs. the AMs. The algorithms are at the space. Equipped with manipulators, aerial manipulators the mass of the payload without explicit communication between the AMs. The algorithms are implemented at the can be utilized for mobile manipulation tasks such as inspace. Equipped with manipulators, aerial manipulators kinematic level using the AMs inverse kinematics of the the AMs. AMs. their mobility inmanipulation three dimensional Euclidean space. Equipped manipulators, aerial manipulators Simulation results of two carrying a flexible of level using the inverse kinematics of can besuperior utilized forwith mobile tasks such as inin- kinematic between AMs. The algorithms are implemented at the kinematicthe level using the inverse kinematics of theload AMs. spection of inaccessible structure, transportation in remote can be utilized for mobile manipulation tasks such as Simulation results of two AMs carrying a flexible load of space. Equipped with manipulators, aerial manipulators can be utilized for mobile manipulation tasks such as inan unknown mass validate the proposed approach. Simulation results of two AMs carrying a flexible load of spection of inaccessible structure, transportation in remote kinematic level using the inverse kinematics of the AMs. Simulation results of two AMs carrying a flexible load of areas and rescue operations. Several of these aerial robots spection of inaccessible structure, transportation in remote an unknown mass validate the proposed approach. can beand utilized for mobile manipulation tasksaerial such as in- an unknown mass validate the proposed approach. spection ofrescue inaccessible structure, transportation in remote areas operations. Several of these robots Simulation results of two AMs carrying a flexible load of an unknown mass validate the proposed approach. can be used to transport heavier payloads thus expanding rest of this paper is organized as follows. We formulate areas operations. Several of aerial robots spection ofrescue inaccessible structure, transportation in remote areasbeand and rescue operations. Several of these these aerial robots The can used to transport transport heavier payloads thus expanding The rest of this paper is organized as follows. We formulate an unknown mass validate the proposed approach. the capabilities of a single AM. the dynamics of the payload transportation in Section 2. can be used to heavier payloads thus expanding rest is as formulate areas rescue Several of these aerial robots The can capabilities beand used to transport payloads thus expanding The dynamics rest of of this thisofpaper paper is organized organized as follows. follows. We We formulate the ofoperations. a single singleheavier AM. the the payload transportation in Section Section 2. In Section 3, we develop a decentralized control law for the capabilities of a AM. the dynamics of the payload transportation in 2. can be used to transport thus aexpanding the capabilities of a aerial singleheavier AM. payloads The rest of this is organized as follows. We formulate theSection dynamics ofpaper the payload transportation in Section 2. Control of multiple robots transporting common In 3, we develop a decentralized control law for unknown payload transportation. We applycontrol the algorithms In Section 3, we develop aa decentralized law for Control of been multiple aerial robots transporting a common common the capabilities of a aerial single AM.by different the dynamics of the payload transportation in Section 2. In Section 3, we develop decentralized control law for object has widely studied researchers. For Control of multiple robots transporting a unknown payload transportation. We apply the the algorithms Controlhas of been multiple aerial robots transporting a common in Section 33, to we aerial manipulators in Section 4. Simulation transportation. We apply algorithms object widely studied by different researchers. For unknown In Sectionpayload develop a decentralized control law for unknown payload transportation. We apply the algorithms example, Michael et al. (2011) uses teams of aerial robots object has been widely studied by different researchers. For in Section 3 to aerial manipulators in Section 4. Simulation Control ofMichael multiple robots transporting a common object has been widely by different researchers. For in results arepayload in Section 5.We Conclusions and future 33presented to manipulators in Section 4. example, et aerial al.studied (2011) uses teams of aerial aerial robots unknown transportation. apply the algorithms in Section Section to aerial aerial manipulators Section 4. Simulation Simulation transporting a payload via cables and develop robot conexample, et (2011) uses teams of robots results arediscussed presented inSection Section6. 5. 5. in Conclusions and future future object hasMichael been widely by different researchers. For results example, Michael et al. al.studied (2011) uses teams of aerial robots work are in are presented in Section Conclusions and transporting a payload via cables and develop robot conin Section 3presented to aerialinmanipulators Section 4. Simulation results arediscussed inSection Section6. 5. in Conclusions and future figurations that ensure static equilibrium of aerial the payload transporting aa payload via cables and develop robot conwork are example, Michael et al. (2011) uses teams of robots transporting payload via cables and develop robot conwork figurations that ensure staticMellinger equilibrium of(2013) the payload payload resultsare arediscussed presentedin Section6. ininSection Section 6. 5. Conclusions and future at a desiredthat pose. Reference al.of develfigurations ensure static equilibrium the transporting a payload via cables and et develop robot con- work are discussed figurations that ensure static equilibrium of(2013) the payload 2. PAYLOAD TRANSPORTATION at a desired pose. Reference Mellinger et al. develwork are discussed in Section 6. ops gripping mechanisms and control laws for cooperative at a desired pose. Reference Mellinger et al. (2013) devel2. PAYLOAD TRANSPORTATION figurations ensure static equilibrium of(2013) the payload at a gripping desiredthat pose. Reference Mellinger et al. devel2. ops mechanisms and control laws for cooperative 2. PAYLOAD PAYLOAD TRANSPORTATION TRANSPORTATION grasping and manipulation of a group of quadrotors. Refops gripping mechanisms and control laws for cooperative at desired Reference et quadrotors. al. developsa gripping mechanisms and control laws for(2013) cooperative we review the main results in Bai and Wen grasping andpose. manipulation ofMellinger group of Ref- In this section 2. PAYLOAD TRANSPORTATION erence Maza et al. (2010) and develops a software grasping and manipulation of aaa group of RefIn this section section weNreview review the main results results in Bai Bai and Wen Wen ops gripping laws for architecture cooperative grasping and mechanisms manipulation of control group of quadrotors. quadrotors. Ref- In (2010). Consider agents holding a common flexible load this we the main in and erence Maza et al. (2010) develops a software architecture In this section weNreview the main results in Bai and Wen for planning, control, collision avoidance and architecture deployment erence Maza et al. (2010) develops aa software (2010). Consider agents holding a common flexible load grasping and manipulation of a group of quadrotors. Reference Maza et al. (2010) develops software architecture as shown in Fig. 1 (N = 2). We assume that the load is (2010). Consider N agents holding a common flexible load for planning, control, collision avoidance and deployment In this section weN review the main results in Bai and Wen (2010). Consider agents holding a common flexible load of cable suspended payloads using multiple helicopters. as for planning, control, collision avoidance and deployment shown in Fig. 1 (N = 2). We assume that the load is erence Maza et al. (2010) develops a software architecture for planning, control, collision avoidance and deployment initially undeformed, and agent i is attached to the load at as shown in Fig. 1 (N = 2). We assume that is of cable suspended payloads using multiple helicopters. (2010). Consider N agents holding a common flexible load as shown in Fig. 1 (N = 2). We assume that the load is Recently, Meissen et al. (2017) develops a passivity based of cable suspended payloads using multiple helicopters. initially undeformed, and agent i is attached to the load at 3 for planning, control, collision avoidance and deployment of cable suspended payloads using multiple helicopters. the pointundeformed, ain x1i ∈(Nand R3=be theWe position of that theto end-effector initially agent i is attached the load at Recently, Meissen et al. (2017) develops a passivity based i . Let as shown Fig. 2). assume the is initially undeformed, and agent i is attached to load at formation control for mutiple UAVs cooperatively carrying Recently, Meissen et al. develops aa passivity based theagent pointi a aini ..the Letinertial x R be the position 3 the end-effector 3frame i ∈ of cable suspended using multiple helicopters. Recently, Meissen et payloads al. (2017) (2017) develops passivity based the of and riattached ∈ Rof a end-effector fixed vector 3 be point x R the position of the formation control for mutiple UAVs cooperatively carrying i . Let i ∈ 3 be initially undeformed, and agent i is to the load at the point a Let x ∈ R be the position of the end-effector aRecently, suspended payload. Reference Lee et al. (2016) proposes formation control for mutiple UAVs cooperatively carrying i i agent ii in the inertial frame and rri ∈ R33 xbe aa fixed Meissen et mutiple al. (2017) develops passivity based of formation control for UAVs cooperatively carrying the body frame of Rthe load. Initially, = aivector (0) = 3frame of inertial fixed vector suspended payload. Reference Lee et al. al.a (2016) (2016) proposes i (0) theagent point Let xi ∈ be theand position the of agent i ain ini .the the inertial frame and rii ∈ ∈R Rofxbe be a end-effector fixed vector motion planning algorithm forLee cooperative aerial trans- in aaformation suspended payload. Reference et proposes in the body frame of the Initially, (0) = a (0) = 3 load. i i control for mutiple UAVs cooperatively carrying a suspended payload. Reference Lee et al. (2016) proposes x (0) + r , where x ∈ R is the position of the center 3 in the body body frame of the the load. Initially, xbe (0) = a aivector (0) of = motion planning planning algorithm for cooperative cooperative aerial trans- in c agent iin the cof i (0) 3 load. of i inertial frame and r ∈ R a fixed the frame Initially, x = (0) = portation using multiple aerial manipulators. However, i aa motion algorithm for aerial transi i x (0) + r , where x ∈ R is the position of the center of 3 c i c a suspended payload. Reference et al. (2016) proposes motion planning algorithm forLee cooperative aerial trans- mass ofbody load in inertial frame. Fig. 1 also shows 3 load. x + rrthe x ∈ R is the position of the center of portation using multiple aerial manipulators. However, c (0) i ,, where cofthe in the frame the Initially, x (0) = a (0) = x (0) + where x ∈ R is the position of the center of force control is not considered in these references. i i portation using multiple aerial manipulators. However, c i c of the load in the inertial 11 also shows a motion planning algorithm for aerial trans- mass portation using multiple aerial manipulators. However, the coordinate system defined toframe. derive Fig. the kinematics of 3 mass of the load in the inertial frame. Fig. also shows force control is not not considered in cooperative these references. x (0) + r , where x ∈ R is the position of the center of mass of the load in the inertial frame. Fig. 1 also shows c i c force control is considered in these references. the coordinate system defined to derive the kinematics of portation using multiple aerial manipulators. However, force control is we not considered in control these references. In this paper, develop force algorithms for a the system. is the world fixed inertial frame. Oc,i ,shows Oi,jof coordinate defined to derive the kinematics I system of the Σ load in the inertial 1 also the coordinate system defined toframe. derive Fig. the kinematics of, In this paper, we develop force control algorithms forthe a mass the system. Σ the world fixed inertial frame. O O I is c,i ,,frame i,j ,, force control istransporting notdevelop considered in control these references. group of AMs a flexible load. We draw on O , (i = 1, ..., N , j = 1, 2 ), and O is the body In this paper, we force algorithms for a the system. Σ is the world fixed inertial frame. O O j,Tsystem. L the I system the coordinate defined to derive kinematics of, In thisofpaper, we develop aforce control algorithms Σ..., inertial Oc,i Oi,j I isNthe c,i ,frame i,j group AMs transporting flexible load. Weitdraw draw onforthe thea attached O = ,, jj world = 1, 22fixed ), and O isframe. the j,T ,, (i L the past work in Bai and Wen (2010) andload. extend to address to1, the quadrotor, links and endbody effector of, group of AMs transporting aaforce flexible We on O (i = 1, ..., N = 1, ), and O the body frame j,Tsystem. L is In this paper, we develop control algorithms for a the Σ is the world fixed inertial frame. O , O group of AMs transporting flexible load. We draw on the O , (i = 1, ..., N , j = 1, 2 ), and O is the body frame I c,i i,j j,T L past work in intransportation Bai and and Wen Wen (2010) and extend extend it tounknown address attached to the quadrotor, links and the end effector of cooperative a payload with an robot respectively. past Bai and to address attached quadrotor, links the end effector of group of AMs transporting aof flexible Weit draw on the the Oj,Ttwo , (i link =to ..., N ,and j =the 1, 2payload ), andand O is the frame past work work intransportation Bai and Wen (2010) (2010) andload. extend it tounknown address attached to1,the the quadrotor, links and endbody effector of L the cooperative of a payload with an the two link robot and the payload respectively. cooperative transportation of a payload with an unknown the two link robot and the payload respectively. past work in Bai and Wen (2010) and extend it to address attached to the quadrotor, links and the end effector of cooperative transportation of a payload with an unknown the two link robot and the payload respectively. Copyright © 2018 IFAC 38 cooperative transportation of a payload with an unknown the two link robot and the payload respectively. 2405-8963 © 2018, IFAC (International Federation of Automatic Control) Hosting by Elsevier Ltd. All rights reserved. Copyright © 2018 IFAC 38 Copyright 2018 38 Peer review© responsibility of International Federation of Automatic Copyright © under 2018 IFAC IFAC 38 Control. 10.1016/j.ifacol.2018.07.085 Copyright © 2018 IFAC 38
2018 IFAC NAASS June 13-15, 2018. Santa Fe, NM USA
Sandesh Thapa et al. / IFAC PapersOnLine 51-12 (2018) 38–43
39
The control objective is to design Fi in a decentralized way such that all the agents and the payload converge to a constant velocity v d and the contact force fi is regulated to a setpoint fid , i = 1, · · · , N . Since the load converges to a constant velocity, the sum of fid ’s in x and y direction is zero respectively and the sum of fid ’s in the z direction balances the weight of the load, i.e., N fid = Mc ge3 . (9) i=1
We assume that for a given fid there exists a locally unique deformation zid , such that (10) fid = ∇Pi (zid ) and ∇2 Pi (zid ) > 0. Assumption (10) is satisfied by linear spring-force model as well as certain class of nonlinear models, such as fi = bi |zi |2 zi , with bi > 0. As shown in Bai and Wen (2010), the following decentralized control law (11) Fi = −Γi (x˙ i − v d ) + fid , T where Γi = Γi > 0, guarantees asymptotic stability of the equilibrium of (7), (8) and (11) given by Eo = {(x˙ i , x˙ c , fi )|x˙ i = v d , x˙ c = v d , and fi = fid }. (12)
Fig. 1. Two aerial manipulators transport a common flexible load. Note that body frame Oc,i coincides with O1,j (not shown in the figure) and ai is the initial position of agent i. As the agents move, the payload is deformed, the new position of the agent is xi , and the deformation is approximated by zi = xi − ai . We assume that orientation of the load is controlled separately. It follows that (1) ai (t) := xc (t) + Rc ri , where Rc = I3×3 . The kinematics of the load is given by, (2) a˙i = x˙c The deformation of the payload is approximated as: (3) zi = xi − ai , i = 1, ..., N.
3. PAYLOAD TRANSPORT WITH AN UNKNOWN MASS In Section 2, the mass of the payload is assumed available so that fid in (9) can be specified. However, in practical scenarios, an exact knowledge of the mass of the payload may not be available. To address such a scenario, we develop an adaptive controller to estimate Mc . c,i as an estimate of mass of payload for each We define M agent i = 1, ..., N and propose the following update law c,i for M T ˙ M ˙ i − v d ) e3 , (13) c,i = γi (x
When the agents move, the payload experiences tension or compression. Therefore, ai (t) = xi (t). The reaction force fi to the agent i is given by the gradient of a positivedefinite potential function Pi (zi ), (4) fi = ∇Pi (zi ). Note that when zi = 0, Pi (zi ) satisfies the following constraints: (5) Pi (zi ) = 0 ⇐⇒ zi = 0, ∇Pi (zi ) = 0 ⇐⇒ zi = 0. (6) The dynamics of the load is given by ¨c = Mc x
N i=1
fi − Mc ge3 .
c,i stops updating when each where γi > 0. Note that M agent reaches the predesigned velocity v d . We modify the design in (11) as:
Fi = −Γi (x˙ i − v d ) + fˆid , and estimate the z-component of fid as: c,i g. fˆd = M
(14)
(15) d ˆ The x and y components of fi are the steady state values of the squeeze forces that are pre-designed to satisfy the following constraints N N d d = 0 and = 0. (16) fˆi,x fˆi,y i,z
(7)
where Mc is the mass of the load, g is the gravitational T constant, and e3 is the unit vector [0 0 1] . In Bai and Wen (2010) only planar agents are considered. We extend the planar dynamics to three-dimensional in (7), where the Mc ge3 term is the gravity of the payload.
i=1
i=1
The dynamics (8) with the proposed control law (13), (14), takes the following form: mi x ¨i = −Γi (x˙ i − v d ) + fˆid − fi . (17) Proposition 1. Consider the decentralized control law in (14) together with (13) and (15). The equilibrium of (7), (13), and (17) given by c,i )|x˙ i = v d , x˙ c = v d , E = {(x˙ i , fi , v d , M
2.1 Force Control for Payload Transport In Bai and Wen (2010), only the translational part of the motion is considered since the orientation of the load can be controlled separately. The translational dynamics of the N agents are given by ¨i = Fi − fi , i = 1, ..., N, (8) mi x where mi is the mass of i-th agent, Fi is the applied force to the agent i and fi is the contact force to the agent i.
N i=1
39
c,i = Mc , M
and
fi = fid },
(18)
2018 IFAC NAASS 40 13-15, 2018. Santa Fe, NM USA June
Sandesh Thapa et al. / IFAC PapersOnLine 51-12 (2018) 38–43
is asymptotically stable.
Let W = V + V1 . The time derivative of W yields ˙ = V˙ + V˙ 1 , W
Proof. Consider the following energy-motivated positive definite Lyapunov function N T Pi (zi ) − Pi (zid ) − (fid ) (zi − zid ) V = i=1
+
1 2
N
ξiT mi ξi + ξcT Mc ξc .
i=1
N i=1
T
(fi − fid ) z˙i +
N
ξiT mi x ¨i + ξcT Mc x ¨c .
N i=1
T
+ ξcT
N i=1
=
N i=1
i=1
=
N i=1
+
(fi −
N i=1
=−
N
T
+ ξcT
N
i=1
i=1
(21)
(22)
ξiT (Fi − fi )
−
N i=1
ξiT Γi ξi +
i=1
N
N i=1
T fid ) ξc
−
N i=1
fi − ξcT Mc ge3 ,
ξiT f˜id , (23)
(24)
From (23) and (24),
ξiT Γi ξi +
i=1
N
˜ c,i ge3 . ξiT M
I b and ωi,c be the angular velocity of the quadrotor Let ωc,i in the inertial frame and the body-fixed frame respectively. I We can map the time derivative of Euler angles Φ˙ i to ωc,i by a transformation matrix Ti . The following equalities hold: I I b = Rc,i ωc,i , (31) ωc,i I ˙ (32) ωc,i = Ti Φi ,
(25)
i=1
Consider another positive definite Lyapunov function: N
V1 =
1 ˜ 2 Λi Mc,i , 2 i=1
(26)
where Λi = g/γi . The time derivative of V1 yields V˙ 1 =
N
(29)
Each aerial manipulator is a 2-DOF robotic arm mounted at the bottom of a quadrotor. Similar derivations for single aerial manipulator can be found in Lippiello and Ruggiero (2012); Kim et al. (2013). Position of the center of mass of the quadrotor in the inertial frame ΣI is given T by, pIc,i = [xc,i yc,i zc,i ] , orientation of the quadrotor is described by the triple of ZYX (yaw-pitch-roll) Euler T angles, Φi = [ψi θi φi ] and joint angles of the two-DOF T manipulator are given by, ηij = [ηi1 ηi2 ] (see Fig 1). Both of the joint angles are defined about positive xc,i axis. The origin of frame for the load and each link of the manipulator is placed at its center of mass making the axis coincident with the inertial axes. The vector containing all the generalized coordinates for agent i is given by T qi = (pIc,i )T ΦTi ηiT . (30)
ξiT Γi ξi
where f˜id = fˆid − fid , i.e., 0 0 ˜ c,i ge3 . = 0 =M 0 f˜id = ˜ Mc,i g (Mc,i − Mc,i )g N
(28)
In this section we present the kinematic model of an aerial manipulator and discuss the implementation of the control (14) at the kinematic level.
ξiT (−Γi ξi + fˆid − fi )
i=1
V˙ = −
i=1
˜ c,i Λi γi ξiT e3 . M
4.1 Kinematics
(fi −
ξiT (fˆid − fi ) + ξcT
N
4. PAYLOAD TRANSPORTATION WITH AERIAL MANIPULATOR
fi − ξcT Mc ge3 , T fid ) ξi
i=1
˜ c,i ge3 − ξiT M
To conclude the asymptotic stability of the system, we apply the LaSalle invariance principle (Khalil (1996)) to investigate the largest invariant set M . On M , ξi = 0 which implies from (21) that x˙ i = v d . Since x˙ i is constant, x ¨i = 0, which implies from (17) that fˆid = fi . Because ˙ x˙ i = v d , M c,i = 0, which means Mc,i is a constant. d ˆ It follows from (15) that fi is constant and equal to fi . Since fi is constant, zi is constant, which leads to x˙ c = N ¨c = 0. From (7), we get i=1 fi = Mc ge3 , x˙ i = v d and x N which, together with fi = fˆid , results in i=1 fˆid = Mc g. N Given (15), we conclude i=1 M c,i = Mc .
fi − ξcT Mc g3 ,
(fi − fid ) (ξi − ξc ) + N
N
i=1
N
which implies the stability of the equilibrium E.
(20)
i=1
(fi − fid ) (x˙ i − x˙ c ) +
ξiT Γi ξi +
i=1
We rewrite (22) from (7), (8), (9), (14) and (20) as:
V˙ =
N
Now from (13) and (28), if Λi γi = g, N ˙ =− ξiT Γi ξi ≤ 0, W
(19)
From (2) and (3), the kinematics of zi are given by z˙i = x˙ i − a˙ i = x˙ i − x˙ c . Define ξi = x˙ i − v d and ξc = x˙ c − v d . The time derivative of V yields V˙ =
=−
˜˙ c,i . ˜ c,i M Λi M
b I T = (Rc,i ) Ti Φ˙ i = Qi Φ˙ i , ωc,i
(33)
I where Rc,i ∈ SO(3) is the rotation matrix between the body frame Oc,i and the inertial frame ΣI .
(27)
i=1
40
2018 IFAC NAASS June 13-15, 2018. Santa Fe, NM USA
Sandesh Thapa et al. / IFAC PapersOnLine 51-12 (2018) 38–43
where Ji ∈ R12×qi is the geometric Jacobian of the system (Spong et al. (2006)) whose elements is computed as:
Let pbi,j be the position of center of mass of link j = 1, 2 of the i -th aerial manipulator in the body-fixed frame Oc,i . The position pIij of center of mass of link j is given by pIi,j
pIc,i
T
Ji = [Jt,b Jr,b Jt,i Jr,i ] .
I Rc,i pbi,j .
= + (34) The linear and angular velocity of each link of manipulator are related to the time derivatives of joint angles ηi,j by p˙ bi,j = Jv,ij η˙ i,j ,
(35)
b ωi,j
(36)
= Jω,ij η˙ i,j , 3×2
I I I p˙Ii,j = p˙Ic,i + ω c,i Rc,i pbi,j + Rc,i Jv,ij η˙ i,j ,
I ωi,j
=
I ωc,i
+
I Rc,i Jω,ij η˙ i,j ,
(51)
We can represent the position/orientation of the endeffector as the minimal representation (Spong et al. (2006)) and express the forward kinematics as: (52) xi = k(qi ), where k(·) is an m × 1 vector function based on the system configuration and xi is an m × 1 state vector representing the system in minimal configuration. The time derivative of xi and qi can be related by, (53) x˙ i = Ja,i q˙i , where Ja,i ∈ Rm×nqi is the analytical Jacobian of the system which can be derived via differentiation of k(·) (Spong et al. (2006)). In this paper, we use the Cartesian position of the end-effector as a minimal representation, which gives us Ja,i = Jt,i .
and Jω,ij ∈ R are the linear and where Jv,ij ∈ R angular Jacobian of the manipulator in the body-fixed frame Spong et al. (2006). We can compute the linear and angular velocity of each link in the inertial frame from the following equations, I I pbi,j + Rc,i p˙ bi,j , (37) p˙ Ii,j = p˙Ic,i + R˙ c,i 3×2
41
(38) (39)
where the operator converts a given vector ω = T [ω1 ω2 ω3 ] , into a skew symmetric matrix as: 0 −ω3 ω2 0 −ω1 . ω = ω3 (40) −ω2 ω1 0
4.2 Control Implementation
C C C xbi = Pc1 Rc1 P12 + Rc2 P2T , (41) C where Pc1 is the vector representing the initial distance between the body frame of UAV and the body frame of the C 1st link of the manipulator expressed in frame C. Rc1 , and C Rc2 are the rotation matrices between the frames of the manipulator and its base frame. P12 is the initial distance between the first and the second link, P2T is the initial distance between the second link and the end-effector both expressed in its body-frame.
where, Ki > 0 and xi is the position of the end-effector of i-th the aerial manipulator.
As a preliminary investigation, we implement the control in Section 3 at the kinematic level. This is equivalent to assuming that there exists a sufficiently-fast velocity tracking controller for each AM. To do so, we set righthand side of (17) to 0 and solve for x˙ i : x˙ i = Ki (fˆd − fi ) + v d , i = 1, ..., N, (54)
Let xbi be the position of the end-effector in the body-fixed frame Oc,i , which is given by
i
The end-effector velocity x˙ i is computed from (54) and transformed to the generalized velocities using (48). Because of the redundancy in the system, we use the partialinverse of the Jacobian matrix to calculate the joint velocities for 3 desired states. An example to calculate the joint velocities for 3 states, xc,i , yc,i and φi from the partialinverse of the Jacobian is shown below. Let (55) J(t,i)s = [Jt,i (:, 1), Jt,i (:, 2), Jt,i (:, 6)] be the partial Jacobian matrix for the xc,i , yc,i and φi states. Then the corresponding joint velocities are computed as −1 q˙(i)prtl = J(t,i)s x˙ i , T q˙i = q˙(i)prtl (1), q˙(i)prtl (2), 0, 0, 0, q˙(i)prtl (3), 0, 0 . (56)
The position of the end effector xIi in the inertial frame for the i -th aerial manipulator is given by, I xIi = pIc,i + Rc,i (42) xbi . The linear and angular velocities of the end effector can be computed as: I I (43) xbi + Rc,i x˙ bi , x˙ Ii = p˙Ic,i + R˙ c,i I I I x˙ Ii = p˙Ic,i + ω c,i Rc,i xbi + Rc,i Jv,ij η˙ i,j ,
ωiI
I ωc,i
I Rc,i Jω,ij η˙ i,j .
= + Equations (42), (44) and (45) can be simplified as: p˙Ic,i = [I3×3 03×3 03×2 ] q˙i =: Jt,b q˙i , I ωc,i
Ti 03×2 ] q˙i =: Jr,b q˙i ,
= [03×3 I xb )T RI J q˙i =: Jt,i q˙i , = I3×3 −(R c,i v,ij c,i i I ωiI = 03×3 Ti Rc,i Jw,ij q˙i =: Jr,i q˙i . x˙ Ii
(44)
5. SIMULATION RESULTS
(45)
In this section, we present simulation results for two AMs transporting a load. Each quadrotor has a mass of 5 kg. Each link of the robotic manipulator has a length of 20 cm and a mass of 0.5 kg. The mass of the payload is 0.5 kg with a radius of 15 cm. All the joints of the manipulator are assumed to be spherical. The first joint rotates about the x axis and the second joint rotates about the y axis as shown in Fig. 1. We select xc,i , yc,i and φi as state variables to be controlled for all the results presented in this section. We use a linear spring-force model to compute the contact force fi in this simulation, (57) fi = kzi , where k is the spring constant of the payload. Note that k is not used in the controller (54).
(46) (47) (48) (49)
T I T Define vi = (p˙ Ic,i )T (ωc,i ) (x˙ Ii )T (ωiI )T , which contains the linear and angular velocities of the UAV and the end-effector of the i -th aerial manipulator. The time derivative of generalized joints vector qi can be mapped to vi by the following equation, vi = Ji q˙i , (50) 41
2018 IFAC NAASS 42 13-15, 2018. Santa Fe, NM USA June
Sandesh Thapa et al. / IFAC PapersOnLine 51-12 (2018) 38–43
5.1 Unknown Payload with a Predesigned v d T
c,i = 0.1 kg, Ki = 0.1, k = 100 We set v d = [0.2 0 0] , M d ˆ c g T and f2d = 0 −2.0 0.5M ˆ cg T . N/m, f1 = 0 2.0 0.5M The initial conditions of the two AMs are set as: T q1 = [0.1 −0.35 0.7 0 0 −0.1 0 −π/2] , T
q2 = [0 0.35 0.68 0 0 0.1 0 π/2] .
Fig. 5. Contact forces for both AMs in all The forces in the x direction converge the y-direction, they converge to 2.0 N respectively. In the z direction, the sum is equal to the weight of the payload.
3 direction. to zero. In and -2.0 N, of the force
5.2 Unknown Payload with a Time Varying v d In this case study, we test the robustness of our controllers to time varying velocities. We keep the other parameters to be the same as in the previous case and change the desired velocity of the agents to T t ≤ 2 s, 2.0, 0, 0 , T v d (t) = 2 − 2 sin(t − 2), 2 sin(t − 2), 0 , 2 < t ≤ 4 s, T t > 4.0 s. 0, 2.0, 0 ,
Fig. 2. Linear velocities for both agents. The x component of the velocity converges to 0.2 m/s and the rest converge to zero.
Fig. 6 and 7 show the trajectories of the agents and the payload in 2D and 3D, respectively. We also observe from Fig. 8 and 9 that the velocities of the agents and the payload in both x and y directions closely track v d . The contact force is also regulated close to the setpoint fid as shown in Fig. 11.
Fig. 3. Linear velocity for the payload. The x component of the velocity converges to 0.2 m/s and the rest converge to zero.
Fig. 6. X and Y position trajectories for both agents (time varying v d ). Both agents travel in the x -direction until 2 seconds and make a smooth turn towards the ydirection after 2 seconds. 6. CONCLUSION AND FUTURE WORK
Fig. 4. Estimation of the unknown mass. The sum of the individual estimates converges to the actual mass of the load. We observe from Fig. 2 and 3 that the velocities of the agents and the payload converge to v d . The update law from (13) successfully recovers the actual mass of the payload as seen in Fig. 4. We observe from Fig. 5 that the squeeze force is regulated to the setpoint fid .
We have developed an adaptive decentralized cooperative control law that transports a payload with an unknown mass. The control law also regulates the squeeze forces and guarantees velocity convergence for all agents. We verify the performance of the controller using simulation results. Future work will involve implementation of the control laws at the dynamic level and experimental validation. 42
2018 IFAC NAASS June 13-15, 2018. Santa Fe, NM USA
Sandesh Thapa et al. / IFAC PapersOnLine 51-12 (2018) 38–43
43
z axis
T 1 = 0.00 s
0.6 T 2 = 0.75 s
0.4 0.2
T 3 = 1.50 s
0
T 4 = 2.25 s T 9 = 6.00 s T 8 = 5.25 s T 7 = 4.50 s T 6 = 3.75 s T 5 = 3.00 s
2
4
8 6
4
2
x axis 6
y axis
0
Fig. 10. Estimation of the unknown mass. The sum of individual estimate converges to the actual mass of the load, which is 0.5 kg.
Fig. 7. 3D position trajectory for both agents and the payload at different time interval. Note that all 3 agents stay at an equal distance and will never collide. They approach the y-axis by being aligned in a straight line as shown in Fig. 6. Notice that if the quadrotors are at the same height, they must keep a gap and the manipulator must be large enough to avoid their blades clashing.
Fig. 11. Contact forces of both agents in all The forces in the x direction converge the y-direction, they converge to 2.0 N respectively. In the z direction, the sum is equal to the weight of the payload.
telligent Robots and Systems (IROS), 2013 IEEE/RSJ International Conference on, 4990–4995. IEEE. Lee, H., Kim, H., and Kim, H.J. (2016). Planning and control for collision-free cooperative aerial transportation. IEEE Transactions on Automation Science and Engineering. Lippiello, V. and Ruggiero, F. (2012). Cartesian impedance control of a uav with a robotic arm. IFAC Proceedings Volumes, 45(22), 704–709. Maza, I., Kondak, K., Bernard, M., and Ollero, A. (2010). Multi-uav cooperation and control for load transportation and deployment. J. Intell. Robotics Syst., 57(1-4), 417–449. Meissen, C., Klausen, K., Arcak, M., Fossen, T.I., and Packard, A. (2017). Passivity-based formation control for uavs with a suspended load. IFAC-PapersOnLine, 50(1), 13150–13155. Mellinger, D., Shomin, M., Michael, N., and Kumar, V. (2013). Cooperative grasping and transport using multiple quadrotors. In Distributed autonomous robotic systems, 545–558. Springer. Michael, N., Fink, J., and Kumar, V. (2011). Cooperative manipulation and transportation with aerial robots. Autonomous Robots, 30(1), 73–86. Spong, M.W., Hutchinson, S., and Vidyasagar, M. (2006). Robot modeling and control. Wiley New York.
Fig. 8. Velocities of the 2 agents in all 3 directions. The x and y velocities are smooth during transition and the z velocity is zero. 3.5 3
velocity of load (m/s)
2.5 2 X Velocity vd,x Y Velocity vd,y Z Velocity vd,z
1.5 1 0.5 0 -0.5 -1 -1.5 0
1
2
3
4
5
3 direction. to zero. In and -2.0 N, of the force
6
time (sec)
Fig. 9. Desired and actual velocities of the load in all 3 directions. REFERENCES Bai, H. and Wen, J.T. (2010). Cooperative load transport: A formation-control perspective. IEEE Transactions on Robotics, 26(4), 742–750. Khalil, H.K. (1996). Noninear Systems. Prentice Hall. Kim, S., Choi, S., and Kim, H.J. (2013). Aerial manipulation using a quadrotor with a two dof robotic arm. In In43
ScienceDirect
IFAC PapersOnLine 51-12 (2018) 38–43
Cooperative Cooperative Cooperative Cooperative Cooperative
Aerial Load Transport Aerial Load Transport Aerial Load Aerial Load Transport Transport Force Control Force Control Aerial Load Transport Force Control Force Control ∗ ∗ ´ Acosta ∗∗ Sandesh Thapa Bai J.A. ForceHeControl ´ Acosta ∗∗ Sandesh Thapa ∗∗ He Bai ∗∗ J.A. ∗∗
with with with with with
´ Sandesh A. Acosta ∗∗ ´ Acosta Sandesh Thapa Thapa ∗ He He Bai Bai ∗ J. J.A. ∗ ∗ ∗ ∗∗ School of Mechanical and Aerospace Engineering, Oklahoma State ´ Sandesh Thapa He BaiEngineering, J.A. Acosta ∗ Mechanical and Aerospace Oklahoma ∗ School of Stillwater, University, (email: [email protected], ∗ School of MechanicalOK, and USA. Aerospace Engineering, Oklahoma State State
School of Stillwater, MechanicalOK, and USA. Aerospace Engineering, Oklahoma State University, (email: [email protected], [email protected]). ∗ University, USA. (email: [email protected], School of Stillwater, MechanicalOK, and Aerospace Engineering, Oklahoma State University, Stillwater, OK, USA. (email: [email protected], [email protected]). ∗∗ Ingenier´ıa de USA. Sistemas y Autom´ atica, Universidad de [email protected]). ∗∗ Depto. de University, Stillwater, OK, (email: [email protected], [email protected]). Depto. de Ingenier´ ıa de Sistemas y Autom´ a tica, Universidad de ∗∗ Sevilla, de losıa Descubrimientos s.n., a Sevilla, Spain. ∗∗ Depto. de de tica, de Depto.Camino de Ingenier´ Ingenier´ ı[email protected]). de Sistemas Sistemas yy Autom´ Autom´ a41092 tica, Universidad Universidad de Sevilla, Camino de los Descubrimientos s.n., 41092 Sevilla, Spain. (email: [email protected]). ∗∗ Sevilla, Camino de los Descubrimientos s.n., 41092 Sevilla, Spain. Depto.Camino de Ingenier´ de Sistemas y Autom´ tica, Universidad de Sevilla, de losıa(email: Descubrimientos s.n., a41092 Sevilla, Spain. [email protected]). [email protected]). Sevilla, Camino de los (email: Descubrimientos s.n., 41092 Sevilla, Spain. (email: [email protected]). [email protected]). (AM) collaboratively transporting a Abstract: We consider a group of (email: aerial manipulators Abstract: We consider a group of aerial manipulators (AM) collaboratively transporting flexible payload. Each AM is a combination of an Unmanned Aerial Vehicle (UAV) with a two-aaa Abstract: We a of (AM) collaboratively transporting Abstract: We consider consider a isgroup group of aerial aerial manipulators manipulators (AM) collaboratively transporting flexible payload. Each AM a combination of an Unmanned Aerial Vehicle (UAV) with aa twodegree-of-freedom robotic manipulator (RM) attached to it. Contact forces between the agents flexible payload. payload. Each AM AMa is isgroup a combination combination of an an Unmanned Unmanned Aerial Vehicle (UAV) (UAV) with two-a Abstract: We consider of aerial manipulators (AM) collaboratively transporting flexible Each a of Aerial Vehicle with a twodegree-of-freedom robotic manipulator (RM) attached to it. Contact forces between the agents (AMs) and the payload are modeled as(RM) the of gradient ofto nonlinear potentials that describe the degree-of-freedom robotic manipulator attached it. Contact forces between the agents flexible payload. Each AM is a combination an Unmanned Aerial Vehicle (UAV) with a twodegree-of-freedom robotic manipulator (RM) attached to it. Contact forces between the agents (AMs) and the payload are modeled as the gradient of nonlinear potentials that describe the deformation of the payload. We develop an adaptive decentralized control law for transporting (AMs) and the payload are modeled as the gradient of nonlinear potentials that describe the degree-of-freedom robotic manipulator attacheddecentralized it. Contact forceslaw between the agents (AMs) and the payload are modeled as(RM) theadaptive gradient oftononlinear potentials that describe the deformation of the payload. We develop an control for transporting adeformation payload with an unknown mass without explicit communication between the agents. The of the payload. We develop an adaptive decentralized control law for transporting (AMs) andwith the payload are We modeled as an theadaptive gradient decentralized of nonlinear potentials describe the deformation of the payload. develop control lawthat for transporting a payload an unknown mass without explicit communication between the agents. The algorithm that allWe thedevelop agents converge to decentralized a desired velocity and the a with an unknown mass without explicit communication between the agents. The deformation of the an adaptive control law forcontact transporting a payload payload guarantees with an payload. unknown mass without explicit communication between the agents.forces The algorithm guarantees that all the agents converge to a desired velocity and the contact forces are regulated. of the estimates of the unknown mass from all the converge to algorithm guarantees that all the agents converge to aa desired velocity and the forces a payload withThe an sum unknown without explicit between thecontact agents. The algorithm guarantees that all mass the agents converge to communication desired velocity and agents the contact forces are regulated. The sum of the estimates of the unknown mass from all the agents converge to the true mass. Using the inverse kinematics of the AM, we implement the developed algorithm are regulated. The sum of the estimates of the unknown mass from all the agents converge to algorithm guarantees that all the agents of converge to a desired velocity and the contact forces are regulated. The sum ofinverse the estimates the unknown mass from allthe the agents converge to the true mass. Using the kinematics of the AM, we implement developed algorithm at the kinematic level for the AMs and demonstrate its effectiveness in simulations. the true mass. Using the inverse kinematics of the AM, we implement the developed algorithm are regulated. The sum of the estimates of the unknown mass from all the agents converge to thethe true mass. Using the inverse kinematics of the AM, we implementinthe developed algorithm at kinematic level for the AMs and demonstrate its effectiveness simulations. at the kinematic level for the AMs and demonstrate its effectiveness in simulations. the true mass. Using the inverse kinematics of the AM, we implement the developed algorithm at the kinematic level for the AMs and demonstrate its effectiveness in simulations. © 2018, IFAC (International Federation of Automatic Control) Hosting by Elsevier Ltd. All rights reserved. Keywords: Cooperative control, aerial loadeffectiveness transport, force control, multi-agent at the kinematic level for the AMs andmanipulators, demonstrate its in simulations. Keywords: Cooperative control, aerial manipulators, load transport, force control, multi-agent system. Keywords: Keywords: Cooperative Cooperative control, control, aerial aerial manipulators, manipulators, load load transport, transport, force force control, control, multi-agent multi-agent system. system. Keywords: Cooperative control, aerial manipulators, load transport, force control, multi-agent system. 1. INTRODUCTION mass. Specifically, we develop decentralized control and system. 1. INTRODUCTION INTRODUCTION mass. Specifically, we develop develop decentralized control and estimation algorithms that guarantees force control regulation, 1. mass. we decentralized and 1. INTRODUCTION mass. Specifically, Specifically, we develop decentralized control and estimation algorithms that guarantees force regulation, velocity convergence convergence offorce the control estimate of algorithms that guarantees regulation, INTRODUCTION mass. Specifically, weand develop decentralized and estimation algorithms that guarantees force regulation, Aerial robots are 1. popular research platforms because of estimation velocity convergence and convergence of the estimate of the mass of the payload without explicit communication velocity convergence and convergence of the estimate of Aerial robots are popular research platforms because of estimation algorithms that guarantees force regulation, velocity convergence and without convergence of the estimate of their superior mobility in three dimensional Euclidean Aerial robots are popular research platforms because of the mass of the payload explicit communication Aerialsuperior robots are popular platforms because of the between the Theand algorithms are implemented at the of the without explicit communication their mobility in research three dimensional dimensional Euclidean velocity convergence of the estimate of the mass massconvergence of AMs. the payload payload without explicit communication space. Equipped with manipulators, aerial manipulators their mobility in three Euclidean between the AMs. Thethe algorithms are implemented implemented at the Aerial robots are popular platforms because of between their superior superior mobility in research three dimensional Euclidean kinematic level using inverse kinematics of the AMs. the AMs. The algorithms are at the space. Equipped with manipulators, aerial manipulators the mass of the payload without explicit communication between the AMs. The algorithms are implemented at the can be utilized for mobile manipulation tasks such as inspace. Equipped with manipulators, aerial manipulators kinematic level using the AMs inverse kinematics of the the AMs. AMs. their mobility inmanipulation three dimensional Euclidean space. Equipped manipulators, aerial manipulators Simulation results of two carrying a flexible of level using the inverse kinematics of can besuperior utilized forwith mobile tasks such as inin- kinematic between AMs. The algorithms are implemented at the kinematicthe level using the inverse kinematics of theload AMs. spection of inaccessible structure, transportation in remote can be utilized for mobile manipulation tasks such as Simulation results of two AMs carrying a flexible load of space. Equipped with manipulators, aerial manipulators can be utilized for mobile manipulation tasks such as inan unknown mass validate the proposed approach. Simulation results of two AMs carrying a flexible load of spection of inaccessible structure, transportation in remote kinematic level using the inverse kinematics of the AMs. Simulation results of two AMs carrying a flexible load of areas and rescue operations. Several of these aerial robots spection of inaccessible structure, transportation in remote an unknown mass validate the proposed approach. can beand utilized for mobile manipulation tasksaerial such as in- an unknown mass validate the proposed approach. spection ofrescue inaccessible structure, transportation in remote areas operations. Several of these robots Simulation results of two AMs carrying a flexible load of an unknown mass validate the proposed approach. can be used to transport heavier payloads thus expanding rest of this paper is organized as follows. We formulate areas operations. Several of aerial robots spection ofrescue inaccessible structure, transportation in remote areasbeand and rescue operations. Several of these these aerial robots The can used to transport transport heavier payloads thus expanding The rest of this paper is organized as follows. We formulate an unknown mass validate the proposed approach. the capabilities of a single AM. the dynamics of the payload transportation in Section 2. can be used to heavier payloads thus expanding rest is as formulate areas rescue Several of these aerial robots The can capabilities beand used to transport payloads thus expanding The dynamics rest of of this thisofpaper paper is organized organized as follows. follows. We We formulate the ofoperations. a single singleheavier AM. the the payload transportation in Section Section 2. In Section 3, we develop a decentralized control law for the capabilities of a AM. the dynamics of the payload transportation in 2. can be used to transport thus aexpanding the capabilities of a aerial singleheavier AM. payloads The rest of this is organized as follows. We formulate theSection dynamics ofpaper the payload transportation in Section 2. Control of multiple robots transporting common In 3, we develop a decentralized control law for unknown payload transportation. We applycontrol the algorithms In Section 3, we develop aa decentralized law for Control of been multiple aerial robots transporting a common common the capabilities of a aerial single AM.by different the dynamics of the payload transportation in Section 2. In Section 3, we develop decentralized control law for object has widely studied researchers. For Control of multiple robots transporting a unknown payload transportation. We apply the the algorithms Controlhas of been multiple aerial robots transporting a common in Section 33, to we aerial manipulators in Section 4. Simulation transportation. We apply algorithms object widely studied by different researchers. For unknown In Sectionpayload develop a decentralized control law for unknown payload transportation. We apply the algorithms example, Michael et al. (2011) uses teams of aerial robots object has been widely studied by different researchers. For in Section 3 to aerial manipulators in Section 4. Simulation Control ofMichael multiple robots transporting a common object has been widely by different researchers. For in results arepayload in Section 5.We Conclusions and future 33presented to manipulators in Section 4. example, et aerial al.studied (2011) uses teams of aerial aerial robots unknown transportation. apply the algorithms in Section Section to aerial aerial manipulators Section 4. Simulation Simulation transporting a payload via cables and develop robot conexample, et (2011) uses teams of robots results arediscussed presented inSection Section6. 5. 5. in Conclusions and future future object hasMichael been widely by different researchers. For results example, Michael et al. al.studied (2011) uses teams of aerial robots work are in are presented in Section Conclusions and transporting a payload via cables and develop robot conin Section 3presented to aerialinmanipulators Section 4. Simulation results arediscussed inSection Section6. 5. in Conclusions and future figurations that ensure static equilibrium of aerial the payload transporting aa payload via cables and develop robot conwork are example, Michael et al. (2011) uses teams of robots transporting payload via cables and develop robot conwork figurations that ensure staticMellinger equilibrium of(2013) the payload payload resultsare arediscussed presentedin Section6. ininSection Section 6. 5. Conclusions and future at a desiredthat pose. Reference al.of develfigurations ensure static equilibrium the transporting a payload via cables and et develop robot con- work are discussed figurations that ensure static equilibrium of(2013) the payload 2. PAYLOAD TRANSPORTATION at a desired pose. Reference Mellinger et al. develwork are discussed in Section 6. ops gripping mechanisms and control laws for cooperative at a desired pose. Reference Mellinger et al. (2013) devel2. PAYLOAD TRANSPORTATION figurations ensure static equilibrium of(2013) the payload at a gripping desiredthat pose. Reference Mellinger et al. devel2. ops mechanisms and control laws for cooperative 2. PAYLOAD PAYLOAD TRANSPORTATION TRANSPORTATION grasping and manipulation of a group of quadrotors. Refops gripping mechanisms and control laws for cooperative at desired Reference et quadrotors. al. developsa gripping mechanisms and control laws for(2013) cooperative we review the main results in Bai and Wen grasping andpose. manipulation ofMellinger group of Ref- In this section 2. PAYLOAD TRANSPORTATION erence Maza et al. (2010) and develops a software grasping and manipulation of aaa group of RefIn this section section weNreview review the main results results in Bai Bai and Wen Wen ops gripping laws for architecture cooperative grasping and mechanisms manipulation of control group of quadrotors. quadrotors. Ref- In (2010). Consider agents holding a common flexible load this we the main in and erence Maza et al. (2010) develops a software architecture In this section weNreview the main results in Bai and Wen for planning, control, collision avoidance and architecture deployment erence Maza et al. (2010) develops aa software (2010). Consider agents holding a common flexible load grasping and manipulation of a group of quadrotors. Reference Maza et al. (2010) develops software architecture as shown in Fig. 1 (N = 2). We assume that the load is (2010). Consider N agents holding a common flexible load for planning, control, collision avoidance and deployment In this section weN review the main results in Bai and Wen (2010). Consider agents holding a common flexible load of cable suspended payloads using multiple helicopters. as for planning, control, collision avoidance and deployment shown in Fig. 1 (N = 2). We assume that the load is erence Maza et al. (2010) develops a software architecture for planning, control, collision avoidance and deployment initially undeformed, and agent i is attached to the load at as shown in Fig. 1 (N = 2). We assume that is of cable suspended payloads using multiple helicopters. (2010). Consider N agents holding a common flexible load as shown in Fig. 1 (N = 2). We assume that the load is Recently, Meissen et al. (2017) develops a passivity based of cable suspended payloads using multiple helicopters. initially undeformed, and agent i is attached to the load at 3 for planning, control, collision avoidance and deployment of cable suspended payloads using multiple helicopters. the pointundeformed, ain x1i ∈(Nand R3=be theWe position of that theto end-effector initially agent i is attached the load at Recently, Meissen et al. (2017) develops a passivity based i . Let as shown Fig. 2). assume the is initially undeformed, and agent i is attached to load at formation control for mutiple UAVs cooperatively carrying Recently, Meissen et al. develops aa passivity based theagent pointi a aini ..the Letinertial x R be the position 3 the end-effector 3frame i ∈ of cable suspended using multiple helicopters. Recently, Meissen et payloads al. (2017) (2017) develops passivity based the of and riattached ∈ Rof a end-effector fixed vector 3 be point x R the position of the formation control for mutiple UAVs cooperatively carrying i . Let i ∈ 3 be initially undeformed, and agent i is to the load at the point a Let x ∈ R be the position of the end-effector aRecently, suspended payload. Reference Lee et al. (2016) proposes formation control for mutiple UAVs cooperatively carrying i i agent ii in the inertial frame and rri ∈ R33 xbe aa fixed Meissen et mutiple al. (2017) develops passivity based of formation control for UAVs cooperatively carrying the body frame of Rthe load. Initially, = aivector (0) = 3frame of inertial fixed vector suspended payload. Reference Lee et al. al.a (2016) (2016) proposes i (0) theagent point Let xi ∈ be theand position the of agent i ain ini .the the inertial frame and rii ∈ ∈R Rofxbe be a end-effector fixed vector motion planning algorithm forLee cooperative aerial trans- in aaformation suspended payload. Reference et proposes in the body frame of the Initially, (0) = a (0) = 3 load. i i control for mutiple UAVs cooperatively carrying a suspended payload. Reference Lee et al. (2016) proposes x (0) + r , where x ∈ R is the position of the center 3 in the body body frame of the the load. Initially, xbe (0) = a aivector (0) of = motion planning planning algorithm for cooperative cooperative aerial trans- in c agent iin the cof i (0) 3 load. of i inertial frame and r ∈ R a fixed the frame Initially, x = (0) = portation using multiple aerial manipulators. However, i aa motion algorithm for aerial transi i x (0) + r , where x ∈ R is the position of the center of 3 c i c a suspended payload. Reference et al. (2016) proposes motion planning algorithm forLee cooperative aerial trans- mass ofbody load in inertial frame. Fig. 1 also shows 3 load. x + rrthe x ∈ R is the position of the center of portation using multiple aerial manipulators. However, c (0) i ,, where cofthe in the frame the Initially, x (0) = a (0) = x (0) + where x ∈ R is the position of the center of force control is not considered in these references. i i portation using multiple aerial manipulators. However, c i c of the load in the inertial 11 also shows a motion planning algorithm for aerial trans- mass portation using multiple aerial manipulators. However, the coordinate system defined toframe. derive Fig. the kinematics of 3 mass of the load in the inertial frame. Fig. also shows force control is not not considered in cooperative these references. x (0) + r , where x ∈ R is the position of the center of mass of the load in the inertial frame. Fig. 1 also shows c i c force control is considered in these references. the coordinate system defined to derive the kinematics of portation using multiple aerial manipulators. However, force control is we not considered in control these references. In this paper, develop force algorithms for a the system. is the world fixed inertial frame. Oc,i ,shows Oi,jof coordinate defined to derive the kinematics I system of the Σ load in the inertial 1 also the coordinate system defined toframe. derive Fig. the kinematics of, In this paper, we develop force control algorithms forthe a mass the system. Σ the world fixed inertial frame. O O I is c,i ,,frame i,j ,, force control istransporting notdevelop considered in control these references. group of AMs a flexible load. We draw on O , (i = 1, ..., N , j = 1, 2 ), and O is the body In this paper, we force algorithms for a the system. Σ is the world fixed inertial frame. O O j,Tsystem. L the I system the coordinate defined to derive kinematics of, In thisofpaper, we develop aforce control algorithms Σ..., inertial Oc,i Oi,j I isNthe c,i ,frame i,j group AMs transporting flexible load. Weitdraw draw onforthe thea attached O = ,, jj world = 1, 22fixed ), and O isframe. the j,T ,, (i L the past work in Bai and Wen (2010) andload. extend to address to1, the quadrotor, links and endbody effector of, group of AMs transporting aaforce flexible We on O (i = 1, ..., N = 1, ), and O the body frame j,Tsystem. L is In this paper, we develop control algorithms for a the Σ is the world fixed inertial frame. O , O group of AMs transporting flexible load. We draw on the O , (i = 1, ..., N , j = 1, 2 ), and O is the body frame I c,i i,j j,T L past work in intransportation Bai and and Wen Wen (2010) and extend extend it tounknown address attached to the quadrotor, links and the end effector of cooperative a payload with an robot respectively. past Bai and to address attached quadrotor, links the end effector of group of AMs transporting aof flexible Weit draw on the the Oj,Ttwo , (i link =to ..., N ,and j =the 1, 2payload ), andand O is the frame past work work intransportation Bai and Wen (2010) (2010) andload. extend it tounknown address attached to1,the the quadrotor, links and endbody effector of L the cooperative of a payload with an the two link robot and the payload respectively. cooperative transportation of a payload with an unknown the two link robot and the payload respectively. past work in Bai and Wen (2010) and extend it to address attached to the quadrotor, links and the end effector of cooperative transportation of a payload with an unknown the two link robot and the payload respectively. Copyright © 2018 IFAC 38 cooperative transportation of a payload with an unknown the two link robot and the payload respectively. 2405-8963 © 2018, IFAC (International Federation of Automatic Control) Hosting by Elsevier Ltd. All rights reserved. Copyright © 2018 IFAC 38 Copyright 2018 38 Peer review© responsibility of International Federation of Automatic Copyright © under 2018 IFAC IFAC 38 Control. 10.1016/j.ifacol.2018.07.085 Copyright © 2018 IFAC 38
2018 IFAC NAASS June 13-15, 2018. Santa Fe, NM USA
Sandesh Thapa et al. / IFAC PapersOnLine 51-12 (2018) 38–43
39
The control objective is to design Fi in a decentralized way such that all the agents and the payload converge to a constant velocity v d and the contact force fi is regulated to a setpoint fid , i = 1, · · · , N . Since the load converges to a constant velocity, the sum of fid ’s in x and y direction is zero respectively and the sum of fid ’s in the z direction balances the weight of the load, i.e., N fid = Mc ge3 . (9) i=1
We assume that for a given fid there exists a locally unique deformation zid , such that (10) fid = ∇Pi (zid ) and ∇2 Pi (zid ) > 0. Assumption (10) is satisfied by linear spring-force model as well as certain class of nonlinear models, such as fi = bi |zi |2 zi , with bi > 0. As shown in Bai and Wen (2010), the following decentralized control law (11) Fi = −Γi (x˙ i − v d ) + fid , T where Γi = Γi > 0, guarantees asymptotic stability of the equilibrium of (7), (8) and (11) given by Eo = {(x˙ i , x˙ c , fi )|x˙ i = v d , x˙ c = v d , and fi = fid }. (12)
Fig. 1. Two aerial manipulators transport a common flexible load. Note that body frame Oc,i coincides with O1,j (not shown in the figure) and ai is the initial position of agent i. As the agents move, the payload is deformed, the new position of the agent is xi , and the deformation is approximated by zi = xi − ai . We assume that orientation of the load is controlled separately. It follows that (1) ai (t) := xc (t) + Rc ri , where Rc = I3×3 . The kinematics of the load is given by, (2) a˙i = x˙c The deformation of the payload is approximated as: (3) zi = xi − ai , i = 1, ..., N.
3. PAYLOAD TRANSPORT WITH AN UNKNOWN MASS In Section 2, the mass of the payload is assumed available so that fid in (9) can be specified. However, in practical scenarios, an exact knowledge of the mass of the payload may not be available. To address such a scenario, we develop an adaptive controller to estimate Mc . c,i as an estimate of mass of payload for each We define M agent i = 1, ..., N and propose the following update law c,i for M T ˙ M ˙ i − v d ) e3 , (13) c,i = γi (x
When the agents move, the payload experiences tension or compression. Therefore, ai (t) = xi (t). The reaction force fi to the agent i is given by the gradient of a positivedefinite potential function Pi (zi ), (4) fi = ∇Pi (zi ). Note that when zi = 0, Pi (zi ) satisfies the following constraints: (5) Pi (zi ) = 0 ⇐⇒ zi = 0, ∇Pi (zi ) = 0 ⇐⇒ zi = 0. (6) The dynamics of the load is given by ¨c = Mc x
N i=1
fi − Mc ge3 .
c,i stops updating when each where γi > 0. Note that M agent reaches the predesigned velocity v d . We modify the design in (11) as:
Fi = −Γi (x˙ i − v d ) + fˆid , and estimate the z-component of fid as: c,i g. fˆd = M
(14)
(15) d ˆ The x and y components of fi are the steady state values of the squeeze forces that are pre-designed to satisfy the following constraints N N d d = 0 and = 0. (16) fˆi,x fˆi,y i,z
(7)
where Mc is the mass of the load, g is the gravitational T constant, and e3 is the unit vector [0 0 1] . In Bai and Wen (2010) only planar agents are considered. We extend the planar dynamics to three-dimensional in (7), where the Mc ge3 term is the gravity of the payload.
i=1
i=1
The dynamics (8) with the proposed control law (13), (14), takes the following form: mi x ¨i = −Γi (x˙ i − v d ) + fˆid − fi . (17) Proposition 1. Consider the decentralized control law in (14) together with (13) and (15). The equilibrium of (7), (13), and (17) given by c,i )|x˙ i = v d , x˙ c = v d , E = {(x˙ i , fi , v d , M
2.1 Force Control for Payload Transport In Bai and Wen (2010), only the translational part of the motion is considered since the orientation of the load can be controlled separately. The translational dynamics of the N agents are given by ¨i = Fi − fi , i = 1, ..., N, (8) mi x where mi is the mass of i-th agent, Fi is the applied force to the agent i and fi is the contact force to the agent i.
N i=1
39
c,i = Mc , M
and
fi = fid },
(18)
2018 IFAC NAASS 40 13-15, 2018. Santa Fe, NM USA June
Sandesh Thapa et al. / IFAC PapersOnLine 51-12 (2018) 38–43
is asymptotically stable.
Let W = V + V1 . The time derivative of W yields ˙ = V˙ + V˙ 1 , W
Proof. Consider the following energy-motivated positive definite Lyapunov function N T Pi (zi ) − Pi (zid ) − (fid ) (zi − zid ) V = i=1
+
1 2
N
ξiT mi ξi + ξcT Mc ξc .
i=1
N i=1
T
(fi − fid ) z˙i +
N
ξiT mi x ¨i + ξcT Mc x ¨c .
N i=1
T
+ ξcT
N i=1
=
N i=1
i=1
=
N i=1
+
(fi −
N i=1
=−
N
T
+ ξcT
N
i=1
i=1
(21)
(22)
ξiT (Fi − fi )
−
N i=1
ξiT Γi ξi +
i=1
N
N i=1
T fid ) ξc
−
N i=1
fi − ξcT Mc ge3 ,
ξiT f˜id , (23)
(24)
From (23) and (24),
ξiT Γi ξi +
i=1
N
˜ c,i ge3 . ξiT M
I b and ωi,c be the angular velocity of the quadrotor Let ωc,i in the inertial frame and the body-fixed frame respectively. I We can map the time derivative of Euler angles Φ˙ i to ωc,i by a transformation matrix Ti . The following equalities hold: I I b = Rc,i ωc,i , (31) ωc,i I ˙ (32) ωc,i = Ti Φi ,
(25)
i=1
Consider another positive definite Lyapunov function: N
V1 =
1 ˜ 2 Λi Mc,i , 2 i=1
(26)
where Λi = g/γi . The time derivative of V1 yields V˙ 1 =
N
(29)
Each aerial manipulator is a 2-DOF robotic arm mounted at the bottom of a quadrotor. Similar derivations for single aerial manipulator can be found in Lippiello and Ruggiero (2012); Kim et al. (2013). Position of the center of mass of the quadrotor in the inertial frame ΣI is given T by, pIc,i = [xc,i yc,i zc,i ] , orientation of the quadrotor is described by the triple of ZYX (yaw-pitch-roll) Euler T angles, Φi = [ψi θi φi ] and joint angles of the two-DOF T manipulator are given by, ηij = [ηi1 ηi2 ] (see Fig 1). Both of the joint angles are defined about positive xc,i axis. The origin of frame for the load and each link of the manipulator is placed at its center of mass making the axis coincident with the inertial axes. The vector containing all the generalized coordinates for agent i is given by T qi = (pIc,i )T ΦTi ηiT . (30)
ξiT Γi ξi
where f˜id = fˆid − fid , i.e., 0 0 ˜ c,i ge3 . = 0 =M 0 f˜id = ˜ Mc,i g (Mc,i − Mc,i )g N
(28)
In this section we present the kinematic model of an aerial manipulator and discuss the implementation of the control (14) at the kinematic level.
ξiT (−Γi ξi + fˆid − fi )
i=1
V˙ = −
i=1
˜ c,i Λi γi ξiT e3 . M
4.1 Kinematics
(fi −
ξiT (fˆid − fi ) + ξcT
N
4. PAYLOAD TRANSPORTATION WITH AERIAL MANIPULATOR
fi − ξcT Mc ge3 , T fid ) ξi
i=1
˜ c,i ge3 − ξiT M
To conclude the asymptotic stability of the system, we apply the LaSalle invariance principle (Khalil (1996)) to investigate the largest invariant set M . On M , ξi = 0 which implies from (21) that x˙ i = v d . Since x˙ i is constant, x ¨i = 0, which implies from (17) that fˆid = fi . Because ˙ x˙ i = v d , M c,i = 0, which means Mc,i is a constant. d ˆ It follows from (15) that fi is constant and equal to fi . Since fi is constant, zi is constant, which leads to x˙ c = N ¨c = 0. From (7), we get i=1 fi = Mc ge3 , x˙ i = v d and x N which, together with fi = fˆid , results in i=1 fˆid = Mc g. N Given (15), we conclude i=1 M c,i = Mc .
fi − ξcT Mc g3 ,
(fi − fid ) (ξi − ξc ) + N
N
i=1
N
which implies the stability of the equilibrium E.
(20)
i=1
(fi − fid ) (x˙ i − x˙ c ) +
ξiT Γi ξi +
i=1
We rewrite (22) from (7), (8), (9), (14) and (20) as:
V˙ =
N
Now from (13) and (28), if Λi γi = g, N ˙ =− ξiT Γi ξi ≤ 0, W
(19)
From (2) and (3), the kinematics of zi are given by z˙i = x˙ i − a˙ i = x˙ i − x˙ c . Define ξi = x˙ i − v d and ξc = x˙ c − v d . The time derivative of V yields V˙ =
=−
˜˙ c,i . ˜ c,i M Λi M
b I T = (Rc,i ) Ti Φ˙ i = Qi Φ˙ i , ωc,i
(33)
I where Rc,i ∈ SO(3) is the rotation matrix between the body frame Oc,i and the inertial frame ΣI .
(27)
i=1
40
2018 IFAC NAASS June 13-15, 2018. Santa Fe, NM USA
Sandesh Thapa et al. / IFAC PapersOnLine 51-12 (2018) 38–43
where Ji ∈ R12×qi is the geometric Jacobian of the system (Spong et al. (2006)) whose elements is computed as:
Let pbi,j be the position of center of mass of link j = 1, 2 of the i -th aerial manipulator in the body-fixed frame Oc,i . The position pIij of center of mass of link j is given by pIi,j
pIc,i
T
Ji = [Jt,b Jr,b Jt,i Jr,i ] .
I Rc,i pbi,j .
= + (34) The linear and angular velocity of each link of manipulator are related to the time derivatives of joint angles ηi,j by p˙ bi,j = Jv,ij η˙ i,j ,
(35)
b ωi,j
(36)
= Jω,ij η˙ i,j , 3×2
I I I p˙Ii,j = p˙Ic,i + ω c,i Rc,i pbi,j + Rc,i Jv,ij η˙ i,j ,
I ωi,j
=
I ωc,i
+
I Rc,i Jω,ij η˙ i,j ,
(51)
We can represent the position/orientation of the endeffector as the minimal representation (Spong et al. (2006)) and express the forward kinematics as: (52) xi = k(qi ), where k(·) is an m × 1 vector function based on the system configuration and xi is an m × 1 state vector representing the system in minimal configuration. The time derivative of xi and qi can be related by, (53) x˙ i = Ja,i q˙i , where Ja,i ∈ Rm×nqi is the analytical Jacobian of the system which can be derived via differentiation of k(·) (Spong et al. (2006)). In this paper, we use the Cartesian position of the end-effector as a minimal representation, which gives us Ja,i = Jt,i .
and Jω,ij ∈ R are the linear and where Jv,ij ∈ R angular Jacobian of the manipulator in the body-fixed frame Spong et al. (2006). We can compute the linear and angular velocity of each link in the inertial frame from the following equations, I I pbi,j + Rc,i p˙ bi,j , (37) p˙ Ii,j = p˙Ic,i + R˙ c,i 3×2
41
(38) (39)
where the operator converts a given vector ω = T [ω1 ω2 ω3 ] , into a skew symmetric matrix as: 0 −ω3 ω2 0 −ω1 . ω = ω3 (40) −ω2 ω1 0
4.2 Control Implementation
C C C xbi = Pc1 Rc1 P12 + Rc2 P2T , (41) C where Pc1 is the vector representing the initial distance between the body frame of UAV and the body frame of the C 1st link of the manipulator expressed in frame C. Rc1 , and C Rc2 are the rotation matrices between the frames of the manipulator and its base frame. P12 is the initial distance between the first and the second link, P2T is the initial distance between the second link and the end-effector both expressed in its body-frame.
where, Ki > 0 and xi is the position of the end-effector of i-th the aerial manipulator.
As a preliminary investigation, we implement the control in Section 3 at the kinematic level. This is equivalent to assuming that there exists a sufficiently-fast velocity tracking controller for each AM. To do so, we set righthand side of (17) to 0 and solve for x˙ i : x˙ i = Ki (fˆd − fi ) + v d , i = 1, ..., N, (54)
Let xbi be the position of the end-effector in the body-fixed frame Oc,i , which is given by
i
The end-effector velocity x˙ i is computed from (54) and transformed to the generalized velocities using (48). Because of the redundancy in the system, we use the partialinverse of the Jacobian matrix to calculate the joint velocities for 3 desired states. An example to calculate the joint velocities for 3 states, xc,i , yc,i and φi from the partialinverse of the Jacobian is shown below. Let (55) J(t,i)s = [Jt,i (:, 1), Jt,i (:, 2), Jt,i (:, 6)] be the partial Jacobian matrix for the xc,i , yc,i and φi states. Then the corresponding joint velocities are computed as −1 q˙(i)prtl = J(t,i)s x˙ i , T q˙i = q˙(i)prtl (1), q˙(i)prtl (2), 0, 0, 0, q˙(i)prtl (3), 0, 0 . (56)
The position of the end effector xIi in the inertial frame for the i -th aerial manipulator is given by, I xIi = pIc,i + Rc,i (42) xbi . The linear and angular velocities of the end effector can be computed as: I I (43) xbi + Rc,i x˙ bi , x˙ Ii = p˙Ic,i + R˙ c,i I I I x˙ Ii = p˙Ic,i + ω c,i Rc,i xbi + Rc,i Jv,ij η˙ i,j ,
ωiI
I ωc,i
I Rc,i Jω,ij η˙ i,j .
= + Equations (42), (44) and (45) can be simplified as: p˙Ic,i = [I3×3 03×3 03×2 ] q˙i =: Jt,b q˙i , I ωc,i
Ti 03×2 ] q˙i =: Jr,b q˙i ,
= [03×3 I xb )T RI J q˙i =: Jt,i q˙i , = I3×3 −(R c,i v,ij c,i i I ωiI = 03×3 Ti Rc,i Jw,ij q˙i =: Jr,i q˙i . x˙ Ii
(44)
5. SIMULATION RESULTS
(45)
In this section, we present simulation results for two AMs transporting a load. Each quadrotor has a mass of 5 kg. Each link of the robotic manipulator has a length of 20 cm and a mass of 0.5 kg. The mass of the payload is 0.5 kg with a radius of 15 cm. All the joints of the manipulator are assumed to be spherical. The first joint rotates about the x axis and the second joint rotates about the y axis as shown in Fig. 1. We select xc,i , yc,i and φi as state variables to be controlled for all the results presented in this section. We use a linear spring-force model to compute the contact force fi in this simulation, (57) fi = kzi , where k is the spring constant of the payload. Note that k is not used in the controller (54).
(46) (47) (48) (49)
T I T Define vi = (p˙ Ic,i )T (ωc,i ) (x˙ Ii )T (ωiI )T , which contains the linear and angular velocities of the UAV and the end-effector of the i -th aerial manipulator. The time derivative of generalized joints vector qi can be mapped to vi by the following equation, vi = Ji q˙i , (50) 41
2018 IFAC NAASS 42 13-15, 2018. Santa Fe, NM USA June
Sandesh Thapa et al. / IFAC PapersOnLine 51-12 (2018) 38–43
5.1 Unknown Payload with a Predesigned v d T
c,i = 0.1 kg, Ki = 0.1, k = 100 We set v d = [0.2 0 0] , M d ˆ c g T and f2d = 0 −2.0 0.5M ˆ cg T . N/m, f1 = 0 2.0 0.5M The initial conditions of the two AMs are set as: T q1 = [0.1 −0.35 0.7 0 0 −0.1 0 −π/2] , T
q2 = [0 0.35 0.68 0 0 0.1 0 π/2] .
Fig. 5. Contact forces for both AMs in all The forces in the x direction converge the y-direction, they converge to 2.0 N respectively. In the z direction, the sum is equal to the weight of the payload.
3 direction. to zero. In and -2.0 N, of the force
5.2 Unknown Payload with a Time Varying v d In this case study, we test the robustness of our controllers to time varying velocities. We keep the other parameters to be the same as in the previous case and change the desired velocity of the agents to T t ≤ 2 s, 2.0, 0, 0 , T v d (t) = 2 − 2 sin(t − 2), 2 sin(t − 2), 0 , 2 < t ≤ 4 s, T t > 4.0 s. 0, 2.0, 0 ,
Fig. 2. Linear velocities for both agents. The x component of the velocity converges to 0.2 m/s and the rest converge to zero.
Fig. 6 and 7 show the trajectories of the agents and the payload in 2D and 3D, respectively. We also observe from Fig. 8 and 9 that the velocities of the agents and the payload in both x and y directions closely track v d . The contact force is also regulated close to the setpoint fid as shown in Fig. 11.
Fig. 3. Linear velocity for the payload. The x component of the velocity converges to 0.2 m/s and the rest converge to zero.
Fig. 6. X and Y position trajectories for both agents (time varying v d ). Both agents travel in the x -direction until 2 seconds and make a smooth turn towards the ydirection after 2 seconds. 6. CONCLUSION AND FUTURE WORK
Fig. 4. Estimation of the unknown mass. The sum of the individual estimates converges to the actual mass of the load. We observe from Fig. 2 and 3 that the velocities of the agents and the payload converge to v d . The update law from (13) successfully recovers the actual mass of the payload as seen in Fig. 4. We observe from Fig. 5 that the squeeze force is regulated to the setpoint fid .
We have developed an adaptive decentralized cooperative control law that transports a payload with an unknown mass. The control law also regulates the squeeze forces and guarantees velocity convergence for all agents. We verify the performance of the controller using simulation results. Future work will involve implementation of the control laws at the dynamic level and experimental validation. 42
2018 IFAC NAASS June 13-15, 2018. Santa Fe, NM USA
Sandesh Thapa et al. / IFAC PapersOnLine 51-12 (2018) 38–43
43
z axis
T 1 = 0.00 s
0.6 T 2 = 0.75 s
0.4 0.2
T 3 = 1.50 s
0
T 4 = 2.25 s T 9 = 6.00 s T 8 = 5.25 s T 7 = 4.50 s T 6 = 3.75 s T 5 = 3.00 s
2
4
8 6
4
2
x axis 6
y axis
0
Fig. 10. Estimation of the unknown mass. The sum of individual estimate converges to the actual mass of the load, which is 0.5 kg.
Fig. 7. 3D position trajectory for both agents and the payload at different time interval. Note that all 3 agents stay at an equal distance and will never collide. They approach the y-axis by being aligned in a straight line as shown in Fig. 6. Notice that if the quadrotors are at the same height, they must keep a gap and the manipulator must be large enough to avoid their blades clashing.
Fig. 11. Contact forces of both agents in all The forces in the x direction converge the y-direction, they converge to 2.0 N respectively. In the z direction, the sum is equal to the weight of the payload.
telligent Robots and Systems (IROS), 2013 IEEE/RSJ International Conference on, 4990–4995. IEEE. Lee, H., Kim, H., and Kim, H.J. (2016). Planning and control for collision-free cooperative aerial transportation. IEEE Transactions on Automation Science and Engineering. Lippiello, V. and Ruggiero, F. (2012). Cartesian impedance control of a uav with a robotic arm. IFAC Proceedings Volumes, 45(22), 704–709. Maza, I., Kondak, K., Bernard, M., and Ollero, A. (2010). Multi-uav cooperation and control for load transportation and deployment. J. Intell. Robotics Syst., 57(1-4), 417–449. Meissen, C., Klausen, K., Arcak, M., Fossen, T.I., and Packard, A. (2017). Passivity-based formation control for uavs with a suspended load. IFAC-PapersOnLine, 50(1), 13150–13155. Mellinger, D., Shomin, M., Michael, N., and Kumar, V. (2013). Cooperative grasping and transport using multiple quadrotors. In Distributed autonomous robotic systems, 545–558. Springer. Michael, N., Fink, J., and Kumar, V. (2011). Cooperative manipulation and transportation with aerial robots. Autonomous Robots, 30(1), 73–86. Spong, M.W., Hutchinson, S., and Vidyasagar, M. (2006). Robot modeling and control. Wiley New York.
Fig. 8. Velocities of the 2 agents in all 3 directions. The x and y velocities are smooth during transition and the z velocity is zero. 3.5 3
velocity of load (m/s)
2.5 2 X Velocity vd,x Y Velocity vd,y Z Velocity vd,z
1.5 1 0.5 0 -0.5 -1 -1.5 0
1
2
3
4
5
3 direction. to zero. In and -2.0 N, of the force
6
time (sec)
Fig. 9. Desired and actual velocities of the load in all 3 directions. REFERENCES Bai, H. and Wen, J.T. (2010). Cooperative load transport: A formation-control perspective. IEEE Transactions on Robotics, 26(4), 742–750. Khalil, H.K. (1996). Noninear Systems. Prentice Hall. Kim, S., Choi, S., and Kim, H.J. (2013). Aerial manipulation using a quadrotor with a two dof robotic arm. In In43