International Journal of Automation and Computing  2018, Vol. 15 Issue (5): 547-558 PDF
Rotorcraft with a 3DOF Rigid Manipulator: Quaternion-based Modeling and Real-time Control Tolerant to Multi-body Couplings
J. Alvarez-Munoz1, N. Marchand2, J. F. Guerrero-Castellanos3, J. J. Tellez-Guzman2, J. Escareno4, M. Rakotondrabe5
1 Polytechnic Institute of Advanced Sciences, FR 94200 Paris, France;
2 Univ. Grenoble Alpes, GIPSA-Lab, FR-38000 Grenoble, France;
3 Autonomous University of Puebla (BUAP), Faculty of Electronics, MX 72570 Puebla, Mexico;
4 University of Limoges ENSIL - ENSCI, XLIM Laboratory UMR CNRS 7252, Parc Ester Technopole, 16 rue Atlantis, Limoges 87068, France;
5 FEMTO-ST Institute, UMR CNRS-UFC/ENSMM/UTBM, Automatic Control and Micro-Mechatronic Department, FR Besançon, France
Abstract: This paper proposes a simple solution for the stabilization of a mini-quadcopter carrying a 3DoF (degrees of freedom) manipulator robot in order to enhance its achievable workspace and application profile. Since the motion of the arm induces torques which degrade the stability of the system, in the present work, we consider the stabilization of both subsystems: the quadcopter and the robotic arm. The mathematical model of the system is based on quaternions. Likewise, an attitude control law consisting of a bounded quaternion-based feedback stabilizes the quadcopter to a desired attitude while the arm is evolving. The next stage is the translational dynamics which is simplified for control (nonlinear) design purposes. The aforementioned controllers are based on saturation functions whose stability is explicitly proved in the Lyapunov sense. Finally, experimental results and a statistical study validate the proposed control strategy.
Key words: Observer-based control     quaternion and Newton-Euler modeling     bounded-input control     aerial manipulator     disturbance rejection
1 Introduction

Aerial manipulation has been an active area of research in recent times for unmanned aerial vehicles (UAVs) since it increases the application in both the military and civilian sectors. Unlike fixed-wing UAV configurations, whose flight profile lacks hovering flight, vertical take-off and landing (VTOL) rotorcrafts with three, four, six, or eight rotary propellers are well-suited for aerial manipulation operations. However, numerous scientific challenges, technological and theoretical, remain open. The main issue arises from their limited payload capacity. Thus, alternatively, multiple robots can carry heavier payloads using cables or grippers[1] which feature light and dexterous end-effectors. Furthermore, the dynamics of the robot is significantly altered while shifting and/or carrying payloads, due to the center of gravity shifting and external disturbances. Indeed, this is also an attraction in assembly because aerial robots can use this to sense disturbance forces and moments, as in [2]. Moreover, the performance of the aerial manipulation task relies on effective estimation of dynamic couplings for compensation purposes.

Numerous approaches have been proposed to deal with such problems. In [3], a Lyapunov based model reference adaptive control is used to stabilize a quadrotor with a multi degree of freedom (DoF) manipulator. However, the stability analysis is carried out with a linear approach and only the dynamics of the quadrotor was concerned due to the complexity of the system. Jimenez-Cano et al.[4] present a Newton-Euler approach to model and control a quadrotor through a variable parameter integral backstepping (VPIB) approach. However, the parametrization of the system is made through Euler angles, which present attitude estimation singularities. Kim et al.[5] present aerial manipulation using a quadrotor and a 2DoF robot arm. The dynamic model of the system is obtained by the Euler-Lagrange formulation. Then, an adaptive sliding mode controller is designed. The effectiveness of the proposed method is showed experimentally by picking up and delivering an object.

In [6], the problem is solved through an autonomous avian-inspired grasping method, however the design of the controller is formulated in the vertical plane, which supposes a limitation to the full 3 D space. Sun et al.[7] present an amplitude-saturated nonlinear strategy for underactuated cranes with double-pendulum dynamics. Such a strategy is twice validated: theoretically and practically. The experimental platform consists of a car and a double-pendulum crane, where the coupling between the two systems (car and pendulum) is taken into account and a dynamic model is obtained. However, compared to an aerial vehicle, the attitude and position dynamics must be taken into account.

Finally, in [8], a new class of aerial manipulator is presented. It consists in a planar vertical take-off and landing (PVTOL) equipped with parallel manipulator arms attached to the center of mass (CoM) of the aerial vehicle, which is called protocentric. A control law has been proposed for the case of rigid joints and validated through simulations.

The contribution of the present paper is centered on a strategy that combines an alternative torque compensation approach with a nonlinear control design. Specifically, a mathematical model is presented in detail using quaternions and it takes into account the coupling between the two systems, quadcopter and manipulator. Unlike the research previously cited, the design of the attitude control law uses the quaternion parametrization, which avoids the presence of singularities.

With quaternion parametrization, we proposes a constructive control law for the attitude and position stabilization. First, the design of a smooth, almost globally asymptotical control law for attitude stabilization which takes into account the dynamics of the robotic arm, is carried out. After that, a globally asymptotical nonlinear controller for the translational dynamics is proposed. In general, the control law is based in the usage of a sum of nested saturation functions in order to take into account the actuator′s limitation. Real-time experimental results and a statistical study of the obtained results validate the proposed strategy.

The paper is structured as follows. In Section 2, the attitude model of the quadcopter with the manipulator arm is presented. Then, the attitude control design is formulated in Section 3. Section 4 gives a strategy to estimate the angular position of each link in the robot manipulator through a Luenberger observer. Section 5 is devoted to the design of the position control law. Section 6 presents the hardware setup, experimental results, as well as a statistical study. Finally, some conclusions are presented in Section 7.

2 System modeling 2.1 Unit quaternion and attitude kinematics

Consider two orthogonal right-handed coordinate frames: the body coordinate frame, $B(x_b, y_b, z_b)$ , located at the center of mass of the rigid body and the inertial coordinate frame, $N(x_n, y_n, z_n)$ , located at some point in the space (for instance, the earth north-east-down (NED) frame). The rotation of the body frame $B$ with respect to the fixed frame $N$ is represented by the attitude matrix $R \in SO(3)=$ $\{R \in {\bf R}^{3\times 3}: {R}^{\rm T} R=I, \mbox{det}\, R=1\}$ .

The cross product between two vectors $\xi, \varrho \in\mathbb {\bf R}^3$ is represented by a matrix multiplication $[\xi^\times] \varrho=\xi \times \varrho$ , where $[\xi^\times]$ is the well known skew-symmetric matrix.

The $n$ -dimensional unit sphere embedded in $\mathbb {\bf R}^{n+1}$ is denoted as $\mathbb S^n = \{x \in \mathbb {\bf R}^{n+1}: x^{\rm T}x=1 \}$ . Members of $SO(3)$ are often parameterized in terms of a rotation $\beta \in \mathbb {\bf R}$ about a fixed axis $e_v \in \mathbb S^2$ by the map $\mathcal{U}: \mathbb {\bf R} \times \mathbb S^2 \rightarrow$ $SO(3)$ defined as

 $\mathcal{U}(\beta, e_v)=I_3+\sin(\beta) [{\rm e}_v^\times] + (1-\cos(\beta))[{\rm e}_v^\times]^2.$ (1)

Hence, a unit quaternion, $q \in \mathbb S^3$ , is defined as

 q=\left(\begin{aligned}& \; \cos \frac{\beta}{2} \\& e_v \sin \frac{\beta}{2} \\\end{aligned}\right)=\left(\begin{aligned} q_0 \\ q_v \\\end{aligned}\right) \in \mathbb S^3 (2)

where $q_v=(q_1\ \ q_2 \ \ q_3)^{\rm T} \in \mathbb {\bf R}^3$ and $q_0 \in \mathbb {\bf R}$ are known as the vector and scalar parts of the quaternion, respectively. The quaternion $q$ represents an element of $SO(3)$ through the map $\mathcal{R}: \mathbb S^3 \rightarrow SO(3)$ defined as

 $\mathcal{R}:=I_3+2q_0 [q_v^\times] + 2[q_v^\times]^2.$ (3)

Remark 1. $R=\mathcal{R}(q)=\mathcal{R}(-q)$ for each $q \in \mathbb S^3$ , i.e., even quaternions $q$ and $-q$ represent the same physical attitude.

Denoting by $\vec{\omega}=(\omega_1\ \ \omega_2\ \ \omega_3)^{\rm T}$ the angular velocity vector of the body coordinate frame $B$ , relative to the inertial coordinate frame $N$ , the kinematics equation is given by

 \left(\begin{aligned} \dot{q}_0 \\ \dot{q}_v \\\end{aligned}\right) = \frac{1}{2}\left(\begin{aligned} & \quad -{q}_v^{\rm T} \\ & I_3q_0 + [{q}_v^\times]\end{aligned}\right) \vec{\omega}=\frac{1}{2}\Xi(q)\vec{\omega}. (4)

The attitude error is used to quantify mismatch between two attitudes. If $q$ defines the current attitude quaternion and $q_d$ defines the desired quaternion, i.e., the desired orientation, then the error quaternion that represents the attitude error between the current orientation and the desired one is given by

 $q_e=q_d^{-1} \otimes q=(q_{e_{0}} \ {q}_{e_{v}}^{\rm T})^{\rm T}$ (5)

where $q^{-1}$ is the complementary rotation of the quaternion $q$ which is given by $q^{-1}=(q_0 \ \ -q_v^{\rm T})^{\rm T}$ and $\otimes$ denotes the quaternion multiplication[9].

2.2 Model of a quadcopter carrying a manipulator arm

The attitude dynamics and kinematics for the quadcopter have been reported in many works $\textit{\rm e.g.,}$ [1012]. In these works the quadcopter mass distribution is considered to be symmetric. However, the mass distribution of a quadcopter with a manipulator is no longer symmetrical and varies with the movement of the arm. Consider a quadcopter with a manipulator arm with $n$ links attached to its lower part. If the dynamics of the arm is neglected, the attitude kinematics and dynamics is given by

 \left(\begin{aligned}& \dot{q}_0 \\& \dot{q_v}\end{aligned}\right)=\frac{1}{2}\Xi(q)\vec{\omega}\;\; (6)
 $J\dot{\vec{\omega}}=-\vec{\omega}^\times J\vec{\omega}+\Gamma_T$ (7)

where $J\in\mathbb{\bf R}^{3\times3}$ is the symmetric positive definite constant inertial matrix of the rigid body expressed in the body frame $B$ and $\Gamma_T\in\mathbb{\bf R}^3$ is the vector of applied torques. $\Gamma_T$ depends on the (control) couples generated by the actuators, the aerodynamic couples such as gyroscopic couples, the gravity gradient or, as in the case of the present work, the couple generated by the movement of a robot manipulator placed under the body. Here, only the control couples, gyroscopic couples and the couple generated by the manipulator is considered in the control design. Consequently,

 $\Gamma_T= \Gamma + \Gamma_{arm} + \Gamma_G$ (8)

where $\Gamma$ and $\Gamma_{G}$ will be described in Section 2.3. On the other hand, the vector $\Gamma_{arm}$ is the torque generated by the total propulsive force being applied at the quadcopter geometric center which is displaced from the center of mass.

Taking the robot manipulator as a physical pendulum attached to the fuselage of the aerial vehicle and following a similar process to the one in [13], the manipulator torque can be computed by

 $\Gamma_{arm_{dyn}}=m_ag\zeta_{cd}\times \mathcal{R}(q)e_3$ (9)

where $\Gamma_{arm_{dyn}}$ is the manipulator torque taking into account the dynamics of each servomotor in the robot manipulator, $m_a =\Sigma_{i=1}^nm_{mi}+m_l$ is the total mass of the manipulator plus the load and $\zeta_{cd}=(\zeta_{c{_x}d} \ \zeta_{c{_y}d} \ \zeta_{c{_z}d})^{\rm T}$ $\in \mathbb{\bf R}^3$ is the position of center of mass of the quadrotor with respect to the pivot point. Then, the center of mass can be computed by

 $\zeta_{cd}=\frac{1}{m_a}\left[\sum_{i=1}^nm_{mi}\varrho_{id}+m_l\varrho_{ld}\right]$ (10)

