Flying State Sensing and Estimation Method of Large-Scale Bionic Flapping Wing Flying Robot

Abstract

A large bionic flapping wing robot has unique advantages in flight efficiency. However, the fluctuation of fuselage centroid during flight makes it difficult for traditional state sensing and estimation methods to provide stable and accurate data. In order to provide stable and accurate positioning and attitude information for a flapping wing robot, this paper proposes a flight state sensing and estimation method integrating multiple sensors. Combined with the motion characteristics of a large flapping wing robot, the autonomous flight, including the whole process of takeoff, cruise and landing, is realized. An explicit complementary filtering algorithm is designed to fuse the data of inertial sensor and magnetometer, which solves the problem of attitude divergence. The Kalman filter algorithm is designed to estimate the spatial position and speed of a flapping wing robot by integrating inertial navigation with GPS (global positioning system) and barometer measurement data. The state sensing and estimation accuracy of the flapping wing robot are improved. Finally, the flying state sensing and estimation method is integrated with the flapping wing robot, and the flight experiments are carried out. The results verify the effectiveness of the proposed method, which can provide a guarantee for the flapping wing robot to achieve autonomous flight beyond the visual range.

1. Introduction

Flying in the blue sky like a bird has always been a dream of people. Learning about and utilizing birds’ body structures and flight mechanisms so as to develop bionic flapping aircraft with high maneuverability and low energy consumption has broad application prospects [1,2]. Large-scale bionic flapping wing robots developed by imitating the flight patterns of birds in nature have unique advantages in flight efficiency, wind resistance, and bionic concealment [3,4]. In order to improve the survivability of the flapping wing robot in complex environments, it is extremely important for the position and attitude calculation and state prediction of the flapping wing robot to perform beyond visual range and long-distance flight missions. Since a single sensor is affected by environmental disturbances such as gusts and magnetic field pulses during the flight of the flapping wing robot, the position and attitude estimation information of the flapping wing robot is seriously inconsistent with the actual situation [5,6]. The use of the flying state sensing and estimation method can reasonably use the measurement information of each sensor to complement the advantages and disadvantages, thereby improving the reliability of the flapping wing robot in performing tasks.

At present, there are two main methods for solving the attitude of flapping wing robots. One is to use MEMS (Micro-Electro-Mechanical System, MEMS) (Norwood, MD, USA) sensors (including accelerometers, gyroscopes and magnetometers) to estimate the attitude. This method is suitable for large- and medium-sized flapping wing robots with strong load capacity, such as Bat Bot flapper from University of Illinois at Urbana-Champaign, and pigeon flapper from Northwestern Polytechnic University [7,8]. At the same time, Kamankesh et al. and Yu et al. [9,10] studied the stability characteristics of periodic flapping wing structures and attitude stabilization of the aircraft. Verboom et al. and Hao et al. proposed a method based on periodic filtering to estimate the attitude of the flapping wing robot [11,12]. Yang et al. and Tu et al. [13,14] used Kalman filtering and adaptive attitude update methods to study the attitude information of UAVs (unmanned aerial vehicles). He et al. [15,16] proposed several adaptive control schemes and position estimation methods. In addition, Taha, Bialy, Dou, Zhang and other researchers [17,18,19,20] have also conducted in-depth research on the attitude and control of flapping wing robots. The other is the visual image pose solution method using the ground station for image transmission. This method is relatively common in small flapping wing robots, mainly including RoboBee developed by Harvard University and Delfly micro flapping wing robot developed by Karasek [21,22].

Solving the position and attitude based on flying state sensing and estimation is an important prerequisite for the flapping wing robot to achieve attitude control. Only by accurately obtaining the state parameters of the prototype can the position and attitude be controlled, and then the prototype can be guided to fly according to the predetermined trajectory by adjusting the attitude [23,24]. In a word, solving the pose of the flapping wing robot based on the flying state sensing and estimation method can improve the adaptability of the flapping wing robot to complex environments and improve the reliability of task execution.

However, different from small- and micro-sized flapping wing flying robots, the two wings of the large flapping wing robots have a large proportion of mass and inertia, and the fluctuation of the fuselage center of mass during flight cannot be ignored. The asymmetric flapping or unbalanced force leads to serious attitude deflection, and the existence of delay aggravates the inaccuracy and time inconsistency of perceptual information. Therefore, the traditional flying state sensing and estimation methods for fixed wing and rotor are difficult to provide stable and accurate data, which seriously limits the autonomous flight ability of large bionic flapping wing flying robots. Combined with the long-term accumulation of research on large-scale bionic flapping wing flying robot in the laboratory, and based on its flight characteristics in different stages of takeoff, cruise and landing, this paper proposes a method of flying state sensing and estimation, which realizes its autonomous navigation in the whole process of flight. It was tested in the actual flight experiment to verify the effectiveness of the proposed method.

2. Flying State Sensing and Estimation Scheme Design and Sensor Calibration

2.1. Mechanical Structure Analysis and Hardware Connection Design

In this paper, the bionic flapping wing robot “Phoenix” independently developed by the laboratory is taken as the research object. The characteristic parameters and flight control hardware connection of the prototype are shown in Figure 1. The design of the transmission mechanism and steering mechanism of the flapping wing robot is shown in Figure 2. The wingspan of the prototype is 2.2 m, and the average chord length is 40 cm. The driving element is a brushless DC (direct current) motor (specification: T-motor AT2308, kv2600), which drives the crank to create circular motion after deceleration by a two-stage gear set (the two-stage gear set is composed of gear 1, gear 2 and gear 3, and gear 1 is fixed on the motor shaft). Then the linkage mechanism is used to control the flutter of the wings up and down. The transmission ratio of the first stage gear (composed of gear 1 and gear 2) is 7, the transmission ratio of the second stage gear (composed of gear 2 and gear 3) is 11. The addendum circle diameter of gear 1 is 7.5 mm, the addendum circle diameter of gear 2 is 48 mm, and the addendum circle diameter of gear 3 is 73 mm. The lift and thrust generated by the periodic flapping of wings are the main source of power for the “Phoenix” flapping robot.

The steering mechanism is composed of two parallelogram mechanisms, which are driven by two steering engines, respectively, and are responsible for steering adjustment of the flapping wing robot. The specification of the pitching steering engine is GDW DS298MG, the torque is 6.5 kg.cm, and the speed is 0.12 s/60°. The specification of the yaw steering engine is Emax ES08MD, the torque is 2.0 kg.cm, and the speed is 0.1 s/60°. The flight control module is powered by the signal line of the electronic speed controller. After completing the tasks of sensor data acquisition and processing, position and attitude solution, and position and attitude control, it outputs three control signals to control the adjustment of motor speed, pitch and roll attitude adjustment, respectively.

2.2. Design of Flying State Sensing and Estimation Scheme

The design of the flying state sensing and estimation function is shown in Figure 3, and the black solid arrows indicate the flow direction of data. First, the RC (remote control) sends control commands, including the selection of flight mode, sensor calibration, etc. After receiving the sensor calibration and filtering command, the flight control module will run the calibration task to calibrate the original data of IMU (inertial measurement unit), barometer and magnetometer. The calibrated data will enter the flying state sensing and estimation task module after filtering.

In the flying state sensing and estimation task module, the attitude of the “Phoenix” flapping wing robot is solved by fusing IMU and magnetometer data, and the position of the “Phoenix” flapping wing robot is solved by fusing GPS, a barometer and inertial navigation. When the position and attitude information of the “Phoenix” flapping wing robot is obtained, the flapping wing robot will run the corresponding guidance algorithm according to its own position information and enter the position and attitude control link. The position control outputs the desired attitude, and then combines the estimated attitude information of the flapping wing robot into the position and attitude control links of autonomous takeoff, cruise and landing flight to output three-way attitude control quantities. It can be seen from Figure 2 that the roll and yaw attitudes of the “Phoenix” flapping wing robot are coupled with each other. The yaw control quantities and the roll control quantities need to be converted into a control quantities output through the preset logic of the mixer, and then used for the control of the roll servo. The pitch control quantities is used to control the pitch servo.

2.3. Sensor Calibration and Filtering

Due to an assembly error and the influence of the surrounding environment, the original data output by the sensor is not accurate enough. Therefore, it is necessary to calibrate and filter the sensor data to obtain relatively accurate sensor data.

(1)

Calibration of the sensor. The calibration of the sensor mainly includes the calibration of the gyroscope, magnetometer and accelerometer. The gyroscope is a device that detects the angular motion of an object, and its error can be calibrated by calculating the average value and Kalman filter by collecting the angular velocity data of three-axis output of gyroscope in static state. The magnetometer is an electronic device that obtains the heading angle by measuring the strength of the geomagnetic field. Here, the ellipsoid fitting based on the least squares method is used to calibrate the magnetometer. The calibration principle of the magnetometer can be found in the literature [12,25]. The effect comparison after calibration is shown in Figure 4. It can be seen that the center of the sphere formed by the raw magnetometer data is not at the origin, which indicates that there is an obvious offset error in the magnetometer. The fitted data are more evenly distributed on the spherical surface and the center of the sphere is located at the origin, which indicates that the ellipsoid fitting method has achieved better results.

For the accelerometer, the ellipsoid fitting method based on nine parameters is used to calibrate it, and the comparison of calibration results is shown in Figure 5. The calibration principle of accelerometer can be found in the literature of Verboom and Hao [11,26]. It can be seen that after the accelerometer is calibrated, the data envelope forms a sphere, and the center of the sphere is closer to the origin, which shows that the ellipsoid fitting method is effective in correcting the systematic error of the accelerometer. Then, keeping the prototype in a horizontal state, the raw data is collected and the calibrated data output by the IMU and the magnetometer when stationary are shown in

Table 1. Data comparison table before and after sensor calibration.

Static Measurement		Raw Data	Calibrated Data
Accelerometer (m/s2)	X	0.42	0.15
	Y	0.04	−0.20
	Z	10.19	−9.69
	Module	10.21	9.70
Gyroscope (°/s)	X	2.38	−0.04
	Y	1.70	0.005
	Z	1.34	0.05
	Module	3.17	0.06
Magnetometer	X	0.15	0.16
	Y	0.49	0.47
	Z	0.14	0.14
	Module	0.53	0.51

. It can be seen that the influence of the sensor system error is basically eliminated by the calibration.

(2)

Sensor filtering. During the flapping process of the flapping wing robot, a lot of vibration noise will be mixed with the flapping of the wings, which will seriously affect the attitude and position estimation, so filtering processing is required.

The comparisons of the accelerometer and gyroscope signals before and after calibration filtering are shown in Figure 6 and Figure 7, respectively. It can be seen that the fluctuation of the horizontal acceleration and the three-axis angular velocity data curve after calibration filtering is significantly reduced, and the signal-to-noise ratio and anti-interference ability are enhanced. At the same time, because the magnetometer signal is not easily affected by mechanical vibration, the sliding mean filter is used to suppress the magnetic field pulse noise.

3. Solution Method of Position and Attitude for Flying State Sensing and Estimation

3.1. Coordinate System Definition and Attitude Kinematics

In order to describe the state of the flapping wing robot in three-dimensional space, including the position of its own center of mass and its attitude in space, the coordinate system fixed with flapping wing robot is defined as the body system {O_BX_BY_BZ_B}. The origin of the body system is selected at the position of the centroid of the flapping wing robot. The reference coordinate system that characterizes the attitude change of the flapping wing robot is called the inertial frame {O_IX_IY_IZ_I}, as shown in Figure 8.

The rotation matrix ${}_{\mathrm{B}}{}^{\mathrm{I}}R$ is used to describe the attitude change of the body system {B} relative to the inertial frame {I}, which is called the rotation transformation matrix of the body system {B} relative to the inertial frame {I}. Similarly, the rotation matrix ${}_{\mathrm{I}}{}^{\mathrm{B}}R$ is used to describe the attitude of the inertial frame relative to the body system. Due to the large range of attitude changes of the flapping wing robot, in order to avoid the appearance of attitude jumping, the unit quaternion q = [q₀ q₁ q₂ q₃] is used to represent the attitude of the flapping wing robot. The quaternion represents the rotation matrix ${}_{\mathrm{B}}{}^{\mathrm{I}}R$ of the body system relative to the inertial frame as shown in Equation (1)

$${}_{\mathrm{B}}{}^{\mathrm{I}}R=\left[\begin{array}{ccc}2(q_0^2+q_1^2)-1& 2(q_1{q}_2-q_0{q}_3)& 2(q_1{q}_3+q_0{q}_2)\\ 2(q_1{q}_2+q_0{q}_3)& 2(q_0^2+q_2^2)-1& 2(q_2{q}_3-q_0{q}_1)\\ 2(q_1{q}_3-q_0{q}_2)& 2(q_2{q}_3+q_0{q}_1)& 2(q_0^2+q_3^2)-1\end{array}\right]$$

(1)

Quaternion was used to solve the differential equation of the attitude of flapping wing robot, as shown in Equation (2), and the first-order Runge–Kutta method was also used to update the quaternion differential equation. Then the quaternion expression of the current moment was obtained as shown in Equation (3). Due to the instability of the gyroscope’s bias, the bias error changes gradually over time. Therefore, the key to attitude calculation is to compensate the change of gyroscope bias in real time. In this paper, an explicit complementary filtering algorithm is used to filter the gyroscope data to eliminate the interference of the deviation instability on the attitude solution.

$$\dot{\hat{q}}=\frac{1}{2}\hat{q}\otimes \left({\widehat{\omega }}^b+\delta \right)$$

(2)

where $\dot{\hat{q}}$ is the derivative of quaternion, $\widehat{q}$ is the estimated value of the quaternion at any time; ${\widehat{w}}^b$ is the angular velocity vector under the body system; $\delta$ is the error compensation term; $\otimes$ is the Quaternion multiplication.

$$\hat{q}={\hat{q}}_{\mathrm{init}}+\dot{\hat{q}}\Delta t$$

(3)

where ${\hat{q}}_{\mathrm{init}}$ is the quaternion at the initial moment; $\Delta t$ is the integration time.

3.2. Periodic Equivalent Strategy

On the basis of the coordinate system in Figure 8, the whole-body coordinate system {O_sysX_sysY_sysZ_sys} is added, as shown in Figure 9. r_sys is the position coordinate vector of the center of mass of the whole-body system relative to the inertial reference frame. r₀ is the position vector of the center of mass of the fuselage relative to the center of mass of the whole-body system. r₁ and r₂ are the position vectors of the left and right wings relative to the center of mass of the whole system, respectively. The mass of the fuselage is m₀, and the masses of the left and right wings are m₁ and m₂, respectively.

Because the center of mass of the fuselage fluctuates relative to the center of mass of the whole system during the flapping process of the aircraft, and the sensor is fixedly connected to the fuselage, the height measured by the sensor is not the actual flying height of the aircraft. The relationship between the center of mass of the fuselage and the center of mass of the whole system is:

$$m_0{r}_0+m_1{r}_1+m_2{r}_2=(m_0+m_1+m_2)r_{\mathrm{sys}}$$

(4)

The position vector of the body’s center of mass can be simplified as:

$$\begin{array}{cc}\hfill r_0& =\frac{(m_0+m_1+m_2)r_{\mathrm{sys}}-m_1{r}_1-m_2{r}_2}{m_0}\hfill \\ & =\frac{m_0+m_1+m_2}{m_0}{r}_{sys}-\frac{m_1}{m_0}{r}_1-\frac{m_2}{m_0}{r}_2\hfill \end{array}$$

(5)

Equation (5) can be rewritten as

$$r_0=\stackrel{\wedge }{r_{\mathrm{sys}}}-\stackrel{\wedge }{r_1}-\stackrel{\wedge }{r_2}$$

(6)

where

