A Novel Robotic Platform for Aerial Manipulation Using Quadrotors as Rotating Thrust Generators

We propose a novel robotic platform for aerial operation and manipulation, a spherically connected multiquadrotor (SmQ) platform, which consists of a rigid frame and multiple quadrotors that are connected to the frame via passive spherical joints and act as distributed rotating thrust generators to collectively propel the frame by adjusting their attitude and thrust force. Depending on the number of quadrotors and their configuration, this SmQ platform can fully (or partially) overcome the issues of underactuation of the standard multirotor drones for aerial operation/manipulation (e.g., body-tilting with sideway gust/force, dynamic interaction hard to attain, complicated arm-drone integration, etc.). We present the dynamics modeling of this SmQ platform system and establish the condition for its full actuation in SE(3). We also show how to address limited range of spherical joints and rotor saturations as a constrained optimization problem by noticing the similarity with the multifingered grasping problem under the friction-cone constraint. We then design and analyze feedback control laws for the S3Q and S2Q systems as a combination of high-level Lyapunov control design and low-level constrained optimization and show that the (fully actuated) S3Q system can assume any trajectory in SE(3), whereas the S2Q system in R³ × S² with its unactuated dynamics is still internally stable. Experiments are also performed to show the efficacy of the theory.

Nomenclature

$\mathcal {F}_W$	Inertial world frame.
$\mathcal {F}_0, \mathcal {F}_i$	Body frame of the SmQ frame and quadrotor $i$.
$n \in \mathbb {N}^+$	Number of quadrotors of the system
$m_o, m_i \in \Re$	Mass of the SmQ frame and the quadrotor $i$.
$J_o, J_i \in \Re$	Inertia of the SmQ frame and the quadrotor $i$.
$x_o^w, x_i^w \in \Re ^3$	CoM positions of the SmQ frame and the quadrotor $i$.
$R_o, R_i \in \text{SO(3)}$	Attitudes of the SmQ frame and the quadrotor $i$.
$v_i:=R_i^T\dot{x}_i^w$	Translation velocity in the body-fixed frame.
$\omega _o, \omega _i \in \mathfrak {so}(3)$	Angular velocities in the body-fixed frames.
$x_c^w, v_c^0 \in \Re ^3$	Position/velocity of the system CoM [see (5)].
$\xi :=[v_c;\omega _o] \in \Re ^6$	Translation/angular velocities of the system CoM.
$d_i:=x_i - x_c$	Position of the quadrotor $i$ with respect to the system CoM.
$\Lambda _i^0 \in \Re ^3$	Thrust vector of the quadrotor $i$ in $\mathcal {F}_0$.
$\lambda _i \in \Re, \lambda _i \leq \bar{\lambda }_i$	Thrust magnitude of the quadrotor $i$, and its bound.
$N_i^w \in \Re ^3$	Constraint between the SmQ frame and the quadrotor $i$.
${p}_i^o \in \Re ^3$	Center axis of the $i$th spherical joint.
$\phi _i \in \Re$	Motion range of the spherical joint $i$.
${f}^o_e, {\tau }^o_e \in \Re ^3$	External force/torque at the CoM of the SmQ frame.
$S({\omega }) \in \Re ^{3 \times 3}$	$S({\omega }){\nu } = {\omega } \times {\nu }, \forall {\omega },{\nu } \in \Re ^3$.

SECTION I. Introduction

Multirotor drones (e.g., quadrotor, hexarotor, octarotor, etc.) have received considerable attention from research communities and the general public alike due to their potential in a wide range of applications. The rapid developments in sensors [e.g., microelectromechanical system (MEMS), inertial measurement unit (IMU), global navigation satellite system (GNSS), and camera], motors [e.g., brushless dc (BLDC) motors], batteries (e.g., lithium-polymer battery), materials (e.g., carbon fiber), and onboard computing and wireless communication technologies, spurred by consumer electronics and information technology industry, have substantially lowered the price of the multirotor drones, even with many now affordable to general customers, hobbyists, etc. At the same time, the fast advancements in sensor fusion, simultaneous localization and mapping (SLAM), and computer vision and control technologies have enabled many successful applications for the multirotor drones, with aerial photography, geosurvey, traffic monitoring, pesticide spraying, surveillance, and reconnaissance being the most representative ones. Despite this, as can also be seen just above, the successful applications of the multirotor drones have been mostly limited to passive observation, and it is deemed by many groups and companies that the potential of the multirotor drones for physical interaction (e.g., aerial operation/manipulation) should be tapped not only for academic research but also to drastically expand the commercial market of the multirotor drones.

The key challenge in using the multirotor drones for aerial operation/manipulation is their underactuation, i.e., with all the rotors collinearly attached to the body, it cannot change the forcing (or thrust) direction without tilting the platform itself. To better see this, consider the drone–manipulator systems (i.e., multirotor drones with multi-degree-of-freedom (DOF) robotic arms), which, although most widely studied for aerial manipulation (e.g., [1]–​ [9]), possess the following limitations due to their underactuation coupled with the current limited motor technology.¹

1. It typically cannot push sideways or resist lateral gust without tilting its body (e.g., difficult to maintain the operation/manipulation contact), as their majority is equipped with a robot arm only with a small number of DOFs (e.g., two DOFs [1], [2] , [6], [8], [9]).

2. They typically cannot attain “dynamic” (i.e., fast/smooth) interaction control, as their majority is not equipped with a torque-controlled robot arm (e.g., [1], [2], [8], [9]) and such a dynamic interaction may destabilize the unactuated dynamics.

3. Their arm–drone integration and control synthesis are typically fairly complicated, since the issue of underactuation (e.g., instability) can only be properly addressed by considering their full dynamics.

This, we believe, is because the multirotor 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 paper, we propose a novel robotic platform for aerial operation and manipulation, namely spherically connected multiquadrotor (SmQ) platform (see Fig. 1). This SmQ platform consists of a rigid frame (or tool) and multiple quadrotors (or multirotor 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 number of quadrotors and their configuration, this SmQ platform can fully (e.g., S3Q platform of Section IV) or partially (e.g., S2Q platform of Section V) overcome the aforementioned issues of underactuation of the multirotor drones. Adopting multiple quadrotors, this SmQ platform often possesses a larger payload than a single multidrone system. 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 (see Section VI) 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 the SmQ platform and robot arm impedance controls, or even their kinematic control combination), as the (fully actuated) SmQ system is kinematically reducible [10]. Note also that this SmQ platform can be quickly constructed by using off-the-shelf/commercial multirotor drones, which are often shipped with well-functioning low-level attitude/thrust controls.

Fig. 1.

SmQ (with $m=3$) platform consisting of multiple quadrotors connected to a rigid frame (or tool) via passive spherical joints.

The main goal of this paper is to provide a 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., dependence 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 a 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 multifingered grasping problem under the friction-cone constraint [11]. 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 $\Re^3 \, \times\, $ S$^2$ with its unactuated dynamics is 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.

A. Comparison With Other Aerial Manipulation Systems

In contrast to the drone–manipulator system (e.g., [1]–​ [9]) with some of its limitations as stated above, our SmQ platform can resist sideway gust/force while holding its attitude due to its full actuation in SE(3), attain “dynamic” interaction control with its full actuation and force/torque-level actuation, and 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 form factor due to its requiring additional quadrotors. This large form factor may be detrimental (or even not allowable) for some applications, for which the drone–manipulator system or other aerial manipulation systems, as stated in the following, would be preferred.

The issue of underactuation has been recognized by many researchers, and some of them, including us, have proposed tilted multirotor platforms (e.g., [12]–​[16]), 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 $^\circ$ pitch-turning and large-force downward pushing (e.g., [15], [16]), all impossible with our SmQ platform system. This tilted multirotor platform is also generally of smaller form factor than the SmQ platform. However, due to the interrotor flow interference, necessarily arising from the rotors not collinear with each other, the energy efficiency and, consequently, the flight time of this tilted multirotor platform are rather limited.

Another system along this line is the tilting multirotor platform, where the tilting angles of the rotors of a multirotor drone are actively controlled with extra actuators via some tilting mechanisms (e.g., [17]–​[20]). This tilting multirotor platform can then overcome the issue of underactuation 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 multirotor 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 [21] and [22] rigidly attach multiple quadrotors essentially to a frame to increase the loading capability of the system. However, with all the rotors again collinear, these platforms are again under the same limitations stemming from the underactuation. On the other hand, cable-suspended systems using multiple quadrotors are presented in [23]–​[25], 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.

Portions of this paper were presented in [26]. In this paper, we generalize and complete the results in our previous work. We first expand the control design for general pose tracking for both the S2Q platform and the S3Q platform with the proof of their effectiveness. We relax the assumption on the symmetry of the S2Q system and present the derivation and analysis of the internal dynamics of the S2Q platform here for the first time. We also provide a complete treatment for the problem of optimal control allocation with the analysis of solution existence and a real-time solver. New experiments are also performed to illustrate the capabilities of the proposed platform.

The rest of this paper is organized as follows. The design and the dynamical model of the system are presented in Section II. The actuation capacity and the control feasibility of the system are analyzed in Section III. 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 Sections IV and V, respectively. In Section VI, we present experimental results and discussions. Some concluding remarks are given in Section VII.

SECTION II. System Design and Modeling

A. System Description

Consider the SmQ platform system, with $m$ ($m=1,2,...,n$) being the number of quadrotors attached to the frame, as shown in Fig. 1. We design the joint-quadrotor connection in such a way that each spherical joint is attached at the center of mass (CoM) 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 Section VI . We then have the following relation between the CoM position² ${x}_{i}^w \in \Re ^3$ of the $i$th quadrotor and that of the SmQ frame ${x}_{o}^w \in \Re ^3$, all expressed in the inertial frame $\lbrace \mathcal {F}_W\rbrace$ such that \begin{equation} {x}_i^w = {x}_o^w +{R}_o {x}_{i}^o,\,\,\,i=1,2,...,n \end{equation} where ${x}_{i}^o \in \Re ^3$ is $x_i$ expressed in the body frame $\lbrace \mathcal {F}_0 \rbrace$ fixed to the CoM of the SmQ frame, and ${R}_o \in$ SO(3) is the attitude of the frame expressed in $\lbrace {\mathcal F}_W\rbrace$ . See Fig. 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 (with respect to the CoM) decoupled from that of the SmQ frame, while generating the thrust force vector ${ \Lambda }_i^o := \lambda _i {R}^T_o {R}_i {e}_3 \in \Re ^3$ and exerting the constraint force ${N}_i^w \in \Re ^3$ on the SmQ frame through the attaching point ${x}_i \in \Re ^3$ to enforce constraint (1). Here, $\lambda _i >0$ and ${R}_i \in$ SO(3) are, respectively, the thrust force magnitude and the attitude of the $i$th quadrotor expressed in $\lbrace {\mathcal F}_W \rbrace$. No reaction moment is transmitted from the quadrotor to the SmQ platform via the passive spherical joint.

