Interactive multi-model factor graph integrated navigation method based on tight coupling attitude constraint

Abstract

The embodiment of the invention provides an interactive multi-model factor graph integrated navigation method based on tight coupling attitude constraint. In a specific embodiment, the method comprises the following steps: constructing an integrated navigation system; constructing an inertial measurement unit factor pre-integrated by the inertial navigation system; constructing a direct celestial navigation factor and a refraction celestial navigation factor which are respectively and tightly coupled with an inertial measurement unit factor based on the star sensor observation condition of the celestial navigation system; constructing a horizontal radio navigation factor and a vertical radio navigation factor based on the position information of the land-based radio navigation system; acquiring height measurement data of the barometric altimeter to construct barometric altimeter factors; dynamically adjusting the weight ratio of the height measurement data of the vertical radio navigation factor to the height measurement data of the barometric altimeter factor through an interactive multi-model algorithm; and carrying out global state estimation of the integrated navigation system on a factor graph framework formed by the factors.

Description

Technical Field

[0001] This invention relates to the field of aircraft navigation technology, and more specifically to an interactive multi-model factor graph integrated navigation method based on tightly coupled attitude constraints. Background Technology

[0002] Currently, with the increasing complexity of flight missions, ensuring accurate positioning of aircraft in situations where GPS signals are unavailable or unreliable (such as satellite denial scenarios) has become a pressing issue. Existing single navigation methods such as Inertial Navigation Systems (INS), Ground-based Radio Navigation Systems (RNS), Celestial Navigation Systems (CNS), and Barometric Altimeters (BA) each have their advantages and disadvantages, but none can independently meet the requirements for high-precision positioning. Therefore, a combined navigation system capable of effectively fusing information from multiple sensors is needed to improve positioning accuracy. However, traditional multi-source information fusion navigation methods suffer from difficulties in data synchronization and error accumulation when processing sequential measurement information from distributed navigation sensors. Furthermore, these methods typically employ loosely coupled models for adaptive optimization estimation of the navigation state, failing to fundamentally address the error problem caused by the time-varying zero bias of inertial devices. Summary of the Invention

[0003] The purpose of this disclosure is to provide an interactive multi-model factor graph integrated navigation method based on tightly coupled attitude constraints that can significantly improve the attitude and position accuracy of an aircraft in a satellite-denied environment, thereby solving at least one of the problems existing in the prior art.

[0004] To achieve the above objectives, the present disclosure adopts the following technical solution:

[0005] The first aspect of this disclosure provides an interactive multi-model factor graph integrated navigation method based on tightly coupled attitude constraints, the method comprising:

[0006] Construct a combined navigation system, which includes an inertial navigation system, an astronomical navigation system, a land-based radio navigation system, and a barometric altimeter;

[0007] Construct the pre-integrated inertial measurement unit factor for the inertial navigation system;

[0008] Based on the star-sensitive observations of the astronomical navigation system, direct astronomical navigation factors and refractional astronomical navigation factors are constructed and tightly coupled with the inertial measurement unit factors, respectively. The direct astronomical navigation factors and refractional astronomical navigation factors are used to correct the attitude information output by the inertial navigation system.

[0009] Horizontal and vertical radio navigation factors are constructed based on the position information of the land-based radio navigation system. These factors are used to correct errors in the position information output by the inertial navigation system.

[0010] Altitude measurement data from a barometric altimeter are acquired to construct a barometric altimeter factor, which is used to suppress altitude errors in land-based radio navigation systems.

[0011] The weight ratio of the altitude measurement data of the vertical radio navigation factor and the altitude measurement data of the barometric altimeter factor is dynamically adjusted by an interactive multi-model algorithm. The weight ratio is used to correct the error of the altitude information output by the inertial navigation system.

[0012] Global state estimation of a combined navigation system is performed using a factor graph framework consisting of inertial measurement unit factors, direct astronomical navigation factors, refracted astronomical navigation factors, horizontal radio navigation factors, vertical radio navigation factors, and barometric altimeter factors.

[0013] A second aspect of the present invention provides a computer device including a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor, when executing the program, implements the method as described in any one of the first aspects.

[0014] The beneficial effects of this disclosure are as follows:

[0015] The attitude-constrained interactive multi-model factor graph integrated navigation method provided in this invention is applicable to long-endurance high-speed aircraft. Compared with traditional Kalman filtering and unconstrained factor graph algorithms, this method effectively improves the attitude and positioning accuracy of the navigation system. By introducing direct astronomical navigation factors and refractional astronomical navigation factors with different star-sensor observation information, and tightly coupling them with inertial measurement unit factors, this method solves the problem of navigation and positioning error divergence caused by inaccurate zero-bias estimation of inertial devices, thus improving the attitude estimation accuracy of integrated navigation. At the same time, to address the problem that altitude errors are difficult to converge due to spatial constraints in radio navigation, an interactive multi-model algorithm is introduced with barometric altimeter factors and vertical radio navigation factors to suppress altitude estimation errors, thereby improving the position estimation accuracy of integrated navigation. Attached Figure Description

[0016] The specific embodiments of this disclosure will be described in further detail below with reference to the accompanying drawings.

[0017] Figure 1 This is a flowchart illustrating the overall execution of the present invention.

[0018] Figure 2 This is a schematic diagram of the overall execution structure of the present invention.

[0019] Figure 3 This is a schematic diagram of the inertial navigation system, astronomical navigation system, land-based radio navigation system, and barometric altimeter navigation principle based on factor graphs of the present invention.

[0020] Figure 4 This is a schematic diagram illustrating the principle of direct star vector observation in this invention.

[0021] Figure 5 This diagram illustrates the relationship between the starlight refraction altitude, apparent altitude, and the spacecraft and the Earth's center in this invention.

