阅读说明：本技术 一种无人机相对导航信息融合方法 (Unmanned aerial vehicle relative navigation information fusion method ) 是由 孙瑶洁 熊智 李文龙 王融 张玲 刘建业 崔雨晨 段胜青 陈明星 许建新 王钲淳 于 2019-10-25 设计创作，主要内容包括：本发明公开了一种无人机相对导航信息融合方法,步骤为：计算无人机相对不同卫星的双差和双差变化率；计算UWB传感器实时测量得到的无人机之间的相对位置和相对速度；计算基于双机定位数据做差的相对位置和相对速度；采用扩展卡尔曼滤波算法建立相对导航状态方程；建立基于相对差分/UWB/双机定位数据做差的观测方程；利用神经网络对UWB相对导航信息进行预测补偿,并通过扩展卡尔曼滤波算法实现信息融合。本发明可显著提高编队无人机中相对定位精度,且适用于相对导航传感器UWB失效等复杂飞行环境,适合工程实际应用。(The invention discloses a method for fusing relative navigation information of an unmanned aerial vehicle, which comprises the following steps: calculating double differences and double difference change rates of the unmanned aerial vehicle relative to different satellites; calculating the relative position and relative speed between the unmanned aerial vehicles measured by the UWB sensor in real time; calculating the relative position and relative speed based on the difference of the double-machine positioning data; establishing a relative navigation state equation by adopting an extended Kalman filtering algorithm; establishing an observation equation based on the relative difference/UWB/dual-computer positioning data difference; and (4) carrying out prediction compensation on UWB relative navigation information by utilizing a neural network, and realizing information fusion by an extended Kalman filtering algorithm. The invention can obviously improve the relative positioning precision in the formation unmanned aerial vehicle, is suitable for complex flying environments such as UWB failure of relative navigation sensors and the like, and is suitable for practical engineering application.)

1. An unmanned aerial vehicle relative navigation information fusion method is characterized in that: each unmanned aerial vehicle is provided with a GPS/INS tightly combined navigation system for acquiring single-machine absolute positioning information and a UWB sensor for acquiring relative positioning information; the method comprises the following steps:

(1) establishing a relative difference measurement equation, and calculating the double differences of the unmanned aerial vehicle relative to different satellites

(2) Establishing a relative navigation state equation by adopting an extended Kalman filtering algorithm;

(3) establishing an observation equation based on the relative difference/UWB/dual-computer positioning data difference;

(4) and (4) carrying out prediction compensation on UWB relative navigation information by utilizing a neural network, and realizing information fusion by an extended Kalman filtering algorithm.

2. The unmanned aerial vehicle relative navigation information fusion method according to claim 1, characterized in that: in step (1), double differences of unmanned aerial vehicle with respect to different satellites

Wherein the content of the first and second substances,

3. The unmanned aerial vehicle relative navigation information fusion method according to claim 1, characterized in that: in the step (1), the UWB sensor measures the relative position between the unmanned aerial vehicles in real time

Wherein r is_x、r_y、r_zFor each of the three-axis components of the relative position vector,

4. The unmanned aerial vehicle relative navigation information fusion method according to claim 1, characterized in that: in step (1), making a poor relative position based on the dual-computer positioning dataAnd relative velocity

Wherein, δ r_BAIs the compensation of the time-varying relative positioning error caused by the positioning error of a single machine, n_Δabs、n_{v_Δabs}White noise is calculated for the corresponding position and velocity.

5. The unmanned aerial vehicle relative navigation information fusion method according to claim 1, characterized in that: in step (2), a state vector X is defined:

wherein r is_BAIs a vector of the relative position of the two,as a vector of the relative velocity, the velocity vector,

constructing a relative navigation state equation:

wherein, w_p、w_v、w_aAnd w_δpSystematic noise, 0, respectively, of relative position, velocity, acceleration and time-varying position error_3×3Is a 3 × 3 zero matrix, I_3×3Is a 3 × 3 unit array.

6. The unmanned aerial vehicle relative navigation information fusion method according to claim 1, characterized in that: in step (3), an observation equation is constructed:

Z＝H·X+V

wherein Z is an observation vector, H is an observation matrix, X is a state vector, and V is an observation noise vector;

constructing an observation vector and an observation noise vector:

wherein the ellipses represent the results of traversing all satellitesAnd

in the above formula, H₁And H₂Respectively representing an observation value and an estimation value by a symbol A & ltlambda & gt above the observation matrix for UWB distance measurement and speed measurement;

constructing an observation matrix:

wherein, relative difference observation submatrix H_dd：

Wherein the content of the first and second substances,

double-machine positioning data difference observation sub-matrix H_abs：

Wherein, I₃Is a third order unit array;

UWB measurement submatrix H_uwb：

Wherein, 0_1×9A zero matrix of 1 × 9;

r_{BA_uwb}＝||r_BA||₂(1+bias_BA)

in the above formula, r_x、r_y、r_zFor each of the three-axis components of the relative position vector,

and after an observation equation is constructed, solving the state variable by adopting an extended Kalman filtering algorithm in combination with the state equation.

7. The unmanned aerial vehicle relative navigation information fusion method according to claim 1, characterized in that: in the step (4), the unmanned aerial vehicle relative navigation information fusion method based on the neural network is divided into two scenes:

firstly, in the effective stage of UWB signals, on one hand, a neural network is trained to obtain UWB output through absolute position information of two unmanned aerial vehicles, and on the other hand, an extended Kalman filter fuses position information, speed information and UWB output of two unmanned aerial vehicles to obtain correction quantity of an absolute positioning error of a wing plane;

and secondly, in the UWB signal failure stage, the absolute navigation information correction of a wing plane cannot be obtained through an extended Kalman filter, at this time, a neural network prediction mode is adopted, the UWB output value is predicted through the absolute position information of the unmanned aerial vehicle, then the compensation value of the relative navigation position information and the speed information is obtained through the extended Kalman filter, and the absolute navigation precision of the wing plane is corrected, so that the filtering is not influenced by the UWB signal failure.

8. The unmanned aerial vehicle relative navigation information fusion method according to claim 1, characterized in that: in step (4), the neural network adopts an RBF neural network.

Technical Field

The invention belongs to the technical field of unmanned aerial vehicle navigation, and particularly relates to a method for fusing relative navigation information of an unmanned aerial vehicle.

Background

The unmanned aerial vehicle dense cluster formation navigation technology has great application value and application prospect in various fields such as military, civil and the like. In order to ensure the high precision requirement of the task executed when the unmanned aerial vehicle formation flies, the position of the relative navigation information precision is undoubted. Generally, the relative distance between the formation unmanned aerial vehicles is short, relative navigation is an indispensable means for realizing multi-machine formation navigation, and a relative navigation sensor has higher positioning accuracy, stronger anti-interference capability and lower cost compared with an absolute navigation sensor.

Relative navigation modes include radio navigation, visual navigation and others, wherein radio navigation is the most core means and includes satellite navigation, laser radar, Ultra Wide Band (UWB) and the like. Compared with other positioning technologies, the UWB has the advantages of excellent performance, high precision, low power consumption, good multipath resistance effect, capability of providing high-precision positioning and the like, and is widely applied and researched. The high-frequency short-wave characteristics of UWB enable the emittance to be strong and the penetrability to be weak, and the UWB ranging device is particularly suitable for ranging. But obtaining high accuracy UWB positioning information requires the absence of occlusion and this principle drawback limits the wider use of UWB. In a complex application environment, non-line-of-sight and multipath errors are also main influence factors of positioning accuracy. Therefore, when the UWB sensor fails, the information fusion processing error of the relative navigation becomes large, and it is not possible to provide accurate position and speed compensation values, so that the accuracy and real-time performance of the formation relative to the navigation system, which only depends on the UWB auxiliary unmanned aerial vehicle, becomes weak.

Disclosure of Invention

In order to solve the technical problems mentioned in the background art, the invention provides a relative navigation information fusion method for an unmanned aerial vehicle.

In order to achieve the technical purpose, the technical scheme of the invention is as follows:

a relative navigation information fusion method for unmanned aerial vehicles is characterized in that each unmanned aerial vehicle is provided with a GPS/INS tightly-combined navigation system for acquiring single-machine absolute positioning information and a UWB sensor for acquiring relative positioning information; the method comprises the following steps:

(1) establishing a relative difference measurement equation, and calculating the double differences of the unmanned aerial vehicle relative to different satellites

Sum and double difference rate of change

Establishing a UWB (ultra Wide band) measurement equation, and calculating the relative position between the unmanned aerial vehicles measured by the UWB sensor in real time

And relative velocity

Calculating relative position based on difference of double-machine positioning data

And relative velocity

(2) Establishing a relative navigation state equation by adopting an extended Kalman filtering algorithm;

(3) establishing an observation equation based on the relative difference/UWB/dual-computer positioning data difference;

(4) and (4) carrying out prediction compensation on UWB relative navigation information by utilizing a neural network, and realizing information fusion by an extended Kalman filtering algorithm.

Further, in step (1), double differences of the unmanned aerial vehicle with respect to different satellites

Sum and double difference rate of change

Wherein the content of the first and second substances,

satellite receiver for unmanned aerial vehicle A and unmanned aerial vehicle B respectively and satellite S_iThe inter-station single difference between the two,

satellite receiver for unmanned aerial vehicle A and unmanned aerial vehicle B respectively and satellite S_jThe inter-station single difference between the two,two unmanned planes are connected with a midpoint to a satellite S_iThe direction of the cosine vector of (a),

two unmanned planes are connected with a midpoint to a satellite S_jDirection cosine vector of r_BAIs a relative position vector between two drones,

noise existing in pseudo-range double difference calculation is obtained;

is the relative velocity vector between the drones,andrespectively two unmanned planes from the midpoint to the satellite S_iAnd S_jThe rate of change of the direction cosine vector of (c),to calculate the noise present in the double difference rate of change.

Further, in the step (1), the UWB sensor measures the relative position between the drones in real time

And relative velocity

Wherein r is_x、r_y、r_zFor each of the three-axis components of the relative position vector,

respectively, the three-axis component, bias, of the relative velocity vector_BAIs the error proportional coefficient caused by crystal oscillator, | | r_BA||₂Is a relative position vector r_BAL of₂Norm, n_uwbAnd n_{v_uwb}White noise for UWB position measurements and velocity measurements, respectively.

Further, in step (1), making a poor relative position based on the dual-computer positioning dataAnd relative velocity

Wherein, δ r_BAIs the compensation of the time-varying relative positioning error caused by the positioning error of a single machine, n_Δabs、n_{v_Δabs}White noise is calculated for the corresponding position and velocity.

Further, in step (2), a state vector X is defined:

wherein r is_BAIs a vector of the relative position of the two,as a vector of the relative velocity, the velocity vector,

as a relative acceleration vector, δ r_BACompensation quantity of relative positioning error caused by single machine positioning error;

constructing a relative navigation state equation:

wherein, w_p、w_v、w_aAnd w_δpSystematic noise, 0, respectively, of relative position, velocity, acceleration and time-varying position error_3×3Is a 3 × 3 zero matrix, I_3×3Is a 3 × 3 unit array.

Further, in step (3), an observation equation is constructed:

Z＝H·X+V

wherein Z is an observation vector, H is an observation matrix, X is a state vector, and V is an observation noise vector;

constructing an observation vector and an observation noise vector:

wherein the ellipses represent the results of traversing all satellites

Andin order to observe the noise for the double difference,for double-difference rate of change observation noise, V_ΔabsMaking a difference observation noise for the positions of the two machines,

making differential observation noise, V, for dual machine speeds_uwbIn order for the UWB ranging noise to be present,

for UWB speed measurement noise, superscript T represents transposition;

in the above formula, H₁And H₂Respectively representing an observation value and an estimation value by a symbol A & ltlambda & gt above the observation matrix for UWB distance measurement and speed measurement;

constructing an observation matrix:

wherein, relative difference observation submatrix H_dd：

