A Novel Active Anti-Disturbance Control Strategy for Unmanned Aerial Manipulator Based on Variable Coupling Disturbance Compensation

Abstract

Inspired by the kangaroo’s active tail wagging to stabilize its body posture while jumping, this paper proposes an active anti-disturbance control strategy for unmanned aerial manipulators based on variable coupling disturbance compensation (${\mathrm{AADC}}_{\mathrm{VCD}}$), which can achieve the active and energy-saving anti-disturbance performance of “using the enemy’s strength against the enemy” to keep the UAM stable under disturbances. First, the goal of using the coupling disturbance generated by the active swing of the manipulator as a control input signal for active anti-disturbance is clarified. Then, based on the proposed variable coupling disturbance model, this goal is formulated as a nonlinear programming optimization problem under specific physical constraints and solved. Finally, the coupling disturbance torque generated when the manipulator executes an active swing to the solved desired joint angles can be used to compensate and suppress other disturbances of the UAM, thereby achieving active anti-disturbance. The effectiveness and superiority of the proposed ${\mathrm{AADC}}_{\mathrm{VCD}}$ were validated through two simulations in Simscape. The simulation results demonstrated that our approach achieved a good active anti-disturbance and energy-saving performance, significantly reducing the position offset of the UAM caused by disturbances and improving the UAM’s ability to maintain stability.

1. Introduction

An unmanned aerial manipulator (UAM), typically comprising an unmanned aerial vehicle (UAV) and a n-degree of freedom (DOF) manipulator, allows UAVs to physically interact with the environment and carry out aerial works, making it a current research hotspot [1,2,3,4]. UAMs often work in intricate environments, subject to multi-source disturbances [5]. These disturbances affecting UAMs can be classified into two main types: coupling disturbance and lumped disturbances. The former arises from the interaction of different components within the UAM system, while the latter stem from uncertainties, unmodeled dynamics, and external disturbances [6]. Improving the anti-disturbance control performance of UAMs under these disturbances is crucial for maintaining stability and flight safety.

The movement of the manipulator coupled with the load on the end-effector can alter the center of mass (CoM) and moment of inertia (MoI) of UAMs and generate strong coupling disturbances, which are a big challenge for the stable flight of UAMs [7,8]. Current research on compensation and suppression of coupling disturbance mainly focuses on the oscillations between feedforward and feedback compensation [9,10]. For instance, force sensors have been used to directly measure the coupled disturbance and perform direct feedforward compensation [11]. In [7,12,13], the authors established a coupling disturbance model by considering the center of mass offset and carrying out feedforward compensation. In [14,15,16], disturbance estimation methods such as adaptive neural networks and an extended state observer were designed to estimate and feedback compensate coupling disturbances. While these studies achieved good results in coupling disturbance compensation and suppression, none of them considered the perspective that the coupling disturbance is exploitable. With the development of certain fault-tolerant control research in recent years, this lack has been made up for. In [17,18,19], the authors achieved a fail-safe flight solution under motor failure by analyzing and utilizing the coupling disturbance torque exerted by the manipulator on the UAV. However, these coupling disturbance utilization schemes under extreme fault conditions are not applicable for regular UAM flights.

An UAM is subjected to various lumped disturbances in the actual aerial work environment, arising from factors including wind gusts, unmodeled dynamics, and other external factors [20]. To provide UAMs with robustness against these disturbances, existing methods basically rely on various advanced controllers combined with various online disturbance-estimation-based approaches to estimate and compensate for the lumped disturbances [6,21,22,23,24]. For example, an adaptive sliding-mode disturbance observer-based finite-time control scheme [6], compliant control strategy with FIR-based disturbance observer [21], and an adaptive NN backstepping control method [9] were proposed to estimate and compensate for this effect.

Although various methods for compensation and suppression of coupling disturbance and lumped disturbances have been proposed, as mentioned above, there is a common feature among them. The final execution of these methods uses the aerodynamic force generated by the propellers to suppress these disturbances, and they do not take full advantage of the active advantages of the UAM’s manipulator. There are many animals in nature that fully exploit the advantages of their own limbs [25], and their behaviors can give unique inspiration to our research [26]. Kangaroos are well known jumping animals with hind limbs and are good at using their strong tails for active swinging to maintain balance during jumping, as shown in Figure 1 below.

Inspired by the kangaroo’s active tail wagging to stabilize its body posture while jumping, this paper proposes an active anti-disturbance control strategy based on variable coupling disturbance compensation ${\mathrm{AADC}}_{\mathrm{VCD}}$ for UAMs, which can achieve an active and energy-saving anti-disturbance performance and keep the UAM stable under disturbances. First, we set the goal of using the coupling disturbance generated by the active swing of the UAM’s own manipulator arm as the control input signal for active disturbance rejection. Subsequently, utilizing the proposed variable coupling disturbance model, we formulated the objective of active disturbance rejection as a nonlinear programming optimization problem [28] under specific physical constraints. Solving this optimization problem could obtain the desired joint angles of the manipulator for the active swing. Finally, the coupling disturbance torque, which is directly generated when the manipulator actively swings to the desired joint angles, could be used to compensate and suppress the impact of other disturbances on the UAM, thereby achieving active and energy-saving anti-disturbance control effects.

The salient contribution of this work is to provide a new active anti-disturbance implementation mechanism for UAMs, namely ${\mathrm{AADC}}_{\mathrm{VCD}}$. This kind of active anti-disturbance strategy of “using the enemy’s strength against the enemy” has the following advantages:

• The active advantage of using the UAM’s own manipulator can reduce energy consumption, increase UAM endurance time, and further improve the capability and limit of resisting external lumped disturbances torque. The variable load on the end-effector also contributes to this improvement.
• The coupling disturbance torque generated by the active swing of the manipulator can be faster and more direct than the aerodynamic torque generated by the propellers, because the latter can be affected by a slow aerodynamic response and slow motor response.
• The proposed ${\mathrm{AADC}}_{\mathrm{VCD}}$ can also be used to achieve posture balance in highly dynamic scenarios and provide the required tilting torque for UAMs and UAVs to perform quick attitude maneuvers, such as maintaining stability at the moment when a heavy object falls in an UAM pick and place scenario.

The remainder of this paper is structured as follows: In Section 2, a dynamics model of UAMs, variable coupling disturbance model, and problem statement are described. After presenting the details of the proposed ${\mathrm{AADC}}_{\mathrm{VCD}}$ in Section 3, we validate it through two simulations in Simscape in Section 4, namely keeping stable in a pick and place scenario and suppressing lumped disturbance using ${\mathrm{AADC}}_{\mathrm{VCD}}$. Finally, Section 5 concludes this paper and discusses potential avenues for future research.

2. Dynamics Modeling and Problem Statement

2.1. UAM Dynamics Modeling

In this paper, the considered UAM comprised a UAV base and a n-DOF manipulator. Let ${\Sigma }_B$ and ${\Sigma }_I$ denote the body-fixed coordinate system and the inertial coordinate system (NED), respectively. The former has its $X_B$-axis aligned with the UAV’s head direction, with the $Z_B$-axis pointing towards the ground, while the original point o of the coordinate system is situated at the UAV’s center of mass. Figure 2 illustrates the established coordinate system for the UAM. ${\Sigma }_{i(i=1,2\cdots ,n)}$ represent each link’s coordinate system within the manipulator, which are formulated based on a modified DH parameter method [29].

According to reference [7], the dynamics model of UAMs can be articulated under the influence of both coupling disturbances and lumped disturbances. This formulation is presented as follows:

$$\left\{\begin{array}{cc}\hfill {\dot{p}}_b=& v_b\hfill \\ \hfill {\dot{v}}_b=& -\frac{F_l}{m_s}{ }^I{R}_B{e}_3+g{e}_3\hfill \\ \hfill {\dot{\Phi }}_b=& T\left({\Phi }_b\right){ }^B{\omega }_b\hfill \\ \hfill { }^B{\dot{\omega }}_b=& I_b^{-1}(\tau -{ }^B{\omega }_b\times \left(I_b{ }^B{\omega }_b\right)+{ }^B{\tau }_{cd}+{\tau }_{lum})\hfill \end{array}\right.$$

(1)

where $p_b={[x,y,z]}^T$ and $v_b={[\dot{x},\dot{y},\dot{z}]}^T$ denote the position vector and velocity vector of the UAV in ${\Sigma }_I$. $m_s$ represents the total mass of the UAM, which encompasses both the mass of the UAV $m_b$ and that of the manipulator $m_{man}$. $F_l$ and $\tau$ represent the lift force and the combined torque vector generated by the propellers of the UAV. g is the gravity acceleration. ${ }^I{R}_B$ represents the rotation matrix from ${\Sigma }_B$ to ${\Sigma }_I$. $e_3={[0,0,1]}^T$. ${ }^B{\tau }_{cd}$ represents the actual coupling disturbance torque exerted on the UAM. ${\tau }_{lum}$ denotes the lumped disturbances torque caused by uncertainties, unmodeled dynamics, or external disturbances. ${\Phi }_b={[\varphi ,\theta ,\psi ]}^T$ denote the attitude angle of roll, pitch, and yaw. ${ }^B{\omega }_b={[p,q,r]}^T$ denotes the angle velocity vector of the UAV. $T\left({\Phi }_b\right)$ signifies the rotation matrix that converts body angular velocity into attitude angular velocity. $I_b$ refers to the moment of inertia matrix of the UAV.

The motion of the manipulator, coupled with the changes in the load on the end-effector cause the center of mass (CoM)${ }^B{r}_{oc}$ and moment of inertia (MoI)${ }^B{I}_{man}^o$ of the UAM to change, generating variable coupling disturbance. We propose a variable coupling disturbance model (VCD) to describe this, as follows:

$$\begin{array}{cc}\hfill { }^B{\widehat{\tau }}_{cd}=& m_s({ }^B{r}_{oc}\times \left({ }^B{R}_{I}g{e}_3\right)-{ }^B{r}_{oc}\times { }^B{\ddot{r}}_o-{ }^B{\dot{r}}_{oc}\times { }^B{\dot{r}}_o)-m_{man}({ }^B{\dot{r}}_o\times { }^B{\dot{r}}_{omc} +\hfill \\ \hfill & { }^B{\dot{r}}_o\times ({ }^B{\omega }_b\times { }^B{r}_{omc})+{ }^B{\omega }_b\times ({ }^B{r}_{omc}\times { }^B{\dot{r}}_{omc})+{ }^B{r}_{omc}\times { }^B{\ddot{r}}_{omc})\hfill \\ \hfill & -{ }^B{\dot{I}}_{man}^o{ }^B{\omega }_b-{ }^B{\omega }_b\times \left({ }^B{I}_{man}^o{ }^B{\omega }_b\right)-{ }^B{I}_{man}^o{ }^B{\dot{\omega }}_b\hfill \end{array}$$

(2)

where $m_l$ is the total mass exerted on the end-effector. ${ }^B{\widehat{\tau }}_{cd}$ is the coupling disturbance torque obtained by the VCD model. ${ }^B{I}_{man}^o$ represents the inertia tensor of the manipulator relative to point o with respect to ${\Sigma }_B$. ${ }^B{r}_{oc}$ and ${ }^B{r}_{omc}$ represent the CoM vector of the UAM and the manipulator in the body coordinate system ${\Sigma }_B$, respectively. The relationship between ${ }^B{r}_{omc}$ and ${ }^B{r}_{oc}$ is given by

$${ }^B{r}_{omc}=\frac{m_s+m_l}{m_{man}+m_l}{ }^B{r}_{oc}$$

(3)

The variable inertia parameters utilized to delineate the variations in ${ }^B{r}_{oc}$ and ${ }^B{I}_{man}^o$ of the UAM can be formulated as follows:

$${ }^B{r}_{oc}=\frac{1}{m_s+m_l}\left(\sum _{i=1}^n{m_i}^B{p}_{ci}+{m_l}^B{p}_{cn}\right)$$

(4)

$${ }^B{I}_{man}^o=\sum _{i=1}^n{(}^B{R}_{i}I{{ }_i^{ci}}^B{R}_i^{-1}+m_i({\Vert }^B{p}_{ci}{\Vert }^2{I}_{3\times 3}{-}^B{p}_{ci}{(}^B{p}_{ci}{)}^T\left)\right)+m_l\left(\right({\Vert }^B{p}_{cn}{\Vert }^2{I}_{3\times 3}{-}^B{p}_{cn}{(}^B{p}_{cn}{)}^T)$$

(5)

where $m_{i(i=1,2,...,n)}$ are the masses of manipulator links. ${ }^B{p}_{ci}$ is the CoM position of the i-th link of the manipulator in ${\Sigma }_B$, respectively. ${ }^B{R}_i$ is the rotation matrix from ${\Sigma }_i$ to ${\Sigma }_B$. $I_i^{ci}$ is the moment of inertia matrix of the i-th link in ${\Sigma }_i$. $I_{3\times 3}$ is the identity matrix.

In (4) and (5), except for ${ }^B{p}_{ci}$, the remaining variables are inherent physical parameters of the manipulator or can be derived from the modified DH parameter model. And for ${ }^B{p}_{ci}$, we have

$${ }^B{p}_{ci}={ }^B{T}_i{\left(q\right)}^i{r}_{ci}$$

(6)

where $q={[q_1,q_2,\cdots ,q_n]}^T$ represents the angle of each joint of the manipulator. ${ }^i{r}_{ci}$ is the CoM of link i in ${\Sigma }_i$. ${ }^B{T}_i$ is the homogeneous coordinate transformation matrix from ${\Sigma }_i$ to ${\Sigma }_B$. According to the modified DH parameter method [29], the homogeneous coordinate transformation matrix ${ }^{i-1}{T}_i$ from ${\Sigma }_i$ to ${\Sigma }_{i-1}$ is defined as follows:

$${ }^{i-1}{T}_i=\left[\begin{array}{cc}{ }^{i-1}{R}_i& { }^{i-1}{p}_i\\ O_{1\times 3}& 1\end{array}\right]$$

(7)

where ${ }^{i-1}{R}_i$ represents the rotation matrix from ${\Sigma }_i$ to ${\Sigma }_{i-1}$. The position vector of the origin of ${\Sigma }_i$ expressed in ${\Sigma }_{i-1}$ is denoted as ${ }^{i-1}{p}_i$. $O_{1\times 3}=\left[000\right]$ is a zero vector.

$${ }^{i-1}{R}_i=\left[\begin{array}{ccc}c{\theta }_i& -s{\theta }_i& 0\\ s{\theta }_{i}c{\alpha }_{i-1}& c{\theta }_{i}c{\alpha }_{i-1}& -s{\alpha }_{i-1}\\ s{\theta }_{i}s{\alpha }_{i-1}& c{\theta }_{i}s{\alpha }_{i-1}& -c{\alpha }_{i-1}\end{array}\right]$$

(8)

$${ }^{i-1}{p}_i={\left[\begin{array}{ccc}{a}_{i-1}& -d_{i}s{\alpha }_{i-1}& d_{i}c{\alpha }_{i-1}\end{array}\right]}^T$$

(9)

where $a_{i-1}$, ${\alpha }_{i-1}$, $d_i$ and ${\theta }_i$ are the link length, the link rotation angle, the link offset, and the joint angle, respectively, and they are the MDH parameters of the i-th link. The joint angles ${\theta }_i$ are the design parameters that are focused on in this paper. s and c correspond to the trigonometric functions $sin\left(\right)$ and $cos\left(\right)$.

2.2. Problem Statement

In the existing research, the compensation and suppression of coupling disturbance and lumped disturbances were achieved through the force and torque produced by the propellers of the UAV, based on the instructions of the control algorithm. For this purpose, various controllers have proven to be useful, see for instance [6,7,24] and references therein. However, as proposed in this paper, the term ${ }^B{\tau }_{cd}$ can be utilized as a control input signal to offset and mitigate lumped disturbances, thereby reducing both the disturbance that necessitates compensation by the control algorithm and the energy consumption of the UAV. This method of “using the enemy’s strength against the enemy” provides a new active disturbance rejection implementation mechanism for UAMs.

Combined with the above Equations (2)–(9), we can see that the coupling disturbance is determined by the motion state of the manipulator and the change in the load on the end-effector. The Equation (2) reveals the mapping relationship from the manipulator state and load conditions to the generated coupling disturbance. However, when we need to use the coupling disturbance term ${ }^B{\tau }_{cd}$ as the control input signal, it is very difficult to solve in reverse the desired joint angle using the desired coupling disturbance torque.

$${ }^B{\widehat{\tau }}_{des}\in {\mathbb{R}}^3\to \widehat{q}={[{\widehat{q}}_1,{\widehat{q}}_2,\cdots ,{\widehat{q}}_n]}^T\in {\mathbb{R}}^n$$

(10)

From (10), one can obtain that the reverse solution process is not only a complex nonlinear programming problem (NLP), but that the reverse solution result may also have no solution or multiple solutions. Meanwhile, it is also necessary to consider that the desired joint angle of the manipulator will be subject to physical constraints. These physical constraints include joint angle limits and the self-collision avoidance of the manipulator with the UAV.

As a result, to achieve the goal of using the coupling disturbance generated by the active swing of the manipulator as a control input signal for active disturbance rejection, two questions need to be addressed:

• How to formulate the above problem into a nonlinear programming problem under specific physical constraints and solve it, which is the focus of this paper.
• How to obtain the desired coupling disturbance torque (i.e., the estimation of the lumped disturbances torque).

3. Method

3.1. NLP Formulation

Directly reverse solving the desired joint angles $\widehat{q}$ by the proposed VCD model (2) is complex and computationally expensive, which is not practical for the limited onboard computing resources of UAMs. To simplify the problem, this paper considers a scenario where the UAM remains hovering without aerial operation tasks. In this scenario, the manipulator is free to execute active anti-disturbance swings. When the UAM keeps hovering, ${ }^B{\omega }_b\approx 0,{ }^B{\dot{\omega }}_b\approx 0,{ }^B{\dot{r}}_o\approx 0,{ }^B{\ddot{r}}_o\approx 0,{ }^B{\ddot{r}}_{omc}\approx 0$, the Equation (2) can be reasonably simplified as follows:

$${ }^B{\widehat{\tau }}_{cd}=(m_s+m_l)({ }^B{r}_{oc}\times \left({ }^B{R}_{I}g{e}_3\right))$$

(11)

Using the coupling disturbance torque generated by the active swing of the manipulator as a control input for active anti-disturbance is a nonlinear programming optimization problem with constraints. The objective function and design parameters can be formulated in the following equations:

$$\begin{array}{c}\underset{\widehat{q}}{min}F\left(\widehat{q}\right)=\frac{1}{2}{\tilde{\tau }}_d^T{W}_1{\tilde{\tau }}_d+\frac{1}{2}{\tilde{q}}^T{W}_2\tilde{q}\hfill \\ \mathrm{with}\mathrm{respect}\mathrm{to}\widehat{q}={[{\widehat{q}}_1,{\widehat{q}}_2,\cdots ,{\widehat{q}}_n]}^T\hfill \\ \mathrm{subject}\mathrm{to}constrain 1, 2\hfill \end{array}$$

(12)

where $\tilde{q}=\widehat{q}-q$, $\widehat{q}={[{\widehat{q}}_1,{\widehat{q}}_2,\cdots ,{\widehat{q}}_n]}^T$ denote the desired joint angles of the manipulator and $q={[q_1,q_2,\cdots ,q_n]}^T$ represent the current actual joint angles of the manipulator. $W_1\in {\mathbb{R}}^3$ and $W_2\in {\mathbb{R}}^n$ are positive diagonal weight matrices. ${\tilde{\tau }}_d={ }^B{\widehat{\tau }}_{des}-{\widehat{\tau }}_{lum}$. ${\widehat{\tau }}_{lum}$ denotes the estimation of lumped disturbances, i.e., the desired generated coupling disturbance torque through the active swing of the manipulator. ${ }^B{\widehat{\tau }}_{des}$ represents the coupling disturbance torque generated by executing the optimization solution result. ${\widehat{\tau }}_{lum}$ can be obtained by various disturbance observer methods, as described in Section 3.2. Combining the Equations (4), (6) and (11), ${ }^B{\widehat{\tau }}_{des}$ can be obtained by the following equation:

$$\left\{\begin{array}{c}{ }^B{\widehat{\tau }}_{des}=(m_s+m_l)({ }^B{\widehat{r}}_{oc}\times \left({ }^B{R}_{I}g{e}_3\right))\\ { }^B{\widehat{r}}_{oc}=\frac{1}{m_s+m_l}\left({\displaystyle \sum _{i=1}^n}{m_i}^B{\widehat{p}}_{ci}+{m_l}^B{\widehat{p}}_{cn}\right)\\ { }^B{\widehat{p}}_{ci}={ }^B{T}_i{\left(\widehat{q}\right)}^i{r}_{ci}\end{array}\right.$$

(13)

In the proposed nonlinear programming optimization problem formulation, as shown in (12), there are two objectives of the objective function. The first objective is to make the coupling disturbance torque generated by executing the optimization solution result as close as possible to the desired torque value, which is also the primary requirement to achieve active anti-disturbance. The second objective is to minimize the swing magnitude of the manipulator, which means that when multiple solutions occur, a solution with smaller changes in joint angles is preferred. Weight matrices $W_1$ and $W_2$ are used to adjust the priorities of the two optimization objectives. $W_1$ with larger values means the first objective has a higher priority, $W_1$ with smaller values means the first objective has a lower priority.

Constraint 1: self-collision avoidance of the manipulator with the UAV. The physical motion space of the manipulator is limited, which means that the position of each joint link of the manipulator cannot exceed the propeller plane from the perspective of flight safety, otherwise self-collision may occur. This constraint can be formulated as follows:

$$\left\{\begin{array}{c}{ }^B{\widehat{p}}_{ci}\left(3\right)\ge d_s\\ { }^B{\widehat{p}}_{ci}={ }^B{T}_i{\left(\widehat{q}\right)}^i{r}_{ci}\end{array}\right.$$

(14)

where ${ }^B{\widehat{p}}_{ci}\left(3\right)$ denotes the third component of the CoM position of each link of the manipulator, $d_s$ represents the allowable vertical distance between the manipulator and propeller plane.

Constraint 2: joint angles limits. The physical limits of each joint angle of the manipulator are formulated as follows:

$$\underline{q}\le \widehat{q}\le \overline{q}$$

(15)

where $\overline{q}$ and $\underline{q}$ denote the upper and lower bound of joint angles, respectively.

3.2. Disturbance Observer Design

Before solving the above nonlinear programming optimization problem, it is necessary to obtain the desired generated coupling disturbance torque (i.e., the estimation of the lumped disturbance). In the existing disturbance rejection research for UAMs, a variety of disturbance observation methods have been proposed. These methods are primarily based on the online disturbance estimation strategy and feedback compensation, such as extended state observer (ESO) [14], adaptive sliding-mode disturbance observer [6], finite impulse response (FIR)-based disturbance observer [21], and so on. Among these disturbance estimators, ESO is widely used and requires the least amount of plant information [30]. As a result, ESO was adopted to obtain the desired estimation of the lumped disturbances.

According to reference [14], an ESO of attitude was designed to estimate the lumped disturbances ${\widehat{\tau }}_{lum}$. Let ${ }^B{\widehat{\omega }}_b$ and ${\widehat{\tau }}_{lum}$ represent the estimations of ${ }^B{\omega }_b$ and ${\tau }_{lum}$. To estimate ${\tau }_{lum}$, the ESO of attitude was designed as follows:

$$\begin{array}{c}{ }^B{\dot{\widehat{\omega }}}_{b,i}=u_{\omega ,i}+I_b^{-1}{\widehat{\tau }}_{lum,i}+2w_{\omega ,i}({ }^B{\omega }_{b,i}-{ }^B{\widehat{\omega }}_{b,i})\hfill \\ {\dot{\widehat{\tau }}}_{lum,i}=w_{\omega ,i}^2({ }^B{\omega }_{b,i}-{ }^B{\widehat{\omega }}_{b,i}),i=1,2,3\hfill \end{array}$$

(16)

where $u_{\omega }$ is the system input, which is defined in the controller design process. $u_{\omega ,i}$ refers to the i-th element of $u_{\omega }$. ${ }^B{\widehat{\omega }}_{b,i}$ and ${\widehat{\tau }}_{lum,i}$ refer to the i-th elements of ${ }^B{\widehat{\omega }}_b$ and ${\widehat{\tau }}_{lum}$, respectively. $w_{\omega }={[w_{\omega ,1},w_{\omega ,2},w_{\omega ,3}]}^T\in {\mathbb{R}}^3$ denotes the bandwidth vector of the ESO, which can be adjusted.

According to [30], the estimation error of the ESO is constrained within a certain range, which means

$$\underset{t\to \infty }{lim}\left|{\tau }_{lum,i}-{\widehat{\tau }}_{lum,i}\right|\le {\varsigma }_{\omega ,i},i=1,2,3$$

(17)

where ${\varsigma }_{\omega ,i}$ is inversely proportional to $w_{\omega ,i}$ to the k-th power. This means that the larger gain vector $w_{\omega }$ of the ESO results in a reduced estimation error bound.

3.3. Nominal Controller Design

In this paper, we place a greater emphasis on achieving active disturbance rejection by solving the nonlinear programming optimization problem regarding the desired joint angles. Hence, the robust tracking controller based on the feedback linearization method previously proposed in [14] is followed. Without doubt, other controllers are also acceptable, as long as their closed-loop systems are stable in the absence of disturbances.

From the dynamics model of UAM (1), the position dynamics of the UAM can be rewritten in a compact form as

$$\left\{\begin{array}{c}{\dot{p}}_b=v_b\hfill \\ {\dot{v}}_b=u_v\hfill \end{array}\right.$$

(18)

where $u_v=-\frac{f}{m_s}+g{e}_3$, $f=F_l{ }^I{R}_B{e}_3$ represents the combined lift vector.

The position control law can be designed as

$$u_v={\dot{v}}_{b\_d}-K_v{\tilde{v}}_b$$

(19)

where $K_v\in {\mathbb{R}}^{3\times 3}$ is the positive diagonal matrix. ${\tilde{v}}_b=v_b-v_{b\_d}$, where $v_{b\_d}$ is the desired velocity reference. $v_{b\_d}$ is designed as

$$v_{b\_d}={\dot{p}}_{b\_d}-2K_p{\tilde{p}}_b-K_p^2{\int }_0^t{\tilde{p}}_b\left(t\right)dt$$

(20)

where $K_p\in {\mathbb{R}}^{3\times 3}$ is the positive diagonal matrix. $p_{b\_d}$ and ${\tilde{p}}_b=p_b-p_{b\_d}$ represent the desired position and the position error, respectively. Substituting (20) into ${\tilde{v}}_b$ yields

$${\tilde{v}}_b={\dot{\tilde{p}}}_b+2K_p{\tilde{p}}_b+K_p^2{\int }_0^t{\tilde{p}}_b\left(t\right)dt$$

(21)

Combining the above (18)–(21), the control force vector can be deduced as

$$f=m_s(g{e}_3+K_v{\tilde{v}}_b-{\dot{v}}_{b\_d})$$

(22)

The attitude dynamics of the UAM can be written as

$$\left\{\begin{array}{c}{ }^I{\dot{R}}_B={ }^I{R}_B{\left[{ }^B{\omega }_b\right]}_{\times }\hfill \\ { }^B{\dot{\omega }}_b=u_{\omega }+I_b^{-1}({ }^B{\tau }_{cd}+{\tau }_{lum})\hfill \end{array}\right.$$

(23)

where $u_{\omega }=I_b^{-1}(\tau -{ }^B{\omega }_b\times \left(I_b{ }^B{\omega }_b\right))$. ${\left[A\right]}_{\times }$ denotes the skew-symmetric matrix of A.

Let ${ }^I{R}_{B\_d}$ denote the desired rotation matrix, which can be obtained through the control force vector (22) and the conversion relationship. ${ }^B{\omega }_{b\_d}={\left[{ }^I{R}_B^T{ }^I{\dot{R}}_{B\_d}\right]}^{\vee }$ refers to the desired angular velocity, where ${\left[A\right]}^{\vee }$ denotes the inverse operation of ${\left[A\right]}_{\times }$. The error rotation matrix can be defined as ${ }^I{\tilde{R}}_B={ }^I{R}_{B\_d}^T{ }^I{R}_B$. According to [31], the error function used in SO(3) is identical to the vector part of error quaternion $\tilde{\beta }={\left[{\tilde{\beta }}_0,{\tilde{\beta }}_v^T\right]}^T\in {\mathbb{R}}^4$ from ${ }^I{\tilde{R}}_B$ with ${\tilde{\beta }}_0\ge 0$ and

$${\tilde{\beta }}_0=\frac{1}{2}\sqrt{1+tr\left({ }^I{\tilde{R}}_B\right)}, {\tilde{\beta }}_v=\frac{1}{4{\tilde{\beta }}_0}{\left[{ }^I{\tilde{R}}_B-{ }^I{\tilde{R}}_B^T\right]}^{\vee }$$

(24)

According to [14], the desired angular velocity reference ${\omega }_r$ is designed as

$${\omega }_r={ }^I{\tilde{R}}_B^T{ }^B{\omega }_{b\_d}-2K_R{\tilde{\beta }}_v$$

(25)

where $K_R\in {\mathbb{R}}^{3\times 3}$ is the positive diagonal matrix. Then, we have $r_{\omega }={ }^B{\omega }_b-{\omega }_r={ }^B{\omega }_b-{ }^I{\tilde{R}}_B^T{ }^B{\omega }_{b\_d}+2K_R{\tilde{\beta }}_v$.

The attitude controller can be designed as follows:

$$u_{\omega }={\dot{\omega }}_r-I_b^{-1}{K}_{\omega }{r}_{\omega }-I_b^{-1}{k}_{\beta }{\tilde{\beta }}_v$$

(26)

where $K_{\omega }\in {\mathbb{R}}^{3\times 3}$ is the positive diagonal matrix and $k_{\beta }>0$ is a constant gain.

Subsequently, the control torque vector can be represented as

$$\tau =I_b{\dot{\omega }}_r+{ }^B{\omega }_b\times \left(I_b{ }^B{\omega }_b\right)-K_{\omega }{r}_{\omega }-k_{\beta }{\tilde{\beta }}_v$$

(27)

Assuming that the manipulator can actively swing to the desired joint angle $\widehat{q}$ obtained by solving the NLP in each control cycle, this means that the actual generated coupling disturbance torque can approach the desired generated coupling disturbance and that they are both bounded. Combining (17), for the $I_b^{-1}({ }^B{\tau }_{cd}+{\tau }_{lum})$ term in (23) under our proposed ${\mathrm{AADC}}_{\mathrm{VCD}}$, there exists a positive constant $\overline{\varsigma }$ such that

$$\varsigma =I_b^{-1}{(}^B{\tau }_{cd}-{ }^B{\widehat{\tau }}_{des}+{\tau }_{lum}-{\widehat{\tau }}_{lum}),\left|\varsigma \right|\le \overline{\varsigma }$$

(28)

where $\varsigma$ represents the disturbance residual after performing ${\mathrm{AADC}}_{\mathrm{VCD}}$ that is bounded. Combining the nominal controller closed-loop system stability analysis in [14], we can conclude that our proposed ${\mathrm{AADC}}_{\mathrm{VCD}}$ does not destroy the stability of the closed-loop system.

Remark 1. 

The operating mechanism of the proposed ${\mathrm{AADC}}_{\mathrm{VCD}}$ can be summarized as follows: ${\tau }_{lum}$ is estimated by the ESO presented in Section 3.2, and the ESO estimation result ${\widehat{\tau }}_{lum}$ is applied to construct the nonlinear programming optimization problem, as shown in Section 3.1. Then, the desired joint angles of the manipulator, denoted as $\widehat{q}$ can be ascertained by solving the nonlinear programming optimization problem. Finally, the coupling disturbance torque ${ }^B{\tau }_{cd}$ generated by the active swing of the manipulator to the desired joint angles is used to compensate for the lumped disturbances ${\tau }_{lum}$, thereby achieving the goal of active anti-disturbance using the active swing of the manipulator.

4. Simulation Results

This section demonstrates the effectiveness and superiority of the proposed active anti-disturbance control strategy ${\mathrm{AADC}}_{\mathrm{VCD}}$ through simulation experiments conducted on UAM within Simscape, which can provide a 3D mechanical system and high-fidelity multibody simulation environment [32]. In the simulations, an UAM with a 4-DOF manipulator was utilized to conduct two sets of experiments, namely Simulation I and Simulation II.

4.1. Simulation Setups

In the simulations, the UAM model created in Simscape primarily comprised a hex-rotor UAV base and a 4-DOF manipulator, as depicted in Figure 3. The physical parameters of the UAV base were specified as: $m_b$ = 2.65 kg, $J_{\varphi }$ = 0.067 kg·m², $J_{\theta }$ = 0.067 kg·m², $J_{\psi }$ = 0.127 kg·m², $l\left(wheelbase\right)$ = 0.55 m. The manipulator was an open-source 4-DOF manipulator, namely OpenMANIPULATOR-X, produced by ROBOTIS Co. Ltd (Seoul, South Korea) incorporating five DYNAMIXEL XM430-W350-T actuators. These actuators can deliver high-precision joint angles, velocities, and torque states information in real time. The physical parameters of the manipulator are detailed in

Table 1. Physical parameters of the OpenMANIPULATOR-X.

Parameter	Value
$m_{man}$ (kg)	0.702
$m_{1,2,3,4}$ (kg)	0.238, 0.123, 0.118, 0.224
${ }^1{r}_{c1}$ (m)	(−0.006794, 0.000253, −0.048813)
${ }^2{r}_{c2}$ (m)	(0.107084, −0.010616, 0.000467)
${ }^3{r}_{c3}$ (m)	(0.094329, 0.0000, 0.000489)
${ }^4{r}_{c4}$ (m)	(0.060527, −0.006058, −0.000021)
$I_1^{c1}$ (kg·m2)	${10}^{-4}*\left[\begin{array}{ccc}2.90202& 0.00335& 0.32543\\ 0.00335& 3.24158& 0.02059\\ 0.32543& 0.02059& 1.41275\end{array}\right]$
$I_2^{c2}$ (kg·m2)	${10}^{-4}*\left[\begin{array}{ccc}0.33028& -0.06189& 0.01212\\ -0.06189& 1.84812& -0.0002\\ 0.01212& -0.0002& 1.89169\end{array}\right]$
$I_3^{c3}$ (kg·m2)	${10}^{-4}*\left[\begin{array}{ccc}0.20796& 0.00002& 0.01064\\ 0.00002& 1.45545& 0.00\\ 0.01064& 0.00& 1.38574\end{array}\right]$
$I_4^{c4}$ (kg·m2)	${10}^{-4}*\left[\begin{array}{ccc}1.43765& 0.21123& 0.00001\\ 0.21123& 2.12697& 0.00485\\ 0.00001& 0.00485& 1.80588\end{array}\right]$

. The kinematic modeling based on the modified DH parameter method is illustrated in

Table 2. Modified DH parameters of the OpenMANIPULATOR-X.

i	${\mathit{\alpha }}_{\mathit{i}-1}\left(\mathbf{rad}\right)$	${\mathit{a}}_{\mathit{i}-1}\left(\mathbf{m}\right)$	${\mathit{d}}_{\mathit{i}}\left(\mathbf{m}\right)$	${\mathit{\theta }}_{\mathit{i}}\left(\mathbf{rad}\right)$
1	0	0.012	0.0935	$q_1$
2	$-\frac{\pi }{2}$	0	0	$q_2-1.3855$
3	0	0.13023	0	$q_3+1.3855$
4	0	0.124	0	$q_4$

. In addition, a red ball weighing 0.4 kg was used as the load on the end-effector of the manipulator; that is, $m_l$ = 0.4 kg.

To verify the effectiveness and advantages of the proposed active anti-disturbance control strategy, two simulations of keeping stable in a pick and place scenario (see Section 4.2) and suppressing lumped disturbance using ${\mathrm{AADC}}_{\mathrm{VCD}}$ (see Section 4.3) were designed.

In the simulations, the nominal controller parameters were set as follows: $K_p=diag\left(\left[1,1,2\right]\right)$, $K_v=diag\left(\left[3,3,5\right]\right),K_R=diag\left(\left[4,4,2\right]\right),K_{\omega }=diag\left(\left[3,3,0.5\right]\right),k_{\beta }=6$. The bandwidth vector of the ESO in Section 3.2 was set as follows: $w_{\omega }={[8,8,2]}^T$.

4.2. Simulation I: Keeping Stable in Pick and Place Scenario

In a pick and place scenario for UAM application, when a grabbed heavy object is suddenly dropped, the position and attitude stability of the UAM will be significantly affected. Under this working condition, this simulation needed to verify whether the UAM could be stably maintained at the moment the heavy object fell using the active swing of the manipulator. For this purpose, a set of comparative simulations were conducted. In the simulations, the hovering position of an UAM with a grabbed ball on the end-effector was set as $p_{b\_d}={[0,0,0]}^T$ and ${\psi }_d$ was set to 0. The gripper of the manipulator opened and the grabbed ball fell freely at 10 s. As a control group, the UAM’s manipulator executed no swing. As an experimental group, when the gripper of the manipulator opened, the manipulator executed an active swing to generate coupling disturbance torque to help the UAM maintain stability.

The results of Simulation I are presented in Figure 4. Figure 4a,b show the simulation snapshots of the control group and experimental group. Figure 4c and Figure 4d display the response of position X and attitude $\theta$ during the Simulation I, respectively.

From Figure 4, we can conclude that at the moment when the ball was dropped in the pick and place scenario, the active swing of the manipulator achieved better anti-disturbance and stability maintenance performance compared to no swing, and it significantly reduced the hovering position offset of UAM. When the gripper of the manipulator opened at 10 s and the ball began to fall freely, the additional control torque used to balance the ball caused the UAM’s attitude to tilt, thereby causing a position shift, as shown in Figure 4c,d. In this case, the coupling disturbance torque generated by the active swing of the manipulator could compensate and suppress the impact of this additional control torque, thereby significantly reducing the position shift and making the UAM more stable. Therefore, the simulation results verified the effectiveness of the active swing of the manipulator in maintaining the stability of the UAM in the pick and place scenario, and laid the foundation for the development of Simulation II.

On the other hand, the principle of using the active swing of the manipulator to generate coupling disturbance torque could also be used to provide the required tilting torque for UAMs and UAVs to perform quick attitude maneuvers, which has further research potential.

4.3. Simulation II: Suppressing Lumped Disturbances Using $AAD{C}_{VCD}$

The real-world aerial environment imposes lumped disturbances on UAMs, attributable to factors such as gusts, unmodeled dynamics, and other external influences. In order to verify the suppression effect of the proposed ${\mathrm{AADC}}_{\mathrm{VCD}}$ on lumped disturbances, a set of comparative simulations were conducted. In the simulations, an UAM with a grabbed ball was also set to keep hovering at $p_{b\_d}={[0,0,0]}^T$ and ${\psi }_d$ was also set to 0. In the simulation, lumped disturbances were treated as random sinusoidal signals and commenced their influence on the unmanned aerial vehicle (UAM) at 5 s. As a control group, the UAM’s manipulator did not execute any swing and only relied on the robustness of the nominal control algorithm to resist lumped disturbances. As an experimental group, our proposed ${\mathrm{AADC}}_{\mathrm{VCD}}$ was used to compensate and suppress lumped disturbances. $d_s$ in Constrain 1 was set as: $d_s=0$. In Constrain 2, $\underline{q}={[-\pi ,-2.05,-2.0,-1.8]}^T$ and $\overline{q}={[\pi ,2.05,1.53,2.0]}^T$. $W_1=diag\left(\left[3,3,3\right]\right)$ and $W_2=diag\left(\left[1,1,1,1\right]\right)$. Sequential quadratic programming (SQP) was utilized to solve the NLP formulated in Section 3.1. The expression of lumped disturbances is shown as ${\tau }_{lum}={\left[-0.6sin\left(\frac{\pi (t_1-10)}{5}\right),1.2sin\left(\frac{\pi (t_2-5)}{15}\right),0.0\right]}^T,10\le t_1\le 25,$$5\le t_2\le 20$, and its specific action form can be seen in Figure 5.

The results of simulation II are shown in Figure 6, Figure 7, Figure 8, Figure 9 and Figure 10. Figure 6 displays the simulation snapshots of the control group and experimental group. The response of position X,Y, and attitude $\varphi$, $\theta$ are illustrated in Figure 7. The total control torque and the motor speeds of the control group and experimental group are shown in Figure 8, respectively. Figure 9a shows the desired joint angles $\widehat{q}$ of the manipulator by solving NLP and the actual joint angles q of the manipulator, respectively. The desired generated coupling disturbance torque ${\widehat{\tau }}_{lum}$ and the coupled disturbance torque ${ }^B{\widehat{\tau }}_{des}$ generated by the active swing with ${\mathrm{AADC}}_{\mathrm{VCD}}$ are presented in Figure 9b. Figure 10 displays the changes in the objective function value during the ${\mathrm{AADC}}_{\mathrm{VCD}}$ simulation. Visible by the naked eye, our proposed ${\mathrm{AADC}}_{\mathrm{VCD}}$ performed much better than No Swing.

From Figure 6 and Figure 7, we can see that the ${\mathrm{AADC}}_{\mathrm{VCD}}$ we proposed showed a good performance in suppressing the lumped disturbances and significantly reduced the position offset caused by the lumped disturbances. This was because when the lumped disturbances started to act on the UAM, the ESO presented in Section 3.2 could accurately estimate the desired generated coupling disturbance torque and use it to construct the NLP. Then, by actively swinging to the desired joint angles $\widehat{q}$, which is obtained by solving NLP, the generated coupling disturbance torque ${ }^B{\widehat{\tau }}_{des}$ could directly compensate for the lumped disturbances ${\widehat{\tau }}_{lum}$, as illustrated in Figure 9.

On the other hand, from the smaller control torque and lower motor speed of ${\mathrm{AADC}}_{\mathrm{VCD}}$ in Figure 8, it can be seen that the proposed ${\mathrm{AADC}}_{\mathrm{VCD}}$ took full advantage of the UAM’s own manipulator and also effectively reduced the energy consumption during the suppression of lumped disturbances, which was conducive to improving the working endurance time of UAM. In addition, the mechanism of using the coupling disturbance generated by the active swing of the manipulator to compensate for the lumped disturbance is faster and more direct than the traditional compensation through the aerodynamic torque generated by the propeller, because the latter is subject to a slow aerodynamic response and other factors. Meanwhile, the variable load on the end-effector was also able to further improve the capability and limit of resisting the torque of the external lumped disturbance.

5. Conclusions

This paper proposed a novel active anti-disturbance control strategy for unmanned aerial manipulators, predicated on variable coupling disturbance compensation, which can realize the active disturbance rejection effect of “using the enemy’s strength against the enemy”, so that UAMs can maintain stability under multi-source disturbances. First, learning from the behavior of kangaroos that use the active wagging of their tail to generate torque to adjust their posture, we proposed using the coupling disturbance generated by the active swing of the UAM’s own manipulator as a control input signal for active disturbance rejection. Then, utilizing the proposed variable coupling disturbance model, we formulated the goal of active disturbance rejection as a nonlinear programming optimization problem under specific physical constraints. Solving this optimization problem could obtain the desired joint angles of the manipulator for active swinging. Finally, the coupling disturbance torque directly generated when the manipulator actively transitions to the predetermined desired joint angles could be used to compensate and suppress the other disturbances of the UAM, thereby achieving active and energy-saving anti-disturbance control effects. Combining the above components, we proposed the ${\mathrm{AADC}}_{\mathrm{VCD}}$ for UAMs to achieve active anti-disturbance. The effectiveness and superiority of the proposed ${\mathrm{AADC}}_{\mathrm{VCD}}$ were validated through two simulations, namely keeping stable in pick and place scenario and suppressing lumped disturbance using ${\mathrm{AADC}}_{\mathrm{VCD}}$. In the two simulations, the proposed ${\mathrm{AADC}}_{\mathrm{VCD}}$ both showed good anti-disturbance and energy-saving performance, significantly reducing the position offset of the UAM caused by disturbances.

The principle of using the active swing of the manipulator to generate coupling disturbance torque as proposed in this article could also be used to provide the required tilting torque for UAMs and UAVs to perform quick attitude maneuvers, which has further research potential and will be the subject of the authors’ upcoming research.