CN113295162A - Generalized factor graph fusion navigation method based on unmanned aerial vehicle state information - Google Patents
Generalized factor graph fusion navigation method based on unmanned aerial vehicle state information Download PDFInfo
- Publication number
- CN113295162A CN113295162A CN202110468764.3A CN202110468764A CN113295162A CN 113295162 A CN113295162 A CN 113295162A CN 202110468764 A CN202110468764 A CN 202110468764A CN 113295162 A CN113295162 A CN 113295162A
- Authority
- CN
- China
- Prior art keywords
- navigation
- factor graph
- information
- aerial vehicle
- unmanned aerial
- 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
Links
- 230000004927 fusion Effects 0.000 title claims abstract description 49
- 238000000034 method Methods 0.000 title claims abstract description 15
- 238000005259 measurement Methods 0.000 claims abstract description 51
- 238000012937 correction Methods 0.000 claims abstract description 16
- 230000006870 function Effects 0.000 claims abstract description 15
- 238000013528 artificial neural network Methods 0.000 claims abstract description 7
- 238000013507 mapping Methods 0.000 claims abstract description 5
- 238000001914 filtration Methods 0.000 claims description 18
- 239000011159 matrix material Substances 0.000 claims description 12
- NAWXUBYGYWOOIX-SFHVURJKSA-N (2s)-2-[[4-[2-(2,4-diaminoquinazolin-6-yl)ethyl]benzoyl]amino]-4-methylidenepentanedioic acid Chemical compound C1=CC2=NC(N)=NC(N)=C2C=C1CCC1=CC=C(C(=O)N[C@@H](CC(=C)C(O)=O)C(O)=O)C=C1 NAWXUBYGYWOOIX-SFHVURJKSA-N 0.000 claims description 6
- 238000004364 calculation method Methods 0.000 claims description 6
- 125000004432 carbon atom Chemical group C* 0.000 claims description 6
- 230000014509 gene expression Effects 0.000 claims description 6
- 238000005315 distribution function Methods 0.000 claims description 3
- 230000009466 transformation Effects 0.000 claims description 3
- 230000008569 process Effects 0.000 claims description 2
- 239000013589 supplement Substances 0.000 abstract 1
- 238000013461 design Methods 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 230000001360 synchronised effect Effects 0.000 description 2
- 230000009471 action Effects 0.000 description 1
- 230000002457 bidirectional effect Effects 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 125000004122 cyclic group Chemical group 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 238000007689 inspection Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000007500 overflow downdraw method Methods 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 239000007787 solid Substances 0.000 description 1
- 230000000007 visual effect Effects 0.000 description 1
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/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
- G01C21/165—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 combined with non-inertial navigation instruments
-
- 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
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
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 generalized factor graph fusion navigation method based on unmanned aerial vehicle state information. Firstly, designing a navigation system scheme according to an actual situation, constructing a virtual sensor in an online neural network learning mode based on airborne physical sensor data, and introducing a system to supplement navigation state quantity measurement when other sensors have faults; secondly, a factor graph multi-source autonomous navigation fusion algorithm based on unmanned aerial vehicle state information is established, and unmanned aerial vehicle state information is divided into a position factor graph, a speed factor graph and an attitude factor graph and estimated in parallel based on unmanned aerial vehicle combined navigation sensor information; and finally, analyzing a function mapping relation between the attitude error of the unmanned aerial vehicle and the position information and the speed information based on the position and speed information after the unmanned helicopter is fused, correcting the fusion information of the generalized attitude factor graph filter on the basis, and improving the measurement precision of the position, the speed and the attitude of the unmanned aerial vehicle by feedback correction based on the generalized attitude factor graph fusion navigation framework.
Description
Technical Field
The invention belongs to the technical field of integrated navigation, and particularly relates to a generalized factor graph fusion navigation method of an unmanned aerial vehicle.
Background
In recent years, the unmanned aerial vehicle is highly valued by all countries in the world by virtue of huge application market and potential expansion field, and the unmanned aerial vehicle technology is widely applied in military field and civil field. The unmanned aerial vehicle has the capabilities of unmanned driving and remote control, and is widely applied to the fields of maritime search and rescue, forest fire rescue, energy detection, commercial transportation and the like. Although there are a lot of research on the flight safety of the unmanned aerial vehicle, and the unmanned aerial vehicle has better flight characteristics, it is still affected by the complex atmospheric environment. Especially low-altitude atmospheric disturbances can cause changes in the attitude of the drone. Under the interference of complex atmosphere, the high-precision attitude and other navigation state information of the unmanned aerial vehicle are problems to be solved urgently.
The unmanned aerial vehicle is mainly positioned by an airborne navigation system, and the unmanned aerial vehicle navigation system outputs the position, the speed and the attitude information of the unmanned aerial vehicle in real time to provide accurate position coordinates and state information for the aircraft. In the unmanned aerial vehicle navigation system, because the navigation principle that each sensor adopted is different, there is extremely strong complementarity between all kinds of sensors. In practical application, due to the fact that updating frequencies of different sensors are different, time is not synchronous, the complex and changeable application requirements are difficult to meet by adopting a fixed filtering structure and a fixed filtering method, and the filtering structure with flexible and changeable factor graphs is more suitable for a multi-source combined navigation filtering fusion method.
The probabilistic graphical model is a theory that the graphical model represents the probability dependence relationship of variables. The factor graph is a bidirectional probability graph model, and the graph comprises two types of nodes: one is a variable node, which represents a variable in the global multivariate function; one is the factor node, which represents a local function in the factorization. Each local function is only related to partial variables in the global multivariate function, and if and only if the variable is an argument of the local function, a connecting edge exists between a variable node corresponding to the variable node and a factor node in the factor graph. The factor graph is used as a graphic tool for analyzing problems, and is probably helpful for solving the problems of unequal intervals, asynchrony, dynamic change and the like of the measurement information of the unmanned aerial vehicle sensor.
The attitude measurement information of the unmanned aerial vehicle is insufficient, only an IMU in the unmanned aerial vehicle integrated navigation system provides all attitude information, a magnetometer only provides course angle correction, and other sensors provide position and speed information. The factor graph framework divides the position, the speed and the attitude into three factor graphs for synchronous filtering, so that the measurement of the attitude factor graphs is less, the flight safety of the unmanned aerial vehicle is ensured by accurate measurement of the attitude, and meanwhile, the improvement of the attitude accuracy also corrects the accuracy of the position and the speed.
Disclosure of Invention
In order to solve the technical problems mentioned in the background art, the invention provides a generalized factor graph fusion navigation method based on unmanned aerial vehicle state information, so that the accuracy of unmanned aerial vehicle attitude measurement is improved, the flight safety of the unmanned aerial vehicle is ensured, and meanwhile, the accuracy of position and speed is also corrected by improving the attitude accuracy.
In order to achieve the technical purpose, the technical scheme of the invention is as follows:
the generalized factor graph fusion navigation method based on the state information of the unmanned aerial vehicle comprises the following steps
Designing a navigation system according to the task requirement, the environment and a navigation sensor of the unmanned aerial vehicle, constructing a virtual sensor based on the state and measurement information of a physical navigation sensor in an online neural network learning mode, determining the working performance of the airborne navigation sensor of the unmanned aerial vehicle, and introducing system supplementary navigation state quantity measurement when a relevant physical sensor fails;
acquiring sensor measurement information obtained by each airborne navigation system, dividing the sensor measurement information into three factor graphs of position, speed and attitude for information fusion, defining the combined state variable of the unmanned aerial vehicle as a variable node of the factor graph, defining the measured value of each airborne navigation sensor as a factor node, and constructing a multi-source navigation information fusion algorithm based on the position factor graph, the speed factor graph and the attitude factor graph;
and thirdly, introducing the position and speed information estimated by filtering based on the position factor graph and the speed factor graph into a generalized attitude factor graph filter, and correcting the result of filtering of the attitude factor graph.
Further, the second step includes the following specific steps:
step 2a, defining state variables of a navigation system of the unmanned aerial vehicle as variable nodes of a factor graph, defining carrier measurement information acquired by an inertial measurement unit, a virtual sensor and other various airborne sensors as factor nodes of the factor graph, and constructing a multi-source navigation information fusion system based on the factor graph;
step 2b, under a factor graph multi-source navigation information fusion framework, selecting a constraint rule of multi-source navigation information fusion, establishing factor node expressions of an inertia measurement unit and other various onboard navigation sensors and virtual sensors, establishing a joint probability distribution function through analyzing the factor graph fusion rule and the factor node expressions of the various sensors, obtaining estimation of a state variable when the joint probability distribution is maximized, and completing effective fusion of the multi-source navigation information through real-time filtering estimation and correction;
the navigation system state variable X is as follows:
in the above formula, the first and second carbon atoms are,
error angle of plateau, δ vE,δvN,δvUThe speed error in the northeast direction is shown, and the delta L, the delta lambda and the delta h are latitude, longitude and altitude position errors;
inertial navigation variable factor node of the factor graph
The following were used:
in the above formula, fb、ωbThe specific force and the angular velocity obtained by the inertia measurement unit are respectively;
the factor nodes of other navigation systems except inertial navigation and virtual navigation systems in the factor graph are uniformly determined as
The formula is as follows:
in the above formula, the first and second carbon atoms are,
measurement values, h, representing other assisted navigation systemsSensorIs a measurement equation of other assisted navigation systems, nSensorIs the measurement noise of other assistant navigation systems;
the state quantity estimated by the unmanned aerial vehicle navigation system is the maximum probability of X (t) under the occurrence condition of Z (t), so the maximum posterior probability estimated value of the joint distribution probability function is obtained, namely the state quantity which is most likely to occur:
wherein h isi(Xi) To observe the equation, ziFor true quantity measurement, sigmaiIs a covariance matrix;
for non-linear observation equation hi(Xi) Performing first-order Taylor expansion to realize linearization, and obtaining a state update vector:
wherein HiIs the observed Jacobian matrix, Δ*Updating the vector, Δ, for the estimated stateiUpdating the vector for the state at the time i; and calculating to obtain the speed, position and attitude fusion information of the unmanned aerial vehicle based on the factor graph after solving the state update vector.
Further, the third step includes the following specific steps:
on the basis of not considering the second-order small amount of attitude error, the geographic coordinate system is taken as a navigation coordinate system, and the platform error angle equation of inertial navigation is as follows:
wherein L isfgAnd hfgRespectively longitude and altitude information after position factor graph fusion,
and
are east-oriented and north-oriented speed information after the speed factor graph is fused respectively,
and
is inertial navigation platform angular information, omega, fused with a pose factor graphieIs the earth selfAngular velocity, RMRadius of curvature in meridian plane, RNThe curvature radius of the prime circle is;
converting the unmanned aerial vehicle platform error angle into an equivalent rotation matrix, expressed as follows:
due to the existence of the attitude angle error, the attitude angle fused by the attitude factor graph is regarded as a calculation system, and a transformation matrix from the calculation system to a body system is shown as the following formula:
wherein theta isfg、γfgAnd psifgRespectively representing the attitude angles of the attitude factor graph;
multiplying the formula (9) by the formula (10) to obtain a function mapping relation between the attitude angle error and the information after the fusion of other factor graphs, wherein the attitude correction equation is as follows:
bringing formula (9) and formula (10) into formula (11):
further, navigation information with higher precision is obtained by optimizing the generalized factor graph filter through feedback correction.
Drawings
FIG. 1 is a view of a fusion structure of factor graphs of each flight phase according to the present invention;
FIG. 2 is an overall flow chart of the present invention.
Detailed Description
The technical scheme of the invention is explained in detail in the following with the accompanying drawings.
The generalized factor graph fusion navigation method based on the state information of the unmanned aerial vehicle disclosed by the embodiment comprises the following contents
Designing a navigation system according to the task requirement, the environment and a navigation sensor of the unmanned aerial vehicle, constructing a virtual sensor based on the state and measurement information of a physical navigation sensor in an online neural network learning mode, determining the working performance of the airborne navigation sensor of the unmanned aerial vehicle, and introducing system supplementary navigation state quantity measurement when a relevant physical sensor fails;
acquiring sensor measurement information obtained by each airborne navigation system, dividing the sensor measurement information into three factor graphs of position, speed and attitude for information fusion, defining the combined state variable of the unmanned aerial vehicle as a variable node of the factor graph, defining the measured value of each airborne navigation sensor as a factor node, and constructing a multi-source navigation information fusion algorithm based on the position factor graph, the speed factor graph and the attitude factor graph;
and thirdly, introducing the position and speed information estimated by filtering based on the position factor graph and the speed factor graph into a generalized attitude factor graph filter, and correcting the result of filtering of the attitude factor graph.
Further, the second step includes the following specific steps:
step 2a, defining state variables of a navigation system of the unmanned aerial vehicle as variable nodes of a factor graph, defining carrier measurement information acquired by an inertial measurement unit, a virtual sensor and other various airborne sensors as factor nodes of the factor graph, and constructing a multi-source navigation information fusion system based on the factor graph;
step 2b, under a factor graph multi-source navigation information fusion framework, selecting a constraint rule of multi-source navigation information fusion, establishing factor node expressions of an inertia measurement unit and other various onboard navigation sensors and virtual sensors, establishing a joint probability distribution function through analyzing the factor graph fusion rule and the factor node expressions of the various sensors, obtaining estimation of a state variable when the joint probability distribution is maximized, and completing effective fusion of the multi-source navigation information through real-time filtering estimation and correction;
the navigation system state variable X is as follows:
in the above formula, the first and second carbon atoms are,
error angle of plateau, δ vE,δvN,δvUThe speed error in the northeast direction is shown, and the delta L, the delta lambda and the delta h are the position errors of latitude, longitude and altitude;
inertial navigation variable factor node of the factor graph
The following were used:
in the above formula, fb、ωbThe specific force and the angular velocity obtained by the inertia measurement unit are respectively;
the factor nodes of other navigation systems except inertial navigation and virtual navigation systems in the factor graph are uniformly determined as
The formula is as follows:
in the above formula, the first and second carbon atoms are,
measurement values, h, representing other assisted navigation systemsSensorIs a measurement equation of other assisted navigation systems, nSensorIs the measurement noise of other assistant navigation systems;
the state quantity estimated by the unmanned aerial vehicle navigation system is the maximum probability of X (t) under the occurrence condition of Z (t), so the maximum posterior probability estimated value of the joint distribution probability function is obtained, namely the state quantity which is most likely to occur:
wherein h isi(Xi) To observe the equation, ziFor true quantity measurement, sigmaiIs a covariance matrix;
for non-linear observation equation hi(Xi) Performing first-order Taylor expansion to realize linearization, and obtaining a state update vector:
wherein HiIs the observed Jacobian matrix, Δ*Updating the vector, Δ, for the estimated stateiUpdating the vector for the state at the time i; and calculating to obtain the speed, position and attitude fusion information of the unmanned aerial vehicle based on the factor graph after solving the state update vector.
Further, the third step includes the following specific steps:
on the basis of not considering the second-order small amount of attitude error, the geographic coordinate system is taken as a navigation coordinate system, and the platform error angle equation of inertial navigation is as follows:
wherein L isfgAnd hfgRespectively longitude and altitude information after position factor graph fusion,
and
are east-oriented and north-oriented speed information after the speed factor graph is fused respectively,
and
is inertial navigation platform angular information, omega, fused with a pose factor graphieIs the rotational angular velocity of the earth, RMRadius of curvature in meridian plane, RNThe curvature radius of the prime circle is;
converting the unmanned aerial vehicle platform error angle into an equivalent rotation matrix, expressed as follows:
due to the existence of the attitude angle error, the attitude angle fused by the attitude factor graph is regarded as a calculation system, and a transformation matrix from the calculation system to a body system is shown as the following formula:
wherein theta isfg、γfgAnd psifgRespectively representing the attitude angles of the attitude factor graph;
multiplying the formula (9) by the formula (10) to obtain a function mapping relation between the attitude angle error and the information after the fusion of other factor graphs, wherein the attitude correction equation is as follows:
bringing formula (9) and formula (10) into formula (11):
further, navigation information with higher precision is obtained by optimizing the generalized factor graph filter through feedback correction.
Taking an unmanned aerial vehicle airborne navigation system as an example, firstly, based on task requirements, obtaining sensor measurement information of each airborne navigation system, dividing the sensor measurement information into three factor graphs of position, speed and attitude for information fusion, then, based on position factor graphs and speed factor graphs, filtering estimated position and speed information, introducing the information into a generalized attitude factor graph filter, correcting the result of the attitude factor graph filtering, and optimizing the generalized factor graph filter through feedback correction to obtain navigation information with higher precision. The entire process of the present invention will be described in detail below.
1. Under the unmanned aerial vehicle flight environment, according to actual conditions, task demand and the environment design unmanned aerial vehicle flight track, sensor signal, motion state etc..
Here, an unmanned aerial vehicle is taken as an example, and the design of the flight path of the unmanned aerial vehicle is specifically described. According to the task target, a flight track meeting the flight requirement of the unmanned aerial vehicle, the complex environment where the carrier is located and the task characteristics needs to be designed. The requirements for different tasks, for example: military attack, emergency rescue, line inspection, express delivery, flight performance and the like, the working environment, flight action and task requirements of the system are also greatly different, and the flight configuration and flight track are also different. For drones, typical flight configurations can be classified as follows:
a triangle shape; a rhombus shape; ③ S shape; fourthly, 8-shaped; a trapezoid shape; sixthly, the shape is circular; and the straight line shape.
To accomplish different flight missions, the drone may be combined in one or more of the configurations described above. According to the requirements, a configuration which meets the actual situation and respective flight paths are designed, so that the configuration can meet the task requirements and the actual requirements.
2. Since the mission requirements of drones are different, considering the cost and the onboard load of the drones, it is necessary to determine the available sensor types.
The navigation sensors which can be used for the factor graph algorithm are various in types, the scheme of a navigation system of the unmanned aerial vehicle is designed according to task requirements and actual conditions, an inertial navigation sensor, a satellite navigation sensor, a visual camera, an atmospheric data system and a magnetometer are selected as the scheme for specifically implementing sensor selection, and a virtual navigation system is constructed in an online learning mode through a BP neural network, a long-short term memory neural network, a cyclic neural network and the like.
3. Establishment of generalized state factor graph information fusion correction framework
The factor graph is a probabilistic graph model G ═ (F, X, E), containing two types of nodes: one is factor node fiE.g. F, representing a local function in factorization; one is variable node xjE.x, represents a variable in the global multivariate function. State variable node x in the current factor graph architecturejAnd the factor node f corresponding theretoiWhen related, there is a connecting edge e between themij∈E。
As shown in fig. 1, in the present invention, the unmanned aerial vehicle adopts a generalized attitude factor graph architecture as an attitude information fusion correction scheme. In the figure, the hollow circles represent state variable nodes, the solid circles represent factor nodes, X represents the navigation state of the system, and in a position factor graph, a speed factor graph and a posture factor graph, respectively, are position state quantity, speed state quantity and posture state quantity, f represents the measurement information of each sensor, and f isPriorRepresenting previous measurement information, fIMURepresenting measurement information from the IMU, related to the navigation state at time k and at time k + 1, fGNSS、fADS、fSAR、fBAAnd also measurement information of other navigation systems respectively. And under a factor graph framework, establishing a filtering estimation equation, and performing real-time filtering estimation and correction to complete effective fusion of the multi-source sensor information.
In fig. 2, the attitude factor graph architecture defines the attitude state quantity of the unmanned aerial vehicle as a variable node, and selects the measurement values of the attitude measurement sensors such as the inertial navigation system and the magnetic heading system of the unmanned aerial vehicle as factor nodes.
The generalized attitude factor graph filter introduces the position factor graph and the speed information fused in the position factor graph and the speed factor graph into the attitude factor graph, corrects the effect of fusing the attitude factor graph of the unmanned aerial vehicle based on the function mapping relation among the position, the speed and the attitude error, improves the attitude measurement precision of the unmanned aerial vehicle, and achieves the effect of correcting the position factor graph and the speed factor graph through feedback correction of the high-precision attitude information.
The embodiments are only for illustrating the technical idea of the present invention, and the technical idea of the present invention is not limited thereto, and any modifications made on the basis of the technical scheme according to the technical idea of the present invention fall within the scope of the present invention.
Claims (4)
1. The generalized factor graph fusion navigation method based on the state information of the unmanned aerial vehicle is characterized by comprising the following steps
Designing a navigation system according to the task requirement, the environment and a navigation sensor of the unmanned aerial vehicle, constructing a virtual sensor based on the state and measurement information of a physical navigation sensor in an online neural network learning mode, determining the working performance of the airborne navigation sensor of the unmanned aerial vehicle, and introducing system supplementary navigation state quantity measurement when a relevant physical sensor fails;
acquiring sensor measurement information obtained by each airborne navigation system, dividing the sensor measurement information into three factor graphs of position, speed and attitude for information fusion, defining the combined state variable of the unmanned aerial vehicle as a variable node of the factor graph, defining the measured value of each airborne navigation sensor as a factor node, and constructing a multi-source navigation information fusion algorithm based on the position factor graph, the speed factor graph and the attitude factor graph;
and thirdly, introducing the position and speed information estimated by filtering based on the position factor graph and the speed factor graph into a generalized attitude factor graph filter, and correcting the result of filtering of the attitude factor graph.
2. The generalized factor graph fusion navigation method based on unmanned aerial vehicle state information according to claim 1, wherein the second step comprises the following specific processes:
step 2a, defining state variables of a navigation system of the unmanned aerial vehicle as variable nodes of a factor graph, defining carrier measurement information acquired by an inertial measurement unit, a virtual sensor and other various airborne sensors as factor nodes of the factor graph, and constructing a multi-source navigation information fusion system based on the factor graph;
step 2b, under a factor graph multi-source navigation information fusion framework, selecting a constraint rule of multi-source navigation information fusion, establishing factor node expressions of an inertia measurement unit and other various onboard navigation sensors and virtual sensors, establishing a joint probability distribution function through analyzing the factor graph fusion rule and the factor node expressions of the various sensors, obtaining estimation of a state variable when the joint probability distribution is maximized, and completing effective fusion of the multi-source navigation information through real-time filtering estimation and correction;
the navigation system state variable X is as follows:
in the above formula, the first and second carbon atoms are,
error angle of plateau, δ vE,δvN,δvUThe speed error in the northeast direction is shown, and the delta L, the delta lambda and the delta h are latitude, longitude and altitude position errors;
inertial navigation variable factor node of the factor graph
The following were used:
in the above formula, fb、ωbThe specific force and the angular velocity obtained by the inertia measurement unit are respectively;
the factor nodes of other navigation systems except inertial navigation and virtual navigation systems in the factor graph are uniformly determined as
The formula is as follows:
in the above formula, the first and second carbon atoms are,
measurement values, h, representing other assisted navigation systemsSensorIs a measurement equation of other assisted navigation systems, nSensorIs other auxiliary navigationThe measured noise of the system;
the state quantity estimated by the unmanned aerial vehicle navigation system is the maximum probability of X (t) under the occurrence condition of Z (t), so the maximum posterior probability estimated value of the joint distribution probability function is obtained, namely the state quantity which is most likely to occur:
wherein h isi(Xi) To observe the equation, ziFor true quantity measurement, sigmaiIs a covariance matrix;
for non-linear observation equation hi(Xi) Performing first-order Taylor expansion to realize linearization, and obtaining a state update vector:
wherein HiIs the observed Jacobian matrix, Δ*Updating the vector, Δ, for the estimated stateiUpdating the vector for the state at the time i; and calculating to obtain the speed, position and attitude fusion information of the unmanned aerial vehicle based on the factor graph after solving the state update vector.
3. The generalized factor graph fusion navigation method based on unmanned aerial vehicle state information according to claim 1, wherein the third step comprises the following specific procedures:
on the basis of not considering the second-order small amount of attitude error, the geographic coordinate system is taken as a navigation coordinate system, and the platform error angle equation of inertial navigation is as follows:
wherein L isfgAnd hfgRespectively longitude and altitude information after position factor graph fusion,
and
are east-oriented and north-oriented speed information after the speed factor graph is fused respectively,
and
is inertial navigation platform angular information, omega, fused with a pose factor graphieIs the rotational angular velocity of the earth, RMRadius of curvature in meridian plane, RNThe curvature radius of the prime circle is;
converting the unmanned aerial vehicle platform error angle into an equivalent rotation matrix, expressed as follows:
due to the existence of the attitude angle error, the attitude angle fused by the attitude factor graph is regarded as a calculation system, and a transformation matrix from the calculation system to a body system is shown as the following formula:
wherein theta isfg、γfgAnd psifgRespectively representAttitude factor graph attitude angle;
multiplying the formula (9) by the formula (10) to obtain a function mapping relation between the attitude angle error and the information after the fusion of other factor graphs, wherein the attitude correction equation is as follows:
bringing formula (9) and formula (10) into formula (11):
4. the generalized factor graph fusion navigation method based on unmanned aerial vehicle state information according to any one of claims 1 to 3, wherein the generalized factor graph filter is optimized through feedback correction to obtain navigation information with higher precision.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110468764.3A CN113295162B (en) | 2021-04-28 | 2021-04-28 | Generalized factor graph fusion navigation method based on unmanned aerial vehicle state information |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110468764.3A CN113295162B (en) | 2021-04-28 | 2021-04-28 | Generalized factor graph fusion navigation method based on unmanned aerial vehicle state information |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN113295162A true CN113295162A (en) | 2021-08-24 |
| CN113295162B CN113295162B (en) | 2024-06-11 |
Family
ID=77320347
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202110468764.3A Active CN113295162B (en) | 2021-04-28 | 2021-04-28 | Generalized factor graph fusion navigation method based on unmanned aerial vehicle state information |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN113295162B (en) |
Cited By (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114545472A (en) * | 2022-01-26 | 2022-05-27 | 中国人民解放军国防科技大学 | Navigation method and device of GNSS/INS combined system |
| CN114562992A (en) * | 2022-02-09 | 2022-05-31 | 南京航空航天大学 | Multi-path environment combined navigation method based on factor graph and scene constraint |
| CN114563804A (en) * | 2021-12-27 | 2022-05-31 | 中国人民解放军空军工程大学 | Adaptive fault-tolerant method of GNSS/INS tightly-combined navigation system |
| CN116165690A (en) * | 2023-04-21 | 2023-05-26 | 山西省娄烦县皇姑山矿业有限责任公司 | GNSS/INS-based double-adaptive-factor combined navigation positioning method |
| CN117076824A (en) * | 2023-09-26 | 2023-11-17 | 腾讯科技（深圳）有限公司 | State prediction method and device of mobile terminal, electronic equipment and storage medium |
Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20110089949A1 (en) * | 2009-10-02 | 2011-04-21 | Kyriakos Walid | Parallel Imaging Apparatus and Method |
| CN109798891A (en) * | 2019-01-25 | 2019-05-24 | 上海交通大学 | Inertial Measurement Unit calibration system based on high-precision motion capture system |
| CN110274588A (en) * | 2019-06-19 | 2019-09-24 | 南京航空航天大学 | Double-layer nested factor graph multi-source fusion air navigation aid based on unmanned plane cluster information |
-
2021
- 2021-04-28 CN CN202110468764.3A patent/CN113295162B/en active Active
Patent Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20110089949A1 (en) * | 2009-10-02 | 2011-04-21 | Kyriakos Walid | Parallel Imaging Apparatus and Method |
| CN109798891A (en) * | 2019-01-25 | 2019-05-24 | 上海交通大学 | Inertial Measurement Unit calibration system based on high-precision motion capture system |
| CN110274588A (en) * | 2019-06-19 | 2019-09-24 | 南京航空航天大学 | Double-layer nested factor graph multi-source fusion air navigation aid based on unmanned plane cluster information |
Non-Patent Citations (2)
| Title |
|---|
| WEINA CHEN: "Research on thE Multi-sensor information fusion method based on factor graph", 2016 PLANS, pages 502 - 506 * |
| 李杰: "基于因子图的无人机多源组合导航算法研究", 航天控制, vol. 39, no. 1, pages 52 - 57 * |
Cited By (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114563804A (en) * | 2021-12-27 | 2022-05-31 | 中国人民解放军空军工程大学 | Adaptive fault-tolerant method of GNSS/INS tightly-combined navigation system |
| CN114563804B (en) * | 2021-12-27 | 2024-05-24 | 中国人民解放军空军工程大学 | Self-adaptive fault-tolerant method of GNSS/INS (Global navigation satellite System/inertial navigation System) tightly combined navigation system |
| CN114545472A (en) * | 2022-01-26 | 2022-05-27 | 中国人民解放军国防科技大学 | Navigation method and device of GNSS/INS combined system |
| CN114562992A (en) * | 2022-02-09 | 2022-05-31 | 南京航空航天大学 | Multi-path environment combined navigation method based on factor graph and scene constraint |
| CN116165690A (en) * | 2023-04-21 | 2023-05-26 | 山西省娄烦县皇姑山矿业有限责任公司 | GNSS/INS-based double-adaptive-factor combined navigation positioning method |
| CN117076824A (en) * | 2023-09-26 | 2023-11-17 | 腾讯科技（深圳）有限公司 | State prediction method and device of mobile terminal, electronic equipment and storage medium |
| CN117076824B (en) * | 2023-09-26 | 2024-01-12 | 腾讯科技（深圳）有限公司 | State prediction method and device of mobile terminal, electronic equipment and storage medium |
Also Published As
| Publication number | Publication date |
|---|---|
| CN113295162B (en) | 2024-06-11 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110274588B (en) | Double-layer nested factor graph multi-source fusion navigation method based on unmanned aerial vehicle cluster information | |
| CN113295162B (en) | Generalized factor graph fusion navigation method based on unmanned aerial vehicle state information | |
| CN109813311B (en) | Unmanned aerial vehicle formation collaborative navigation method | |
| CN111238467B (en) | Bionic polarized light assisted unmanned combat aircraft autonomous navigation method | |
| CN112505737B (en) | GNSS/INS integrated navigation method | |
| CN109708639B (en) | Method for generating lateral guidance instruction of aircraft for tracking straight line and circular arc path in flat flight | |
| CN111189442B (en) | CEPF-based unmanned aerial vehicle multi-source navigation information state prediction method | |
| Petrich et al. | On-board wind speed estimation for uavs | |
| CN108458709B (en) | Airborne distributed POS data fusion method and device based on vision-aided measurement | |
| Wolf et al. | Toward improved landing precision on Mars | |
| CN106352897B (en) | It is a kind of based on the silicon MEMS gyro estimation error of monocular vision sensor and bearing calibration | |
| Brossard et al. | Tightly coupled navigation and wind estimation for mini UAVs | |
| Roh et al. | Dynamic accuracy improvement of a MEMS AHRS for small UAVs | |
| Flores et al. | A nonlinear path-following strategy for a fixed-wing MAV | |
| CN116105730A (en) | Angle measurement-only optical combination navigation method based on cooperative target satellite very short arc observation | |
| Emran et al. | A cascaded approach for quadrotor's attitude estimation | |
| Yang et al. | Model-free integrated navigation of small fixed-wing UAVs full state estimation in wind disturbance | |
| Oshman et al. | Mini-UAV altitude estimation using an inertially stabilized payload | |
| Kecheng et al. | Modified attitude factor graph fusion method for unmanned helicopter under atmospheric disturbance | |
| Tahir et al. | A review of UAV platforms for autonomous applications: comprehensive analysis and future directions | |
| Song | An integrated GPS/vision UAV navigation system based on Kalman filter | |
| CN111473786A (en) | Two-layer distributed multi-sensor combined navigation filtering method based on local feedback | |
| Dai et al. | Unmanned ground vehicle‐unmanned aerial vehicle relative navigation robust adaptive localization algorithm | |
| Kopecki et al. | A proposal of AHRS yaw angle correction with the use of roll angle | |
| Park et al. | Directional axis estimation including wind velocity for fixed wing 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 |