Wherein the content of the first and second substances,

two unmanned planes are connected with a midpoint to a satellite S_iThe direction of the cosine vector of (a),

two unmanned planes are connected with a midpoint to a satellite S_jThe direction of the cosine vector of (a),and

respectively two unmanned planes from the midpoint to the satellite S_iAnd S_jDirection cosine vector change rate of 0_1×3And 0_1×6Zero matrices of 1 × 3 and 1 × 6, respectively;

double-machine positioning data difference observation sub-matrix H_abs：

Wherein, I₃Is a third order unit array;

UWB measurement submatrix H_uwb：

Wherein, 0_1×9A zero matrix of 1 × 9;

r_{BA_uwb}＝||r_BA||₂(1+bias_BA)

in the above formula, r_x、r_y、r_zFor each of the three-axis components of the relative position vector,

respectively, the three-axis component, bias, of the relative velocity vector_BAIs the error proportional coefficient caused by crystal oscillator, | | r_BA||₂Is a relative position vector r_BAL of₂A norm;

and after an observation equation is constructed, solving the state variable by adopting an extended Kalman filtering algorithm in combination with the state equation.

Further, in the step (4), the unmanned aerial vehicle relative navigation information fusion method based on the neural network is divided into two scenes:

firstly, in the effective stage of UWB signals, on one hand, a neural network is trained to obtain UWB output through absolute position information of two unmanned aerial vehicles, and on the other hand, an extended Kalman filter fuses position information, speed information and UWB output of two unmanned aerial vehicles to obtain correction quantity of an absolute positioning error of a wing plane;

and secondly, in the UWB signal failure stage, the absolute navigation information correction of a wing plane cannot be obtained through an extended Kalman filter, at this time, a neural network prediction mode is adopted, the UWB output value is predicted through the absolute position information of the unmanned aerial vehicle, then the compensation value of the relative navigation position information and the speed information is obtained through the extended Kalman filter, and the absolute navigation precision of the wing plane is corrected, so that the filtering is not influenced by the UWB signal failure.

Further, in step (4), the neural network adopts an RBF neural network.

Adopt the beneficial effect that above-mentioned technical scheme brought:

the invention carries out prediction compensation on the relative navigation information of the UWB by utilizing the neural network, realizes information fusion by the extended Kalman filtering on the basis, greatly improves the positioning precision and the real-time property of the relative navigation of the unmanned aerial vehicle under the fault condition, and has outstanding application value. The experimental results show that: compared with an unmanned aerial vehicle relative navigation system without assistance, the unmanned aerial vehicle relative navigation system can enable the relative navigation precision in the directions of speed x, y and z to be improved by 8.2, 24.5 and 8.2 times in 100s on average; the positioning accuracy in the x, y and z directions of the position is improved by 4.3, 2.8 and 2.7 times in an average way within 50 s.

Drawings

FIG. 1 is a diagram of a relative navigation information fusion structure based on neural network according to the present invention;

FIG. 2 is a graph of a dual-machine simulation track;

FIG. 3 is a comparison of X-direction velocity error curves with and without assistance;

fig. 4-6 are comparative plots of X, Y, Z directional position error curves using assisted and unassisted conditions.

Detailed Description

The technical scheme of the invention is explained in detail in the following with the accompanying drawings.

In the invention, two unmanned aerial vehicles form a master-slave structure, and a host and a wing plane respectively carry a GPS/INS tightly combined navigation system based on pseudo range and pseudo range rate to obtain high-precision single-machine absolute positioning information; and a UWB sensor is carried to measure the relative distance and relative speed information so as to obtain the relative positioning information with high reliability. In addition, the data link transmits the satellite navigation receiver data and the airborne inertial navigation measurement data to the airborne computer of each unmanned aerial vehicle, so that fusion processing of relative navigation information is realized.

The invention designs a method for fusing relative navigation information of an unmanned aerial vehicle, which comprises the following steps:

step 1: establishing a relative difference measurement equation, and calculating the double differences of the unmanned aerial vehicle relative to different satellites

Sum and double difference rate of change

