CN114608578A - Weighted uncertainty unmanned aerial vehicle cluster collaborative navigation method - Google Patents

Weighted uncertainty unmanned aerial vehicle cluster collaborative navigation method Download PDF

Info

Publication number
CN114608578A
CN114608578A CN202210214825.8A CN202210214825A CN114608578A CN 114608578 A CN114608578 A CN 114608578A CN 202210214825 A CN202210214825 A CN 202210214825A CN 114608578 A CN114608578 A CN 114608578A
Authority
CN
China
Prior art keywords
unmanned aerial
aerial vehicle
information
particle
coordinate system
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN202210214825.8A
Other languages
Chinese (zh)
Other versions
CN114608578B (en
Inventor
蔡庆中
李健
杨功流
闻泽阳
陈天宇
牛浩飞
李辉
李晶
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Beihang University
Original Assignee
Beihang University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Beihang University filed Critical Beihang University
Priority to CN202210214825.8A priority Critical patent/CN114608578B/en
Publication of CN114608578A publication Critical patent/CN114608578A/en
Application granted granted Critical
Publication of CN114608578B publication Critical patent/CN114608578B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Navigation (AREA)

Abstract

The invention discloses a weighted uncertainty unmanned aerial vehicle cluster collaborative navigation method, which comprises the following steps: s1, each unmanned aerial vehicle obtains the reliability information of the current moment S2, each unmanned aerial vehicle receives the reliability information broadcasted by other unmanned aerial vehicles, and the reliability information of other unmanned aerial vehicles is converted into a particle set form; s3, solving the weight of each reliability information for the particle set of the received reliability information by each unmanned aerial vehicle, and carrying out normalization processing; s4, calculating the corrected current time calculation position; the method combines the dynamic error statistical characteristics of the inertial measurement unit to represent the dynamic position error and related influence factors of the unmanned aerial vehicle, and can improve the robustness of the collaborative navigation algorithm under the dynamic condition of the unmanned aerial vehicle; according to the characteristic that the position error of each unmanned aerial vehicle changes in real time, the position information received in the unmanned aerial vehicle collaborative navigation process is quantized, and a self-adaptive weight adjusting function is constructed, so that the position information of each unmanned aerial vehicle in the unmanned aerial vehicle cluster collaborative navigation is effectively fused and corrected.

Description

Weighted uncertainty unmanned aerial vehicle cluster collaborative navigation method
Technical Field
The invention relates to the technical field of collaborative navigation, in particular to a weighted uncertainty unmanned aerial vehicle cluster collaborative navigation method.
Background
In recent years, with the rapid development of unmanned aerial vehicle technology, the unmanned aerial vehicle has been widely applied in the fields of military operations, earthquake relief, agriculture and forestry plant protection and the like. The single unmanned aerial vehicle is difficult to independently execute complex tasks due to the constraints of self power, load, limited functions and the like. And the cluster system that comprises a certain amount of unmanned aerial vehicle can further improve whole load and perception ability on low-cost basis, furthest exerts unmanned aerial vehicle's advantage.
The accurate position information provided by radio modes such as satellite navigation is the basis for distributed control and formation of unmanned aerial vehicle clusters, but when the unmanned aerial vehicle clusters pass through environments with invalid satellite navigation signals such as urban tunnels and caves, the unmanned aerial vehicle needs to acquire navigation information by means of inertial navigation equipment. However, due to various errors of the inertial devices, the position error of the inertial navigation system is dispersed with time, so that the unmanned aerial vehicle cluster cannot smoothly perform tasks. The cooperative navigation technology realizes cooperative optimization of individual position information by adopting a specific algorithm through accurate distance measurement and direction finding and information sharing among unmanned aerial vehicles, can improve the positioning and navigation performance of independent individuals, and is the basis for a cluster to finish complex task organization and accurate cooperative matching.
The collaborative navigation technology under the satellite navigation rejection condition is generally divided into a master-slave mode and a parallel mode, and the master-slave mode generally requires that a main node unmanned aerial vehicle carrying a high-precision inertial navigation system in a cluster provides navigation reference information to calibrate the positions of other unmanned aerial vehicles in the cluster. But the dependency on the master node is high, which results in poor robustness of the cluster system. In the parallel method, the positioning precision of a specific unmanned aerial vehicle is not depended on, mutual correction can be still realized when faults or communication failures of individual unmanned aerial vehicles exist, and accurate positioning of other individuals in the cluster is guaranteed. The traditional parallel collaborative navigation algorithm directly performs equal-weight fusion on navigation information, and the influence of various factors such as the mass of an inertial device, the maneuvering state of the unmanned aerial vehicle and the time of losing satellite navigation signals on the position error of each unmanned aerial vehicle is not considered, so that the collaborative navigation precision is limited in improvement effect.
Disclosure of Invention
The invention aims to provide a weighted uncertainty unmanned aerial vehicle cluster cooperative navigation method for solving the problem that the error of an unmanned aerial vehicle inertial navigation system accumulates along with time under the condition that a satellite navigation signal is lost.
In order to solve the problems, the invention provides a collaborative navigation algorithm based on weighted belief propagation, which is used for completing information transmission and fusion in a factor graph model by utilizing an improved belief propagation algorithm on the basis of constructing a collaborative navigation Bayes factor graph model so as to realize collaborative navigation of a plurality of unmanned aerial vehicles.
Therefore, the specific technical scheme of the invention is as follows:
a weighted uncertainty unmanned aerial vehicle cluster collaborative navigation method comprises the following specific steps:
1. a weighted uncertainty unmanned aerial vehicle cluster collaborative navigation method is characterized by comprising the following steps:
s1, each unmanned aerial vehicle obtains the reliability information of the current moment of the unmanned aerial vehicle according to the displacement, angular rate information and acceleration information output by the inertial measurement unit of the unmanned aerial vehicle, wherein the reliability information comprises the calculated position of the unmanned aerial vehicle at the current moment and the position uncertainty diagonal matrix;
s2, each unmanned aerial vehicle receives the reliability information broadcast by other unmanned aerial vehicles and converts the reliability information of other unmanned aerial vehicles into a particle set form;
for any drone i:
s201, receiving reliability information broadcasted by other unmanned aerial vehicles j by the unmanned aerial vehicle i;
s202, calculating the current time of the unmanned aerial vehicle j to obtain the position
Figure BDA0003534017620000021
And position uncertainty diagonal array
Figure BDA0003534017620000022
As a normal distribution
Figure BDA0003534017620000023
Randomly extracting M particles to form a computing position particle set
Figure BDA0003534017620000024
Wherein p is the calculated position of the particle, w is the weight corresponding to the particle, and subscript k represents the particle number inside the particle set;
s203, measuring the relative distance between the unmanned aerial vehicle j and the unmanned aerial vehicle i
Figure BDA0003534017620000025
And variance of relative distance measurement between unmanned plane j and unmanned plane i
Figure BDA0003534017620000026
As a normal distribution
Figure BDA0003534017620000027
Randomly extracting M particles to form a distance particle set
Figure BDA0003534017620000028
Wherein the subscript k denotes the particle number inside the particle set,
Figure BDA0003534017620000029
measured by a radio ranging device carried by the drone,
Figure BDA0003534017620000031
a radio ranging device related parameter;
s204, according to uniform distribution (0,2 pi)]Randomly extracting M particles to form an angle particle set for describing the relative angle between the unmanned plane j and the unmanned plane i
Figure BDA0003534017620000032
Wherein the subscript k represents the particle number inside the particle set;
s205, based on the steps S202 to S204, the unmanned aerial vehicle i receives the particle set of the credibility information of each rest of unmanned aerial vehicles j
Figure BDA0003534017620000033
According to the formula:
Figure BDA0003534017620000034
calculating to obtain;
s3, solving the weight of each reliability information for the particle set of the received reliability information by each unmanned aerial vehicle, and carrying out normalization processing;
s301, unmanned aerial vehicle i particle set according to received credibility information
Figure BDA0003534017620000035
Calculating weight w of position information between the unmanned aerial vehicle j and the unmanned aerial vehicle jij(t)：
Figure BDA0003534017620000036
Where c is a parameter related to the accuracy of the inertial device, Ji{ p (t) } is the integrated position uncertainty for drone i, which is represented by the northbound position uncertainty matrix E { δ p for drone iN(t) } and east position uncertainty matrix E { δ p }E(t) is obtained by calculation, and the calculation formula is as follows:
Figure BDA0003534017620000037
calculating to obtain; j is a unit ofj{ p (t) } is the integrated position uncertainty for drone j, which is represented by the northbound position uncertainty matrix E { δ p for drone jN(t) and east position uncertainty matrix E [ δ p ]E(t) is obtained by calculation, and the calculation formula is as follows:
Figure BDA0003534017620000038
calculating to obtain;
s302, weighting value w of all credibility informationij(t) according to the formula:
Figure BDA0003534017620000039
carrying out normalization calculation to obtain a normalized weight vij(t); wherein N is the unmanned aerial vehicle quantity that constitutes the unmanned aerial vehicle cluster.
S4, calculating the corrected current time calculation position;
s401, respectively converting particle sets of reliability information receiving reliability information received by unmanned aerial vehicle i into probability density functions
Figure BDA00035340176200000310
Figure BDA0003534017620000041
In the formula,
Figure BDA0003534017620000042
gaussian kernel, σ is the standard deviation of the ranging information,
Figure BDA0003534017620000043
and
Figure BDA0003534017620000044
particle sets respectively being credibility information
Figure BDA0003534017620000045
The position of the particle in (1) and the weight of the particle;
s402, calculating the position of the unmanned aerial vehicle i at the current moment
Figure BDA0003534017620000046
Conversion to probability density function
Figure BDA0003534017620000047
Figure BDA0003534017620000048
In the formula,
Figure BDA0003534017620000049
and
Figure BDA00035340176200000410
calculating the positions of the particles in the particle set of the positions and the weights of the particles for the unmanned aerial vehicle i at the current moment respectively;
s403, collecting all the credibility information particle sets obtained in the step S2, uniformly sampling the collected credibility information particle sets, randomly extracting 500 particles, and obtaining a particle set representing the i-position distribution of the unmanned aerial vehicle
Figure BDA00035340176200000411
Wherein,
Figure BDA00035340176200000412
is the position of the particles and is,
Figure BDA00035340176200000413
the new weight of the particle is calculated by the following formula:
Figure BDA00035340176200000414
in the formula,
Figure BDA00035340176200000415
and
Figure BDA00035340176200000416
probability density functions for computed location information and received confidence information obtained by kernel density estimation,
Figure BDA00035340176200000417
representing the sum of all the confidence information particle sets received by drone i,
Figure BDA00035340176200000418
representing a set of reliability information of the unmanned aerial vehicle j which can be received by the unmanned aerial vehicle i;
s404, unmanned aerial vehicle i:
Figure BDA00035340176200000419
and solving the minimum mean square error of the position, namely the corrected current time position of the unmanned aerial vehicle i.
Further, in step S1, the calculation formula of the calculated position of the drone at the current time is:
Figure BDA00035340176200000420
in the formula, the subscript i represents the drone number,
Figure BDA00035340176200000421
the position of the unmanned aerial vehicle at the last moment is shown, f (omega) is the displacement output by the inertial navigation system,
Figure BDA00035340176200000422
and calculating the position of the unmanned aerial vehicle at the current moment.
Further, unmanned aerial vehicle position uncertainty diagonal array U(t)The calculation formula of (2) is as follows:
U(t)＝diag{E{δpN(t)}E{δpE(t)}}，
in the formula, E { δ pN(t) is the northbound position uncertainty matrix E { δ p) for the droneN(t)}，E{δpE(t) is an east position uncertainty matrix of the unmanned aerial vehicle, and the east position uncertainty matrix are obtained through calculation of angular rate information and acceleration information output by an inertial measurement unit of the unmanned aerial vehicle; wherein,
Figure BDA0003534017620000051
Figure BDA0003534017620000052
in the formula, δ KGxIs the gyro scale factor error in the x-axis direction, delta K, in the carrier coordinate systemGyIs the gyro scale factor error in the y-axis direction under the carrier coordinate system, delta KGzThe gyroscope scale factor error in the z-axis direction under the carrier coordinate system is obtained; delta KAyIs the degree factor error, delta K, of the accelerometer in the y-axis direction in the carrier coordinate systemAxThe degree factor error of the accelerometer in the x-axis direction under the carrier coordinate system is obtained; omegaxIs the output angular rate, omega, of the gyroscope in the x-axis direction under a carrier coordinate systemyIs the output angular rate, omega, of the gyroscope in the y-axis direction under a carrier coordinate systemzThe gyroscope output angular rate in the z-axis direction under a carrier coordinate system is obtained; epsilonxIs a gyro zero offset value epsilon in the x-axis direction under a carrier coordinate systemyIs a gyro zero offset value epsilon in the y-axis direction under a carrier coordinate systemzThe gyro zero offset value in the z-axis direction under the carrier coordinate system is obtained; vxIs a zero bias value of the accelerometer in the x-axis direction under the carrier coordinate systemyThe zero offset value of the accelerometer in the y-axis direction under the carrier coordinate system is obtained;
Figure BDA0003534017620000053
a transformation matrix from a carrier coordinate system to a navigation coordinate system; g is the acceleration of gravity, fNComponent of acceleration output from accelerometer in north direction, fEThe component of the accelerometer output in the east direction, fyAcceleration, f, output by the accelerometer in the y-axis direction in the carrier coordinate systemxThe acceleration output by the accelerometer in the x-axis direction under the carrier coordinate system; t is the satellite navigation signal loss time; the carrier specifically refers to unmanned aerial vehicle i.
4. The weighted uncertainty unmanned aerial vehicle cluster collaborative navigation method according to claim 1, wherein M ≧ 500.
Compared with the prior art, the weighted uncertainty unmanned aerial vehicle cluster cooperative navigation method combines the dynamic error statistical characteristics of the inertial measurement unit to represent the dynamic position error and relevant influence factors of the unmanned aerial vehicle, and can improve the robustness of the cooperative navigation algorithm under the dynamic condition of the unmanned aerial vehicle; meanwhile, according to the characteristic that the position error of each unmanned aerial vehicle changes in real time, the position information received in the unmanned aerial vehicle collaborative navigation process is quantized, and a self-adaptive weight adjusting function is constructed, so that the position information of each unmanned aerial vehicle in the unmanned aerial vehicle cluster collaborative navigation is effectively fused and corrected.
Drawings
FIG. 1 is a flow chart of a weighted uncertainty unmanned aerial vehicle cluster collaborative navigation method of the present invention;
fig. 2 is a diagram comparing the specific flight trajectory of five unmanned aerial vehicles with the path output by the inertial navigation simulation in the embodiment of the present invention;
fig. 3 is a graph of a change with time of an average position error obtained by using the collaborative navigation method, the collaborative navigation method based on reliability propagation, and the inertial navigation self-output data according to 50 simulation tests in the embodiment of the present invention.
Detailed Description
The invention will be further described with reference to the following figures and specific examples, which are not intended to limit the invention in any way.
As shown in fig. 1, the specific implementation steps of the weighted uncertainty unmanned aerial vehicle cluster collaborative navigation method are as follows:
s1, each unmanned aerial vehicle obtains the reliability information of the current moment of the unmanned aerial vehicle according to the displacement, angular rate information and acceleration information output by the inertial measurement unit of the unmanned aerial vehicle, wherein the reliability information comprises the calculated position of the unmanned aerial vehicle at the current moment
Figure BDA0003534017620000061
And position uncertainty diagonal array U(t)；
Specifically, the specific process of step S1 is:
s101, the unmanned aerial vehicle obtains the calculated position of the current moment according to the displacement output by the inertial navigation system:
Figure BDA0003534017620000062
in the formula, the subscript i represents the drone number,
Figure BDA0003534017620000063
the position of the unmanned aerial vehicle at the last moment is shown, f (omega) is the displacement output by the inertial navigation system,
Figure BDA0003534017620000064
calculating the position of the unmanned aerial vehicle at the current moment;
s102, calculating a north position uncertainty matrix E { δ p of the unmanned aerial vehicle according to angular rate information and acceleration information output by an inertial measurement unit of the unmanned aerial vehicleN(t) and east position uncertainty matrix E [ δ p ]E(t) }, including:
(1) northbound position uncertainty matrix E { δ pN(t)}：
Figure BDA0003534017620000071
(2) East-oriented position uncertainty matrix E [ delta ] pE(t)}：
Figure BDA0003534017620000072
In the formula, δ KGxFor gyro scale factor error in the x-axis direction in the carrier coordinate system, delta KGyIs the gyro scale factor error in the y-axis direction under the carrier coordinate system, delta KGzThe gyroscope scale factor error in the z-axis direction under the carrier coordinate system is obtained; delta KAyDegree factor error, delta K, of an accelerometer in the y-axis direction in a carrier coordinate systemAxThe degree factor error of the accelerometer in the x-axis direction under the carrier coordinate system is obtained; omegaxIs the output angular rate, omega, of the gyroscope in the x-axis direction under a carrier coordinate systemyIs the output angular rate, omega, of the gyroscope in the y-axis direction under a carrier coordinate systemzFor gyro output in z-axis direction under carrier coordinate systemAn angular rate; epsilonxIs a gyro zero offset value epsilon in the x-axis direction under a carrier coordinate systemyIs a gyro zero offset value epsilon in the y-axis direction under a carrier coordinate systemzThe gyro zero offset value in the z-axis direction under the carrier coordinate system is obtained; vxIs a zero bias value of the accelerometer in the x-axis direction under the carrier coordinate systemyThe zero offset value of the accelerometer in the y-axis direction under the carrier coordinate system is obtained;
Figure BDA0003534017620000073
a transformation matrix from the carrier coordinate system to the navigation coordinate system; g is the acceleration of gravity, fNComponent of acceleration output from accelerometer in north direction, fEThe component of the accelerometer output in the east direction, fyAcceleration, f, output by the accelerometer in the y-axis direction in the carrier coordinate systemxThe acceleration output by the accelerometer in the x-axis direction under the carrier coordinate system; t is the satellite navigation signal loss time; the carrier specifically refers to an unmanned aerial vehicle i;
s103, the unmanned aerial vehicle obtains a north position uncertainty matrix E { δ p in the step S102N(t) and east position uncertainty matrix E [ δ p ]E(t) calculating to obtain a position uncertainty diagonal matrix U(t)：U(t)＝diag{E{δpN(t)}E{δpE(t)}}；
S2, each unmanned aerial vehicle receives the reliability information broadcast by other unmanned aerial vehicles and converts the reliability information of other unmanned aerial vehicles into a particle set form;
specifically, for any drone i, the specific process of step S2 includes:
s201, receiving reliability information broadcasted by other unmanned aerial vehicles j by the unmanned aerial vehicles, namely calculating the positions of the other unmanned aerial vehicles at the current time
Figure BDA0003534017620000081
Sum position uncertainty diagonal matrix
Figure BDA0003534017620000082
S202, using the unmanned plane jCalculating the position at the current time
Figure BDA0003534017620000083
And position uncertainty diagonal array
Figure BDA0003534017620000084
As a normal distribution
Figure BDA0003534017620000085
Randomly extracting 500 particles to form a computing position particle set
Figure BDA0003534017620000086
Wherein p is the calculated position of the particle, w is the weight corresponding to the particle, and subscript k represents the particle number inside the particle set;
s203, measuring the relative distance between the unmanned plane j and the unmanned plane i
Figure BDA0003534017620000087
And variance of relative distance measurement between unmanned plane j and unmanned plane i
Figure BDA0003534017620000088
As a normal distribution
Figure BDA0003534017620000089
The parameters in (1) are randomly extracted to form a distance particle set
Figure BDA00035340176200000810
Wherein the subscript k denotes the particle number inside the particle set,
Figure BDA00035340176200000811
measured by a radio ranging device carried by the drone,
Figure BDA00035340176200000812
a radio ranging device related parameter;
s204, uniformly distributing according to (0,2 pi)]Randomly extracting 500 particlesConstitute the set of angle particles for describing the relative angle between drone j and drone i
Figure BDA00035340176200000813
Wherein the subscript k represents the particle number inside the particle set;
s205, based on the steps S202 to S204, the unmanned aerial vehicle i receives the particle set of the credibility information of each rest of unmanned aerial vehicles j
Figure BDA00035340176200000814
According to the formula:
Figure BDA00035340176200000815
and (4) calculating.
S3, particle set of credibility information received by each unmanned aerial vehicle
Figure BDA00035340176200000816
Solving the weight w of each credibility informationij(t) carrying out normalization processing;
specifically, the step S3 includes the following steps:
s301, unmanned aerial vehicle i particle set according to received credibility information
Figure BDA00035340176200000817
Calculating the weight w of the position information between the unmanned aerial vehicle j and the unmanned aerial vehicle jij(t)：
Figure BDA00035340176200000818
Where c is a parameter related to the accuracy of the inertial device, Ji{ p (t) } is the integrated position uncertainty for drone i, which is represented by the northbound position uncertainty matrix E { δ p for drone iN(t) and east position uncertainty matrix E [ δ p ]E(t) is obtained by calculation, and the calculation formula is as follows:
Figure BDA0003534017620000091
is calculated to obtain；Jj{ p (t) } is the integrated position uncertainty for drone j, which is represented by the northbound position uncertainty matrix E { δ p for drone jN(t) and east position uncertainty matrix E [ δ p ]E(t) is obtained by calculation, and the calculation formula is as follows:
Figure BDA0003534017620000092
calculating to obtain;
s302, weighting value w of all credibility informationij(t) according to the formula:
Figure BDA0003534017620000093
carrying out normalization calculation to obtain a normalized weight vij(t); wherein N is the number of unmanned aerial vehicles forming the unmanned aerial vehicle cluster;
s4, calculating the corrected current time calculation position;
specifically, the step S4 includes the following steps:
s401, respectively converting the particle sets of the reliability information receiving reliability information received by the unmanned aerial vehicle i into probability density functions
Figure BDA0003534017620000094
Figure BDA0003534017620000095
In the formula,
Figure BDA0003534017620000096
gaussian kernel, σ is the standard deviation of the ranging information,
Figure BDA0003534017620000097
and
Figure BDA0003534017620000098
particle sets respectively being credibility information
Figure BDA0003534017620000099
Position of the particles in (1) and particlesThe weight of (2);
s402, calculating the position of the unmanned aerial vehicle i at the current moment
Figure BDA00035340176200000910
Conversion to probability density function
Figure BDA00035340176200000911
Figure BDA00035340176200000912
In the formula,
Figure BDA00035340176200000913
and
Figure BDA00035340176200000914
calculating the positions of the particles in the particle set of the positions and the weights of the particles for the unmanned aerial vehicle i at the current moment respectively;
s403, collecting all the credibility information particle sets obtained in the step S2, uniformly sampling the collected credibility information particle sets, randomly extracting 500 particles, and obtaining a particle set representing the i-position distribution of the unmanned aerial vehicle
Figure BDA00035340176200000915
Wherein,
Figure BDA00035340176200000916
is the position of the particles and is,
Figure BDA00035340176200000917
the new weight of the particle is calculated by the following formula:
Figure BDA0003534017620000101
in the formula,
Figure BDA0003534017620000102
and
Figure BDA0003534017620000103
probability density functions of calculated position information and reception reliability information obtained by kernel density estimation,
Figure BDA0003534017620000104
representing the sum of all the confidence information particle sets received by drone i,
Figure BDA0003534017620000105
representing a set of reliability information of the unmanned aerial vehicle j which can be received by the unmanned aerial vehicle i;
s404, unmanned aerial vehicle i:
Figure BDA0003534017620000106
and solving the minimum mean square error of the position, namely the corrected current time position of the unmanned aerial vehicle i.
After the correction
Figure BDA0003534017620000107
Current time calculation position for replacing unmanned aerial vehicle i
Figure BDA0003534017620000108
And finishing the fusion updating of the position state of the unmanned aerial vehicle.
In order to verify the effectiveness of the method, the unmanned aerial vehicle cluster collaborative navigation method and the widely used collaborative navigation method based on credibility propagation are subjected to simulation comparison verification.
Specifically, the simulation conditions are set as: assuming that five unmanned aerial vehicles fly at the same height, the initial speeds are all 2 m/s; the unmanned aerial vehicle No. 1, the unmanned aerial vehicle No. 2 and the unmanned aerial vehicle No. 5 adopt a constant-speed linear flight mode, the unmanned aerial vehicle No. 3 adopts a flight mode of firstly accelerating and then decelerating, and the unmanned aerial vehicle No. 4 adopts a flight mode of maneuvering turning; in the motion process, the unmanned aerial vehicle carries out mutual distance measurement and navigation information interaction every 1 second; the simulation time is 100s, the zero offset value and the noise of the gyroscope are respectively 1 degree/h and 0.1 degree/h, the zero offset value and the noise of the accelerometer are respectively 1mg and 0.1mg, the error of a scale factor is 100ppm, and the relative distance measurement standard deviation is 0.2 m;
fig. 2 is a graph comparing the specific flight trajectory of five drones with the path output by the inertial navigation simulation. In the figure, a solid line is a true value of flight trajectories of five unmanned aerial vehicles, and a dotted line is an error path output by inertial navigation simulation; as can be seen from the comparison of the actual flight trajectory and the error path of each unmanned aerial vehicle, the path directly output by adopting inertial navigation simulation has great difference from the actual path.
Fig. 3 is a graph showing the time-dependent change of the average position error obtained by the cooperative navigation method, the cooperative navigation method based on the belief propagation, and the output data of the inertial navigation system according to the present invention, which are obtained from 50 simulation tests. As is apparent from fig. 3, the average position error obtained by the cooperative navigation method, the cooperative navigation method based on the belief propagation, and the self-output data of the inertial navigation of the present invention gradually increases, and the test result shows that the self-position error of the inertial navigation is 52 meters, while the position error of the conventional cooperative navigation method based on the belief propagation is 33 meters, and the position error of the present application is 18 meters.

Claims (4)

1. A weighted uncertainty unmanned aerial vehicle cluster collaborative navigation method is characterized by comprising the following steps:
s1, each unmanned aerial vehicle obtains the reliability information of the current moment of the unmanned aerial vehicle according to the displacement, angular rate information and acceleration information output by the inertial measurement unit of the unmanned aerial vehicle, wherein the reliability information comprises the calculated position of the unmanned aerial vehicle at the current moment and the position uncertainty diagonal matrix;
s2, each unmanned aerial vehicle receives the reliability information broadcast by other unmanned aerial vehicles and converts the reliability information of other unmanned aerial vehicles into a particle set form;
for any drone i:
s201, receiving reliability information broadcasted by other unmanned aerial vehicles j by the unmanned aerial vehicle i;
s202, enabling the unmanned aerial vehicle jCalculating the position at the present time
Figure FDA0003534017610000011
And position uncertainty diagonal array
Figure FDA0003534017610000012
As a normal distribution
Figure FDA0003534017610000013
Randomly extracting M particles to form a computing position particle set
Figure FDA0003534017610000014
Wherein p is the calculation position of the particle, w is the weight corresponding to the particle, and subscript k represents the particle number inside the particle set;
s203, measuring the relative distance between the unmanned aerial vehicle j and the unmanned aerial vehicle i
Figure FDA0003534017610000015
And variance of relative distance measurement between unmanned plane j and unmanned plane i
Figure FDA0003534017610000016
As a normal distribution
Figure FDA0003534017610000017
Randomly extracting M particles to form a distance particle set
Figure FDA0003534017610000018
Wherein the subscript k denotes the particle number inside the particle set,
Figure FDA0003534017610000019
measured by a radio ranging device carried by the drone,
Figure FDA00035340176100000110
a radio ranging device related parameter;
s204, according to uniform distribution (0,2 pi)]Randomly extracting M particles to form an angle particle set for describing the relative angle between the unmanned plane j and the unmanned plane i
Figure FDA00035340176100000111
Wherein the subscript k represents the particle number inside the particle set;
s205, based on the steps S202 to S204, the unmanned aerial vehicle i receives the particle set of the credibility information of each rest of unmanned aerial vehicles j
Figure FDA00035340176100000112
According to the formula:
Figure FDA00035340176100000113
calculating to obtain;
s3, solving the weight of each reliability information for the particle set of the received reliability information by each unmanned aerial vehicle, and carrying out normalization processing;
s301, unmanned aerial vehicle i particle set according to received credibility information
Figure FDA00035340176100000114
Calculating the weight w of the position information between the unmanned aerial vehicle j and the unmanned aerial vehicle jij(t)：
Figure FDA0003534017610000021
Where c is a parameter related to the accuracy of the inertial device, Ji{ p (t) } is the integrated position uncertainty for drone i, which is represented by the northbound position uncertainty matrix E { δ p for drone iN(t) and east position uncertainty matrix E [ δ p ]E(t) is obtained by calculation, and the calculation formula is as follows:
Figure FDA0003534017610000022
calculating to obtain; j. the design is a squarej{ p (t) } is unmannedThe integrated position uncertainty of drone j, which is determined by the northbound position uncertainty matrix E { δ p of drone jN(t) and east position uncertainty matrix E [ δ p ]E(t) is obtained by calculation, and the calculation formula is as follows:
Figure FDA0003534017610000023
calculating to obtain;
s302, weighting value w of all credibility informationij(t) according to the formula:
Figure FDA0003534017610000024
carrying out normalization calculation to obtain a normalized weight vij(t); wherein N is the unmanned aerial vehicle quantity that constitutes the unmanned aerial vehicle cluster.
S4, calculating the corrected current time calculation position;
s401, respectively converting particle sets of reliability information receiving reliability information received by unmanned aerial vehicle i into probability density functions
Figure FDA0003534017610000025
Figure FDA0003534017610000026
In the formula,
Figure FDA0003534017610000027
gaussian kernel, σ is the standard deviation of the ranging information,
Figure FDA0003534017610000028
and
Figure FDA0003534017610000029
particle sets respectively being confidence information
Figure FDA00035340176100000210
Position of the particle in (1) and weight of the particle；
S402, calculating the position of the unmanned aerial vehicle i at the current moment
Figure FDA00035340176100000211
Conversion to probability density function
Figure FDA00035340176100000212
Figure FDA00035340176100000213
In the formula,
Figure FDA00035340176100000214
and
Figure FDA00035340176100000215
calculating the positions of the particles in the particle set of the positions and the weights of the particles for the unmanned aerial vehicle i at the current moment respectively;
s403, collecting all the credibility information particle sets obtained in the step S2, uniformly sampling the collected credibility information particle sets, randomly extracting 500 particles, and obtaining a particle set representing the i-position distribution of the unmanned aerial vehicle
Figure FDA0003534017610000031
Wherein,
Figure FDA0003534017610000032
is the position of the particles and is,
Figure FDA0003534017610000033
the new weight of the particle is calculated by the following formula:
Figure FDA0003534017610000034
in the formula,
Figure FDA0003534017610000035
and
Figure FDA0003534017610000036
probability density functions for computed location information and received confidence information obtained by kernel density estimation,
Figure FDA0003534017610000037
representing the sum of all the confidence information particle sets received by drone i,
Figure FDA0003534017610000038
representing a set of reliability information of the unmanned aerial vehicle j which can be received by the unmanned aerial vehicle i;
s404, unmanned aerial vehicle i:
Figure FDA0003534017610000039
and solving the minimum mean square error of the position, namely the corrected current time position of the unmanned aerial vehicle i.
2. The method for weighted uncertainty unmanned aerial vehicle cluster collaborative navigation according to claim 1, wherein in step S1, the calculation formula of the calculated position of the unmanned aerial vehicle at the current time is:
Figure FDA00035340176100000310
in the formula, the subscript i represents the drone number,
Figure FDA00035340176100000311
the position of the unmanned aerial vehicle at the last moment is shown, f (omega) is the displacement output by the inertial navigation system,
Figure FDA00035340176100000312
and calculating the position of the unmanned aerial vehicle at the current moment.
3. The method of claim 1, wherein the unmanned aerial vehicle position uncertainty diagonal matrix U is a global position uncertainty(t)The calculation formula of (2) is as follows:
U(t)＝diag{E{δpN(t)} E{δpE(t)}}，
in the formula, E { δ pN(t) is the northbound position uncertainty matrix E { δ p) for the droneN(t)}，E{δpE(t) is an east position uncertainty matrix of the unmanned aerial vehicle, and the east position uncertainty matrix are obtained through calculation of angular rate information and acceleration information output by an inertial measurement unit of the unmanned aerial vehicle; wherein,
Figure FDA00035340176100000313
Figure FDA00035340176100000314
in the formula, δ KGxFor gyro scale factor error in the x-axis direction in the carrier coordinate system, delta KGyFor gyro scale factor error in y-axis direction under carrier coordinate system, delta KGzThe gyroscope scale factor error in the z-axis direction under the carrier coordinate system is obtained; delta KAyDegree factor error, delta K, of an accelerometer in the y-axis direction in a carrier coordinate systemAxThe degree factor error of the accelerometer in the x-axis direction under the carrier coordinate system is obtained; omegaxIs the gyro output angular rate, omega, in the x-axis direction under the carrier coordinate systemyIs the output angular rate, omega, of the gyroscope in the y-axis direction under a carrier coordinate systemzThe gyroscope output angular rate in the z-axis direction under a carrier coordinate system is obtained; epsilonxIs a gyro zero offset value epsilon in the x-axis direction under a carrier coordinate systemyIs a gyro zero offset value epsilon in the y-axis direction under a carrier coordinate systemzThe gyro zero offset value in the z-axis direction under the carrier coordinate system is obtained;
Figure FDA0003534017610000041
is the accelerometer zero offset value in the x-axis direction under the carrier coordinate system,
Figure FDA0003534017610000042
the zero offset value of the accelerometer in the y-axis direction under the carrier coordinate system is obtained;
Figure FDA0003534017610000043
a transformation matrix from a carrier coordinate system to a navigation coordinate system; g is the acceleration of gravity, fNComponent of acceleration output from accelerometer in north direction, fEThe component of the accelerometer output in the east direction, fyAcceleration, f, output by the accelerometer in the y-axis direction in the carrier coordinate systemxThe acceleration output by the accelerometer in the x-axis direction under the carrier coordinate system; t is the satellite navigation signal loss time; the carrier specifically refers to unmanned aerial vehicle i.
4. The weighted uncertainty unmanned aerial vehicle cluster collaborative navigation method according to claim 1, wherein M ≧ 500.
CN202210214825.8A 2022-03-07 2022-03-07 Weighted uncertainty unmanned aerial vehicle cluster collaborative navigation method Active CN114608578B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210214825.8A CN114608578B (en) 2022-03-07 2022-03-07 Weighted uncertainty unmanned aerial vehicle cluster collaborative navigation method

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210214825.8A CN114608578B (en) 2022-03-07 2022-03-07 Weighted uncertainty unmanned aerial vehicle cluster collaborative navigation method

Publications (2)

Publication Number Publication Date
CN114608578A true CN114608578A (en) 2022-06-10
CN114608578B CN114608578B (en) 2024-04-09

Family

ID=81860302

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210214825.8A Active CN114608578B (en) 2022-03-07 2022-03-07 Weighted uncertainty unmanned aerial vehicle cluster collaborative navigation method

Country Status (1)

Country Link
CN (1) CN114608578B (en)

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108151737A (en) * 2017-12-19 2018-06-12 南京航空航天大学 A kind of unmanned plane bee colony collaborative navigation method under the conditions of the mutual observed relationships of dynamic
CN109813311A (en) * 2019-03-18 2019-05-28 南京航空航天大学 A kind of unmanned plane formation collaborative navigation method
WO2020220729A1 (en) * 2019-04-29 2020-11-05 南京航空航天大学 Inertial navigation solution method based on angular accelerometer/gyroscope/accelerometer

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108151737A (en) * 2017-12-19 2018-06-12 南京航空航天大学 A kind of unmanned plane bee colony collaborative navigation method under the conditions of the mutual observed relationships of dynamic
CN109813311A (en) * 2019-03-18 2019-05-28 南京航空航天大学 A kind of unmanned plane formation collaborative navigation method
WO2020220729A1 (en) * 2019-04-29 2020-11-05 南京航空航天大学 Inertial navigation solution method based on angular accelerometer/gyroscope/accelerometer

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
刘晓洋;李瑞涛;徐胜红;: "基于测距/测速信息的无人机协同导航算法研究", 战术导弹技术, no. 02, 30 April 2019 (2019-04-30) *

Also Published As

Publication number Publication date
CN114608578B (en) 2024-04-09

Similar Documents

Publication Publication Date Title
CN110487301B (en) Initial alignment method of radar-assisted airborne strapdown inertial navigation system
CN113175931B (en) Cluster networking collaborative navigation method and system based on constraint Kalman filtering
US20110209544A1 (en) Sensor cluster navigation device and method
CN112346104B (en) Unmanned aerial vehicle information fusion positioning method
CN110243377A (en) A kind of cluster aircraft collaborative navigation method based on hierachical structure
CN112902967A (en) Anti-cheating navigation method based on residual error chi-square-improved sequential probability ratio
CN109471146A (en) A kind of self-adapted tolerance GPS/INS Combinated navigation method based on LS-SVM
CN110398242B (en) Attitude angle determination method for high-rotation-height overload condition aircraft
CN112697154B (en) Self-adaptive multi-source fusion navigation method based on vector distribution
CN108303063B (en) High-precision vehicle-mounted combined elevation measurement method
CN116086445A (en) Multi-source information time delay fusion navigation method based on factor graph optimization
Chu et al. Performance comparison of tight and loose INS-Camera integration
CN112284388B (en) Unmanned aerial vehicle multisource information fusion navigation method
CN106886037B (en) POS data method for correcting error suitable for weak GNSS signal condition
CN114608578B (en) Weighted uncertainty unmanned aerial vehicle cluster collaborative navigation method
CN113359167A (en) Method for fusing and positioning GPS and laser radar through inertial measurement parameters
CN112683265A (en) MIMU/GPS integrated navigation method based on rapid ISS collective filtering
CN115560757B (en) Unmanned aerial vehicle direct positioning correction method based on neural network under random attitude error condition
CN118276135A (en) Combined navigation method based on self-adaptive maximum entropy criterion Kalman filtering
CN117419723A (en) Interactive model self-adaptive integrated navigation method based on factor graph
CN117156382A (en) Aircraft cluster collaborative navigation optimization method with different airspace distribution
CN117320148A (en) Multi-source data fusion positioning method, system, electronic equipment and storage medium
Mai et al. Mobile target localization and tracking techniques in harsh environment utilizing adaptive multi‐modal data fusion
CN115127591A (en) Airborne DPOS transfer alignment method based on statistical confidence distance measurement bootstrapping
Zakharin et al. Concept of navigation system design of UAV

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant