쿼드로터기반공중작업시스템의동역학및제어

Transcription

1 공학박사학위논문 쿼드로터기반공중작업시스템의동역학및제어 Dynamics and Control of Quadrotor-based Aerial Manipulation Systems 2018 년 2 월 서울대학교대학원 기계항공공학부 뉴엔하이뉴엔

2 쿼드로터기반공중작업시스템의동역학및제어 Dynamics and Control of Quadrotor-based Aerial Manipulation Systems 지도교수이동준이논문을공학박사학위논문으로제출함 2017 년 10 월서울대학교대학원기계항공공학부뉴엔하이뉴엔뉴엔하이뉴엔의공학박사학위논문을인준함 2017 년 12 월 위원장 : 부위원장 : 박종우ㅐㅐㅐㅐ 이동준ㅐㅐㅐㅐ 위원 : 조규진ㅐㅐㅐㅐ 위원 : 김경수ㅐㅐㅐㅐ 위원 : 안효성ㅐㅐㅐㅐ

3 i Abstract Dynamics and Control of Quadrotor-based Aerial Manipulation Systems Hai-Nguyen Nguyen Mechanical & Aerospace Engineering, Seoul National University Quadrotor-type UAVs or simply drones have received considerable attention from the research communities and public alike due to their potentialities in a wide range of applications. Thanks to advancements in related technologies and to achievements in research, there are numerous successful applications of the drones, with aerial photography, geo-survey, traffic monitoring, pesticide spraying, surveillance and reconnaissance as the most representative ones. These applications, however, are mostly limited to passive observation capability of the drones. It is deemed by many research groups and companies that the potential of the drones equipped with active manipulation capability, i.e., the ability to physically interact with the surroundings, should be tapped not only for academic research but also to drastically expand the commercial market of the UAVs. The aim of this thesis is to contribute to the fundamental science and technology needed to realize such aerial manipulation capabilities. The focus of this work is on the dynamics and control problems related to aerial manipulation. The main specific contributions are 1) proposing a control framework for the quadrotortool system to enable a quadrotor to operate a tool rigidly attached on it [1, 2],

4 ii and 2) developing a novel aerial platform named Spherically-connected multi- Quadrotor platform to overcome the issue of under-actuation and limited payload of typical UAVs [3, 4]. Although the primary UAVs used throughout this thesis are quadrotors, our analysis, design and control scheme are not limited to this particular type of platform. In fact, our approaches work equally well on any thrust propelled vehicles such as helicopter, ducted-fan UAVs and other multirotor drones. Keywords: Aerial robotics, aerial manipulation, quadrotor, internal dynamics, multiple quadrotors Student Number:

5 Contents Acknowledgements iii List of Figures List of Tables Abbreviations vi viii ix 1 Introduction Motivation Research Problems and Approach Preview of Contributions Quadrotor-Tool System Introduction Modeling and Control Generation Dynamics Modeling Cartesian Control Generation at the Tool-Tip y Internal Dynamics and Stability of Quadrotor Tool Operation Internal Dynamics Structural Condition of Zero Dynamics Instability Finite-Time Escape Prevention Hybrid Position/Force Control iv

6 Contents v 2.5 Conclusion Spherically-connected multi-quadrotor Platform Introduction System Design and Modeling System Description Dynamics Modeling and Reduced Dynamics Control Generation and Feasibility Analysis Necessary Rank Condition for Full-Actuation in SE(3) Control Feasibility Control of S3Q-Platform System Lyapunov-Based Control Design Optimal Control Allocation Control of S2Q-Platform System Passive Decomposition of S2Q-Platform Dynamics Lyapunov-Based Control Design and Internal Stability Optimal Control Allocation and Closed-Form Expression Experimental Results Setup Control of S3Q-Platform Prototype Control of S2Q-Platform Prototype Conclusion Conclusions and Future Works Conclusions Future Works Distributed Cooperative Manipulation Estimation of Physical Interaction

7 List of Figures 2.1 Quadrotor-tool system Illustration of transformed angular velocity ν Unstable configurations of zero-dynamics Effect of d 3 on the internal dynamics Invariant set S The vector field ( ν 1, ν 2 ) along the boundaries S 1 and S The vector field ( ν 1, ν 2 ) along the boundary S The vector field ( ν 1, ν 2 ) along the boundary S Hybrid position/force control with d = (0.35; 0; 0.3)m Hybrid position/force control with d = (0.35; 0; 0.3)m Hybrid position/force control with the ν 3 -action deactivated Spherically-connected multi-quadrotor (SmQ) platform Range limit of spherical joint S2Q-platform with the two quadrotors and the tool Feasible control set of S2Q-platform and S3Q-platform Geometry of the reduced optimization problem of S2Q platform Hovering performance of S3Q-Platform prototype and single quadrotor Trajectory tracking: center-of-mass of the S3Q-frame tracking a horizontal rectangular shape while maintaining hovering posture (i.e., R o I 3 3 ) Stable interaction: position and attitude of the S3Q-frame response to the external wrench applied to the frame at t {4, 7, 12, 15, 21} [s] Manipulation task: tele-manipulating an object using haptic device vi

8 List of Figures vii 3.10 Trajectory tracking: the virtual tool-tip on the line connecting the centerof-mass positions of two quadrotors tracking a horizontal circular shape while maintaining a certain attitude (e.g., pitch at zero and yaw at 45 o ) Stable interaction: geometric center point x e and the pointing direction of S2Q-platform under the external wrench applied to the frame at t {8, 20, 32} [s] Manipulation task: tele-manipulating an object of [cm] using haptic device

9 List of Tables 3.1 Specifications of S2Q-platform and S3Q-platform prototypes viii

10 Abbreviations UAV MEMS IMU GNSS BLDC SLAM Unmanned Aerial Vehicle Micro-ElectroMechanical System Inertial Measurement Unit Global Navigation Satellite System Brush-Less Direct Current Simultaneous Localization And Mapping ix

11 Chapter 1 Introduction 1.1 Motivation Quadrotor-type UAVs or simply drones have received considerable attention from the research communities and public alike due to their potentialities in a wide range of applications. The rapid developments in sensors (e.g., MEMS IMU, GNSS, cameras), motors (e.g., BLDC motors), batteries (e.g., lithium-polymer batteries), materials (e.g., carbon fiber), onboard computing and wireless communication technologies, spurred by consumer electronics and information technology industry, have substantially lowered the price of the drones to the reach of general customers and hobbyists. At the same time, the fast advancements in 1

12 Chapter 1. Introduction 2 sensor fusion (e.g., [5]), SLAM (e.g., [6, 7]), computer vision (e.g., [8]) and control (e.g., [9 12]) technologies have enabled many successful applications for the drones, with aerial photography, geo-survey, traffic monitoring, pesticide spraying, surveillance and reconnaissance as the most representative ones. Despite the aforementioned advances in technology and research, the successful applications of the drones have been mostly limited to passive observation capability, and it is deemed by many research groups and companies that the potential of the drones equipped with active manipulation capability, i.e., the ability to physically interact with the surroundings, should be tapped not only for academic research but also to drastically expand the commercial market of the UAVs. The focus of this thesis is in the field of aerial manipulation, investigating the potential capabilities and scientific challenges for the drones to physically interact with the surrounding environment. The motivation, as mentioned above, comes from the possibility of opening up new application domains where the drones are capable of accomplishing meaningful tasks beyond the passive observation role played by typical commercial drones, turning the drones into truly versatile aerial robots 1. We envision that with such manipulation capability, the aerial robots would be useful for several novel applications. First, the unique capability of flying 1 In this thesis, we use the phrase quadrotor-type UAV, quadrotor and drone interchangeably, while the term aerial robot being limited to the drone equipped with manipulation capability.

13 Chapter 1. Introduction 3 over terrain allows UAVs to reach the target with speed and efficiency where other robots struggle to roll or crawl over. In a search and rescue mission, the UAVs can fly to the top floor building and open or even break the window to enter. The main advantage here is the time to reach the location which is crucial in certain emergent situations such as delivering first aid kit, throwing fire extinguishing balls. Second, the ability to manipulate in the air enables aerial robots to perform unique tasks which otherwise would be impossible or inefficient for ground-bound robots. One example of such tasks is contact inspection to detect and measure cracks of bridges, buildings and aerial pipes. Aerial robots would play an important role in inspection and maintenance of infrastructures such as offshore plants, power stations, and fuel processing facilities where the high costs involved. 1.2 Research Problems and Approach The field of aerial manipulation has gained the attention of the robotics community recently. There are initially a few results with the most intensively investigated aerial manipulation systems by far being the drone-arm configurations, in which a typical drone is equipped with a simple un-actuated tool [13 16], with a gripper [17 20], or with multi-dof robotic arms [21 29]. The main challenge of aerial manipulation, aside from the well-known limited payload issue, lays in the under-actuation of the conventional aerial platforms

14 Chapter 1. Introduction 4 (e.g., quadrotor, helicopter, ducted-fan) using in the drone-arm configurations. This under-actuation property appears not so detrimental for remote observing applications which only require motion control, particularly the motion control of the center-of-mass position which is well-known to define a flat-output. This under-actuation however poses a problem for certain applications such as aerial photography, where the camera view can suddenly tilt when the aerial platform starts to translate if some extra camera-gimbal actuation is not adopted. This issue of under-actuation can even be more problematic and crucial for aerial manipulation and operation. For example manipulation tasks, where the presence of side-way gust or lateral reaction force from the task itself (e.g., pulling/pushing force) is unavoidable, the under-actuated flying platform becomes susceptible, i.e., the platform would tilt over to counteract the gust or the external force, thereby, the attitude of the flying platform and consequently the tool/arm attached on it would be disturbed. This susceptibility, of course, can be suppressed with some actuated mechanisms with large enough DOF attached on the UAV (e.g., 7-DOF robotic manipulator), which, yet, is difficult to accommodate in practice due to the limited payload of typical drones. The aim of this thesis is to contribute to the fundamental science and technology needed to realize aerial manipulation capabilities. We approach the field from an engineering point of view. In particular, we utilize the tools from the rich literature in control theory, nonlinear systems and robotics to identify the technical limits and to design a new generation of aerial manipulation systems which involves developing novel control methods and mechanisms.

15 Chapter 1. Introduction Preview of Contributions This thesis focuses on the design, dynamics and control problems related to aerial manipulation. The main specific contributions are: 1) proposing a control framework for the quadrotor-tool system to enable a quadrotor to operate a tool rigidly attached on it [1, 2], and 2) developing a novel aerial platform named Spherically-connected multi-quadrotor platform to overcome the issue of underactuation and limited payload of typical UAVs [3, 4]. The quadrotor-tool system, in our opinion, is promising for certain applications, since with a simple un-actuated tool (e.g., screw-driver, contact probe), the system would be more affordable and also can accommodate higher interaction force with longer flight-time in comparison to a quadrotor-arm system with a heavy multi-dof arm. The control problem of such system, however, turns out to be much more challenging than controlling the center-of-mass of the quadrotor since the tool operation requires simultaneous control of the 6-DOF quadrotor position and orientation, yet, the quadrotor itself is under-actuated with only four actuations. This issue of under-actuation, in fact, might create unstable internal dynamics or even trigger finite-time escape. In chapter 2, the internal dynamics of the spatial quadrotor tool operation is fully characterized and requires the tool-tip should be located above the quadrotor s center-of-mass for the internal stability. We also propose a stabilizing action to prevent the finite-time escape occurred during physical interaction. This work identifies the

16 Chapter 1. Introduction 6 issue of under-actuation of the typical UAVs in aerial manipulation and proposed a control framework that enables the system to accomplish important tool operation tasks such as hybrid position/force control against a working surface. To extend the practical applicability of single-quadrotor tool systems, we further developed the Spherically-connected multi-quadrotor (SmQ) platform. This SmQ platform consists of multiple quadrotors connected to a rigid frame using spherical joints, where the quadrotors are utilized as distributed rotating thrust generators. In chapter 3, we show that the platform can be controlled by adjusting the attitude of the quadrotors and the magnitude of their thrusts. The platform is reconfigurable to be under-actuated or fully-actuated depending on the number of quadrotors attached to the frame and their geometry.the feedback control strategies are tailored for the under-actuated and fully-actuated configurations accordingly to guarantee stability in the presence of parameter uncertainty and external disturbance. The motion limits of the spherical joints and the boundedness of the thrust actuation of the quadrotors are also taken into account in a form of a constrained optimization problem, which is solved real-time in the implementations. This proposed SmQ platform can allow us to circumvent the issue of under-actuation while providing increased payload thanks to its adoption of multiple quadrotors.

17 Chapter 2 Quadrotor-Tool System 2.1 Introduction In this chapter, we focus on the case where the quadrotor operates a simple rigid tool attached on it (e.g., screw-driver, contact probe, etc.) acting itself as the actuator for the tool. See Fig We envision this quadrotor-tool system to be useful for such applications as inspection and repair of remote infrastructure, operation on high-rise building exteriors, etc. We also believe this usage of simple This chapter is a slightly adapted version of Hai-Nguyen Nguyen, Chansu Ha and Dongjun Lee, Mechanics, control and internal dynamics of quadrotor tool operation, Automatica, 61, pp ,

18 Chapter 2. Quadrotor-Tool System 8 tool would be more promising at least for some application than using a multi-link actuated robotic arm (with motors) attached to the quadrotor (e.g., [22, 25, 30]), given the limited payload of typical quadrotor platforms. The problem of quadrotor tool operation control, however, turns out to be much more challenging than that of the motion control of the quadrotor [9 12], as a proper tool control (e.g., tool-tip position y R 3 of Fig. 2.1) would require simultaneous control of both the translation and rotation of the quadrotor, which, yet, is under-actuated only with four control inputs. More specifically, in this chapter, we first show that we can generate any Cartesian control action at the tool-tip y, if and only if this tool-tip y is located either strictly above or strictly below the quadrotor s center-of-mass x R 3 (i.e., d see Fig We then reveal that, due to the quadrotor s under-actuation, this Cartesian control generation gives a rise to four-dimensional internal dynamics [31, 32] and elucidate a seemingly counter-intuitive necessary condition for its stability, that is, the tool-tip y should be installed above the quadrotor s center-of-mass x (i.e., d 3 < 0 - see Fig. 2.1), and not below it as would likely be attempted and designed in practice. We further manifest that this internal dynamics can even exhibit finite-time escape [31] due to some quadratic terms therein and propose a stabilizing control action to prevent finite-time escape thereof. Finally, relying on this theoretical finding, we propose novel control laws for the perhaps practically most important tool operation tasks, i.e., hybrid position/force control against a working surface.

19 Chapter 2. Quadrotor-Tool System 9 In contrast to the plethora of quadrotor motion control results [9 12], that for the quadrotor aerial manipulation are much fewer. Among them, we believe the followings are the most related ones to our work. The issue of stability with perturbed/changing center-of-mass (e.g., with payload) was considered in [33, 34]. These results, however, were limited only to the stabilization problem (e.g., stability of the linearized planar dynamics under the PID-stabilization [33]; different output than ours, which produces different and always stable internal dynamics [34]), and not applicable to our problem of spatial quadrotor-tool operation, where we not only need to achieve the quadrotor stability but also to drive the tool in the three-dimensional space by recruiting the quadrotor s translation and rotation motions. In [35, 36], the authors considered the problem of a quadrotor manipulating a pole and two quadrotors achieving airborne-transfer of a pole between them. For this, they assumed that the pole motion is coupled only to the quadrotor center-of-mass position x, not to its rotation R SO(3), and also the control inputs are the quadrotor s thrust force λ R and its angular rate w R 3. This in fact is in a stark contrast to our result here, where the quadrotor s rotation R must be recruited on top of its translation x to properly operate the tool, and the internal dynamics, which may exhibit even finite-time escape, arises with the angular torque input τ R 3. Hybrid position/force control problem was studied for the quadrotor [37, 38] and

20 Chapter 2. Quadrotor-Tool System 10 thrust E B x N B D B d 3 N O y d 1 tool mg D O E O fe τe Fig. 2.1: Quadrotor-tool system: {O} := {N o, E o, D o } and {B} := {N B, E B, D B } are inertial and body frames, with thrust and gravity along D B and D o directions. Tool is attached at d = [d 1, d 2, d 3 ] R 3 expressed in {B} with d 2 = 0. for the ducted-fan UAV [16], where, however, the quadrotor system was modeled only as a quasi-static black box wrench generator [37] or simplified as a combination of the decoupled linearized north-pitch, east-roll, and down dynamics [38]; or the dynamics of the ducted-fan UAV was again restricted to the saggital plane and further linearized [16]. Therefore, the complex (and potentially unstable) nonlinear and spatial dynamics of the quadrotor-tool operation (e.g., quadratic terms possibly triggering finite-time escape, and control action to prevent that by using pitch-roll-yaw coupling) could be neither revealed nor addressed in those works. Another line of relevant research is [39, 40], where the authors obtained similar internal dynamics and condition for its stability as ours for PVTOL (planar vertical take-off and landing) systems. These results [39, 40], however, 1) were limited only to the planar dynamics, whose extension to the spatial operation