Establishing a UWB (ultra Wide band) measurement equation, and calculating the relative position between the unmanned aerial vehicles measured by the UWB sensor in real time

And relative velocity

Calculating relative position based on difference of double-machine positioning data

And relative velocity

Step 2: establishing a relative navigation state equation by adopting an extended Kalman filtering algorithm;

and step 3: establishing an observation equation based on the relative difference/UWB/dual-computer positioning data difference;

and 4, step 4: and (4) carrying out prediction compensation on UWB relative navigation information by utilizing a neural network, and realizing information fusion by an extended Kalman filtering algorithm.

In this embodiment, the following preferred scheme may be adopted to implement the step 1:

double difference of unmanned aerial vehicle relative to different satellites

Sum and double difference rate of change

Wherein the content of the first and second substances,

satellite receiver for unmanned aerial vehicle A and unmanned aerial vehicle B respectively and satellite S_iThe inter-station single difference between the two,

satellite receiver for unmanned aerial vehicle A and unmanned aerial vehicle B respectively and satellite S_jThe inter-station single difference between the two,

two unmanned planes are connected with a midpoint to a satellite S_iThe direction of the cosine vector of (a),

two unmanned planes are connected with a midpoint to a satellite S_jDirection cosine vector of r_BAIs a relative position vector between two drones,

noise existing in pseudo-range double difference calculation is obtained;

is the relative velocity vector between the drones,and

respectively two unmanned planes from the midpoint to the satellite S_iAnd S_jThe rate of change of the direction cosine vector of (c),

to calculate the noise present in the double difference rate of change.

Relative position between unmanned aerial vehicles measured by UWB sensor in real time

And relative velocity

Wherein r is_x、r_y、r_zFor each of the three-axis components of the relative position vector,respectively, the three-axis component, bias, of the relative velocity vector_BAIs the error proportional coefficient caused by crystal oscillator, | | r_BA||₂Is a relative position vector r_BAL of₂Norm, n_uwbAnd n_{v_uwb}White noise for UWB position measurements and velocity measurements, respectively.

Relative position difference based on double-machine positioning data

And relative velocity

Wherein, δ r_BAIs the compensation of the time-varying relative positioning error caused by the positioning error of a single machine, n_Δabs、n_{v_Δabs}White noise is calculated for the corresponding position and velocity.

In this embodiment, the following preferred scheme may be adopted to implement the step 2:

defining a state vector X:

wherein r is_BAIs a vector of the relative position of the two,

as a vector of the relative velocity, the velocity vector,

as a vector of the relative acceleration,

compensation quantity of relative positioning error caused by single machine positioning error;

constructing a relative navigation state equation:

wherein, w_p、w_v、w_aAnd w_δpSystematic noise, 0, respectively, of relative position, velocity, acceleration and time-varying position error_3×3Is a 3 × 3 zero matrix, I_3×3Is a 3 × 3 unit array.

In this embodiment, the following preferred scheme may be adopted to implement step 3:

constructing an observation equation:

Z＝H·X+V

wherein Z is an observation vector, H is an observation matrix, X is a state vector, and V is an observation noise vector;

constructing an observation vector and an observation noise vector:

wherein the ellipses represent the results of traversing all satellites

And

in order to observe the noise for the double difference,

for double-difference rate of change observation noise, V_ΔabsMaking a difference observation noise for the positions of the two machines,making differential observation noise, V, for dual machine speeds_uwbIn order for the UWB ranging noise to be present,for UWB speed measurement noise, superscript T represents transposition;

in the above formula, H₁And H₂Respectively representing an observation value and an estimation value by a symbol A & ltlambda & gt above the observation matrix for UWB distance measurement and speed measurement;

constructing an observation matrix:

wherein, relative difference observation submatrix H_dd：

Wherein, 0_1×3And 0_1×6Zero matrices of 1 × 3 and 1 × 6, respectively;

double-machine positioning data difference observation sub-matrix H_abs：

Wherein, I₃Is a third order unit array;

UWB measurement submatrix H_uwb：

Wherein, 0_1×9A zero matrix of 1 × 9;

