More Detailed Disturbance Measurement and Active Disturbance Rejection Altitude Control for a Flapping Wing Robot Under Internal and External Disturbances

Abstract

With the goal of designing a biologically inspired robot that can hold a stable hover under internal and external disturbances. We designed a tailless Flapping-wing Micro Aerial Vehicle (FMAV) with onboard 3D velocity perception. In this way, the wind disturbance caused by the relative motion of the FMAV can be quantified in real time based on the established altitudinal dynamics model. For the rest of the total disturbance, an active disturbance rejection controller is proposed to estimate and suppress those disturbances. In comparison with the traditional PID controller, this proposed approach has been validated. The results show that, in the hovering flight with the internal unmodeled dynamics, the root-mean-square of height controlled is only 2.53 cm. Even with the different weights of loads mounting on the FMAV, the ascending trajectory of flights remains impressively consistent. In the forward flight with the external disturbance, the root-mean-square error of height controlled is 2.78 cm. When the FMAV flies over a ladder introducing an abrupt external disturbance, the maximum overshoot is only half of that controlled by the PID controller. To our best knowledge, this is the first demonstration of FMAVs with the capability of sensing motion-generated wind disturbance onboard and handling the internal and external disturbances in hover flight.

1 Introduction

By virtue of rich sensory feedback in multiple dimensions, insects can obtain more disturbance information and rely upon active control to maintain stable flight in various disturbances. To explore this amazing environmental adaptability, a batch of hummingbird-like Flapping-wing Micro Aerial Vehicle (FMAV) prototypes weighing less than 30 g were designed [1,2,3,4,5,6]. It is already a remarkable achievement to realize a complete untethered hovering flight. However, how to comprehensively improve the anti-disturbance capability of the FMAV from the perspective of disturbance measurement and compensation is still a problem worthy of study.

To better sense the state and disturbance, insects are more “sensor-rich” in comparison with artificial aircraft [7]. On common aircraft such as quadrotors, there are several mature 3D localization solutions, such as Global Position System (GPS) [8], visual-inertial navigation [9, 10], etc. However, due to the constraints on physical weight and power consumption, these solutions are not feasible. The majority of autonomous free-flight FMAVs can only sense attitude using MEMS IMU onboard. In terms of 3D position and velocity perception, the tailed FMAV—DelFly II can only sense altitude with an onboard barometer [11]. The RoboBee [12, 13], which weighs only 101mg, estimates altitude using the onboard optical flow sensor, although the energy supply, control algorithm, and sensor data process are all implemented on the ground station. A 102 g tailless FMAV developed by Ref. [14] can realize the perception of 3D position with Ultra Wide Band (UWB), which needs to arrange UWB anchor in the surrounding environment in advance. For completely unfamiliar environments, this solution may not work. Thus a lightweight, low-power, low computational power required 3D velocity and position sensing solution suitable for an unfamiliar indoor environment is needed.

For the disturbances that can not be measured, how to estimate and suppress these is of greater research value. Disturbances can be divided into internal disturbances and external disturbances according to the sources. Internal disturbances include unmodeled dynamics and model parameter perturbations, such as wing damage and the change in vehicle weight. The external disturbances involve unpredictable incoming wind and sudden changes in the surrounding environment. Much study has been conducted on the estimation of disturbances, one of the popular methods is to simulate wind disturbances with detailed Computational Fluid Dynamics (CFD) analysis [15, 16]. This offline simulation can be used to analyze the influence of wind disturbance on FMAV. However, due to the diversity of the winds, it is very difficult to establish a simplified and universal model, which is very essential for height control under the disturbances. In addition, based on the constant or slowly time-vary wind assumption, many researchers have designed state observers, such as Disturbance Observer (DOB) [17], to estimate the wind disturbance. However, this approach requires a relatively accurate model which is difficult to establish sometimes. From the perspective of disturbance rejection controller design, compared with the quadrotor, the FMAV has a larger windward surface, so it is more susceptible to the wind. To improve the anti-disturbance ability of FMAV, some novel controllers are designed and verified in a simulation environment, such as iterative learning controller [18] and adaptive neural network controller [19]. In actual free flight, an adaptive controller is proposed to deal with internal disturbance introduced by the damaged wing [20]. In [21], an adaptive tracking flight controller was proposed to compensate for the slowly time-vary wind disturbance. These controllers have better control effects for a certain disturbance, which inspire us to explore whether it is possible to design a controller that can deal with both internal and external disturbances?

In this work, We embrace the idea that the more detailed perception, the better anti-disturbance. A tailless FMAV with onboard 3D velocity perception is proposed, which makes it possible to quantify the wind disturbance caused by relative motion velocity. For random disturbances and unmodeled dynamics that cannot be predicted, an Active Disturbance Rejection Controller (ADRC) is proposed to estimate and compensate for those disturbances. To validate the proposed scheme, both the internal and external disturbance experiments are set up. The result shows that the proposed scheme has an obvious advantage in reducing the overshoot and improving the accuracy of height control regardless of internal or external disturbances.

1.1 Outliner

The rest of the article is organized as follows. Section 2 introduces the design of the tailless FAMV as well as the 3D velocity measuring scheme. Section 3 introduces the altitudinal dynamics model of the FMAV, furthermore, the effect of the FMAV’s relative motion velocity on the altitude control is investigated. Section 4 presents a model-based ADRC that can estimate and alleviate disturbance in flight; In section 5, the proposed comprehensive scheme is verified by comparison with the traditional PID controller in a series of flight experiments. Section 6 summarizes this work.