21 Chapter 2. Quadrotor-Tool System 11 not only requires substantial development as done in this thesis (from the planar result of [13]) but also turns out necessary for us to design the control action to prevent finite-time escape; 2) overlooked the issue of finite-time escape, which, different from the pure motion control of PVTOL [39, 40], is crucial for the aerial tool operation, where the quadrotor can experience sudden (escape-triggering) surges of its velocity after impacts with environments/objects (see Fig. 2.11); and 3) considered a different problem (i.e., pure motion control of PVTOL) with a focus on a different mechanism of non-minimum phase dynamics (i.e., small force due to slant wing-tip jets) and only the position tracking control by using a specific flat output (i.e., Huygens center of oscillation). Due to this reason, our results in this thesis may be thought of as an extension (or re-finding) of [39, 40] to the quadrotor aerial tool operation with a complete analysis of its spatial/nonlinear (internal) dynamics and a novel control action to subdue its finite-time escape. The rest of the chapter is organized as follows. The dynamics of quadrotor-tool system is derived and the condition to generate Cartesian control at its tooltip is established in Sec The internal dynamics of spatial quadrotor tool operation is fully characterized in Sec. 2.3, with the seemingly counter-intuitive necessary condition for its stability revealed (i.e., tool above the quadrotor) and the finite-time escape prevention action designed/analyzed therein as well. The obtained theoretical results are then applied in Sec. 2.4 to the problems of hybrid position/force control against a working surface. Sec. 2.5 summarizes the paper with comments on future research.

22 Chapter 2. Quadrotor-Tool System Modeling and Control Generation of Quadrotor- Tool System Dynamics Modeling Consider the quadrotor with a rigid tool as shown in Fig Similar to [35, 36], we assume that the tool is light enough (or counter-balanced with symmetric design) so that the center-of-mass of the total quadrotor-tool system is still close to that of the quadrotor. This assumption is adopted here only for simplicity: the results of this work can be similarly derived when these two centers-of-mass are not coincident with each other. With this assumption, the dynamics of the quadrotor-tool system can still be described by the well-known quadrotor dynamics: mẍ = λre 3 + mge 3 + f e (2.1) J ω + ω Jω = τ + τ c, Ṙ = RS(ω) (2.2) where x R 3 is the quadrotor s center-of-mass position represented in the inertial frame {O} := {N o, E o, D o }, m > 0 is the mass, λ R is the thrust force, R SO(3) represents the rotation of the body-frame {B} := {N B, E B, D B } w.r.t {O}, f e R 3 is the tool interaction force represented in {O}, g is the gravitation constant, and e 3 = [0, 0, 1] T is the basis vector specifying the down direction. Also, for (2.2), J R 3 is the body-fixed rotational inertia, ω := [ω 1, ω 2, ω 3 ] T R 3

23 Chapter 2. Quadrotor-Tool System 13 is the angular velocity of {B} relative to {O} represented in {B}, τ, τ c R 3 are respectively the torque input and the external torque acting at the quadrotor s center-of-mass (defined below) all represented in {B}, and any ν R 3. S(ω)ν := ω ν for Now, as in Fig. 2.1, denote by d = [d 1, d 2, d 3 ] T R 3 the tool-tip position y from the quadrotor s center-of-mass x expressed in {B}. Then, the tool-tip position y in the inertial frame {O} can be written as y = x + Rd. (2.3) For simplicity, in this chapter, we assume d 2 = 0. We also assume the tool is interacting with the environment only through its tool-tip y via the contact force f e R 3 and the moment τ e R 3, with other external effects (e.g., aerodynamics force) much less than them. Then, τ c in (2.2) is given by τ c = d R T f e + R T τ e, with R T f e and R T τ e being f e and τ e represented in {B} Cartesian Control Generation at the Tool-Tip y In contrast to the motion control of quadrotor center-of-mass x, for the quadrotor tool operation, as can be seen from (2.3), we need to control both the translation x and the rotation R of the quadrotor at the same time. To better see this, we first transform the dynamics (2.1) into that of the tool-tip position y. For this,

24 Chapter 2. Quadrotor-Tool System 14 differentiating (2.3), we can obtain ẏ = ẋ + RS(ω)d, ÿ = ẍ + R[S( ω) + S 2 (ω)]d and, injecting these relations to (2.1), we can achieve the dynamics of the tool-tip position y s.t., mÿ = u + f e (2.4) where u := mr[s( ω) + S 2 (ω)]d λre 3 + mge 3 R 3 (2.5) is the Cartesian control action at the tool-tip position y, which is generated by λ R and ẇ R 3. The dynamics (2.4) with (2.5) now clearly shows that the tool control u requires simultaneous control of the quadrotor s translation (i.e., λ) and rotation (i.e., ω). An immediate question would then be whether it is possible to generate any desired control u R 3 via some combination of (λ, ω), and, if so, how. Here, we consider ω as the control input with the assumption that τ in (2.2) is strong enough, since, in this case, we can directly assign any value ω d R 3 to ω via the control torque τ in (2.2) (e.g., via (2.7)). With ω and λ as the control inputs, we

25 Chapter 2. Quadrotor-Tool System 15 can rewrite (2.5) s.t., 0 d 3 d 2 ω 1 0 d 3 0 d 1 ω m = S2 (ω)d+ gr T e 3 1 m RT u (2.6) d 2 d 1 0 ω 3 λ where we use S( ω)d = S(d) ω and u R 3 is the desired Cartesian control at the tool-tip position y (2.4). The following Th. 1 is a direct consequence of the structure of this control generation equation (2.6). Theorem 1. Given (λ, ω 1, ω 2, ω 3 ) as the control input, the left hand side (LHS) of (2.6) can produce any desired Cartesian control u R 3 if and only if d 3 0. Proof. For sufficiency, note that, if d 3 0, we can generate any value u for the first and second lines in the LHS of (2.6) by using ω 2, ω 3, while that for the last line by using λ. For necessity, suppose d 3 = 0. Then, the first and second lines of the LHS are generated only by ω 3 (even if d 2 0), implying that the LHS of (2.6) cannot produce arbitrary control action u. This Th. 1 implies that, to achieve a desired control u R 3 for the y-dynamics, we must have d 3 0, i.e., the tool-tip position y, when expressed in {B}, needs to be strictly above, or strictly below, the quadrotor s center-of-mass x. This Th. 1 is valid even if d 2 0. If d 3 0, we can then utilize (2.6) to compute the thrust command λ and angular acceleration command ω 1 d, ωd 2 for any given desired tool-tip Cartesian

26 Chapter 2. Quadrotor-Tool System 16 control u R 3. The remaining command ω d 3 is redundant and can be simply set to be zero or utilized for other purpose (e.g., see Sec ). To achieve ω ω d, we may then use the following simple PI-control with feedforward cancelation: τ = ω Jω + J ( [ ω d k ω t 0 ]) ω d (s)ds τ c (2.7) where k > 0 is the I-control gain. The closed-loop attitude dynamics is then given by ė ω + ke ω = 0, with e ω := ω(t) t 0 ω d (s)ds, implying ω ω d, as long as τ is powerful enough as assumed so above. Since the attitude dynamics (2.2) is fully-actuated and passive, many control techniques can also be used here even in the presence of uncertainty or absence of force sensing (e.g., sliding mode control [32, 41]). It may then appear that, as long as d 3 0, we can generate any arbitrary control u and drive the y-dynamics (2.3) however we want. This, however, may not be possible, since the control generation equation (2.6) defines a dynamic relation among ẇ ( ẇ d ), w and R, which may be unstable. As shown in Sec , this dynamics, unobservable from the y-dynamics, in fact constitutes the internal dynamics [31, 32] of the quadrotor tool operation, which arises because the tool-tip y control requires both the translation and attitude control, yet, the quadrotor is under-actuated only with the four actuations (λ, τ) R 4. In the next Sec. 2.3, we analyze this internal dynamics of quadrotor tool operation, reveal a (seemingly counter-intuitive) structural condition to avoid internal instability, show that this internal dynamics can exhibit finite-time escape, and propose a

27 Chapter 2. Quadrotor-Tool System 17 stabilizing control to prevent that. 2.3 Internal Dynamics and Stability of Quadrotor Tool Operation Internal Dynamics Although the equation (2.6) contains three rows, all of them may not constitute the internal dynamics, since: 1) the arbitrarily-assignable thrust input λ may effectively eliminate one-dimension of the internal dynamics (2.6); and 2) rank[s(d)] = 2, thus, only two-dimensional vector space among all possible ẇ d R 3 are relevant to the internal dynamics. To overcome this geometric complication, instead of ω, we utilize the transformed angular velocity ν := [ν 1, ν 2, ν 3 ] T R 3 as defined by ω = 1 d d 3 0 d 1 0 d 0 d 1 0 d 3 ν =: Σν (2.8) where d =: d d2 3 and Σ = ΣT = Σ 1. See Fig. 2.2 for an illustration of ν and ω. Here, with d 2 = 0 as assumed above, the last column of Σ characterizes the nullspace of S(d), while the first two the row space of S(d). Different velocity

28 Chapter 2. Quadrotor-Tool System 18 space decompositions were also used in [35, virtual body frame] and [42, passive decomposition] to facilitate dynamics analysis and control design. Using the transformed angular velocity ν of (2.8), we can rewrite (2.6) s.t., d 3 ν 2 d 1 ν1 2 + d 1ν2 2 + d 3ν 1 ν 3 0 d ν 1 + dν 2 ν m = grt e 3 1 m RT u (2.9) d 1 ν 2 d 3 ν1 2 + d 3ν2 2 d 1ν 1 ν 3 λ where the first two rows define the internal dynamics of ν 1 and ν 2, yet, the third row that of none, since we can arbitrarily assign λ to match the third row of RHS without creating any internal dynamics. Note also that there is no internal dynamics of ν 3. This ν 3 in fact vanishes when (2.8) is combined in (2.6), which can also be seen from Fig. 2.2 that ν 3 induces no motion at the tool-tip y. The internal dynamics in (2.9), yet, is rather difficult to analyze, since the quadrotor s rotation is expressed by the non-vector quantity R SO(3). To render (2.9) more amenable to analysis, here, we parameterize R using the yaw, pitch and roll angles, (φ, θ, ψ) =: q R 3, that is, c φ c θ s φ c ψ + c φ s θ s ψ s φ s ψ + c φ s θ c ψ R = s φ c θ c φ c ψ + s φ s θ s ψ c φ s ψ + s φ s θ c ψ s θ c θ s ψ c θ c ψ

29 Chapter 2. Quadrotor-Tool System 19 with θ < π/2 and s = sin, c = cos [41]. We then have the following kinematic relation: φ θ ψ = Γω = ΓΣν, Γ(θ, ψ) := 1 c θ 0 s ψ c ψ 0 c θ c ψ c θ s ψ c θ s θ s ψ s θ c ψ (2.10) and, combining (2.9) and (2.10) with the fact that gr T e 3 is a function of only (θ, ψ), we can obtain the following four-dimensional dynamics: θ d 3 s ψ d ν 3 d d ψ 1 +d 3 t θ c ψ ν dt = F(θ, ψ, ν 1, ν 2 ) + d 3 ν 1 1 d u 2 ν 2ν 3 1 ν 2 d 3 u 1 + ν 1ν 3 (2.11) where the drift vector field F(θ, ψ, ν 1, ν 2 ) is given by d ν 1 d 1 s ψ + ν 2 c ψ F(θ, ψ, ν 1, ν 2 ) = d ν 3 d 1 + d ν 1 d 1 t θ c ψ + ν 2 t θ s ψ ḡ d c θ s ψ R 4 γ(ν1 2 + ν2 2 ) + g d 3 s θ with γ := d 1 d 3, u = [u 1, u 2, u 3 ]T := 1 m RT u, and t := tan.

30 Chapter 2. Quadrotor-Tool System 20 ν 1 ω ν 2 2 ω 1 ω 3 ν 3 N O y D O E O Fig. 2.2: Illustration of angular velocity ω = [ω 1, ω 2, ω 3 ] T and the transformed angular velocity ν = [ν 1, ν 2, ν 3 ] T. This four-dimensional dynamics (2.11) completely describes the internal dynamics of the quadrotor tool operation, which arises due to the quadrotor s underactuation. In fact, this four-dimensional dynamics (2.11) is the internal dynamics [32], if we choose the output (y, φ) R 4 and the state (y, ẏ, q, ν) R 12 with the input (λ, τ) R 4. More precisely, 1) each of these output variables has relative degree two, thus, the internal dynamics should be of four-dimension as (2.11); 2) the internal dynamics (2.11), which specifies the internal quadrotor rotation necessary to drive the tool-tip position y, can be obtained by twice-differentiating y with (2.10); and 3) the yaw angle φ has relative degree two with ν 3 as the input, which unlike ν 2, ν 3 in (2.11), is not required for the control generation equation (2.9), thus, any value can be assigned to ν 3 without affecting the internal dynamics and also the generation of the control u for the y-dynamics (2.4).

31 Chapter 2. Quadrotor-Tool System Structural Condition of Zero Dynamics Instability In this Sec , we analyze stability property of the internal dynamics (2.11) around its equilibriums and elucidate a seemingly counter-intuitive structural condition for its stability, i.e., the tool-tip y should be installed above the the quadrotor s center-of-mass x, not below it. For this, we first define the zero dynamics by constraining (2.11) to the zero output manifold (i.e., (y, φ) 0). More specifically, we obtain the following conditions for the internal dynamics (2.11): ü 1 = ü 2 = 0 from ÿ = 0 in (2.4); and d 3 ν 3 = d 1 ν 1 dν 2 tan ψ from φ = 0 in (2.10). Applying these conditions to (2.11), we can achieve the zero dynamics as follows: 1 θ c ψ ν 2 d ψ d dt = d 3 ν 1 + γν 2 t ψ ν 1 ḡ d c θ s ψ γν 1ν 2 + d d 3 ν2 2 t ψ g ν 2 d 3 s θ d d 3 t ψν 1 ν 2 γν2 2 (2.12) From (2.12), we can show the zero-dynamics possesses the two equilibriums: [θ, ψ, ν 1, ν 2 ] = [0, 0, 0, 0] and [θ, ψ, ν 1, ν 2 ] = [0, π, 0, 0], with the former representing the quadrotor upright hovering posture, while the latter the upside-down hovering posture. See Fig We linearize the zero-dynamics (2.12) around

32 Chapter 2. Quadrotor-Tool System 22 N O E B N B d 3 D B N B D B d 3 E B D O E O d 1 d >0 3 d 1 d <0 3 Fig. 2.3: Unstable configurations of zero-dynamics: (θ, ψ, ν 1, ν 2 ) = (0, 0, 0, 0) with d 3 > 0 (left); and (θ, ψ, ν 1, ν 2 ) = (0, π, 0, 0) with d 3 < 0 (right). these two equilibriums. First, around the equilibrium [θ, ψ, ν 1, ν 2 ] = [0, 0, 0, 0], we can obtain: θ θ ψ 0 0 d = d 3 0 ψ ν 1 0 ḡ 0 0 ν d 1 g ν 2 d ν 2 (2.13) with the characteristic polynomial given by λ 4 2 g d 3 λ 2 + g2 d 2 3 = 0 and the four eigenvalues given by λ 2 = g d 3, implying that, if d 3 > 0, the zero-dynamics is unstable around the equilibrium [θ, ψ, ν 1, ν 2 ] = [0, 0, 0, 0].

33 Chapter 2. Quadrotor-Tool System 23 x d3>0 θ y mg e2 y e2 e3 x θ e3 d3<0 mg Fig. 2.4: Effect of d 3 on internal dynamics: 1) if d 3 > 0, gravity serves positivefeedback similar to inverted pendulum; 2) if d 3 < 0, negative-feedback similar to downward pendulum. On the other hand, for the equilibrium [θ, ψ, ν 1, ν 2 ] = [0, π, 0, 0], the linearization of the zero-dynamics (2.12) yields θ θ ψ 0 0 d = d 3 0 ψ ν 1 ḡ ν d 1 g ν 2 d ν 2 (2.14) with the characteristic polynomial given by λ g d 3 λ 2 + g2 = 0 and the four d 2 3 eigenvalues given by λ 2 = g d 3, implying that, if d 3 < 0, the zero-dynamics (2.11) will be unstable around the equilibrium [θ, ψ, ν 1, ν 2 ] = [0, π, 0, 0]. Note that these two unstable equilibriums of the zero-dynamics, (θ, ψ, ν 1, ν 2 ) = (0, 0, 0, 0) with d 3 > 0 for (2.13) and (θ, ψ, ν 1, ν 2 ) = (0, π, 0, 0) with d 3 < 0 for (2.14), represent the same configuration, that is, the quadrotor hovers with the tool-tip y located below the quadrotor s center-of-mass x. See Fig. 2.3.

34 Chapter 2. Quadrotor-Tool System 24 This observation then shows that, for internal stability of the quadrotor tool operation, the tool-tip y should be located above the quadrotor s center-of-mass x, not below it, even though the latter would appear more reasonable at a first glance and also typically aimed for in practice. The above analysis also shows that, if y is located above x, the internal dynamics may be stable, which we indeed observed from our simulation with some viscous damping effect (e.g., aerodynamics dissipation), when the quadrotor angular velocity ω is not so fast and γ = d 1 /d 3 is not so large. This seemingly counter-intuitive necessary condition for the internal dynamics stability is summarized in the following Th. 2. Theorem 2. Consider the quadrotor tool operation system (2.1)-(2.2) with a rigid/light tool attached with its tool-tip y located at d = [d 1, 0, d 3 ] T as measured in {B} from the quadrotor center-of-mass x. Then, a necessary condition to avoid internal dynamics instability is d 3 < 0 for the equilibrium (θ, ψ, ν 1, ν 2 ) = (0, 0, 0, 0) or d 3 > 0 for the equilibrium (θ, ψ, ν 1, ν 2 ) = (0, π, 0, 0), both implying the tool-tip y should be above the center-of-mass x, not below it. This Th. 2 can also be understood as follow. If the tool-tip y is located below the quadrotor s center-of-mass x (e.g., d 3 > 0 for upright hovering posture) and if we want, for example, to regulate the tool-tip position, the thrust and angular torque inputs will be used up to maintain this tool-tip position, yet, the gravity will still pull the system down similar to the case of an inverted pendulum - see Fig This similarity is also evident in (2.13), where the zero dynamics has a saddle equilibrium with two unstable and two stable poles (i.e., (2.13) at

35 Chapter 2. Quadrotor-Tool System 25 (θ, ψ, ν 1, ν 2 ) = (0, 0, 0, 0) with d 3 > 0). On the other hand, if the tool-tip y is above x (e.g., d 3 < 0 for upright hovering posture), the system would behave similar to the stable downward pendulum as again can be seen from (2.13) with pure imaginary poles when d 3 < 0 (right plot of Fig. 2.4). Note also that Th. 2 holds regardless of any ν 3 -action (2.16). Even if we upheld the necessary condition of Th. 2, sometimes, we could still observe (fairly severe) instability of the internal dynamics, particularly when the quadrotor s angular velocity ω gets faster with a large γ = d 1 /d 3. This is in fact due to the quadratic terms (i.e., γ(ν1 2 + ν2 2 )) in (2.11), which can trigger finite-time escape [22]. In the next Sec , we analyze this finite-time escape and propose a stabilizing control action to prevent that Finite-Time Escape Prevention To facilitate analysis of finite-time escape and synthesis of the control action to prevent that, we rewrite the dynamics of ν 1 and ν 2 in (2.11) s.t., ν 1 0 = + 0 ν 3 ν 2 γ(ν1 2 + ν2 2 ) ν 3 0 ν 1 + ū1 (2.15) ν 2 ū 2 where γ = d 1 d 3, ū 1 := g c θ s ψ+u 2 d and ū 2 := g s θ+u 1 d 3. Here, without loss of generality, we assume d 1 > 0. We also choose d 3 < 0 to respect the necessary condition for internal stability of Th. 2. We then have γ > 0. We can then see

36 Chapter 2. Quadrotor-Tool System 26 from (2.15) that the quadratic terms with γ in (2.15) will always pull (ν 1, ν 2 ) downward in the (ν 1, ν 2 )-plane (see Fig. 2.5) and, moreover, can induce finitetime escape when ν 2 < 0. We can see from (2.15) that this tendency would be more intense with larger γ = d 1 /d 3 and faster (ν 1, ν 2 ). We may also consider ū 1, ū 2 in (2.15) as bounded disturbance, as the desired control u is bounded (with u = (1/m)R T u). To prevent the finite-time escape, here, we attempt to utilize ν 3 to regulate (ν 1, ν 2 ). Recall from Sec that this ν 3 can be arbitrarily assigned without affecting the tool-tip control u generation (2.6). This ν 3 -action, however, is limited, as its action is embedded in the skew-symmetric matrix in (2.15), thus, is only norm-preserving with its direction always tangential to the circle in the (ν 1, ν 2 )-plane with the radius of ν ν2 2 and the center at the origin. With this circumferential ν 3 -action, we may yet still think of the following strategy. Since the quadratic terms with γ in (2.15) always pull down (ν 1, ν 2 ) along the ν 2 -axis and can trigger the finite-time escape only when ν 2 < 0, when ν 2 proceeds down along the ν 2 -axis, if we can apply large enough ν 3 -action to swing up (ν 1, ν 2 ) back to the upper-side of the (ν 1, ν 2 )-plane (i.e., ν 2 0) in a suitable way to ensure the boundedness of ν 1 as well, we would be able to prevent the finite-time escape even with only this limited ν 3 -action. To achieve this strategy, we design the ν 3 -action s.t., ν 3 := γν 1 (1 + ν 2 2) + ρsgn(ν 1 ) (2.16)

37 Chapter 2. Quadrotor-Tool System 27 ν 2 4 / 4 _ γ η ν 1 3 / 1 / / -2 2 Fig. 2.5: Invariant set S with finite-time escape prevention. where ρ := 2η γν 2, with η > 0 defined s.t., η > ( ū 1 (t) + ū 2 (t) )/2 t 0, and sgn(ν 1 ) = 1 if ν 1 0 and sgn(ν 1 ) = 1 if ν 1 < 0. Theorem 3. Consider the dynamics (2.15) under the ν 3 -action (2.16) with γ 0. Then, there exists a positive-invariant compact set S {(ν 1, ν 2 ) ν ν2 2 ( η γ + 5)2 + η2 γ 2 } s.t., if (ν 1 (0), ν 2 (0)) S, (ν 1 (t), ν 2 (t)) S t 0.

38 Chapter 2. Quadrotor-Tool System 28 Proof. Let us first define the compact set S with its boundary given by S := S i + S i, where i i S 1 := {(ν 1, ν 2 ) ν 2 = ν 1 1, 0 ν 1 1} S 2 := {(ν 1, ν 2 ) ν 2 = 2, 1 ν 1 3} S 3 := {(ν 1, ν 2 ) ν 1 = ν 2 + 5, 2 ν 2 η γ } S 4 := {(ν 1, ν 2 ) ν ν 2 2 = ( η γ + 5)2 + η2 γ 2, η γ ν 2 ( η γ + 5)2 + η2 } γ 2 and S i := {( ν 1, ν 2 ) (ν 1, ν 2 ) S i }. See Fig. 2.5 To show that the set S is positive-invariant, here, we show that the vector field ( ν 1, ν 2 ) of (2.15) under the ν 3 -action (2.16) always points inward S along its boundary S. For this, note that ν 3 in (2.16) is an odd function w.r.t. ν 1. This then implies that ν 1 and ν 2 in (2.15) are respectively odd and even functions w.r.t., ν 1. Therefore, if we can show that ( ν 1, ν 2 ) points inward S along the right half of S, the same will also be true for the other half S. We now analyze the vector field ( ν 1, ν 2 ) along each segment S i of the right half of S. On the boundary S 1 (see Fig. 2.6a), we have ν 1 = γν 1 ν 2 (1 + ν 2 2) ρν 2 + ū 1 = γν 1 ν 2 (1 + ν 2 2) + ρ ν 2 + ū 1 > ρ ν 2 + ū 1 > 0 with ρ = γ ν 2 + 2η > ū 1 and ν 2 1 on S 1. On the other hand, on S 1, ν 2

39 Chapter 2. Quadrotor-Tool System 29 ν 2 ν 2 1 ν ν ν 2 ν ν ν 2 2 ν 1 (a) ( ν 1, ν 2 ) on S 1. (b) ( ν 1, ν 2 ) on S 2. Fig. 2.6: The vector field ( ν 1, ν 2 ) points inward the set S along the boundaries S 1 and S 2. can be positive or negative. If ν 2 0, the vector ( ν 1, ν 2 ) will point inward S, thus, here, we only consider the case ν 2 < 0, for which ( ν 1, ν 2 ) points inwards if ν 1 ν 2 (2.17) or, using (2.15) with (2.16), γν 1 ν 2 (1 + ν 2 2) + ρ ν 2 + ū 1 γν 2 2(ν 2 1 1) ρν 1 ū 2 Since, on S 1, we have γν 1 ν 2 (1 + ν 2 2 ) 0 and γν2 2 ν2 1 ρν 1 0, (2.17) will be satisfied if ρ ν 2 + ū 1 γν ū 2 (2.18) which is indeed true, since, substituting ρ = 2η + γ ν 2 on S 1 into (2.18) yields γν η ν 2 + ū 1 γν ū 2

40 Chapter 2. Quadrotor-Tool System 30 _ η γ ν ν 1 Fig. 2.7: The vector field ( ν 1, ν 2 ) along the boundary S 3. ν 2 ν 1 which holds, since, on S 1, ν 2 1, thus, 2η ν 2 > ū 1 + ū 2. This shows that, on segment S 1, the vector field ( ν 1, ν 2 ) points inwards the set S, as depicted in Fig. 2.6a. On the boundary S 2 (see Fig. 2.6b), from (2.15) with (2.16), we have ν 1 > ρ ν 2 + ū 1 = 2η ν 2 + γ ν ū 1 > 0 ν 2 ρν 1 + ū 2 = (2η + γ ν 2 ) ν 1 + ū 2 > 0 since ν 1 1, ν 2 = 2 and ρ = 2η + γ ν 2 > ū 1 + ū 2 on S 2, implying that the vector ( ν 1, ν 2 ) on the boundary S 2 also points into S, as illustrated in Fig. 2.6b. On the boundary S 3 (see Fig. 2.7), we have: ρ = 2η γν 2 η (2.19)

41 Chapter 2. Quadrotor-Tool System 31 with 2 ν 2 η/γ. Then, similar to the case of S 2 above, we can show that ν 2 > ρν 1 + ū 2 > 0 since ν 1 3 on S 3. On the other hand, along the boundary S 3, ν 1 can be positive or negative. If ν 1 is negative then the vector field points inwards. Thus, here, we only need to consider the case of ν 1 0. When ν 1 0, the vector ( ν 1, ν 2 ) will point into S if ν 2 ν 1 (2.20) or, using (2.15) with (2.16) and (2.19), we can rewrite (2.20) s.t., γν 2 2 (ν2 1 1) γν 1 ν 2 + 2ην 1 + ū 2 γν 1 ν 2 (1 + ν 2 2 ) + γν2 2 2ην 2 + ū 1, which can be rearranged as γν 2 2(ν 2 1 2) + 2ην 1 + ū 2 γν 1 ν 3 2 2ην 2 + ū 1. (2.21) Now, for the boundary S 3, let us first consider the upper segment overlapped with the region {(ν 1, ν 2 ) ν 1 5, ν 2 0}. Then, since γν 1 ν ην 2 0 with ν 2 0 and ν 1 > 0, (2.21) will be attained if γν 2 2(ν 2 1 2) + 2ην 1 + ū 2 ū 1 which is satisfied, since ν 1 5 on this segment. On the other hand, for the lower

42 Chapter 2. Quadrotor-Tool System 32 ν 2 ν 4 _ γ η V ν Fig. 2.8: The vector field ( ν 1, ν 2 ) along the boundary S 4. segment of S 3 within the region {(ν 1, ν 2 ) 3 ν 1 < 5, 2 ν 2 < 0}, we can rewrite (2.21) as γν 2 2(ν 2 1 ν 2 ν 1 2) + 2η(ν 1 ν 2 ) + ū 2 ū 1 which is also satisfied, since on this segment, ν 2 1 ν 2 ν and ν 1 ν 2 1. Thus proves that, on the boundary S 3, the vector ( ν 1, ν 2 ) points into the set S. For the arc segment S 4, let us define V := 1 2 (ν2 1 + ν 2 2) which then assumes a constant value along this segment, that is, V (ν 1, ν 2 ) =

43 Chapter 2. Quadrotor-Tool System [(η/γ + 5)2 + (η/γ) 2 ] > 25/2, (ν 1, ν 2 ) S 4. Differentiating this V with (2.15) with the fact that η ū ū2 2, we can then obtain V = γν 2 (ν1 2 + ν2) 2 + (ū 1 ν 1 + ū 2 ν 2 ) η(ν1 2 + ν2) 2 + 2η ν1 2 + ν2 2 η(2v 2 2V ) < 0 since, on S 4, ν 2 η/γ and V > 5/ 2. This then implies that (ν 1, ν 2 ) evolves into the set S when it is on the boundary S 4 - see Fig This Th. 3 shows that the ν 3 -action (2.16), albeit limited as stated above, can indeed prevent the finite-time escape of the internal dynamics as long as (ν 1 (0), ν 2 (0)) S. This also means that, using the ν 3 -action (2.16) with (ν 1 (0), ν 2 (0)) S and also with the necessary condition of Th. 2 satisfied, we can achieve any desired tool-tip position control u R 3 via (2.6) while avoiding the issue of internal dynamics instability. Mathematical proof of this finite-time escape is not available at the moment, although we indeed experienced many of its occurrences during our simulation study (e.g., see Fig and [13, Fig. 3]), which our ν 3 -action turns out to be able to adequately subdue. A precise/mathematical treatment of this finite-time escape itself would require a significant technical development as in [43] and is beyond the scope of this work. The ν 3 -action (2.16) contains the discontinuous function sgn( ), which poses an implementation problem since our control input is (λ, τ) R 4. For this, we may replace sgn(ν 1 ) by its continuous approximation (e.g., sat(ν 1 /ɛ) or 2 π atan(ν 1/ɛ),

44 Chapter 2. Quadrotor-Tool System 34 with approximation layer ɛ > 0). Note that this approximation would affect the ν 3 -action (2.16) only around the two intersection points between the ν 2 -axis and the boundary S (see Fig. 2.5), implying that the resultant performance degradation would be only mild. In fact, we found, during our simulations (e.g., Sec. 2.4), that 2 π atan(ν 1/ɛ) works just fine when used instead of sgn(ν 1 ). Note also that, if the task is formulated s.t., (ν 1, ν 2 ) is kept small enough (e.g., Sec. 2.4), the ν 3 -action would not be necessary, since, in this case, the quadratic terms with γ in (2.15) will be dominated by other (e.g., linear) terms therein. The necessary condition of Th. 2, yet, should still be enforced even in this case to avoid internal dynamics instability. See Sec Our proposed ν 3 -action (2.16) does not interfere at all with the generation of the tool-tip Cartesian control u R 3, since, as can be seen from (2.9), any desired control u R 3 can be generated only by using ( ν 1, ν 2, λ) regardless of ν 3. This ν 3 -action, however, will produce some extra rotational motion of the quadrotor (e.g., φ via (2.10)) on top of those necessary to generate the u-control through (2.9). This extra motion may then induce collision when the quadrotor is interacting with external environments/objects. One possible approach to avoid such collision, and further to endow the quadrotortool system with collision avoidance capability, is to activate the ν 3 -action only when (ν 1, ν 2 ) is outside S, while, when inside S, exploit the ν 3 -action for collision avoidance. This idea then naturally suggests to switch the ν 3 -action (2.16) with S as its switching surface. For this, the proposed ν 3 -action (2.16) is not

45 Chapter 2. Quadrotor-Tool System 35 directly applicable, since, with (λ, τ) as the control input, we cannot instantaneously switch ν 3 on S. Instead, we would need to define a new switching surface S w S, whose gap from S should be designed with the convergence rate of the ν 3 -control and ( ν 1 (t), ν 2 (t)) taken into account, so that, even with the (indirect) control τ, the ν 3 -action is guaranteed to be activated before (ν 1, ν 2 ) escapes from S, while also minimizing the gap between S w and S. How to design such switching ν 3 -action with an optimal S w is a topic of future research. 2.4 Hybrid Position/Force Control We first set d 3 0, so that, following Th. 1, we can generate any Cartesian control action u R 3 at the tool-tip y R 3 by using (λ, τ) via (2.6), or, more precisely, by using (λ, ν 1, ν 2 ) via (2.9). We may then consider the y-dynamics as a simple fully-actuated point mass dynamics (2.4), i.e., mÿ = u + f e (2.22) where we can assign u R 3 arbitrarily and f e R 3 is the interaction force between the tool-tip and the working surface. Once we have this fully-actuated y-dynamics (2.22), it becomes rather a standard task to design the hybrid position/force control for u. Of course, this statement and the validity of (2.22) hinge upon the well-behavedness of the internal dynamics (2.11). To ensure this, we apply the ν 3 -action (2.16),