$$\{\begin{array}{l}\stackrel{\wedge }{r_{\mathrm{sys}}}=\frac{m_0+m_1+m_2}{m_0}{r}_{\mathrm{sys}}\hfill \\ \stackrel{\wedge }{r_1}=\frac{m_1}{m_0}{r}_1\hfill \\ \stackrel{\wedge }{r_2}=\frac{m_2}{m_0}{r}_2\hfill \end{array}$$

(7)

Then the fluctuation equation of the center of mass of the fuselage relative to the center of mass of the whole system is:

$$\begin{array}{cc}\hfill r_0-\stackrel{\wedge }{r_{\mathrm{sys}}}& =-\stackrel{\wedge }{r_1}-\stackrel{\wedge }{r_2}\hfill \\ & =\Delta \mathrm{hsin}(2\mathsf{\pi }ft)\hfill \end{array}$$

(8)

The fluctuation curve of the center of mass of the fuselage relative to the center of mass of the whole system is obtained, as is shown in Figure 10.

Then the change equation of the center of mass of the fuselage is:

$$h\left(t\right)=H\left(t\right)\pm \Delta \mathrm{hsin}\left(2\mathsf{\pi }ft\right)$$

(9)

where H(t) is the height change of the center of mass of the whole system. $\Delta \mathrm{h}$ is the fluctuation range of the center of mass of the fuselage relative to the center of mass of the whole system. f is the flapping frequency of the wings.

(1)

When the aircraft is in cruise flight

$\stackrel{\wedge }{\left|r_{\mathrm{sys}}\right|}$= H(t) is a constant value, then the variation curve of the center of mass of the fuselage relative to the center of mass of the whole system is shown in Figure 11.

(2)

When the aircraft is in maneuvering flight

$\stackrel{\wedge }{\left|r_{\mathrm{sys}}\right|}$ = H(t) a is a variable value, then the variation curve of the center of mass of the fuselage relative to the center of mass of the whole system is shown in Figure 12.

3.3. Attitude Solution Based on Explicit Complementary Filtering

Because the traditional complementary filtering algorithm is not suitable for the flapping wing system with severe vibration [9,27], a more widely practical display complementary filtering algorithm was used to solve the attitude of the “Phoenix” flapping wing robot. It combines the data of the accelerometer, magnetometer and gyroscope, and uses the estimation error of the accelerometer and the magnetometer to a certain constant vector to correct the zero-drift change of the three axes of the gyroscope so that the angular velocity data is closer to the real value, thereby suppressing the attitude divergent.

3.3.1. Gravitational Acceleration Compensation

The unit vector of the estimated gravitational acceleration in the inertial frame is shown in Equation (10).

$${}^{\mathrm{I}}\overline{g}=\frac{{}^{\mathrm{I}}g}{\Vert {}^{\mathrm{I}}g\Vert }={\left[\begin{array}{ccc}0& 0& 1\end{array}\right]}^{\mathrm{T}}$$

(10)

where ${}^{\mathrm{I}}g$ and $\Vert {}^{\mathrm{I}}g\Vert$ are the gravitational acceleration vector and modulo length (m/s²) in the inertial frame, respectively. With the help of the rotation matrix (1), the representation of the estimated unit vector of the gravitational acceleration in the body system can be obtained, which is given by the Equation (11).

$$\overline{v}={}_{\mathrm{I}}{}^{\mathrm{B}}R\ast \overline{g}={}_{\mathrm{B}}{}^{\mathrm{I}}R{}^{\mathrm{T}}\ast \overline{g}={[2(q_1{q}_3-q_0{q}_2)\hspace{0.17em}\hspace{0.17em}2(q_2{q}_3+q_0{q}_1)\hspace{0.17em}\hspace{0.17em}2(q_0^2+q_3^2)-1]}^{\mathrm{T}}$$

(11)

The vector of the specific acceleration measured by the accelerometer under the body system is shown in Equation (12),

$${}^{\mathrm{B}}f={}^{\mathrm{B}}a-{}^{\mathrm{B}}g$$

(12)

where ^Ba is the inertial acceleration vector under the body system; ^Bg is the gravitational acceleration vector under the body system.

When the flapping robot is in equilibrium,${}^{\mathrm{B}}a\approx 0$,then the output signal of the accelerometer is the estimation of the gravitational acceleration, that is,${}^{\mathrm{B}}\widehat{g}=-{}^{\mathrm{B}}f$. After normalization, the Equation (13) will be obtained,

$$\widehat{v}=\frac{{}^{\mathrm{B}}\widehat{g}}{\Vert {}^{\mathrm{B}}\widehat{g}\Vert }=\frac{-{}^{\mathrm{B}}f}{\Vert {}^{\mathrm{B}}f\Vert }$$

(13)

Then the unit vector $\overline{v}$ of the estimated gravitational acceleration under the body system is cross-multiplied with the unit vector $\widehat{v}$ of the measured gravitational acceleration to obtain the gravity error correction vector, as shown in Equation (14), which is used to compensate the zero-drift change of the gyroscope.

$$e_1=\widehat{v}\times \overline{v}$$

(14)

3.3.2. Magnetic Deflection Compensation

In this paper, the magnetometer is used to correct the yaw angle, and the divergence of the yaw angle is suppressed by the invariance of the magnetic field resultant vector in the horizontal plane. Firstly, the geomagnetic field signal measured under the body system is converted to the inertial frame through the rotation matrix, as shown in Equation (15).

$${}^{\mathrm{I}}m={}_{\mathrm{B}}{}^{\mathrm{I}}R\times {}^{\mathrm{B}}m$$

(15)

where ^Bm is the geomagnetic vector under the body system; ^Im is the geomagnetic vector in the inertial frame.

The equation for solving the geomagnetic declination in the inertial frame is shown in Equation (16).

$${\widehat{\psi }}_0{=\arctan (\mathrm{m}}_{\mathrm{y}}^{\mathrm{I}}/m_{\mathrm{x}}^{\mathrm{I}})$$

(16)

The geomagnetic declination of Shenzhen is ${\widehat{\psi }}_0=-0.052$. Thus, it can be obtained that the difference between the measured geomagnetic declination and the predicted geomagnetic declination is $\Delta {\psi }_0={\widehat{\psi }}_0-{\overline{\psi }}_0$, which is rewritten into a vector form, as shown in Equation (17).

$$e={\left[\begin{array}{ccc}0& 0& \Delta {\psi }_0\end{array}\right]}^{\mathrm{T}}$$

(17)

Convert the geomagnetic error vector into the body system, as shown in Equation (18).

$$e_2={}_{\mathrm{I}}{}^{\mathrm{B}}Re$$

(18)

Adding the gravitational acceleration error vector and the geomagnetic error vector, and then the error compensation vector was obtained through the proportional–integral link, as shown in Equation (19). The error compensation vector and the angular velocity vector output by the gyroscope are added to obtain the corrected angular velocity vector, which is then substituted into the quaternion differential equation for integration. The quaternion is then iteratively updated to finally obtain the precise attitude information of the “Phoenix” flapping wing robot.

$$\delta =K_{\mathrm{p}}(e_1+e_2)+K_{\mathrm{I}}{\displaystyle \int (e_1+e_2)}$$

(19)

where K_p and K_I are proportional coefficient and integral coefficient, respectively.

When the “Phoenix” flapping wing robot performs the attitude calculation task, it needs to obtain the accelerometer and magnetometer data to estimate the initial attitude of the “Phoenix” flapping wing robot. In the equilibrium state of the “Phoenix” flapping wing robot, the three-axis accelerometer data is collected to calculate the initial roll angle and pitch angle of the flapping wing robot, as shown in Equations (20) and (21).

$$\phi =\arctan (a_{\mathrm{y}}^{\mathrm{b}}/a_{\mathrm{x}}^{\mathrm{b}})$$

(20)

$$\theta =\arcsin (a_{\mathrm{x}}^{\mathrm{b}})$$

(21)

where $\phi$ is the roll angle of flapping wing robot, and $\theta$ is the pitch angle.

The geomagnetic vector obtained by the magnetometer under the body system is projected into the horizontal plane of the inertial frame, as shown in Equations (22) and (23).

$$m_{\mathrm{x}}=m_{\mathrm{x}}^{\mathrm{b}}\cos \theta +m_{\mathrm{y}}^{\mathrm{b}}\sin \theta \sin \phi -m_{\mathrm{z}}^{\mathrm{b}}\sin \theta \cos \phi$$

(22)

$$m_{\mathrm{y}}=m_{\mathrm{y}}^{\mathrm{b}}\cos \phi +m_{\mathrm{z}}^{\mathrm{b}}\sin \phi$$

(23)

Initialize the yaw angle (looking from the tail to the nose, the right is positive) using the magnetometer data in the north (X-axis) and east (Y-axis) directions, as shown in Equation (24).

$$\psi =\mathrm{atan}2(-m_{\mathrm{y}},m_{\mathrm{x}})-\mathrm{mag}\_\mathrm{decline}$$

(24)

where mag_decline is the magnetic declination angle, which is about −0.052 rad in Shenzhen.

Then, the initial Euler angle is converted into a quaternion by Equation (25), and then the quaternion vector at the initial moment can be obtained by normalizing.

$$\{\begin{array}{cc}\hfill q_0& =\cos (\psi /2)\cos (\theta /2)\cos (\phi /2)+\sin (\psi /2)\sin (\theta /2)\sin (\phi /2)\hfill \\ \hfill q_1& =\cos (\psi /2)\cos (\theta /2)\sin (\phi /2)-\sin (\psi /2)\sin (\theta /2)\cos (\phi /2)\hfill \\ \hfill q_2& =\cos (\psi /2)\sin (\theta /2)\cos (\phi /2)+\sin (\psi /2)\cos (\theta /2)\sin (\phi /2)\hfill \\ \hfill q_3& =-\cos (\psi /2)\sin (\theta /2)\sin (\phi /2)+\sin (\psi /2)\cos (\theta /2)\cos (\phi /2)\hfill \end{array}$$

(25)

After the quaternion initialization is completed, the offset of the three-axis gyroscope is compensated by displaying the complementary filtering algorithm to obtain the error compensation vector $\delta$, and then the quaternion is updated by using Equations (2) and (3). If the solved quaternion is a finite real number, the solution is considered successful, otherwise the solution result is discarded. After the quaternion solution is completed, the normalization processing is carried out, and the corresponding rotation matrix ${}_{\mathrm{B}}{}^{\mathrm{I}}R$ is updated. In this way, the attitude solution task of one cycle (0.01 s) is completed.

3.4. Position Solution Based on Kalman Filter

The position solution of the “Phoenix” flapping wing robot in the horizontal direction requires the use of sensor data. The GPS sensor output data has long-term stability and does not diverge over time, but the data refresh rate is low and is easily affected by surrounding obstructions. The INS (inertial navigation system) can be used in any complex environment, with high accuracy in a short time, but the position measured by INS will produce integral drift for a long time. A barometer and GPS can be used to estimate altitude information. In a windless and stable flight environment, the barometer is used to perceive the altitude information of the “Phoenix” flapping wing robot. When the flapping robot is disturbed by strong winds or its own flapping frequency is too fast, it will cause large fluctuations in the surrounding air pressure, resulting in inaccurate data output of the barometer [20,23]. When the output data measured by the barometer fluctuates unreasonably, the altitude data output by GPS is selected as the height measurement information of the flapping robot.

In summary, it is difficult for each navigation system to be used independently to meet the reliability requirements. A feasible way to improve the accuracy of the navigation system is the integrated navigation technology, that is, two or more navigation systems are used to measure and solve the same navigation information to form a more accurate measurement value.

3.4.1. Design of Kalman Filter

In this study, the Kalman filter is used to fuse multi-sensor data to solve the position and velocity information of the “Phoenix” flapping wing robot [13,28]. The current state can be calculated only by inputting the measured value of the state vector in the current solution cycle (which may contain multiple sensor data) and the estimated value of the previous solution cycle.

Setting x = [p v b]^T as the northward state vector of the flapping wing robot, where p represents the position; v represents the velocity; b represents the inertial acceleration bias. The solution using the Kalman filter includes prediction process and update process. The prediction process is shown in Equations (26) and (27).

$${\widehat{x}}_k^{\prime }=F{\widehat{x}}_{k-1}+B{a}_k+\Gamma {\zeta }_{k-1}$$

(26)

where $F$ is the state transition matrix; B is the control matrix; $\Gamma$ is the process noise driving matrix. ${\widehat{x}}_k^{\prime }$ and ${\widehat{x}}_{k-1}$ are the prior state vector at time k and the posterior state vector at time (k–1), respectively; a_k is the inertial acceleration in the inertial frame at time k; ${\zeta }_{k-1}$ is the zero mean white noise.

$${\widehat{P}}_k^{\prime }=F{\widehat{P}}_{k-1}{F}^{\mathrm{T}}+Q$$

(27)

where ${\widehat{P}}_k^{\prime }$ is the prior estimated state error covariance matrix at time k; ${\widehat{P}}_{k-1}$ is the posterior estimated state error covariance matrix at time k–1; Q is the process excitation noise covariance matrix.

The update process of the Kalman filter is shown in Equations (28)–(31). Equations (26) and (28) are collectively referred to as the Kalman filter system equation of the “Phoenix” flapping wing robot.

$$z_k=H{\widehat{x}}_k^{\prime }+v_k$$

(28)

$${\widehat{x}}_k={\widehat{x}}_k^{\prime }+K(z_k-H{\widehat{x}}_k^{\prime })$$

(29)

$$K=\frac{{\widehat{P}}_k^{\prime }{H}^T}{H{P}_k^{\prime }{H}^T+R}$$

(30)

$${\widehat{P}}_k=(I-KH){\widehat{P}}_k^{\prime }$$

(31)

where $z_k$ is measurement vector, $z_k={\left[p_k^m v_k^m\right]}^{\mathrm{T}}$; $p_k^m$ and $v_k^m$ are the position and velocity measured by the sensor, respectively; v_k is the measurement error vector, $v_k={\left[p_e^{\prime }{v}_e^{\prime }\right]}^{\mathrm{T}}$; $p_e^{\prime }$ and $v_e^{\prime }$ represent the measurement error of position and velocity, respectively. I is the three order unit matrix; K is the Kalman gain matrix; H is the measurement matrix; R is the measurement noise covariance matrix.

The design of the Kalman filter focuses on three covariance matrices, namely the initial state error covariance matrix ${\widehat{P}}_0$, the process excitation noise covariance matrix Q and the measurement noise covariance matrix R. Here, the initial state error covariance matrix ${\widehat{P}}_0$ is a third-order square matrix, the elements on the diagonal are the variance of the initial estimation error of the state vector, and the elements on the non-diagonal line are the covariance of the initial estimation error of the state vector.

When setting the process excitation noise covariance matrix, it can be seen from the system model that the errors of position and speed are related to the change of acceleration, and the relationship is shown in Equations (32) and (33). Assuming that the variance of the acceleration is D(a), the expressions of the position and velocity variances are shown in (34) and (35) according to the properties of the variance, thus determining the elements on the diagonal of the Q matrix. The calculation of covariance is given by Equation (36), and the correlation coefficient is taken as $\rho =\pm 1$. When the direction of position and velocity is the same as the direction of acceleration, the correlation coefficient $\rho =1$, otherwise $\rho =-1$. In summary, under the condition that the state quantity is positively correlated with the driving quantity, the final expression of the process excitation noise Q matrix is shown in Equation (37).

$$\Delta v=a\Delta t$$

(32)

$$\Delta p=v_0\Delta t+\frac{1}{2}a\Delta t^2$$

(33)

$$D(v)=D(at)=\Delta t^{2}D(a)$$

(34)

$$D(p)=D(\frac{1}{2}a\Delta t^2)=\frac{1}{4}\Delta t^{4}D(a)$$

(35)

$$\mathrm{cov}(X,Y)=\rho \sqrt{D(X)D(Y)}$$

(36)

where D(X) and D(Y) represent the variance of X variable and Y variable, respectively; $\rho$ represents the correlation coefficient, $-1\le \rho \le 1$;

$$Q=\left[\begin{array}{ccc}\frac{1}{4}\Delta t^4& \frac{1}{2}\Delta t^3& \frac{1}{2}\Delta t^2\\ \frac{1}{2}\Delta t^3& \Delta t^2& \Delta t\\ \frac{1}{2}\Delta t^2& \Delta t& 1\end{array}\right]D(a)$$

(37)

The measurement noise covariance is mainly related to the measurement accuracy of the sensor, and the measurement error can be determined according to the horizontal (vertical) positioning accuracy factor output by the GPS sensor. It is also assumed that the GPS measurements of position and velocity are independent of each other, then the non-diagonal elements of the measurement noise covariance matrix R are set to 0, and the diagonal elements are related to the positioning accuracy factor. The final expression of the measurement noise covariance moment R_H in the horizontal direction is shown in Equation (38).

$$R_{\mathrm{H}}=\left[\begin{array}{cc}D(hdop)& 0\\ 0& D(vdop)\end{array}\right]$$

(38)

where D(hdop) and D(vdop) represent the variance of the horizontal position and horizontal velocity positioning accuracy factor, respectively. The measurement noise covariance R_v in the vertical direction is related to the vertical positioning factor of the barometer or GPS. When the barometer is used to measure the height information of the “Phoenix” flapping wing robot, the expression of R_v is shown in Equation (39).

$$R_{\mathrm{V}}=D(baro)$$

(39)

where D(baro) is the variance of the height measured by the barometer.

When the error of the barometer output data is large, GPS is used to sense the height change of the flapping wing robot, and the expression of R_v is shown in Equation (40).

$$R_{\mathrm{V}}=D(vdop)$$

(40)

where D(vdop) is the variance of the vertical positioning precision factor of GPS.

3.4.2. Improvement of the Kalman Filter Algorithm

(1)

H∞ filter

If the prior information of the covariance matrices Q, R, and P₀ in the Kalman filter equation is uncertain, when the flapping robot is disturbed by gusts, it will easily lead to inaccuracy of the system model and unclear statistical characteristics of noise. At this time, it is necessary to appropriately increase the value on the diagonal of the process noise covariance matrix Q to increase the utilization weight of the real-time measured value and reduce the utilization weight of the predicted value. This process has great randomness, and it is impossible to determine the appropriate value of the process noise covariance matrix Q to optimize the performance of the filter. H∞ filtering is an effective method to solve the above problems [19,22], which is of great help to improve the robustness of the system. The discrete H∞ filter system equations and algorithms are shown in Equations (41) and (42), respectively.

$$\begin{array}{cc}\hfill {\widehat{x}}_{k+1}^{\prime }& =F{\widehat{x}}_k+B{a}_k+w_k\hfill \\ \hfill z_k& =H{\widehat{x}}_k^{\prime }+v_k\hfill \\ \hfill y_k& =L_k{\widehat{x}}_k^{\prime }\hfill \end{array}$$

(41)

where y_k is the estimated vector, which can be a linear combination of state variables, and it is necessary to ensure that L_k is full rank; w_k, v_k are system noise and measurement noise, respectively.

$$\begin{array}{cc}\hfill {\widehat{x}}_{k+1}^{\prime }& =F{\widehat{x}}_k^{\prime }+FK\left(z_k-H{\widehat{x}}_k^{\prime }\right)\hfill \\ \hfill K& =P_k^{\prime }{\left(I+H^{\mathrm{T}}{R}^{-1}H{P}_k^{\prime }\right)}^{-1}H{R}^{-1}\hfill \\ \hfill P_{k+1}^{\prime }& =F{P}_k^{\prime }{\left(I+H^T{R}^{-1}H{P}_k^{\prime }\right)}^{-1}{F}^{\mathrm{T}}+Q\hfill \end{array}$$

(42)

After many experiments, the values of each covariance matrix are as follows. Among them, initial state error covariance matrix and process noise matrix in the X direction:

$${\widehat{P}}_0^{\mathrm{x}}=\left[\begin{array}{ccc}8& 0& 0\\ 0& 0.18& 0\\ 0& 0& 6.4\times {10}^{-6}\end{array}\right]\hspace{0.17em}\hspace{0.17em}\hspace{0.17em},\hspace{0.17em}\hspace{0.17em}{Q}_{\mathrm{x}}=\left[\begin{array}{ccc}1.15& 0& 0\\ 0& 0.8& 0\\ 0& 0& 1\times {10}^{-6}\end{array}\right]\hspace{0.17em}$$

Initial state error covariance matrix and process noise matrix in Y direction:

$${\widehat{P}}_0^{\mathrm{y}}=\left[\begin{array}{ccc}32& 0& 0\\ 0& 0.5& 0\\ 0& 0& 6.4\times {10}^{-6}\end{array}\right]\hspace{0.17em}\hspace{0.17em},\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}{Q}_{\mathrm{y}}=\left[\begin{array}{ccc}5.8& 0& 0\\ 0& 5& 0\\ 0& 0& 1\times {10}^{-6}\end{array}\right]\hspace{0.17em}$$

Horizontal measurement noise matrix:

$$R_{\mathrm{H}}=\left[\begin{array}{cc}1& 0\\ 0& 0.04\end{array}\right]$$

Initial state error covariance matrix and process noise matrix in Z direction:

$${\widehat{P}}_0^{\mathrm{z}}=\left[\begin{array}{ccc}12.5& 0& 0\\ 0& 3.38& 0\\ 0& 0& 1\times {10}^{-4}\end{array}\right]\hspace{0.17em}\hspace{0.17em},\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}{Q}_{\mathrm{z}}=\left[\begin{array}{ccc}3.5& 0& 0\\ 0& 2.0& 0\\ 0& 0& 1\times {10}^{-6}\end{array}\right]$$

When using barometer data to represent height measurements, the measurement noise in the Z direction is:

$$R_{\mathrm{V}}=1.524$$

When using the GPS output data to represent the height measurement value, the measurement noise in the Z direction is:

$$R_{\mathrm{V}}=9$$

(2)

Suppression of filter divergence

In the actual flight experiment, it was found that with the prolongation of flight time, the navigation parameters output by the Kalman filter tended to be stable, but the deviation from the real value became larger and larger. This is because the torsion, bending and other deformations will occur during the flapping process of the wings, which makes it difficult to establish an accurate mechanism model to analyze the force of the flapping robot during flight. Therefore, the physical model error is the main factor leading to the divergence of the filter. The basic idea of model error correction is to change the value of the elements in the covariance matrix to affect the size of the elements of the Kalman gain matrix, and then reasonably assign the weights of the predicted value and measured value of the state quantity. Here, the attenuation memory method is used to suppress the filter divergence.

The attenuation memory method filtering equations are shown in Equations (43)–(45). Compared with Equation (27), the filtering equation only has one more scalar factor $\lambda$ in Equation (43). Since $\lambda >1$, the prior state error covariance matrix ${\tilde{P}}_k^{\prime }$ is always larger than $P_k^{\prime }$, so there is always a Kalman gain matrix $K^{\prime }>K$. This shows that the use of the attenuation filter algorithm for the new measurement value is more weighted than the Kalman filter, which will cause the filter gain matrix to converge to a larger value, thus reducing the impact of model error. The larger the value of the scalar factor $\lambda$ is, the higher the trust in the latest measurement value and the lower the trust in the historical measurement value. Through actual debugging, it is determined that $\lambda =1.02$.

$${\tilde{P}}_k^{\prime }={\lambda }^{2}F{\tilde{P}}_{k-1}{F}^{\mathrm{T}}+Q$$

(43)

where $\lambda$ is the scalar factor, $\lambda >1$.

$$K^{\prime }={\tilde{P}}_k^{\prime }{H}^{\mathrm{T}}{(H{\tilde{P}}_k^{\prime }{H}^{\mathrm{T}}+R)}^{-1}$$

(44)

$${\tilde{P}}_k={\tilde{P}}_k^{\prime }-K^{\prime }H{\tilde{P}}_k^{\prime }$$

(45)

4. Flight Test Verification

4.1. Verification Experiment of Attitude Solution

In order to verify the actual effect of the complementary filtering algorithm, the flight control module is installed at the center of mass of the “Phoenix” flapping wing robot, as shown in Figure 13. The flight control module also integrates the industrial-grade attitude calculation module Xsens-MTi3 and the MPU6050 chip at the same time. Among them, the MPU6050 is a traditional motion tracking chip, which is packaged with a three-axis gyroscope, a three-axis accelerometer and a DMP (Motion Digital Processor, DMP). The performance comparison between the chip and the Xsens-MTi3 module is shown in

Table 2. Comparison between MPU6050 and Xsens-MTi3.

	MPU6050	Xsens-MTi3
size	4 × 4 × 0.9 mm	12 × 12 × 2 mm
weight	<1 g	8 g
Calculating power	200 Hz	1 kHz
accuracy	Ø 10°	<1°
cost	8~12¥	1000~9000¥

. It can be seen that the Xsens-MTi3 module has higher attitude estimation capability and attitude calculation accuracy, but it is expensive. Therefore, from the perspective of cost, MPU6050 is more suitable for attitude estimation of the “Phoenix” flapping wing robot.

After the flight control module is installed, the flapping wing robot is controlled by the remote controller to fly for a period of time. The attitude information solved by the display complementary filtering algorithm and the attitude data output by Xsens-MTi3 are recorded by the SD (Secure Digital, SD) card. The comparison of attitude changes over time is shown in Figure 14.

It can be seen that the initial attitude solution by complementary filtering gradually converges with time. During the flight process of the “Phoenix” flapping wing robot, the results of the display complementary filtering algorithm and the output of Xsens-MTi3 are basically consistent, and the attitude estimation is always convergent during the whole flight process. The results show that the attitude calculation accuracy is less than 5 degrees, which basically meets the stability and rapidity indicators of the system design.

4.2. Verification Experiment of Position Solution

The flight test was conducted in an open sports field with no obstructions and strong magnetic field interference. The SD card was used to record the flight trajectory of the “Phoenix” flapping wing robot, and the accuracy and stability of the position calculation algorithm were evaluated. First of all, in order to verify the accuracy of the position calculation, let the “Phoenix” flapping robot flap at a certain frequency, making a circle around the middle of the sports field (the length of the middle ring of the sports field is about 158 m and the width is about 78 m). Then, walk around the outside of the sports field for half a circle, and the obtained plane trajectory is shown in Figure 15 (left). From the figure, the northward distance between points 1 and 2 is 158.13 m, and the eastward distance between points 3 and 4 is 78.98 m. It can be seen that the estimation accuracy of the horizontal position is less than 1.5 m, which meets the relevant requirements. The variation of height with time is shown in Figure 15 (right). Taking the height at the beginning of the detour as the benchmark, it can be seen that the fluctuation of the vertical position is within 1 m, which also meets the requirements.

Then, the “Phoenix” flapping wing robot is controlled by the remote control to fly over the playground for a period of time, and the position and speed information during the flight are recorded. The variation of speed with time during flight is shown in Figure 16. It can be seen that the maximum horizontal resultant velocity speed of the “Phoenix” flapping wing robot is about 10 m/s, and the average horizontal resultant velocity speed is 7 m/s, which has strong maneuverability. In the vertical direction, due to the reciprocating vibration of the fuselage, the speed changes very drastically. The maximum climbing speed is about 2.5 m/s, and the maximum descending speed is about 5 m/s.

Based on the above two experiments, it can be seen that in the whole flight process, the Kalman filter algorithm does not have any divergence in the estimation of the horizontal position, and the stability is strong. The estimation of the flying height of the flapping wing robot tends to be stable on the whole. the Kalman filter algorithm is used to solve the position and speed of the “Phoenix” flapping wing robot, which meets the requirements of accuracy and stability, and has received better results.

5. Conclusions

In view of the large mass ratio of the wings of the large bionic flapping wing flying robot and the fluctuation of the fuselage centroid during the flight, the traditional state perception and estimation methods struggle to provide stable and accurate data. This paper takes the “Phoenix” large-scale bionic flapping wing flying robot with a wingspan of 2.2 m as the research object, studies its integrated navigation method of attitude and position, and develops a corresponding flight controller to carry out flight experiment verification. A three-axis attitude estimation method for flapping wing flying robot based on the fusion of inertial sensor and magnetometer data is proposed, which solves the problem of attitude divergence caused by periodic heaving and pitching motions. The developed explicit complementary filtering algorithm fuses the IMU and magnetometer data to solve the attitude information of the flapping wing robot. The results obtained by the explicit complementary filtering algorithm are compared with the high-precision attitude module Xsens-MTi3. The results show that the attitude calculation accuracy is less than 5 degrees, which indicates that the use of low-cost sensors has achieved high-precision position and attitude estimation.

The position and velocity of Phoenix flapping wing robot in three-dimensional space are solved by using integrated navigation technology and the Kalman filter to fuse inertial navigation, GPS and barometer data. The Kalman filter is optimized by using the filtering algorithm, which improves the robustness of the Kalman filter. Aiming at the problem of filter divergence, the attenuation memory method is used to correct the model error, which improves the accuracy of position estimation. Finally, a complete position calculation process is planned and flight experiments are carried out. The results show that the accuracy and stability of the position solution meet the requirements when the sensor signal is good. At present, this method is mainly studied from the perspective of multi rigid bodies, and satisfactory results are obtained. In the future, the flexible vibration characteristics and fluid structure coupling characteristics of the wing will be considered, and on this basis, the method will be further optimized to improve the accuracy of state perception and estimation, ensuring real-time performance.