2 The FMAV and the 3D Velocity Measuring Scheme

2.1 The Overall Design of FMAV

Fig. 1

a Illustration of the designed FMAV. b Schematic diagram of control torque generation. Translucent arrows represent the nominal wingbeat-average thrust vectors before torque generating. Solid arrows show wingbeat-average thrust and torque after torque generating

The FMAV in this article weighs 29.6 g and has a wingspan of 32.5 cm. On both sides of this FMAV, there are two separate flapping-wing mechanisms consisting of two pairs of wings, each flapping-wing mechanism is operated independently by a hollow cup motor through a gearbox. As shown in Fig. 1, the control moment of FMAV is generated by the inclination of the flapping plane driven by the servo. When the flapping plane is inclined in the same direction or the opposite direction, the pitching moment and the yawing moment are generated respectively. The roll moment is produced by the different flapping frequencies of the left and right wing (pair).

Fig. 2

Custom-built autopilot board (left) and sensor board (right)

The control system consists of a main autopilot board and a sensor board, as shown in Fig. 2, they are all custom-built. The main autopilot board is equipped with 180 MHz Cortex-M4 32-bit MCU(STM32F446ME), an MPU9250 inertial measurement unit (IMU), 2.4Ghz wireless communication circuit, power regulator circuits, two motor drivers, and SPL06 barometer. All of these are integrated into a 24 mm $\times$ 19 mm $\times$ 4 mm package with a weight of only 2.3 g. The sensor board involves a Vl53L1x laser sensor and a PMW3901MB-TXQT optical flow sensor.

2.2 3D Velocity Measuring Scheme

Drawing inspiration from rich sensory feedback in insects, multiply sensors, including optical flow sensor, laser sensor, and barometer, are used to measure the 3D position and velocity of the FMAV. These sensors all feature small packages, light weight, and low power consumption, allowing them to be carried onboard. The optical flow sensor is a vision-based sensor with a hard-coded digital signal processing system integrated inside the chip, it provides a 2D measurement of the displacement and has a resolution of 2 mm at a height of 1 m. The height of FMAV is measured by the laser sensor which has a repeat accuracy of 2.5 mm in the range of 0–4 m. When the height of FMAV exceeds 4 m, the barometer is switched to measure the height, resulting in a wider measurement range, but it is more susceptible to airflow.

Fig. 3

The relationship between the sensor measurement data and the body displacement data when the body rotates. The light-colored black line and solid-colored black line represent the body of the vehicle at times t1 and t2, respectively

As illustrated in Fig. 3, the laser range sensor and optical flow sensor are installed at the lowest point F of the vehicle, even if there is no translational velocity of the body, the change in the attitude will result in a displacement that can be detected by the sensor. To obtain the relationship between the 3D motion velocity of the body and sensor measurement data, the pinhole camera model [22] is adapted. As shown in Fig. 3, at time t1, the vehicle is in a vertical posture. The point $T_{1}$ below the FMAV on the ground is treated as the tracked point. The projection of point $T_{1}$ on the optical flow sensor’s image plane is the intersection of the extension line where the body is located and the image plane, i.e. point $M_{1}$. At time t2, the projection of point $T_{1}$ on the image plane is changed to point $M_{2}$. Therefore, the rotation-caused displacement perceived by the optical flow sensor on the image plane is $\overrightarrow{M_{1} M_{2}}$, corresponding to the displacement $\overrightarrow{T_{1} T_{2}}$ on the ground. So the rotation-caused velocity $\varvec{V}_{\omega }$ satisfies $\varvec{V}_{\omega }=\overrightarrow{T_{1} T_{2}} / d t=\varvec{\omega } \times \overrightarrow{O_{b} T_{2}}$. $\varvec{\omega }$ stands for the angular velocity of the body. Considering any position of the body, the body velocity $\varvec{V}_{b}$ has the following relationship with the velocity $\varvec{V}_{s}$ output by the optical flow sensor under the body coordinate system:

$$\begin{aligned} \varvec{V}_{b}=-\varvec{R}_{o}^{b} \varvec{V}_{s}-\varvec{V}_{\omega }=-\varvec{R}_{o}^{b} \varvec{V}_{s}-\varvec{\omega } \times \left[ \begin{array}{c} 0 \\ 0 \\ L+h_{s} \end{array}\right] \end{aligned}$$

(1)

where $\varvec{R}_{o}^{b}$ is the Direction Cosine Matrix(DCM) mapping from the sensor coordinates to the body coordinates, these coordinates will be illustrated in Fig. 7. L is the distance between the sensor and the center of gravity. $h_{s}$ is the data output by the laser sensor. In terms of height measurement, the actual height of the sensor h satisfies $h=r_{33}h_{s}$, $r_{33}$ is the element of the 3rd row and 3rd column of the DCM mapping from body coordinate frame to inertial coordinate frame at this moment. Then, the associated vertical velocity can be obtained by designing a standard Luenberger observer. The horizontal displacement can be obtained by integrating the horizontal motion velocity.

Fig. 4

Comparison of the data captured by VICON with the data obtained by the onboard sensors. a x-axis speed in body coordinates. b Position and velocity in the vertical direction