46 Chapter 2. Quadrotor-Tool System 36 which turns out to be crucial here, as it can prevent finite-time escape of the internal dynamics (2.11) even against the sudden surges of angular rate ω due to the tool-surface collision impacts (even with d 3 < 0 - see Fig. 2.9) or aggressive transition from the unstable equilibrium to the stable equilibrium with d 3 > 0 (see Fig. 2.10). If it were not for this ν 3 -action, these sudden surges of w would trigger finite-time escape, thereby, invalidate the dynamics (2.22). As stated after Th. 3 of Sec , this ν 3 -action does not at all interfere with the tool-tip control u, implying that, here, we can just focus on the design of u in (2.22). This ν 3 -action, as also mentioned in Sec , yet, can produce extra rotation motion, which may induce quadrotor-surface collision. Although no collision occurs here in our simulation, how to prevent collision, or, even further, how to achieve collision avoidance ability, by exploiting this ν 3 -action is a topic of our on-going work (see the second last paragraph of Sec ). With this stabilizing, yet, decoupled ν 3 -action, we can then just focus on (2.22) to design the hybrid position/force control for u. For this, we particularly utilize passive decomposition [42, 44], which not only provides both the tangential and normal dynamics w.r.t. the surface (in contrast to [45]), but also requires less cancelation of the open-loop dynamics (as compared to [46]). A full treatment of general hybrid position/force control is not a main topic of this thesis and we refer readers to the above references.

47 Chapter 2. Quadrotor-Tool System 37 We first define the two-dimensional working surface of the tool-tip y R 3 by h(y) = 0 (2.23) where h : R 3 R is a smooth function with full-rank Jacobian, with its level sets constituting a foliation of the working surface. Then, following [42], at each y, we can decompose the tangent space T y M and the cotangent space Ty M of the y-dynamics (2.22) s.t., T q M = D D and T q M = Ω Ω where D and D are the distributions of the null-space of h y and the orthogonal complement of D w.r.t. the inertia metric m of (2.22); and Ω and Ω are the annihilating co-distributions of D and D, respectively. With this decomposition of the tangent and co-tangent spaces, we can write the velocity ẏ and control/contact-force u + f e of the y-dynamics (2.22) s.t., [ ] ẏ =: D D v l, u + f e =: [ Ω T }{{} v ΩT ] u l + f l (2.24) h }{{} u h + f h =:D(y) =:Ω T (y) where v := [v l ; v h ] R 3 is the transformed velocity, and the matrices D R 3 2, D R 3 1, Ω R 2 3 and Ω R 1 3 identify D, D, Ω and Ω, respectively. Since the inertia metric m of (2.22) is Euclidean, following [42], we can obtain the following closed-form expressions for these matrices: D null( h/ y)

48 Chapter 2. Quadrotor-Tool System 38 with D T D = I, Ω = h y, D = Ω T (Ω Ω T ) 1, and Ω = (D T D ) 1 D T. We can then show that D T D = 0, ΩD = I, dh dt = Ω ẏ = Ω D v h = v h where the last equation clearly shows that v h R is the velocity of the tool-tip y normal to the working surface, while v l R 2 is that tangential to the surface. Differentiating (2.24), we can further obtain ÿ = D v + Ḋv and, inserting this into (2.22) with the left-multiplication by D T, we can achieve m v l + Q l (y, ẏ)v l + Q lh (y, ẏ)v h = u l + f l (2.25) m v h + Q h (y, ẏ)v h + Q hl (y, ẏ)v l = u h + f h (2.26) where v h = ḣ and T md (y)ḋ(y) =: Q l(y, ẏ) Q lh (y, ẏ) Q hl (y, ẏ) Q h (y, ẏ) The dynamics in (2.25) is called locked system, which specifies the y-dynamics tangential to the working surface (2.23), whereas the dynamics in (2.26) shape system, which describes the y-dynamics normal to the surface (2.23). Both of these locked and shape systems inherit the Lagrange structure and passivity from

49 Chapter 2. Quadrotor-Tool System 39 the original dynamics (2.22), with Q l R 2 2 being skew-symmetric, Q h = 0 and Q T lh = Q hl. This preserved structure and passivity can be utilized for control synthesis (e.g., passivity-based control) as shown below. For more details on passive decomposition, refer to [42, 44]. One we have the locked-shape dynamics decomposition (2.25)-(2.26), it becomes rather straightforward to design the hybrid position/force control. First, to drive the the tool-tip position y on the working surface (2.23), we can control the locked system (2.25). For this, denote a local parameterization of the surface by q l := q l (y) R 2. Differentiating q l with the fact that q l (y) does not change along the normal direction to the surface, we then have q l := q l y ẏ = q l y D v l =: B 1 (y)v l where B(y) := ( q l y D ) 1 R 2 2 is assumed to exist locally. With this B, we can further obtain that v l = Bq l and v l = B q l + Ḃq l, and, substituting these to (2.25) and left multiplying by B T, we have mb T B q l + B T (mḃ + Q lb) q l + B T Q lh v h = B T (u l + f l ) where mb T B R 2 2 is the positive-definite projected inertia on the surface with d dt (mbt B) 2B T (mḃ + Q lb) being skew-symmetric. We can then design the following exponentially-converging passivity-based tracking control on the

50 Chapter 2. Quadrotor-Tool System 40 surface: u l := Q lh v h f l + mb q d l + (mḃ + Q lb) q d l B T [b l ( q l q l d) + k l(q l ql d )] (2.27) where b l, k l > 0 are the control gains, and q d l (t) R2 is the desired trajectory on the surface. On the other hand, for the tool-tip behavior normal to the surface, we can control the shape system (2.26). For this, when y is outside the surface, we first apply the following control u l to approach to the surface: u h := Q lh v l b h v h k h (h h ) (2.28) where v h = ḣ, Q h = 0 and h R is the set-position slightly within (or close to) the surface (2.23), and k v, k p > 0 are the control gains. Once contact is detected, we then switch to the following normal force control: u h := Q lh v l b h v h f d h k i t 0 (f d h f h)ds (2.29) where f d h R is the desired contact force, and b h, k i > 0 are the control gains. See [47] for some gain conditions, with which we can ensure f h f d h after only a finite number of switchings between these controls u h. This designed hybrid position/force control (u l, u h ) is then decoded into the

51 Chapter 2. Quadrotor-Tool System 41 control inputs (λ, τ) of the quadrotor (2.1)-(2.2) as follows: 1) transform (u l, u h ) back to the desired tool-tip control u by (2.24); 2) compute the thrust input λ and the desired ( ν 1, ν 2 ) to produce this u from the internal dynamics (2.9); 3) given ( ν 1, ν 2 ) and the ν 3 -action of (2.16), obtain the desired angular acceleration ω d R 3 by using (2.8); and 4) compute the torque input τ via (2.7) to achieve ω ω d. The following Th. 4 summarizes main property of the hybrid position/force control (2.27)-(2.29), for which the decoupling between the ν 3 -action and the generation of u via (2.9) is crucial. Theorem 4. Consider the quadrotor-tool system (2.1)-(2.2) under the hybrid position/force control (2.27)-(2.29) and the ν 3 -action (2.16). Then, q l ql d exponentially and f h fh d asymptotically (under the condition of [47]) without finite-time escape of internal dynamics. We perform simulations of this hybrid position/force control. For that, we assume 2% of parametric uncertainty and 3% actuation uncertainty. We approximate the discontinuous sgn( ) function by the continuous atan( ) function for the ν 3 -action (2.16) as mentioned in Sec We define the working surface to be a vertical plane defined by x = 2[m] as shown in Fig For the first simulation, we set d = [0.35; 0; 0.3]m (i.e., tool above the quadrotor) according to Th. 2 to maintain the stable tool operation configuration. We also activate the ν 3 -action (2.16) to prevent finite-time escape according to Th. 3. The results are shown in Fig. 2.9, where we can observe that, even with the surge of angular velocity due the impact between the tool-tip and the surface

52 Chapter 2. Quadrotor-Tool System 42 (around 3sec.), the quadrotor can still maintain stable operation thanks to the stabilizing ν 3 -action, with which (ν 1, ν 2 ) can stay within the compact set S of Th. 3 (second plot of Fig. 2.9). We can also see from Fig. 2.9 that the trajectory tracking can be ensured even during the impact (with no limit imposed on the control λ, τ) and also with the ν 3 -action (see the last third paragraph of Sec ). The contact force control is also decoupled from the ν 3 -action as well, which is yet difficult to see from Fig. 2.9, as the transient behavior of the contact force control after the impact excites the ν 3 -action (2.16). For the second simulation, we set d = [0.35; 0; 0.3]m to intentionally violate the necessary condition of Th. 2, yet, still keep the ν 3 -action of Th. 3. The results are presented in Fig. 2.10, where we can observe that, as the quadrotor starts from the unstable configuration (i.e., with the tool below it), it is flipping over by itself to the stable equilibrium (i.e., tool above the quadrotor). It also hits the wall around 2 sec., (indicator (3) of Fig. 2.10) with a large impact force, bounces back from the wall, approaches and makes a contact with the wall again around 5 sec. (indicator (4)). After that, the quadrotor attains the steady-state trajectory tracking and the normal force regulation while maintaining the stable posture (i.e., tool above the quadrotor). During the course of flipping (indicator (2) in Fig. 2.10) and also after the impacts, the quadrotor gains fairly large angular velocity, which is then suppressed by the ν 3 -action not to trigger finite-time escape. Similar to Fig. 2.9, in Fig. 2.10, both the trajectory tracking and the contact force control are achieved without

53 Chapter 2. Quadrotor-Tool System ν 2 (rad/s) Trajectory Invariant set ν 1 (rad/s) Reaction force(n) Thrust (N) 0 1 Reaction force Desired force Time(s) Time (s) Tracking error (m) ν (rad/s) 1 0 y 1 y 1 d y 2 y 2 d y 3 y 3 d Time(s) 1 0 ν 1 ν 2 ν Time(s) Fig. 2.9: Hybrid position/force control with d = [0.35; 0; 0.3]m and the stabilizing ν 3 -action (2.16). being affected by the ν 3 -action, with the first even ensured during the impacts as well. It is also worthwhile to mention that the indicators (1) and (2) outside the invariant set S in Fig do not contradict to Th. 3, as the invariant set S in Th. 3 is defined for the stable posture (i.e., tool above the quadrotor), yet, the indicators (1) and (2) of Fig are associated when the quadrotor is operating around the unstable equilibrium (i.e., tool below the quadrotor).

54 Chapter 2. Quadrotor-Tool System 44 5 (3) (2) 2.5 (4) (3) (4) (1) ν 2 (rad/s) (1) Reaction force(n) Thrust (N) (1)(3) (4) 0 1 Reaction force Desired force 2 0 (2) Time(s) (1)(3) 15 (4) (2) Time (s) Tracking error (m) ν (rad/s) 0 5 (1)(3) (4) 1 (2) Trajectory Invariant set ν 1 (rad/s) y 1 y 1 d y 2 y 2 d y 3 y 3 d 1 0 (2) Time(s) (1)(3) (4) 50 0 ν 1 ν 2 ν (2) Time(s) Fig. 2.10: Hybrid position/force control with d = [0.35; 0; 0.3]m and the stabilizing ν 3 -action (2.16). We also perform the simulation with the same setting as Fig. 2.9, yet, in this case, turn off the ν 3 -action. The results are shown in Fig. 2.11, where we can see that, even if the necessary condition of Th. 2 is granted, without the ν 3 -action, the sudden increase of the angular velocity when the quadrotor hits the wall (around 3 sec. both for Fig. 2.9 and Fig. 2.11) indeed triggers finite-time escape. We observe similar occurrence of the finite-time escape when we turn off the

55 Chapter 2. Quadrotor-Tool System ν 2 (rad/s) Trajectory Invariant set ν 1 (rad/s) Tracking error (m) 2 0 d y 1 y 1 d y 2 y 2 d y 3 y Time(s) 5 ν (rad/s) 0 ν 1 ν 2 ν Time(s) Fig. 2.11: Hybrid position/force control with d = [0.35; 0; 0.3]m and the ν 3 -action (2.16) deactivated. ν 3 -action for the simulation of Fig (not shown here), for which finite-time escape occurs as the quadrotor is flipping over (i.e., indicator (2) of Fig. 2.10).

56 Chapter 2. Quadrotor-Tool System Conclusion In this chapter, we investigate issues salient to the dynamics and control of the quadrotor, when it is operating a simple rigid tool attached on it. Some structural conditions are elucidated to generate arbitrary tool-tip Caertesian control action (Th. 1) and also to avoid instability of the internal dynamics (Th. 2), which inevitably arises due to the quadrotor s under-actuation. This necessary condition for the internal stability (Th. 2) is particularly interesting, since it suggests a design, which would appear counter-intuitive to the practitioners, that is, the tool should be installed above the quadrotor, not below it. We also fully characterize the four-dimensional nonlinear internal dynamics of the spatial quadrotor tool-operation, manifest that some quadratic terms therein can trigger finite-time escape, and propose a stabilizing control action to suppress that (Th. 3). The obtained theoretical results are then illustrated/validated for the two practically-important application scenarios: 1) rotational tool operation including screw-driver and vertical-jack; and 2) hybrid position/force control against a working surface.

57 Chapter 3 Spherically-connected multi-quadrotor Platform 3.1 Introduction The key challenge in using the quadrotors for aerial operation/manipulation is their under-actuation, i.e., with all the rotors colinearly attached to the body, it cannot change the forcing (or thrust) direction without tilting the platform This chapter is a slightly adapted version of Hai-Nguyen Nguyen, Sangyul Park, Junyoung Park and Dongjun Lee, A novel robotic platform for aerial manipulation using quadrotors as rotating thrust generators, IEEE Transactions on Robotics (T-RO), (to appear) 47

58 Chapter 3. Spherically-connected multi-quadrotor Platform 48 itself. To better see this, consider the drone-manipulator systems (i.e., quadrotor with multi-dof robotic arm), which, although most-widely studied for aerial manipulation (e.g., [21 29, 48]), possess the following limitations due to its underactuation coupled with the current limited motor technology: 1) they typically cannot push side-way or resist lateral gust without tilting its body (e.g., difficult to maintain operation/manipulation contact) as their majority is equipped with a robot arm only with small-dof (e.g., two-dof [21, 22, 26, 28, 29]); 2) they typically cannot attain dynamic (i.e., fast/smooth) interaction control, as their majority is equipped not with a torque-controlled robot arm (e.g., [21, 22, 28, 29]) and such dynamic interaction may destabilize the un-actuated dynamics; and 3) their arm-drone integration and control synthesis are typically fairly complicated, since the issue of under-actuation (e.g., instability) can only be properly addressed by considering their full dynamics. This we believe is because the drones are optimized for the ease of flying (e.g., for hobbyists), but not necessarily for other applications such as aerial operation/manipulation. In this chapter, we propose a novel robotic platform for aerial operation and manipulation, namely, SmQ (spherically-connected multi-quadrotor) platform - see Fig This SmQ platform consists of a rigid frame (or tool) and multiple quadrotors (or multi-rotor drones), which are connected to the frame via passive spherical joints. Each of these quadrotors is then used as the actuator for the frame, that is, by controlling its attitude and thrust force, they can collectively produce the motion of the frame in SE(3). In other words, we use the quadrotors as distributed rotating thrust generators. Depending on the

59 Chapter 3. Spherically-connected multi-quadrotor Platform 49 Fig. 3.1: Spherically-connected multi-quadrotor (SmQ) platform for aerial manipulation, consisting of multiple quadrotors connected to a frame or tool using passive spherical joints. number of quadrotors and their configuration, this SmQ platform can fully (e.g., S3Q platform of Sec. 3.4) or partially (e.g., S2Q platform of Sec. 3.5) overcome the aforementioned issues of under-actuation of the multi-rotor drones. Adopting multiple quadrotors, this SmQ platform often possesses larger payload than a single drone. This SmQ platform may be used as a stand-alone aerial tool system with some end-effector (e.g., drill, driver, etc.) attached to it (e.g., Sec. 3.6); or as a platform for a multi-dof robotic arm for dexterous aerial manipulation, for which the platform-arm integration can be simple and even modular (e.g., combination of SmQ platform and robot arm impedance controls; or even their kinematic controls combination), as the (fully-actuated) SmQ system is kinematically-reducible [49]. Note also that this SmQ platform can be quickly constructed by using off-the-shelf/commercial multi-rotor drones, which are often shipped with well-functioning low-level attitude/thrust controls.