[0022] Figure 6 This is a schematic diagram of the interactive multi-model algorithm proposed in this invention.

[0023] Figure 7 A schematic diagram of the structure of a computer system for implementing the apparatus provided in the embodiments of this disclosure is shown. Detailed Implementation

[0024] To more clearly illustrate this disclosure, the following description, in conjunction with embodiments and accompanying drawings, provides further insight. Similar components in the drawings are indicated by the same reference numerals. Those skilled in the art should understand that the specific description below is illustrative rather than restrictive and should not be construed as limiting the scope of protection of this disclosure.

[0025] The first embodiment of the present invention provides an interactive multi-model factor graph integrated navigation method based on tightly coupled attitude constraints, such as... Figure 1 and Figure 2 As shown, the method includes:

[0026] Step S10: Construct an integrated navigation system, which includes an inertial navigation system, an astronomical navigation system, a land-based radio navigation system, and a barometric altimeter; specifically, such as... Figure 3 As shown, a navigation system for a long-endurance aircraft is constructed, consisting of an inertial navigation system, an astronomical navigation system, a land-based radio navigation system, and a barometric altimeter; the main structure of this system within the factor graph framework is composed of the pre-integration of the inertial navigation system.

[0027] Step S20: Construct the inertial measurement unit (IMU) factor for pre-integration of the inertial navigation system; the inertial measurement unit factor is the factor obtained after error compensation of the inertial measurement unit;

[0028] Step S30: Based on the star-sensitive observation of the celestial navigation system, construct the Direct Celestial Navigation System (TDCNS) factor and the Refraction Celestial Navigation System (TRCNS) factor, which are tightly coupled with the inertial measurement unit factor, respectively. By establishing the direct star vector observation model and the refraction apparent altitude observation model, the direct celestial navigation factor and the refraction celestial navigation factor are used to correct the attitude information output by the inertial navigation system. That is, the zero bias and attitude measurement error of the inertial device of the inertial navigation system are corrected by the direct celestial navigation factor and the refraction celestial navigation factor, which are tightly coupled with the inertial measurement unit factor, respectively.

[0029] Specifically, the attitude constraint of the aircraft navigation system is achieved by tightly coupling the direct astronomical navigation factor with the inertial measurement unit factor, and the attitude constraint of the through astronomical navigation factor is achieved by tightly coupling the refracted astronomical navigation factor with the inertial measurement unit factor.

[0030] Step S40: Based on the ground base station ranging and positioning principle of the land-based radio navigation system, specifically, based on the position information of the land-based radio navigation system, construct the Horizontal Radio Navigation System (HRNS) factor and the Vertical Radio Navigation System (VRNS) factor. By establishing a loosely coupled observation model of radio navigation and inertial navigation, the HRNS factor and the VRNS factor can be used to correct the error of the position information output by the inertial navigation system, that is, to correct the position measurement error of the inertial navigation system.

[0031] Step S50: Because the deployment of ground base stations for the land-based radio navigation system is constrained by geometric space, terrain, communication distance, etc., the error is not easy to converge and the altitude measurement data is inaccurate and the error is large; therefore, the altitude measurement data of the barometric altimeter is obtained to construct the barometric altimeter (BA) factor. The barometric altimeter factor is used to suppress the altitude error of the land-based radio navigation system, that is, to correct the altitude measurement error of the land-based radio navigation system.

[0032] Step S60: The weight ratio of the altitude measurement data of the vertical radio navigation factor and the altitude measurement data of the barometric altimeter factor is dynamically adjusted through the Interacting Multiple Model (IMM) algorithm to achieve adaptive optimization. The main process is input interaction, factor graph fusion optimization, probability update and interactive output. This weight ratio is used to correct the error of the altitude information output by the inertial navigation system.

[0033] Step S70: Perform global state estimation of the integrated navigation system using a factor map framework composed of inertial measurement unit factors, direct astronomical navigation factors, refracted astronomical navigation factors, horizontal radio navigation factors, vertical radio navigation factors, and barometric altimeter factors, thereby obtaining the attitude and position of the integrated navigation system to be estimated.

[0034] The attitude-constrained interactive multi-model factor graph integrated navigation method provided in this invention is applicable to long-endurance high-speed aircraft. Compared with traditional Kalman filtering and unconstrained factor graph algorithms, this method effectively improves the attitude and positioning accuracy of the navigation system. By introducing direct astronomical navigation factors and refractional astronomical navigation factors with different star-sensor observation information, and tightly coupling them with inertial measurement unit factors, this method solves the problem of navigation and positioning error divergence caused by inaccurate zero-bias estimation of inertial devices, thus improving the attitude estimation accuracy of integrated navigation. At the same time, to address the problem that altitude errors are difficult to converge due to spatial constraints in radio navigation, an interactive multi-model algorithm is introduced with barometric altimeter factors and vertical radio navigation factors to suppress altitude estimation errors, thereby improving the position estimation accuracy of integrated navigation.

[0035] In one possible implementation, the inertial measurement unit factors include attitude residuals, velocity residuals, position residuals, gyroscope residuals, and accelerometer residuals.

[0036] In one possible implementation, the inertial measurement unit factor for pre-integration of the inertial navigation system includes:

[0037] The output of the gyroscope in the inertial measurement unit after error compensation is modeled:

[0038]

[0039] in, This is the actual output of the gyroscope; Let be the angle increment at time k; The ideal output of the gyroscope; Δθ k-1 Δt is the angular increment at time k-1; Δt is the time interval of the gyroscope error compensation interval.

[0040] The output of the accelerometer in the inertial measurement unit after error compensation is modeled:

[0041]

[0042] in, This is the actual output of the accelerometer; Δv k =f b Δt is the velocity increment at time k; Δv k-1 f is the velocity increment at time k-1; b This is the ideal output of the accelerometer;

[0043] The pre-integration interval of the inertial measurement unit is set to t. i ~t j The update interval of the inertial measurement unit is t. k-1 ~t k Specifically, N = ji represents the update interval width of the pre-integration of the selected inertial measurement unit;

[0044] Construct the attitude residual, velocity residual, position residual, gyroscope residual, and accelerometer residual of the inertial measurement unit, where:

[0045] Considering the Earth's rotation and the actual deviation in the estimation, the attitude residual of the inertial measurement unit is constructed. include:

[0046]

[0047] in, This is the attitude pre-integral quantity in the carrier coordinate system based on the gyroscope output; Let be the transformation matrix from the navigation coordinate system to the vehicle coordinate system at time j; This is the attitude pre-integral quantity in the navigation coordinate system based on the gyroscope output; Let be the transformation matrix from the navigation coordinate system to the vehicle coordinate system at time i;

[0048] in, This refers to the attitude pre-integral quantity in the carrier coordinate system (b-frame) based on the gyroscope output; To and The relevant Jacobian matrix; due to the actual deviation of the inertial measurement unit being controlled by a small amount δb during pre-integration. i =(δb ai ,δb gi The influence of this, thus estimating the actual deviation b of the gyroscope and accelerometer. i =(b ai ,b gi )for: and It is the random zero bias of the gyroscope and accelerometer; Let be the transformation matrix from the navigation coordinate system (n-frame) to the vehicle coordinate system (b-frame) at time j.

[0049] Constructing the velocity residual of the inertial measurement unit include:

[0050]

[0051] in, This represents the velocity at time i in the navigation coordinate system (n-frame); This represents the velocity at time j in the navigation coordinate system (n-frame).

[0052] This represents the velocity increment from time i to time j in the navigation coordinate system due to gyroscope error. This is the attitude pre-integral quantity from time i to time k in the navigation coordinate system based on the gyroscope output. Let be the attitude transformation matrix from time k to time i in the vehicle coordinate system. Let k be the specific force actually output by the accelerometer in the carrier coordinate system at time k. For the random zero bias of the accelerometer, For the random zero bias of the gyroscope, δb gi δb is a small first-order quantity representing the actual deviation of the gyroscope in the inertial measurement unit during the pre-integration interval. ai This is a small first-order quantity representing the actual deviation of the accelerometer in the inertial measurement unit during the pre-integration interval; This represents the attitude conversion error matrix caused by the small first-order deviation of the inertial measurement unit during the pre-integration interval.

[0053] Constructing the position residual of the inertial measurement unit include:

[0054]

[0055] in, This represents the position at time i in the navigation coordinate system (n-frame); This indicates the position at time j in the navigation coordinate system (n-frame); Let Δt be the position increment caused by gyroscope error from time i to time j in the navigation coordinate system. i,j This represents the time interval of the pre-integration interval.

[0056] Gyroscope residuals for constructing inertial measurement units include:

[0057]

[0058] Among them, b gjIt is the random zero bias of the gyroscope at time j, b gi It is the random zero bias of the gyroscope at time i;

[0059] Accelerometer residuals for constructing inertial measurement units for:

[0060]

[0061] Among them, b aj It is the random zero bias of the accelerometer at time j, b ai It is the random zero bias of the accelerometer at time i;

[0062] Construct the inertial measurement unit factor nodes for pre-integration of the inertial measurement unit, and define the inertial measurement unit residual function. for:

[0063]

[0064] For the pre-integrated measurements of the inertial measurement units from i to j, x ij Let i be the state from i to j before integration.

[0065] In one possible implementation, step S30, constructing a direct astronomical navigation factor tightly coupled with the inertial measurement unit factor based on star-sensitive observations of the astronomical navigation system, includes:

[0066] like Figure 4 and Figure 5 As shown, after star map identification, the star data observed by the star sensor are divided into two cases: direct stars and refracted stars. Based on the actual working scenario of the spacecraft, direct astronomical navigation system factors for direct star vector observation and refracted star apparent altitude observation are constructed respectively.

[0067] Step S311: Obtain the unit star vector based on star-sensitive observations from the astronomical navigation system;

[0068] Step S312: Construct the direct star vector based on the unit star vector, and observe the tightly coupled measurement model with the inertial measurement unit at time k as follows:

[0069]

[0070] in, This represents the difference between the inertial measurement unit factor and the direct unit star vector output by the astronomical navigation system, serving as the measurement update for the direct astronomical navigation system. Let be the coordinates of the unit star vector at time k in the inertial coordinate system (i-frame); Let be the coordinates of the unit star vector at time k in the star-sensitive coordinate system (s-frame); This is the coordinate transformation matrix from the carrier coordinate system (b system) to the inertial coordinate system, which can be obtained from the attitude information output by the inertial measurement unit; Design matrix for known star-sensor mounting; As a measurement function for direct astronomical navigation systems; As measurement noise in direct astronomical navigation systems;

[0071] Step S313: Define the direct astronomical navigation factor node f according to the tightly coupled measurement model. TDCNS (x k )for:

[0072]

[0073] Step S314: Calculate the residuals of the direct astronomical navigation factor nodes according to the definition of the direct astronomical navigation factor nodes. for:

[0074]

[0075] In one possible implementation, step S3, constructing a refractive astronomical navigation factor tightly coupled with the inertial measurement unit factor based on star-sensitive observations of the astronomical navigation system, includes:

[0076] Step S321: Establish a mathematical model for the apparent altitude of refracted starlight based on the relationship between the starlight refraction altitude and the starlight line-of-sight altitude within the atmosphere and the spacecraft and the Earth's center, respectively:

[0077]

[0078] Where, r p Represents the position vector from the Earth's center to the spacecraft; u represents the unit star vector measured by the star sensor after starlight is refracted through the atmosphere; R c The radius of curvature of the Earth corresponding to the lowest point C of the starlight line of sight can be compared with the major axis radius R of the Earth's reference ellipsoid. e Approximate transformation, denoted as R c =R e (1+esin 2 L), R e It is approximately equal to 6,378,137 m, where e is the curvature of the Earth's reference ellipsoid, and L is the altitude corresponding to the lowest point C of the starlight line of sight AC in the inertial coordinate system.

[0079] Step S322: Based on the mathematical model, establish the star-sensitive observation model for the apparent altitude of refracted starlight as follows:

[0080]

[0081] in, The apparent altitude of the refracted starlight is calculated from star-sensor observation data and attitude and position information output by the inertial measurement unit. Let be the coordinate transformation matrix from the carrier coordinate system (b-frame) to the inertial coordinate system. Attitude information can be obtained from the output of the inertial measurement unit; Design matrix for known star-sensor mounting; The aircraft position vector is calculated by the inertial measurement unit; The design and installation location of the star sensor within the inertial measurement unit; R e The major axis radius R of the Earth reference ellipsoid e e represents the curvature of the Earth's reference ellipsoid; L represents the altitude corresponding to the lowest point C of the starlight line-of-sight ray AC in the inertial coordinate system.

[0082] Step S323: Based on the apparent altitude of the refracted star light, construct the tightly coupled measurement model of the refracted star vector observation at time k with the inertial measurement unit as follows:

[0083]

[0084] in, This represents the difference between the inertial measurement unit factor and the apparent altitude of the refracted starlight output by the astronomical navigation system, serving as a measurement update for the refracted astronomical navigation system. As a measurement function of a refracting astronomical navigation system; As a form of measurement noise in refracting astronomical navigation systems;

[0085] Step S324: Define the refraction astronomical navigation factor node f according to the tightly coupled measurement model. TRCNS (x k )for:

[0086]

[0087] Step S325: Calculate the residuals of the refraction astronomical navigation factor nodes according to the definition of the refraction astronomical navigation factor nodes. for:

[0088]

[0089] In one possible implementation, the specific steps in step S40 for constructing the horizontal and vertical radio navigation factors based on the location information of the land-based radio navigation system are as follows:

[0090] Step S401: Based on the position information of the land-based radio navigation system, construct loosely coupled measurement models with the inertial measurement unit at time k in both the horizontal and vertical directions, respectively:

[0091]

[0092] in, This represents the difference between the inertial measurement unit factor and the longitude L and latitude λ output by the ground-based radio navigation system, serving as the measurement update for the ground-based radio navigation system in the horizontal direction. This represents the difference between the inertial measurement unit factor and the altitude h output by the ground-based radio navigation system, serving as the measurement update in the vertical direction of the ground-based radio navigation system. and These serve as the horizontal and vertical measurement functions for land-based radio navigation systems, respectively. and These serve as the horizontal and vertical measurement noise for land-based radio navigation systems, respectively.

[0093] Step S402: Define the horizontal radio navigation factor node f according to the measurement model. HRNS (x k ) and vertical radio navigation factor node f VRNS (x k )for:

[0094]

[0095] It should be noted that ||·|| refers to the calculation of Mahalanobis distance, which includes a covariance matrix and considers the statistical distance between the measured and estimated values ​​of the metric based on the structure of the covariance matrix.

[0096] Step S403: Calculate the residuals of the horizontal radio navigation factor according to the definitions of the horizontal and vertical radio navigation factor nodes. and the residual of the vertical radio navigation factor

[0097]

[0098] In one possible implementation, step S50, acquiring altitude measurement data from a barometric altimeter to construct a barometric altimeter factor to suppress altitude errors in the land-based radio navigation system, includes:

[0099] Step S501: Based on the altitude information from the barometric altimeter, construct a loosely coupled measurement model with the inertial measurement unit at time k as follows:

[0100]

[0101] in, This represents the difference between the inertial measurement unit factor and the altitude output of the barometric altimeter, serving as a measurement update for the barometric altimeter. This is the measurement function of the barometric altimeter; The measurement noise of the barometric altimeter;

[0102] Step S502: Define the pressure altimeter factor node f according to the measurement model. BA (x k )for:

[0103]

[0104] Step S503: Calculate the residuals of the barometric altimeter factor nodes according to the definition of the barometric altimeter factor nodes. for:

[0105]

[0106] In one possible implementation, such as Figure 6 As shown, in step S60, dynamically adjusting the weight ratio of the altitude measurement data of the vertical radio navigation factor and the altitude measurement data of the barometric altimeter factor through the interactive multi-model algorithm includes:

[0107] Step S601: Set the algorithm model set Q of the interactive multi-model system to q∈(1,2,…)∈Q. Specifically, the algorithm model set Q includes two sub-models: a sub-model m for altitude radio navigation and a sub-model n for barometric altimeter.

[0108] The transformation of the algorithm model set Q follows a Markov chain;

[0109] Step S602: Calculate the initial mixing weights from sub-model m to sub-model n at time k-1. and the initial mixing weights from submodel n to submodel n at time k-1.

[0110]

[0111] in, p represents the update probability of submodel m at time k-1; mn p represents the state transition probability from submodel m to submodel n. nn This represents the state transition probability of submodel n with respect to itself, and is usually set to 1. The normalization factor is defined as follows:

[0112] In interactive multi-model algorithms, interactive input is used to characterize the mixture accuracy of each sub-model. Then, the measurement estimation of sub-model n after interactive input... Covariance Matrix They are respectively:

[0113]

[0114] in, This represents the measurement information of sub-model m at time k-1; This represents the measurement information of sub-model n at time k-1; Let represent the covariance matrix of submodel n at time k-1; Let represent the covariance matrix of submodel n at time k-1;

[0115] Step S603: Based on the state transition probabilities of sub-model m and sub-model n at the previous time step, construct a highly optimized function for the interactive multi-model algorithm within the factor graph framework. for:

[0116]

[0117] in, This represents the update probability of sub-model n at time k-1; The weight is the same as that of the vertical radio navigation factor; The weighting is the same as that of the altimeter factor altitude information; N represents the width of the pre-integration update interval of the selected inertial measurement unit; is the residual of the vertical radio navigation factor; l represents the total width of the pre-integration interval of the inertial measurement unit;

[0118] Specifically, This refers to summing the probabilities obtained by each time step using an interactive multi-model algorithm, corresponding to the Mahalanobis distance of the residual function of the vertical radio navigation factor at each time step.

[0119] This refers to summing the probabilities obtained by corresponding each of the Mahalanobis distances of the residual functions of the barometric altimeter factor nodes at each time point to an interactive multi-model algorithm.

[0120] Step S604: The model state transition probability is updated using the likelihood function. This indicates that residuals are measured through a model. The covariance matrix of the model measurement residuals Represented as:

[0121]

[0122] Therefore, the update probability of submodel n at time k is defined. for:

[0123]

[0124] Specifically:

[0125]

[0126] in, Normalization factor; defined as For factors and sums.

[0127] Step S605: Weighted merging of the estimation results of sub-model m and sub-model n is performed for iterative computation of the interactive multi-model algorithm at subsequent time steps; the measurement interaction output of sub-model n at time k. Covariance Matrix It can be represented as:

[0128]

[0129] in, This represents the measurement information of sub-model m at time k; This represents the measurement information of sub-model n at time k; Let represent the covariance matrix of submodel m at time k; Let n represent the covariance matrix at time k. Let be the initial mixing weights from submodel m to submodel n at time k; Let be the initial mixing weights from submodel n to submodel n at time k.

[0130] It should be noted that the estimation results of sub-model m and sub-model n refer to the initial mixing weights of sub-model m and sub-model n at the next time step after the probability update, and thus the measurement interaction output and covariance matrix at the next time step, i.e., time k.

[0131] In one possible implementation, step S70, which involves estimating the global state of the integrated navigation system using a factor graph framework composed of inertial measurement unit factors, direct astronomical navigation factors, refractive astronomical navigation factors, horizontal radio navigation factors, vertical radio navigation factors, and barometric altimeter factors, includes:

[0132] Step S701: To address the issue of different update rates among sensors in the navigation system, the frequency of the sensor with the lowest update rate is used as the global optimization cycle. Sequential data from other sensors are aligned using a sliding window to avoid loss of real-time performance of high-frequency sensor data.

[0133] Step S702: Combining the definitions of the pre-integration factor, direct astronomical navigation factor, refractional astronomical navigation factor, horizontal radio navigation factor, vertical radio navigation factor, and barometric altimeter factor of the inertial measurement unit within the factor diagram framework, the optimization function of the navigation system is constructed as follows:

[0134]

[0135] in, This represents the update probability of sub-model m at time k-1; This represents the update probability of sub-model n at time k-1. and Each of these correspondes one-to-one with the weights of the Vehicle Speed ​​Sensor (VSMS) and Brake Assist System (BAS) information; they also correspond one-to-one with the weights of the VRNS factor and BA factor height information; N represents the width of the update interval for the pre-integration of the selected inertial measurement unit; l represents the total width of the pre-integration interval of the inertial measurement unit. An optimization function for state estimation of navigation systems using an interactive multi-model algorithm within a factor graph framework;

[0136] For the residual function of the inertial measurement unit; The residuals of the direct astronomical navigation factor nodes; The residuals of the refracting astronomical navigation factor nodes; The residual of the horizontal radio navigation factor; The residual of the vertical radio navigation factor; The residuals of the barometric altimeter factor nodes; arg min(·) This represents the value of the independent variable that minimizes the objective function. During the fusion optimization at time k, the model update probability calculated in the previous time step is used for altitude correction in the interactive multi-model algorithm. This, combined with multi-source sensor information from the global navigation system, optimizes the attitude and position estimation to achieve navigation functionality. Specifically, during the global fusion optimization at time k, the model update probability calculated in the previous time step is used for updating, which is then used for altitude correction in the interactive multi-model algorithm. This, combined with multi-source sensor information from the global navigation system, optimizes the attitude and position estimation to achieve positioning functionality.

[0137] like Figure 7 As shown, a computer system suitable for implementing the interactive multi-model factor graph integrated navigation method based on tightly coupled attitude constraints provided in the above embodiments includes a central processing unit (CPU), which can perform various appropriate actions and processes according to a program stored in a read-only memory (ROM) or a program loaded from a storage portion into a random access memory (RAM). The RAM also stores various programs and data required for the operation of the computer system. The CPU, ROM, and RAM are connected via a bus. An input / output (I / O) interface is also connected to the bus.

[0138] The following components are connected to the I / O interface: input sections including keyboards, mice, etc.; output sections including liquid crystal displays (LCDs) and speakers, etc.; storage sections including hard disks, etc.; and communication sections including network interface cards such as LAN cards and modems. The communication sections perform communication processing via networks such as the Internet. Drives are also connected to the I / O interface as needed. Removable media, such as disks, optical disks, magneto-optical disks, semiconductor memories, etc., are installed on the drive as needed so that computer programs read from them can be installed into the storage section as required.

[0139] Specifically, according to this embodiment, the process described in the flowchart above can be implemented as a computer software program. For example, this embodiment includes a computer program product comprising a computer program tangibly embodied on a computer-readable medium, the computer program containing program code for performing the methods shown in the flowchart. In such an embodiment, the computer program can be downloaded and installed from a network via a communication component, and / or installed from a removable medium.

[0140] The flowcharts and schematic diagrams in the accompanying drawings illustrate the architecture, functionality, and operation of possible implementations of the system, method, and computer program product of this embodiment. In this regard, each block in the flowchart or schematic diagram may represent a module, segment, or portion of code containing one or more executable instructions for implementing the specified logical function. It should also be noted that in some alternative implementations, the functions marked in the blocks may occur in a different order than those shown in the drawings. For example, two consecutively indicated blocks may actually be executed substantially in parallel, and they may sometimes be executed in reverse order, depending on the functions involved. It should also be noted that each block in the schematic diagram and / or flowchart, and combinations of blocks in the schematic diagram and / or flowchart, can be implemented using a dedicated hardware-based system that performs the specified function or operation, or using a combination of dedicated hardware and computer instructions.

[0141] On the other hand, this embodiment also provides a non-volatile computer storage medium. This non-volatile computer storage medium can be the non-volatile computer storage medium included in the aforementioned device in the above embodiments, or it can exist independently and not assembled into the terminal. The aforementioned non-volatile computer storage medium stores one or more programs. When the aforementioned one or more programs are executed by a device, the device causes the device to: construct a combined navigation system, including an inertial navigation system, an astronomical navigation system, a ground-based radio navigation system, and a barometric altimeter; construct a pre-integrated inertial measurement unit factor for the inertial navigation system; construct a direct astronomical navigation factor and a tightly coupled refractive astronomical navigation factor, respectively, tightly coupled with the inertial measurement unit factor, based on the star-sensitive observations of the astronomical navigation system, to correct the attitude information output by the inertial navigation system; and construct a horizontal radio navigation factor and a vertical radio navigation factor based on the ground base station ranging and positioning principle of the ground-based radio navigation system. The system employs a linear navigation factor to correct errors in the position information output by the inertial navigation system; it acquires altitude measurement data from a barometric altimeter to construct a barometric altimeter factor to suppress altitude errors in the land-based radio navigation system; it dynamically adjusts the weight ratios of altitude measurement data from the vertical radio navigation factor and the barometric altimeter factor using an interactive multi-model algorithm to correct errors in the altitude information output by the inertial navigation system; and it performs global state estimation of the combined navigation system within a factor graph framework composed of inertial measurement unit factors, direct astronomical navigation factors, refractive astronomical navigation factors, horizontal radio navigation factors, vertical radio navigation factors, and barometric altimeter factors. In the description of this disclosure, it should be noted that the terms "upper," "lower," etc., indicating azimuth or positional relationships based on the azimuth or positional relationships shown in the accompanying drawings, are only for the convenience of describing this disclosure and simplifying the description, and do not indicate or imply that the device or element referred to must have a specific azimuth, or be constructed and operated in a specific azimuth, and therefore should not be construed as a limitation of this disclosure. Unless otherwise expressly specified and limited, the terms "installation," "connection," and "linkage" should be interpreted broadly. For example, they can refer to fixed connections, detachable connections, or integral connections; they can refer to mechanical connections or electrical connections; they can refer to direct connections or indirect connections through an intermediate medium; and they can refer to the internal connection of two components. Those skilled in the art can understand the specific meaning of the above terms in this disclosure according to the specific circumstances. It should also be noted that in the description of this disclosure, relational terms such as "first" and "second" are used only to distinguish one entity or operation from another, and do not necessarily require or imply any such actual relationship or order between these entities or operations.Furthermore, the terms "comprising," "including," or any other variations thereof are intended to cover non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements includes not only those elements but also other elements not expressly listed, or elements inherent to such a process, method, article, or apparatus. Without further limitations, an element defined by the phrase "comprising one..." does not exclude the presence of other identical elements in the process, method, article, or apparatus that includes that element. Clearly, the above embodiments of this disclosure are merely examples for clearly illustrating this disclosure and are not intended to limit the implementation of this disclosure. Those skilled in the art can make other variations or modifications based on the above description. It is impossible to exhaustively list all embodiments here. All obvious variations or modifications derived from the technical solutions of this disclosure are still within the protection scope of this disclosure.

Claims

1. An interactive multi-model factor graph integrated navigation method based on tightly coupled attitude constraints, characterized in that, The methods include: Construct a combined navigation system, which includes an inertial navigation system, an astronomical navigation system, a land-based radio navigation system, and a barometric altimeter; Construct the pre-integrated inertial measurement unit factor for the inertial navigation system; Based on the star-sensitive observations of the astronomical navigation system, direct astronomical navigation factors and refractional astronomical navigation factors are constructed and tightly coupled with the inertial measurement unit factors, respectively. The direct astronomical navigation factors and refractional astronomical navigation factors are used to correct the attitude information output by the inertial navigation system. Horizontal and vertical radio navigation factors are constructed based on the position information of the land-based radio navigation system. These factors are used to correct errors in the position information output by the inertial navigation system. Altitude measurement data from a barometric altimeter are acquired to construct a barometric altimeter factor, which is used to suppress altitude errors in land-based radio navigation systems. The weight ratio of the altitude measurement data of the vertical radio navigation factor and the altitude measurement data of the barometric altimeter factor is dynamically adjusted by an interactive multi-model algorithm. The weight ratio is used to correct the error of the altitude information output by the inertial navigation system. Global state estimation of a combined navigation system is performed using a factor graph framework consisting of inertial measurement unit factors, direct astronomical navigation factors, refracted astronomical navigation factors, horizontal radio navigation factors, vertical radio navigation factors, and barometric altimeter factors.

2. The method as described in claim 1, characterized in that, The inertial measurement unit factors include attitude residuals, velocity residuals, position residuals, gyroscope residuals, and accelerometer residuals.

3. The method as described in claim 2, characterized in that, The inertial measurement unit factors used to construct the pre-integration of the inertial navigation system include: The output of the gyroscope in the inertial measurement unit after error compensation is modeled: in, This is the actual output of the gyroscope; Δθ k Δθ is the angular increment at time k. k-1 Δt is the angular increment at time k-1; Δt is the time interval of the gyroscope error compensation interval. The output of the accelerometer in the inertial measurement unit after error compensation is modeled: in, This is the actual output of the accelerometer; Δv k Δv is the velocity increment at time k. k-1 The velocity increment at time k-1; The pre-integration interval of the inertial measurement unit is set to t. i ~t j j and i are integers greater than 0, and j is greater than i; the update interval of the inertial measurement unit is t. k-1 ~t k ; Construct the attitude residual, velocity residual, position residual, gyroscope residual, and accelerometer residual of the inertial measurement unit, where: Constructing the attitude residuals of the inertial measurement unit include: in, This is the attitude pre-integral quantity in the carrier coordinate system based on the gyroscope output; Let be the transformation matrix from the navigation coordinate system to the vehicle coordinate system at time j; This is the attitude pre-integral quantity in the navigation coordinate system based on the gyroscope output; Let be the transformation matrix from the navigation coordinate system to the vehicle coordinate system at time i; Constructing the velocity residual of the inertial measurement unit include: in, This represents the velocity at time i in the navigation coordinate system; This represents the velocity at time j in the navigation coordinate system. This represents the velocity increment from time i to time j in the navigation coordinate system due to gyroscope error. This is the attitude pre-integral quantity from time i to time k in the navigation coordinate system based on the gyroscope output. Let be the attitude transformation matrix from time k to time i in the vehicle coordinate system. Let k be the specific force actually output by the accelerometer in the carrier coordinate system at time k. For the random zero bias of the accelerometer, For the random zero bias of the gyroscope, δb gi δb is a small first-order quantity representing the actual deviation of the gyroscope in the inertial measurement unit during the pre-integration interval. ai This is a small first-order quantity representing the actual deviation of the accelerometer in the inertial measurement unit during the pre-integration interval; Constructing the position residual of the inertial measurement unit include: in, This represents the position at time i in the navigation coordinate system; This indicates the position at time j in the navigation coordinate system; Let Δt be the position increment caused by gyroscope error from time i to time j in the navigation coordinate system. i,j The time interval is the interval between pre-integration intervals; Gyroscope residuals for constructing inertial measurement units include: Among them, b gj It is the random zero bias of the gyroscope at time j, b gj It is the random zero bias of the gyroscope at time i; Accelerometer residuals for constructing inertial measurement units for: Among them, b aj It is the random zero bias of the accelerometer at time j, b ai It is the random zero bias of the accelerometer at time i; Construct the inertial measurement unit factor nodes for pre-integration of the inertial measurement unit, and define the inertial measurement unit residual function. for: Let x be the pre-integrated measurement value of the inertial navigation unit from i to j. ij Let i be the state from i to j before integration.

4. The interactive multi-model factor graph integrated navigation method based on tightly coupled attitude constraints as described in claim 1, characterized in that, The direct astronomical navigation factor, which is constructed based on star-sensitive observations of the astronomical navigation system and is tightly coupled with the inertial measurement unit factor, includes: Obtain the unit star vector based on star-sensitive observations from the astronomical navigation system; Based on the unit star vector, a direct star vector is constructed. The tightly coupled measurement model between the observation at time k and the inertial measurement unit is as follows: in, This represents the difference between the inertial measurement unit factor and the direct unit star vector output by the astronomical navigation system; As a measurement function for direct astronomical navigation systems; As measurement noise in direct astronomical navigation systems; The direct astronomical navigation factor node f is defined based on the tightly coupled measurement model. TDCNS (x k )for: Calculate the residuals of the direct astronomical navigation factor nodes according to the definition of the direct astronomical navigation factor nodes. for:

5. The interactive multi-model factor graph integrated navigation method based on tightly coupled attitude constraints as described in claim 1, characterized in that, The refractive astronomical navigation factors, which are constructed based on star-sensitive observations of the astronomical navigation system and tightly coupled with inertial measurement unit factors, include: A mathematical model for the apparent altitude of refracted starlight is established based on the relationships between the altitude of starlight refraction within the atmosphere, the altitude of starlight line of sight, and the spacecraft and the Earth's center, respectively: Where, r p Represents the position vector from the Earth's center to the spacecraft; u represents the unit star vector measured by the star sensor after starlight is refracted through the atmosphere; R c The radius of curvature of the Earth corresponds to point C, the lowest point of the starlight line of sight. The star-sensitive observation model for the apparent altitude of refracted starlight is established as follows: in, The apparent altitude of the refracted starlight is calculated from star-sensor observation data and attitude and position information output by the inertial measurement unit. This is the coordinate transformation matrix from the carrier coordinate system to the inertial coordinate system; The aircraft position vector is calculated by the inertial measurement unit; The design and installation location of the star sensor within the inertial measurement unit; R e denoted as the major axis radius of the Earth reference ellipsoid; e is the curvature of the Earth reference ellipsoid; L is the elevation corresponding to the lowest point C of the starlight line of sight AC in the inertial coordinate system. Based on the apparent altitude of the refracted star, the tightly coupled measurement model of the refracted star vector observation at time k with the inertial measurement unit is constructed as follows: in, This represents the difference between the inertial measurement unit factor and the apparent altitude of the refracted starlight output by the astronomical navigation system; For the measurement functions of the refracting astronomical navigation system; As a form of measurement noise in refracting astronomical navigation systems; Based on the tightly coupled measurement model, the refractive astronomical navigation factor node f is defined. TRCNS (x k )for: Calculate the residuals of the refraction astronomical navigation factor nodes according to the definition of the refraction astronomical navigation factor nodes. for:

6. The interactive multi-model factor graph integrated navigation method based on tightly coupled attitude constraints as described in claim 1, characterized in that, The construction of horizontal and vertical radio navigation factors based on location information from a land-based radio navigation system includes: Based on the position information from the land-based radio navigation system, loosely coupled measurement models with the inertial measurement unit are constructed in both the horizontal and vertical directions at time k, respectively: in, This represents the difference between the inertial measurement unit factor and the longitude L and latitude λ output by the land-based radio navigation system; This represents the difference between the inertial measurement unit factor and the altitude h output by the land-based radio navigation system; and These serve as the horizontal and vertical measurement functions for land-based radio navigation systems, respectively. and These serve as the horizontal and vertical measurement noise for land-based radio navigation systems, respectively. The horizontal radio navigation factor node f is defined according to the measurement model. HRNS (x k ) and vertical radio navigation factor node f VRNS (x k )for: Calculate the residuals of the horizontal radio navigation factor based on the definitions of the horizontal and vertical radio navigation factor nodes. and the residual of the vertical radio navigation factor 7. The interactive multi-model factor graph integrated navigation method based on tightly coupled attitude constraints as described in claim 1, characterized in that, The step of acquiring altitude measurement data from a barometric altimeter to construct a barometric altimeter factor includes: Based on the altitude information from the barometric altimeter, a loosely coupled measurement model with the inertial measurement unit is constructed at time k as follows: in, This represents the difference between the inertial measurement unit factor and the altitude output of the barometric altimeter, serving as a measurement update for the barometric altimeter. This is the measurement function of the barometric altimeter; The measurement noise of the barometric altimeter; Define the barometric altimeter factor node f based on the measurement model. BA (x k )for: Calculate the residuals of the barometric altimeter factor nodes based on their definition. for:

8. The interactive multi-model factor graph integrated navigation method based on tightly coupled attitude constraints as claimed in claim 7, characterized in that, The method of dynamically adjusting the weight ratio of altitude measurement data of vertical radio navigation factor and barometric altimeter factor through interactive multi-model algorithm includes: Set up an interactive multi-model algorithm model set Q, which includes two sub-models: a sub-model m for altitude radio navigation and a sub-model n for barometric altimeter. The transformation of the algorithm model set Q follows a Markov chain. Calculate the initial mixing weights from submodel m to submodel n at time k-1. and the initial mixing weights from submodel n to submodel n at time k-1. in, p represents the update probability of submodel m at time k-1; mn p represents the state transition probability from submodel m to submodel n. nn This represents the state transition probability of submodel n with respect to itself; Indicates the normalization factor; In interactive multi-model algorithms, interactive input is used to characterize the mixture accuracy of each sub-model. Then, the measurement estimation of sub-model n after interactive input... Covariance Matrix They are respectively: in, This represents the measurement information of sub-model m at time k-1; This represents the measurement information of sub-model n at time k-1; Let represent the covariance matrix of submodel n at time k-1; Let represent the covariance matrix of submodel n at time k-1; Based on the state transition probabilities of sub-models m and n at the previous time step, a highly optimized function for an interactive multi-model algorithm is constructed within the factor graph framework. for: in, This represents the update probability of submodel n at time k-1; N represents the width of the pre-integration update interval of the selected inertial measurement unit. is the residual of the vertical radio navigation factor; l represents the total width of the pre-integration interval of the inertial measurement unit; The update of the model state transition probability uses the likelihood function. This indicates that residuals are measured through a model. The covariance matrix of the model measurement residuals Represented as: The update probability of sub-model n at time k is defined as The estimation results of sub-model m and sub-model n are weighted and merged for iterative computation of the interactive multi-model algorithm at subsequent time steps; the measurement interaction output of sub-model n at time k. Covariance Matrix It can be represented as: in, This represents the measurement information of sub-model m at time k; This represents the measurement information of sub-model n at time k; Let represent the covariance matrix of submodel m at time k; Let n represent the covariance matrix at time k. Let be the initial mixing weights from submodel m to submodel n at time k; Let be the initial mixing weights from submodel n to submodel n at time k.

9. The interactive multi-model factor graph integrated navigation method based on tightly coupled attitude constraints as claimed in claim 8, characterized in that, The global state estimation of the integrated navigation system using a factor graph framework composed of inertial measurement unit factors, direct astronomical navigation factors, refracted astronomical navigation factors, horizontal radio navigation factors, vertical radio navigation factors, and barometric altimeter factors includes: The frequency of the sensor with the lowest update rate is used as the global optimization period, and sequential data from other sensors are aligned using a sliding window. Combining the definitions of the pre-integration factor, direct astronomical navigation factor, refracted astronomical navigation factor, horizontal radio navigation factor, vertical radio navigation factor, and barometric altimeter factor within the factor graph framework, the optimization function of the navigation system is constructed as follows: in, This represents the update probability of sub-model m at time k-1; denoted by , where represents the update probability of submodel n at time k-1, N represents the width of the update interval for the selected inertial measurement unit pre-integration, and l represents the total width of the inertial measurement unit pre-integration interval. An optimization function for state estimation of navigation systems using an interactive multi-model algorithm within a factor graph framework; For the residual function of the inertial measurement unit; The residuals of the direct astronomical navigation factor nodes; The residuals of the refracting astronomical navigation factor nodes; The residual of the horizontal radio navigation factor; The residual of the vertical radio navigation factor; The residuals of the barometric altimeter factor nodes; arg min(·) This represents the value of the independent variable that minimizes the objective function; During the fusion optimization at time k, the model update probability calculated in the previous time step is used for height correction of the interactive multi-model algorithm, and the attitude and position state optimization estimation is performed by combining the multi-source sensor information of the global navigation system to achieve navigation function.

10. A computer device, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that, When the processor executes the program, it implements the method as described in any one of claims 1-9.

Images