CN106647784A - Miniaturized unmanned aerial vehicle positioning and navigation method based on Beidou navigation system - Google Patents

Abstract

The invention relates to the field of unmanned aerial vehicles and flight control and discloses a miniaturized unmanned aerial vehicle positioning and navigation method based on a Beidou navigation system. The introduction of visual navigation, the position estimation accuracy and the robustness are improved. According to the concrete technical scheme provided by the invention, the miniaturized unmanned aerial vehicle positioning and navigation method based on the Beidou navigation system comprises the following steps: obtaining speed information of an unmanned aerial vehicle by use of an optical flow sensor installed at the bottom of the quad-rotor unmanned aerial vehicle, obtaining accelerated speed information by use of an airborne inertial navigation apparatus, obtaining speed information by use of an airborne visual system, and through combination with an original measurement value of the position of the Beidou system, obtaining estimation of the position and the speed through fusion filtering; and accordingly, through a nonlinear position control algorithm, realizing aerial vehicle position control. The method is mainly applied to unmanned aerial vehicle and flight control occasions.

Description

Positioning and navigation method of micro unmanned aerial vehicle based on Beidou navigation system

technical field

The invention relates to the fields of unmanned aerial vehicles and flight control; specifically, it relates to the positioning and navigation of multi-rotor aircraft using the Beidou satellite navigation system.

Background technique

With the emergence of low-cost inertial measurement systems (Inertial Navigation System, INS) and global positioning systems, the application of unmanned aerial vehicle systems is not limited to the military field, and low-cost unmanned aerial vehicles are increasingly used in civilian fields middle. Most of the current research results in the field of unmanned aerial vehicles rely on the Global Positioning System (Global Positioning System, GPS) system for positioning. However, the GPS receiver is a passive sensor, which is very vulnerable to interference or spoofing. In the inertial navigation system composed of commonly used low-cost Micro-Electro-Mechanical System (MEMS) sensors, once the GPS position information is lost update, the accuracy of position and velocity solutions will degrade rapidly. In the potential application scenarios of micro-unmanned aerial vehicles, the reliability of GPS signals cannot be guaranteed, and an alternative positioning method is urgently needed.

The image information obtained by the airborne visual sensor and the related image processing algorithm are used to obtain the position and attitude information of the aircraft and then control it. It is a feasible way for the autonomous positioning and navigation of the micro UAV. However, the visual navigation algorithm alone cannot provide accurate latitude and longitude information of the target, and its application in battlefield reconnaissance is greatly limited.

Compared with the GPS system controlled by the US military, which was fully completed in 1994, China's newly developed Beidou navigation system is highly resistant to jamming and spoofing signals due to its unique two-way communication capabilities. At present, the Beidou navigation system has not been fully completed, and its coverage area is limited to China and the Asia-Pacific region. Compared with the mature GPS system, the positioning accuracy is also far behind. After the improvement of the system, the positioning accuracy has been improved to 1m. The relatively low positioning accuracy of the Beidou system has brought great challenges to the control of quadrotor aircraft that need to hover. Then apply to the Beidou system.

In order to achieve high-precision position and velocity estimation using the Beidou navigation system, combining the airborne visual navigation system is one of the solutions. In the relevant literature, there have been application cases of visual navigation applied to assisted GPS systems. For example: using the speed information obtained by the optical flow sensor to improve the position and speed measurement accuracy of GPS. The speed information provided by the optical flow method can also be compared with the speed of GPS to obtain the relative height of the ground. During space rendezvous and docking, visual navigation algorithms are used to fuse with GPS information to obtain relative position information between spacecraft.

In the past research work, the UAV navigation algorithm using the GPS satellite navigation system has achieved relatively many research results, especially when the GPS-assisted Wide Area Augmentation System (Wide Area Augmentation System, WAAS) was put into application and the U.S. military After removing the SA (Selective Availability) interference, the civilian GPS positioning accuracy has increased by 1 meter from the initial 100 meters. Auxiliary information provided by the WAAS Wide Area Augmentation System improves the reliability and accuracy of GPS position measurement information. After the SA interference was shut down by the US military in 2000, it greatly improved the positioning accuracy of civilian GPS. In the area covered by the WASS system, the positioning accuracy of the GPS system can reach 1 meter, which enables the low-cost inertial navigation system to obtain relatively accurate position estimation with the help of GPS, which in turn greatly promotes the development of the field of civilian unmanned aerial vehicles .

However, as my country's Beidou satellite navigation system is still in the construction stage, augmentation systems like WAAS have not yet been built, so there is a big gap in both positioning accuracy and positioning reliability. The original measurement values of the Beidou navigation system and the GPS navigation system were collected at the same time at the same place for comparison. Through continuous 10-minute measurements in a static state, all GPS position measurements in the east direction are within the range of ±1.5m, while those of the Beidou navigation system are scattered within the range of ±15m.

In addition to the lack of accuracy, due to the limited number of satellites in orbit of the Beidou navigation system in practical applications, the positioning reliability is low, and positioning is often lost due to occlusion. For the traditional system composed of low-cost inertial navigation devices, reliable position measurement is essential, lacking the update of position information, the position estimation of low-cost and low-precision inertial navigation systems can be completed within a few seconds. will diverge. Even with sophisticated navigation-grade inertial devices, the position estimation error of the inertial navigation system reaches more than 30m within five minutes.

Contents of the invention

To achieve precise control of a quadrotor, it is important to obtain an accurate, smooth estimate of position. However, due to the lack of accuracy of the Beidou satellite navigation system, the traditional sensor fusion method applied to the GPS system cannot achieve stable and autonomous hovering of the quadrotor aircraft. On the other hand, the Beidou navigation system is still under construction, and the limitation of the number of satellites in orbit makes the Beidou system more vulnerable to obstacles such as buildings and trees, resulting in deterioration of positioning quality. In order to solve the above problems, the present invention proposes a sensor fusion method suitable for the Beidou system. By introducing visual navigation, the accuracy and Lupine performance of position estimation have been improved. The specific technical solution adopted by the present invention is that the positioning and navigation method of the micro-miniature unmanned aerial vehicle based on the Beidou navigation system includes the following steps: using the optical flow sensor installed at the bottom of the quadrotor drone to obtain the speed information of the drone, using The airborne inertial navigation device obtains the acceleration information, and the airborne vision system obtains the velocity information. Combined with the original measurement value of the position of the Beidou system, the estimation of the position and velocity is obtained through fusion filtering; and then through the nonlinear position control algorithm, the position of the aircraft is realized. control.

The state vector X of the filter of the fusion filter is defined as:

Among them, (x, y) is the position in the horizontal direction, is the velocity in the horizontal direction;

The filter is a Kalman filter, and the state transition equation and observation equation are as follows:

X(k)＝AX(k-1)+Bu(k-1)+ω(k-1)

Z(k)=CX(k)+ν(k)

Among them, k represents the time, u is the input vector, Z is the observation vector, ω and v are the independent input noise and observation noise with normal distribution characteristics, the input vector u and the observation vector Z are defined as follows:

u＝(a _x ,a _y ) ^T

Where (a _x , a _y ) is the horizontal acceleration measurement value obtained by the airborne inertial sensor, the state transition matrix A and the input control matrix B are defined as follows:

Where _δt is the sampling period of the sensor, and the observation matrix C is defined as follows:

The goal of the Kalman filter is to use the observed value Y(k) at time k, the state estimate at the previous time And the input control quantity u(k-1) at the previous moment, for the state at time k For optimal estimation, this statistically optimal filter is given by the following formula:

P(k|k-1)＝AP(k-1)A ^T +BQB ^T

H(k)＝P(k|k-1)C ^T (CP(k|k-1)C ^T +R) ^-1

P(k)=(I-H(k)C)P(k|k-1)

Among them, P is the covariance matrix of the state, the covariance matrix Q represents the noise of the acceleration data, and the covariance matrix R represents the noise of the observation values of the Beidou system and the vision system. These two matrices are diagonal and can be recorded by The actual flight data is determined,

Among them, the value of the matrix Q is small, and the item corresponding to the observation value of the Beidou system in R selects a large value, while the value of the item corresponding to the observation value of the visual system is small

Through the nonlinear position control algorithm, the realization of aircraft position control refers to the use of nonlinear Lupine controller, as follows:

Select the position and yaw angle of the aircraft as the output of the system, expressed as η=[xyz ψ] ^T , the control target is to make the aircraft track a given trajectory, this trajectory can be expressed as η _d =[x _d y _d z _d ψ _d ] ^T ; the lateral position x and longitudinal position y can be obtained through the feedback of the airborne vision system, and the vertical position z can be obtained from the readings of the onboard barometer. The loop is composed of a position loop; the inner loop uses a classic proportional, integral, differential control (Proportion Integration Differentiation, PID) controller, and the outer loop uses a nonlinear Lubang controller. The expected roll and pitch angles And roll, pitch attitude angular velocity Calculated by the outer loop controller, the simplified dynamic model of the translational direction of the quadrotor is expressed as:

When the aircraft reaches the given trajectory η _d =[x _d y _d z _d ψ _d ] ^T ,

Define the auxiliary vector μ=[μ _x μ _y μ _z ] ^T :

Here μ represents the desired acceleration vector or virtual position control vector.

The outer loop position controller uses a new robust controller based on the robust integral of the Signum of the Error (RISE). The error signal that defines position tracking is as follows:

e _x1 ＝x _d -xe _y1 ＝y _d -ye _z1 ＝z _d -z

where x _d , y _d , z _d are time-varying reference trajectories, and the following auxiliary error signals are introduced:

Here α _x , α _y and α _z are positive gains, and the position controller μ is designed:

Where k _sx , k _sy , k _sz , β _x , β _y , β _z are positive gains, and sgn(·) is the marked sign function.

The terms in μ(t) can be expressed as:

Solve for total lift u ₁ (t) and desired attitude angle

Design the control inputs u ₂ , u ₃ , u ₄ of the inner loop, namely the attitude loop controller, as follows:

In the formula k _pθ ,k _dθ , _kiθ ,k _pψ ,k _dψ ,k _iψ are positive gains, where, are the proportional, differential and integral coefficients of the roll angle attitude controller, k _pθ , k _dθ , k _iθ , are the proportional, differential and integral coefficients of the pitch angle attitude controller, k _pψ , k _dψ , k _iψ are the yaw angles Proportional, differential, integral coefficients of attitude controller, tracking error e _θ ,e _ψ are defined as:

e _θ = θ _d -θ e _ψ = ψ _d -ψ

θ _d is obtained by the outer loop controller, and ψ _d is the time-varying trajectory of the yaw angle.

Compared with prior art, technical characteristic and effect of the present invention:

A preliminary study was conducted on the method of high-precision position control of unmanned aerial vehicles when using the relatively low-precision domestic Beidou satellite navigation system. By fusing the Beidou position information with the speed information of the airborne visual navigation system, the estimated value of the aircraft position with high precision and no cumulative error is obtained. At the same time, under the control of the nonlinear position controller, the use of Beidou satellite navigation is realized. High-precision position control of the system. The long-distance flight verification shows that the sensor fusion scheme proposed by the present invention combines the advantages of high short-term position estimation accuracy of the optical flow method and the advantages of no long-term cumulative error of the Beidou satellite navigation system, and initially realizes the Beidou navigation system with the hovering flight capability. applications on unmanned aerial vehicles.

Description of drawings

FIG. 1 is a schematic diagram of a specific embodiment of a multi-sensor fusion filter involved in the present invention.

Fig. 2 is a specific implementation of the airborne Beidou flight control system involved in the present invention.

Fig. 3 is the actual effect of the method involved in the present invention.

detailed description

The technical problem to be solved by the present invention is to provide an autonomous positioning method for unmanned aerial vehicles based on the fusion of Beidou satellite navigation system and visual sensor data, so as to realize accurate and drift-free positioning of unmanned aerial vehicles in outdoor environments.

The technical scheme adopted in the present invention is: adopt the method of data fusion of the Beidou satellite navigation system and the optical flow sensor to be used in the positioning system of the unmanned aerial vehicle, comprising the following steps:

The present invention adopts a "sensor fusion (filtering)-control" framework, and integrates Beidou, optical flow, and inertial navigation through a filter, and realizes flight control accuracy that cannot be achieved by a single Beidou navigation system. In addition, a new nonlinear robust controller based on the robust integral of the sign of error (Rotust Inegral of the Signum of the Error, RISE) is used in the control algorithm of the aircraft, which further improves the control effect.

The optical flow sensor installed on the bottom of the quadrotor UAV is used to obtain the speed information of the UAV, and this speed information is used to improve the accuracy of position estimation. By using the filter structure shown in Figure 1, with the help of the acceleration information of the inertial navigation system and the velocity information of the airborne vision system, combined with the original measurement value of the position of the Beidou system, a high-precision and reliable position and velocity can be obtained. estimate. Furthermore, the non-linear position control algorithm is used to realize high-precision aircraft position control.

The technical solution adopted by the present invention is to fuse the data of the visual sensor with the position information of the Beidou navigation system, and then realize the positioning of the drone, including the following steps:

The optical flow sensor installed on the bottom of the quadrotor UAV is used to obtain the speed information of the UAV, and this speed information is used to improve the accuracy of position estimation. By using the filter structure shown in Figure 1, with the help of the acceleration information of the inertial navigation system and the velocity information of the airborne vision system, combined with the original measurement value of the position of the Beidou system, a reliable estimate of position and velocity with high precision can be obtained .

The state vector of the designed filter defined as:

Among them, (x, y) is the position in the horizontal direction, is the velocity in the horizontal direction.

The state transition equation and observation equation of the Kalman filter are as follows:

X(k)＝AX(k-1)+Bu(k-1)+ω(k-1)

Z(k)=CX(k)+ν(k)

Among them, u is the input vector, Z is the observation vector, w and v are independent input noise and observation noise with normal distribution characteristics. The input vector u, and the observation vector Z are defined as follows:

u＝(a _x ,a _y ) ^T

Where (a _x , a _y ) is the horizontal acceleration measurement value obtained by the airborne inertial sensor, the state transition matrix A and the input control matrix B are defined as follows:

Where _δt is the sampling period of the sensor, and the observation matrix C is defined as follows:

The goal of the Kalman filter is to use the observed value Y(k) at time k, the state estimate at the previous time And the input control quantity u(k-1) at the previous moment, for the state at time k For optimal estimation, this statistically optimal filter is given by the following formula:

P(k|k-1)＝AP(k-1)A ^T +BQB ^T

H(k)＝P(k|k-1)C ^T (CP(k|k-1)C ^T +R) ^-1

P(k)=(I-H(k)C)P(k|k-1)

The covariance matrix Q represents the noise of the acceleration data, and the covariance matrix R represents the noise of the observations of the Beidou system and the vision system. These two matrices are diagonal and can be determined from the recorded actual flight data. In this system, the value of the matrix Q is small, and the item in R corresponding to the observation value of the Beidou system selects a large value, while the value of the item corresponding to the observation value of the visual system is small

In order to improve the ability to suppress external disturbances, a nonlinear Lupine controller is used on the unmanned aerial vehicle. The controller is designed as follows:

Select the position and yaw angle of the aircraft as the output of the system, expressed as η=[xyz ψ] ^T , the control target is to make the aircraft track a given trajectory, this trajectory can be expressed as η _d =[x _d y _d z _d ψ _d ] ^T . The position x and y in the translational direction can be obtained through the feedback of the onboard vision system, and the position z in the vertical direction can be obtained from the reading of the onboard barometer. The controller consists of an inner loop (attitude loop) and an outer loop (position loop). The inner loop uses a classic proportional, integral, differential control (Proportion Integration Differentiation, PID) controller, and the outer loop uses a nonlinear Lupine controller. desired attitude angle and attitude angular velocity Calculated by the outer loop controller, the simplified dynamic model of the translational direction of the quadrotor aircraft can be expressed as:

When the aircraft reaches the given trajectory η _d =[x _d y _d z _d ψ _d ] ^T ,