60 Chapter 3. Spherically-connected multi-quadrotor Platform 50 The main goal of this chapter is to provide theoretical framework for this SmQ platform system, particularly, for its modeling and control. Experimental validation is also performed. Motion planning is briefly touched as well, yet, spared for future research, since, due to its peculiarity (i.e., dependency among control inputs), it cannot be addressed in a standard manner. We first provide the dynamics modeling of this SmQ platform and establish the geometric condition for their full-actuation in SE(3). We also show how to address the issue of limited range of (commercial) spherical joints and the rotor saturations - the key technical challenge of the SmQ platform systems - as a constrained optimization problem by noticing its similarity with the well-known multi-fingered grasping problem under the friction-cone constraint [45]. We then design and analyze control laws for the (fully-actuated) S3Q system and for the (partially-actuated) S2Q system as a combination of high-level Lyapunov control design (to achieve trajectory tracking) and low-level constrained optimization (to comply with the physical limited-range/thrust constraints); and show that the S3Q system can attain the trajectory tracking in SE(3), whereas the S2Q system in R 3 S 2 with its un-actuated dynamics still internally stable in practice. We then perform the experiments for prototype S3Q and S2Q platform systems to validate these theoretical results and demonstrate their performance.

61 Chapter 3. Spherically-connected multi-quadrotor Platform 51 Comparison with Other Aerial Manipulation Systems In contrast to the drone-manipulator system (e.g., [21 29]) with some of its limitations as stated above, our SmQ platform can: 1) resist side-way gust/force while holding its attitude due to its full-actuation in SE(3); 2) attain dynamic interaction control with its full-actuation and force/torque-level actuation; and 3) integrate with a robotic arm in a modular manner (e.g., simply combine impedance controls of arm and platform; or even their kinematic-level control), again due to its full-actuation. Of course, our SmQ platform is not without its shortcomings and perhaps the most significant one would be its large formfactor due to its requiring additional quadrotors. This large form-factor may be detrimental (or even not allowable) for some applications, for which the dronemanipulator system or other aerial manipulation systems as stated below would be preferred. The issue of under-actuation has been recognized by many researchers and some of them, including us, have proposed tilted multi-rotor platforms (e.g., [50 54]), where multiple rotors are asymmetrically attached to the body to directly produce full-actuation in SE(3), with some of them capable of even such challenging behaviors as 360 o pitch-turning and large-force downward pushing (e.g., [53, 54]), all impossible with our SmQ platform system. This tilted multi-rotor platform is also generally of smaller form-factor than the SmQ platform. However, due to the inter-rotor flow interference, necessarily arising from the rotors not colinear

62 Chapter 3. Spherically-connected multi-quadrotor Platform 52 with each other, energy efficiency, and, consequently, flight time of this tilted multi-rotor platform is rather limited. Another system along this line is tilting multi-rotor platform, where the tilting angles of the rotors of a multi-rotor drone are actively controlled with extra actuators via some tilting mechanisms (e.g., [55 58]). This tilting multi-rotor platform can then overcome the issue of under-actuation as our SmQ platform does with its form-factor again typically smaller than the SmQ system. However, addition of these extra actuators and mechanisms may significantly reduce (often already fairly tight) payload/flight-time, and, further, substantially increase system complexity (and, consequently, maintenance difficulty and reliability), losing the very advantage of the multi-rotor platform (as compared to, e.g., helicopter platform with the complex swash plate mechanism). Yet another way for aerial operation/manipulation is to utilize multiple quadrotors. For this, the authors of [59, 60] rigidly attach multiple quadrotors essentially to a frame to increase loading capability of the system. However, with all the rotors again colinear, these platforms are again under the same limitations stemming from the under-actuation. On the other hand, cable-suspended systems using multiple quadrotors are presented in [61 63], which, yet, we believe, would be not so suitable for many aerial operation/manipulation applications, as they cannot control the swaying motion of the object in the presence of gusts. The rest of this chapter is organized as follows. The design and dynamical model of the system are presented in Sec The actuation capacity and the control

63 Chapter 3. Spherically-connected multi-quadrotor Platform 53 feasibility of the system are analyzed in Sec The control design and optimal control allocation for motion control of the system with three quadrotors and that with only two quadrotors are presented in Sec. 3.4 and Sec. 3.5, respectively. In Sec. 3.6 we present experimental results and discussions. Some concluding remarks are given in Sec System Design and Modeling System Description Consider the SmQ (spherically-connected multi-quadrotor) platform system with m (m = 1, 2,..., n) being the number of quadrotors attached to the frame as shown in Fig We design the joint-quadrotor connection in such a way that each spherical joint is attached at the center-of-mass of the quadrotors as close as possible. Some offset inevitably arises from this connection design, which yet appears to be effectively suppressed by the feedback control as demonstrated in Sec We then have the following relation between the center-of-mass position 1 x w i R 3 of the i-th quadrotor and that of the SmQ-frame x w o R 3, all expressed in the inertial frame {F W } s.t., x w i = x w o + R o x o i, i = 1, 2,..., n (3.1) 1 We use the superscript throughout the chapter to specify in which frame the entity is expressed if deemed beneficial to eliminate confusion (e.g., x w i and x o i in (3.1) denote the same x i expressed in {F W } and in {F O}, respectively).

64 Chapter 3. Spherically-connected multi-quadrotor Platform 54 where x o i R3 is x i expressed in the body frame {F 0 } fixed to the center-of-mass of the SmQ-frame, and R o SO(3) is the attitude of the frame expressed in {F W }. See Fig. 3.1, where we use the north-east-down convention to represent each frame. We can then assume that each quadrotor can rotate about the passive spherical joint with its rotation dynamics (w.r.t. center-of-mass) decoupled from that of the SmQ-frame, while generating the thrust force vector Λ o i := λ iro T R i e 3 R 3 and exerting the constraint force Ni w R 3 on the SmQ-frame through the attaching point x i R 3 to enforce the constraint (3.1). Here, λ i > 0 and R i SO(3) are respectively the thrust force magnitude and the attitude of the i-th quadrotor expressed in {F W }. No reaction moment is transmitted from the quadrotor to the SmQ platform via the passive spherical joint. Real spherical joints only allow for limited range of motion, typically smaller than ( 35, +35 ) in pitch and roll directions, although the yaw motion can be unconstrained. This limited motion range of the spherical joints can be modeled by the following cone constraint: (p o i ) T Λ o i Λ o i cos φ i, i = 1, 2,..., n (3.2) where p o i R3 is the unit vector along the direction of the center axis of the i-th spherical joint, φ i R, 0 < φ i < π/2, is its maximum range of angle motion, and Λ o i := (Λ o i )T Λ o i 0, which is also in general bounded, i.e., there exists λ i 0 s.t., Λ o i < λ i, i = 1, 2,..., n. See Fig 3.2. Note that this joint constraint (3.2)

65 Chapter 3. Spherically-connected multi-quadrotor Platform 55 Fig. 3.2: Range limit of spherical joint: p i R 3 is the center-axis unit vector, φ i the maximum allowable motion range, and Λ i R 3 the quadrotor thrust vector. with φ i < π/2 also implies the quadrotor thrust λ i always be positive. The design, control, and analysis of the SmQ platform system should then fully incorporate this range constraint of the spherical joints and also the limited actuation of thrust generation - see Sec. 3.3, Sec. 3.5 and Sec. 3.4.

66 Chapter 3. Spherically-connected multi-quadrotor Platform Dynamics Modeling and Reduced Dynamics With the assumption that the spherical joint is attached at the center-of-mass of each quadrotor, the Newton-Euler dynamics of the n-quadrotors and the SmQframe can be written as: m i v i + S(ω i )m i v i = m i gr T i e 3 + R T i N w i R T i R o Λ o i J i ω i + S(ω i )J i ω i = τ i n m o v o + S(ω o )m o v o = m o gro T e 3 Ro T Ni w J o ω o + S(ω o )J o ω o = i=1 n S(x o i )Ro T Ni w i=1 + τ o e + f o e (3.3) where m i > 0, J i R 3 3 and m o > 0, J o R 3 3 are the mass and inertia of the i-th quadrotor and the frame respectively, g R is the gravity acceleration, v i := Ri T ẋw i R 3, v o := Ro T ẋ w o R 3 and ω i, ω o so(3) are their body translation and angular velocities represented in their body-fixed frames {F i } and {F 0 } with the superscripts omitted for simplicity, Ni w R 3 is the constraint force to enforce the constraint (3.1), Λ o i = λ iro T R i e 3 is the thrust force vector constrained according to (3.2), τ i R 3 is the torque control input of the i-th quadrotor, S(ω) is the skew-symmetric matrix such that S(ω)ν = ω ν, ω, ν R 3, and fe o, τe o R 3 are the external force and torque acting at the center-of-mass of the SmQ-frame. By eliminating the constraint forces N w i in (3.3) of the constraint (3.1), we can

67 Chapter 3. Spherically-connected multi-quadrotor Platform 57 reduce the dynamics (3.3) into the lumped 6-DOF dynamics of the SmQ-platform in SE(3) and the 3n-DOF attitude dynamics of the n-quadrotors. First, the reduced 6-DOF SmQ-frame dynamics can be obtained s.t., M ξ + Cξ + G = U + F e (3.4) where ξ := [v c ; ω o ] R 6 with v c being the body translation velocity of the center-of-mass of the total system expressed in {F 0 } as defined by v c := R T o [ d dt 1 n i=0 m i ] n m i x w i R 3 (3.5) i=0 } {{ } =:x w c and M := mi 0, 0 J n n m := m i, J := Jo m i S(x o i )S(x o i x o c), i=0 i=1 C := S( mω o) 0 0 S( Jω, o ) G := mgrt o e 3, F e = f e o 0 τ o e

68 Chapter 3. Spherically-connected multi-quadrotor Platform 58 are the lumped (symmetric/positive-definite) inertia matrix, the (skew-symmetric) Coriolis matrix, the gravity effect and the external forcing, respectively, U := BΛ R 6 (3.6) is the control input to the SmQ-platform, where Λ := [Λ o 1 ; Λo 2 ;...; Λo n] R 3n is the collective rotating thrust vectors, and B(d) := I I... I R 6 3n (3.7) S(d 1 ) S(d 2 )... S(d n ) is the mapping matrix, which defines how the thrust actuation of each quadrotor affects the SmQ-platform dynamics depending on its mechanical structure design (i.e., design of d i ), with d i := x o i xo c R 3. On the other hand, the attitude dynamics of each quadrotor is given by: J i ω i + S(ω i )J i ω i = τ i, (3.8) which constitutes the 3n-DOF attitude dynamics of the n-quadrotors in SO n (3) in addition to the 6-DOF reduced dynamics of the SmQ-platform (3.4). Note that, due to our design of connecting the spherical joint at the center-ofmass of the quadrotors, the attitude dynamics of each quadrotor (3.8) is decoupled from the SmQ-platform dynamics (3.4). This attitude dynamics of the

69 Chapter 3. Spherically-connected multi-quadrotor Platform 59 quadrotor can be typically controlled much faster than the SmQ-platform dynamics. This then means that we can consider Λ o i = λ i Ro T R i e 3 R 3 as the control input for (3.4) and the n-quadrotors as rotating thrust actuators for the 6-DOF SmQ-platform dynamics (3.4) with the fast-enough low-level attitude control embedded for each quadrotor (see Sec. 3.6). We can also see that the structure of B(d) R 6 3n in (3.4) dictates whether we can generate an arbitrary control action U R 6 by using the thrust inputs Λ o i of the n-quadrotors. This structure of B(d) depends on the the number of quadrotors n and the arrangement of the attaching points x o i (see Fig. 3.1). In the next Sec. 3.3, we analyze how this structure design of the SmQ-platform is related to its actuation capacity and also when a given task is guaranteed to be feasible for the SmQ-platform system under the range constraint of the spherical joint (3.2) and the thrust actuation bound λ i. 3.3 Control Generation and Feasibility Analysis For the control of the 6-DOF SmQ-platform dynamics (3.4), it is desirable to generate any arbitrary control input U R 6 by recruiting the thrust actuation Λ i of the n-quadrotors. This, yet, is not in general possible, since the thrust actuation of each quadrotor Λ i R 3 is constrained by the the spherical joint rotation range (3.2) and the thrust generation bound λ i. This control generation may also be limited depending on the SmQ-platform design, i.e., the structure of

70 Chapter 3. Spherically-connected multi-quadrotor Platform 60 the mapping matrix B(d) in (3.7). At the same time, we would like to minimize any internally-dissipated thrust actuation as much as possible. This problem, that is, how to optimally realize a desired control wrench U R 6 for the SmQ-platform by using n-quadrotors as rotating thrust generators under the constraints can be formulated as the following constrained optimization problem: Λ op = subject to arg min Λ=(Λ i,λ 2,...,Λ n) 1 2 ΛT Λ (3.9) B(d)Λ = U (3.10) Λ T i [cos 2 φ i I 3 3 p i p T i ]Λ i 0 (3.11) Λ T i Λ i λ 2 i 0 (3.12) where (3.9) is to maximize energy efficiency by eliminating internally-dissipated thrust generations (or to minimize internal forces producing no net platform motion), (3.10) is to generate the desired control wrench U R 6, (3.11) is the spherical joint motion range constraint written in a matrix form, and (3.12) is to reflect the boundedness of the thrust actuation Λ i. The desired platform control U R 6 will be designed based on Lyapunov design in Sec and Sec , which is then optimally decoded into each rotor thrust Λ i while respecting the constraints (3.11)-(3.12). This means that our (closed-loop) control strategy is hierarchical, consisting of high-level Lyapunov control design and low-level constrained optimization. See Sec. 3.4 (for S3Q system) and Sec. 3.5 (for S2Q

71 Chapter 3. Spherically-connected multi-quadrotor Platform 61 system). The constrained optimization (3.9)-(3.12) is a second-order cone programming (SOCP), which is a convex optimization problem [64]. Further, feasibility and continuity of the desired control U R 6 for (3.10) is to be ensured via the task planning as stated in Sec With its convexity and the continuity of its constraints and objective function, the solution continuity of this constrained optimization (3.9)-(3.12) follows [65]. This constrained optimization (3.9)-(3.12) also has similarity with the multi-finger grasping problem under friction-cone constraint [45], which we will exploit in the sequel to solve this (3.9)-(3.12) and also to analyze some salient SmQ-platform behaviors. For the SmQ-platform, this constrained optimization will be real-time solved during the operation given the desired platform control U R 6, for which dimension reduction turns out to be instrumental (see Sec and Sec ). Design of U and its real-time optimal allocation to Λ i for the specific S3Q-platform and S2Q-platform are the topics of Sec. 3.5 and Sec Instead, in this Sec. 3.3, analyzing (3.10)-(3.12), we present some conditions on the mechanical structure design for the full-actuation of the SmQ-platform in SE(3) and further propose a framework to design the task (i.e., U) so that its feasibility under the constraints (3.10)-(3.12) is guaranteed.

72 Chapter 3. Spherically-connected multi-quadrotor Platform Necessary Rank Condition for Full-Actuation in SE(3) Now, let us focus only on the first condition (3.10) of the constrained optimization problem. Then, since U R 6 can in general be an arbitrary control command, the existence of the solution, Λ R 3n, depends on the rank of the matrix B(d) R 6 3n. As can be seen from (3.7), B(d) is a function of only mechanical design parameters d i (i.e., the attaching point of quadrotors on the SmQ-frame). In other words, this d i affects the structure of B, which in turn decides the existence of solution for (3.10). This suggests us to choose these mechanical design parameters, d i, i = 1, 2,..., n, to ensure that: rank(b) = 6 (3.13) Note that this rank condition (3.13) is necessary for the 6-DOF SmQ-platform (3.4) to be fully-actuated in SE(3). The next Prop. 1 shows that, for the 6-DOF full-actuation of the SmQ-platform, the number of deployed quadrotors should be at least three and the configuration of their attaching points (i.e., d i R 3 ) should avoid certain singular configurations. Proposition 1. Consider the SmQ-platform (3.4) consisting of n-quadrotors connected via spherical joints. Then, the necessary rank condition (3.13) for its full-actuation in SE(3) is granted, if and only if there are at least three quadrotors with their attaching points d i (expressed in {F 0 }) not collinear with each other,

73 Chapter 3. Spherically-connected multi-quadrotor Platform 63 i.e., (d 2 d 1 ) (d 3 d 1 ) 0 (3.14) Proof. First, we consider the case where there are three quadrotors attached to the tool/frame, whose attaching points d i are not collinear as stated above. Then, the control wrench generation for the SmQ-platform by these three quadrotors is given by BΛ = I I I S(d 1 ) S(d 2 ) S(d 3 ) Λ 1 Λ 2 Λ 3 where we can decompose B s.t., B = I 0 I 0 0 I I I 0 I 0 S(d 1 ) I 0 S(d 2 d 1 ) S(d 3 d 1 ) }{{}}{{} 0 0 I :=L :=Σ }{{} :=D by using the linearity of the operator S( ). We can then see that rank(b) = rank(σ), since L and D are both full-rank. We now show that rank(σ) = 6 if and only if the non-collinear condition (3.14) is ensured with the three quadrotors, i.e., for the S3Q-platform. For this, recall that rank(s(ν)) = 2 ν 0. Thus, we

74 Chapter 3. Spherically-connected multi-quadrotor Platform 64 Fig. 3.3: S2Q-platform with the two quadrotors and the tool, where we assume: 1) d 1, d 2, d e are within the plane spanned by the N o and E o axes of {F O }; 2) d 2 d 1 and d e d 1 are all parallel to the N o -axis with (3.17); and 3) the spherical joint center axes p i are all along the D o -axis of {F O }. have [ ] rank(σ) = 3 + rank( S(d 2 d 1 ) S(d 3 d 1 ) ) Suppose that the non-collinear condition (3.14) is satisfied, yet, rank(σ) < 6. Then, there should exists y R 3 s.t., y T [S(d 2 d 1 ) S(d 3 d 1 )] = 0. This then implies y = k 1 (d 2 d 1 ) = k 2 (d 3 d 1 ) for some k 1, k 2 R, which yet results in (d 2 d 1 ) (d 3 d 1 ) = 0, contradicting the non-collinear assumption (3.14). Note that addition of more quadrotors to the S3Q-platform with the non-collinear condition (3.14) will still ensure the necessary rank condition (3.13). Consider also the case that there are only two quadrotors, i.e., the S2Q-platform or all the quadrotors in the system are attached collinear with each other. We can then easily show that rank(σ) 5, thus, the platform is underactuated. This completes the proof. The Prop. 1 characterizes singular designs of the SmQ-platform, that should

