阅读说明：本技术 一种基于速度预测的无人机鲁棒性导航方法 (Unmanned aerial vehicle robustness navigation method based on speed prediction ) 是由 闫斌 陶超 叶润 周小佳 于 2020-11-19 设计创作，主要内容包括：本发明公开了一种基于速度预测的无人机鲁棒性导航方法,涉及无人机导航领域,该方法根据速度来确定自身的位置,主要是根据回声神经网络来预测导航中所需要的速度,通过无人机的自身参数去预测速度。相比于惯性导航预测速度而言,本发明所设计的速度预测器不会因时间的累积而使速度误差增大并且精度也比较高。这是因为,无人机的速度是由最近的输入序列决定的,速度估计误差会被逐渐遗忘。回声状态网路的遗忘特性正适合于学习这种特性,这也是本发明速度估计器能有效工作的物理基础。(The invention discloses an unmanned aerial vehicle robustness navigation method based on speed prediction, and relates to the field of unmanned aerial vehicle navigation. Compared with the speed predicted by inertial navigation, the speed predictor designed by the invention does not increase speed error due to time accumulation and has higher precision. This is because the speed of the drone is determined by the most recent input sequence and speed estimation errors are gradually forgotten. The forgetting feature of the echo-state network is well suited to learning this feature, which is also the physical basis on which the velocity estimator of the present invention can work effectively.)

1. An unmanned aerial vehicle robust navigation method based on speed prediction, wherein the unmanned aerial vehicle is a quad-rotor unmanned aerial vehicle, and the method comprises the following steps:

s1), the body of the unmanned aerial vehicle is regarded as a rigid body, and a rigid body kinematic model is shown as the formula (1):

wherein v and omega are respectively the velocity and angular velocity in the b system, the b system represents a carrier coordinate system, F represents resultant force, J represents an inertia matrix, M represents resultant moment, M represents the mass of the object,which is indicative of the rate of change of the speed,the change rate of the angular speed is represented, and the speed of the unmanned aerial vehicle can be obtained by the resultant force borne by the system, the system quality and the stable measurement of the angular speed according to the formula (1);

s2) from equation (1), the rate of change of the velocity is related to the angular velocity and the resultant force and the mass, the value of the angular velocity can be measured by a gyroscope, the mass can be measured in advance, and the resultant force is mainly calculated by the following equation (2):

where g is the acceleration of gravity, a known quantity,a coordinate transformation matrix from a b system to an n system is represented, wherein the b system represents a carrier coordinate system, and the n system represents a navigation coordinate system, as shown in formulas (3) and (4); f_zExpressing the pneumatic main power as shown in formula (5);

wherein ω is_x、ω_y、ω_zThe projection components of angular velocity in b system, and quaternion q ═ q₀,q₁,q₂,q₃]^TQ in (1)₀,q₁,q₂,q₃Representing four numbers defined in advance by quaternions, wherein the basic basis of description is that for any two Cartesian coordinate systems with coincident origin points in space, an axis passing through the origin point always exists, so that one coordinate system is coincident with the other coordinate system after rotating around the axis for a certain angle; as shown in formula (3), the rotation matrix represented by quaternion and the rotation matrix represented by euler angle can be mutually converted; equation (4) describes an attitude differential equation expressed by a quaternion, the elements of which are not singular because the pitch angle is equal to 90 °;

F_Z＝(w_r1 ²+w_r2 ²+w_r3 ²+w_r4 ²) (5)

wherein w_ri(i ═ 1,2,3,4) represents four motor rotation angular rates of quad-rotor unmanned aerial vehicle, the four motors are identical, lift coefficient K_f＝ρSr²C_TstatIn which C is_TstatExpressing a dimensionless lift coefficient, S expressing a rotor area, r expressing a rotor radius, and rho expressing an air density;

s3) the resultant force F can be obtained from the coefficients of the drone itself, and the total velocity in the whole rigid body model is related to the velocity, the resultant force and the angular velocity, so that the velocity can be predicted by using the system parameters known to the drone and the angular velocity measured by the gyroscope, and the velocity can be predicted by using the echo state network, as follows:

s31) according to the dimensionless lift coefficient C_TstatCalculating lift coefficient K according to rotor wing area S, rotor wing radius r and air density rho_fThen, the pneumatic main power is obtained according to the rotation rate of each motor in the four motors, so that the required resultant force F is obtained;

s32) constructing a neural network, wherein W_inRepresenting transfer of external input to storageAmong the pools, W denotes the passing of internal states, W_outRepresents a control output; the storage pool stores internal state vectors x of the echo state network, and the relation between the internal state vectors x is determined by a state transition matrix W; the number of input states is 5 in total, and is respectively omega_x、ω_y、ω_zF and m, the number of output states amounting to 3, respectively v_x、v_yAnd v_z(ii) a The mathematical form of the echo state network is shown in equations (6) and (7),

x(n+1)＝αx(n)+βf(W_inu(n+1)+Wx(n)) (6)

where N denotes the time N, u denotes the input vector of dimension K, x denotes the internal state of dimension N, y denotes the output vector of dimension L, W_inAssociating an input with an internal state, W representing the passing of the internal state, W_outRepresenting a control output matrix, wherein alpha and beta represent undetermined parameters;

s33) parameters α and β are to be determined in equation (6), and it is understood from equation (8) that parameter β can be determined as long as parameter α is determined; selecting as a random matrix W for the input matrix_inA value between-1 and 1; for the state transition matrix W, firstly, a random matrix is selected, the singular value of the random matrix is calculated, the matrix is divided by the singular value to obtain the state transition matrix W, and for the output matrix W_outTraining will be performed using an echo state training network algorithm as described below;

| α | < 1, and β ═ 1- | α | (8)

Wherein the echo state training network algorithm adopts a recursive least square algorithm suitable for the echo state training network determined by the equations (6) and (7), and comprises the following steps:

s331) randomly generating a matrix W with values in the range of-1 to 1_inW and W_outCalculating the singular value σ of W_WAnd dividing W by σ_WObtaining W;

s332) obtaining from the training sample libraryTaking a sample, calculating the expected output y according to equation (9)_d(n) an error vector δ with the current net output y (n);

s333) selecting a proper forgetting factor mu, and recursively estimating an output matrix W according to the formula (10)_out；

S334) repeating the steps S332) and S333) until the training sample library is completely traversed once;

defining an error vector delta

δ＝y_d(n)-y(n) (9)

The recursive least square algorithm is as follows, the weight value corrected at the current time n is used at the time n +1, and the output matrix corrected at the current time is recorded as W_out(n+1)。

Wherein μ represents a forgetting factor, and 0.95<μ≤1，P_RLSThe initial value of (a) may be selected as a diagonal matrix with sufficiently large elements, L_RLS(n) represents an intermediate variable.

2. The unmanned aerial vehicle robust navigation method based on speed prediction as claimed in claim 1, wherein the lift coefficient K is_f＝6.20566×10^-6The undetermined parameter α is 0.996, and the number of internal states is N is 50.

Technical Field

The invention relates to the field of unmanned aerial vehicle navigation, in particular to an unmanned aerial vehicle robust navigation method based on speed prediction.

Background

The unmanned aerial vehicle usually uses a GPS and inertial navigation combined navigation mode to navigate, but when the aircraft flies in urban building groups or remote mountainous areas, GPS signals are weak, and navigation can be performed by using inertial navigation. Inertial navigation generally consists of a gyroscope and an accelerometer, and has accumulated errors when position information is obtained, so that an accurate position cannot be obtained. The method is a new idea, system parameters of the unmanned aerial vehicle cannot change along with changes of external factors of flying, errors caused by sensor measurement cannot exist, and stability and accuracy performance are guaranteed. Can obtain wide application in the unmanned aerial vehicle navigation field.

In unmanned aerial vehicle navigation, inertial navigation is a navigation mode which is not affected by the strength of an external signal, so that the application of the inertial navigation is wide, but speed information in the inertial navigation is obtained through acceleration integration. The acceleration data is mainly measured by the accelerometer sensor. Two problems arise in this way, first, the error of the measurement is amplified by integration; second, the process of integration itself introduces errors.

Disclosure of Invention

The invention aims to provide a new navigation idea, speed is an important data required by navigation, and the invention provides the navigation by measuring the parameters of the system to predict speed information. The problem of error caused by integral in measurement by an accelerometer is solved, and the precision is improved.

In order to solve the two problems of measurement error integral amplification and algorithm error in the integral process, the invention provides the method which utilizes the system parameters of the unmanned aerial vehicle, so that the two problems can not occur in the system parameter measurement process, and the influence of the external environment can not be caused. The unmanned aerial vehicle body is regarded as a rigid body, a rigid body kinematic model is shown in a formula (1), and the unmanned aerial vehicle is a quadrotor unmanned aerial vehicle:

wherein v and ω are the velocity and angular velocity in the b system, respectively, F represents the resultant force, J represents the inertia matrix,m represents the resultant moment, M represents the mass of the object,which is indicative of the rate of change of the speed,indicating the rate of change of angular velocity. As can be seen from the above formula, the speed of the unmanned aerial vehicle can be obtained by the resultant force borne by the system, the system quality and the stable measurement angular speed, and the accuracy is higher.

As can be seen from equation (1), the rate of change of the velocity is related to the angular velocity, the resultant force applied, and the mass, the value of the angular velocity can be measured by the gyroscope, the mass can be measured in advance, and the resultant force is mainly calculated by the following equation (2):

where g is the acceleration of gravity, a known quantity,a coordinate transformation matrix from a b system to an n system is represented, wherein the b system represents a carrier coordinate system, and the n system represents a navigation coordinate system, as shown in formulas (3) and (4); f_zExpressing the pneumatic main power as shown in formula (5);

wherein ω is_x、ω_y、ω_zThe projection components of angular velocity in b system, and quaternion q ═ q₀,q₁,q₂,q₃]^TQ in (1)₀,q₁,q₂,q₃Representing a predefined four-figure of quaternions, the basic basis for the description isIn any two Cartesian coordinate systems with coincident original points in space, an axis passing through the original points always exists, so that one coordinate system is coincident with the other coordinate system after rotating around the axis for a certain angle; as shown in formula (3), the rotation matrix represented by quaternion and the rotation matrix represented by euler angle can be mutually converted; equation (4) describes an attitude differential equation expressed by a quaternion, the elements of which are not singular because the pitch angle is equal to 90 °;

Fz＝(w_r1 ²+w_r2 ²+w_r3 ²+w_r4 ²) (5)

wherein w_ri(i ═ 1,2,3,4) represents four motor rotation angular rates of quad-rotor unmanned aerial vehicle, the four motors are identical, lift coefficient K_f＝ρSr²C_Tstat. Wherein C is_TstatDenotes a dimensionless lift coefficient, S denotes a rotor area, r denotes a rotor radius, and ρ denotes an air density.

As can be seen from equations (2), (3), (4) and (5), the resultant force F can be obtained from the coefficients of the drone itself, and as can be seen from equation (1), the rate of change of velocity is related to velocity, resultant force and angular velocity in the entire rigid body model, so that the velocity can be predicted using the known system parameters of the drone and the angular velocity measured by the gyroscope, and the velocity can be predicted using the echo state network.

Compared with the speed predicted by inertial navigation, the speed predictor designed by the invention does not increase speed error due to time accumulation and has higher precision. This is because the speed of the drone is determined by the most recent input sequence and speed estimation errors are gradually forgotten. The forgetting feature of the echo-state network is well suited to learning this feature, which is also the physical basis on which the velocity estimator of the present invention can work effectively.

Drawings

FIG. 1 is a schematic diagram of the definition of coordinate systems of n-system and b-system according to the present invention

FIG. 2 is a diagram of an echo state network structure according to the present invention

