Aerial Grasping of an Object in the Strong Wind: Robust Control of an Aerial Manipulator

Abstract

An aerial manipulator is a new kind of flying robot system composed of a rotorcraft unmanned aerial vehicle (UAV) and a multi-link robotic arm. It gives the flying robot the capacity to complete manipulation tasks. Steady flight is essential for an aerial manipulator to complete manipulation tasks. This paper focuses on the steady flight control performance of the aerial manipulator. A separate control strategy is used in the aerial manipulator system, in which the UAV and the manipulator are controlled separately. In order to complete tasks in environments with strong wind disturbance, an acceleration feedback enhanced robust H∞ controller was designed for the UAV in the aerial manipulator. The controller is based on the hierarchical inner-outer loop control structure of the UAV and composed of a robust H∞ controller and acceleration feedback enhanced term, which is used to compensate for the wind disturbance. Experimental results of aerial grasping of a target object show that the controller can suppress the wind disturbance effectively, and make the aerial manipulator hover steadily with sufficient accuracy to complete aerial manipulation tasks in strong wind.

1. Introduction

In recent years, aerial manipulation has become a hot research topic in the field of UAVs [1,2]. To complete aerial manipulation tasks, researchers have integrated tools with UAVs. For example, in one study, a gripper was installed on an UAV in so that the UAV could complete aerial grasping tasks [3,4]. For canopy sampling tasks, a quadrotor was equipped with scissors [5]. Although an UAV with a tool can accomplish simple aerial manipulation tasks, it has weaker operational capability. When a multilink robotic arm is installed on the UAV, it will give the traditional UAV a strong operational capability. So, the aerial manipulator composed of a rotorcraft UAV and a multi-link robotic arm can extend the application prospects of the UAV greatly. For example, a dual-arm aerial manipulator is used to screw the valve in disaster scenarios [6]. An open-tilted hex-rotor with a passive arm aids human operators to move long bars for assembly tasks [7], and a hex-rotor with a 7 degree of freedom (DoF) manipulator grasps a moving target [8]. In addition, an aerial manipulator system with advanced manipulation capability for industrial inspection and maintenance is being developed in such projects as AEROARMS [9] and AEROWORKS [10] financed by the European Community.

Without doubt, the steady flight of the UAV and the accurate operation of the end-effector are essential for an aerial manipulator to complete manipulation tasks. Thus, the aerial manipulator system control has attracted considerable attention, and a variety of control methods have been used in aerial manipulator systems. These include adaptive control [11,12,13], back-stepping [14,15], model predictive control (MPC) [16,17,18], impedance control [19,20], and so on. In one study, in order to carry and transport an unknown payload, an adaptive controller was designed for an aerial manipulator, composed of a quadrotor and a 2-DoF of robotic arm, which can estimate parameters of the payload online and track the desired trajectory [12]. In another study, a passive decomposition method is applied to decouple the dynamic of the aerial manipulator [14]. Based on the decoupled dynamic, a back-stepping controller was designed to control the UAV and end-effector simultaneously. In order to achieve optimal performance in the pick-and-place task, the nonlinear MPC method was used in an aerial manipulator composed of a quadrotor and a 2-DoF of robotic arm in [17]. To complete contact tasks, in one study an impedance control mothed was applied in an aerial manipulator composed of a helicopter and 7-DoF manipulator to pull a pole out of the fixing [20].

The above works on aerial manipulator system control have provided various methods to complete different manipulation tasks. These methods have been applied in experiments in structured environments, in which nothing can affect UAV control performance. However, in practical applications, there are various threats from the environment, such as strong gusty wind. The wind disturbance seriously damages the steady flight performance of the aerial manipulator, since the UAV’s driving force almost comes from aerodynamic force. Thus, robustness against wind disturbance is a critical characteristic of the aerial manipulator in practical applications.

So far, numerous studies have been focused on disturbance rejection control of the UAV, using different disturbance estimators. In one study a wind disturbance estimator was used in quadrotor control to reject wind disturbance based on the wind model, in which measurement of wind velocity is needed to estimate the disturbance [21]. In another study, a generalized extend state observer (GESO)-based disturbance estimator is used in the quadrotor attitude wind disturbance reject control, in which the disturbance is considered as a time polynomial function [22]. In a third study, a disturbance observer (DOB)-based method was used in quadrotor position control to reject the external force disturbance, in which the disturbance was estimated by the disturbance observer [23]. Whether wind disturbance or other external disturbance, their effects are always reflected by acceleration information. Thus, acceleration can be used to estimate the external disturbance of the UAV [24], and we call this method the acceleration feedback method for disturbance rejection control. In our previous work [25], the acceleration feedback method was applied to unmanned helicopter disturbance rejection control in simulations.

In this paper, a separate control strategy was used in the aerial manipulator system in which the UAV and the manipulator were controlled separately. In the UAV controller, the system center of mass (CoM) offset was used to compensate partial disturbance of the robotic arm. In the manipulator controller, the inverse kinematics of the aerial manipulator system was used to eliminate effects of the kinematic coupling between the UAV and the manipulator. Our previous work [8] showed that this control strategy works well in the target grasping tasks.

This paper pays more attention to the steady flight of the aerial manipulator in strong wind, and the acceleration feedback control method was applied into aerial manipulator control. The main contributions of this article are as follows:

• An acceleration feedback enhanced robust H∞ controller of UAV was designed, which can give the aerial manipulator the ability to reject the wind disturbance. The proof of bounded input bounded output) stability of the UAV under the H∞ controller is also presented.
• Successful grasping of an object using an aerial manipulator in a strong wind was performed. To the best of our knowledge, this is the first time an aerial manipulator system has completed this task in strong wind.

The rest of this paper is organized as follows. In Section 2, kinematics and dynamics of the aerial manipulator system are introduced. After that, the control scheme of the aerial manipulator system and acceleration feedback enhanced robust H∞ controller are given in Section 3. Then, the composition of the aerial manipulator system and the experiments of aerial grasping in the wind are presented in Section 4. Finally, conclusions are presented in Section 5.

2. System Model

In this section, firstly, system kinematics that reflect the kinematic coupling between the UAV and the robotic arm are introduced. Then, the dynamics of UAV with CoM offset are introduced, in which CoM offset term is used to describe the effect of the robotic arm on the dynamics of the UAV. They are both necessary for the system controller design based on the separate control strategy.

2.1. System Kinematics

The aerial manipulator presented in this paper is composed of a hex-rotor and a 7-DoF manipulator. The coordinate frames of the whole system are defined in Figure 1. The North-East-Down (NED) inertial frame, the body fixed frame of the hex-rotor and the end-effector frame are denoted by Σ_I, Σ_B and Σ_E, respectively. Point O is the coordinate origin of Σ_B and it coincides with the CoM of the hex-rotor. The CoM of the whole aerial manipulator system is denoted by point C. When the manipulator moves, the position of point C relative to point O will change. Hence, we use $r_{\mathit{oc}}$ to denote the relative position between point O and point C. The position and attitude of the hex-rotor and the end-effector are represented by those of Σ_B and Σ_E, respectively.

The absolute position of the hex-rotor with respect to Σ_I is denoted by $p_b$, and the attitude is described by the Z−Y−X Euler angles denoted by ${\Phi }_b={\left[\begin{array}{ccc}\phi & \theta & \psi \end{array}\right]}^T$, where φ, θ and ψ are roll, pitch and yaw angle, respectively. The rotation matrix from Σ_B to Σ_I is denoted by ${}^I{R}_B$. The detail of ${}^I{R}_B$ is as follow:

$${}^{I}R{}_B=\left[\begin{array}{ccc}c\theta c\psi & s\phi s\theta c\psi -c\phi s\psi & c\phi s\theta c\psi +s\phi s\psi \\ c\theta s\psi & s\phi s\theta s\psi +c\phi c\psi & c\phi s\theta s\psi -s\phi c\psi \\ -s\theta & s\phi c\theta & c\phi c\theta \end{array}\right]$$

(1)

where c and s denote trigonometric functions cos( ) and sin( ), respectively.

The absolute position of the end-effector with respect to Σ_I is denoted by $p_e$. The absolute attitude of the end-effector with respect to Σ_I is denoted by ${\Phi }_e$. The attitude of the end-effector relative to hex-rotor with respect to Σ_B is denoted by ${}^B\Phi {}_{\mathit{be}}$. As in Equation (1), ${\Phi }_e$ determines the rotation matrix from Σ_E to Σ_I, denoted by ${}^{I}R{}_E$, and ${}^B\Phi {}_{\mathit{be}}$ determines the rotation matrix from Σ_E to Σ_B, denoted by ${}^{B}R{}_E$. They have relation with the position and attitude of the hex-rotor as follows:

$$\{\begin{array}{l}{p}_e=p_b+{}^{I}R{}_B{}^{B}p{}_{\mathit{be}}\\ {}^{I}R{}_E={}^{I}R{}_B{}^{B}R{}_E\end{array}$$

(2)

where ${}^{B}p{}_{\mathit{be}}$ is position of the end-effector relative to hex-rotor with respect to Σ_B. For ${}^{B}p{}_{\mathit{be}}$ and ${}^{B}R{}_E$, they can be got through forward kinematics of the manipulator with the joint angle [26].$v_b$ denotes the absolute velocity of the hex-rotor with respect to Σ_I. ${}^B\omega {}_b$ denotes the absolute angular velocity of hex-rotor with respect to Σ_B. They have a relationship with $p_b$ and ${\Phi }_b$ as follows:

$$\{\begin{array}{l}{\dot{p}}_b=v_b\\ {\dot{\Phi }}_b=T({\Phi }_b){}^B\omega {}_b\end{array}$$

(3)

where $T({\Phi }_b)=\left[\begin{array}{ccc}1& s\phi \mathit{tan}\theta & c\phi \mathit{tan}\theta \\ 0& c\phi & -s\phi \\ 0& s\phi \mathit{sec}\theta & c\phi \mathit{sec}\theta \end{array}\right]$.

The absolute velocity and angular velocity of end-effector with respect to Σ_I are denoted by $v_e$ and ${\omega }_e$, respectively. They have a relationship with $v_b$ and ${}^B\omega {}_b$ as follows:

$$\{\begin{array}{l}{v}_e=v_b+{}^{I}R{}_B({}^B\omega {}_b\times {}^{B}p{}_{\mathit{be}}+{}^{B}v{}_{\mathit{be}})\\ {\omega }_e={}^{I}R{}_B({}^B\omega {}_b+{}^B\omega {}_{\mathit{be}})\end{array}$$

(4)

where ${}^{B}v{}_{\mathit{be}}$ and ${}^B\omega {}_{\mathit{be}}$ are the velocity and angular velocity of the end-effector relative to the UAV body fixed frame with respect to Σ_B. ${}^{B}v{}_{\mathit{be}}$ and ${}^{B}v{}_{\mathit{be}}$ can be got through velocity kinematics of the robotic arm, as follow:

$$\left[\begin{array}{c}{}^B{v}_{\mathit{be}}\\ {}^B{\omega }_{\mathit{be}}\end{array}\right]={}^B{J}_{\mathit{be}}(q)\dot{q}$$

(5)

where q is the joint angle vector of the robotic arm. $\dot{q}$ is joint velocity vector of the robotic arm. ${}^B{J}_{\mathit{be}}(q)$ is the Jacobian matrix of the robotic arm and it is a function of q.

2.2. Dynamic of the UAV

The aerial manipulator can be taken as a special aerial platform whose mass distribution could be changed due to the movement of the robotic arm. The disturbance that the manipulator imposes on the hex-rotor can be partially represented by the motion of the CoM offset. Thus, the CoM offset in the UAV’s dynamic model can partially reflect influence of the robotic arm, as in a previous work [27]. The dynamics of the hex-rotor with the CoM offset, $r_{\mathit{oc}}$, is expressed as follows [28]:

$${\dot{v}}_b=-\frac{F_t}{m_s}{}^{I}R{}_B{e}_3+g{e}_3+{}^{I}R{}_B({}^B\omega {}_b\times ({}^B\omega {}_b\times {}^{B}r{}_{\mathit{oc}}(q)))+\frac{{\Delta }_F}{m_s}$$

(6)

$${}^B\dot{\omega }{}_b=I_b^{-1}(\tau -{}^B\omega {}_b\times (I_b{}^B\omega {}_b)+{}^{B}r{}_{\mathit{oc}}(q)\times {}^{I}R{}_B^{-1}{m}_{s}g{e}_3)+I_b^{-1}{\Delta }_{\tau }$$

(7)

where $m_s$ is the total mass of the aerial manipulator system. g is the gravity acceleration. $e_1={\left[\begin{array}{ccc}1& 0& 0\end{array}\right]}^T$, $e_2={\left[\begin{array}{ccc}0& 1& 0\end{array}\right]}^T$ and $e_3={\left[\begin{array}{ccc}0& 0& 1\end{array}\right]}^T$ are the unit vectors with three dimensions. $I_b$ is the inertia matrix of the hex-rotor. F_t and τ are the thrust and torque acting on the hex-rotor, respectively, and they are generated by the rotors of the hex-rotor. ${}^{B}r{}_{\mathit{oc}}(q)$ is CoM offset with respect to Σ_B. ${}^{B}r{}_{\mathit{oc}}(q)$ is a function of the q that is joint angle vector of the robotic arm. Thus, based on the kinematics of the robotic arm [26], ${}^{B}r{}_{\mathit{oc}}(q)$ can be calculated through the measurement of the joint angles. ${\Delta }_F$ and ${\Delta }_{\tau }$ are force and torque disturbance, respectively, and they include wind disturbance and disturbance of the robotic arm excepting CoM offset terms.

Remark I: The CoM offset terms in the hex-rotor dynamics, Equations (6) and (7), contain the partial static disturbance of the robotic arm, and don’t contain the disturbance caused by the inertia matrix changing, and don’t contain the dynamic disturbance that is caused by joint velocity and acceleration of the robotic arm. We consider other parts of the disturbance in ${\Delta }_F$ and ${\Delta }_{\tau }$.

As we know, the driving thrust and torque of the hex-rotor come from the aerodynamic force of high speed rotating rotors. The hex-rotor totally has six rotors, and serial number and rotation direction of the rotors are shown in Figure 2. The angular velocity of rotor i is denoted by ${\omega }_i$ (i = 1; 2; …; 6). Thrust and torque are related to the rotor speed of the rotors as follow:

$$\left[\begin{array}{c}{F}_t\\ \tau \end{array}\right]=\left[\begin{array}{cccccc}{c}_T& c_T& c_T& c_T& c_T& c_T\\ -d{c}_T& d{c}_T& \raisebox{1ex}{$1$}\!\left/ \!\raisebox{-1ex}{$2$}\right.d{c}_T& -\raisebox{1ex}{$1$}\!\left/ \!\raisebox{-1ex}{$2$}\right.d{c}_T& -\raisebox{1ex}{$1$}\!\left/ \!\raisebox{-1ex}{$2$}\right.d{c}_T& \raisebox{1ex}{$1$}\!\left/ \!\raisebox{-1ex}{$2$}\right.d{c}_T\\ 0& 0& \raisebox{1ex}{$\sqrt{3}$}\!\left/ \!\raisebox{-1ex}{$2$}\right.d{c}_T& -\raisebox{1ex}{$\sqrt{3}$}\!\left/ \!\raisebox{-1ex}{$2$}\right.d{c}_T& \raisebox{1ex}{$\sqrt{3}$}\!\left/ \!\raisebox{-1ex}{$2$}\right.d{c}_T& -\raisebox{1ex}{$\sqrt{3}$}\!\left/ \!\raisebox{-1ex}{$2$}\right.d{c}_T\\ -c_{\tau }& c_{\tau }& -c_{\tau }& c_{\tau }& c_{\tau }& -c_{\tau }\end{array}\right]\left[\begin{array}{c}{\omega }_1{}^2\\ {\omega }_2{}^2\\ {\omega }_3{}^2\\ {\omega }_4{}^2\\ {\omega }_5{}^2\\ {\omega }_6{}^2\end{array}\right]=M\overline{\omega }$$

(8)

where $c_T$ is thrust coefficient. $c_{\tau }$ is torque coefficient. d is the distance from center of rotor to the geometrical center of the hex-rotor. M is the mixer matrix of the hex-tor and $\overline{\omega }$ is the vector of square of rotor speed.

3. System Control

3.1. Hex-rotor Control

The control structure of the aerial manipulator is shown in Figure 3. The hex-rotor controller is composed of a H∞ controller and acceleration feedback enhance term. The controller is based on a hierarchical inner-outer loop structure, in which the inner loop is the attitude control loop and the outer loop is the position control loop [29]. The H∞ controller is designed to ensure the BIBO stability of the hex-rotor. The acceleration feedback enhance term is used to compensate for wind disturbance, to improve control performance.

3.1.1. Translational and Rotational Dynamic Decoupling

The system dynamic models in Equations (6) and (7) are the translational dynamic and rotational dynamic, respectively. In the hierarchical inner-outer loop control structure, the UAV is seen as a cascade system composed of two subsystems, that is, the translational dynamic subsystem and rotational dynamic subsystem, respectively. From Equation (6), we can see that the translational dynamic couples with the rotational dynamic through the rotation matrix ${}^I{R}_B$. In order to decouple them, a virtual control input, ${\nu }_1$, is introduced [29], and it is defined as follows:

$${\nu }_1=-\frac{F_t}{m_s}{}^{I}R{}^{B,d}e{}_3+g{e}_3+{}^{I}R{}_B({}^B\omega {}_b\times ({}^B\omega {}_b\times {}^{B}r{}_{\mathit{oc}}(q)))$$

(9)

where ${}^I{R}_{B,d}$ is the rotation matrix determined by the desired attitude angle, ${\Phi }_{b,d}={\left[\begin{array}{ccc}{\phi }_d& {\theta }_d& {\psi }_d\end{array}\right]}^T$, which is needed in the position control loop. Substituting Equation (9) into Equation (6), the translational dynamic of the hex-rotor can be expressed as follows:

$${\dot{v}}_b={\nu }_1+\frac{{\Delta }_F}{m_s}+\delta (e_{{\Phi }_b})$$

(10)

$$\delta (e_{{\Phi }_b})=\frac{F_t}{m_s}({}^{I}R{}^{B,d}-{}^{I}R{}^B)e_3$$

(11)

where $e_{{\Phi }_b}$ is the attitude error defined as $e_{{\Phi }_b}={\Phi }_b-{\Phi }_{b,d}$. $\delta (e_{{\Phi }_b})$ is the interconnection term between the translational and rotational dynamic after decoupled by the virtual input ${\nu }_1$. This virtual input is given by the H∞ position controller, which will be presented in the next subsection. Then, in order to actuate the outer loop actually, we need to translate the virtual input into the thrust and desired attitude angle, which can be tracked fast enough in the attitude control loop [29]. We define the virtual input vector as ${\nu }_1={\left[\begin{array}{ccc}{\nu }_{1x}& {\nu }_{1y}& {\nu }_{1z}\end{array}\right]}^T$. Combining Equation (1) and Equation (9), we get the thrust, desired roll angle and desired pitch angle as follows:

$$\{\begin{array}{l}{F}_t=\Vert m_s{\nu }_1-m_{s}g{e}_3-m_s{}^{I}R{}_B({}^B\omega {}_b\times ({}^B\omega {}_b\times {}^{B}r{}_{\mathit{oc}}(q)))\Vert \\ {\phi }_d=\mathit{arcsin}(\frac{m_s}{F_t}({\nu }_1{}_{x}s{\psi }_d-{\nu }_1{}_{y}c{\psi }_d))\\ {\theta }_d=\mathit{arctan}(\frac{1}{v_1{}_z-g}({\nu }_1{}_{x}c{\psi }_d-{\nu }_1{}_{y}s{\psi }_d))\end{array}$$

(12)

Linearization of the rotational dynamic is essential to design the H∞ controller. Based on the feedback linearization theory of the MIMO (Multiple Input Multiple Output) system [30], the rotational dynamic in Equation (7) is feedback linearizable at $\theta \ne \pi /2$ through the following equation:

$$\tau =I_b{T}^{-1}({\Phi }_b)(-\dot{T}({\Phi }_b){}^B\omega {}_b+T({\Phi }_b)I_b^{-1}({}^B\omega {}_b\times (I_b{}^B\omega {}_b)+{\nu }_2))-{}^{B}r{}_{\mathit{oc}}(q)\times {}^{I}R{}_B^{-1}{m}_{s}g{e}_3$$

(13)

where ${\nu }_2$ is the virtual input of the linearized system, and it is given by the H∞ attitude controller. Thus, after decoupling and linearizing, the system dynamic will be as follows:

$$\{\begin{array}{c}{\dot{v}}_b={\nu }_1+\delta (e_{{\Phi }_b})+\frac{{\Delta }_F}{m_s}\\ {\ddot{\Phi }}_b={\nu }_2+T({\Phi }_b)I_b^{-1}{\Delta }_{\tau }\end{array}$$

(14)

3.1.2. H∞ Controller

The new system in Equation (14) becomes a linear system including the interconnection term and disturbance. For a system without disturbance, a linear controller can guarantee the asymptotic stability [29]. In this section, the design of a robust H∞ control to ensure the BIBO stability of the system with the interconnection term and disturbance is described. The robust H∞ controller was designed under the assumption that the external disturbance is bounded. Although several works show such an assumption is restrictive [31,32], such an assumption can be taken in practice for an aerial manipulator system, because the manipulator always works as a fixed base manipulator in real aerial manipulation applications. So, we consider it as bound disturbance in this paper. There exist works in the literature [31,33] that tackle uncertainty in a better way, which can be considered in the future.

Remark II: In practice, the unmodeled part disturbance of the manipulator, ${\Delta }_F$ and ${\Delta }_{\tau }$, can be seen as bound disturbance in the working states.

For the hex-rotor, the position and yaw angle are usually chosen as system outputs. We use $p_{b,d}$ to denote the desired position. The system state error is denoted by $e_{p_b}=p_b-p_{b,d}$, $e_{v_b}=v_b-v_{b,d}$, $e_{{\Phi }_b}={\Phi }_b-{\Phi }_{b,d}$ and $e_{{\dot{\Phi }}_b}={\dot{\Phi }}_b-{\dot{\Phi }}_{b,d}$. Combining Equation (3) with Equation (14), the error dynamic of the system can be written as the following equation:

$$\{\begin{array}{l}\dot{x}=\mathit{Ax}+\mathit{Bu}+E\delta (e_{{\Phi }_b})+D\Delta \\ y=\mathit{Cx}\end{array}$$

(15)

where $x=\left[\begin{array}{c}{e}_{p_b}\\ e_{v_b}\\ e_{{\Phi }_b}\\ e_{{\dot{\Phi }}_b}\end{array}\right]$, $y=\left[\begin{array}{c}{e}_{p_b}\\ e_{\psi }\end{array}\right]$, $A=\left[\begin{array}{cccc}{O}_{3\times 3}& I_{3\times 3}& O_{3\times 3}& O_{3\times 3}\\ O_{3\times 3}& O_{3\times 3}& O_{3\times 3}& O_{3\times 3}\\ O_{3\times 3}& O_{3\times 3}& O_{3\times 3}& I_{3\times 3}\\ O_{3\times 3}& O_{3\times 3}& O_{3\times 3}& O_{3\times 3}\end{array}\right]$, $B=\left[\begin{array}{cc}{O}_{3\times 3}& O_{3\times 3}\\ I_{3\times 3}& O_{3\times 3}\\ O_{3\times 3}& O_{3\times 3}\\ O_{3\times 3}& I_{3\times 3}\end{array}\right]$, $C=\left[\begin{array}{ccccc}{I}_{3\times 3}& O_{3\times 3}& O_{3\times 2}& O_{3\times 1}& O_{3\times 3}\\ O_{1\times 3}& O_{1\times 3}& O_{1\times 2}& I_{1\times 1}& O_{3\times 3}\end{array}\right]$, $D=\left[\begin{array}{cc}{O}_{3\times 3}& O_{3\times 3}\\ I_{3\times 3}& O_{3\times 3}\\ O_{3\times 3}& O_{3\times 3}\\ O_{3\times 3}& I_{3\times 3}\end{array}\right]$, $E=\left[\begin{array}{c}{O}_{3\times 3}\\ I_{3\times 3}\\ O_{3\times 3}\\ O_{3\times 3}\end{array}\right]$, $\Delta =\left[\begin{array}{c}\frac{1}{m_s}{\Delta }_F\\ T({\Phi }_b)I_b^{-1}{\Delta }_{\tau }\end{array}\right]$, $u=\left[\begin{array}{c}{\nu }_1-{\ddot{p}}_{b,d}\\ {\nu }_2-{\ddot{\Phi }}_{b,d}\end{array}\right]$.

For the interconnection term $\delta (e_{{\Phi }_b})$, we have following theorem.

Theorem 1.

For$\delta (e_{{\Phi }_b})$in the system (15), there exists a constant$\sigma$, such that

$$\Vert \delta (e_{{\Phi }_b})\Vert \le \Vert \mathit{Fx}\Vert$$

(16)

where$F=\left[\begin{array}{cccc}{O}_{3\times 3}& O_{3\times 3}& \sigma I_{3\times 3}& O_{3\times 3}\end{array}\right]$.

The proof of Theorem 1 is given in the Appendix A.

Theorem 2.

If a feedback controller$u=\mathit{Kx}$and a positive definite symmetric matrix P can be found satisfying the following inequality:

$$P(A+\mathit{BK})+{(A+\mathit{BK})}^{T}P+\frac{1}{{\gamma }^2}\mathit{PD}{D}^{T}P+\frac{1}{\lambda }\mathit{PE}{E}^{T}P+C^{T}C+\lambda F^{T}F\le 0$$

(17)

where, λ is a positive constant. Then, system in Equation (15) will be finite gain L₂-stable from disturbances Δ to outputs y and the L₂ gain is less than or equal to γ.

Proof. 

The Lyapunov function of the closed-loop system is defined as follow:

$$V(x)=x^T\mathit{Px}$$

(18)

Thus we have,

$$\begin{array}{ll}\dot{V}(x)& =x^T((A+\mathit{BK}{)}^{T}P+P(A+\mathit{BK}))x+{\Delta }^T{D}^T\mathit{Px}+x^T\mathit{PD}\Delta +{\delta }^T(e_{{\Phi }_b})E^T\mathit{Px}+x^T\mathit{PE}\delta (e_{{\Phi }_b})\\ & =x^T((A+\mathit{BK}{)}^{T}P+P(A+\mathit{BK}))x+\frac{1}{{\gamma }^2}{x}^T\mathit{PD}{D}^T\mathit{Px}+\frac{1}{\lambda }{x}^T\mathit{PE}{E}^T\mathit{Px}\\ & +x^T{C}^T\mathit{Cx}+\lambda {\Vert \delta (e_{{\Phi }_b})\Vert }^2-{\gamma }^2{\Vert \Delta -\frac{1}{{\gamma }^2}{D}^T\mathit{Px}\Vert }^2-\lambda {\Vert \delta (e_{{\Phi }_b})-\frac{1}{\lambda }{E}^T\mathit{Px}\Vert }^2+{\gamma }^2{\Vert \Delta \Vert }^2-{\Vert y\Vert }^2\end{array}$$

(19)

Combining Equation (16) with Equation (19), yields,

$$\begin{array}{ll}\dot{V}(x)& \le x^T((A+\mathit{BK}{)}^{T}P+P(A+\mathit{BK}))x+\frac{1}{{\gamma }^2}{x}^T\mathit{PD}{D}^T\mathit{Px}+\frac{1}{\lambda }{x}^T\mathit{PE}{E}^T\mathit{Px}+x^T{C}^T\mathit{Cx}\\ & +\lambda {\Vert \delta (e_{{\Phi }_b})\Vert }^2+{\gamma }^2{\Vert \Delta \Vert }^2-{\Vert y\Vert }^2\\ & \le x^T((A+\mathit{BK}{)}^{T}P+P(A+\mathit{BK}))x+\frac{1}{{\gamma }^2}{x}^T\mathit{PD}{D}^T\mathit{Px}+\frac{1}{\lambda }{x}^T\mathit{PE}{E}^T\mathit{Px}+x^T{C}^T\mathit{Cx}\\ & +\lambda x^T{F}^T\mathit{Fx}+{\gamma }^2{\Vert \Delta \Vert }^2-{\Vert y\Vert }^2\end{array}$$

(20)

If Equation (17) is satisfied, we have,

$$\dot{V}(x)\le {\gamma }^2{\Vert \Delta \Vert }^2-{\Vert y\Vert }^2$$

(21)

This means that, for any T>0,

$$\begin{array}{l}{\displaystyle {\int }_0^T({\Vert y(t)\Vert }^2-{\gamma }^2{\Vert \Delta (t)\Vert }^2)}\mathit{dt}\\ ={\displaystyle {\int }_0^T({\Vert y(t)\Vert }^2-{\gamma }^2{\Vert \Delta (t)\Vert }^2+\dot{V}(x(t)))}\mathit{dt}+V(x(T))\\ \le V(x(T))\end{array}$$

(22)

That is,

$${\displaystyle {\int }_0^T{\Vert y(t)\Vert }^2}\mathit{dt}\le {\displaystyle {\int }_0^T{\gamma }^2{\Vert \Delta (t)\Vert }^2}\mathit{dt}+V(x(T))$$

(23)

This means that the system is finite gain L₂-stable from disturbance Δ to outputs y and the L₂ gain is less than or equal to γ.

The gain matrix K can be got through the following LMI:

$$\left[\begin{array}{ccccc}\mathit{AX}+\mathit{BW}+{(\mathit{AX}+\mathit{BW})}^T& \frac{1}{\gamma }D& \frac{1}{\sqrt{\lambda }}E& {(\mathit{CX})}^T& \sqrt{\lambda }{(\mathit{FX})}^T\\ \frac{1}{\gamma }{D}^T& -I& O& O& O\\ \frac{1}{\sqrt{\lambda }}{E}^T& O& -I& O& O\\ \mathit{CX}& O& O& -I& O\\ \sqrt{\lambda }(\mathit{FX})& O& O& O& -I\end{array}\right]\le 0$$

(24)

where X is a positive definite matrix. Once W and X are obtained, the control law is as follow:

$$u=\mathit{Kx}=W{X}^{-1}x$$

(25)

Then the closed-loop system will become asymptotically stable and the H∞ norm from disturbance ∆ to output y is less than or equal to γ. So, the virtual inputs, ${\nu }_1$ and ${\nu }_2$, are as follows:

$$\left[\begin{array}{c}{\nu }_1\\ {\nu }_2\end{array}\right]=\mathit{Kx}+\left[\begin{array}{c}{\ddot{p}}_{b,d}\\ {\ddot{\Phi }}_{b,d}\end{array}\right]$$

(26)

Combining Equation (12) and Equation (13), the F_t and τ needed for the hex-rotor can be found. It is the H∞ controller of the hex-rotor that can ensure system BIBO stability and compensate partial disturbance of the robotic arm. □

3.2. Acceleration Feedback Enhanced

When the aerial manipulator works in strong wind, although the H∞ controller can ensure system stability with proper parameters, the control performance will be seriously reduced by the wind disturbance. To attenuate disturbance of wind and unmolded part of the robotic arm, the acceleration feedback enhanced method is used in the H∞ controller. Firstly, we used the Kalman Filter to estimate the acceleration and angular acceleration of the hex-rotor, and they are denoted by ${\widehat{a}}_b$ and ${\widehat{\alpha }}_b$, respectively [34]. As we know, velocity and acceleration can form a first order linear system:

$$\{\begin{array}{l}\left[\begin{array}{c}{v}_k\\ a_k\end{array}\right]=\left[\begin{array}{cc}1& \Delta t\\ 0& 1\end{array}\right]\left[\begin{array}{c}{v}_{k-1}\\ a_{k-1}\end{array}\right]+{\xi }_k\\ {\tilde{v}}_k=v_k+{\eta }_k\end{array}$$

(27)

where, $v_k$ is true velocity at time $t_k$. ∆t is the time interval, and ${\xi }_k$ is the process noise which is assumed to be zero mean multivariate normal distribution with covariance $Q_k$, and ${\xi }_k\sim N(0,Q_k)$. ${\tilde{v}}_k$ is the measurement velocity at time $t_k$. ${\eta }_k$ is the observation noise, assumed to be zero mean Gaussian white noise with covariance $R_k$, and ${\eta }_k\sim N(0,R_k)$. The critical elements for the Kalman Filter are the covariance of process and observation noise. With the measurement velocity ${\tilde{v}}_k$ and system Equation (27), the acceleration can be estimated by Kalman Filter. For the angular acceleration, it is similar to the linear acceleration.

Based on Equation (14), the total disturbance in the translational and rotational dynamic can be estimated as follows:

$$\widehat{\Delta }=\left[\begin{array}{c}{\widehat{\Delta }}_1\\ {\widehat{\Delta }}_2\end{array}\right]=\left[\begin{array}{c}{\nu }_1-{\widehat{a}}_b\\ {\nu }_2-{\widehat{\alpha }}_b\end{array}\right]$$

(28)

where ${\widehat{\Delta }}_1$ and ${\widehat{\Delta }}_2$ are the estimated disturbance in the translational and rotational dynamic, respectively

Usually, the estimated acceleration includes high frequency noise because of the vibration, so that estimated disturbance, $\widehat{\Delta }$, cannot be used directly in the controller. The wind disturbance tends to lower frequency. As shown in Figure 3, to eliminate high frequency noise, $\widehat{\Delta }$ is combined with a low pass filter as follow:

$${\nu }_{\mathit{af}}(S)=Q(S)\widehat{\Delta }(S)$$

(29)

where $Q(S)$ is a second order low frequency pass filter. ${\nu }_{\mathit{af}}={\left[\begin{array}{cc}{\nu }_{1\mathit{af}}& {\nu }_{2\mathit{af}}\end{array}\right]}^T$ is an acceleration feedback enhanced term, and can be added into the controller of Equation (12) and (13) to compensate wind disturbance.

3.3. Manipulator Control

The manipulator controller structure is shown in Figure 3. The position controller of the end-effector is a PID (Proportion Integral Derivative) controller, so that we can get the desired velocity of the end-effector in the frame Σ_I, and they are denoted by $v_{e,d}$ and ${\omega }_{e,d}$, respectively. The velocity control of the end-effector is realized in the frame Σ_B. The desired velocities of the end-effector in the frame Σ_B are denoted by ${}^{B}v{}_{\mathit{be},d}$ and ${}^B\omega {}_{\mathit{be},d}$, respectively. To get ${}^{B}v{}_{\mathit{be},d}$ and ${}^B\omega {}_{\mathit{be},d}$, inverse velocity kinematics of the aerial manipulator system is used in the manipulator controller. Through inversing Equation (4), ${}^{B}v{}_{\mathit{be},d}$ and ${}^B\omega {}_{\mathit{be},d}$ can be found as follows:

$$\{\begin{array}{l}{}^{B}v{}_{\mathit{be},d}={}^{I}R{}_B^{-1}(v_{e,d}-v_b)-{}^B\omega {}_b\times {}^{B}p{}_{\mathit{be}}\\ {}^B\omega {}_{\mathit{be},d}={}^{I}R{}_B^{-1}{\omega }_{e,d}-{}^B\omega {}_b\end{array}$$

(30)

The theme of velocity control of a manipulator is to find the right joint velocity that can generate the desired velocity of the manipulator end-effector. After getting the desired joint velocity, denoted by ${\dot{q}}_d$, the actuators have a low level controller, which can track the desired joint velocity with enough accuracy. The velocity control of the redundant manipulator has a framework that is introduced in a previous work [35], which can minimize $\frac{1}{2}{\dot{q}}^{T}W\dot{q}+\alpha \dot{f}(q)$, where W is a symmetric positive-definite weighting matrix and $\alpha$ is a scalar. This frame can be described as follow:

$${\dot{q}}_d={\left[\frac{{}^{B}J{}_{\mathit{be}}(q)}{N_J^{T}W}\right]}^{-1}\left[\frac{V_d}{-\alpha N_J^T\Delta f}\right]$$

(31)

where $V_d={\left[\begin{array}{cc}{}^{B}v{}_{\mathit{be},d}& {}^B\omega {}_{\mathit{be},d}\end{array}\right]}^T$. $N_J^{}$ is a matrix whose columns are a spanning set of the null space of ${}^{B}J{}_{\mathit{be}}(q)$. That is ${}^{B}J{}_{\mathit{be}}(q)N_J^{}=0$. $\Delta f$ is the gradient of f(q), which is a function of the joint angle vector.

4. Experiments and Results

In order to validate the wind disturbance rejection performance of the acceleration feedback enhanced H∞ controller presented in Section 3, the experiments of aerial grasping of a target object in the wind are conducted. The experimental platform and results will be introduced in this section.

4.1. Experimental Platform

The experimental platform is shown in Figure 4. The axial flow fan is used to generate wind disturbance. The anemometer is used to measure the wind speed. The aerial manipulator system mainly consists of a hex-rotor UAV, a 7-DoF robotic arm and an under-actuated gripper. The main parameters of the aerial manipulator are listed in

Table 1. Parameters of aerial manipulator.

Parameter	Value
Hex-rotor mass	3 kg
Maximum takeoff weight of the hex-rotor	5.5 kg
Hex-rotor diagonal size	80 cm
Flight time	15 min
Arm mass (including gripper)	1.5 kg
Arm length	53.4 cm
Maximum opening of gripper	12 cm

. The tuned propulsion system of the hex-rotor is E800 of DJI powered by a 6 cell Li-Po battery. The flight stack of the hex-rotor is an open source controller for autonomous drones named PixHawk. The robotic arm is composed of Dynamixel smart actuators. The computer used as the arm controller is Intel NUC with a core-i7 processor. The under-actuated gripper is designed based on the Yale OpenHand Project [36], with some structures and parameters changed. The experiments are conducted in the OptiTrack, an indoor motion capture system.

The operational space of the aerial manipulator system is shown in Figure 5. The operational space Ω is a space in the front of the hex-rotor, in which the target object is away from the hex-rotor with a safe distance, and is also reachable for the robotic arm. The space is part of the space between two concentric spherical surfaces, whose center is point O (the coordinate origin Σ_B.) and radiuses are l_min and l_max, respectively. The area is also limited in four planes. Its projection on the X_B-O-Y_B and X_B-O-Z_B planes are shown in Figure 6. Any point T in the space will satisfy the following inequalities:

$$\{\begin{array}{l}-\frac{1}{2}\alpha \le \langle X_B,{\mathit{Proj}}_{\mathit{xoy}}{(}^B{p}_{\mathit{bt}})\rangle \le \frac{1}{2}\alpha \\ l_{\mathit{min}}\le \Vert {}^B{p}_{\mathit{bt}}\Vert \le l_{\mathit{max}}\\ {\beta }_{\mathit{min}}\le \langle X_B,{\mathit{Proj}}_{\mathit{xoz}}{(}^B{p}_{\mathit{bt}})\rangle \le {\beta }_{\mathit{max}}\end{array}$$

(32)

where ${}^B{p}_{\mathit{bt}}$ is the position of point T relative to hex-rotor body fixed frame with respect to Σ_B. Proj(.) is a function that projects a vector in the coordinate plane (X-O-Y or X-O-Z plane). α is the yaw angle range of the space Ω. β_min and β_max are the pitch angle range limit of the space Ω.

4.2. Aerial Grasping in the Wind

When the aerial manipulator grasps a target object, the hex-rotor is hovering at one point to keep the object in the operational space, so that the manipulator can grasp the target safely. Thus, high precision hovering is essential for the aerial manipulator to grasp a target in the wind. If the hex-rotor cannot keep the target in the operational space all the time, the target will not be reachable for the robotic arm.

The speed of wind disturbance in the experiments is shown in Figure 7a. The wind was sweeping periodically, as in Figure 7a, because the axial flow fan is switched between turn-on and turn-off to generate gusty wind disturbance. The mean and variance of the gusty wind speed were 6.54 m/s and 1.5265 m/s, respectively. The wind speed in the frequency domain is shown in Figure 7b. The main frequency of the wind disturbance was 0-0.9 Hz. Therefore, to eliminate the main part of the wind disturbance, 0.9 Hz was chosen as the cut-off frequency of the low pass filter, Q(S), in the acceleration feedback enhanced controller.

In order to validate the wind disturbance rejection performance of the acceleration feedback method, in the experiments the aerial manipulator was controlled by the robust H∞ controller without and with the acceleration feedback enhanced term. The system output control results are shown in Figure 8. The mean and variance of absolute value of control errors are shown in Figure 9. Contrasting the position and yaw error in the two experiments, we can see that the position control accuracy of the hex-rotor was improved significantly with acceleration feedback enhanced term. It means that the acceleration feedback term can reject the wind disturbance effectively.

The position of the target object in the operational space in the experiments is shown in Figure 10. It can be seen that without the acceleration feedback enhanced term, the aerial manipulator cannot keep the target object in the operational space all the time because of the weaker disturbance rejection ability. Therefore, the target becomes unreachable for the manipulator, as shown in Figure 11. As a result, the aerial grasping task fails. With the acceleration feedback enhanced term, the hex-rotor has sufficient disturbance rejection ability, and the position control accuracy of the hex-rotor is improved. Thus, the target object is kept within the operational space all the time, and the aerial grasping task is accomplished in the end, as shown in Figure 11. (The experimental video is available at the following website: https://v.youku.com/v_show/id_XNDA4NDEwODE4NA==.html?spm=a2h3j.8428770.3416059.1)

5. Conclusions

In this paper, in order to give an aerial manipulator the ability to work in the wind, we present an acceleration feedback enhanced H∞ controller. The proposed controller is based on the hierarchical inner-outer loop structure, and composed of a robust H∞ controller and the acceleration feedback enhanced term, which is used to reject the wind disturbance. This controller can improve the position control accuracy of an aerial manipulator in the wind, so that it can complete grasping tasks with wind disturbance. In future work, we will pay more attention to impedance control of the aerial manipulator to complete more complicated tasks.