75 Chapter 3. Spherically-connected multi-quadrotor Platform 65 be avoided if the full-actuation in SE(3) is necessary. It is however worthwhile to mention that, even if a SmQ-platform is not fully-actuated in SE(3) (i.e., the condition (3.13) not satisfied), it may be able to provide limited, yet, still useful motion capability. For instance, consider the S2Q-platform system as shown in Fig. 3.3 with B(d) := I I R 6 6 S(d 1 ) S(d 2 ) We can then verify that: [d T2 ST (d 1 ) (d 2 d 1 ) T ] BΛ = 0, Λ R 6 (3.15) that is, if d 1 d 2, rank[b] = 5 and null[b T ] S(d 1)d 2 = d 2 d 1 I S(d 1) 0 I 0 (3.16) d 2 d 1 This null motion is in fact a pure rotation of the S2Q-platform about the (d 2 d 1 )- axis, with (0; d 2 d 1 ) and (S(d 1 )d 2 ; d 2 d 1 ) in (3.16) respectively being the body velocity of the (N,E,D)-coordinate frames attached at x 1 and x c with the same attitude, and the mapping between them an adjoint operator [45]. For the S2Qsystem in Fig. 3.3 with x 1, x 2, x e all on the same line with (d e d 1 ) (d 2 d 1 ) = 0 (3.17)

76 Chapter 3. Spherically-connected multi-quadrotor Platform 66 this then implies that the system can only generate 5-DOF actuation with the moment about the N 0 -axis (i.e., x o 2 xo 1 ) of the tool-tip frame {F E} not generatable/resistable. Even so, by designing the tool-tip located at d e R 3 in Fig. 3.3, we can still fully control the tool-tip translation in R 3 as well as its pointing direction in S 2. The roll rotation of the tool-tip is not controllable, which, yet, may not be so detrimental for such applications as pushing/pulling task, button operation, assembly of parts symmetric about the N 0 -axis, contact probe operation, etc. Of course, if the full-actuation in SE(3) is necessary, we may use a SmQ-platform with m 3 and designed with the non-collinear condition (3.14) ensured Control Feasibility Now, suppose that we have a SmQ-platform satisfying Prop. 1. This then ensures that there exists thrust vectors Λ i R 3, i = 1, 2,..., n for (3.10) to produce any desired platform control U R 6. This, however, is true only if Λ i is not constrained, and what is unclear is whether these solution thrust vectors Λ i would also respect the constraints (3.11) and (3.12). We say the control wrench U R 6 is feasible if there exists a solution Λ = (Λ 1, Λ 2,..., Λ n ) R 3n, which also complies with the constraints (3.11) and (3.12). In this Sec , we analyze this control feasibility of the SmQ-platforms and also show how to design a task for them while guaranteeing this control feasibility.

77 Chapter 3. Spherically-connected multi-quadrotor Platform 67 For this, we would first like to recognize that this control feasibility of the SmQplatform is analogous to the force-closure of the multi-fingered grasping under the friction cone constraint. More precisely, notice from Fig. 3.2 that each thrust vector Λ i of the SmQ-platform is confined within the cone defined by the limited motion range of their spherical joint. The control feasibility problem then boils down to the question whether a given desired control wrench U R 6 can be generated by the thrust vectors Λ i each residing in their respective cone. This is exactly the same question of the force closure [45, 66], i.e., whether it is possible to resist an external wrench by using the contact force of multiple fingers, with its normal and shear forces constrained to adhere Coulomb friction model (i.e., cone with the angle specified by the friction coefficient µ). The following Prop. 2 is due to this analogy between the control feasibility and the force closure. Proposition 2. For the SmQ-platforms satisfying Prop. 1, we can generate any control wrench U R 6 in (3.10) for the platform using the thrust vectors Λ i R 3, i = 1, 2,..., n, while respecting the constraints (3.11)-(3.12), if and only if the desired wrench U d is in force closure with Λ i R 3 being the contact forces residing inside their friction cone defined by the constraints (3.11)-(3.12). To ensure the control feasibility of the SmQ-platform, we may take one of the following two approaches. The first is to real-time solve the desired control wrench U R 6 and the collective thrust vectors Λ i, i = 1, 2,...n simultaneously to drive the SmQ-platform (3.4) according to a certain task objective while also taking into account all the constraints (3.10)-(3.12) in a manner similar to the technique of MPC (model predictive control). This approach, yet, typically results

78 Chapter 3. Spherically-connected multi-quadrotor Platform 68 (a) Feasible set U of the S2Q-platform in Fig (b) Feasible set U of the S3Q-platform in Fig Fig. 3.4: (Top) Feasible force sets U f and (bottom) feasible moment sets U τ of the S2Q-platform and S3Q-platform systems. The blue dash lines represent the required control wrenches for the infeasible (aggressive) motion tracking tasks. The red solid lines are the control wrenches implemented in Sec. 3.6 with the performance presented in Fig and Fig. 3.7, respectively. The star dots are the gravity wrench G at the hovering posture, which is also the required control wrench U d for stabilization control in Sec in a computationally-complex algorithm, the real-time running of which is often challenging in practice. Instead, in this chapter, we adopt the other task-design approach, where we first construct the (quasi-static) feasible control set U se(3) (expressed in the body-frame {F 0 }) under the constraints (3.11)-(3.12): U := {U = BΛ p T i Λ i Λ i cos φ i, Λ i λ i }

79 Chapter 3. Spherically-connected multi-quadrotor Platform 69 and design the task in such a way that its required control U is always within this U, thereby, guaranteeing the control feasibility. This task-design approach also allows for an hierarchical control architecture, where U R 6 for (3.4) (via (3.10)) is computed using, e.g., Lyapunov-based control design for the task, which is designed a prior for control feasibility, while the constrained optimization (3.9)- (3.12) is real-time solved given the control U with the solution existence always guaranteed. See Sec and Sec The control feasible sets U of the S2Q-platform (Fig. 3.3) and S3Q-platform (Fig. 3.1) are shown in Fig. 3.4, where the feasible control force set U f and the feasible control moment set U τ are separately presented. For this, we use the parameters of the S2Q-platform and S3Q-platform prototypes in Sec The star dots represent the gravity wrench G = [g f ; g τ ] R 6 in (3.4) expressed in {F 0 } at the hovering posture (i.e., R o = I), which the control wrench U d should always compensate for to maintain flying. Note from Fig. 3.4a that, due to its under-actuation (with the condition (3.13) not granted - see Prop. 1), U τ of the S2Q-platform is given by surface instead of volume. For a given task, we can then examine its feasibility by drawing its required control U d and seeing if it is within this set U or not before performing the task. Here, note that the feasible set U is convex with the hovering wrench G strictly within its interior. This then means that, if the desired motion and contact wrench are small enough and also so are the uncertainty and initial error, there always exists control input U within U in the neighborhood of G (i.e., solution

80 Chapter 3. Spherically-connected multi-quadrotor Platform 70 existence guaranteed). To better show this, we draw the control inputs of two time-scaled trajectories in Fig. 3.4 for the S2Q and S3Q systems, respectively. The slower (i.e., smaller/red orbits in Fig. 3.4) is in fact that used during the experiments in Sec and Sec and we can see that it is feasible satisfying the constraints (3.11)-(3.12); whereas the faster (i.e., larger/blue orbits in Fig. 3.4) is not feasible since some of its trajectory are outside of U. If we further slow down the trajectory, the control trajectory will further shrink and eventually converge to the point of G. This task-design approach to ensure the control feasibility is rather conservative. How to more aggressively transverse the feasible set U by adopting some dynamics-based planning for the SmQ system is a topic for future work, for which standard motion planning techniques are rather limited, since the control force/moment input sets, U f and U τ, of the SmQ systems are not independent from each other as typically assumed in the motion planning literatures. 3.4 Control of S3Q-Platform System In this Sec. 3.4, we consider the tracking control problem of the S3Q-platform system of Fig With the rank condition of Prop. 1 and the control feasibility of Prop. 2 satisfied, the S3Q-platform system becomes fully-actuated with the solution of the control allocation (3.9)-(3.12) always guaranteed to exist, thereby, rendering the tracking problem in R 3 SO(3) rather straightforward. This then means that we can freely control the position and orientation of a

81 Chapter 3. Spherically-connected multi-quadrotor Platform 71 mechanical tool (e.g., drill, driver, etc.) rigidly attached to the S3Q-platform for performing aerial operation tasks. Similar tracking control is also addressed for the S2Q-platform system in Sec. 3.5, where its under-actuation (see Sec ) substantially complicates the control design and analysis. Let us denote the tool-tip position by x e R 3 and the tool attitude by R o SO(3). We then have the following relation between the tool pose (x w e, R o ) SE(3) and the platform pose (x w c, R o ): x w e = x w c + R o d e, ẋ w e = R o v c + R o S(ω o )d e (3.18) where x w e, x w c R 3 are x e, x c expressed in {F W }, d e R 3 is the vector from the system center-of-mass x c to x e expressed in {F O } (i.e., d e = x o e x o c), and v c is defined in (3.5). The control objective is then to design U R 6 in (3.4) s.t. (x w e (t), R o (t)) (x w d (t), R d(t)) while satisfying the constraints (3.10)-(3.12), where (x w d (t), R d(t)) SE(3) is the timed-trajectory of the desired tool pose (specified by the coordinate frame {F D } and expressed in {F W }) and is assumed to be planned off-line to ensure its feasibility as explained in Sec

82 Chapter 3. Spherically-connected multi-quadrotor Platform Lyapunov-Based Control Design To design U R 6 in (3.4) to achieve the tracking as stated above, we define a smooth non-negative potential function on SE(3) s.t., Φ := 1 2 k xe T x e x + k r Ψ R where k x, k r R are positive gains, and e x = x w e x w d, Ψ R = 1 2 tr(η ηrt d R o) with η = diag[η 1, η 2, η 3 ] R 3 3 and η 1, η 2, η 3 being distinct positive constants. This potential Φ has following properties: 1) Φ 0 with the equality hold iff (x w e, R o ) = (x w d, R d); 2) its derivative is given by Φ = Φ (ξ ξ d ) where Φ R 1 6 is the one-form of Φ defined s.t., Φ T = I 0 k x Ro T e x R 6 S(d e ) I k r (ηrd T R o Ro T R d η) ξ d : = S(d e)ro T R d ω d + Ro T R d v d, R 6 Ro T R d ω d

83 Chapter 3. Spherically-connected multi-quadrotor Platform 73 where v d, ω d R 3 are the desired velocities expressed in {F D } as defined by: ẋ w d = R dv d, w d = (R T d Ṙd) (3.19) and : R 3 3 so(3) is the inverse operator of S( ). We then design the control U R 6 in (3.4) s.t. U = M ξ d + Cξ d k v e ξ Φ T + G F e (3.20) where e ξ := ξ ξ d with the damping gain k v > 0. The closed-loop dynamics (3.4) of the S3Q system with (3.20) is then given by Mė ξ + Ce ξ + k v e ξ + Φ T = 0 (3.21) using which we can show that the SE(3) pose of the S3Q system will converge to the desired one almost everywhere as formalized in the following Th. 5. Here, the convergence is almost everywhere, i.e., except some unstable equilibriums, which in fact stems from the inherent topology of SO(3), yet, can rather easily be avoided in practice by running the operations close enough to the desired pose trajectory - see [67, 68]. Theorem 5. Consider the dynamics of S3Q-platform (3.4) with the control U as designed in (3.20), which is assumed to satisfy the control feasibility condition

84 Chapter 3. Spherically-connected multi-quadrotor Platform 74 of Prop. 2. Then, ξ ξ d and (x w e, R o ) (x w d, R d) asymptotically almost everywhere except unstable equilibriums E := {(x w e, R o ) x w e = x w d, RT d R o {diag[ 1, 1, 1], diag[ 1, 1, 1], diag[1, 1, 1]}}. Proof. Define the Lyapunov function as follows: V := Φ et ξ Me ξ which is a positive-definite function on SE(3) se(3) with V = 0 iff (x w e, R o, ξ) = (x w d, R d, ξ d ). The derivative of V along the closed-loop dynamics (3.21) is then given by V = Φe ξ + e T ξ ( Ce ξ k v e ξ Φ T ) = k v e T ξ e ξ 0 where C is skew-symmetric from the passivity of the system (3.4), i.e., Ṁ 2C is symmetric with this, we can further obtain: Ṁ = 0 (see after (3.5) for the definition of C). Integrating T V (T ) V (0) = k v e ξ 2 dt T 0. This then implies e ξ L L 2. We also have Φ(t) V (t) V (0), t 0, thus Φ T L as well. From (3.21) with the assumption that v o d, ωo d and o

85 Chapter 3. Spherically-connected multi-quadrotor Platform 75 their derivatives be bounded, we then have ė ξ L. Using Barbalat s lemma [41], we can then conclude that e ξ 0. Differentiating (3.21), we can also show that ë ξ L and, applying Barbalat s lemma again, we have ė ξ 0. Applying (e ξ, ė ξ ) 0 to (3.21), we have Φ T = 0. This then means that e x = 0 and, further, (ηrd T R o Ro T R d η) = 0 which, following [67, 68], suggests the almost global asymptotic stability of R o R d as stated above. This completes the proof Optimal Control Allocation To decode the designed control U in (3.20) into the thrust vector of each quadrotor, we solve the constrained optimization (3.9)-(3.12), which is a convex SOCP with affine equality constraint (3.10), convex inequality constraints (3.11)-(3.12) and convex object function (3.9). As explained in Sec , this problem is in fact similar to the robot grasping problem under the friction-cone constraint. To solve this optimization problem (3.9)-(3.12) in real-time, here, we employ the barrier method [64] to convert the problem to an unconstrained optimization problem. For this, we exploit the peculiar structure of (3.9)-(3.12) to reduce the size of the problem, thereby, significantly speeding up the computation time.

86 Chapter 3. Spherically-connected multi-quadrotor Platform 76 More specifically, we write the general solution of (3.10) s.t., Λ := B T [BB T ] 1 U + Zγ where B T [BB T ] 1 R 9 6 is the Moore-Penrose pseudo-inverse of B(d) in (3.10), Z null[b(d)] R 9 3 (i.e., identifies the null-space of B(d)), and γ R 3 is associated with the internal force term. Utilizing the structure of B(d) in (3.7), we can write this general solution of (3.10) for each thrust vector Λ i s.t., Λ i (γ) = Λ i + Z iγ (3.22) where Λ i := [I S(d i)](bb T ) 1 U R 3 and Z =: [Z 1 ; Z 2 ; Z 3 ] with Z i R 3 3. Here, note that only the unknown is γ R 3. We can then reduce the size of the original optimization problem (3.9)- (3.12) by writing that with this γ R 3 as the new optimization variable s.t., subject to 1 γ op = arg min γ R Λ T i (γ) Λ i (γ) (3.23) i=1 C 1,i := Λ T i (γ)(cos 2 φ i I p i p T i ) Λ i (γ) 0 (3.24) C 2,i := Λ T i (γ) Λ i (γ) λ 2 i 0 (3.25)

87 Chapter 3. Spherically-connected multi-quadrotor Platform 77 Following the procedure of the barrier method [64], we then convert (3.23)-(3.25) into the following unconstrained optimization: γ op = arg min γ R 3 3 [sπ i (γ) + Ξ i (γ)] (3.26) i=1 where Π i (γ) := 1 2 Λ T i (γ) Λ i (γ) is from (3.23) and Ξ i (γ) := log( C 1,i (γ)) log( C 1,i (γ)) is a logarithm barrier function to enforce the constraints (3.24)- (3.25), and s R balances between the cost minimization and the constraint enforcement. We then apply the standard way to solve this barrier method, i.e., solve a sequence of unconstrained problem (3.26) by using the Newton iteration for increasing value of s until s > 2n ɛ, where n is the number of quadrotors and ɛ > 0 is the desired tolerance. The solution of (3.26) is known to be the 2n s -suboptimal solution of (3.23)-(3.25) with n = 3 for the S3Q system [64]. We implement and run this optimization-solving algorithm in real-time for our S3Q-platform system as experimented in Sec From this size reduction, we can obtain the closed-form solution of the inverse of a certain Hessian matrix H, which is important for the Newton iteration and, in this case, it is positive definite and also of only the dimension of 3 3. This greatly helps to speed up our solving the optimization problem (3.26). It is also well-known that the convergence of Newton iteration is quadratic and guaranteed within the bounded number of iterations [64]. For our experiment in Sec. 3.6, we observe that this Newton iteration only requires at most 20 iterations for each s, and, including the s-scaling procedure and computing feasible starting point [64], the total control

88 Chapter 3. Spherically-connected multi-quadrotor Platform 78 allocation algorithm can run with around 500Hz (on a PC with Intel i7 3.2 GHz CPU), which is proved to be adequate for the experiment. See Sec Control of S2Q-Platform System In this Sec. 3.5, we consider motion control problem of S2Q-platform system as shown in Fig. 3.3 with (3.17). As elucidated in Sec , this S2Q-platform system is (frame) under-actuated with only 5-DOF actuation possible for its frame motion in SE(3) (see (3.15)). Even so, we can still achieve some limited, yet, useful behavior by using this S2Q-platform system, e.g., controlling the tip position and the pointing direction of a tool rigidly attached to the S2Q-system as shown in Fig The tracking control of this tool-tip position and direction in R 3 S 2 of the S2Q-platform in Fig. 3.3 is the topic of this Sec As compared to the case of the S3Q-platform tracking control in SE(3) of Sec. 3.4, this tracking control of the S2Q-platform in R 3 S 2 is more complicated due to the underactuation. For this, similar to (3.18), we denote the tool-tip position by x e. We also use the line connecting the center-of-mass positions of two quadrotors as the pointing direction of the tool - see Fig We denote this line by r e S 2, which can be expressed in {F W } and in {F 0 } by r w e = R o r o e, r o e = (d 2 d 1 )/ d 2 d 1 (3.27)

89 Chapter 3. Spherically-connected multi-quadrotor Platform 79 where r o e is a unit constant vector. The tracking control objective for the underactuated S2Q-platform can then be written by (x w e (t), re w (t)) (x w d (t), rw d (t)) (3.28) where (x w d (t), rw d (t)) R3 S 2 are the timed-trajectory of the desired tool-tip position and its pointing direction expressed in {F W }. Here, we construct r w d (t) S 2 by defining R d (t) SO(3) s.t., r w d (t) := R d(t)r o e (3.29) Note that, given r w d and ro e, this R d (t) is not unique. The time-derivative of these r w e and r w d are then computed by: ṙ w e = R o S(ω o )r o e, ṙ w d = R ds(w d )r o e (3.30) where w d so(3) is the angular rate of the coordinate frame {F D } specified by R d SO(3) and expressed in {F D } as defined in (3.19). Recall that ω o so(3) is also the body angular velocity of {F O }. Our goal here is then to design U R 6 in (3.4) to achieve this tracking objective in R 3 S 2 under the under-actuation limitation (3.15). The physical constraints (3.10)-(3.12) are assumed to be satisfied by designing (x w d (t), rw d (t)) in such a way to enforce control feasibility of Prop. 2. Due to the under-actuation, the S2Q-platform dynamics can split into: 1) fully-actuated 5-DOF dynamics, which

90 Chapter 3. Spherically-connected multi-quadrotor Platform 80 will be controlled to attain the tracking objective of (3.28); and 2) un-actuated 1-DOF dynamics, which constitutes internal dynamics (with (x w e (t), re w (t)) as 5- DOF output) and whose stability must be established for the tracking control to have any meaning. To facilitate the analysis of these fully-actuated and unactuated dynamics of the S2Q-platform, here, we utilize passive decomposition [42] as detailed below Passive Decomposition of S2Q-Platform Dynamics Recall from (3.7) that, for the S2Q-platform in Fig. 3.3, we have: B(d) := I I R 6 6 S(d 1 ) S(d 2 ) with rank[b] = 5 as shown in (3.15). Following [42], at each configuration q = (x w c, R o ) of the S2Q-platform, we can then decompose the tangent (or velocity) space T q M R 6 and the cotangent (or wrench) space T q M R 6 s.t., T q M = a u, T q M = Ω a Ω u or, in coordinates, v ] c = [ a u ν [ ] a, U = Ω T a Ω T u a (3.31) u ω o }{{} ν u }{{} u u =: R 6 6 =:Ω T R 6 6

91 Chapter 3. Spherically-connected multi-quadrotor Platform 81 where: 1) Ω T a span[b] R 6 5 denotes the fully-actuated direction; 2) u null[b T ] R 6 1 the velocity space of the un-actuated dynamics; 3) a = M 1 Ω T a (Ω a M 1 Ω T a ) 1 R 6 5 the orthogonal complement of u w.r.t. the M- metric (i.e., T a M u = 0); and 4) Ω T u = M u ( T u M u ) 1 R 6 1 the control space of the un-actuated dynamics with the M-orthogonality. Here, ν a, u a R 5 and ν u, u u R are respectively the velocity components and transformed control of the S2Q system in the fully-actuated and un-actuated spaces, respectively, with u u = 0 from U span[b]. Note also that the following properties hold for (3.31): 1) T a M u = (orthogonal w.r.t the M-metric); 2) Ω = I 6 6 ; and 3) T a U = u a and T u U = 0. The coordinate expression of this decomposition is not unique, and, for that, here, following (3.16), we choose u = S(d 1)d 2 null[b T ] (3.32) d 2 d 1 which, as explained after (3.16), represents pure rotation motion of the S2Qplatform of Fig. 3.3 along the tool-tip axis. Then, differentiating (3.31) with the above u and substituting them into (3.4), we obtain: M a ν a + C a ν a + C au ν u + g a = u a + f a (3.33) M u ν u + C u ν u + C ua ν a + g u = u u + f u (3.34)

92 Chapter 3. Spherically-connected multi-quadrotor Platform 82 where M a := T a M a, M u := T u M u, C a C ua C au C u := T a C a T u C a T a C u T u C u with C u = 0, and (g a, g u ) and (f a, f u ) are the transformed gravity G and external wrench F e in (3.4) computed similarly to (u a, u u ) in (3.31) with u u = 0 (i.e., ν u - dynamics (3.16) is un-actuated). Note that the passive decomposition decomposes the S2Q-platform dynamics (3.4) into the fully-actuated ν a -dynamics on a with u a R 5 and the unactuated ν u -dynamics on u with u u = 0. This is done while preserving the Lagrangian structure and passivity of the original dynamics (3.4), i.e., constant symmetric and positive-definite M a R 5 5 with skew-symmetric C a ; constant M u > 0 with C u = 0; and C au + Cua T = 0. For more details on passive decomposition, refer to [42] and references therein. These decomposed dynamics (3.33)-(3.34) then serve the basis for our control design and internal stability analysis as below Lyapunov-Based Control Design and Internal Stability Here, we design the control U R 6 in (3.4) or, equivalently, u a R 5 in (3.33), for the S2Q-platform as shown in Fig. 3.3 with (3.17) to achieve the trajectory tracking in R 3 S 2 as stated in (3.28). First, similar to the case of S3Q-platform

93 Chapter 3. Spherically-connected multi-quadrotor Platform 83 control in Sec , we define a potential function on R 3 S 2 s.t., Φ = 1 2 k xe T [ x e x + k r 1 (r w d ) T re w ] where e x := x w e x w d R3 expressed in {F W }, re w, rd w S2 are the real and desired pointing direction of the tool as defined in (3.27) and (3.29), and k x, k r > 0 are the gains. Then, we can see that Φ 0 with the equality hold iff (x w e, re w ) = (x w d, rw d ). We can then compute the time-derivative of Φ s.t., Φ = k x e T x (ẋ w e ẋ w d ) k r(r o e) T d dt (RT d R o)r o e = k x e T x [R o (v c S(d e )ω o ) ẋ w d ] k r(r o e) T R T d R os(e ω )r o e = k x e T x R o [ vc S(d e )e ω S(d e )R T o R d ω d R T o R d v d ] + k r (r o e) T R T d R os(r o e)e ω where we use (3.18), (3.19), d dt (RT d R o) = R T d R os(ω o R T o R d ω d ) and e ω := ω o R T o R d ω d, which is the angular rate error expressed in {F O }. Rearranging this, we can further write: Φ = k xr T o e x k r R T o r w d I S(d e) (ξ ξ d ) (3.35) 0 } S(re) o {{} =: S R6 6 T

94 Chapter 3. Spherically-connected multi-quadrotor Platform 84 where ξ = [v c ; ω o ] and ξ d := S(d e)ro T R d ω d + Ro T R d v d R 6 Ro T R d ω d i.e., the desired translation and rotation velocities of the total system center-ofmass expressed in {F O }. This derivative of the potential Φ depends only on the evolution of the S2Q-platform dynamics in a (3.33), as can be seen by: S u = S(d 1)d 2 S(d e )(d 2 d 1 ) = 0 S(re)(d o 2 d 1 ) where we use d e := k(d 2 d 1 ) + d 1 from (3.17) for some constant k R. Define ν ad R 5 and ν ud R s.t., ] ξ d =: [ a u ν a d ν ud with which we can write Φ = Φ (ν a ν ad ) (3.36) where Φ := k xr T o e x k r R T o r w d T S a R 1 5

95 Chapter 3. Spherically-connected multi-quadrotor Platform 85 We then design the control u a R 5 in (3.33) s.t., u a = C au ν u + g a f a (3.37) + M a ν ad + C a ν ad k v (ν a ν ad ) Φ T with which the closed-loop ν a -dynamics (3.33) becomes: M a ė a + C a e a + k ν e a + Φ T = 0 (3.38) with e a := ν a ν ad and k v > 0. Theorem 6. Consider the dynamics (3.4) of the S2Q-platform system of Fig. 3.3 with the geometric condition (3.17) and the tracking control (3.37), which is assumed to satisfy the control feasibility of Prop. 2. Define V := Φ et a M a e a (3.39) and suppose that V (0) < 2k r. Then, ν a ν ad, (x w e (t), r w e (t)) (x w d (t), rw d (t)) Proof. Here, we use V in (3.39) as a Lyapunov function, which is positive-definite with V = 0 iff (x w e, r w e, ν a ) = (x w d, rw d, ν a d ). Differentiating V along (3.38), we

96 Chapter 3. Spherically-connected multi-quadrotor Platform 86 obtain: V = Φ e a + e T a [ C a e a k v e a Φ T ] = k v e T a e a 0 where C a is skew-symmetric from the (preserved) passivity through the passive decomposition (see Sec ). Integrating this, we can then attain T V (T ) V (0) = k v e a 2 dt 0 (3.40) T 0. This implies that e a L L 2. Also, since Φ(t) V (t) V (0), t 0, Φ L. From (3.38) with v d, ω d and their derivatives being bounded, this then means that ė a L, implying e a 0 due to Barbalat s lemma [41]. Differentiating (3.38), we also can show that ë a L, implying ė a 0 again due to Barbalat s lemma. Applying (e a, ė a ) 0 to (3.38), we then have Φ T 0. Since a R 5 5 is non-singular, Φ T 0 then means that: S T k xro T e x = I 0 k xro T e x 0 k r Ro T rd w S(d e ) S(re) o k r Ro T rd w o where we have e x 0 from the first row, whereas, from the second row, S(re)R o o T rd w = Ro T S(re w )rd w 0, implying that rw e rd w or rw e rd w. Here, rw e rd w is impossible, since, from (3.40) with V (0) < 2k r as assumed above, we have: t 0, k r [1 (r w d (t))t r w e (t)] V (t) V (0) < 2k r

97 Chapter 3. Spherically-connected multi-quadrotor Platform 87 where k r [1 (r w d (t))t r w e (t)] attains its maximum 2k r only when r w e r w d, implying that only r w e r w d is possible. The condition of V (0) < 2k r of Th. 6 is to ensure that the initial error e a (0), e x (0) is small enough with re w (0) far enough from rd w (0) so that the attitude of the S2Q system always converges to the desired equilibrium (i.e., re w rd w ). This condition can be easily granted by initiating the tracking operation from, e.g., the steady-state hovering while designing x d to start close enough from x e (0). Even though this Th. 6 proves that the tracking objective (3.28) in R 3 S 2 is attained with the control (3.37), it only provides a partial verdict for its practical applicability, as the un-actuated dynamics (3.34), which in fact constitutes the internal dynamics with the 5-DOF output (x e, r e ), may become unstable under the control (3.37). This internal dynamics is given by the un-actuated ν u -dynamics (3.34), which we rewrite here in a more compact form: M u ν u + C ua ν a + g u = f u (3.41) where g u = T u G = [ mgr T o e 3 ] T S(d1 )d 2 from (3.31) with m := 2 i=0 m i, which is the moment generated by the gravity about the N E -axis of the tool-tip frame {F E }. Further, as captured by (3.16) and

98 Chapter 3. Spherically-connected multi-quadrotor Platform 88 explained there, the motion of this ν u -dynamics, i.e., u ν u is the pure rotation motion about the same N E -axis with its magnitude given by d 2 d 1 ν u. To facilitate the stability analysis of this internal dynamics (3.41), we parametrize the rotation matrix R o by the yaw, pitch, roll angles [φ; θ; ψ] R 3 s.t., c φ c θ s φ c ψ + c φ s θ s ψ s φ s ψ + c φ s θ c ψ R o = s φ c θ c φ c ψ + s φ s θ s ψ c φ s ψ + s φ s θ c ψ s θ c θ s ψ c θ c ψ with the following differential kinematics: φ 0 s ψ c ψ θ = 1 0 c θ c ψ c θ s ψ c θ ω o (3.42) ψ c θ s θ s ψ s θ c ψ and r w e = R o e 1 = [ ] T c φ c θ s φ c θ s θ where θ < π/2 and s = sin, c = cos. Note here that r w e only (φ, θ). is a function of Following the explanation above on u ν u with (3.16), we have ω o = [ u ν u ; 0; 0]+ Aν a, and, injecting this into (3.42), we can write: ψ = d 2 d 1 ν u + Aν a

99 Chapter 3. Spherically-connected multi-quadrotor Platform 89 where A R 6 5 is a function of the configuration (x e, R o ). Incorporating this ψ into (3.41) while also writing g u with [φ, θ, ψ], we can then write the internal dynamics (3.41) s.t., m u ψ + mg d1 d 2 c θ s φ (3.43) = m u [ A(ν a, ν u )ν a + A ν a ] C ua ν a + f u where m u := M u / d 2 d 1 > 0 and A(ξ) and C ua (ξ) are linear in ν a and ν u. We can then see that the internal dynamics (3.43) is indeed similar to the downward pendulum dynamics, suggesting that, if the perturbation (i.e., ν a, ν a, f u ) is small enough, it would be stable with bounded ψ, ψ as formalized in the next Lem. 1. Lemma 1. Consider the internal dynamics (3.43) and assume the followings: 1) the tracking control u a (3.37) is designed slow enough with ν a, ν a bounded; 2) f u R is bounded; and 3) there is unmodeled/physical damping b u ψ for (3.43). Then, there always exists small enough ν a 0, ν a (t) ν a, t 0, such that (ψ(t), ψ(t)) is bounded t 0. Proof. Incorporating the physical/unmodeled damping b u and also linearizing the internal dynamics (3.43) around (ψ, ψ) = 0, we can obtain: m u ψ + bu ψ + k u c θ ψ = g 1 (ν a, ν u )ν a + g 2 ν a + f u where k u := mg d 1 d 2, g 1 R 1 5 is linear in its arguments, g 2 R 1 5 is bounded, and the terms in RHS (right hand side) are all bounded, if so is ν u.