Define the auxiliary vector μ=[μ _x μ _y μ _z ] ^T :

Here μ(t) represents the desired acceleration vector or virtual position control vector.

Due to the small weight of the micro UAV, it is easily affected by external disturbances such as airflow. In order to improve the robustness of the controller, the outer loop position controller uses a new robust controller based on the robust integral of the Signum of the Error (RISE). The error signal that defines position tracking is as follows:

e _x1 ＝x _d -xe _y1 ＝y _d -ye _z1 ＝z _d -z

Where x _d , y _d , z _d are time-varying reference trajectories. In order to facilitate the subsequent controller design, the following auxiliary error signal is introduced:

Here α _x , α _y and α _z are positive gains. Design the position controller μ(t):

Where k _sx , k _sy , k _sz , β _x , β _y , β _z are positive gains, and sgn(·) is the marked sign function.

The terms in μ(t) can be expressed as:

The total lift u ₁ and the expected attitude angle can be solved

Design the control inputs u ₂ , u ₃ , u ₄ of the inner loop (attitude loop) controller as follows:

In the formula k _pθ ,k _dθ , _kiθ ,k _pψ ,k _dψ ,k _iψ are positive gains, where, are the proportional, differential and integral coefficients of the roll angle attitude controller, k _pθ , k _dθ , k _iθ , are the proportional, differential and integral coefficients of the pitch angle attitude controller, k _pψ , k _dψ , k _iψ are the yaw angles Proportional, differential, integral coefficients of attitude controller, tracking error e _θ ,e _ψ are defined as:

e _θ = θ _d -θ e _ψ = ψ _d -ψ

θ _d is obtained by the outer loop controller, and ψ _d is the time-varying trajectory of the yaw angle.

Specific examples are given below:

1. System hardware connection and configuration

As shown in Figure 2, the vision-based quadrotor UAV autonomous flight control method of the present invention adopts a flight control structure based on an embedded architecture, and the built experimental platform includes a quadrotor UAV body, a ground station, a remote control, etc. . Among them, the quadrotor drone is equipped with an embedded computer (the computer is embedded with an Intel Core i3 dual-core processor with a main frequency of 1.8GHz), an airborne PX4FLOW optical flow sensor, GPS and a flight controller (including an inertial navigation unit and a barometer module. Wait). The ground station includes a notebook computer equipped with a Linux operating system, which is used for the start-up of the airborne program and remote monitoring. The platform can be manually taken off and landed through the remote control, and can be switched to manual mode in case of an accident to ensure the safety of the experiment.

2. Flight experiment results

In this embodiment, multiple groups of flight control experiments are carried out on the above-mentioned experimental platform, and the flight experiment environment is an outdoor campus environment. The goal of the experiment is to use the Beidou navigation system information to achieve high-precision drift-free positioning of unmanned aerial vehicles.

The flight trajectory curve during the outdoor handheld experiment is shown in Figure 3. Among them, the curve marked with ● is the measured value of the high-precision GPS receiver used as a reference, the curve marked with ▲ is the measured value of the visual odometer obtained by using the speed information integration of the visual sensor, and the curve marked with ■ is the measured value of the present invention. Fusion results of the designed multi-sensor fusion method. It can be seen from the figure that the accuracy of the original measurement value of the Beidou satellite navigation system is relatively low, and the visual odometry method has produced obvious cumulative errors after long-distance work, but using the fusion method designed in the present invention, The position estimation with high precision and no accumulative error can be obtained, which proves the effectiveness of the fusion algorithm designed in the present invention.

Apparently, the above examples are only examples for clear description, rather than limiting the implementation. For those of ordinary skill in the art, other changes or changes in different forms can be made on the basis of the above description. It is not necessary and impossible to exhaustively list all implementation manners here. However, the obvious changes or variations derived therefrom are still within the scope of protection of the present invention.

Claims (4)

1. a kind of microminiature unmanned vehicle positioning and air navigation aid based on triones navigation system, is characterized in that, using installation Light flow sensor in four rotor wing unmanned aerial vehicle bottoms obtains the velocity information of unmanned plane, is obtained using Airborne Inertial guider and is added Velocity information, using airborne vision system acquisition speed information, with reference to the original measurement value of dipper system position, fused filtering Obtain for the estimation of position and speed；And then by nonlinear positional control algorithm, realize position of aircraft control.

2. the self-adaption gradient threshold value anisotropic filtering method of partial statistics characteristic is based on as claimed in claim 1, and it is special Levying is, state vector X of its wave filter of fused filtering is defined as：

$X={(x,y,\stackrel{\·}{x},\stackrel{\·}{y})}^T,$

Wherein, (x, y) is the position of horizontal direction,For the speed of horizontal direction；

Wave filter is Kalman filter, and state transition equation and observational equation are shown below：

X (k)=AX (k-1)+Bu (k-1)+ω (k-1)

Z (k)=CX (k)+ν (k)

Wherein, k represents the moment, and u is input vector, and Z is observation vector, and ω and v is with the separate of Normal Distribution Characteristics Input noise and observation noise, input vector u, and observation vector Z be defined as follows：

U=(a_x,a_y)^T

Wherein (a_x,a_y) the horizontal direction acceleration measurement that obtains for Airborne Inertial sensor, state transition matrix A and input Control matrix B is defined as follows：

$\begin{array}{cc}A=\left[\begin{array}{cccc}1& 0& {\mathrm{\δ}}_t& 0\\ 0& 1& 0& {\mathrm{\δ}}_t\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]& B=\left[\begin{array}{cc}\frac{1}{2}{\mathrm{\δ}}_t^2& 0\\ 0& \frac{1}{2}{\mathrm{\δ}}_t^2\\ {\mathrm{\δ}}_t& 0\\ 0& {\mathrm{\δ}}_t\end{array}\right]\end{array}$

Wherein δ_tFor the sampling period of sensor, observing matrix C is defined as follows：

$C=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$

The target of Kalman filter is using observation Y (k) at k moment, the state estimation of previous momentWith And input control quantity u (k-1) of previous moment, the state to the k momentOptimal estimation is carried out, this is statistically most Excellent wave filter is shown below：

$\hat{X}\left(k\right|k-1)=A\hat{X}(k-1)+Bu(k-1)$

P (k | k-1)=AP (k-1) A^T+BQB^T

H (k)=P (k | k-1) C^T(CP(k|k-1)C^T+R)^-1

$\hat{X}\left(k\right)=\hat{X}\left(k\right|k-1)+H\left(k\right)\left(Z\right(k)-C\hat{X}(k|k-1\left)\right)$

P (k)=(I-H (k) C) P (k | k-1)

Wherein P is the covariance matrix of state, and covariance matrix Q represents the noise of acceleration information, and covariance matrix R is represented The noise of the observation of dipper system and vision system, the two matrixes are diagonal matrix, it is possible to by the practical flight for recording Data determination,

The numerical value of wherein matrix Q is less, and correspondence have chosen larger numerical value with the item of dipper system observation in R, and corresponds to The item numerical value of vision system observation is less

$Q=\left[\begin{array}{cccc}0& 0& 0& 0\\ 0& 0& 0& 0\\ 0& 0& 0.005& 0\\ 0& 0& 0& 0.005\end{array}\right]$

$R=\left[\begin{array}{cccc}36& 0& 0& 0\\ 0& 36& 0& 0\\ 0& 0& 0.003& 0\\ 0& 0& 0& 0.003\end{array}\right].$

3. the self-adaption gradient threshold value anisotropic filtering method of partial statistics characteristic is based on as claimed in claim 1, and it is special Levying is, by nonlinear positional control algorithm, realizes that position of aircraft control is referred to using nonlinear Shandong nation controller, tool Body is as described below：The position of selection aircraft and yaw angle are expressed as η=[x y z ψ] as the output of system^T, control mesh Mark is to make aircraft track a certain given track, and this track is represented by η_d=[x_d y_d z_d ψ_d]^T；Lateral attitude x and vertical Can be obtained by the feedback of airborne vision system to position y, the position z of vertical direction can be obtained from onboard barometrical reading, Controller is made up of inner ring both attitude ring and outer shroud both position ring；Inner ring employs ratio, integration, the differential Control PID of classics (Proportion Integration Differentiation) controller, outer shroud has used non-linear Shandong nation controller, phase The roll of prestige, pitch attitude angleAnd roll, pitch attitude angular speedCalculated by outer ring controller Arrive, the kinetic model in the translation direction of the quadrotor after simplifying is expressed as：

