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

Microminiature unmanned vehicle positioning and air navigation aid based on triones navigation system

Technical field

The present invention relates to unmanned vehicle and flight control method；Specifically, it is directed to use with Beidou satellite navigation system Multi-rotor aerocraft is positioned and navigation.

Background technology

With inexpensive inertial measurement system (Inertial Navigation System, INS) and global positioning system Appearance, unmanned vehicle systematic difference has been not only limited to military domain, and the unmanned vehicle of low cost is more and more Be applied in civil area.At present the overwhelming majority in unmanned plane area research achievement all relies on global positioning system (Global Positioning System, GPS) system is positioned.However, GPS is a kind of passive sensor, pole Be vulnerable to interference or cheat, conventional inexpensive MEMS (Micro-Electro-Mechanical System, MEMS) sensor group into inertial navigation system in, once lose the renewal of GPS position information, position and speed resolving essence Degree will be reduced rapidly.In miniature unmanned vehicle potential application scene, the reliability of gps signal cannot be guaranteed, It is badly in need of a kind of replacement positioning mode.

The image information obtained by airborne vision sensor, by associated picture Processing Algorithm, obtains position of aircraft Further it is controlled with attitude information, is a feasible approach of miniature unmanned vehicle autonomous localization and navigation aspect.But Independent vision guided navigation algorithm, it is impossible to provide the accurate latitude and longitude information of target, in battlefield, the application of the aspect such as investigation is subject to very big Limit.

The GPS system controlled by US military, built up completely in 1994, by comparison, the Big Dipper that China develops recently Navigation system, the two-way communications capabilities due to possessing uniqueness so as to for interference and curve have very high resistivity. At present, triones navigation system not yet builds up completely, and overlay area is only limitted to China and the Asian-Pacific area, with ripe GPS system phase Than positioning precision also has gap, and the positioning precision in CONTINENTAL AREA OF CHINA is about 10m, and GPS system is complete through decades Deal with problems arising from an accident, positioning precision has brought up to 1m.The relatively low positioning precision of dipper system, flies for four rotors of hovering flight are needed The control of row device brings very big challenge, and the GPS+ inertial navigations that existing commercial, ripe flight control system is adopted are melted Conjunction mode, has been no longer desirable for dipper system.

In order to realize using high precision position, the velocity estimation of triones navigation system, it is with reference to airborne vision navigation system One of solution.In pertinent literature, vision guided navigation is applied to the application case of assisted GPS sys.For example：Use The velocity information that light flow sensor is obtained improves the position of GPS and tachometric survey precision.The velocity information that optical flow method is provided may be used also It is compared with the speed of GPS, and then obtains the relative altitude on ground.During Technique in Rendezvous and Docking, vision guided navigation algorithm Be used to merge with GPS information, to obtain the relative position information between spacecraft.

In past research work, using the unmanned vehicle navigation algorithm of GPS satellite navigation system achieved with phase To many achievements in research, especially assistant GPS WAAS (Wide Area Augmentation System, WAAS) input application and US military are removed after SA (Selective Availability) interference, civilian GPS location essence Degree improves 1 meter from 100 meters of the initial stage.The auxiliary information that WAAS WAASs are provided, can improve GPS location measurement The reliability and precision of information.And after SA interference was closed in 2000 by U.S. army, more substantially increase the positioning accurate of civilian GPS Degree.In the region that WASS systems are covered, up to 1 meter, this just makes inexpensive inertial navigation system also can to the positioning precision of GPS system Relatively accurate location estimation is obtained with the help of GPS, and then has promoted the development in civilian unmanned vehicle field significantly.

But as China still in the Beidou satellite navigation system of construction period, it is similar to strengthening system as WAAS still Do not build, therefore either positioning precision or reliability of positioning aspect, there is larger gap.In same place synchronization Collection triones navigation system is contrasted with the original measurement value of GPS navigation system.By continuous 10 minutes under inactive state Measurement, in east, the position measurements of all GPS upwards are in the range of ± 1.5m, and the position measurement of triones navigation system Value is dispersed in the scope of ± 15m.