100 Chapter 3. Spherically-connected multi-quadrotor Platform 90 Define the following Lyapunov function: V u := 1 2 m u ψ 2 + ɛψ ψ c θψ2 where ɛ > 0 is a cross-coupling term [45] chosen s.t., (i) ɛ < m u c θ to ensure V u be positive-definite. Differentiating this V u along the above internal dynamics, we can then obtain: V u = ψ ψ T b u ɛ 1 2 b uɛ 1 2 b uɛ ɛ c θ s θ θ ψ ψ + (g 1 (ν a, ν u )ν a + g 2 ν a + f u ) ( ψ + ɛφ) where the first term in RHS will be negative-definite if: (ii) b u ɛ > 0; (iii) ɛ > 1 2 tan θ θ ; and (iv) (b u ɛ)(ɛ c θ s θ θ) > 1 4 b2 uɛ 2. The solution ɛ of the inequalities (i)-(iii) is then given by 1 2 tan θ θ < ɛ < min( m u c θ, b u ) which will always have an intersection with the solution of (iv), i.e., ɛ 0 s.t., ( 1 4 b2 u + c θ ) ɛ 2 (b u c θ η)ɛ + b u η < 0 as θ ν a 0, where η := 1 2 s θ θ 0 as well with ν a 0. This then means that, with small enough ν a, V u will always be positive-definite with V u α 1 V u + α 2 Vu for some α 1, α 2 > 0, implying the boundedness of (ψ(t), ψ(t))

101 Chapter 3. Spherically-connected multi-quadrotor Platform 91 t 0. Note that the physical/unmodeled damping b u assumed in Lem. 1 in general exists in the real systems (e.g., aero-dynamic dissipation). This physical damping would also perturb the tracking control objective of Th. 6, whose effect yet can be adequately reduced by increasing the feedback gains k ν, k x, k r of (3.37) during our experimentations - see Sec The assumption of f u being bounded would also be likely granted in practice for the S2Q-platform system of Fig. 3.3, since it denotes the reaction moment along the N E -axis exerted only at the tool-tip with the moment-arm length being the radius of tool-tip itself Optimal Control Allocation and Closed-Form Expression Given the desired control wrench U = Ω T a u a in (3.31) with u a in (3.37) and u u = 0, the next task is to allocate this U =: [u f, u τ ] into the thrust of each quadrotor Λ o i of the S2Q-platform of Fig. 3.3 via the constrained optimization (3.9)-(3.12). Existence of solution of this allocation problem is assumed by designing the task to be control feasible as stated in Sec Further, as shown below, for the S2Q-platform system of Fig. 3.3, we can in fact find a closed-form expression of the optimization problem (3.9)-(3.12), eliminating the necessity of relying on iterative procedures as for the S3Q-platform system in Sec For this, similar to Sec , we reduce the size of the optimization problem (3.9)-(3.12) by utilizing the control generation equation (3.10), which can be

102 Chapter 3. Spherically-connected multi-quadrotor Platform 92 rewritten as I I u Λ = f (3.44) 0 S(d 2 d 1 ) S(d 1 )u f + u τ by multiplying both sides of (3.10) with I 0 S(d 1 ) I This matrix is the transpose of the adjoint operator, mapping U = (u f, u τ ) expressed in {F O } attached at x c to its equivalent wrench expressed in {F 1 } attached at x 1 with the same attitude as {F O }. Note that this adjoint operator also appears in (3.16). Denote the general solution of (3.44) by Λ = [ Λ 1 ; Λ 2 ] with Λ i := [ λ i1 ; λ i2 ; λ i3 ] R 3. We can then easily determine λ 22 and λ 23 from (3.44) s.t., 1 λ 22 = d 2 d 1 et 3 [ S(d 1 )u f + u τ ] 1 λ 23 = d 2 d 1 et 2 [ S(d 1 )u f + u τ ] and, further, compute λ 12, λ 13 s.t., λ 12 = e T 2 u f λ 22, λ13 = e T 3 u f λ 23 Recall from (3.15) that the rank of the first matrix in LHS (left hand side)

103 Chapter 3. Spherically-connected multi-quadrotor Platform 93 Fig. 3.5: Geometry of the reduced optimization problem (3.45) with transversing anchoring point ( 1 2 u f 1, 1 2 u f 1 ) and the shaded feasible region: (1) solution exists with non-zero γ op ; (2) solution exists with γ op = 0; and (3) no solution exists for γ op. of (3.44) is five. This then means that the solution of (3.44) assumes onedimensional null-space, which can be obtained by using the first row of (3.44), i.e., λ 11 = 1 2 u f 1 + γ, λ21 = 1 2 u f 1 γ where u f =: [u f1 ; u f2 ; u f3 ] R 3 and γ R identifies the null-space. Substituting these Λ 1, Λ 2 R 3 into (3.9)-(3.12), we can obtain the reduced form of the constrained optimization problem w.r.t. γ R s.t., γ op = arg min γ R γ [ u 2 f 1 + ] 2 ( λ 2 i 2 + λ 2 i 3 ) i=1 (3.45)

104 Chapter 3. Spherically-connected multi-quadrotor Platform 94 subject to a u f 1 + γ a 1, b u f 1 + γ b 1 a u f 1 γ a 2, b u f 1 γ b 2 where a i := 1 cos 2 φ i λ2 cos 2 φ i i3 λ 2 i 2, b i := λ2 i λ 2 i 2 λ 2 i 3 are to be positive constants when the control feasibility condition of Prop. 2 is enforced by the task design. Define c i := min(a i, b i ). Note also that the last term in RHS of (3.45) is a constant, thus, can be simply dropped off. We can then think of the geometry of this reduced constrained optimization problem w.r.t. γ as illustrated in Fig. 3.5, where the anchoring point ( λ 11, λ 21 ) = 1 2 (u f 1, u f1 ) is transversing according to a given u f1 and a solution exists when the slant line intersects with the shaded feasible region. From this Fig. 3.5, we can obtain the following closed-form solution: 0 if 1 2 u f 1 min(c 1, c 2 ) γ op = 1 2 u f 1 c 2 if 1 2 u f 1 > min(c 1, c 2 ) and c 1 c u f 1 + c 1 if 1 2 u f 1 > min(c 1, c 2 ) and c 1 < c 2 No solution γ op exists when u f1 > c 1 + c 2, which however is ruled out here by enforcing the control feasibility of Prop. 2.

105 Chapter 3. Spherically-connected multi-quadrotor Platform Experimental Results Setup We build the prototypes of S3Q-platform and S2Q-platform using AscTec Hummingbird quadrotors connecting to a frame using passive spherical joints. The frames are constructed of lightweight carbon fiber. The geometry of the prototypes are shown in Fig. 3.1 and Fig. 3.3 with the following geometric design parameter: d i d j = 1 [m], and (d i d j ) T e 3 = 0. The spherical joints allow the range of angle motion φ i = 32 and be arranged aligned with the D o -axis to maximize the system payload (i.e., p i = e 3 ). We locate the spherical joints close to the quadrotor center-of-masses (see Fig. 3.3) to satisfy the assumption in Sec. 3.2 as much as we can. Some important specifications of these prototypes are given in Table 3.1. We here use AscTec Hummingbird quadrotors with its weight of 0.6 [kg] and recommended payload of 0.2 [kg]. In our experiments, the maximum payload the quadrotor can carry is 0.6 [kg], equivalent to the maximum thrust of 11.8 [N]. The quadrotors are embedded with the attitude controller running onboard and receiving roll and pitch angles, yaw rate and the throttle as the quadrotor input command. We compute the input command on a remote PC and send it to the quadrotors via XBee communication. We use the motion capture system (MOCAP: VICON R ) to measure the attitude of quadrotors and the position and

106 Chapter 3. Spherically-connected multi-quadrotor Platform 96 Prototype System weight Payload Horizontal force S2Q-platform 1.54 [kg] 0.7 [kg] 14 [N] S3Q-platform 2.31 [kg] 1.2 [kg] 20 [N] Table 3.1: Specifications of S2Q-platform and S3Q-platform prototypes. attitude of the frame. To deal with the problem of noisy angular rate measurements with MOCAP, we employ a low-pass filter, in which the filter gains are tuned by comparing the measurements using IMU and MOCAP. We then need to translate the desired thrust Λ i given in Sec. 3.4 and Sec. 3.5 into each rotor command of the quadrotors. There are many techniques available to control the quadrotors to track that desired Λ i. For the S2Q-platform, we use the backstepping control [12] to provide a good performance since we have the explicit expression of the thrust Λ i and its derivative for S2Q-platform. For S3Q system, where the thrust Λ i are determined numerically, we employ a method that computes the quadrotor commands directly from the thrust Λ i - see Appendix Control of S3Q-Platform Prototype To evaluate the performance of the prototype, we first perform the hovering experiment. The results are shown in Fig. 3.6a. For comparison, we also include the hovering results of a single AscTec Hummingbird quadrotor. From Fig. 3.6a and Fig. 3.6b, we can see that the performance of the S3Q platform (i.e., RMS error of 2.3 [cm] in position and 2.2 o in attitude), despite its high system complexity,

107 Chapter 3. Spherically-connected multi-quadrotor Platform 97 x [cm] Angle [ o ] Time [s] Time [s] (a) S3Q-platform hovering with RMS errors of 2.3 [cm] and 2.2 o. e x e y e z e ψ e θ e φ x [cm] Angle [ o ] Time [s] Time [s] (b) Quadrotor hovering with RMS errors of 1.4 [cm] and 0.9 o. e x e y e z e ψ e θ e φ Fig. 3.6: Hovering performance of S3Q-Platform prototype and single quadrotor. is still comparable to that of the well-built AscTec Hummingbird quadrotor (i.e., RMS error of 1.4 [cm] and 0.9 o ). We now consider the motion tracking control of S3Q-platform, which requires the center-of-mass of the frame x w e = x w o to track a horizontal rectangular-like shape while maintaining the platform hovering posture (i.e., R d I 3 3 ). The desired motion is designed to be feasible according to the task planning precedure in Sec see Fig The results are shown in Fig. 3.7, where we see that the frame center-of-mass tracks the desired motion with the RMS position error of 4.6 [cm] while maintaining the hovering attitude with the RMS angle error of 4.2 o. To examine the compliant/backdrivable interaction capability of the S3Q system, we stabilize the system in the hovering posture at a fixed position (x w o [0; 0; 1.5]

108 Chapter 3. Spherically-connected multi-quadrotor Platform 98 x [m] y [m] z [m] Angle [ o ] Time [s] w x e1 w x d1 w x e2 w x d2 w x e3 w x d3 e ψ e θ e φ Fig. 3.7: Trajectory tracking: center-of-mass of the S3Q-frame tracking a horizontal rectangular shape while maintaining hovering posture (i.e., R o I 3 3 ). [m]) and apply some external wrench to that in N o, E o, D o directions at different positions, as shown in the snapshot of Fig The pose deviation from the hovering pose is proportionally to the external wrenches. Fig. 3.8 depicts the response of the system during the interaction with the markings (1), (2), (3), (4), (5) corresponding to the platform response at five instances t {4, 7, 12, 15, 21} [s].

109 Chapter 3. Spherically-connected multi-quadrotor Platform 99 (1) (2) (4) (5) x [m] (2) w x e1 w x d1 y [m] z [m] Angle [ o ] (3) (1) (1) (4) (5) (4) 0-20 (5) Time [s] w x e2 w x d2 w x e3 w x d3 e ψ e θ e φ Fig. 3.8: Stable interaction: position and attitude of the S3Q-frame response to the external wrench applied to the frame at t {4, 7, 12, 15, 21} [s]. In this experiment, we give high stiff gain k r on the attitude error, which gives higher priority to maintaining the attitude of the frame. We can see that under the external wrench at the instance (4) and (5), the position of the frame change accordingly, i.e., position error of 0.14 [m] in the z-axis, while the attitude error is fairly small with 8 o at (4) and 10 o at (5) since this given external wrench can

110 Chapter 3. Spherically-connected multi-quadrotor Platform 100 x [m] y [m] z [m] Angle [ o ] Time [s] w x e1 w x d1 w x e2 w x d2 w x e3 w x d3 e ψ e θ e φ Fig. 3.9: Manipulation task: tele-manipulating an object using haptic device. be compensated for by the combination of the torque about E o, N o axes and the force along D o axis. Compare to Sec , where higher priority is given to maintaining the position. We can see that the attitude and position error of the system are bounded under this external wrench, and when the external wrench is removed, the S3Q-frame recovers to its hovering pose/position. Here, note that

111 Chapter 3. Spherically-connected multi-quadrotor Platform 101 this compliant interaction is attained with no force/torque sensors, clearly showing that the SmQ system is backdrivable. Recall that, depending on the location of the tool-tip, direct interaction with standard quadrotors can be unstable due to their internal dynamics stemming from the under-actuation [2]. In contrast to that, here, regardless of the contact location (e.g., tool-tip shape), the interaction stability is guaranteed due to the full-actuation of the S3Q system in SE(3). The combination of the precise motion tracking and compliant interaction capability makes the S3Q-platform suitable for aerial manipulation. One such example would be tele-mapulating an object using haptic device. The results are shown in Fig. 3.9, where we can see that the system follows precisely the desired command (i.e., position tracking RMS error of 4.7 [cm] and attitude RMS error of 2.6 o ) from the user while compliantly interacting with the environment Control of S2Q-Platform Prototype Similar to Sec , we perform the motion control experiment of the S2Q system, i.e., the tool-tip position tracking of a horizontal circular-like shape while maintaining a certain pointing direction (e.g., pitch at zero and yaw at 45 o ). This task is designed to be feasible according to the task-planning in Sec see Fig. 3.4a. The results are shown in Fig. 3.10, where we can see that the S2Q-platform can track this feasible motion with the tracking RMS error of 2.7 [cm] while maintaining the desired pointing direction (i.e., yaw and pitch angles 2 Experiment video of S3Q platform is available at

112 Chapter 3. Spherically-connected multi-quadrotor Platform 102 x [m] y [m] z [m] Angle [ o ] Time [s] w x e1 w x d1 w x e2 w x d2 w x e3 w x d3 e ψ e θ e φ Fig. 3.10: Trajectory tracking: the virtual tool-tip on the line connecting the center-of-mass positions of two quadrotors tracking a horizontal circular shape while maintaining a certain attitude (e.g., pitch at zero and yaw at 45 o ). with RMS error of 5.6 o ). Since this task requires purely motion tracking, we adopt a virtual tool-tip located on the line connecting the center-of-mass of two quadrotors and utilize the control designed in Sec Fig shows the results of the interaction experiment wit the S2Q platform

113 Chapter 3. Spherically-connected multi-quadrotor Platform 103 (1) xo e (2) (3) x [m] y [m] z [m] Angle [ o ] (1) (2) (3) (1) (2) (3) Time [s] w x e1 w x d1 w x e2 w x d2 w x e3 w x d3 e ψ e θ e φ Fig. 3.11: Stable interaction: geometric center point x e and the pointing direction of S2Q-platform under the external wrench applied to the frame at t {8, 20, 32} [s]. through its tool-tip. For this, we utilize a physical tool with the tool-tip located on the line connecting the two quadrotors, i.e., satisfying the condition (3.17). See Fig We can see that the point-contact force at the tool-tip does not create external torque on the un-actuated direction of S2Q system. In this task we perform stabilization with the geometric center point of two quadrotors x e =

114 Chapter 3. Spherically-connected multi-quadrotor Platform 104 x [m] y [m] z [m] Angle [ o ] Time [s] w x e1 w x d1 w x e2 w x d2 w x e3 w x d3 e ψ e θ e φ Fig. 3.12: Manipulation task: tele-manipulating an object of [cm] using haptic device. 1 2 (x 1 + x 2 ) [0; 0; 1.5] [m]. We then apply point-contact forces at the tool-tip along N o, E o, D o directions. Fig presents the response of the system to the interaction wrench with the markings (1), (2), (3) corresponding to the responses at instances t {8, 20, 32} [s]. We then see that the contact force along N o direction causes a proportional position error in e 1 axis. With the contact force