CN113175931A - Cluster networking collaborative navigation method and system based on constraint Kalman filtering - Google Patents

Cluster networking collaborative navigation method and system based on constraint Kalman filtering Download PDF

Info

Publication number
CN113175931A
CN113175931A CN202110361651.3A CN202110361651A CN113175931A CN 113175931 A CN113175931 A CN 113175931A CN 202110361651 A CN202110361651 A CN 202110361651A CN 113175931 A CN113175931 A CN 113175931A
Authority
CN
China
Prior art keywords
node
matrix
cluster
inertial navigation
representing
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
CN202110361651.3A
Other languages
Chinese (zh)
Other versions
CN113175931B (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.)
Shanghai Institute of Electromechanical Engineering
Original Assignee
Shanghai Institute of Electromechanical Engineering
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 Shanghai Institute of Electromechanical Engineering filed Critical Shanghai Institute of Electromechanical Engineering
Priority to CN202110361651.3A priority Critical patent/CN113175931B/en
Publication of CN113175931A publication Critical patent/CN113175931A/en
Application granted granted Critical
Publication of CN113175931B publication Critical patent/CN113175931B/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
    • 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
    • G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
    • G01C25/005—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or starting-up of inertial devices
    • 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
    • Y02D—CLIMATE CHANGE MITIGATION TECHNOLOGIES IN INFORMATION AND COMMUNICATION TECHNOLOGIES [ICT], I.E. INFORMATION AND COMMUNICATION TECHNOLOGIES AIMING AT THE REDUCTION OF THEIR OWN ENERGY USE
    • Y02D30/00—Reducing energy consumption in communication networks
    • Y02D30/70—Reducing energy consumption in communication networks in wireless communication networks

Landscapes

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

Abstract

The invention provides a cluster networking collaborative navigation method and system based on constraint Kalman filtering, and relates to the technical field of collaborative navigation, wherein the method comprises the following steps: step S1: determining the number of cluster networking nodes, and measuring the relative distance between the nodes; step S2: establishing a cluster networking collaborative navigation filtering equation based on the measured relative distance between the nodes; step S3: designing inertial navigation position error weighting and minimum constraint conditions of each node of a cluster networking; step S4: constructing constraint Kalman filtering of cluster networking collaborative navigation; step S5: and compensating pure inertial navigation errors by using the inertial navigation error estimation values of each node estimated by the constraint Kalman filtering. The method can recursion and estimate the inertial navigation error in real time to correct the inertial navigation system and inhibit pure inertial navigation error divergence of each node.

Description

Cluster networking collaborative navigation method and system based on constraint Kalman filtering
Technical Field
The invention relates to the technical field of collaborative navigation, in particular to a cluster networking collaborative navigation method and system based on constraint Kalman filtering.
Background
At present, the inertial navigation technology is widely applied to various scenes in life such as aerospace, aviation, unmanned driving, underwater navigation, indoor positioning and the like, and becomes a common navigation mode.
The invention discloses a Chinese patent with publication number CN110207691A, and discloses a multi-unmanned vehicle cooperative navigation method based on data chain distance measurement, which measures the relative distance between unmanned vehicles by using a data chain, and further estimates the position of each unmanned vehicle node in a formation by a multi-navigation sensor information fusion method. Each unmanned vehicle node in the unmanned vehicle formation carries an inertial navigation system, a vehicle odometer and a data chain transceiving device, and the inertial navigation system and the vehicle odometer in each unmanned vehicle node are fused through a Kalman filter to obtain speed, position and attitude information of each unmanned vehicle node; and then optimizing the position estimation result of each unmanned vehicle through equality constraint by utilizing the distance information between the unmanned vehicles measured by the data chain. By the method, the navigation and positioning accuracy of the unmanned vehicle formation under the satellite rejection condition can be improved.
In summary, in the prior art, other navigation sensors must be used to assist in building constraints during collaborative navigation, such as vehicle odometers and the like, which brings additional economic cost and is limited in use in many scenarios. According to the invention, only by depending on the inertial navigation system carried by each carrier, the data chain is used for ranging, and the constraint Kalman filtering is constructed by designing geometric constraint conditions, so that the problem that the inertial navigation system has long-term error quick divergence and cannot be used for long-term navigation is solved.
Disclosure of Invention
Aiming at the defects in the prior art, the invention aims to provide a cluster networking collaborative navigation method and system based on constraint Kalman filtering.
According to the cluster networking collaborative navigation method and system based on the constraint Kalman filtering, the scheme is as follows:
in a first aspect, a cluster networking collaborative navigation method based on constrained kalman filtering is provided, where the method includes:
step S1: determining the number of cluster networking nodes, and measuring the relative distance between the nodes;
step S2: establishing a cluster networking collaborative navigation filtering equation based on the measured relative distance between the nodes;
step S3: designing inertial navigation position error weighting and minimum constraint conditions of each node of a cluster networking;
step S4: constructing constraint Kalman filtering of cluster networking collaborative navigation;
step S5: and compensating pure inertial navigation errors by using the inertial navigation error estimation values of each node estimated by the constraint Kalman filtering.
Preferably, the cluster networking collaborative navigation filtering equation in step S2 includes a state equation, where the state equation specifically is:
X(t)＝F(t)X(t)+G(t)W(t)
in the formula,
Figure BDA0003005782290000021
respectively representing three-axis position errors of each node in the cluster under a geographic coordinate system, wherein n represents the number of the cluster nodes, F (t) represents a state transition matrix, G (t) represents a system noise coefficient matrix, and W (t) represents a system noise matrix;
wherein,
Figure BDA0003005782290000022
Figure BDA0003005782290000023
in the formula, Ve iRepresenting the east speed of the inode; l isiRepresenting the latitude of the inode; h isiRepresenting the height of the inode; rNRepresenting the curvature radius of the earth fourth-unit circle; rMRepresents the radius of curvature of the earth's meridian; vn iRepresenting the i-node northbound speed.
Preferably, the cluster networking collaborative navigation filtering equation in step S2 further includes a measurement equation, where the measurement equation specifically is:
L(t)＝HX(t)+Δ
wherein L (t) is an (n-1) × 1-dimensional observation quantity matrix, n-1 is the number of observation quantities, i.e., the number of distance measurement values, H is an (n-1) × 3 n-dimensional observation coefficient matrix, and Δ is an (n-1) × 1-dimensional observation noise matrix;
wherein,
Figure BDA0003005782290000031
Figure BDA0003005782290000032
in the formula,
Figure BDA0003005782290000033
the measured distance between node i and node j in the cluster,
Figure BDA0003005782290000034
and calculating the distance for inertial navigation between the node i and the node j, wherein L changes in real time along with the change of the number of the nodes n of the missile group.
Preferably, in step S3, the inertial navigation position error weighting and minimum constraint condition of each node of the cluster networking are provided, where the constraint condition includes:
GTPxDaX＝0
in the formula, GTIs formed by HTA 6 x 3 n-dimensional matrix composed of eigenvectors corresponding to the zero eigenvalue of PH, P being a diagonal weight matrix of L, HTA positive definite matrix of a diagonal weight matrix with PH of L; pxIs a 3n multiplied by 3n dimensional weight matrix of a parameter vector X to be estimated, which is determined according to the precision level of each node inertial navigation system, wherein n is the number of cluster nodes,
Figure BDA0003005782290000035
in the formula,
Figure BDA0003005782290000036
for the inertial navigation position of the cluster node i under the earth-centered earth-fixed system, PxVarying in real time as the number of cluster nodes n varies.
Preferably, in step S3, the inertial navigation position error weighting and minimum constraint condition of each node of the cluster networking is that a transformation matrix for transforming the latitude and longitude high error in the geographic coordinate system to the XYZ triaxial error in the geocentric geostationary coordinate system is:
Figure BDA0003005782290000041
Figure BDA0003005782290000042
in the formula, Rn represents the curvature radius of the earth unitary fourth of twelve earthly branches;
Figure BDA0003005782290000043
representing the i-node inertial navigation resolving height;
Figure BDA0003005782290000044
representing the i-node inertial navigation resolving latitude;
Figure BDA0003005782290000045
to representResolving longitude by i-node inertial navigation; e.g. of the type2Representing the curvature of the earth.
Preferably, in step S4, the constraint condition is projected to the conventional kalman filter by using a projection method, and the constraint kalman filter for constructing the clustered network collaborative navigation is as follows:
constraint condition matrix: b isk＝GTPxDa；
And (3) state one-step prediction:
Figure BDA0003005782290000046
and (3) restraining the state after correction for one-step prediction:
Figure BDA0003005782290000047
and (3) state estimation:
Figure BDA0003005782290000048
filtering gain:
Figure BDA0003005782290000049
one-step prediction of mean square error:
Figure BDA00030057822900000410
estimating the mean square error: pk＝(I-KkHk)Pk/k-1；
In the formula, BkIn order to be a matrix of the constraint conditions,
Figure BDA00030057822900000411
in order to predict the state quantity matrix in one step,
Figure BDA00030057822900000412
is a previous state quantity matrix, phik，k-1In order to be a state transition matrix,
Figure BDA00030057822900000413
correcting for constraintsPost-prediction state quantity matrix, Pk-1Is a state error covariance matrix, KkFor filtering the gain matrix, ZkAs a matrix of observations, HkFor observing the coefficient matrix, Pk/k-1Predicting a state error covariance matrix, R, for one stepkTo observe the noise matrix, QkIs a state noise matrix and I is an identity matrix.
Preferably, in step S5, the inertial navigation error estimation value of each node estimated by using constrained kalman filtering
Figure BDA00030057822900000414
The method for compensating the pure inertial navigation error comprises the following steps:
Figure BDA00030057822900000415
Figure BDA00030057822900000416
Figure BDA00030057822900000417
in the formula,
Figure BDA0003005782290000051
representing the i-node inertial navigation resolving height;
Figure BDA0003005782290000052
representing the i-node inertial navigation resolving latitude;
Figure BDA0003005782290000053
representing the i-node inertial navigation resolving longitude,
Figure BDA0003005782290000054
the latitude error estimated for the inode,
Figure BDA0003005782290000055
for the longitude error estimated for the inode,
Figure BDA0003005782290000056
the estimated height error for the inode.
In a second aspect, a cluster networking collaborative navigation system based on constrained kalman filtering is provided, the system includes:
module M1: determining the number of cluster networking nodes, and measuring the relative distance between the nodes;
module M2: establishing a cluster networking collaborative navigation filtering equation based on the measured relative distance between the nodes;
module M3: designing inertial navigation position error weighting and minimum constraint conditions of each node of a cluster networking;
module M4: constructing constraint Kalman filtering of cluster networking collaborative navigation;
module M5: and compensating pure inertial navigation errors by using the inertial navigation error estimation values of each node estimated by the constraint Kalman filtering.
Preferably, the cluster networking collaborative navigation filtering equation in the module M2 includes a state equation, where the state equation specifically is:
X(t)＝F(t)X(t)+G(t)W(t)
in the formula,
Figure BDA0003005782290000057
respectively representing three-axis position errors of each node in the cluster under a geographic coordinate system, wherein n represents the number of the cluster nodes, F (t) represents a state transition matrix, G (t) represents a system noise coefficient matrix, and W (t) represents a system noise matrix;
wherein,
Figure BDA0003005782290000058
Figure BDA0003005782290000059
in the formula, Ve iRepresenting the east speed of the inode; l isiRepresenting the latitude of the inode; h isiRepresenting the height of the inode; rNRepresenting the curvature radius of the earth fourth-unit circle; rMRepresents the radius of curvature of the earth's meridian; vn iRepresenting the i-node northbound speed.
Preferably, the cluster networking collaborative navigation filtering equation in the module M2 further includes a measurement equation, where the measurement equation specifically is:
L(t)＝HX(t)+Δ
wherein L (t) is an (n-1) × 1-dimensional observation quantity matrix, n-1 is the number of observation quantities, i.e., the number of distance measurement values, H is an (n-1) × 3 n-dimensional observation coefficient matrix, and Δ is an (n-1) × 1-dimensional observation noise matrix;
wherein,
Figure BDA0003005782290000061
Figure BDA0003005782290000062
in the formula,
Figure BDA0003005782290000063
the measured distance between node i and node j in the cluster,
Figure BDA0003005782290000064
(i, j ≠ 1, …, n, and i ≠ j) is an inertial navigation calculation distance between the node i and the node j, and L changes in real time along with the change of the number of the missile group nodes n.
Compared with the prior art, the invention has the following beneficial effects:
the method is independent of other navigation equipment, only utilizes networking ranging among nodes in the cluster, designs networking geometric constraint conditions, constructs a state equation and a measurement equation based on the constraint conditions, designs a new Kalman filter, and carries out real-time recursive estimation on inertial navigation errors to correct the inertial navigation system so as to inhibit pure inertial navigation error divergence of each node.
Drawings
Other features, objects and advantages of the invention will become more apparent upon reading of the detailed description of non-limiting embodiments with reference to the following drawings:
FIG. 1 is a flowchart of a cluster networking collaborative navigation method based on constraint Kalman filtering according to an embodiment of the present invention;
FIG. 2 is a diagram of a constrained Kalman filter update process.
Detailed Description
The present invention will be described in detail with reference to specific examples. The following examples will assist those skilled in the art in further understanding the invention, but are not intended to limit the invention in any way. It should be noted that it would be obvious to those skilled in the art that various changes and modifications can be made without departing from the spirit of the invention. All falling within the scope of the present invention.
The embodiment of the invention provides a cluster networking collaborative navigation method based on constraint Kalman filtering, aiming at the problem of long-term accumulation of inertial navigation errors, the networking ranging between nodes in a cluster is utilized, the networking constraint condition is designed, the constraint Kalman filtering is constructed, the inertial navigation errors are estimated, and the pure inertial navigation error divergence is restrained. The scheme is as follows: referring to fig. 1 and 2, first, the number of nodes of the cluster networking is determined, and the relative distance between the two nodes is measured.
In the embodiment of the invention, the nodes participating in networking are determined according to the geometric configuration, the distance measurement among all the nodes in the networking is realized through means such as a data link, a millimeter wave radar and the like, and the distance measurement is used as the measurement distance among all the nodes in the collaborative navigation and is used for distance measurement networking.
Secondly, establishing a collaborative navigation filtering equation of the cluster networking, which comprises a state equation and a measurement equation, wherein the collaborative navigation filtering state equation:
X(t)＝F(t)X(t)+G(t)W(t)
in the formula,
Figure BDA0003005782290000071
three-axis position error of each node in the cluster in the geographic coordinate system, n is the number of cluster nodes, F (t) is a state transition matrix, G (t) is a system noise coefficient matrix,w (t) is a system noise matrix;
wherein,
Figure BDA0003005782290000072
Figure BDA0003005782290000073
in the formula, Ve iRepresenting the east speed of the inode; l isiRepresenting the latitude of the inode; h isiRepresenting the height of the inode; rNRepresenting the curvature radius of the earth fourth-unit circle; rMRepresents the radius of curvature of the earth's meridian; vn iRepresenting the i-node northbound speed.
The collaborative navigation filtering measurement equation specifically comprises:
L(t)＝HX(t)+Δ
wherein L (t) is an (n-1) × 1-dimensional observation quantity matrix, n-1 is the number of observation quantities, i.e., the number of distance measurement values, H is an (n-1) × 3 n-dimensional observation coefficient matrix, and Δ is an (n-1) × 1-dimensional observation noise matrix;
wherein,
Figure BDA0003005782290000074
Figure BDA0003005782290000081
in the formula,
Figure BDA0003005782290000082
the measured distance between node i and node j in the cluster,
Figure BDA0003005782290000083
(i, j ≠ 1, …, n, and i ≠ j) is an inertial navigation calculation distance between the node i and the node j, and L changes in real time along with the change of the number of the missile group nodes n.
And then, designing inertial navigation position error weighting and minimum constraint conditions of each node of the cluster network.
Specifically, in the embodiment of the present invention, the weighted sum minimum constraint condition of inertial navigation position errors of each node in the cluster networking is:
GTPxDaX＝0
in the formula, GTIs formed by HTA 6 x 3 n-dimensional matrix composed of eigenvectors corresponding to the zero eigenvalue of PH, P being a diagonal weight matrix of L, HTA positive definite matrix of a diagonal weight matrix with PH of L; pxIs a 3n multiplied by 3n dimensional weight matrix of a parameter vector X to be estimated, which is determined according to the precision level of each node inertial navigation system, wherein n is the number of cluster nodes,
Figure BDA0003005782290000084
in the formula,
Figure BDA0003005782290000085
for the inertial navigation position of the cluster node i under the earth-centered earth-fixed system, PxVarying in real time as the number of cluster nodes n varies.
The transformation matrix for transforming the high errors of the latitude and longitude under the geographic coordinate system to the errors of the three X, Y, Z axes under the geocentric geostationary coordinate system is as follows:
Figure BDA0003005782290000086
Figure BDA0003005782290000087
in the formula, Rn represents the curvature radius of the earth unitary fourth of twelve earthly branches;
Figure BDA0003005782290000088
representing the i-node inertial navigation resolving height;
Figure BDA0003005782290000089
representing the i-node inertial navigation resolving latitude;
Figure BDA00030057822900000810
representing the i-node inertial navigation resolving longitude; e.g. of the type2Representing the curvature of the earth.
And constructing constraint Kalman filtering of cluster networking collaborative navigation.
In the embodiment of the invention, a 'projection method' is adopted to project the constraint condition to the traditional Kalman filtering, and the constraint Kalman filtering for constructing the cluster networking cooperative navigation is as follows:
constraint condition matrix: b isk＝GTPxDa；
And (3) state one-step prediction:
Figure BDA0003005782290000091
and (3) restraining the state after correction for one-step prediction:
Figure BDA0003005782290000092
and (3) state estimation:
Figure BDA0003005782290000093
filtering gain:
Figure BDA0003005782290000094
one-step prediction of mean square error: pk/k-1＝φk，k-1Pk-1φT k，k-1+Qk；
Estimating the mean square error: pk＝(I-KkHk)Pk/k-1；
In the formula, BkIn order to be a matrix of the constraint conditions,
Figure BDA0003005782290000095
in order to predict the state quantity matrix in one step,
Figure BDA0003005782290000096
is a previous state quantity matrix, phik，k-1In order to be a state transition matrix,
Figure BDA0003005782290000097
predicting the state quantity matrix after constraint modification, Pk-1Is a state error covariance matrix, KkFor filtering the gain matrix, ZkAs a matrix of observations, HkFor observing the coefficient matrix, Pk/k-1Predicting a state error covariance matrix, R, for one stepkTo observe the noise matrix, QkIs a state noise matrix and I is an identity matrix.
And finally, compensating the pure inertial navigation error by using the inertial navigation error estimation value of each node estimated by the constraint Kalman filtering.
In the embodiment of the invention, the inertial navigation error estimation value of each node is estimated by using the constraint Kalman filtering
Figure BDA0003005782290000098
The method for compensating the pure inertial navigation error comprises the following steps:
Figure BDA0003005782290000099
Figure BDA00030057822900000910
Figure BDA00030057822900000911
in the formula,
Figure BDA00030057822900000912
representing the i-node inertial navigation resolving height;
Figure BDA00030057822900000913
representing the i-node inertial navigation resolving latitude;
Figure BDA00030057822900000914
representing the i-node inertial navigation resolving longitude,
Figure BDA00030057822900000915
the latitude error estimated for the inode,
Figure BDA00030057822900000916
for the longitude error estimated for the inode,
Figure BDA00030057822900000917
the estimated height error for the inode.
The embodiment of the invention provides a cluster networking collaborative navigation method based on constraint Kalman filtering, which is independent of other navigation equipment, only utilizes networking ranging between nodes in a cluster, designs networking constraint conditions, constructs a state equation and a measurement equation based on the constraint conditions, designs a new Kalman filter, carries out real-time recursive estimation on inertial navigation errors to correct an inertial navigation system, inhibits the pure inertial navigation error divergence of each node, and realizes pure inertial navigation high-precision positioning.
Those skilled in the art will appreciate that, in addition to implementing the system and its various devices, modules, units provided by the present invention as pure computer readable program code, the system and its various devices, modules, units provided by the present invention can be fully implemented by logically programming method steps in the form of logic gates, switches, application specific integrated circuits, programmable logic controllers, embedded microcontrollers and the like. Therefore, the system and various devices, modules and units thereof provided by the invention can be regarded as a hardware component, and the devices, modules and units included in the system for realizing various functions can also be regarded as structures in the hardware component; means, modules, units for performing the various functions may also be regarded as structures within both software modules and hardware components for performing the method.
The foregoing description of specific embodiments of the present invention has been presented. It is to be understood that the present invention is not limited to the specific embodiments described above, and that various changes or modifications may be made by one skilled in the art within the scope of the appended claims without departing from the spirit of the invention. The embodiments and features of the embodiments of the present application may be combined with each other arbitrarily without conflict.

Claims (10)

1. A cluster networking collaborative navigation method based on constraint Kalman filtering is characterized by comprising the following steps:
step S1: determining the number of cluster networking nodes, and measuring the relative distance between the nodes;
step S2: establishing a cluster networking collaborative navigation filtering equation based on the measured relative distance between the nodes;
step S3: designing inertial navigation position error weighting and minimum constraint conditions of each node of a cluster networking;
step S4: constructing constraint Kalman filtering of cluster networking collaborative navigation;
step S5: and compensating pure inertial navigation errors by using the inertial navigation error estimation values of each node estimated by the constraint Kalman filtering.
2. The constrained kalman filter-based clustered networking collaborative navigation method according to claim 1, wherein the clustered networking collaborative navigation filter equation in the step S2 includes a state equation, and the state equation specifically is:
X(t)＝F(t)X(t)+G(t)W(t)
in the formula,
Figure FDA0003005782280000011
Figure FDA0003005782280000012
respectively representing three-axis position errors of each node in the cluster under a geographic coordinate system, wherein n represents the number of the cluster nodes, F (t) represents a state transition matrix, G (t) represents a system noise coefficient matrix, and W (t) represents a system noise matrix;
wherein,
Figure FDA0003005782280000013
Figure FDA0003005782280000014
in the formula, Ve iRepresenting the east speed of the inode; l isiRepresenting the latitude of the inode; h isiRepresenting the height of the inode; rNRepresenting the curvature radius of the earth fourth-unit circle; rMRepresents the radius of curvature of the earth's meridian; vn iRepresenting the i-node northbound speed.
3. The constrained kalman filter-based clustered networking collaborative navigation method according to claim 1, wherein the clustered networking collaborative navigation filtering equation in step S2 further includes a measurement equation, and the measurement equation specifically is:
L(t)＝HX(t)+Δ
wherein L (t) is an (n-1) × 1-dimensional observation quantity matrix, n-1 is the number of observation quantities, i.e., the number of distance measurement values, H is an (n-1) × 3 n-dimensional observation coefficient matrix, and Δ is an (n-1) × 1-dimensional observation noise matrix;
wherein,
Figure FDA0003005782280000021
Figure FDA0003005782280000022
in the formula,
Figure FDA0003005782280000023
the measured distance between node i and node j in the cluster,
Figure FDA0003005782280000024
and calculating the distance for inertial navigation between the node i and the node j, wherein L changes in real time along with the change of the number of the nodes n of the missile group.
4. The collaborative navigation method for cluster networking based on constrained kalman filtering according to claim 3, wherein the inertial navigation position error weighting and the minimum constraint condition of each node of the cluster networking in the step S3 are included in the following steps:
GTPxDaX＝0
in the formula, GTIs formed by HTA 6 x 3 n-dimensional matrix composed of eigenvectors corresponding to the zero eigenvalue of PH, P being a diagonal weight matrix of L, HTPositive definite matrix of diagonal weight matrix with PH L, PxIs a 3n multiplied by 3n dimensional weight matrix of a parameter vector X to be estimated, which is determined according to the precision level of each node inertial navigation system, wherein n is the number of cluster nodes,
Figure FDA0003005782280000025
in the formula,
Figure FDA0003005782280000026
for the inertial navigation position of the cluster node i under the earth-centered earth-fixed system, PxVarying in real time as the number of cluster nodes n varies.
5. The collaborative navigation method for cluster networking based on constrained kalman filtering according to claim 4, wherein in step S3, the inertial navigation position error weighting and the minimum constraint condition of each node of the cluster networking are obtained by transforming the latitude and longitude high error in the geographic coordinate system to the XYZ triaxial error in the geocentric geostationary coordinate system by using a transformation matrix:
Figure FDA0003005782280000031
Figure FDA0003005782280000032
in the formula, Rn represents the curvature radius of the earth unitary fourth of twelve earthly branches;
Figure FDA0003005782280000033
representing the i-node inertial navigation resolving height;
Figure FDA0003005782280000034
representing the i-node inertial navigation resolving latitude;
Figure FDA0003005782280000035
representing the i-node inertial navigation resolving longitude; e.g. of the type2Representing the curvature of the earth.
6. The constraint Kalman filtering-based trunking networking collaborative navigation method according to claim 1, wherein in the step S4, the constraint condition is projected to the traditional Kalman filtering by a projection method, and the constraint Kalman filtering for constructing trunking networking collaborative navigation is as follows:
constraint condition matrix: b isk＝GTPxDa；
And (3) state one-step prediction:
Figure FDA0003005782280000036
and (3) restraining the state after correction for one-step prediction:
Figure FDA0003005782280000037
and (3) state estimation:
Figure FDA0003005782280000038
filtering gain:
Figure FDA0003005782280000039
one-step prediction of mean square error:
Figure FDA00030057822800000310
estimating the mean square error: pk＝(I-KkHk)Pk/k-1；
In the formula, BkIn order to be a matrix of the constraint conditions,
Figure FDA00030057822800000311
in order to predict the state quantity matrix in one step,
Figure FDA00030057822800000312
is a previous state quantity matrix, phik，k-1In order to be a state transition matrix,
Figure FDA00030057822800000313
predicting the state quantity matrix after constraint modification, Pk-1Is a state error covariance matrix, KkFor filtering the gain matrix, ZkAs a matrix of observations, HkFor observing the coefficient matrix, Pk/k-1Predicting a state error covariance matrix, R, for one stepkTo observe the noise matrix, QkIs a state noise matrix and I is an identity matrix.
7. The collaborative navigation method for cluster networking based on constrained Kalman filtering according to claim 6, wherein the step S5 is implemented by using inertial navigation error estimation values of nodes estimated by constrained Kalman filtering
Figure FDA00030057822800000314
The method for compensating the pure inertial navigation error comprises the following steps:
Figure FDA00030057822800000315
Figure FDA0003005782280000041
Figure FDA0003005782280000042
in the formula,
Figure FDA0003005782280000043
representing the i-node inertial navigation resolving height;
Figure FDA0003005782280000044
representing the i-node inertial navigation resolving latitude;
Figure FDA0003005782280000045
representing the i-node inertial navigation resolving longitude,
Figure FDA0003005782280000046
the latitude error estimated for the inode,
Figure FDA0003005782280000047
for the longitude error estimated for the inode,
Figure FDA0003005782280000048
the estimated height error for the inode.
8. A cluster networking collaborative navigation system based on constraint Kalman filtering is characterized by comprising:
module M1: determining the number of cluster networking nodes, and measuring the relative distance between the nodes;
module M2: establishing a cluster networking collaborative navigation filtering equation based on the measured relative distance between the nodes;
module M3: designing inertial navigation position error weighting and minimum constraint conditions of each node of a cluster networking;
module M4: constructing constraint Kalman filtering of cluster networking collaborative navigation;
module M5: and compensating pure inertial navigation errors by using the inertial navigation error estimation values of each node estimated by the constraint Kalman filtering.
9. The constraint kalman filter-based trunking networking collaborative navigation system according to claim 8, wherein the trunking networking collaborative navigation filter equations in the module M2 include state equations, and the state equations are specifically:
X(t)＝F(t)X(t)+G(t)W(t)
in the formula,
Figure FDA0003005782280000049
Figure FDA00030057822800000410
respectively representing three-axis position errors of each node in the cluster under a geographic coordinate system, wherein n represents the number of the cluster nodes, F (t) represents a state transition matrix, G (t) represents a system noise coefficient matrix, and W (t) represents a system noise matrix;
wherein,
Figure FDA00030057822800000411
Figure FDA00030057822800000412
in the formula, Ve iRepresenting the east speed of the inode; l isiRepresenting the latitude of the inode; h isiRepresenting the height of the inode; rNRepresenting the curvature radius of the earth fourth-unit circle; rMRepresents the radius of curvature of the earth's meridian; vn iRepresenting the i-node northbound speed.
10. The constraint Kalman filtering-based trunking networking collaborative navigation system according to claim 6, wherein the clustering networking collaborative navigation filtering equation in the module M2 further comprises a measurement equation, and the measurement equation is specifically:
L(t)＝HX(t)+Δ
wherein L (t) is an (n-1) × 1-dimensional observation quantity matrix, n-1 is the number of observation quantities, i.e., the number of distance measurement values, H is an (n-1) × 3 n-dimensional observation coefficient matrix, and Δ is an (n-1) × 1-dimensional observation noise matrix;
wherein,
Figure FDA0003005782280000051
Figure FDA0003005782280000052
in the formula,
Figure FDA0003005782280000053
the measured distance between node i and node j in the cluster,
Figure FDA0003005782280000054
and calculating the distance for inertial navigation between the node i and the node j, wherein L changes in real time along with the change of the number of the nodes n of the missile group.
CN202110361651.3A 2021-04-02 2021-04-02 Cluster networking collaborative navigation method and system based on constraint Kalman filtering Active CN113175931B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110361651.3A CN113175931B (en) 2021-04-02 2021-04-02 Cluster networking collaborative navigation method and system based on constraint Kalman filtering

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110361651.3A CN113175931B (en) 2021-04-02 2021-04-02 Cluster networking collaborative navigation method and system based on constraint Kalman filtering

Publications (2)

Publication Number Publication Date
CN113175931A true CN113175931A (en) 2021-07-27
CN113175931B CN113175931B (en) 2022-08-16

Family

ID=76923105

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110361651.3A Active CN113175931B (en) 2021-04-02 2021-04-02 Cluster networking collaborative navigation method and system based on constraint Kalman filtering

Country Status (1)

Country Link
CN (1) CN113175931B (en)

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114279466A (en) * 2021-12-23 2022-04-05 中国电子科技集团公司第十四研究所 Sensor error correction method
CN114964228A (en) * 2022-05-05 2022-08-30 上海机电工程研究所 Inertial navigation cooperation method based on rank deficiency constraint cascade filtering
CN116147429A (en) * 2023-03-10 2023-05-23 中国电子科技集团公司第二十六研究所 Aided projectile navigation method based on data chain and radar
CN116401618A (en) * 2023-03-03 2023-07-07 南京航空航天大学 Cross-domain unmanned cluster collaborative navigation information fusion method based on geometric distribution sampling

Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102901514A (en) * 2012-09-25 2013-01-30 北京航空航天大学 Collaborative initial alignment method based on multiple-inertia-unit informational constraint
CN109471102A (en) * 2018-10-23 2019-03-15 湖北航天技术研究院总体设计所 A kind of used grouping error modification method
CN109471103A (en) * 2018-10-23 2019-03-15 湖北航天技术研究院总体设计所 A kind of missile-borne Bistatic SAR data fusion positioning error correcting method
CN110207691A (en) * 2019-05-08 2019-09-06 南京航空航天大学 A kind of more unmanned vehicle collaborative navigation methods based on data-link ranging
CN110208792A (en) * 2019-06-26 2019-09-06 哈尔滨工业大学 The arbitrary line constraint tracking of dbjective state and trajectory parameters is estimated simultaneously
CN110285804A (en) * 2019-06-26 2019-09-27 南京航空航天大学 Vehicle collaborative navigation method based on the constraint of relative motion model
CN110988880A (en) * 2019-12-12 2020-04-10 南京莱斯电子设备有限公司 Geographic information extraction and target tracking method based on SMR target track
CN111221018A (en) * 2020-03-12 2020-06-02 南京航空航天大学 GNSS multi-source information fusion navigation method for inhibiting marine multipath

Patent Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102901514A (en) * 2012-09-25 2013-01-30 北京航空航天大学 Collaborative initial alignment method based on multiple-inertia-unit informational constraint
CN109471102A (en) * 2018-10-23 2019-03-15 湖北航天技术研究院总体设计所 A kind of used grouping error modification method
CN109471103A (en) * 2018-10-23 2019-03-15 湖北航天技术研究院总体设计所 A kind of missile-borne Bistatic SAR data fusion positioning error correcting method
CN110207691A (en) * 2019-05-08 2019-09-06 南京航空航天大学 A kind of more unmanned vehicle collaborative navigation methods based on data-link ranging
CN110208792A (en) * 2019-06-26 2019-09-06 哈尔滨工业大学 The arbitrary line constraint tracking of dbjective state and trajectory parameters is estimated simultaneously
CN110285804A (en) * 2019-06-26 2019-09-27 南京航空航天大学 Vehicle collaborative navigation method based on the constraint of relative motion model
CN110988880A (en) * 2019-12-12 2020-04-10 南京莱斯电子设备有限公司 Geographic information extraction and target tracking method based on SMR target track
CN111221018A (en) * 2020-03-12 2020-06-02 南京航空航天大学 GNSS multi-source information fusion navigation method for inhibiting marine multipath

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
PEYMAN HADI MOHAMMADABADI: "Cooperative Node Positioning In Vehicular Networks Using Inter-Node Distance Measurements", 《2014 IEEE 25TH INTERNATIONAL SYMPOSIUM ON PERSONAL, INDOOR AND MOBILE RADIO COMMUNICATIONS》 *
刘俊成: "基于相互测距信息的机群组网协同定位技术", 《北 京 航 空 航 天 大 学 学 报》 *
张京娟: "基于飞行器三维组网测距信息的导航误差估计技术研究", 《空天防御》 *