When aircraft reaches given track η_d=[x_d y_d z_d ψ_d]^TWhen,

Define auxiliary vector μ=[μ_x μ_y μ_z]^T:

Here μ represents desired vector acceleration or virtual location dominant vector.

4. the self-adaption gradient threshold value anisotropic filtering method of partial statistics characteristic is based on as claimed in claim 3, and it is special Levying is, outer shroud positioner has been used based on robust error symbol functional integration RISE (Rotust Inetgral of the Signum of the Error) novel robust control device.The error signal for defining position tracking is as follows：

e_x1=x_d-x e_y1=y_d-y e_z1=z_d-z

Wherein x_d,y_d,z_dFor the reference locus of time-varying, following auxiliary error signal is introduced：

$\begin{array}{ccc}{e}_{x2}=e_{x1}+{\mathrm{\α}}_x{\stackrel{\·}{e}}_{x1}& e_{y2}=e_{y1}+{\mathrm{\α}}_y{\stackrel{\·}{e}}_{y1}& e_{z2}=e_{z1}+{\mathrm{\α}}_z{\stackrel{\·}{e}}_{z1}\end{array}$

Here α_x,α_yAnd α_zIt is positive gain, design attitude controller μ：

${\mathrm{\μ}}_x=k_{sx}{e}_{x2}+{\∫}_0^t\left(k_{sx}{\mathrm{\α}}_x{e}_{x2}\right(\mathrm{\τ})+{\mathrm{\β}}_x\mathrm{sgn}(e_{x2}\left(\mathrm{\τ}\right)\left)\right)d\mathrm{\τ}$

${\mathrm{\μ}}_y=k_{sy}{e}_{y2}+{\∫}_0^t\left(k_{sy}{\mathrm{\α}}_y{e}_{y2}\right(\mathrm{\τ})+{\mathrm{\β}}_y\mathrm{sgn}(e_{y2}\left(\mathrm{\τ}\right)\left)\right)d\mathrm{\τ}$

${\mathrm{\μ}}_z=k_{sz}{e}_{z2}+{\∫}_0^t\left(k_{sz}{\mathrm{\α}}_z{e}_{z2}\right(\mathrm{\τ})+{\mathrm{\β}}_z\mathrm{sgn}(e_{z2}\left(\mathrm{\τ}\right)\left)\right)d\mathrm{\τ}$

Wherein k_sx,k_sy,k_sz,β_x,β_y,β_zFor positive gain, sgn () is the sign function of mark.

Items are represented by μ (t)：

Solve total life u₁(t) and desired attitude angle

$u_1=m{({\mathrm{\μ}}_x^2+{\mathrm{\μ}}_y^2+{\left({\mathrm{\μ}}_z+g\right)}^2)}^{1/2}$

${\mathrm{\θ}}_d={\tan }^{-1}\left(\frac{{\mathrm{\μ}}_x{\mathrm{cos\ψ}}_d+{\mathrm{\μ}}_y{\mathrm{sin\ψ}}_d}{{\mathrm{\μ}}_z+g}\right)$

Design inner ring is control input u of attitude ring controller₂,u₃,u₄It is as follows：

In formulak_pθ,k_dθ,k_iθ,k_pψ,k_dψ,k_iψIt is positive gain, wherein,For roll angle The ratio of attitude controller, differential, integral coefficient, k_pθ,k_dθ,k_iθ, it is ratio, differential, the integration of luffing angle attitude controller Coefficient, k_pψ,k_dψ,k_iψFor the ratio of yaw angle attitude controller, differential, integral coefficient, tracking errore_θ,e_ψIt is defined as：

e_θ=θ_d-θ e_ψ=ψ_d-ψ

θ_dObtained by outer ring controller, ψ_dFor the time-varying track of yaw angle.