In addition to the deficiency of precision, due to satellite in orbit limited amount, positioning can for triones navigation system in practical application It is relatively low by property, because blocking positioning can be caused to lose often.And for traditional system with inexpensive inertial navigation device composition For, reliable position measurement is requisite, lacks the renewal of positional information, the inertial navigation system of inexpensive, low precision The location estimation of system will dissipate in seconds.Even using accurate navigation level inertia device, inertial navigation system Position estimation error has just reached more than 30m in five minutes.

The content of the invention

In order to realize the precise control to quadrotor, accurate, the smooth estimation for obtaining position is very important. But due to the deficiency of Beidou satellite navigation system precision, using sensor fusion method that is traditional, being applied to GPS system, The stable autonomous hovering of quadrotor can not be realized.On the other hand, triones navigation system is in-orbit still in the middle of building The restriction of number of satellite, makes dipper system be more susceptible to blocking for the barriers such as building, trees, causes the deterioration of alignment quality.For Solve the above problems, the present invention proposes a kind of sensor fusion method of suitable dipper system, by introducing vision guided navigation, position The accuracy and Lu Bang for putting estimation is improved.The concrete technical scheme that the present invention is adopted is, based on Beidou navigation system The microminiature unmanned vehicle positioning of system and air navigation aid, comprise the steps：Using installed in four rotor wing unmanned aerial vehicle bottoms Light flow sensor obtains the velocity information of unmanned plane, and using Airborne Inertial guider acceleration information is obtained, and is regarded using airborne Feel system acquisition speed information, with reference to the original measurement value of dipper system position, fused filtering is obtained for position and speed Estimation；And then by nonlinear positional control algorithm, realize position of aircraft control.

State vector X of its wave filter of fused filtering is defined as：

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 mutual of Normal Distribution Characteristics Independent input noise and observation noise, input vector u, and observation vector Z is defined as follows：

U=(a_x,a_y)^T

Wherein (a_x,a_y) for Airborne Inertial sensor obtain horizontal direction acceleration measurement, state transition matrix A and Input control matrix B is defined as follows：

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

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

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)

Wherein P is the covariance matrix of state, and covariance matrix Q represents the noise of acceleration information, covariance matrix R The noise of the observation of dipper system and vision system is represented, the two matrixes are diagonal matrix, it is possible to by the reality for recording Flying quality 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 The item numerical value of correspondence vision system observation is less

By nonlinear positional control algorithm, realize that position of aircraft control is referred to and controlled using nonlinear Shandong nation Device, it is described in detail below：

The position of selection aircraft and yaw angle are expressed as η=[x y z ψ] as the output of system^T, control targe is Aircraft is set to track a certain given track, this track is represented by η_d=[x_d y_d z_d ψ_d]^T；Lateral attitude x and longitudinal position Putting y can be obtained by the feedback of airborne vision system, and the position z of vertical direction can be obtained from onboard barometrical reading, be controlled Device is made up of inner ring both attitude ring and outer shroud both position ring；Inner ring employs ratio, integration, the differential control of classics (Proportion Integration Differentiation, PID) controller, outer shroud has used non-linear Shandong nation to control Device, desired roll, pitch attitude angleAnd roll, pitch attitude angular speedBy outer ring controller meter Obtain, 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.

Outer shroud positioner has been used based on robust error symbol functional integration (Rotust Inetgral of the Signum of the Error, RISE) 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：

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

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

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 rolling The ratio of angular pose controller, differential, integral coefficient, k_pθ,k_dθ,k_iθ, be the ratio of luffing angle attitude controller, differential, Integral coefficient, k_pψ,k_dψ,k_iψFor the ratio of yaw angle attitude controller, differential, integral coefficient, tracking errore_θ,e_ψIt is fixed Justice is：

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

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

Compared with the prior art, technical characterstic of the invention and effect：

Using relatively low precision domestic Beidou satellite navigation system when, unmanned vehicle high precision position control method Primary Study is carried out.By the way that Big Dipper positional information is merged with the velocity information of airborne vision navigation system, obtain To position of aircraft high accuracy, the estimate without accumulated error, meanwhile, under the control of non-linear placement controller, realize Controlled using the high precision position of Beidou satellite navigation system.The flight validation of long range shows, sensing proposed by the invention Device integration program, combines optical flow method short-term position estimation accuracy height and Beidou satellite navigation system without the excellent of long-term accumulated error Point, tentatively realizes application of the triones navigation system on the unmanned vehicle for possessing hovering flight ability.

Description of the drawings

Fig. 1 is a kind of schematic diagram of specific embodiment of multi-Sensors Fusion Filtering device involved in the present invention.

Fig. 2 is a kind of specific embodiment of airborne Big Dipper flight control system involved in the present invention.

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

Specific embodiment

The technical problem to be solved is to provide one kind based on Beidou satellite navigation system and vision sensor number According to the unmanned plane autonomic positioning method of fusion, realize that the accurate of unmanned plane positions without drift under outdoor environment.

The technical solution used in the present invention is：Using Beidou satellite navigation system and the method for light stream Data Fusion of Sensor In for the alignment system of unmanned plane, comprise the steps：

The present invention is entered the Big Dipper by wave filter using " sensor fusion (filtering)-control " framework with light stream, inertial navigation Row fusion, realizes the flight control accuracy that independent triones navigation system cannot be realized.Additionally, also by nonlinear based on robust The New Type of Robust control of error symbol functional integration (Rotust Inetgral of the Signum of the Error, RISE) Device processed is used in the control algolithm of aircraft, further increases control effect.

The velocity information of unmanned plane is obtained using the light flow sensor installed in four rotor wing unmanned aerial vehicle bottoms, and using this speed Degree information is improving position estimation accuracy.It is acceleration information in inertial navigation, airborne by using the filter construction that such as Fig. 1 shows With the help of the velocity information of vision system, with reference to the original measurement value of dipper system position, it is possible to obtain for position and speed The high-precision reliable estimation of degree.And then by nonlinear positional control algorithm, realize high-precision position of aircraft control System.

The present invention adopts the technical scheme that, the data of vision sensor is carried out with the positional information of triones navigation system Fusion, and then the positioning of unmanned plane is realized, comprise the steps：

The velocity information of unmanned plane is obtained using the light flow sensor installed in four rotor wing unmanned aerial vehicle bottoms, and using this speed Degree information is improving position estimation accuracy.It is acceleration information in inertial navigation, airborne by using the filter construction that such as Fig. 1 shows With the help of the velocity information of vision system, with reference to the original measurement value of dipper system position, it is possible to obtain for position and speed The high-precision reliable of degree is estimated.

The state vector of designed wave filterIt is defined as：

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

The state transition equation and observational equation of Kalman filter is shown below：

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

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

Wherein, u is input vector, and Z is observation vector, and w and v is that the separate input with Normal Distribution Characteristics is made an uproar Sound and observation noise.Input vector u, and observation vector Z is defined as follows：

U=(a_x,a_y)^T

Wherein (a_x,a_y) for Airborne Inertial sensor obtain horizontal direction acceleration measurement, state transition matrix A and Input control matrix B is defined as follows：

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

The target of Kalman filter is using observation Y (k) at k moment, the state estimation of previous momentAnd input control quantity u (k-1) of previous moment, the state to the k momentOptimal estimation is carried out, this is unified The optimal filter that meter is learned is shown below：

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)

Wherein covariance matrix Q represents the noise of acceleration information, and covariance matrix R represents dipper system and vision system The noise of the observation of system, the two matrixes are diagonal matrix, it is possible to determined by the practical flight data for recording.In the system In, the numerical value of matrix Q is less, and to correspond in R and have chosen larger numerical value with the item of dipper system observation, and corresponds to vision system The item numerical value of overall view measured value is less