FIG. 3 is a diagram of a speed predictor according to the present invention

Detailed Description

The following provides a more detailed description of the embodiments and the operation of the present invention with reference to the accompanying drawings.

The main idea of unmanned aerial vehicle navigation prediction is to determine the position of the unmanned aerial vehicle according to the speed, mainly predict the speed required in navigation according to an echo neural network, and predict the speed according to the parameters of the unmanned aerial vehicle. The structure is shown in fig. 2. The reason for choosing an echo state network is that the network has a high prediction accuracy and it has one of the most important characteristics: according to the echo characteristic, when the selection of the network parameters meets a certain constraint condition, the internal state of the echo state network is gradually attenuated, and then the echo state network is globally consistent and gradually stable from the viewpoint of the stability of the system. And online learning can be performed when different learning algorithms are selected. The invention selects a recursive least square algorithm.

In order to describe the kinematic rules and dynamics of the quad-rotor unmanned aerial vehicle in the near-ground space, a coordinate system suitable for the characteristics of the quad-rotor unmanned aerial vehicle needs to be established. The following four coordinate systems are often involved in quad-rotor drone research: an inertial system (system i), a navigation coordinate system (system n, northeast), a carrier coordinate system (system b), and a WGS84 coordinate system; the inertial system (i system) defines an inertial coordinate system to perform stress analysis on the quad-rotor unmanned aerial vehicle, the influence of the inertial force does not need to be considered in the stress analysis of an analysis object in the inertial system, and the influence of the inertial force needs to be considered in the stress analysis in the non-inertial system. And in a local navigation coordinate system (n system, northeast), the origin is located at the unmanned aerial vehicle flying point or a reference point set by people. In the carrier coordinate system (b system), the heading angle is 0 ° when the carrier x-axis points to true north, as shown in fig. 1, fig. 1(a) is an onNED navigation coordinate system, i.e. n system; fig. 1(b) is an oxyz carrier coordinate system, that is, a b system, the advancing direction of the quad-rotor unmanned aerial vehicle is the direction indicated by a large arrow, and the motor steering is the direction of a curved arrow.

In order to solve the two problems of measurement error integral amplification and algorithm error in the integral process, the invention provides the method which utilizes the system parameters of the unmanned aerial vehicle, so that the two problems can not occur in the system parameter measurement process, and the influence of the external environment can not be caused. The unmanned aerial vehicle body is regarded as a rigid body, a rigid body kinematic model is shown in a formula (1), and the unmanned aerial vehicle is a quadrotor unmanned aerial vehicle:

wherein v and omega are respectively the velocity and angular velocity in the system b, F represents resultant force, J represents inertia matrix, M represents resultant moment, M represents the mass of the object,which is indicative of the rate of change of the speed,indicating the rate of change of angular velocity. As can be seen from the above formula, the speed of the unmanned aerial vehicle can be obtained by the resultant force borne by the system, the system quality and the stable measurement angular speed, and the accuracy is higher.

From equation (1), the rate of change of velocity is related to angular velocity and the resultant force and mass. The value of the angular velocity may be measured by a gyroscope and the mass may be measured in advance. The resultant force is mainly calculated by the following formula (2):

where g is the acceleration of gravity, a known quantity,and (3) a coordinate transformation matrix from a b system to an n system is represented, wherein the b system represents a carrier coordinate system, and the n system represents a navigation coordinate system, as shown in the formulas (3) and (4). F_zRepresents the pneumatic main force as shown in equation (5).

Wherein ω is_x、ω_y、ω_zThe projection components of the angular velocity in the b-system are respectively. Quaternion q ═ q₀,q₁,q₂,q₃]^TQ in (1)₀,q₁,q₂,q₃Representing a predefined four-number quaternion. The basic basis for this description is that for any cartesian coordinate system where two origins coincide in space, there is always an axis through the origins such that one coordinate system coincides with the other coordinate system after rotating around the axis by a certain angle. As shown in formula (3), the rotation matrix represented by quaternion and the rotation matrix represented by euler angle can be mutually converted. Equation (4) describes an attitude differential equation expressed by a quaternion, the elements of which are not singular because the pitch angle is equal to 90 °.

Fz＝(w_r1 ²+w_r2 ²+w_r3 ²+w_r4 ²) (5)

Wherein w_ri(i ═ 1,2,3,4) represents four motor rotation angular rates of quad-rotor unmanned aerial vehicle, the four motors are identical, lift coefficient K_f＝ρSr²C_Tstat. Wherein C is_TstatDenotes a dimensionless lift coefficient, S denotes a rotor area, r denotes a rotor radius, and ρ denotes an air density.

As can be seen from the equations (2), (3), (4) and (5), the resultant force F can be obtained from the coefficients of the drone itself, and as can be seen from the equation (1), the rate of change of the velocity in the entire rigid body model is related to the velocity, the resultant force and the angular velocity, so that the velocity can be predicted by using the known system parameters of the drone and the angular velocity measured by the gyroscope, the invention uses an echo state network to predict the velocity, and the entire structure is as shown in fig. 3, and the steps are as follows:

the method comprises the following steps: according to dimensionless lift coefficient C_TstatThe lift coefficient K can be calculated according to the rotor wing area S, the rotor wing radius r and the air density rho_fAnd then the pneumatic main power can be obtained according to the rotation rate of each motor in the four motors, so that the required power can be obtainedThe resultant force F of.

Step two: as shown in FIG. 2, a neural network is constructed in which W_inIndicating the transfer of external input into the storage pool, W indicating the transfer of internal state, W_outRepresenting the control output. The storage pool stores internal state vectors x of the echo state network, and the relationship between the internal state vectors x is determined by a state transition matrix W. The resultant force F and the gyroscope are used for measuring the angular velocity omega of the body_x、ω_y、ω_zFor input, the velocity v in the carrier coordinate system_x、v_y、v_zIs the desired output. The number of states input amounts to 5 (ω)_x、ω_y、ω_zF, m), the number of states output amounting to 3 (v)_x、v_y、v_z). The mathematical form of the echo state network is shown in equations (6) and (7),

x(n+1)＝αx(n)+βf(W_inu(n+1)+Wx(n)) (6)

where N denotes the time N, u denotes the input vector of dimension K, x denotes the internal state of dimension N, y denotes the output vector of dimension L, W_inAssociating an input with an internal state, W representing the passing of the internal state, W_backRepresenting the output vector feedback matrix, W_outRepresenting the control output matrix. Both α and β represent pending parameters.

Step three: in the formula (6), the parameters α and β are to be determined, and it is understood from the formula (8) that the parameter β can be determined as long as the parameter α is determined. Selecting as a random matrix W for the input matrix_inThe value is between-1 and 1. For the state transition matrix W, a random matrix is first selected, the singular value of the random matrix is calculated, and the matrix is divided by the singular value to obtain the state transition matrix W. For the output matrix W_outThe training will be performed using an echo state training network algorithm as described below.

| α | < 1, and β ═ 1- | α | (8)

The echo state training network algorithm: the learning algorithm of the echo state training network mainly comprises a generalized inverse method and a recursion least square algorithm at present. The generalized inverse method is only suitable for batch processing of data and is not suitable for online operation. Therefore, the present invention derives a recursive least squares algorithm suitable for the echo state training network determined by equations (6) and (7).

Step 1 random generation of a matrix W with values in the range-1 to 1_inW and W_outCalculating the singular value σ of W_WAnd dividing W by σ_WW is obtained.

Step 2, obtaining a sample from a training sample library, and calculating expected output y according to formula (9)_d(n) error vector δ with current net output y (n).

Step 3, selecting a proper forgetting factor mu, and estimating an output matrix W in a recursion mode according to the formula (10)_out。

And 4, repeating the steps 2 and 3 until the training sample library is completely traversed once.

Defining an error vector delta

δ＝y_d(n)-y(n) (9)