Real spherical joints only allow for a limited range of motion, typically smaller than $(-{\text{35}}^\circ,+{\text{35}}^\circ)$ 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: \begin{equation} ({p}_i^o)^T { \Lambda }_i^o \geq \Vert { \Lambda }_i^o \Vert \cos \phi _i,\,\,\, i = 1,2,..., n \end{equation} where ${p}_i^o \in \Re ^3$ is the unit vector along the direction of the center axis of the $i$th spherical joint, $\phi _i \in \Re$, $0 < \phi _i < \pi /2$, is its maximum range of angle motion, and $\Vert { \Lambda }_i^o \Vert := \sqrt{ ({ \Lambda }^o_i)^T { \Lambda }_i^o} \geq 0$ , which is also in general bounded, i.e., there exists $\bar{\lambda }_i \geq 0$ s.t., $||{ \Lambda }_i^o|| < \bar{\lambda }_i$, $i=1,2,...,n$ (see Fig 2 ). Note that this joint constraint (2) with $\phi _i < \pi /2$ also implies that the quadrotor thrust $\lambda _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 Sections III–​V).

Fig. 2.

Range limit of the spherical joint. $p_i \in \Re ^3$ is the center-axis unit vector, $\phi _i$ the maximum allowable motion range, and $\Lambda _i \in \Re ^3$ is the quadrotor thrust vector.

B. Dynamics Modeling and Reduced Dynamics

With the assumption that the spherical joint is attached at the CoM of each quadrotor, the Newton–Euler dynamics of the $n$ quadrotors and the SmQ frame can be written as \begin{align} m_i \dot{ v}_i + S({\omega }_i) m_i {v}_i =& m_i g { R}_i^T { e}_3 + { R}_i^T { N}_i^w - {R}_i^T {R}_o {\Lambda }_i^o \nonumber \\ J_i\dot{{\omega }}_i + S({\omega }_i) J_i {\omega }_i =& {\tau }_i \nonumber\\ m_o \dot{ v}_o + S({\omega }_o) m_o {v}_o =& m_o g { R}_o^T { e}_3 -\sum _{i=1}^n { R}_o^T { N}_i^w + f^o_e \nonumber\\ J_o\dot{{\omega }}_o + S({\omega }_o) J_o {\omega }_o =& -\sum _{i=1}^n S({x}_i^o) {R}^T_o {N}_i^w + {\tau }^o_e \end{align} where $m_i>0, J_i \in \Re ^{3 \times 3}$ and $m_o >0, J_o \in \Re ^{3 \times 3}$ are the mass and inertia of the $i$th quadrotor and the frame, respectively, $g \in \Re$ is the gravity acceleration, ${v}_i:= {R}^T_i \dot{{x}}_i^w \in \Re ^3$, ${v}_o:= {R}^T_o \dot{{x}}_o^w \in \Re ^3$, and ${\omega }_i, {\omega }_o \in \mathfrak {so}$(3) are their body translation and angular velocities represented in their body-fixed frames $\lbrace \mathcal {F}_i \rbrace$ and $\lbrace \mathcal {F}_0 \rbrace$ with the superscripts omitted for simplicity, ${N}_i^w \in \Re ^3$ is the constraint force to enforce constraint (1), ${\Lambda }_i^o = \lambda _i {R}_o^T {R}_i {e}_3$ is the thrust force vector constrained according to (2), ${\tau }_i \in \Re ^3$ is the torque control input of the $i$th quadrotor, $S({\omega })$ is the skew-symmetric matrix such that $S({\omega }){\nu } = {\omega } \times {\nu }, \forall {\omega },{\nu } \in \Re ^3$, and ${f}^o_e, {\tau }^o_e \in \Re ^3$ are the external force and torque acting at the CoM of the SmQ frame.

By eliminating the constraint forces ${N}_i^w$ in (3) of constraint (1), we can reduce the dynamics (3) into the lumped six-DOF dynamics of the SmQ platform in SE(3) and the 3$n$-DOF attitude dynamics of the $n$ quadrotors. First, the reduced six-DOF SmQ-frame dynamics can be obtained s.t. \begin{equation} \begin{aligned} M \dot{\xi } + C \xi + G = U + F_e \end{aligned} \end{equation} where $\xi := [v_c; \omega _o] \in \Re ^6$ , with ${v}_c$ being the body translation velocity of the CoM of the total system expressed in $\lbrace \mathcal {F}_0\rbrace$, as defined by \begin{eqnarray} {v}_c := {R}^T_o \frac{d}{dt} \underbrace{ \left[ \frac{1}{\sum _{i=0}^n m_i} \sum _{i=0}^n m_i {x}_i^w \right] }_{=:{x}_c^w} \in \Re ^3 \end{eqnarray} and \begin{align*} M &:= \left[\begin{array}{cc} \bar{m}I & 0 \\ 0 & \bar{J} \end{array}\right], \\ \bar{m} &:= \sum _{i=0}^n m_i, \, \bar{J}:= J_o - \sum _{i=1}^n m_i S({x}_i^o) S({x}_i^o - {x}_c^o),\\ C &:= {\left[\begin{array}{cc}S(\bar{m}\omega _o) & 0 \\ 0 & -S(\bar{J}\omega _o) \end{array}\right]}, \\ G &:= {\left[\begin{array}{c}-\bar{m} gR_o^Te_3 \\ 0 \end{array}\right]},\text{ and } F_e = {\left[\begin{array}{c} f^o_e \\ \tau ^o_e \end{array}\right]} \end{align*} are the lumped (symmetric/positive-definite) inertia matrix, the (skew-symmetric) Coriolis matrix, the gravity effect, and the external forcing, respectively: \begin{eqnarray} {U}: = - {B} {\Lambda } \in \Re ^6 \end{eqnarray} is the control input to the SmQ platform, where ${\Lambda } :=[{\Lambda }_1^o;{\Lambda }_2^o;...; {\Lambda }_n^o ] \in \Re ^{3n}$ is the collective rotating thrust vectors, and \begin{equation} {B}(d):= {\left[\begin{array}{cccc} I & {I} & ... & {I}\\ S(d_1) & S(d_2) & ...&S(d_n) \end{array}\right]} \in \Re ^{6 \times {3n}} \end{equation} 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}_i^o - {x}_c^o \in \Re ^3$. On the other hand, the attitude dynamics of each quadrotor is given by \begin{eqnarray} J_i \dot{{\omega }}_i + S(\omega _i) J_i{\omega }_i = {\tau }_i \end{eqnarray} which constitutes the $3n$-DOF attitude dynamics of the $n$ quadrotors in SO$^n$(3) in addition to the six-DOF reduced dynamics of the SmQ platform (4).

Note that, due to our design of connecting the spherical joint at the CoM of the quadrotors, the attitude dynamics of each quadrotor (8) is decoupled from the SmQ platform dynamics (4). This attitude dynamics of the quadrotor can be typically controlled much faster than the SmQ platform dynamics. This then means that we can consider ${\Lambda }_i^o = \lambda _i {R}_o^T {R}_i {e}_3 \in \Re ^3$ as the control input for (4) and the $n$ quadrotors as rotating thrust actuators for the six-DOF SmQ platform dynamics (4) with the fast-enough low-level attitude control embedded for each quadrotor (see Section VI).

We can also see that the structure of $B(d) \in \Re ^{6 \times 3n}$ in (4) dictates whether we can generate an arbitrary control action $U \in \Re ^6$ by using the thrust inputs $\Lambda _i^o$ of the $n$ quadrotors. This structure of $B(d)$ depends on the number of quadrotors $n$ and the arrangement of the attaching points $x_i^o$ (see Fig. 1). In Section III, 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 (2) and the thrust actuation bound $\bar{\lambda }_i$.

SECTION III. Control Generation and Feasibility Analysis

For the control of the six-DOF SmQ platform dynamics (4), it is desirable to generate any arbitrary control input ${U} \in \Re ^6$ by recruiting the thrust actuation ${\Lambda }_i$ of the $n$ quadrotors. This, yet, is not, in general, possible, since the thrust actuation of each quadrotor $\Lambda _i \in \Re ^3$ is constrained by the spherical joint rotation range (2) and the thrust generation bound $\bar{\lambda }_i$. This control generation may also be limited depending on the SmQ platform design, i.e., the structure of the mapping matrix $B(d)$ in (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 \in \Re ^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: \begin{equation} \begin{aligned} \Lambda _\text{op} = \underset{\Lambda = (\Lambda _i, \Lambda _2,...,\Lambda _n)}{\arg \min } \frac{1}{2} \Lambda ^T \Lambda \qquad \qquad \end{aligned} \end{equation} subject to \begin{eqnarray} && B(d) \Lambda = -U \\ && \Lambda ^T_i [ \cos ^2 \phi _i I_{3 \times 3} - p_i p^T_i ] \Lambda _i \leq 0 \\ && \Lambda _i^T \Lambda _i - \bar{\lambda }_i^2 \leq 0 \end{eqnarray} where (9) is to maximize the energy efficiency by eliminating internally dissipated thrust generations (or to minimize internal forces producing no net platform motion), (10) is to generate the desired control wrench $U \in \Re ^6$, (11) is the spherical joint motion range constraint written in a matrix form, and (12) is to reflect the boundedness of the thrust actuation $\Lambda _i$. The desired platform control $U \in \Re ^6$ will be designed based on the Lyapunov design in Sections IV-A and V-B, which is then optimally decoded into each rotor thrust $\Lambda _i$ while respecting constraints (11) and (12). This means that our (closed-loop) control strategy is hierarchical, consisting of high-level Lyapunov control design and low-level constrained optimization [see Section IV (for the S3Q system) and Section V (for the S2Q system)].

The constrained optimization (9)–​(12) is a second-order cone programming (SOCP), which is a convex optimization problem [27]. Furthermore, feasibility and continuity of the desired control $U \in \Re ^6$ for (10) is to be ensured via the task planning, as stated in Section III-B. With its convexity and the continuity of its constraints and the objective function, the solution continuity of this constrained optimization (9)–​(12) follows [28]. This constrained optimization (9)–​(12) also has a similarity with the multifinger grasping problem under the friction-cone constraint [11], which we will exploit in the following to solve this constrained optimization (9)–​(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 \in \Re ^6$, for which dimension reduction turns out to be instrumental (see Sections IV-B and V-C). The design of $U$ and its real-time optimal allocation to $\Lambda _i$ for the specific S3Q platform and S2Q platform are the topics of Sections V and IV, respectively. Instead, in this section, analyzing (10)–​(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 constraints (10)–​ (12) is guaranteed.

A. Necessary Rank Condition for Full Actuation in SE(3)

Now, let us focus only on the first condition (10) of the constrained optimization problem. Then, since $U \in \Re ^6$ can, in general, be an arbitrary control command, the existence of the solution, $\Lambda \in \Re ^{3n}$, depends on the rank of the matrix $B(d) \in \Re ^{6 \times 3n}$. As can be seen from (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 the solution for (10). This suggests us to choose these mechanical design parameters, $d_i$, $i=1, 2, ..., n$, to ensure that \begin{equation} {rank}(B) = 6. \end{equation} Note that this rank condition (13) is necessary for the six-DOF SmQ platform (4) to be fully actuated in SE(3). Proposition 1 shows that, for the six-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 \in \Re ^3$) should avoid certain “singular” configurations.

Proposition 1:

Consider the SmQ platform (4) consisting of $n$ quadrotors connected via spherical joints. Then, the necessary rank condition (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 $\lbrace \mathcal {F}_0 \rbrace$) not collinear with each other, i.e., \begin{eqnarray} (d_2-d_1) \times (d_3-d_1) \ne 0. \end{eqnarray}

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 \begin{equation*} B \Lambda = {\left[\begin{array}{ccc} I & I & I\\ S(d_1) & S(d_2) &S(d_3) \end{array}\right]} {\left[\begin{array}{c} \Lambda _1\\ \Lambda _2 \\ \Lambda _3 \end{array}\right]} \end{equation*} where we can decompose $B$ s.t. \begin{eqnarray*} B =\underbrace{ {\left[\begin{array}{cc} I & 0\\ S(d_1) & I \end{array}\right]} }_{:=L} \underbrace{ {\left[\begin{array}{ccc} I & 0 &0\\ 0 &S(d_2-d_1) &S(d_3-d_1) \end{array}\right]} }_{:=\Sigma } \underbrace{ {\left[\begin{array}{ccc} I & I & I\\ 0 & I & 0 \\ 0 & 0 &I \end{array}\right]} }_{:=D} \end{eqnarray*} by using the linearity of the operator $S(\star)$. We can then see that ${rank}(B) = {rank}(\Sigma)$ , since $L$ and $D$ are both full rank. We now show that ${rank}(\Sigma) = 6$ if and only if the noncollinear condition (14) is ensured with the three quadrotors, i.e., for the S3Q platform. For this, recall that ${rank}(S(\nu)) = 2$ $\forall \nu \ne 0$ . Thus, we have \begin{eqnarray*} {rank}(\Sigma) = 3 + {rank}({\left[\begin{array}{cc} S(d_2 -d_1) & S(d_3 -d_1) \end{array}\right]}). \end{eqnarray*} Suppose that the noncollinear condition (14) is satisfied; yet, ${rank}(\Sigma) < 6$. Then, there should exists $y \in \Re ^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 \in \Re$, which yet results in $(d_2 -d_1) \times (d_3 - d_1) = 0$, contradicting the noncollinear assumption (14). Note that addition of more quadrotors to the S3Q platform with the noncollinear condition (14) will still ensure the necessary rank condition (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}(\Sigma) \leq 5$; thus, the platform is underactuated. This completes the proof. $\blacksquare$

Proposition 1 characterizes “singular” designs of the SmQ platform, which should be avoided if the full actuation in SE(3) is necessary. It is, however, worthwhile to mention that, even if an SmQ platform is not fully actuated in SE(3) (i.e., condition (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, with \begin{eqnarray*} {B}(d):= {\left[\begin{array}{cc}I & {I} \\ S(d_1) & S(d_2) \end{array}\right]} \in \Re ^{6 \times {6}} . \end{eqnarray*} We can then verify that \begin{eqnarray} {\left[\begin{array}{cc} d_2^T S^T(d_1) & (d_2 - d_1)^T \end{array}\right]} B \Lambda = 0\quad \forall \Lambda \in \Re ^6 \end{eqnarray} that is, if $d_1 \ne d_2$, ${rank}[B] =5$ and \begin{eqnarray} {null}[B^T] \approx {\left(\begin{array}{c}S(d_1) d_2 \\ d_2-d_1 \end{array}\right)} = \left[ \begin{array}{cc}I & S(d_1) \\ 0 & I \end{array} \right] \left(\begin{array}{c}0 \\ d_2-d_1 \end{array}\right). \end{eqnarray} 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 (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 [11]. For the S2Q system in Fig. 3 with $x_1, x_2,\text{ and } x_e$ all on the same line with \begin{eqnarray} (d_e-d_1) \times (d_2-d_1) =0 \end{eqnarray} this then implies that the system can only generate five-DOF actuation with the moment about the $N^0$-axis (i.e., $x_2^o - x_1^o$) of the tool-tip frame $\lbrace \mathcal {F}_E \rbrace$ not generatable/resistable. Even so, by designing the tool-tip located at $d_e \in \Re ^3$ in Fig. 3, we can still fully control the tool-tip translation in $\Re ^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 an SmQ platform with $m \geq 3$ and designed with the noncollinear condition (14) ensured.

Fig. 3.

S2Q platform with the two quadrotors and the tool, where we assume the following: 1) $d_1, d_2, \text{ and }d_e$ are within the plane spanned by the $N^\circ$- and $E^\circ$-axes of $\lbrace {\mathcal F}_O \rbrace$; 2) $d_2-d_1$ and $d_e-d_1$ are all parallel to the $N^\circ$-axis with (17); and 3) the spherical joint center axes $p_i$ are all along the $D^\circ$-axis of $\lbrace {\mathcal F}_O \rbrace$.

B. Control Feasibility

Now, suppose that we have an SmQ platform satisfying Proposition 1. This then ensures that there exists thrust vectors $\Lambda _i \in \Re ^3$ , $i=1, 2,..., n$, for (10) to produce any desired platform control $U \in \Re ^6$. This, however, is true only if $\Lambda _i$ is not constrained, and what is unclear is whether these solution thrust vectors $\Lambda _i$ would also respect constraints (11) and (12). We say the control wrench $U \in \Re ^6$ is feasible if there exists a solution $\Lambda =(\Lambda _1, \Lambda _2,...,\Lambda _n) \in \Re ^{3n},$ which also complies with constraints (11) and (12). In this section, we analyze this control feasibility of the SmQ platforms and also show how to design a task for them while guaranteeing this control feasibility.

For this, we would first like to recognize that this control feasibility of the SmQ platform is analogous to the force closure of the multifingered grasping under the friction-cone constraint. More precisely, notice from Fig. 2 that each thrust vector $\Lambda _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 \in \Re ^6$ can be generated by the thrust vectors $\Lambda _i$ each residing in their respective cone. This is exactly the same question of the force closure [11], [29], 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 the Coulomb friction model (i.e., cone with the angle specified by the friction coefficient $\mu$). Proposition 2 is due to this analogy between the control feasibility and the force closure.

Proposition 2:

For the SmQ platforms satisfying Proposition 1, we can generate any control wrench $U \in \Re ^6$ in (10) for the platform using the thrust vectors $\Lambda _i \in \Re ^3$, $i=1, 2, ..., n$, while respecting constraints (11) and (12), if and only if the desired wrench $U_d$ is in force closure, with $\Lambda _i \in \Re ^3$ being the contact forces residing inside their friction cone defined by constraints (11) and (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 \in \Re ^6$ and the collective thrust vectors $\Lambda _i$ , $i = 1, 2, ..., n$ , simultaneously to drive the SmQ platform (4) according to a certain task objective while also taking into account all constraints (10) –​(12) in a manner similar to the technique of model-predictive control. This approach, yet, typically results in a computationally complex algorithm, the real-time running of which is often challenging in practice. Instead, in this paper, we adopt the other “task-design” approach, where we first construct the (quasi-static) feasible control set ${\mathcal U} \in$ se(3) (expressed in the body frame $\lbrace \mathcal {F}_0 \rbrace$) under constraints (11) and (12): \begin{eqnarray*} \mathcal {U} := \lbrace U = B \Lambda \,| \,p_i^T \Lambda _i \geq \Vert \Lambda _i \Vert \cos \phi _i, \Vert \Lambda _i \Vert \leq \bar{\lambda }_i \rbrace \end{eqnarray*} and design the task in such a way that its required control $U$ is always within this ${\mathcal U}$ , thereby guaranteeing the control feasibility. This “task-design” approach also allows for an hierarchical control architecture, where $U \in \Re ^6$ for (4) [via (10)] is computed using, e.g., Lyapunov-based control design for the task, which is designed a priori for control feasibility, while the constrained optimization (9)–​(12) is real-time solved given the control $U$ with the solution existence always guaranteed (see Sections IV-B and V-C).

The control feasible sets ${\mathcal U}$ of the S2Q platform (see Fig. 3) and S3Q platform (see Fig. 1) are shown in Fig. 4, where the feasible control force set ${\mathcal U}_f$ and the feasible control moment set ${\mathcal U}_\tau$ are separately presented. For this, we use the parameters of the S2Q platform and S3Q platform prototypes in Section VI. The cross dots represent the gravity wrench $G = [g_f; g_\tau ] \in \Re ^6$ in (4) expressed in $\lbrace \mathcal {F}_0 \rbrace$ 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. 4(a) that, due to its underactuation (with condition (13) not granted—see Proposition 1), ${\mathcal U}_\tau$ 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 ${\mathcal U}$ or not before performing the task.

Fig. 4.

(Top) Feasible force sets ${\mathcal U}_f$ and (bottom) feasible moment sets ${\mathcal U}_\tau$ 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 Section VI with the performance presented in Figs. 8(a) and 9(a). The cross dots are the gravity wrench $G$ at the hovering posture, which is also the required control wrench $U_d$ for stabilization control in Section VI. (a) Feasible set ${\mathcal U}$ of the S2Q platform in Fig. 3. (b) Feasible set ${\mathcal U}$ of the S3Q platform in Fig. 1.

Here, note that the feasible set ${\mathcal 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 ${\mathcal U}$ in the neighborhood of $G$ (i.e., solution existence guaranteed). To better show this, we draw the control inputs of two time-scaled trajectories in Fig. 4 for the S2Q and S3Q systems, respectively. The slower (i.e., smaller/red orbits in Fig. 4) is, in fact, used during the experiments in Sections VI-B and VI-C, and we can see that it is feasible satisfying constraints (11) and (12), whereas the faster (i.e., larger/blue orbits in Fig. 4) is not feasible, since some of its trajectories are outside of ${\mathcal 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 ${\mathcal 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, ${\mathcal U}_f$ and ${\mathcal U}_\tau$, of the SmQ systems are not independent of each other as typically assumed in the motion planning literature studies.

SECTION IV. Control of the S3Q Platform System

In this section, we consider the tracking control problem of the S3Q platform system of Fig. 1. With the rank condition of Proposition 1 and the control feasibility of Proposition 2 satisfied, the S3Q platform system becomes fully actuated with the solution of the control allocation (9)–​(12) always guaranteed to exist, thereby rendering the tracking problem in $\Re^3 \,\times$SO(3) rather straightforward. This then means that we can freely control the position and orientation of a 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 Section V , where its underactuation (see Section III-A) substantially complicates the control design and analysis.

Let us denote the tool-tip position by $x_e \in \Re ^3$ and the tool attitude by $R_o \in$ SO(3). We then have the following relation between the tool pose $(x_e^w,R_o) \in$SE(3) and the platform pose $(x_c^w, R_o)$: \begin{eqnarray} x_e^w = x_c^w + R_o d_e, \,\,\,\, \dot{x}_e^w &= R_o v_c + R_o S(\omega _o) d_e \end{eqnarray} where $x^w_e, x^w_c \in \Re ^3$ are $x_e, x_c$ expressed in $\lbrace {\mathcal F}_W \rbrace$, $d_e \in \Re ^3$ is the vector from the system CoM $x_c$ to $x_e$ expressed in $\lbrace {\mathcal F}_0 \rbrace$ (i.e., $d_e = x^o_e - x_c^o$), and $v_c$ is defined in (5). The control objective is then to design $U \in \Re ^6$ in (4) s.t. \begin{eqnarray*} (x_e^w(t),R_o(t)) \rightarrow (x^w_d(t),R_d(t)) \end{eqnarray*} while satisfying constraints (10)–​(12), where $(x_d^w(t),R_d(t))$ $\in$ SE(3) is the timed trajectory of the desired tool pose (specified by the coordinate frame $\lbrace {\mathcal F}_D \rbrace$ and expressed in $\lbrace {\mathcal F}_W \rbrace$) and is assumed to be planned offline to ensure its feasibility, as explained in Section III-B.

A. Lyapunov-Based Control Design

To design $U \in \Re ^6$ in (4) to achieve the tracking as stated above, we define a smooth nonnegative potential function on SE(3) s.t. \begin{equation*} \Phi := \frac{1}{2} k_x e_x^T e_x + k_r \Psi _R \end{equation*} where $k_x, k_r \in \Re$ are positive gains, and \begin{equation*} e_x = x_e^w - x_d^w, \quad \Psi _R = \frac{1}{2} {tr}(\eta - \eta R_d^T R_o) \end{equation*} with $\eta = {diag}[\eta _1, \eta _2, \eta _3] \in \Re ^{3 \times 3}$ and $\eta _1, \eta _2, \eta _3$ being distinct positive constants. This potential $\Phi$ has following properties: 1) $\Phi \geq 0$ with the equality hold iff $(x_e^w, R_o) = (x^w_d, R_d)$; and 2) its derivative is given by \begin{equation*} \dot{\Phi } = \nabla \Phi \cdot (\xi - \xi _d) \end{equation*} where $\nabla \Phi \in \Re ^{1 \times 6}$ is the one-form of $\Phi$ defined s.t. \begin{eqnarray*} \nabla \Phi ^T &=& {\left[\begin{array}{cc}I & 0 \\ -S(d_e) & I \end{array}\right]} {\left[\begin{array}{c}k_x R_o^T e_x \\ k_r (\eta R_d^T R_o - R_o^T R_d \eta)^\vee \end{array}\right]} \in \Re ^6 \\ \xi _d &:=& {\left[\begin{array}{c}S(d_e) R_o^T R_d \omega _d + R_o^TR_d v_d, \\ R_o^T R_d \omega _d \end{array}\right]} \in \Re ^6 \end{eqnarray*} where $v_d, \omega _d \in \Re ^3$ are the desired velocities expressed in $\lbrace {\mathcal F}_D \rbrace$ as defined by \begin{eqnarray} \dot{x}_d^w = R_d v_d, \,\,\,\, w_d = (R^T_d \dot{R}_d)^\vee \end{eqnarray} and $\vee : \Re ^{3 \times 3} \rightarrow \mathfrak {so}(3)$ is the inverse operator of $S(\star)$.

We then design the control $U \in \Re ^6$ in (4) s.t. \begin{equation} \begin{aligned} U = M \dot{\xi }_d + C\xi _d - k_v e_\xi - \nabla \Phi ^T + G - F_e \end{aligned} \end{equation} where $e_\xi := \xi - \xi _d$ with the damping gain $k_v >0$. The closed-loop dynamics (4) of the S3Q system with (20) is then given by \begin{eqnarray} M \dot{e}_\xi + C e_\xi + k_v e_\xi +\nabla \Phi ^T = 0 \end{eqnarray} 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 Theorem 1. Here, the convergence is almost everywhere, i.e., except some unstable equilibriums, which, in fact, stems from the inherent topology of SO(3), yet it can rather easily be avoided in practice by running the operations close enough to the desired pose trajectory (see [30] and [31]).

Theorem 1:

Consider the dynamics of S3Q platform (4) with the control $U$ as designed in (20), which is assumed to satisfy the control feasibility condition of Proposition 2. Then, $\xi \rightarrow \xi _d$ and \begin{eqnarray*} (x^w_e,R_o) \rightarrow (x^w_d,R_d) \end{eqnarray*} asymptotically almost everywhere except unstable equilibriums $\mathcal {E} := \lbrace (x_e^w, R_o) \,|\, x_e^w = x_d^w, R_d^T R_o \in \lbrace {diag}[-1,-1,1], {diag}[-1,1,-1], {diag}[1, -1, -1] \rbrace \rbrace$.

Proof:

Define the Lyapunov function as \begin{equation*} V := \Phi + \frac{1}{2}e^T_\xi M e_\xi \end{equation*} which is a positive-definite function on SE(3) $\times$ se(3) with $V=0$ iff $(x_e^w, R_o, \xi) = (x_d^w, R_d, \xi _d)$. The derivative of $V$ along the closed-loop dynamics (21) is then given by \begin{eqnarray*} \dot{V} = \nabla \Phi e_\xi + e_\xi ^T (-C e_\xi -k_v e_\xi -\nabla \Phi ^T) = - k_v e_\xi ^T e_\xi \leq 0 \end{eqnarray*} where $C$ is skew symmetric from the passivity of the system (4), i.e., $\dot{M}-2C$ is symmetric with $\dot{M}=0$ (see after (5) for the definition of $C$). Integrating this, we can further obtain \begin{equation*} V(T)-V(0) = -k_v \int _o^T \Vert e_\xi \Vert ^2 dt \end{equation*} $\forall T \geq 0$. This then implies $e_\xi \in \mathcal {L}_\infty \cap {\mathcal L}_2$. We also have $\Phi (t) \leq V(t) \leq V(0), \forall t \geq 0$; thus, $\nabla \Phi ^T \in \mathcal {L}_\infty$ as well. From (21) with the assumption that $v^o_d, \omega ^o_d$ and their derivatives be bounded, we then have $\dot{e}_\xi \in \mathcal {L}_\infty$ . Using Barbalat's lemma [32], we can then conclude that $e_\xi \rightarrow 0$. Differentiating (21), we can also show that $\ddot{e}_\xi \in \mathcal {L}_\infty$, and applying Barbalat's lemma again, we have $\dot{e}_\xi \rightarrow 0$. Applying $(e_\xi, \dot{e}_\xi) \rightarrow 0$ to (21), we have $\nabla \Phi ^T = 0$. This then means that $e_x = 0$ and further \begin{equation*} (\eta R_d^T R_o - R_o^T R_d \eta)^\vee = 0 \end{equation*} which, following [30] and [31], suggests the almost global asymptotic stability of $R_o \rightarrow R_d$ as stated above. This completes the proof. $\blacksquare$

B. Optimal Control Allocation

To decode the designed control $U$ in (20) into the thrust vector of each quadrotor, we solve the constrained optimization (9)–​(12), which is a convex SOCP with affine equality constraint (10), convex inequality constraints (11) and (12), and convex object function (9). As explained in Section III-B, this problem is, in fact, similar to the robot grasping problem under the friction-cone constraint. To solve this optimization problem (9)–​(12) in real time, here, we employ the barrier method [27] to convert the problem to an unconstrained optimization problem. For this, we exploit the peculiar structure of (9)–​(12) to reduce the size of the problem, thereby significantly speeding up the computation time.

More specifically, we write the general solution of (10) s.t. \begin{eqnarray*} \Lambda := -B^T [B B^T]^{-1} U + Z \gamma \end{eqnarray*} where $B^T [B B^T]^{-1} \in \Re ^{9 \times 6}$ is the Moore–Penrose pseudoinverse of $B(d)$ in (10), $Z \approx {null}[B(d)] \in \Re ^{9 \times 3}$ (i.e., identifies the null space of $B(d)$), and $\gamma \in \Re ^{3}$ is associated with the internal force term. Utilizing the structure of $B(d)$ in (7), we can write this general solution of (10) for each thrust vector $\Lambda _i$ s.t. \begin{equation} \bar{\Lambda }_i(\gamma) = \Lambda _i^{\dagger } + Z_i \gamma \end{equation} where \begin{equation*} \Lambda _i^{\dagger }:=-[I\,-S(d_i)] (BB^T)^{-1}U \in \Re ^3 \end{equation*} and $Z=: [Z_1; Z_2; Z_3]$ with $Z_i \in \Re ^{3 \times 3}$. Here, note that the only unknown is $\gamma \in \Re ^3$. We can then reduce the size of the original optimization problem (9)–​(12) by writing that with this $\gamma \in \Re ^{3}$ as the new optimization variable s.t. \begin{equation} \begin{aligned} \gamma _\text{op} = \underset{\gamma \in \Re ^3}{\arg \min } \, \frac{1}{2}\sum _{i=1}^3 \bar{\Lambda }_i^T(\gamma)\bar{\Lambda }_i(\gamma) \qquad \qquad \end{aligned} \end{equation} subject to \begin{eqnarray} &{\mathcal C}_{1,i}:= \bar{\Lambda }_i^T(\gamma)(\cos ^2 \phi _i I - p_ip_i^T) \bar{\Lambda }_i(\gamma) \leq 0 \\ &{\mathcal C}_{2,i}:=\bar{\Lambda }_i^T(\gamma) \bar{\Lambda }_i(\gamma) -\bar{\lambda }_i^2 \leq 0. \end{eqnarray}

Following the procedure of the barrier method [27], we then convert (23)–​(25) into the following unconstrained optimization: \begin{equation} \gamma _\text{op} = \underset{\gamma \in \Re ^3}{\arg \min } \, \sum _{i=1}^3 \left[ s \Pi _i (\gamma) + \Xi _i (\gamma) \right] \end{equation} where $\Pi _i (\gamma) := \frac{1}{2} \bar{\Lambda }_i^T(\gamma)\bar{\Lambda }_i(\gamma)$ is from (23) and $\Xi _i (\gamma) := -\log (-{\mathcal C}_{1,i}(\gamma)) - \log (-{\mathcal C}_{1,i}(\gamma))$ is a logarithmic barrier function to enforce constraints (24) and (25), and $s \in \Re$ 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 (26) by using the Newton iteration for increasing value of $s$ until $s>\frac{2n}{\epsilon }$, where $n$ is the number of quadrotors and $\epsilon >0$ is the desired tolerance. The solution of (26) is known to be the $\frac{2n}{s}$-suboptimal solution of (23)–​(25) with $n=3$ for the S3Q system [27].

We implement and run this optimization-solving algorithm in real time for our S3Q platform system, as experimented in Section VI. 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 \times 3$. This greatly helps to speed up our solving the optimization problem (26). It is also well known that the convergence of the Newton iteration is quadratic and guaranteed within the bounded number of iterations [27]. For our experiment in Section VI, 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 [27], the total control allocation algorithm can run with around 500 Hz (on a PC with Intel i7 3.2 GHz CPU), which is proved to be adequate for the experiment (see Section VI).

SECTION V. Control of the S2Q Platform System

In this section, we consider the motion control problem of the S2Q platform system as shown in Fig. 3 with (17). As elucidated in Section III-A, this S2Q platform system is (frame) underactuated with only five-DOF actuation possible for its frame motion in SE(3) [see (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. 3. The tracking control of this tool-tip position and direction in $\Re^3 \,\times$ S$^2$ of the S2Q platform in Fig. 3 is the topic of this section. As compared to the case of the S3Q platform tracking control in SE(3) of Section IV, this tracking control of the S2Q platform in $\Re ^3\, \times$ S$^2$ is more complicated due to the underactuation.

For this, similar to (18), we denote the tool-tip position by $x_e$. We also use the line connecting the CoM positions of two quadrotors as the pointing direction of the tool (see Fig. 3 ). We denote this line by $r_e \in$ S $^2$, which can be expressed in $\lbrace \mathcal {F}_W\rbrace$ and in $\lbrace \mathcal {F}_0\rbrace$ by \begin{eqnarray} r^w_e =R_o r_e^o, \,\,\,\, r_e^o = (d_2 - d_1)/{\Vert d_2- d_1 \Vert } \end{eqnarray} where $r^o_e$ is a unit constant vector. The tracking control objective for the underactuated S2Q platform can then be written by \begin{eqnarray} (x_e^w(t), r_e^w(t)) \rightarrow (x_d^w(t),r_d^w(t)) \end{eqnarray} where $(x^w_d(t), r^w_d(t)) \in$ $\Re ^3\times$S$^2$ are the timed trajectory of the desired tool-tip position and its pointing direction expressed in $\lbrace {\mathcal F}_W \rbrace$ . Here, we construct $r^w_d(t) \in$ S$^2$ by defining $R_d(t) \in$ SO(3) s.t. \begin{eqnarray} r^w_d(t):= R_d(t) r^o_e. \end{eqnarray} Note that, given $r^w_d$ and $r^o_e$, this $R_d(t)$ is not unique. The time derivatives of these $r^w_e$ and $r^w_d$ are then computed by \begin{eqnarray} \dot{r}_e^w = R_oS(\omega _o)r_e^o,\,\,\,\dot{r}^w_d = R_d S(w_d) r^o_e \end{eqnarray} where $w_d \in \mathfrak {so}(3)$ is the angular rate of the coordinate frame $\lbrace {\mathcal F}_D \rbrace$ specified by $R_d \in$ SO(3) and expressed in $\lbrace {\mathcal F}_D \rbrace$ as defined in (19). Recall that $\omega _o \in \mathfrak {so}(3)$ is also the body angular velocity of $\lbrace {\mathcal F}_0 \rbrace$.

Our goal here is then to design $U \in \Re ^6$ in (4) to achieve this tracking objective in $\Re ^3\;\times$S$^2$ under the underactuation limitation (15) . The physical constraints (10)–​(12) are assumed to be satisfied by designing $(x_d^w(t),r_d^w(t))$ in such a way to enforce control feasibility of Proposition 2. Due to the underactuation, the S2Q platform dynamics can split into: 1) fully actuated five-DOF dynamics, which will be controlled to attain the tracking objective of (28); and 2) unactuated one-DOF dynamics, which constitutes internal dynamics (with $(x_e^w(t), r_e^w(t))$ as five-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 [33] as detailed in the following.

A. Passive Decomposition of S2Q Platform Dynamics

Recall from (7) that, for the S2Q platform in Fig. 3, we have \begin{eqnarray*} {B}(d):= {\left[\begin{array}{cc} I & {I} \\ S(d_1) & S(d_2) \end{array}\right]} \in \Re ^{6 \times {6}} \end{eqnarray*} with ${rank}[B]=5$ as shown in (15). Following [33], at each configuration $q = (x_c^w, R_o)$ of the S2Q platform, we can then decompose the tangent (or velocity) space $T_q\mathcal {M} \approx \Re ^6$ and the cotangent (or wrench) space $T_q^*\mathcal {M} \approx \Re ^6$ s.t. \begin{eqnarray*} T_q\mathcal {M} = \Delta _a \oplus \Delta _u, \quad T_q^*\mathcal {M} = \Omega _a \oplus \Omega _u \end{eqnarray*} or, in coordinates, \begin{eqnarray} {\left[\begin{array}{c}v_c \\ {\omega }_o \end{array}\right]} = \underbrace{ {\left[\begin{array}{cc} \Delta _a & \Delta _u \end{array}\right]} }_{=:\Delta \in \Re ^{6 \times 6}} {\left[\begin{array}{c} \nu _a \\ \nu _u \end{array}\right]}, \quad U = \underbrace{ {\left[\begin{array}{cc} \Omega _a^T & \Omega _u^T \end{array}\right]} }_{=:\Omega ^T \in \Re ^{6 \times 6}} {\left[\begin{array}{c} u_a \\ u_u \end{array}\right]} \end{eqnarray} where $\Omega ^T_a \approx {span}[B] \in \Re ^{6 \times 5}$ denotes the fully actuated direction, $\Delta _u \approx {null} [B^T] \in \Re ^{6 \times 1}$ denotes the velocity space of the unactuated dynamics, $\Delta _a = M^{-1}\Omega _a^T (\Omega _aM^{-1}\Omega _a^T)^{-1} \in \Re ^{6 \times 5}$ denotes the orthogonal complement of $\Delta _u$ with respect to the $M$-metric (i.e., $\Delta _a^T M \Delta _u = 0$), and $\Omega ^T_u = M \Delta _u(\Delta _u^T M \Delta _u)^{-1} \in \Re ^{6 \times 1}$ denotes the control space of the unactuated dynamics with the $M$-orthogonality. Here, $\nu _a, u_a \in \Re ^5$ and $\nu _u, u_u \in \Re$ are, respectively, the velocity components and transformed control of the S2Q system in the fully actuated and unactuated spaces, respectively, with $u_u=0$ from $U \in {span}[B]$. Note also that the following properties hold for (31): $\Delta _a^T M \Delta _u = 0_{5 \times 1}$ (orthogonal with respect to the $M$-metric); $\Omega \Delta = I_{6\times 6}$; and $\Delta _a^T U = u_a$ and $\Delta _u^T U = 0$.

The coordinate expression of this decomposition is not unique, and, for that, here, following (16), we choose \begin{eqnarray} \Delta _u = {\left(\begin{array}{c} S(d_1) d_2 \\ d_2 - d_1 \end{array}\right)} \approx {null}[B^T] \end{eqnarray} which, as explained after (16), represents pure rotation motion of the S2Q platform of Fig. 3 along the tool-tip axis. Then, differentiating (31) with the above $\Delta _u$ and substituting them into (4), we obtain \begin{eqnarray} &M_a \dot{\nu }_a + C_a \nu _a + C_{au}\nu _u + g_a = u_a + f_a \\ &M_u \dot{\nu }_u + C_u \nu _u + C_{ua} \nu _a + g_u = u_u + f_u \end{eqnarray} where $M_a := \Delta _a^T M \Delta _a$, $M_u := \Delta _u^T M \Delta _u$, \begin{eqnarray*} {\left[\begin{array}{cc}C_a & C_{au} \\ C_{ua} & C_u \end{array}\right]} := {\left[\begin{array}{cc}\Delta _a^T C \Delta _a & \Delta _a^T C \Delta _u \\ \Delta _u^T C \Delta _a & \Delta _u^T C \Delta _u \end{array}\right]} \end{eqnarray*} 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 (4) computed similarly to $(u_a,u_u)$ in (31) with $u_u=0$ (i.e., $\nu _u$-dynamics (16) is unactuated).

Note that the passive decomposition decomposes the S2Q platform dynamics (4) into the fully actuated $\nu _a$ -dynamics on $\Delta _a$ with $u_a \in \Re ^5$ and the unactuated $\nu _u$-dynamics on $\Delta _u$ with $u_u=0$. This is done while preserving the Lagrangian structure and passivity of the original dynamics (4), i.e., constant symmetric and positive-definite $M_a \in \Re ^{5 \times 5}$ with skew-symmetric $C_a$; constant $M_u >0$ with $C_u=0$ ; and $C_{au} + C^T_{ua} =0$. For more details on passive decomposition, refer to [33] and references therein. These decomposed dynamics (33) and (34) then serve the basis for our control design and internal stability analysis as in the following.

B. Lyapunov-Based Control Design and Internal Stability

Here, we design the control $U \in \Re ^6$ in (4) or, equivalently, $u_a \in \Re ^5$ in (33), for the S2Q platform as shown in Fig. 3 with (17) to achieve the trajectory tracking in $\Re ^3\,\times$ S $^2$, as stated in (28). First, similar to the case of S3Q platform control in Section IV-A, we define a potential function on $\Re ^3\,\times$ S $^2$ s.t. \begin{eqnarray*} \Phi = \frac{1}{2} k_x e_x^T e_x + k_r \left[ 1 - (r^w_d)^T r^w_e \right] \end{eqnarray*} where $e_x := x_e^w - x_d^w \in \Re ^3$ expressed in $\lbrace {\mathcal F}_W \rbrace$, $r^w_e, r^w_d \in$S $^2$ are the real and desired pointing direction of the tool as defined in (27) and (29), and $k_x, k_r >0$ are the gains. Then, we can see that $\Phi \geq 0$ with the equality hold iff $(x_e^w, r_e^w) = (x_d^w, r_d^w)$.

We can then compute the time derivative of $\Phi$ s.t. \begin{align*} \dot{\Phi } & = k_x e_x^T (\dot{x}_e^w - \dot{x}_d^w) - k_r (r_e^o)^T \frac{d}{dt}(R_d^T R_o) r_e^o \\ &= k_x e_x^T \left[ R_o (v_c - S(d_e) \omega _o) - \dot{x}_d^w \right] - k_r (r_e^o)^T R_d^T R_o S(e_\omega) r_e^o \\ &= k_x e_x^T R_o\left[ v_c - S(d_e)e_\omega - S(d_e)R_o^T R_d \omega _d - R_o^T R_d v_d \right] \\ & \,\,\,\,\,\,+ k_r (r_e^o)^T R_d^T R_o S(r_e^o) e_\omega \end{align*} where we use (18), (19), $\frac{d}{dt}(R_d^T R_o) =R_d^T R_o S(\omega _o - R_o^T R_d \omega _d)$, and $e_\omega := \omega _o - R_o^T R_d \omega _d$, which is the angular rate error expressed in $\lbrace {\mathcal F}_0 \rbrace$. Rearranging this, we can further write \begin{eqnarray} \dot{\Phi } = {\left(\begin{array}{c}k_x R_o^T e_x \\ k_r R_o^T r_d^w \end{array}\right)}^T \underbrace{ {\left[\begin{array}{cc} I & -S(d_e) \\ 0 & -S(r_e^o) \end{array}\right]}}_{=: \bar{S} \in \Re ^{6 \times 6}} (\xi -\xi _d) \end{eqnarray} where $\xi = [v_c; \omega _o]$ and \begin{eqnarray*} \xi _d := {\left(\begin{array}{c} S(d_e) R_o^T R_d \omega _d + R_o^TR_d v_d \\ R_o^T R_d \omega _d \end{array}\right)} \in \Re ^6 \end{eqnarray*} i.e., the desired translation and rotation velocities of the total system CoM expressed in $\lbrace {\mathcal F}_O \rbrace$. This derivative of the potential $\Phi$ depends only on the evolution of the S2Q platform dynamics in $\Delta _a$ (33), as can be seen by \begin{eqnarray*} \bar{S} \cdot \Delta _u = {\left(\begin{array}{c}S(d_1)d_2 - S(d_e)(d_2 - d_1) \\ -S(r_e^o) (d_2 - d_1) \end{array}\right)} =0 \end{eqnarray*} where we use $d_e := k(d_2 - d_1) + d_1$ from (17) for some constant $k \in \Re$.

Define $\nu _{a_d} \in \Re ^5$ and $\nu _{u_d} \in \Re$ s.t. \begin{eqnarray*} \xi _d =: {\left[\begin{array}{cc} \Delta _a &\Delta _u \end{array}\right]} {\left(\begin{array}{c}\nu _{a_d} \\ \nu _{u_d} \end{array}\right)} \end{eqnarray*} with which, we can write \begin{eqnarray} \dot{\Phi } = \nabla \Phi \cdot (\nu _a - \nu _{a_d}) \end{eqnarray} where \begin{eqnarray*} \nabla \Phi := {\left(\begin{array}{c}k_x R_o^T e_x \\ k_r R_o^T r_d^w \end{array}\right)}^T \cdot \bar{S} \cdot \Delta _a \in \Re ^{1 \times 5} \end{eqnarray*} We then design the control $u_a \in \Re ^5$ in (33) s.t. \begin{align} u_a =& C_{au} \nu _u + g_a- f_a \nonumber\\ &\,\,\, + M_a \dot{\nu }_{a_d} + C_a \nu _{a_d} - k_v (\nu _a - \nu _{a_d})- \nabla \Phi ^T \end{align} with which the closed-loop $\nu _a$-dynamics (33) becomes \begin{eqnarray} M_a \dot{e}_a + C_a e_a+ k_\nu e_a + \nabla \Phi ^T = 0 \end{eqnarray} with $e_a := \nu _a - \nu _{a_d}$ and $k_v >0$.

Theorem 2:

Consider the dynamics (4) of the S2Q platform system of Fig. 3 with the geometric condition (17) and the tracking control (37), which is assumed to satisfy the control feasibility of Proposition 2. Define \begin{eqnarray} V := \Phi + \frac{1}{2} e_a^T M_a e_a \end{eqnarray} and suppose that $V(0) < 2 k_r$. Then \begin{eqnarray*} \nu _a \rightarrow \nu _{a_d}, \,\,\, (x_e^w(t), r_e^w(t)) \rightarrow (x_d^w(t), r_d^w(t)). \end{eqnarray*}

Proof:

Here, we use $V$ in (39) as a Lyapunov function, which is positive definite with $V = 0$ iff $(x_e^w, r_e^w, \nu _a) = (x_d^w, r_d^w, \nu _{a_d})$. Differentiating $V$ along (38), we obtain \begin{eqnarray*} \dot{V} = \nabla \Phi \cdot e_a + e_a^T [ - C_a e_a -k_v e_a -\nabla \Phi ^T] = - k_v e_a^T e_a \leq 0 \end{eqnarray*} where $C_a$ is skew symmetric from the (preserved) passivity through the passive decomposition (see Section V-A). Integrating this, we can then attain \begin{eqnarray} V(T)-V(0) = -k_v\int _o^T \Vert e_a \Vert ^2 dt \leq 0 \end{eqnarray} $\forall T \geq 0$. This implies that $e_a \in \mathcal {L}_\infty \cap \mathcal {L}_2$ . Also, since $\Phi (t) \leq V(t) \leq V(0), \forall t \geq 0$, $\nabla \Phi \in \mathcal {L}_\infty$. From (38) with $v_d, \omega _d$ and their derivatives being bounded, this then means that $\dot{e}_a \in \mathcal {L}_\infty$, implying $e_a \rightarrow 0$ due to Barbalat's lemma [32]. Differentiating (38), we can also show that $\ddot{e}_a \in \mathcal {L}_\infty$, implying $\dot{e}_a \rightarrow 0$ again due to Barbalat's lemma. Applying $(e_a,\dot{e}_a) \rightarrow 0$ to (38), we then have $\nabla \Phi ^T \rightarrow 0$. Since $\Delta _a \in \Re ^{5 \times 5}$ is nonsingular, $\nabla \Phi ^T \rightarrow 0$ then means that \begin{eqnarray*} \bar{S}^T {\left(\begin{array}{c}k_x R_o^T e_x \\ k_r R_o^T r_d^w \end{array}\right)} = {\left[\begin{array}{cc}I & 0 \\ S(d_e) & S(r_e^o) \end{array}\right]} {\left(\begin{array}{c} k_x R_o^T e_x \\ k_r R_o^T r_d^w \end{array}\right)} \rightarrow 0 \end{eqnarray*} where we have $e_x \rightarrow 0$ from the first row, whereas, from the second row, $S(r_e^o) R_o^T r_d^w = R^T_o S(r^w_e) r^w_d \rightarrow 0$, implying that $r^w_e \rightarrow r^w_d$ or $r^w_e \rightarrow - r^w_d$. Here, $r^w_e \rightarrow - r^w_d$ is impossible, since, from (40) with $V(0) <2 k_r$ as assumed above, we have $\forall t \geq 0$ , \begin{eqnarray*} k_r[1-(r^w_d(t))^T r^w_e(t)] \leq V(t) \leq V(0) < 2 k_r \end{eqnarray*} where $k_r[1-(r^w_d(t))^T r^w_e(t)]$ attains its maximum $2 k_r$ only when $r^w_e \rightarrow - r^w_d$, implying that only $r^w_e \rightarrow r^w_d$ is possible. $\blacksquare$

The condition of $V(0) < 2 k_r$ of Theorem 2 is to ensure that the initial error $e_a(0),e_x(0)$ is small enough with $r^w_e(0)$ far enough from $-r^w_d(0)$ so that the attitude of the S2Q system always converges to the desired equilibrium (i.e., $r^w_e \rightarrow r^w_d$ ). 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 Theorem 2 proves that the tracking objective (28) in $\Re ^3\,\times$ S$^2$ is attained with the control (37), it only provides a partial verdict for its practical applicability, as the unactuated dynamics (34), which, in fact, constitutes the internal dynamics with the five-DOF output $(x_e, r_e)$, may become unstable under the control (37).

This internal dynamics is given by the unactuated $\nu _u$-dynamics (34), which we rewrite here in a more compact form \begin{eqnarray} M_u \dot{\nu }_u + C_{ua} \nu _a + g_u = f_u \end{eqnarray} where \begin{eqnarray*} g_u = \Delta ^T_u G= -\left[ \bar{m} g R^T_o e_3 \right]^T S(d_1) d_2 \end{eqnarray*} from (31) with $\bar{m}:= \sum _{i=0}^2 m_i$, which is the moment generated by the gravity about the $N^E$-axis of the tool-tip frame $\lbrace {\mathcal F}_E \rbrace$ . Furthermore, as captured by (16) and explained there, the motion of this $\nu _u$-dynamics, i.e., $\Delta _u \nu _u$, is the pure rotation motion about the same $N^E$-axis with its magnitude given by $||d_2-d_1|| \cdot || \nu _u ||$.

To facilitate the stability analysis of this internal dynamics (41) , we parameterize the rotation matrix $R_o$ by the yaw, pitch, and roll angles $[\phi ; \theta ; \psi ] \in \Re ^3$ s.t. \begin{eqnarray*} R_o = {\left[\begin{array}{ccc} {c}\phi {c}\theta & -{s}\phi {c}\psi + {c}\phi {s}\theta {s}\psi & {s}\phi {s}\psi + {c}\phi {s}\theta {c}\psi \\ {s}\phi {c}\theta & {c}\phi {c}\psi + {s}\phi {s}\theta {s}\psi & - {c}\phi {s}\psi + {s}\phi {s}\theta {c}\psi \\ -{s}\theta & {c}\theta {s}\psi & {c}\theta {c}\psi \end{array}\right]} \end{eqnarray*} with the following differential kinematics: \begin{eqnarray} {\left(\begin{array}{c}\dot{\phi } \\ \dot{\theta } \\ \dot{\psi } \end{array}\right)} = \frac{1}{ {c}\theta } {\left[\begin{array}{ccc} 0 & {s}\psi & {c}\psi \\ 0 & {c}\theta {c}\psi & -{c}\theta {s}\psi \\ {c}\theta & {s}\theta {s}\psi & {s}\theta {c}\psi \end{array}\right]} \omega _o \end{eqnarray} and \begin{eqnarray*} r_e^w = R_o e_1 = {\left[\begin{array}{ccc} {c}\phi {c}\theta & {s}\phi {c}\theta & - {s}\theta \end{array}\right]}^T \end{eqnarray*} where $\vert \theta \vert < \pi /2$ and ${s}\star = \sin \star$, ${c}\star = \cos \star$. Note here that $r^w_e$ is a function of only $(\phi,\theta)$.

Following the explanation above on $\Delta _u \nu _u$ with (16), we have $\omega _o = [\Delta _u \nu _u; 0;0] + A \nu _a$, and injecting this into (42), we can write \begin{eqnarray*} \dot{\psi }= ||d_2-d_1|| \cdot \nu _u + A \nu _a \end{eqnarray*} where $A \in \Re ^{6 \times 5}$ is a function of the configuration $(x_e, R_o)$ . Incorporating this $\dot{\psi }$ into (41) while also writing $g_u$ with $[\phi,\theta,\psi ]$, we can then write the internal dynamics (41) s.t. \begin{eqnarray} m_u \ddot{\psi }+ \bar{m}g & ||d_1 \times d_2|| {c}\theta {s}\phi \\ & = m_u \cdot [ \nabla A(\nu _a,\nu _u) \nu _a + A \dot{\nu }_a] - C_{ua} \nu _a + f_u \nonumber \end{eqnarray} where $m_u := M_u/||d_2-d_1|| >0$ and $\nabla A(\xi)$ and $C_{ua}(\xi)$ are linear in $\nu _a$ and $\nu _u$ . We can then see that the internal dynamics (43) is indeed similar to the downward pendulum dynamics, suggesting that, if the perturbation (i.e., $\nu _a, \dot{\nu }_a, f_u$) is small enough, it would be stable with bounded $\psi, \dot{\psi }$ as formalized in Lemma 1.

Lemma 1:

Consider the internal dynamics (43) and assume the following: 1) the tracking control $u_a$ (37) is designed slow enough with $||\nu _a||, ||\dot{\nu }_a||$ bounded; 2) $f_u \in \Re$ is bounded; and 3) there is unmodeled/physical damping $- b_u \dot{\psi }$ for (43). Then, there always exists small enough $\bar{\nu }_a \geq 0$, $||\nu _a(t)|| \leq \bar{\nu }_a$, $\forall t \geq 0$, such that $(\psi (t), \dot{\psi }(t))$ is bounded $\forall t \geq 0$.

Proof:

Incorporating the physical/unmodeled damping $b_u$ and also linearizing the internal dynamics (43) around $(\psi, \dot{\psi })=0$, we can obtain \begin{eqnarray*} m_u \ddot{\psi }+ b_u \dot{\psi }+ k_u {c}\theta \cdot \psi = g_1(\nu _a,\nu _u) \nu _a + g_2 \dot{\nu }_a + f_u \end{eqnarray*} where $k_u:= \bar{m} g ||d_1 \times d_2||$, $g_1 \in \Re ^{1 \times 5}$ is linear in its arguments, $g_2 \in \Re ^{1 \times 5}$ is bounded, and the terms on right-hand side (RHS) are all bounded, if so is $\nu _u$.

Define the following Lyapunov function: \begin{eqnarray*} V_u := \frac{1}{2} m_u \dot{\psi }^2 + \epsilon \psi \dot{\psi }+ \frac{1}{2} {c}\theta \psi ^2 \end{eqnarray*} where $\epsilon >0$ is a cross-coupling term [11] chosen s.t. (i) $\epsilon < \sqrt{m_u {c}\theta }$ to ensure $V_u$ be positive definite. Differentiating this $V_u$ along the above internal dynamics, we can then obtain \begin{eqnarray*} \dot{V}_u & = {\left(\begin{array}{c}\dot{\psi }\\ \psi \end{array}\right)}^T \left[ \begin{array}{cc} b_u - \epsilon & \frac{1}{2} b_u \epsilon \\ \frac{1}{2} b_u \epsilon & \epsilon {c}\theta + \frac{1}{2} {s}\theta \cdot \dot{\theta }\end{array} \right] {\left(\begin{array}{c}\dot{\psi }\\ \psi \end{array}\right)} \\ & \,\,\,\,\,\, + (g_1(\nu _a,\nu _u)\nu _a + g_2 \dot{\nu }_a +f_u) \cdot (\dot{\psi }+ \epsilon \phi) \end{eqnarray*} where the first term on the RHS will be negative definite if (ii) $b_u -\epsilon >0$; (iii) $\epsilon > \frac{1}{2} |\tan \theta | \cdot |\dot{\theta }|$ ; and (iv) $(b_u -\epsilon) (\epsilon {c}\theta + \frac{1}{2} {s}\theta \dot{\theta }) > \frac{1}{4} b^2_u \epsilon ^2$. The solution $\epsilon$ of the inequalities (i)–(iii) is then given by \begin{align*} \frac{1}{2} |\tan \theta | \cdot |\dot{\theta }| < \epsilon < \min (\sqrt{m_u {c}\theta }, b_u) \end{align*} which will always have an intersection with the solution of (iv), i.e., $\epsilon \geq 0$, s.t. \begin{align*} \left(\frac{1}{4} b^2_u+{c}\theta \right) \epsilon ^2 - (b_u {c}\theta - \eta) \epsilon + b_u \eta <0 \end{align*} as $|\dot{\theta }| \leq \bar{\nu }_a \rightarrow 0$ , where $\eta :=\frac{1}{2}{s}\theta \cdot \dot{\theta }\rightarrow 0$ as well with $\bar{\nu }_a \rightarrow 0$. This then means that, with small enough $\bar{\nu }_a$, $V_u$ will always be positive definite with $\dot{V}_u \leq -\alpha _1 V_u + \alpha _2 \sqrt{V_u}$ for some $\alpha _1, \alpha _2 >0$, implying the boundedness of $(\psi (t),\dot{\psi }(t))$ $\forall t \geq 0$. $\blacksquare$

Note that the physical/unmodeled damping $b_u$ assumed in Lemma 1, in general, exists in the real systems (e.g., aerodynamic dissipation). This physical damping would also perturb the tracking control objective of Theorem 2, whose effect yet can be adequately reduced by increasing the feedback gains $k_\nu, k_x, \text{ and }k_r$ of (37) during our experimentations (see Section VI). The assumption of $f_u$ being bounded would also be likely granted in practice for the S2Q platform system of Fig. 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.

C. Optimal Control Allocation and Closed-Form Expression

Given the desired control wrench $U = \Omega _a^T u_a$ in (31) with $u_a$ in (37) and $u_u=0$, the next task is to allocate this $U=:[u_f,u_\tau ]$ into the thrust of each quadrotor $\Lambda ^o_i$ of the S2Q platform of Fig. 3 via the constrained optimization (9)–​(12) . Existence of the solution of this allocation problem is assumed by designing the task to be control feasible, as stated in Section III-B. Furthermore, as shown in the following, for the S2Q platform system of Fig. 3, we can, in fact, find a closed-form expression of the optimization problem (9)–​(12), eliminating the necessity of relying on iterative procedures as for the S3Q platform system in Section IV-B.

For this, similar to Section IV-B, we reduce the size of the optimization problem (9)–​(12) by utilizing the control generation equation (10), which can be rewritten as \begin{eqnarray} {\left[\begin{array}{cc} I & I \\ 0 & S(d_2 - d_1) \end{array}\right]} \Lambda = {\left(\begin{array}{c}u_f \\ -S(d_1)u_f + u_\tau \end{array}\right)} \end{eqnarray} by multiplying both sides of (10) with \begin{equation*} {\left[\begin{array}{cc}I & 0\\ -S(d_1) & I \end{array}\right]}. \end{equation*} This matrix is the transpose of the adjoint operator, mapping $U=(u_f,u_\tau)$ expressed in $\lbrace {\mathcal F}_O \rbrace$ attached at $x_c$ to its equivalent wrench expressed in $\lbrace {\mathcal F}_1 \rbrace$ attached at $x_1$ with the same attitude as $\lbrace {\mathcal F}_O \rbrace$ . Note that this adjoint operator also appears in (16).

Denote the general solution of (44) by $\bar{\Lambda } = [\bar{\Lambda }_1;\bar{\Lambda }_2]$ with $\bar{\Lambda }_i:= [\bar{\lambda }_{i_1}; \bar{\lambda }_{i_2}; \bar{\lambda }_{i_3}] \in \Re ^3$. We can then easily determine $\bar{\lambda }_{2_2}$ and $\bar{\lambda }_{2_3}$ from (44) s.t. \begin{eqnarray*} &&\bar{\lambda }_{2_2} = \frac{1}{\Vert d_2 - d_1 \Vert } e_3 ^T [-S(d_1)u_f + u_\tau ]\\ &&\bar{\lambda }_{2_3} = - \frac{1}{\Vert d_2 - d_1 \Vert } e_2 ^T[- S(d_1)u_f + u_\tau ] \end{eqnarray*} and, further, compute $\bar{\lambda }_{1_2}, \bar{\lambda }_{1_3}$ s.t. \begin{eqnarray*} \bar{\lambda }_{1_2} = e_2^T u_f - \bar{\lambda }_{2_2}, \,\,\, \bar{\lambda }_{1_3} = e_3^T u_f - \bar{\lambda }_{2_3}. \end{eqnarray*} Recall from (15) that the rank of the first matrix on the left-hand side of (44) is 5. This then means that the solution of (44) assumes 1-D null space, which can be obtained by using the first row of (44), i.e., \begin{eqnarray*} &&\bar{\lambda }_{1_1} = \frac{1}{2} u_{f_1} + \gamma,\,\, \bar{\lambda }_{2_1} = \frac{1}{2} u_{f_1} - \gamma \end{eqnarray*} where $u_f=:[u_{f_1};u_{f_2};u_{f_3}] \in \Re ^3$ and $\gamma \in \Re$ identifies the null space.

Substituting these $\bar{\Lambda }_1, \bar{\Lambda }_2 \in \Re ^3$ into (9)–​(12), we can obtain the reduced form of the constrained optimization problem with respect to $\gamma \in \Re$ s.t. \begin{eqnarray} \gamma _\text{op} = \underset{\gamma \in \Re }{\arg \min } \, \gamma ^2 + \frac{1}{2} \left[ u^2_{f_1} + \sum _{i=1}^2(\bar{\lambda }_{i_2}^2 + \bar{\lambda }_{i_3}^2) \right] \end{eqnarray} subject to \begin{eqnarray*} & -a_1 \leq \frac{1}{2} u_{f_1} + \gamma \leq a_1, \quad -b_1 \leq \frac{1}{2} u_{f_1} + \gamma \leq b_1 \\ & -a_2 \leq \frac{1}{2} u_{f_1} - \gamma \leq a_2, \quad -b_2 \leq \frac{1}{2} u_{f_1} - \gamma \leq b_2 \end{eqnarray*} where \begin{eqnarray*} a_i := \sqrt{ \frac{1 - \cos ^2 \phi _i}{\cos ^2 \phi _i} \bar{\lambda }_{i_3}^2 - \bar{\lambda }_{i_2}^2}, \,\,\, b_i := \sqrt{\bar{\lambda }_i^2 - \bar{\lambda }_{i_2}^2 - \bar{\lambda }_{i_3}^2} \end{eqnarray*} are to be positive constants when the control feasibility condition of Proposition 2 is enforced by the task design.

Define $c_i := \min (a_i,b_i)$. Note also that the last term on the RHS of (45) is a constant and, thus, can be simply dropped off. We can then think of the geometry of this reduced constrained optimization problem with respect to $\gamma$, as illustrated in Fig. 5, where the anchoring point $(\bar{\lambda }_{1_1},\bar{\lambda }_{2_1}) =\frac{1}{2}(u_{f_1},u_{f_1})$ is transversing according to a given $u_{f_1}$ and a solution exists when the slant line intersects with the shaded feasible region. From Fig. 5, we can obtain the following closed-form solution: \begin{equation*} \gamma _\text{op} = \left\{ \begin{array} {lll} 0, & \text{if }\;\; \frac{1}{2}|u_{f_1}| \leq \text{min}(c_1,c_2) \\ \frac{1}{2}u_{f_1} - c_2, & \text{if } \;\; \frac{1}{2}|u_{f_1}| > \text{min}(c_1,c_2) \, \text{and} \,c_1 \geq c_2\\ -\frac{1}{2}u_{f_1} + c_1, & \text{if }\;\; \frac{1}{2}|u_{f_1}| > \text{min}(c_1,c_2) \, \text{and} \,c_1<c_2. \end{array} \right. \end{equation*} No solution $\gamma _\text{op}$ exists when $|u_{f_1}| > |c_1 + c_2|$, which, however, is ruled out here by enforcing the control feasibility of Proposition 2.

Fig. 5.

Geometry of the reduced optimization problem (45) with transversing anchoring point $(\frac{1}{2}u_{f_1},\frac{1}{2}u_{f_1})$ and the shaded feasible region: (1) solution exists with nonzero $\gamma _\text{op}$; (2) solution exists with $\gamma _\text{op}=0$; and (3) no solution exists for $\gamma _\text{op}$.

SECTION VI. Experimental Results

A. Setup

We build the prototypes of the S3Q platform and the 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 is shown in Figs. 1 and 3 with the following geometric design parameter: $\Vert d_i - d_j \Vert = 1$ [m] and $(d_i- d_j)^T e_3 =0$. The spherical joints allow the range of angle motion $\phi _i = {\text{32}}^\circ$ 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 CoM (see Fig. 3) to satisfy the assumption in Section II as much as we can. Some important specifications of these prototypes are given in Table I.

TABLE I Parameters and Specifications of S2Q Platform and S3Q Platform Prototypes

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) to measure the attitude of quadrotors and the position and 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 $\Lambda _i$ given in Sections IV and V into each rotor command of the quadrotors. There are many techniques available to control the quadrotors to track that desired $\Lambda _i$. For the S2Q platform, we use the backstepping control [34] to provide a good performance, since we have the explicit expression of the thrust $\Lambda _i$ and its derivative for the S2Q platform. For the S3Q system, where the thrust $\Lambda _i$ are determined numerically, we employ a method that computes the quadrotor commands directly from the thrust ${\Lambda }_i$ (see the Appendix ).

B. Control of the S3Q Platform Prototype

To evaluate the performance of the prototype, we first perform the hovering experiment. The results are shown in Fig. 6. For comparison, we also include the hovering results of a single AscTec Hummingbird quadrotor. From Figs. 6 and 7, we can see that the performance of the S3Q platform (i.e., the RMS error of 2.3 cm in position and 2.2$^\circ$ in attitude), despite its high system complexity, is still comparable to that of the well-built AscTec Hummingbird quadrotor (i.e., the RMS error of 1.4 cm and 0.9$^\circ$).

Fig. 6.

S3Q platform hovering with RMS errors of 2.3 cm in position and 2.2$^\circ$ in attitude.

Fig. 7.

Quadrotor hovering with RMS errors of 1.4 cm in position and 0.9 $^\circ$ in attitude.

We now consider the motion tracking control of the S3Q platform, which requires the CoM of the frame $x_e^w = x_o^w$ to track a horizontal rectangular shape while maintaining the platform hovering posture (i.e., $R_{d} \approx I_{3 \times 3}$). The desired motion is designed to be feasible according to the task planning procedure in Section III-B (see Fig. 4). The results are shown in Fig. 8(a), where we see that the frame CoM tracks the desired motion with the RMS position error of $\text{4.6}$ cm while maintaining the hovering attitude with the RMS angle error of 4.2 $^\circ$.

Fig. 8.

Experiments of the S3Q platform prototype. (a) Trajectory tracking: CoM of the S3Q frame tracking a horizontal rectangular shape while maintaining hovering posture (i.e., $R_{o} \approx I_{3 \times 3}$). (b) Stable interaction: position and attitude of the S3Q frame response to the external wrench applied to the frame at $t\in \lbrace 4, 7, 12, 15, 21\rbrace$ [s]. (c) Manipulation task: telemanipulating an object using a haptic device.

To examine the compliant/backdrivable interaction capability of the S3Q system, we stabilize the system in the hovering posture at a fixed position ($x_o^w \rightarrow [0;0;1.5]$ m) and apply some external wrenches to that in $N^\circ$, $E^\circ$ , and $D^\circ$ directions at different positions, as shown in the snapshot of Fig. 8(b). The pose deviation from the hovering pose is proportional to the external wrenches. Fig. 8(b) 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\in \lbrace 4, 7, 12, 15, 21\rbrace$ [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 instances $(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 ${\text{8}}^\circ$ at $(4)$ and ${\text{10}}^\circ$ at $(5)$, since this given external wrench can be compensated for by the combination of the torque about $E^\circ \text{-}\text{ and } N^\circ$-axes and the force along the $D^\circ$-axis. See Section VI-C, where higher priority is given to maintain the position of the frame. 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 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 underactuation [35]. 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 telemapulating an object using a haptic device. The results are shown in Fig. 8(c), where we can see that the system follows precisely the desired command (i.e., the position tracking RMS error of 4.7 cm and the attitude RMS error of 2.6$^\circ$) from the user while compliantly interacting with the environment.³

C. Control of the S2Q Platform Prototype

Similar to Section VI-B, we perform the motion control experiment of the S2Q system, i.e., the tool-tip position tracking of a horizontal circular shape while maintaining a certain pointing direction (e.g., pitch at zero and yaw at ${\text{45}}^\circ$ ). This task is designed to be feasible according to the task planning in Section III-B [see Fig. 4(a)]. The results are shown in Fig. 9(a), where we can see that the S2Q platform can track this feasible motion with the tracking RMS error of $\text{2.7}$ cm while maintaining the desired pointing direction (i.e., yaw and pitch angles with the RMS error of 5.6$^\circ$). Since this task requires purely motion tracking, we adopt a virtual tool-tip located on the line connecting the CoM of two quadrotors and utilize the control designed in Section V.

Fig. 9.

Experiments of the S2Q platform prototype. (a) Trajectory tracking: the virtual tool-tip on the line connecting the CoM positions of two quadrotors tracking a horizontal circular shape while maintaining a certain attitude (e.g., pitch at zero and yaw at 45$^\circ$). (b) Stable interaction: geometric center point $x_e$ and the pointing direction of the S2Q platform under the external wrench applied to the frame at $t\in \lbrace 8, 20, 32\rbrace$ [s]. (c) Manipulation task: telemanipulating an object of 3 $\times$ 4.5 cm using a haptic device.

Fig. 9(b) shows the results of the interaction experiment with the S2Q platform 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 condition (17) [see Fig. 9(b)]. We can see that the point-contact force at the tool-tip does not create external torque on the unactuated direction of the S2Q system. In this task, we perform stabilization with the geometric center point of two quadrotors $x_e = \frac{1}{2} (x_1 + x_2) \rightarrow [0;0;1.5]$ [m]. We then apply point-contact forces at the tool-tip along $N^\circ$, $E^\circ$, and $D^\circ$ directions. Fig. 9(b) presents the response of the system to the interaction wrench with the markings $(1), (2), (3)$ corresponding to the responses at instances $t\in \lbrace 8, 20, 32\rbrace$ [s]. We then see that the contact force along the $N^\circ$ direction causes a proportional position error in the $e_1$-axis. With the contact force along $E^\circ$ and $D^\circ$ directions at the physical tool-tip, i.e., instances $(2), (3)$, the system can generate counteracting force along $E^\circ$ and $D^\circ$ directions at $x_e^o$ and counteracting torque about $D^\circ$ and $E^\circ$ directions. In this implementation, we give higher priority to maintaining the position $x_e^o$ by using high stiff gain $k_x$ . Thus, the attitude errors change accordingly, while the position errors remain fairly small. See the marking points $(2), (3)$ in Fig. 9(b).

We also perform a teleoperation task, i.e., using a haptic device to remotely control the tool-tip to push a box of ${\text{3}} \times {\text{4.5}}$ cm. As expected, the tool-tip tracks the desired motion generated by the haptic device with RMS errors of 3.0 cm and 3.2 $^\circ$, and we can accomplish the task [see Fig. 9(c)]. In all these tasks, the S2Q system exhibits some roll motion. However, as expected, the roll angle remains bounded in all tasks with the RMS error of ${\text{1.4}}^\circ$ in the circular motion, 1.2$^\circ$ in the interaction task, and ${\text{1.5}}^\circ$ in the teleoperation task, which confirms the boundedness of the unactuated dynamic in Lemma 1 (see Fig. 9).⁴

SECTION VII. Conclusion

In this paper, we presented a novel aerial manipulation platform, SmQ platform, which consists of multiple quadrotors connected to a frame through passive spherical joints. This proposed SmQ platform can overcome the well-known issues of underactuation of multirotor drones for aerial operation/manipulation, allowing for more robust operation (particularly against sideway forcing/gust), dynamic control for faster/smoother interaction, and simpler/modular integration with a multi-DOF robotic arm, as compared to systems based on the conventional multirotor drones. In this paper, we provide theoretical framework for this SmQ platform system, particularly for its modeling and control, while also elucidating issues related to: 1) how to design the SmQ systems to be fully actuated in SE(3); and 2) how to attain closed-loop control of the SmQ system while respecting such physical constraints as the range limit of the spherical joints and the thrust saturations. The performance and possible utility of the SmQ system are also demonstrated via experiments for various scenarios, encompassing from motion control and simple mechanical operation to impedance interaction control and telemanipulation.

Some topics for future work include the following:

1. optimal motion planning of the SmQ system with its dynamics, constraints, and uncertainty incorporated;

2. implementation of the SmQ system with only onboard sensing and computing;

3. integration of the SmQ system with a multi-DOF robotic arm and different types of drones (e.g., three rotors).

Appendix Quadrotor Thrust Decode

In Sections IV and V, the required thrust vector $\Lambda _i$ is computed for each quadrotor. Here, we explain how to compute the rotor commands from this desired thrust vector $\Lambda _i$ for the quadrotor. Let us parameterize $R_i \in \text{SO}(3)$ of the $i$th quadrotor using yaw, pitch, and roll angles $\eta _i :=[\phi _i; \theta _i; \psi _i] \in \Re ^3$, that is, $R_i(\eta) = R_{e_3}(\phi _i)R_{e_2}(\theta)R_{e_1}(\psi)$, with $R_{e_i}(\star)$ being the elementary rotation matrix about the $e_i\text{-}$axis [32]. The objective is then computing the throttle $|\lambda _i|$ and attitude command $(\psi _i,\theta _i)$ such that \begin{equation} \lambda _iR_i(\eta)e_3 = R_o\Lambda _i \end{equation} where $\psi _i$ can be arbitrary. We now rewrite (46) as \begin{equation*} \lambda _iR_{e_2}(\theta)R_{e_1}(\psi)e_3 = R_{e_3}^T(\phi _i) R_o \Lambda _i. \end{equation*} We choose the throttle command $|\lambda _i| = \Vert \Lambda _i \Vert$ and the attitude command such that \begin{equation*} {\left[\begin{array}{c}\sin \theta _i \cos \psi _i \\ -\sin \phi _i \\ \cos \theta _i \cos \psi _i \\ \end{array}\right]} = \frac{1}{\Vert \Lambda _i \Vert } R_{e_3}^T(\phi _i)\Lambda _i =:\hat{\Lambda }_i(\psi _i) \in \Re ^3. \end{equation*} This suggests to choose the roll and pitch commands as \begin{equation*} \psi _i = - \text{sin}^{-1} \hat{\Lambda }_{i_2}, \qquad \theta _i = \text{tan}^{-1} \frac{\hat{\Lambda }_{i_1}}{\hat{\Lambda }_{i_3}} . \end{equation*}