Cited By (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114279466A (en) * 2021-12-23 2022-04-05 中国电子科技集团公司第十四研究所 Sensor error correction method
CN114279466B (en) * 2021-12-23 2024-02-27 中国电子科技集团公司第十四研究所 Sensor error correction method
CN114964228A (en) * 2022-05-05 2022-08-30 上海机电工程研究所 Inertial navigation cooperation method based on rank deficiency constraint cascade filtering
CN116401618A (en) * 2023-03-03 2023-07-07 南京航空航天大学 Cross-domain unmanned cluster collaborative navigation information fusion method based on geometric distribution sampling
CN116401618B (en) * 2023-03-03 2023-12-01 南京航空航天大学 Cross-domain unmanned cluster collaborative navigation information fusion method based on geometric distribution sampling
CN116147429A (en) * 2023-03-10 2023-05-23 中国电子科技集团公司第二十六研究所 Aided projectile navigation method based on data chain and radar
CN116147429B (en) * 2023-03-10 2024-07-19 中国电子科技集团公司第二十六研究所 Aided projectile navigation method based on data chain and radar

Also Published As

Publication number Publication date
CN113175931B (en) 2022-08-16

Similar Documents

Publication Publication Date Title
CN113175931B (en) Cluster networking collaborative navigation method and system based on constraint Kalman filtering
CN110487301B (en) Initial alignment method of radar-assisted airborne strapdown inertial navigation system
Li et al. A robust graph optimization realization of tightly coupled GNSS/INS integrated navigation system for urban vehicles
US8452536B2 (en) Method of definition of a navigation system
CN111337020A (en) Factor graph fusion positioning method introducing robust estimation
CN111156987A (en) Inertia/astronomical combined navigation method based on residual compensation multi-rate CKF
CN102353378B (en) Adaptive federal filtering method of vector-form information distribution coefficients
CN109883426A (en) Dynamic allocation and correction multi-sources Information Fusion Method based on factor graph
CN111337029B (en) Polarized light inertia tight combination navigation method based on self-learning multi-rate residual error correction
CN109631883B (en) Method for accurately estimating local attitude of aircraft based on node information sharing
CN109507706B (en) GPS signal loss prediction positioning method
CN108931799A (en) Train combined positioning method and system based on the search of recurrence fast orthogonal
CN112525188B (en) Combined navigation method based on federal filtering
CN114264301B (en) Vehicle-mounted multi-sensor fusion positioning method, device, chip and terminal
CN112346104A (en) Unmanned aerial vehicle information fusion positioning method
CN111220151B (en) Inertia and milemeter combined navigation method considering temperature model under load system
CN115711616B (en) Smooth positioning method and device for indoor and outdoor traversing unmanned aerial vehicle
CN114689047A (en) Deep learning-based integrated navigation method, device, system and storage medium
Wang et al. An adaptive federated filter based on variational Bayes with application to multisource navigation
Taghizadeh et al. A low-cost integrated navigation system based on factor graph nonlinear optimization for autonomous flight
CN117824632A (en) Gravity matching secondary filtering method and device based on noise distribution assumption
CN113434806A (en) Robust adaptive multi-model filtering method
CN117419723A (en) Interactive model self-adaptive integrated navigation method based on factor graph
CN108981689B (en) UWB/INS combined navigation system based on DSP TMS320C6748
CN111982126A (en) Design method of full-source BeiDou/SINS elastic state observer model

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