The recursive least square algorithm is as follows, the weight value corrected at the current time n is used at the time n +1, and the output matrix corrected at the current time is recorded as W_out(n+1)。

Wherein μ represents a forgetting factor, and 0.95<μ≤1，P_RLSThe initial value of (a) may be chosen as a diagonal matrix with sufficiently large elements. L is_RLS(n) represents an intermediate variable.

In addition, the invention establishes a simulation model for simulation, and the specific parameter is K_f＝6.20566×10^-6α is 0.996, the number of internal states is 50, and the angular velocity is measured by a gyroscope. As shown in table 1, which describes the effect of sensor noise on the speed estimate, the speed predictor works well under sensor noise conditions. The standard noise in table 1 is a value obtained when α is 0.996, which is obtained by successively taking values of αThe optimal values, the latter terms, are values when the sensor noise of the term is set to 0 and the other sensor noise is set to standard noise, so that the influence of the sensor noise on training and prediction can be reflected. The impact value may be calculated from the noise value minus the standard noise divided by the standard noise. As can be seen from table 1, the acceleration noise and the velocity noise have a value close to the standard noise when the predicted RMSE (root mean square error) is 0, and have an influence of 0.1% with respect to the standard noise value, and the influence of the velocity and the acceleration noise on the velocity predictor can be practically ignored. The angular velocity is the input to the velocity predictor, and as can be seen from table 1, the difference between the angular velocity noise and the standard noise prediction RMSE is 0.002383. The magnitude of the effect on the predicted RMSE was 4.8%, but there was only a slight deterioration in the absence of angular velocity noise. From table 1, it can be seen that the influence of the position noise on the predicted RMSE is 30.6%, because the position noise greatly changes the drone motion control, so it can be inferred from table 1 that under different motion trajectories, the estimation performance of the velocity estimator will have a large difference.

Table 1 effect of noise on training RMSE and prediction RMSE (N50, α 0.996)

In order to ensure that the speed prediction has good generalization capability, hovering + spiral + straight line is selected as a training track. Table 2 predicts RMSE (root mean square error) and MRE (mean relative error) values for the conditioned speed for different trajectories. As can be seen from table 2, under standard noise conditions, a lateral comparison is first made: v. of_x、v_yBoth the root mean square error and the average relative error of the_zBecause the velocity measurement in the z-axis direction is affected by gravity, but the difference is not large, the root mean square error and the average relative error of the velocities in the three directions are small. When the vertical comparison shows that the flight of the spiral, the root mean square error and the average relative error are larger than those of the other two flight conditions. Because the flight condition of the spiral is complex, the measurement of the speed in the z-axis direction is carried out. From the table, it can be seen that the x, y-axis velocities are more than 94% accurate at hover and straight line.Under all working conditions of hovering, spiral and straight lines, the speed prediction precision is more than 91.7%, and the predicted RMSE value is less than 0.04m/s, which shows that the prediction result precision is high even under the complex flight condition.

TABLE 2 speed prediction Performance (. eta.) under different trajectories and different noise conditions_MRE＝0.05)

Note: eta_MRE0.05 indicates that velocities less than 0.05m/s do not fit within the statistical range of the average relative error.

From the above table it can be concluded that: compared with the speed predicted by inertial navigation, the speed predictor designed by the invention does not increase speed error due to time accumulation and has higher precision. This is because the speed of the drone is determined by the most recent input sequence and speed estimation errors are gradually forgotten. The forgetting feature of the echo-state network is well suited to learning this feature, which is also the physical basis on which the velocity estimator of the present invention can work effectively.

While the invention has been described with reference to specific embodiments, any feature disclosed in this specification may be replaced by alternative features serving the same, equivalent or similar purpose, unless expressly stated otherwise; all of the disclosed features, or all of the method or process steps, may be combined in any combination, except mutually exclusive features and/or steps; any non-essential addition and replacement made by the technical characteristics of the technical scheme of the invention by a person skilled in the art belong to the protection scope of the invention.