In order to improve the rejection ability for disturbing to external world, nonlinear Shandong nation controller is used on unmanned vehicle. The controller design is as follows：

The position of selection aircraft and yaw angle are expressed as η=[x y z ψ] as the output of system^T, control targe is Aircraft is set to track a certain given track, this track is represented by η_d=[x_d y_d z_d ψ_d]^T.The position x in translation direction and Y can be obtained by the feedback of airborne vision system, and the position z of vertical direction can be obtained from onboard barometrical reading.Controller It is made up of inner ring (attitude ring) and outer shroud (position ring).Inner ring employs ratio, integration, the differential control of classics (Proportion Integration Differentiation, PID) controller, outer shroud has used non-linear Shandong nation to control Device.Desired attitude angleAnd attitude angular velocityIt is calculated by outer ring controller, four after simplifying The kinetic model in the translation direction of rotor craft, is represented by：

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

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

Here μ (t) represents desired vector acceleration or virtual location dominant vector.

Because the weight of microminiature unmanned vehicle is less, it is vulnerable to the impact of the external disturbances such as air-flow.In order to improve control The robustness of device processed, outer shroud positioner has been used based on robust error symbol functional integration (Rotust Inetgral of The Signum of the Error, RISE) 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.Controller design behind for convenience, introduces following auxiliary and misses Difference signal：

Here α_x,α_yAnd α_zIt is positive gain.Design attitude controller μ (t)：

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

Items are represented by μ (t)：

Total life u can be solved₁With desired attitude angle

Control input u of design inner ring (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 rolling The ratio of angular pose controller, differential, integral coefficient, k_pθ,k_dθ,k_iθ, be the ratio of luffing angle attitude controller, differential, Integral coefficient, k_pψ,k_dψ,k_iψFor the ratio of yaw angle attitude controller, differential, integral coefficient, tracking errore_θ,e_ψIt is fixed Justice is：

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

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

Specific example is given below：

First, system hardware connection and configuration

As shown in Fig. 2 four rotor wing unmanned aerial vehicle autonomous flight control methods of the view-based access control model of the present invention are using based on embedded The flight control structure of formula framework, the experiment porch built includes four rotor wing unmanned aerial vehicle bodies, earth station, remote control etc..Its In four rotor wing unmanned aerial vehicles be equipped with embedded computer (the embedded Intel Core i3 dual core processors of the computer, dominant frequency 1.8GHz), airborne PX4FLOW light flow sensor, GPS and flight controller (containing inertial navigation unit and barometer module etc.). Earth station includes a notebook equipped with (SuSE) Linux OS, the startup and remote monitoring for onboard program.Should Platform can carry out manual takeoff and landing by remote control, and promptly switch to manual mode when occurring unexpected, to guarantee reality Test safety.

2nd, flight experiment result

The present embodiment has carried out multigroup flight Control release to above-mentioned experiment porch, and flight experiment environment is outdoor campus ring In border.Object of experiment is the high accuracy for using triones navigation system information realization unmanned vehicle without drift positioning.

Flight path curve in outdoor hand-held experimentation is as shown in Figure 3.Wherein, indicate ● curve be as reference High-precision GPS receiver measured value, indicate ▲ be obtained by velocity information integration using vision sensor in vision Journey meter measured value, indicates the fusion results of the multiple sensor integrated method designed by the present invention of ■.It can be seen that The precision of the original measurement value of Beidou satellite navigation system is relatively low, and visual odometry method is produced after the work of long range Obvious accumulated error is given birth to, and has used the fusion method designed by the present invention, it is possible to obtain in high precision and without accumulated error Location estimation, it was demonstrated that the validity of designed blending algorithm of the invention.

Obviously, examples detailed above is only clearly to illustrate example, and not to the restriction of embodiment, for For those of ordinary skill in the art, the change or change of other multi-forms can also be made on the basis of the above description It is dynamic.There is no need to be exhaustive to all embodiments.And the obvious change thus amplified out or change still Among the protection domain of the 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.