This compensation algorithm is verified by comparing the compensated data onboard with the data captured by the motion capture VICON cameras during the flight, and the results are displayed in Fig. 4. The Root Mean Square Error (RMSE) of altitude measurement, vertical velocity estimation and the x-axis velocity measurement are 2.305 cm, 5.033 cm/s and 5.236 cm/s, respectively. Considering the complexity of the actual airborne situation, this scheme is a trade-off between simplification and measurement accuracy.

3 Altitudinal Dynamics Model and the Effects of Wind Disturbance

To facilitate the design of the controller and study the effect of wind disturbance on the height control, an altitude dynamics model of FMAV is given. The model primarily consists of the following parts: 1. Actuator model, 2. Linear drag model.

3.1 Actuator Model

The actuator model can be separated into two parts: the thrust model and the servo model. The inputs of these models are square waves with different duty cycles. The output of the thrust model is the thrust. Due to the complexity of the aerodynamics, only the cycle-averaged thrust is considered. The output of the servo model is the rotation angle of the flapping plane. These models can be established experimentally in still air using the ATI Nano17-Ti force/torque transducer.

Fig. 5

The actuator model. a The thrust model. b The servo model

Firstly, with the servo in the neutral position, the thrusts are measured under various duty ratio inputs. With the FFT analysis and mean value calculation, the flapping frequency and the average thrust under different inputs can be obtained. Secondly, with the various control input to the servo, the rotation angle of the thrust is measured, so the relationship between the control input of the servo and the rotation angle of the flapping-wing plane can be obtained. The test and the fitting results are shown in Fig. 5. All of the goodness of fits are more than 0.99. So the actuator model can be expressed as:

$$\begin{aligned} \left\{ \begin{array}{l} \begin{aligned} f &{} =25.985 u_{m}-0.0214 \\ T &{} =3.543 \times 10^{-4} f^{2} \\ \beta &{} =-166.08 u_{s}+89.5 \end{aligned} \end{array}\right. \end{aligned}$$

(2)

here f is the flapping frequency, the unit is Hz, $u_{s}$,$u_{m}$ are the duty cycle input to the motor and servo respectively, the range is 0–1. T represents the thrust, the unit is N. $\beta$ is the rotation angle of the flapping-wing plane.

3.2 Linear Drag Model

Fig. 6

The linear drag model

We adapted the linear damping model to study the effect of drag force on the vehicle. This simplified damping modeling method has been applied in many works [23,24,25] and proved to be effective. Employing the quasi-steady model assumption, the drag force acting on the center of pressure (CoP) of the wing is expressed as:

$$\begin{aligned} F_{d}=-\frac{1}{2} \rho S C_{D}(\alpha ) v^{2}=d v^{2} \end{aligned}$$

(3)

where $\rho$ is the density of the air, S is the effective area of the wing perpendicular to the wind direction, $C_{D}(\alpha )$ is the damping coefficient, Assuming that the air density and the angle of attack $\alpha$ remain constant in both upstroke and downstroke, $-\frac{1}{2} \rho S C_{D}(\alpha )$ simplified to a constant d. v is the airspeed at the CoP of the wing, which consists of the body velocity V, and the CoP velocity $V_{f}$ introduced by the flapping of the wing. Assuming the CoP is fixed at half of the wing length R, the CoP velocity can be written as $V_{f}=2 \phi f R / 2=\phi f R$, where f is the flapping frequency, $\phi$ is the flapping amplitude. As shown in Fig. 6, the wing airspeed is $v=V_{f}-V$ during the upstroke, and $v=V_{f}+V$ during the downstroke. Taking the direction of the body velocity as the positive direction, the drag force on the CoP can be rewritten as $F_{d_{-} U}=d(V_{f}-V)^{2}$ and $F_{d_{-} D}=d(V_{f}+V)^{2}$ during the upstroke and the downstroke, respectively. When a pair of wings on one side flaps, the upstroke and downstroke occur synchronously, the linear drag force $F_{d}$ is:

$$\begin{aligned} F_{d}=\frac{1}{2}\left( F_{d_{-} D}+F_{d_{-} U}\right) =-2 d V_{f} V=B f V \end{aligned}$$

(4)

where B is the damping coefficient that can be measured in wind tunnel experiment.

3.3 Altitudinal Dynamics Model

Fig. 7

Illustration of coordinates

As shown in Fig. 7. The body coordinates $\left( x^{b}, y^{b}, z^{b}\right)$ is fixed at the Center of Mass (CoM). The flapping-plane coordinates $\left( x^{s}, y^{s}, z^{s}\right)$ is fixed at the flapping plane with the ys-axis coinciding with the flapping-plane rotation axis. $\left( x^{e}, y^{e}, z^{e}\right)$ is the inertial coordinates. Assuming air drag force acting at the Cop is only introduced by the aerodynamic effects originating from the flapping wings. The altitudinal dynamic model of the FMAV can be written as:

$$\begin{aligned} m \ddot{z}=F_{f z}^{e}+F_{d z}^{e}+ F_{c z}^{e}-m g+d_{z} \end{aligned}$$

(5)

where $F_{d z}^{e}$ and $F_{f z}^{e}$ are the vertical component of the thrust vector $\varvec{F}_{d}^{e}$ and drag force vector $\varvec{F}_{f}^{e}$ in the inertial coordinates, respectively. $F_{c z}^{e}$ is the vertical component of Coriolis force. m is the mass of the FMAV, and the acceleration of gravity g is taken as 9.8 m/s$^2$; $d_{z}$ indicates the vertical component of the total disturbance, which will be estimated by observer described later. According to the linear drag model in Eq. 4, $\varvec{F}_{f}^{e}$ and $\varvec{F}_{d}^{e}$ can be expressed as:

$$\begin{aligned} \begin{array}{l} \varvec{F}_{f}^{e}=\varvec{R}_{b}^{e} \varvec{R}_{s}^{b} \varvec{T}_{s}=\varvec{R}_{b}^{e}\left[ \begin{array}{c} T \sin \beta \\ 0 \\ T \cos \beta \end{array}\right] \\ \varvec{F}_{d}^{e}=\varvec{R}_{b}^{e} \varvec{R}_{s}^{b} \varvec{R}_{d}^{s}=\varvec{R}_{b}^{e} \varvec{R}_{s}^{b} \varvec{B} f \varvec{V}^{s}, \varvec{B}=\left[ \begin{array}{ccc} B_{x} &{} 0 &{} 0 \\ 0 &{} B_{y} &{} 0 \\ 0 &{} 0 &{} B_{z} \end{array}\right] \\ F_{c z}^{e} = m(V_{x}^{e}\times \omega _{y}^{e} - V_{y}^{e}\times \omega _{x}^{e}) \end{array} \end{aligned}$$

(6)

where $\beta$ is the rotation angle of the flapping plane. T is the thrust. $\varvec{R}_{s}^{b}$ is the DCM mapping from the body coordinates to the inertial coordinates. $\varvec{V}^{s}$ is the body velocity in the flapping-plane coordinates. $V_{x}^{e}$, $V_{y}^{e}$ are the body velocity along the respective axes of the ineitial coordinates. $\omega _{x}^{e}$, $\omega _{y}^{e}$ are the body angular velocity in the respective axes of the ineitial coordinates. $B_{x}, B_{y}, B_{z}$ are the damping coefficients in the respective axes of the flapping-plane coordinates.

3.4 The Effect of Vehicle’s Velocity on the Altitude Control of FMAV

It can be seen from the above model that the wind speed determines the size and direction of the air drag force. The airspeed suffered by the vehicle during flight is the sum of the ambient wind speed and the relative wind speed introduced by the motion of the FMAV. Due to the limitations of the sensors, it is difficult for us to perceive wind disturbances directly in the same way birds do. In order to have more information on the air drag force and compensate it in advance in the controller, we can quantify the air drag force caused by the motion of the FMAV. As shown in Fig. 7, $\left( x^{o}, y^{o}, z^{o}\right)$ is the sensor coordinates, its $x^{o}$ and $y^{o}$-axis is the projection of $x^{b}$ and $y^{b}$-axis of the body coordinates on the ground, respectively. The vehicle’s velocity $\varvec{V}_{s}^{o}=\left[ V_{x}^{o}, V_{y}^{o}, V_{z}^{o}\right] ^{T}$ perceived by the onboard sensors is the speed along each axis of the sensor coordinates, satisfying $\varvec{V}^{s} = \varvec{R}_{b}^{s} \varvec{R}_{e}^{b} \varvec{R}_{o}^{e} \varvec{V}_{s}^{o}$. Substituting it into Eq. 6, the vertical component of air drag force can be rewritten as:

$$\begin{aligned} F_{d z}^{e}=\left( a V_{x}^{o}+b V_{y}^{o}+c V_{z}^{o}\right) f \end{aligned}$$

(7)

where

$$\begin{aligned} \begin{aligned} a&=B_{x}(\cos \beta \sin \theta +\cos \phi \sin \beta \cos \theta )(\cos \beta \cos \theta -\cos \phi \sin \beta \sin \theta )\\&\quad +B_{z}(\sin \beta \cos \theta +\cos \beta \cos \phi \sin \theta )(\sin \beta \sin \theta -\cos \beta \cos \phi \cos \theta )\\&\quad -B_{y}\cos \theta \sin \phi ^{2}\sin \theta \\ b&=\sin \phi \left( B_{x}\cos \beta \sin \beta \sin \theta -B_{y}\cos \phi \cos \theta -B_{z}\cos \beta \sin \beta \sin \theta \right. \\&\left. \quad +B_{z}\cos \beta ^{2}\cos \phi \cos \theta +B_{x}\cos \phi \sin \beta ^{2}\cos \theta )\right. \\ c&=B_{x}(\cos \beta \sin \theta +\cos \phi \sin \beta \cos \theta )^{2}\\&\quad +B_{z}(\sin \beta \sin \theta -\cos \beta \cos \phi \cos \theta )^{2}+B_{y}\cos \theta ^{2}\sin \phi ^{2} \end{aligned} \end{aligned}$$

(8)

where, $\beta$, $\phi$, $\theta$ represents the rotation angle of flapping plane, roll angle, and pitch angle of the vehicle respectively.

It can be seen from the above formula that not only does the vertical velocity affect the drag force, but also the horizontal velocity, attitude angle of the aircraft, and the state of the actuator have a significant impact on it. In addition, the Coriolis force $F_{c z}^{e}$ in Eq. 6 is also related to the body velocity. Therefore, to realize a stable height hold, it is necessary to perceive the three-dimensional velocity of the FMAV.

4 Flight Control

In addition to the above-established model, there are still some unknown dynamics and unpredictable disturbances affecting altitude control. Therefore, how to design a controller that can suppress disturbance to achieve stable height control becomes the main challenge in this section.

Active disturbance rejection control is a widely used control technology. The traditional ADRC is mainly composed of three parts, the Tracking Differentiator (TD) for generating the transition process, the Extended State Observer (ESO) for state and disturbance estimation, and the Nonlinear State Error Feedback (NLSEF). Its core idea is to estimate the total disturbance including external and internal disturbance by the ESO, at the same time, these disturbances are compensated in control law which turns the system into a pure integral system ensuring the stable convergence of the error. This strategy has been applied to various fields, and has proven to be effective in disturbance suppression [26].

Fig. 8

The Altitude control loop for the FMAV

In this paper, the altitude control is treated as a single-input and single-output (SISO) system with motor control as input and the altitude as output. To enhance the anti-disturbance ability of the FMAV, a cascade ADRC is proposed, the inner loop is the velocity loop, and the outer loop is the position loop, the structure is shown in Fig. 8:

4.1 Outer Loop

The outer loop is composed of TD and NLSEF. The target height and the instantaneous desired height of a reasonable transient profile are the input and output of TD, respectively. The error between the desired height and the height state, as well as its derivative, are the inputs to the NLSEF. To avoid high-frequency noise introduced, a low-pass filter is added after differentiation. The output of NLSEF is the setpoint of vertical velocity, which is also the input of the inner loop. The detailed introduction of each part is as follows:

4.1.1 Tracking Differentiator (TD)

At the beginning of a flight, the altitude command is often abrupt, resulting in a persistent error between the altitude command and the altitude state. If there is an integral term in the outer loop controller, the system is prone to have an overshoot. Furthermore, the sudden change of the altitude command will also cause the abrupt change in the derivative of error, which will reuslt in the sudden change of the actuator output, hurting the control effect. Therefore, a reasonable transient profile of the command that the plant can follow is needed. This paper adopts the TD proposed by Mr. Han [27], in the form of:

$$\begin{aligned} \left\{ \begin{array}{l} \dot{v}_{1}(t)=v_{2}(t) \\ \dot{v}_{2}(t)={\text {fhan}}\left( v_{1}-v, v_{2}, r_{0}, N_{0} h\right) \end{array}\right. \end{aligned}$$

(9)

where v is the altitude command as the input of TD, $v_{1}$ is the desired trajectory, $v_{2}$ is the derivative of $v_{1}$. $N_{0}$ is the filter factor of TD, which can be tuned according to the smoothness of convergence, h is the sampling period. ${\text {fhan}}\left( x_{1}, x_{2}, r, h\right)$ is a time-optimal solution in discrete form that guarantees the fastest convergence from the desired trajectory $v_{1}$ to the altitude command v. The expression of this function is as follows:

$$\begin{aligned} \left\{ \begin{array}{l} \begin{aligned} &{}d=r h \\ &{}d_{0}=h d \\ &{}y=x_{1}+h x_{2} \\ &{}a_{0}=\sqrt{d^{2}+8 ry}\\ &{}a=\left\{ \begin{array}{cl} x_{2}+\frac{\left( a_{0}-d\right) }{2} {\text {sign}}(y), &{} y>d_{0} \\ x_{2}+\frac{y}{h}, &{} y \leqslant d_{0} \end{array}\right. \\ &{}{\text {fhan}}=-\left\{ \begin{array}{cl} r {\text {sign}}(a), &{} a>d \\ r \frac{a}{d}, &{} a \leqslant d\\ \end{array}\right. \end{aligned} \end{array}\right. \end{aligned}$$

(10)

among them, d, $a_{0}$, $a_{1}$, y are intermediate variables, r is a key parameter, which directly determines the speed of convergence. When setting the value of r, it is necessary to fully consider the physical limitations of the system. For example, in height control, r needs to satisfy $r \leqslant |a_{zmax} |$.

4.1.2 Nonlinear State Error Feedback (NLSEF)

Different from the linear combination of proportional, derivative, and integral terms in the traditional PID controller, this paper adopts the NLSEF in ADRC, the expression is as follows:

$$\begin{aligned} u_{0}=k_{p} {\text {fal}}\left( e_{1}, \alpha _{1}, \delta \right) +k_{d} {\text {fal}}\left( e_{2}, \alpha _{2}, \delta \right) \end{aligned}$$

(11)

The function ${\text {fal}}\left( e, \alpha , \delta \right)$ is defined as follows:

$$\begin{aligned} {\text {fal}}(e, \alpha , \delta )=\left\{ \begin{array}{cc} \frac{e}{\delta ^{1-\alpha }}, &{} e \le \delta \\ |e |^{\alpha } {\text {sign}}(e), &{} e \ge \delta \end{array}\right. \end{aligned}$$

(12)

with $\alpha <1$, even if there is a small error, such as $|e |<\delta$, a large error gain can be obtained so that the magnitude of the steady-state error can be significantly reduced. Since there is no integral term, the overshoot or the oscillation around the setpoint caused by the integral term is also avoided.

4.2 Inner Loop

The inner loop is the velocity control loop. A second-order ESO is added to estimate the total disturbances. At the same time, a control law is designed to offset the estimated disturbance in real time. The inner loop controller consists of three parts: TD, NLSEF, and ESO.

4.2.1 Tracking Differentiator (TD)

When the transition process of the altitude command in the outer loop is completed, the desired altitude is equal to the altitude command as long as the altitude command does not change. If there is a sudden change in the height state due to the terrain changes, which will result in a rapid change in the target velocity as the input of the inner loop. Therefore, a TD is also needed in the inner loop controller. In practice, its key parameter r should be not more than the maximum acceleration jerk of the FMAV. An appropriate value of r should make the desired velocity follow the velocity command without delay when the rate of the change in velocity command does not exceed the maximum acceleration of the FMAV, in contrast, a transition process is needed.

4.2.2 Extended State Observer (ESO)

ESO, like the Luenberger observer, estimates the state by the input and output of the system. In addition, ESO adds an extended state to observe the total disturbance. In this paper, Linear Extended State Observer (LESO) proposed by Ref. [28] is adapted for state and disturbance estimation. To increase the rate of convergence of disturbance estimation, a model-assisted ESO is presented. To make the model easier to apply in ESO and controller design, the actuator model in Eq. 2 is simplified to:

$$\begin{aligned} \left\{ \begin{array}{l} \begin{aligned} f&{}=25.985 u \\ T&{}=0.3543 f^{2} \end{aligned} \end{array}\right. \end{aligned}$$

(13)

Substituting Eq. 7, Eq. 13 and Eq. 6 into Eq. 5, the model is rewritten as:

$$\begin{aligned} \left\{ \begin{array}{l} \begin{aligned} \dot{v_{1}}&{}=v_{2}+\left( A u^{2}+\left( B+Cv_{1}\right) u \right) / m-g + F_{c z}^{e} \\ \dot{v_{2}}&{}=h \end{aligned} \end{array}\right. \end{aligned}$$

(14)

where

$$\begin{aligned} \begin{aligned} A&=0.2393 (\cos \beta \cos \theta \cos \phi -\sin \theta \sin \beta ) \\ B&=25.985 \left( a V_{x}^{o}+b V_{y}^{o}\right) \\ C&=25.985 c \end{aligned} \end{aligned}$$

(15)

Among them, A, B, C and $F_{c z}^{e}$ are only related to the state of the FMAV, which can be obtained online. h is the derivative of total disturbances $d_{z} / m$ in Eq. 5. Based on this model, LESO is designed as follows:

$$\begin{aligned} \left\{ \begin{array}{l} \dot{z}_{1}=z_{2}+\left( A u^{2}+\left( B+Cz_{1}\right) u\right) / m-g + F_{c z}^{e}-\beta _{1} e \\ \dot{z}_{2}=-\beta _{2} e \end{array}\right. \end{aligned}$$

(16)

where $e=z_{1}-v_{1}$, $\beta _{1}=w_{n}^{2}$, $\beta _{2}=2 w_{n}$. As the input of ESO, $v_{1}$ and u are the vertical velocity of the FMAV and the output of the controller, respectively. $\beta _{1}$, $\beta _{2}$ are the observer gains determined by the observer bandwidth $w_{n}$ which is the only parameter that should be tuned in ESO. As the output of ESO, $z_{1}$ and $z_{2}$ are the estimator of the vertical velocity $v_{1}$ and the total disturbances $d_{z} / m$, respectively.

4.2.3 Nonlinear State Error Feedback (NLSEF)

The core of the control law’s design is to design a suitable u using the feedback linearization method, a popular approach for controlling nonlinear systems, which will convert the system described by Eq. 14 into a pure integrator system in the form of

$$\begin{aligned} \left\{ \begin{array}{l} \begin{aligned} \dot{v}_{1}&{}=u_{0} \\ y&{}=v_{1} \end{aligned} \end{array}\right. \end{aligned}$$

(17)

This system can be easily controlled by making u0 a function of the tracking error and its derivative, i.e., a PD controller. In this way, the control problem is transformed to tune the coefficients in u0 ensuring system stability. Based on the system model established in Eq. 14, the control law is designed as:

$$\begin{aligned} \left\{ \begin{array}{l} \begin{aligned} u_{0}&{}=kp\left( {v_{r}}-z_{1}\right) -z_{2}\\ u&{}=\sqrt{\frac{m}{A}\left( u_{0}+g-F_{c z}^{e}\right) +\left( B+Cv_{1}\right) ^{2} / 4 A ^{2}}-\left( B+Cv_{1}\right) / 2 A \end{aligned} \end{array}\right. \end{aligned}$$

(18)

where, $z_{1}$, $z_{2}$ is the estimator of states in Eq. 16. with a well-tuned ESO, it will track the state and the total disturbance closely. $v_{r}$ is the reference of velocity v. kp is the parameter should be tuned in NLSEF. Other parameters can be obtained according to the state of FMAV.

4.3 Stability

It can be seen from the above introduction that the outer loop controller is a PD-like controller, so we only analyze the stability of the inner loop controller. To achieve the stability of the inner loop control system, it is necessary to ensure the stability of ESO and control law. Subtract Eq. 16 from Eq. 14, the error equation can be obtained as

$$\begin{aligned} \dot{\varvec{e}} = {\varvec{A}_{e} } \varvec{e}+ \varvec{E} h \end{aligned}$$

(19)

where $e_{i} = v_{i}-z_{i}$, $i =$ 1, 2, and $\varvec{E}=\left[ \begin{array}{l} 0 \\ 1 \end{array}\right]$, $\varvec{A}_{e}=\left[ \begin{array}{cc} -\beta _{1} &{} 1 \\ -\beta _{2} &{} 0 \end{array}\right]$.

Obviously, the LESO is bounded-input bounded-output (BIBO) stable if h is bounded, and the roots of the characteristic polynomial of $\varvec{A}_{e}$ are all in the left half plane. Then, Substituting Eq. 18 into Eq. 14 and Eq. 16 , the velocity loop can convert to a linear system represented by the state-space equation of

$$\begin{aligned} \left\{ \begin{array} { l } { \dot{ v } _ { 1 } = v _ { 2 } + k _ { p } ( r - z _ { 1 } ) - z _ { 2 } } \\ { \dot{ v } _ { 2 } = h } \\ { \dot{ z } _ { 1 } = z _ { 2 } + k _ { p } ( r - z _ { 1 } ) - z _ { 2 } - \beta _ { 1 } e } \\ { \dot{ z } _ { 2 } = - \beta _ { 2 } e } \end{array} \Rightarrow \left\{ \begin{array}{l} \dot{v}=A v+B u_{0}+E h \\ y=C v \\ \dot{z}=A z+B u_{0}+L(y-\hat{y}) \\ \hat{y}=C z \end{array}\right. \right. \end{aligned}$$

(20)

where

$$\begin{aligned} A=\left[ \begin{array}{ll} 0 &{} 1 \\ 0 &{} 0 \end{array}\right] , B=\left[ \begin{array}{l} 1 \\ 0 \end{array}\right] , C=\left[ \begin{array}{ll} 1&0 \end{array}\right] , E=\left[ \begin{array}{l} 0 \\ 1 \end{array}\right] , L=\left[ \begin{array}{c} -\beta _{1} \\ -\beta _{2} \end{array}\right] \end{aligned}$$

(21)

At this time, the inner-loop system can be regarded as a closed-loop system with r and h as inputs and v and z as state quantities, which can be represented by:

$$\begin{aligned} \left[ \begin{array}{c} \dot{v} \\ \dot{z} \end{array}\right] =\left[ \begin{array}{cc} A &{} B K \\ L C &{} A-L C+B K \end{array}\right] \left[ \begin{array}{l} x \\ z \end{array}\right] +\left[ \begin{array}{l} {\left[ \begin{array}{cc} B &{} E \end{array}\right] } \\ {\left[ \begin{array}{ll} B &{} 0 \end{array}\right] } \end{array}\right] \left[ \begin{array}{l} r \\ h \end{array}\right] \end{aligned}$$

(22)

where $K=\left[ \begin{array}{l} -k_{p} \\ -1 \end{array}\right]$. Eq. 22 is bounded-input bounded-output (BIBO) stable if its eigenvalues are in the left half plane. It is obvious that the velocity-loop eigenvalues satisfy

$$\begin{aligned} \begin{aligned}&eig \left( \left[ \begin{array}{cc} A &{} \bar{B} K \\ L C &{} A-L C+\bar{B} K \end{array}\right] \right) =e i g\left( \left[ \begin{array}{cc} A+\bar{B} K &{} \bar{B} K \\ 0 &{} A-L C \end{array}\right] \right) \\&=e i g(A+\bar{B} K) \cup e i g(A-L C) \\&=\left\{ \text{ roots } \text{ of } s+k_{p}\right\} \cup \left\{ \text{ roots } \text{ of } s^{2}+\beta _{1} s+\beta _{2}\right\} \end{aligned} \end{aligned}$$

(23)

since r, as the velocity reference, is always bounded, the other input of the plant h is also bounded. In other words, the disturbance $d_{z}$ must be differentiable, which is a reasonable assumption.

5 Experimental Verification

To validate the proposed disturbance rejection scheme, internal disturbance and external disturbance experiments are conducted. Internal disturbance experiments are divided into two sets: 1. hover flight; 2. hover flight test with different weight loads. The external disturbance experiments consist of two sets of flights: 1. forward flight, 2, forward flight over a ladder. In all tests, the flight results of the proposed disturbance rejection scheme are compared with that controlled by the traditional cascade PID controller. To improve the control effect of the PID controller as much as possible, the basic throttle is added to the PID controller, and the corresponding lift force of it is approximately equal to the weight of the FMAV, the sum of the basic throttle and the PID controller’s output is used as the control input of the actuator, which makes the operating range of the PID controller as close to the equilibrium point as possible, this approach is similar to the feedforward control. The parameters of the PID controller are well-tuned to the best possible. In all experiments, the altitude reference is 80cm, and both controllers’ parameters remain unchanged. the specific parameters are shown in the Table 1:

Table 1 Parameters of the cascade PID controller and the ADRC

5.1 Experimental Setup

To ensure the accuracy of experimental data, a high-precision motion capture system-Vicon is built to capture the FMAV’s attitude and position in real time, as shown in Fig. 9. At the same time, the state measured onboard, and the control commands are transmitted back to the ground station via wireless communication. The wireless communication transmission frequency is 100 Hz, the sampling rate of the Vicon system is 300 Hz;

Fig. 9

Illustration of the flight experiment setup

5.2 Internal Disturbance Experiments

5.2.1 Hovering Flight

In the case of hover flight, the FMAV suffers from some unmodeled disturbances, which include battery voltage changes, unbalanced thrust on both sides, etc.

Fig. 10

Comparison of the PID controller and the Active Disturbance Rejection Controller (ADRC) in hovering flight

The flight results are shown in Fig. 10. The rise time of the flight controlled by the ADRC and the PID controller is 1.7 s and 2.1 s, respectively, and there is no significant difference between them. After stabilization, the RMS altitude error of the flight controlled by the PID controller and the ADRC is 1.88 cm and 2.53 cm, respectively. Because the PID controller has an integral term resulting in slightly better control accuracy than the ADRC, however, it comes at the expense of a 47.3% overshoot. In contrast, the flight controlled by the ADRC has almost no overshoot, it benefits from the transient profile generated by the TD. As illustrated in Fig. 10, there are significant differences between the desired and achieved velocity for both controllers, in our opinion, it may be caused by two reasons. To begin with, we did not account for the battery’s effect on thrust in the model. When the motor control input is increased, the motor load increases, causing the battery’s terminal voltage to drop. As a result, the vehicle’s actual flapping-wing frequency is less than the expected frequency. When it encounters disturbances, it appears to be underpowered to follow the desired velocity. On the other hand, the parameters of the inner and outer loop controller are not optimal, which will also lead to these velocity errors. How to get better controller parameters is also our future work.

5.2.2 Hovering Flights with Different Weight Loads

By loading different weights to the FMAV, the sensitivity of the controller to internal disturbances is validated. To extend the range of mass change as much as possible, the battery is removed, and the FMAV flies with an external power supply. The added weights are divided into four groups: 0 g, 2 g, 4 g, and 5 g. The number of repeated flights in each group is at least 2 times.

Fig. 11

a–e The results of different weight loads flight controlled by the PID controller. f–j The results of different weight loads flight controlled by the active disturbance rejection controller. The light-colored line represents the trajectory of each test, and the solid-colored lines represent the averaged trajectories from multiple experimental sets. k The total disturbance estimated by extended state observer in flights of different weight loads. The line of dashes represents the mean value of the total disturbance in each group

The flight results are shown in Fig. 11. When the load weighs 0 g, 2 g, 4 g, and 5 g, the rise times of flights controlled by PID are 1.24 s, 1.52 s, 2.24 s, and 2.77 s, respectively. Whereas, the flights of the proposed scheme maintain the almost same ascending trajectory regardless of the weight change, and have almost no overshoot. The rise times of these flights are 1.41 s, 1.57 s, 1.66 s, and 1.57 s respectively. It can be concluded that the proposed scheme has a significant effect on suppressing the disturbance of the internal model parameters, which benefits from the disturbance estimation in real-time. As shown in Fig. 11k, the differences between the mean values of the estimated disturbances in the adjacent two groups of experiments are 72.5 cm/s$^{2}$, 72.9 cm/s$^{2}$, and 35 cm/s$^{2}$, which correspond to the differences in load’s weight of 2 g, 2 g, and 1 g, respectively (35 cm/s$^{2}$ $\times$ 29.6 g $\approx$ 0.01 N, which equals to the gravity of 1 gram). Such results demonstrate that the estimation of disturbances by ESO and the compensation in the control law play a crucial role in suppressing unmodeled internal disturbances.

5.3 External Disturbance Experiments

5.3.1 Forward Flight

Fig. 12

The result of the forward flight. a The flight controlled by the PID controller. b The flight controlled by the ADRC. c Real-time thrust and drag calculated onboard during the forward flight. A and $B+Cv$ are the coefficients of the thrust and drag force in Eq. 14, respectively

As analyzed in Section 3, the relative motion velocity has an important impact on height control. To verify the suppression effect of the controller against external unmodeled disturbances, a forward flight test was carried out. In altitude hold mode, the FMAV is operated by the remote controller to fly back and forth along the x-axis of the body repeatedly. The flight results of the two controllers are depicted in Fig. 12. The flight controlled by the PID controller has obvious vibration around the target height during the forward flight. Vibration usually starts when there is a large forward speed. The RMS altitude error is 6.78 cm; In contrast, the flight controlled by the ADRC has no discernible vibration during flight, and the RMS altitude error is only 2.78 cm. This proves that in the case of disturbance, the control accuracy of the ADRC is better than that of the PID controller. In Fig. 12c, during the forward flight, the drag force and thrust in the vertical direction change greatly. Even in this case, the vertical velocity is still well controlled. It can be concluded that a more detailed 3D velocity measurement is crucial.

5.3.2 Forward Flight over a Ladder

To further validate the controller’s capacity to suppress disturbances, a mutational disturbance is introduced by allowing the vehicle to fly over a 20 cm ladder during the forward flight.

Fig. 13

The result of the external perturbation experiment. a The flight controlled by the PID controller. b The flight controlled by the ADRC

As shown in Fig. 13, when a sudden disturbance is presented, the maximum overshoot in the flight controlled by the ADRC is 25%. However, the maximum overshoot in the flight controlled by the PID controller is 50%. With the help of the disturbances estimation and compensation in ADRC, the maximum overshoot is reduced by 50%. It can be concluded that the proposed scheme has an excellent suppression effect on external disturbances.

6 Conclusion

This paper investigated the problem of height control in the presence of disturbances. We designed a tailless FMAV with 3D velocity perceived onboard. With prior knowledge of the partial wind speed caused by the relative motion of the FMAV, the disturbance introduced by the relative wind speed can be measured, which is more detailed compared with those with only height state measured. For the remaining disturbances that cannot be predicted, an ADRC was designed to estimate and suppress the disturbances. To validate this proposed scheme, four sets of flight experiments with internal and external disturbances introduced were set up. The experimental results show that, compared with the traditional cascade PID controller, the proposed scheme has obvious advantages in reducing overshoot and improving the accuracy of height control regardless of internal or external disturbances, or even external step disturbances. The comprehensive strategy for disturbance suppression proposed in this paper provides a new idea for the design of the FMAV with better adaptability to the natural environment in the future.