r_{BA_uwb}＝||r_BA||₂(1+bias_BA)

and after an observation equation is constructed, solving the state variable by adopting an extended Kalman filtering algorithm in combination with the state equation.

In this embodiment, the following preferred scheme may be adopted to implement the step 4:

a structural block diagram of a relative navigation information fusion method of an unmanned aerial vehicle based on a neural network is shown in fig. 1, wherein an input layer of the neural network is absolute position information of two unmanned aerial vehicles, a hidden layer is a radial basis function, and an output layer is expected output position and speed information of UWB. When the UWB fails, the neural network predicts the output of the UWB and transmits the output to the EKF filter to enable the information fusion not to be influenced.

The unmanned aerial vehicle relative navigation information fusion method based on the neural network comprises two scenes:

firstly, in the effective stage of UWB signals, on one hand, a neural network is trained to obtain UWB output through absolute position information of two unmanned aerial vehicles, and on the other hand, an extended Kalman filter fuses position information, speed information and UWB output of two unmanned aerial vehicles to obtain correction quantity of an absolute positioning error of a wing plane;

and secondly, in the UWB signal failure stage, the absolute navigation information correction of a wing plane cannot be obtained through an extended Kalman filter, at this time, a neural network prediction mode is adopted, the UWB output value is predicted through the absolute position information of the unmanned aerial vehicle, then the compensation value of the relative navigation position information and the speed information is obtained through the extended Kalman filter, and the absolute navigation precision of the wing plane is corrected, so that the filtering is not influenced by the UWB signal failure.

In the present embodiment, an RBF neural network is employed.

In order to verify the method provided by the invention, unmanned aerial vehicle formation relative navigation simulation is carried out. An unmanned aerial vehicle A is set as a host, an unmanned aerial vehicle B is set as a wing aircraft to follow the host for flying, the unmanned aerial vehicle adopts a GPS/INS compact combined navigation system and carries UWB equipment for relative navigation, the ranging range is set to be 75m, the formation flying time is 3600 seconds, and the number of available navigation satellites is set to be 8. Fig. 2 shows a simulated flight path curve of the drone under the ECEF coordinate system.

Suppose that 100s-200s and 1000 + 1100sUWB faults occur in the double-locomotive executing task, the position error and the speed error are increased, the bureaucratic aircraft cannot compensate the relative information of the speed and the position in real time in the UWB fault stage of the whole formation flight process, only the GPS/INS tight combination is adopted for absolute positioning, and the accuracy of the relative navigation positioning error drifts along with the time. In order to verify the effect of the invention on the improvement of the relative navigation positioning accuracy of a wing plane, the analysis is carried out by comparing the curve of the real value with the curve of the RBF neural network assistance without assistance: 1. no assistance is provided: both the long plane and the wing plane adopt a GPS/INS compact combination to realize absolute navigation, UWB carries out relative navigation between unmanned planes, but UWB has fault error increase when 100s-200s and 1000s-1100 s; 2. RBF neural network assistance: both the permanent aircraft and the bureaucratic aircraft adopt a GPS/INS compact combination to realize absolute navigation, UWB carries out relative navigation among the unmanned aerial vehicles, and the UWB uses an RBF neural network to predict and compensate relative navigation information fusion among the unmanned aerial vehicles during the fault period. The error curve in the X direction at speeds from 100s to 110s is shown in fig. 3, and the error is greatly reduced with neural network assistance, and is very close to the true value. The speed error of the invention is improved by 8.2, 24.5 and 8.2 times in 100 s. The error curves of the positions in all directions are shown in FIGS. 4-6, taking 100s-200s as an example during the fault. Under the condition of neural network assistance, the deviation is small, the precision is high, the drift is slow, the stability is good, the high-precision requirement can be met when the UWB fails for 50s, and the compensation effect is slightly poor when the fault time is long. The positioning precision of the relative navigation error position is improved by 4.3, 2.8 and 2.7 times in 50 s. Therefore, the method provided by the invention realizes the balance of real-time performance and precision, and is suitable for the actual application of the formation unmanned relative navigation algorithm in engineering.