where $\varrho_{id}$ and $\varrho_{ld}$ are the position vectors of each link of the manipulator and the load, respectively, both with respect to the reference body frame given by the quadrotor.

In this case, let us consider the scheme in Fig. 1, which shows an anthropomorphic arm manipulator. This system has three degrees of freedom and then, the corresponding $\varrho_{id}$ , where $i=\{1, 2, 3\}$ , is given by

 $\begin{split} {\varrho _{1d}} = &{[0 \quad 0 \quad - {l_{c1}}]^{\rm T}}\\ {\varrho _{2d}} = & [{l_{c2}}\sin{\theta _{a2}}\cos {\theta _{a1}} \quad {l_{c2}}\sin{\theta _{a2}}\sin {\theta _{a1}}\\& -({l_1} + {l_2})\cos{\theta _{a2}}{]^{\rm T}}\\{\varrho _{3d}} = & [({l_2}\sin{\theta _{a2}} + {l_{c3}}\sin ({\theta _{a2}} + {\theta _{a3}}))\cos{\theta _{a1}} \\& ({l_2}\sin{\theta _{a2}} + {l_{c3}}\sin ({\theta _{a2}} + {\theta _{a3}}))\sin {\theta _{a1}}\\& -({l_1} + {l_2})\cos{\theta _{a2}} - {l_{c3}}\cos({\theta _{a2}} + {\theta _{a3}}){]^{\rm T}}\end{split}$ (11)

where $l_{c1}$ , $l_{c2}$ and $l_{c3}$ are the distances from the respective joint axes to the center of mass of each link, $l_1$ , $l_2$ and $l_3$ are the total length of the links, and $\theta_{ai_m}$ measures the angular displacement from $z$ and $x$ axes. Then, since servomotors are used as actuators for the manipulator arm, these can be easily considered as first order systems. For this, a parameter identification is performed in order to know the different constant values of the motors. In general, the found system has the form:

 $\theta_{ai_m}=\frac{Ku(t)-\dot{\theta}_{ai_m}}{a}$ (12)

where $\theta_{ai_m}$ is the angular position of the servo output shaft, $\dot{\theta}_{ai_m}$ is the angular velocity of the servo, $a$ is a time constant linked to the time response of the servo, $K$ is the gain of the system and $u(t)$ is the input.

 Download: larger image Fig. 1. Manipulator arm with three degrees of freedom

2.3 Actuator model

The quadrotor under study has four fixed-pitch rotors mounted at the four ends of a simple cross frame. The model of the rotors is considered as a simple direct current (DC) motor which may reach high speed values (more than 200 rad/s). The collective input (or throttle input) is the sum of the thrusts of each rotor $f_1, f_2, f_3, f_4$ . Therefore, the reactive couple $Q_j$ generated in the free air by rotor $j$ due to the motor drag and the total thrust $T$ produced by the four rotors can be, respectively, approximated by

 $Q_j=ks_j^2 \quad\quad\quad\quad\;$ (13)
 $T = \sum_{j=1}^4 f_j=b\sum_{j=1}^4 s_j^2$ (14)

where $s_j$ represents the rotational speed of rotor $j$ . $k>0$ and $b>0$ are two parameters which depend on the density of air, the radius, the shape, the pitch angle of the blade and other factors[12]. The vector of gyroscopic couples $\Gamma_G$ is a consequence of the simultaneous rotation of the structure of the quadrotor and the high-speed rotation of the actuators, and it is given by

 $\Gamma_G=\sum_{j=1}^4 J_r(\vec{\omega}\times \vec{z}_b)(-1)^{j+1}s_j$ (15)

where $J_r$ is the inertia of the so-called rotor (composed of the motor rotor itself with the gears). The components of the control torque $\Gamma\in\mathbb{\bf R}^3$ generated by the rotors are given by $\Gamma=[\Gamma_1\, \, \Gamma_2\, \, \Gamma_3]^{\rm T}$ , with

 $\Gamma_{1}=d(f_3-f_4)=db(s_3^2-s_4^2)$ (16)
 $\Gamma_{2}=d(f_1-f_2)=db(s_1^2-s_2^2)$ (17)
 $\Gamma_{3}=-Q_1-Q_2+Q_3+Q_4=k(-s_1^2-s_2^2+s_3^2+s_4^2)$ (18)

where $d$ is the distance between the rotor and the center of gravity of the quadrotor.

3 Attitude control design 3.1 Problem statement

The objective is to design a control law which drives the quadcopter to attitude stabilization under the torques and moments exerted to this from the movement of a manipulator arm attached to its lower part. In other words, let $q_d$ denote the constant quadcopter stabilization orientation, then the control objective is described by the following asymptotic conditions

 $q\rightarrow [\pm1\;0\;0\;0]^{\rm T}, \;\vec{\omega}\rightarrow 0\;\text{as}\;t\rightarrow\infty.$ (19)

Furthermore, it is known that actuator saturation reduces the benefits of the feedback. When the controller continuously outputs infeasible control signals that saturate the actuators, system instability may follow. Then, besides the asymptotic stability, the control law also takes into account the physical constraints of the control system, in order to apply only feasible control signals to the actuators.

3.2 Attitude control with manipulator arm

In this subsection, a control law that stabilizes the system described by (6) and (7) is proposed. The goal is to design a control torque that is bounded.

Definition 1. Given a positive constant $M$ , a continuous nondecreasing function $\sigma_M:\mathbb{\bf R}\rightarrow\mathbb{\bf R}$ is defined by

 $\begin{split}&\sigma_M=s\quad \mathrm{if}\, \, |s| Note that the components of$\Gamma_{arm_i}$are always bounded,$\textit{\rm i.e.,}\mid\Gamma_{arm_{i}} \mid < \delta_i$. Then, one has the following result. Theorem 1. Consider a rigid body rotational dynamics described by (6) and (7) with the following bounded control inputs$\Gamma=(\Gamma_1 \ \Gamma_2 \ \Gamma_3)^{\rm T}$such that $\Gamma_i=-\sigma_{M_{i2}}\left(\Gamma_{arm_{i}} + \sigma_{M_{i1}}( \lambda_i [\omega_i + \rho_i q_i])\right)$(21) with$i\in\{1, 2, 3\}$and where$\sigma_{M_{i1}}$and$\sigma_{M_{i2}}$are saturation functions. Assuming$\delta_i < M_{i2}-M_{i1}$and$M_{i1}\geq3\lambda_i\rho_i$, where$\lambda_i$and$\rho_i$are positive parameters. Then the inputs (21) asymptotically stabilize the rigid body to the origin$(1 \ 0^{\rm T} \ 0^{\rm T})^{\rm T}({\rm i.e.,} q_0=1, \quad q_v=0\, \, \mathrm{and}\, \, \vec{\omega}=0)$with a domain of attraction equal to$\mathbb S^3 \times \mathbb {\bf R}^3 \setminus {(-1 \ 0^{\rm T} \ 0^{\rm T})^{\rm T}}$. The proof of Theorem 1 is given in Appendix. Remark 2. Note that the stability analysis has been carried out considering the asymptotic condition$q\rightarrow q_d=[\pm1\;0\;0\;0]^{\rm T}$. In th case where the asymptotic condition$q\rightarrow q_d$with$q_d\neq [\pm1\;0\;0\;0]^{\rm T}$is considered, the control law applied will be $\Gamma_i=-\sigma_{M_{i2}}\left(\Gamma_{arm_{i}} + \sigma_{M_{i1}}( \lambda_i [\omega_i + \rho_i q_{e_i}])\right)$(22) where$q_e$represents the attitude error between the current orientation and the desired on. 4 Manipulator links angular position estimation 4.1 Problem statement The objective is to design a strategy for the estimation of the angles on each link in the manipulator arm, combining the data coming from the first order model of the manipulator actuators and the data coming from the end effector position tracked by the VICON system (motion capture system). Since the first order model does not fully describe the behavior of the arm manipulator (non-modeled dynamics, actuators malfunction, etc.). In order to know the angular positions of the manipulator links with respect to the base body (quadcopter), the inverse kinematics of the manipulator arm is used. 4.2 Manipulator links observer design In this subsection, a Luenberger observer is designed to estimate the angles on each link in the manipulator. For this, the end-effector position is computed through the arm inverse kinematics to know the angle on each link. The expression that describes a link angle is given by $ \theta_{aV}=\theta_a+\mu_V$(23) where$\theta_{aV}$is the estimated angle computed with the inverse kinematics,$\theta_a$is the real angle and$\mu_V$is a noise of minimal value. In addition, the observer allows the computation of the angular velocity. The expressions that represent the observer are given by $ \dot{\hat{\theta}}_\delta=a\hat{\theta}_\delta+Ku_\delta+L(\theta_{aV}-\hat{\theta})$(24) $ \hat{\theta}=\hat{\theta}_V$(25) where$\hat{\theta}_\delta$is the estimated angle on a link in the manipulator,$a$and$K$are parameters of the first order system previously presented and$L$is a positive tuning parameter. Now, given the expression (24), where$\hat{\theta}_\delta$and$\dot{\hat{\theta}}_\delta$were estimated, it is possible to compute the manipulator torque from (12) and use this new term as$\Gamma_{arm}$into the attitude control law (21). 5 Position control design 5.1 Problem statement The objective is to design a control law which stabilizes the quadcopter to a desired position, thereby solving the attitude stabilization problem. In other words, once the control law has stabilized the attitude of the system,$\lim_{t\rightarrow\infty}(q, \vec{\omega})=(q_d, \vec{0})$, the position control law should stabilize the quadcopter in a desired position,$\lim_{t\rightarrow\infty}(\vec{p}, \vec{v})=(\vec{p}_d, \vec{0})$. This stabilization must be ensured even under the disturbances from the manipulator arm. 5.2 Position stabilization strategy The schematic representation of a quadcopter carrying a manipulator arm can be seen in Fig. 2, where the inertial reference frame$N(x_n, y_n, z_n)$, the body reference frame$B(x_b, y_b, z_b)$, the force$u$(thrust) and the weight vector$m\vec{g}$are depicted. The dynamics of the whole system is obtained with the Newton-Euler formalism and the kinematics is represented using the quaternions formalism, given by ${\Sigma _T}:\left\{ {\begin{array}{*{20}{l}}{\dot {\vec p} = \vec v}\\{{m_T}\dot {\vec v} = - {m_T}\vec g + R\left( {\begin{array}{*{20}{c}}0\\0\\u\end{array}} \right)}\end{array}} \right.$(26) ${\Sigma _O}:\left\{ {\begin{aligned}& {\dot q = \frac{1}{2}\Xi (q)\vec \omega }\\& {J\dot {\vec \omega} = - {{\vec \omega }^ \times }J\vec \omega + {\Gamma _T}}\end{aligned}} \right.\quad\quad\quad\quad$(27) where$\vec{p}$and$\vec{v}$are linear position and velocity vectors,$m_T$is the total mass of the system (the quadcopter and the manipulator),$\vec{g}$is the acceleration due to gravity, and$R$is the rotation matrix, given in (3).  Download: larger image Fig. 2. Schematic configuration of a quadrotor carrying a manipulator arm Note that the rotation matrix$R$can also be given as a function of the Euler angles, i.e., $\begin{split}&R(\phi, \theta, \psi ) = \\&\,\,\,\,\,\,\left( {\begin{array}{*{20}{c}}{{\rm{C}}\psi {\rm{C}}\theta }&{{\rm{S}}\psi {\rm{C}}\theta }&{ - {\rm{S}}\theta }\\{{\rm{C}}\psi {\rm{S}}\theta {\rm{S}}\phi - {\rm{S}}\psi {\rm{C}}\theta }&{{\rm{S}}\phi {\rm{S}}\theta {\rm{S}}\psi + {\rm{C}}\psi {\rm{C}}\phi }&{{\rm{C}}\theta {\rm{S}}\phi }\\{{\rm{C}}\psi {\rm{C}}\phi {\rm{S}}\theta + {\rm{S}}\psi {\rm{S}}\phi }&{{\rm{S}}\theta {\rm{S}}\psi {\rm{C}}\phi - {\rm{C}}\psi {\rm{S}}\phi }&{{\rm{C}}\theta {\rm{C}}\phi }\end{array}} \right).\end{split}$(28) Taking into account (26) and (27), this system can be seen as a cascade system, where the translational dynamics (26) depend on the attitude (27), but the attitude dynamics do not depend on the translational one. This property will be used to design the control law. Now, assume that using the control law (21) one can stabilize the yaw dynamics, i.e.,$\psi=0$, then after a sufficiently long time, system (26) becomes $\left( {\begin{aligned}{{{\dot p}_x}}\\{{{\dot p}_y}}\\{{{\dot p}_z}}\end{aligned}} \right){\rm{ = }}\left( {\begin{aligned}{{v_x}}\\{{v_y}}\\{{v_z}}\end{aligned}} \right)\quad\quad\quad\quad\quad\quad\quad$(29) $\left( {\begin{aligned}{{{\dot v}_x}}\\{{{\dot v}_y}}\\{{{\dot v}_z}}\end{aligned}} \right) = \left( {\begin{aligned}& \;\;\;\;\;\;{ - \frac{u}{{{m_T}}}\sin \theta }\\& \;\;\;{\frac{u}{{{m_T}}}\sin \phi \cos \theta }\\& {\frac{u}{{{m_T}}}\cos \phi \cos \theta - g}\end{aligned}} \right).$(30) With an appropriate choice of target configuration, it will be possible to transform (29) and (30) into three independent linear triple integrators. For this, take $\begin{split}&\phi_d=\arctan\left(\frac{r_2}{r_3+g}\right) \\&\theta_d=\arcsin\left(\frac{-r_1}{\sqrt{r_1^2+r_2^2+(r_3+g)^2}}\right)\end{split}$(31) where$r_1$,$r_2$and$r_3$will be defined after. Then, choose as positive thrust the input control $u=m_T\sqrt{r_1^2+r_2^2+(r_3+g)^2}.$(32) Let the state be$p=(p_1, p_2, p_3, p_4, p_5, p_6, p_7, p_8, p_9)=(\int p_x, p_x, v_x, \int p_y, p_y, v_y, \int p_z, p_z, v_z)$, then (29) and (30) become $\begin{aligned}\label{eq:sub1} \,\,\,\,\,\,\,\, \Sigma_x:\left\{ \begin{array}{ll} \dot{p}_1=p_2 & \hbox{} \\ \dot{p}_2=p_3 & \hbox{} \\ \dot{p}_3=r_1 & \hbox{} \end{array} \right. & \!\!\Sigma_y:\left\{ \begin{array}{ll} \dot{p}_4=p_5 & \hbox{} \\ \dot{p}_5=p_6 & \hbox{} \\ \dot{p}_6=r_2 & \hbox{} \end{array} \right. & \!\!\!\!\!\!\!\!\!\Sigma_z:\left\{ \begin{array}{ll} \dot{p}_7=p_8 & \hbox{} \\ \dot{p}_8=p_9 & \hbox{} \\ \dot{p}_9=r_3. & \hbox{} \end{array} \right. \end{aligned}$(33) Note that$u$will be always positive, and$u\geq mg$, in order to compensate the system′s weight. Since the chains of integrators given in (33) have the same form, a control law can be proposed as in [14], and can be established by Theorem 2. Theorem 2. Consider the quadcopter translational dynamics expressed in (29) and (30). Then, the thrust input$u$given by (32) with$r_1, r_2, r_3$as in (34), where$\sigma_{M_1}(\cdot)$is defined in (20) with$M_1=1$and$\varsigma_i$are given by (35),$a_{(1, 2, 3)}, b_{(1, 2, 3)}, c_{(1, 2, 3)}>0$tuning parameters such that$(a, b, c)_{1}>(a, b, c)_{2}+(a, b, c)_{3}$,$(a, b, c)_{2}>(a, b, c)_{3}$, stabilizes globally and asymptotically the quadcopter translational dynamics at the origin. Furthermore, if none of the$\sigma_{M_1}$are saturated, the poles of the linearized closed loop for the subsystems (33) reside at$-(a, b, c)_1, -(a, b, c)_2, -(a, b, c)_3$, respectively. $\begin{split}r_1&=-\varsigma_1\{a_3\sigma_{M1}[\frac{1}{\varsigma_1}(a_2p_1+p_2+p_3)]+\\& \quad a_2\sigma_{M1}[\frac{1}{\varsigma_1}(a_1p_2+p_3)]+a_1\sigma_{M1}[\frac{1}{\varsigma_1}(p_3)]\} \\r_2&=-\varsigma_2\{b_3\sigma_{M1}[\frac{1}{\varsigma_1}(b_2p_4+p_5+p_6)]+\\& \quad b_2\sigma_{M1}[\frac{1}{\varsigma_2}(b_1p_5+p_6)]+b_1\sigma_{M1}[\frac{1}{\varsigma_2}(p_6)]\} \\r_3&=-\varsigma_3\{c_3\sigma_{M1}[\frac{1}{\varsigma_1}(c_2p_7+p_8+p_9)]+\\& \quad c_2\sigma_{M1}[\frac{1}{\varsigma_3}(c_1p_8+p_9)]+c_1\sigma_{M1}[\frac{1}{\varsigma_3}(p_9)]\}\end{split}$(34) $\begin{split}&\varsigma_1=\frac{\bar{r}_1}{(a_1+a_2+a_3)}\quad\quad \\&\varsigma_2=\frac{\bar{r}_2}{(b_1+b_2+b_3)}\quad\quad\\&\varsigma_3=\frac{\bar{r}_3}{(c_1+c_2+c_3)}.\quad\quad\end{split}$(35) Due to space constraints, the proof of Theorem 2 is not presented here, but it can be easily derived from the seminal work of [1517] . Remark 3. In the above Theorem 2, the stabilization goal is the origin. In the case where the asymptotic condition is different from the origin, the variables$p_2, p_5, p_8$should be replaced in the control law (34) by$e_1=p_2-p_x^d$,$e_2=p_5-p_y^d$,$e_3=p_8-p_z^d$, respectively. In this case,$p_x^d, \,p_y^d,\, p_z^d$represent the desired position in the space. 6 Experimental validation 6.1 Hardware setup In order to test the effectiveness of the proposed control law, a set of experiments were performed. The aerial system consists of a home-made quadcopter and arm manipulator, see Fig. 3. Both structures were specially designed and built for this project. The characteristics and parameters of each system are described in Table 1. The total weight of the quadcopter with its arm is 315 g and its carrying capacity is about 50 g.  Download: larger image Fig. 3. Mini-quadcopter with its manipulator arm in flight Table 1 Characteristics and parameters of the nano-hexacopter and the manipulator The attitude control law (21) for the quadcopter was programmed in a Microwii Copter board, which has gyros, accelerometers and the ATMega32u4 as processor. Then, a ground station estimates the position and attitude of the hexacopter using the Vicon Tracker system and T40s cameras[18]. With this system, it is possible to compute the position and attitude up to 100 Hz. The estimated states are sent to Matlab/Simulink through a user datagram protocol (UDP) frame every 2 ms. The position control algorithm is implemented in real-time at 200 Hz on a computer using number personal computer (xPC) target toolbox[19]. The control variables are finally sent back to the quadcopter on the Microwii, through a Grenoble Image Parole Signal Automatique (GIPSA)-Lab′s built-in bridge to DSM2 protocol. For this, a radio-frequency emitter is used. Furthermore, the manipulator arm is controlled by the DS35 Digital Super Sub-Micro Servo[20]. To transmit the control signals to the manipulator, Spektrum digital spread modulation 2 (DSM2) transmitters are connected to the ground station through another built-in bridge. An overview of the whole hardware architecture is presented in Fig. 4.  Download: larger image Fig. 4. Block diagram of the system 6.2 Experimental scenario A set of experiments is carried out in order to compare the performance of the proposed control with and without accounting for the torque generated by the manipulator, scenarios 1 and 2, respectively. The parameters of thecontrol laws are selected according to the characteristics of the quadrotor actuators and the condition for the manipulator arm. For the attitude control law (21) and the position control law given in (34), where$\textrm{max}|\Gamma_{armi}|=0.085\;8\,{\rm Nm}$and$\delta_i=0.1$, the next parameters values are proposed:$M_{11, 21, 31}=0.1$,$M_{12, 22, 32}=0.5$,$\lambda_{1, 2}=0.015$,$\lambda_3=0.013$,$\rho_{1, 2}=10.5$and$\rho_3=11$. For the control (34),$a_1=b_1=2.3$,$c_1=1.65$,$a_2=b_2=1.2$,$c_2=0.55$,$a_3=b_3=0.1$,$c_3=0.015$and$\bar{r}_{1, 2, 3}=5$. The experiments consist in two parts. First, the links of the manipulator arm are initialized to$\theta_{ai}=(0^ \circ \;90^ \circ \;0^ \circ )^{\rm T}$and the quadrotor is driven to the position$\vec{p}_d=(0 \ 0 \ 1)^{\rm T}$. At time 20 s,$\theta_{ai}=(40^ \circ \;30^ \circ \; 0^ \circ )^{\rm T}$. At time 25 s,$\theta_{ai}=(-30^ \circ \;70^ \circ \; 25^ \circ )^{\rm T} $. At time 30 s,$\theta_{ai}=(10^ \circ \;20^ \circ \; 35^ \circ )^{\rm T}$. Finally, at time 35 s,$\theta_{ai}=(0^ \circ \;90^ \circ \; 60^ \circ )^{\rm T}$and the quadrotor lands. 6.3 Experimental results in scenario 1 Fig. 5 shows the general performance of the aerial system under the disturbances coming from the manipulator arm when the arm torque estimation is not taken into account. Fig. 5 shows the angular and linear positions of the quadcopter during the experiment. Note that even when the quaternion parametrization is considered, Euler angles given in (28) are used in order to have a better perspective of the behavior of the system. In this case, attitude stabilization is guaranteed, but the movement of the manipulator causes stability issues for both attitude and linear position.  Download: larger image Fig. 5. General behavior of the system during scenario 1 6.4 Experimental results in scenario 2 Fig. 6 shows the angular and linear positions of the quadrotor as well as the computed manipulator torques. The arm perturbations are compensated through the dynamic model and it results in a general stabilization improvement of the quadrotor compared to the precedent approach. In addition, the importance of this approach is that the precision of the angular position knowledge is enhanced, which guarantees a better torque estimation.  Download: larger image Fig. 6. General behaviour of the system during the experiment using dynamic method estimation torque compensation and the nonlinear observer 6.5 Results analysis and statistical study In order to compare the obtained results of the proposed methods and the flight performance of the aerial vehicle under the disturbances coming from the movement of the manipulator arm, a statistical error study is carried out. For this, the experiments described before were performed 8 times. Figs. 79 show the linear position errors, the normal Gaussian distribution errors and the attitude errors for scenarios 1 and 2, respectively.  Download: larger image Fig. 7. Attitude error and attitude average error value during the different experiments  Download: larger image Fig. 8. Position errors during the different experiments  Download: larger image Fig. 9. Error Gaussian normal distributions In order to calculate the attitude error,$\|2\arccos q_0\|$was used, where$\|\cdot\|$represents the norm and$q_0$was defined as before. The obtained results show that the average attitude error when the dynamic method estimation is applied is reduced, compared with that obtained when the classical approach is implemented. Since the attitude stabilization of the aerial robot is enhanced with the usage of the proposed method, then the linear position stabilization is equally improved, as it is shown in the linear position errors computation. To have another perspective of the obtained results, the normal Gaussian distribution errors are computed. For this, the linear position errors data were used. Then, from the obtained distributions, it is clear that when the dynamic method estimation for the arm manipulator torque is used, the average error value is closer to 0. Furthermore, the area covered by the distributions when the classical approach is used is bigger, consequently, the probability of error increases. In general, the stabilization of the system is improved and guaranteed with the use of the dynamic model and the nonlinear observer. Table 2 shows the different average error values for the set of experiments. The first column shows the attitude average value error, the second, third and fourth columns show the average error value for the$x$-axis,$y$-axis and$z$-axis. Since the experiment was repeated 8 times using the different approaches, Table 2 gives a better perspective of the stabilization improvement for both quadrotor attitude and linear positions, showing the effectiviness of the proposed approach. Table 2 Average error values for the experiment 7 Conclusions In this paper, a novel model for a quadcopter carrying a manipulator arm was proposed. In addition, a control law was designed to asymptotically stabilize the attitude and position of the system. Moreover, this work has presented a method for aiding the solution through the design of a feed-forward term which allows the estimation of the moments and torques exerted by the manipulator. Since input constraints exist in the actuators, the control law takes into account the actuator′s saturations. Experimental results and a statistical study show the effectiveness of the proposed control law in facing the disturbances coming from the manipulator. In a future work, experimental mass estimation and outdoor picking up and delivery of an object will be pursued. Appendix Proof of Theorem 1 Consider the candidate Lyapunov function$V$, which is positive definite. $\begin{split}V&=\frac{1}{2}\vec{\omega}^{\rm T}J\vec{w}+\kappa((1-q_0)^2+q_v^{\rm T}q_v)=\\ & \quad \frac{1}{2}\vec{\omega}^{\rm T}J\vec{w}+2\kappa(1-q_0)\end{split}$(36) where$J$is defined as before, and$\kappa > 0$must be determined. The derivative of (36) after using (6) and (7) is given by $\begin{split}\dot{V}&=\vec{\omega}^{\rm T}J\dot{\vec{\omega}}-2\kappa\dot{q}_0=\\& \quad \vec{\omega}^{\rm T}(-\vec{\omega}^{\times}J\vec{\omega}+\Gamma + \Gamma_{arm}+\Gamma_G)+\kappa q_v^{\rm T}\vec{\omega}=\\& \quad \underbrace{w_1(\Gamma_1+ \Gamma_{arm_{1}}) +\kappa q_1\omega_1}_{\dot{V}_1}+ \\&\quad\underbrace{w_2(\Gamma_2 + \Gamma_{arm_{2}}) + \kappa q_2\omega_2}_{\dot{V}_2}+\\&\quad\underbrace{w_3(\Gamma_3+\Gamma_{arm_{3}})+ \kappa q_3\omega_3}_{\dot{V}_3}\end{split}$(37) where$\dot{V}$is the sum of the three terms$(\dot{V}_1, \dot{V}_2, \dot{V}_3)$. First,$\dot{V}_1$is analyzed. From$\Gamma_1$in (21) and (37), one gets $\begin{split}\dot{V}_1&=\omega_1 (-\sigma_{M_{12}}\left(\Gamma_{arm_{1}} + \sigma_{M_{11}}( \lambda_1 [\omega_1 + \rho_1 q_1])\right)+\\ & \qquad \Gamma_{arm_{1}} )+\kappa q_1\omega_1\end{split}$(38) if we choose$\delta_1 < M_{12}-M_{11}$,$\sigma_{M_{12}}$is always operating in its linear region so$\dot{V}_1$becomes $\dot{V}_1=-\omega_1\sigma_{M_{11}}(\lambda_1[\omega_1+\rho_1q_1])+\kappa q_1\omega_1.$(39) Assume that$|\omega_1|>2\rho_1$, i.e.,$\omega_1\in\,[2\rho_1, +\infty]$. Since$|q_1|\leq1$, it follows that$|\omega_1+\rho_1q_1|\geq\rho_1+\epsilon$for any$\epsilon>0$sufficiently small. Therefore,$\omega_1+\rho_1q_1$has the same sign as$\omega_1$. From (39) and the norm condition on the quaternion,$\dot{V}_1$takes the following form: $\begin{split}\dot{V}_1&=-\omega_1\sigma_{M_{11}}(\lambda_1[\omega_1+\rho_1q_1])+\kappa \omega_1q_1\leq\\& \quad -|\omega_1|\sigma_{M_{11}}(\lambda_1(\rho_1+\epsilon))+\kappa|\omega_1|.\end{split}$(40) Taking $\kappa<\min(M_{11}, \lambda_1\rho_1+\epsilon)$(41) one can assure the decrease of$V_1$, i.e.,$\dot{V}_1<0$. Consequently,$\omega_1$enters$\Phi_1=\{\omega_1:|\omega_1|\leq2\rho_1\}$in finite time$t_1$and remains in it thereafter. In this case,$(\omega_1+\rho_1q_1)\in[-3\rho_1, 3\rho_1]$. Let$M_{11}$verify the next inequality$M_{11}\geq3\lambda_1\rho_1$, (41) then becomes $\kappa<\lambda_1\rho_1+\epsilon.$(42) For$t_2>t_1$, the argument of$\sigma_{M_{11}}$will be bounded as follows: $|\lambda_1(\omega_1+\rho_1q_1)|\leq3\lambda_1\rho_1\leq M_{11}.$(43) Consequently,$\sigma_{M1}$operates in a linear region: $\Gamma_1=-\lambda_1[\omega_1+\rho_1q_1].$(44) As a result, (39) becomes $\dot{V}_1=-\lambda_1\omega_1^2-\lambda_1\rho_1\omega_1q_1+\kappa \omega_1q_1.$(45) Choosing$\kappa=\lambda_1\rho_1$which satisfies inequality (42), one obtains: $\dot{V}_1=-\lambda_1\omega_1^2\leq0.$(46) The same argument is applied to$\dot{V}_2$and$\dot{V}_3$, (37) becomes $\begin{split}\dot{V}&=\dot{V}_1+\dot{V}_2+\dot{V}_3=\\& \quad -(\lambda_1\omega_1^2+\lambda_2\omega_2^2+\lambda_3\omega_3^2)\leq0.\end{split}$(47) In order to complete the proof, the LaSalle invariance principle is invoked. All the trajectories converge to the largest invariant set$\bar{\Omega}$in$\Omega=\{(q_v, \vec{\omega}):\dot{V}=0\}=\{(q_v, \vec{\omega}):\vec{\omega}=0\}$. In the invariant set,$J\vec{\dot{\omega}}=-[\lambda_1\rho_1q_1 \lambda_2\rho_2q_2\;\;\lambda_3\rho_3q_3]^{\rm T}=0,$i.e.,$\bar{\Omega}\$ is reduced to the origin. This ends the proof of the asymptotic stability of the closed loop system. □

Acknowledgements

This work was supported by CONACYT-Mexico, LabEx PERSYVAL-Lab (No. ANR-11-LABX-0025) and Equipex ROBOTEX (No. ANR-10-EQPX-44-01).

References
 [1] D. Mellinger, M. Shomin, N. Michael, V. Kumar. Cooperative grasping and transport using multiple quadrotors. In Proceedings of International Symposium on Distributed Autonomous Robotic Systems, Springer, Lausanne, Switzerland, pp. 545–558, 2010. [2] M. Mohammadi, A. Franchi, D. Barcelli, D. Prattichizzo. Cooperative aerial tele-manipulation with haptic feedback. In Proceedings of IEEE/RSJ International Conference on Intelligent Robots and Systems, Daejeon, South Korea, pp. 5092–5098, 2016. [3] M. Orsag, C. Korpela, S. Bogdan, P. Oh. Lyapunov based model reference adaptive control for aerial manipulation. In Proceedings of International Conference on Unmanned Aircraft Systems, IEEE, Atlanta, USA, pp. 966–973, 2013. [4] A. E. Jimenez-Cano, J. Martin, G. Heredia, A. Ollero, R. Cano. Control of an aerial robot with multi-link arm for assembly tasks. In Proceedings of IEEE International Conference on Robotics and Automation, Karlsruhe, Germany, pp. 4916–4921, 2013. [5] S. Kim, S. Choi, H. J. Kim. Aerial manipulation using a quadrotor with a two DOF robotic arm. In Proceedings of IEEE/RSJ International Conference on Intelligent Robots and Systems, Tokyo, Japan, pp. 4990–4995, 2013. [6] J. Thomas, G. Loianno, J. Polin, K. Sreenath, V. Kumar. Toward autonomous avian-inspired grasping for micro aerial vehicles. Bioinspiration and Biomimetics, vol. 9, no. 2, Article number 025010, 2014. [7] N. Sun, Y. C. Fang, H. Chen, B. Lu. Amplitude-saturated nonlinear output feedback antiswing control for underactuated cranes with double-pendulum cargo dynamics. IEEE Transactions on Industrial Electronics, vol.64, no.3, pp.2135-2146, 2017. DOI:10.1109/TIE.2016.2623258 [8] B. Yuksel, G. Buondonno, A. Franchi. Differential flatness and control of protocentric aerial manipulators with any number of arms and mixed rigid-/elastic-joints. In Proceedings of IEEE/RSJ International Conference on Intelligent Robots and Systems, Daejeon, South Korea, pp. 561–566, 2016. [9] M. D. Shuster. A survey of attitude representations. The Journal of the Astronautical Sciences, vol.41, no.4, pp.439-517, 1993. [10] P. Castillo, A. Dzul, R. Lozano. Real-time stabilization and tracking of a four-rotor mini rotorcraft. IEEE Transactions on Control Systems Technology, vol.12, no.4, pp.510-516, 2004. DOI:10.1109/TCST.2004.825052 [11] J. F. Guerrero-Castellanos, N. Marchand, N. Hably, S. Lesecq, J. Delamare. Bounded attitude control of rigid bodies: Real-time experimentation to a quadrotor mini-helicopter. Control Engineering Practice, vol.19, no.8, pp.790-797, 2011. DOI:10.1016/j.conengprac.2011.04.004 [12] A. Alaimo, V. Artale, C. Milazzo, A. Ricciardello, L. Trefiletti. Mathematical modeling and control of a hexacopter. In Proceedings of International Conference on Unmanned Aircraft Systems, IEEE, Atlanta, USA, pp. 1043–1050, 2013. [13] S. Cho. Dynamics and Control of Underactuated Multibody Spacecraft, Ph. D. dissertation, University of Michigan, USA, 2002. [14] R. Cruz-Jose, J. F. Guerrero-Castellanos, W. F. Guerrero-Sánchez, J. J. Oliveros-Oliveros. Global stabilizarion of a VTOL mini aircraft. In rocedings of the National Conference of Automatic Control, Campeche, Mexico, pp. 180–185, 2012. [15] N. Marchand, A. Hably. Global stabilization of multiple integrators with bounded controls. Automatica, vol.41, no.12, pp.2147-2152, 2005. DOI:10.1016/j.automatica.2005.07.004 [16] A. R. Teel. Global stabilization and restricted tracking for multiple integrators with bounded controls. Systems & Control Letters, vol.18, no.3, pp.165-171, 1992. DOI:10.1016/0167-6911(92)90001-9 [17] E. N. Johnson, S. K. Kannan. Nested saturation with guaranteed real poles. In Proceedings of American Control Conference, IEEE, Denver, USA, vol. 1, pp. 497–502, 2003. [18] VICON, [Online], Available: http://www.vicon.com/products/camera-systems. [19] Build, run, and test real-time applications, [Online], Available: http://fr.mathworks.com/products/simulink-real-time/, December 2017. [20] E-flite RC, [Online], Available: http://www.horizonhobby.com/content/e-flite-rc.