Multi-source combined navigation method, system and equipment and storage medium

Abstract

The invention discloses a multi-source integrated navigation method, system and equipment and a storage medium. The method comprises the following steps: establishing a Kalman filtering state equation based on an inertial navigation speed error equation, a position error equation and an attitude error equation; and establishing a Kalman filtering observation equation by taking multi-source measurement information as observed quantity, carrying out filtering iterative operation, and estimating and compensating an error of an inertial device as well as navigation resolving speed, position, attitude and course errors. By adopting integrated navigation of inertial navigation, celestial navigation, satellite navigation and other measurement sources, multi-source integrated navigation adopting different types of measurement sources can be flexibly configured according to requirements, so as to meet the navigation requirements in various scenes.

Description

TECHNICAL FIELD

[0001] The application belongs to the technical field of navigation, and particularly relates to a multi-source combined navigation method, system, device and storage medium. BACKGROUND

[0002] The existing combined navigation system usually only considers the navigation mode of two-by-two combination of satellite navigation, inertial navigation and celestial navigation, and it is difficult to meet the actual navigation requirements in different scenes. SUMMARY

[0003] The application aims to provide a multi-source combined navigation method, system, device and storage medium to realize the combined navigation of various different measurement sources.

[0004] The application is achieved by the following technical solutions:

[0005] The multi-source combined navigation method comprises the following steps:

[0006] The Kalman filter state equation is established based on the inertial navigation velocity error equation, the position error equation and the attitude error equation;

[0007] The Kalman filter observation equation is established by taking the multi-source measurement information as the observation, and the filter iteration operation is performed to estimate and compensate the inertial device error and the navigation solution velocity, position, attitude and heading error.

[0008] In some embodiments, the operation of determining the observation equation of the Kalman filter, performing the filter iteration operation, and estimating and compensating the inertial device error and the navigation solution velocity, position, attitude and heading error comprises the following steps:

[0009] The error state vector is established:

[0010] X=[φ ENU δV ENU δP ENU ε XYZ ▽ XYZ θ] T ;

[0011] Wherein, φ ENU represents the attitude angle error; δV ENU represents the velocity error; δP ENU respectively represents the position error; ε XYZ represents the gyro drift; ▽ XYZ represents the accelerometer drift; and θ represents the installation error between the celestial navigation system and the inertial navigation system.

[0012] The observation variable vector is established:

[0013]

[0014] wherein, δλ, δL and δh represent warp, weft, height position error; ψ x , ψ y and ψ z represent error angle of multi-source measurement sensor observation data based on platform coordinate system and navigation coordinate system conversion matrix.

[0015] The state matrix is calculated as follows:

[0016]

[0017] wherein:

[0018]

[0019]

[0020] The system noise matrix is calculated as follows:

[0021]

[0022] The observation matrix is calculated as follows:

[0023] H1 = [0 3×3 0 3×3 I 3×3 0 3×3 0 3×3 0 3×3 ];

[0024]

[0025] The state matrix discretization solution is calculated as follows:

[0026]

[0027] The system noise matrix discretization solution is calculated as follows:

[0028]

[0029] The observation equation of the Kalman filter is obtained after time update and measurement update.

[0030] In some embodiments, the time update is used to obtain the prior estimation value at the current time according to the initialization value and the optimal estimation error value at the last time; and is represented as follows:

[0031]

[0032] In some embodiments, the measurement update is used to obtain the optimal estimation error at the current time; and is represented as follows:

[0033]

[0034] P k =(IK k H k )P k / k-1 .

[0035] In some embodiments, the system may further include steps of coarse alignment and fine alignment at system startup before performing integrated navigation;

[0036] The coarse alignment step includes performing coarse alignment using the three-axis angular velocity output by the gyroscope, the specific force output by the accelerometer, internal satellite position information, or binding position information provided by the carrier's VMC system;

[0037] The fine alignment steps include performing fine alignment using internal satellite navigation position information or position information provided by the carrier's VMC system and multi-source measurement information, and estimating and compensating for errors in inertial devices.

[0038] In some embodiments, during the navigation calculation process, the device zero bias error estimation result and temperature information are monitored in real time. When the device zero bias error or temperature change exceeds the threshold, the zero bias and temperature coefficient of the inertial device are closed-loop. When the system is started again, the zero bias error and temperature coefficient of the inertial device are automatically compensated.

[0039] In some embodiments, when satellite navigation information is available, the installation error of the multi-source measurement equipment is estimated. When the estimation result meets the convergence condition, the installation error is automatically compensated, and the compensated installation error is used directly when the system is started next time.

[0040] On the other hand, the present invention also provides a multi-source integrated navigation system for executing the multi-source integrated navigation method, including a power supply module, a main control module, three navigation calculation modules, a satellite navigation module and a data storage module;

[0041] The main control module is used to generate time code information and second synchronization signal, output them to the 3-channel navigation calculation module and the 3-channel IMU module, and receive the combined navigation results output by the 3-channel navigation calculation module, select the best output and write them to the data storage module;

[0042] The navigation calculation module is used to collect data from the three IMU modules, receive data from the three IMU modules and data sent by the main control module, complete the combined navigation calculation and output;

[0043] The satellite navigation module is used to receive GNSS satellite navigation information and output satellite positioning data and second pulses.

[0044] On the other hand, the present invention also provides an electronic device, comprising:

[0045] Processor; and,

[0046] Memory for storing the executable instructions of the processor;

[0047] The processor is configured to execute the multi-source integrated navigation method by executing the executable instructions.

[0048] On the other hand, the present invention also provides a computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the multi-source integrated navigation method.

[0049] Compared with the prior art, the present invention has the following advantages and beneficial effects:

[0050] This invention employs a combination of inertial navigation and various measurement sources such as astronomical navigation and satellite navigation, enabling flexible configuration of multi-source combination navigation using different types of measurement sources to meet navigation needs in various scenarios.

[0051] This invention's multi-source integrated navigation system is based on the principle of inertial navigation and employs a Kalman filter integrated navigation method. During the integrated navigation process, it quickly and accurately corrects inertial navigation calculation errors and inertial device errors, suppressing the divergence of inertial navigation calculation errors at the device level, thereby achieving high-precision navigation positioning and motion state measurement of the integrated navigation system in various navigation modes. Attached Figure Description

[0052] To more clearly illustrate the technical solutions of the embodiments of the present invention, the accompanying drawings in the embodiments will be briefly described below. It should be understood that the following drawings only show some embodiments of the present invention and should not be regarded as a limitation on the scope. For those skilled in the art, other related drawings can be obtained based on these drawings without creative effort.

[0053] Figure 1 This is a schematic diagram of the structure of a multi-source integrated navigation system in an embodiment of the present invention.

[0054] Figure 2 This is a block diagram illustrating the electrical composition principle of a multi-source integrated navigation system in an embodiment of the present invention. In the diagram, a one-way arrow represents one-way communication, and a two-way arrow represents two-way communication.

[0055] Figure 3 This is a flowchart illustrating the multi-source integrated navigation method in an embodiment of the present invention.

[0056] Figure 4 This is a schematic diagram of the startup process of the multi-source integrated navigation system in an embodiment of the present invention.

[0057] Figure 5 This is a schematic diagram of the workflow of the multi-source integrated navigation system in an embodiment of the present invention.

[0058] in:

[0059] 1. Power supply module; 2. Main control module; 3. Navigation calculation module; 4. Satellite navigation module; 5. Data storage module; 6. Filter. Detailed Implementation

[0060] To make the objectives, technical solutions, and advantages of this application clearer, specific embodiments of this application will be described in further detail below with reference to the accompanying drawings. It should be understood that the specific embodiments described herein are merely for explaining this application and not for limiting it. It should also be noted that, for ease of description, only the parts relevant to this application are shown in the drawings, not all of them. Before discussing exemplary embodiments in more detail, it should be mentioned that some exemplary embodiments are described as processes or methods depicted as flowcharts. Although the flowcharts describe operations (or steps) as sequential processes, many of these operations can be performed in parallel, concurrently, or simultaneously. Furthermore, the order of the operations can be rearranged. The process can be terminated when its operation is completed, but may also have additional steps not included in the drawings. The process can correspond to a method, function, procedure, subroutine, subprogram, etc.

[0061] This invention is based on a combined navigation module, which collects IMU and multi-source measurement information through different solution modules to achieve modular, interchangeable measurement information source multi-source combined navigation.

[0062] like Figure 1 The multi-source integrated navigation system includes a power module 1, a main control module 2, three navigation calculation modules 3, a satellite navigation module 4, a data storage module 5, and a filter 6. The above modules are housed in a chassis, which is integrally welded from aluminum alloy material to provide installation space for each module.

[0063] The power module and data storage module use standard 3U boards, which are inserted into the corresponding slots in the chassis and secured with locking strips.

[0064] The chassis is equipped with a mounting base plate, on which the satellite navigation module, main control module and navigation calculation module are installed. After being inserted into the chassis slot, they are locked in place by the locking strips on the mounting base plate.

[0065] The main control module is used for communication with external devices, power-on and power-off control of peripherals, generating time code information and second synchronization signals, and outputting them to the 3-channel navigation calculation module and the 3-channel IMU module; it also receives the combined navigation results output by the 3-channel navigation calculation module, selects the best output and writes it to the data storage module; at the same time, it writes the received data from the satellite navigation module and other data to the data storage module.

[0066] Three navigation calculation modules are used to acquire data from three IMU modules, receive data from the three IMU modules and data sent by the main control module, complete the combined navigation calculation and output; at the same time, it has a non-volatile storage medium that can store installation error correction, lever error correction, etc.

[0067] The satellite navigation module is used to receive GNSS satellite navigation information and output satellite positioning data and second pulses.

[0068] The data storage module has four RS422 interfaces, which can store all the raw data from three IMU modules and one low-frequency sensor. It can be used to store specified pure inertial navigation, integrated navigation results and abnormal event records, and has the function of reading and storing data via 100 Mbps network.

[0069] The electrical composition principle block diagram of a multi-source integrated navigation system is as follows: Figure 2 As shown.

[0070] The multi-source integrated navigation system of this invention is based on the principle of inertial navigation and adopts the Kalman filter integrated navigation method.

[0071] By rapidly and accurately correcting inertial navigation calculation errors and inertial device errors during integrated navigation, the divergence of inertial navigation calculation errors is suppressed at the device level, thereby achieving high-precision navigation positioning and motion state measurement of the integrated navigation system in various navigation modes.

[0072] The multi-source integrated navigation method establishes the Kalman filter state equation based on the inertial navigation velocity error equation, position error equation, and attitude error equation;

[0073] Kalman filter observation equations are established using multi-source measurement information as the observations.

[0074] Based on Kalman filtering, iterative filtering calculations are performed to estimate and compensate for inertial device errors and to solve for navigation errors in velocity, position, attitude, and heading, so as to achieve high-precision navigation in long-endurance navigation conditions.

[0075] After the multi-source integrated navigation system is started, it completes initial alignment, inertial navigation calculation and integrated navigation calculation, and outputs high-precision navigation calculation information to external devices.

[0076] Reference Figure 4 The specific process of a multi-source integrated navigation system starting up for integrated navigation is as follows:

[0077] 1. System startup

[0078] 1.1 Preparation Phase

[0079] Check the system connection status. If the physical connection is normal, power on and perform a self-test. If the self-test is normal, enter the alignment waiting stage. If the self-test is abnormal, a fault alarm will be triggered.

[0080] 1.2 Coarse Alignment Stage

[0081] Coarse alignment is performed using the three-axis angular velocity output from the gyroscope, the specific force output from the accelerometer, and the binding position information provided by the internal satellite position information or the VMC system of the carrier aircraft.

[0082] During this phase, the multi-source integrated navigation system uses internal navigation time information or external time synchronization information to complete time synchronization and maintain time.

[0083] 1.3 Precision Alignment Stage

[0084] In the 15-minute alignment mode, precise alignment is performed using internal satellite navigation position information or position information provided by the carrier's VMC system, as well as multi-source measurement information, and errors of inertial devices are estimated and compensated.

[0085] 2. System Operation

[0086] After the multi-source integrated navigation system completes alignment and enters navigation operation, it outputs the attitude and bearing information required by the user in real time. The control process is as follows: Figure 5 As shown.

[0087] Once the system enters navigation mode, it can automatically or manually switch between combined navigation and pure inertial navigation. It can also estimate and compensate for navigation errors and error sources, and output high-precision information such as position, speed, heading, attitude, and altitude.

[0088] During the navigation calculation process, the main control module monitors the device zero bias error estimation results and temperature information in real time. When the device's zero bias error or temperature change is large, it performs closed-loop calculation of the inertial device's zero bias and temperature coefficient, and writes the data into the main control module's Flash. Upon the next startup, it automatically compensates for the inertial device's zero bias error and temperature coefficient.

[0089] When satellite guidance information is available, the installation error of multi-source measurement equipment can be estimated. When the estimation result meets the convergence condition, the installation error is automatically compensated and written into the internal Flash, which can be used directly on the next startup.

[0090] In some embodiments, after the multi-source integrated navigation system completes alignment and enters the navigation working state, it calculates and outputs navigation information in real time. In the integrated navigation state, all errors are clarified by defining state vectors, the dynamic propagation of errors is established by establishing state equations, the observation equations of the Kalman filter are determined, and the estimated values ​​of errors are corrected using Kalman filter prediction-update iteration operations. This achieves the operation of estimating and compensating for inertial device errors and solving for navigation errors in velocity, position, attitude, and heading. Figure 3 This includes the following steps:

[0091] 1) Solving the linear model

[0092] a) System variables

[0093] State variables are represented as:

[0094] X = [φ ENU δV ENU δP ENU ε XYZ ▽ XYZ θ] T (1);

[0095] Where, φ ENU Indicates attitude angle error; δV ENU Indicates velocity error; δP ENU These represent position errors; ε XYZ Indicates gyroscope drift; ▽ XYZ θ represents the meter drift; θ represents the installation error between the multi-source measurement equipment and the inertial measurement sensor (IMU).

[0096] The observed variable is represented as:

[0097]

[0098] Where δλ, δL, and δh represent the longitude, latitude, and altitude position errors; ψ x ψ y and ψ z The error angle represents the data obtained from the observations of multi-source measurement sensors based on the transformation matrix of the navigation system coordinate system (a coordinate system defined with the system center as the origin and along the side, front, and top directions) and the navigation coordinate system (an orthogonal coordinate system defined with the local horizontal east, horizontal north, and vertical celestial directions as positive directions; the horizontal position is described using spherical coordinate latitude and longitude, and the altitude is described using altitude; the latitude, longitude, and altitude of the geographical location are obtained by rotation and translation of the geocentric inertial frame).

[0099] b) Calculate the state matrix

[0100]

[0101] in, φ ENU δV ENU δP ENU ε XYZ 、▽ XYZ The time derivative of the θ state vector;

[0102]

[0103] Among them, R m R represents the principal curvature radius of the Earth's meridian. nThe radius of curvature of the Earth's circumference is represented by h, the current altitude is represented by L, and the current latitude is represented by L.

[0104]

[0105] Where, ω ie V represents angular velocity. N V represents the northbound velocity. E Indicates eastward speed;

[0106]

[0107] Among them, V U Indicates the upward speed;

[0108]

[0109]

[0110] The antisymmetric matrix representing the angular rate of rotation relative to the Earth in the navigation coordinate system; f n × indicates a 3x3 antisymmetric matrix in the navigation coordinate system. This represents the direction cosine matrix from the navigation system coordinate system to the navigation coordinate system.

[0111] c) Calculate the system noise matrix, expressed as:

[0112]

[0113] Where G represents the system noise matrix, O represents the direction cosine matrix from the navigation system coordinate system to the platform coordinate system. 3×3 Represents a zero matrix.

[0114] d) Calculate the observation matrix, expressed as:

[0115] H1 = [0 3×3 0 3×3 I 3×3 0 3×3 0 3×3 0 3×3 (11);

[0116]

[0117] Among them, I 3×3 Represents the identity matrix, 0 3×3 0 1×3 0 1×10 Let each represent the zero matrix, and we obtain the observation matrix H.

[0118] 2) Solving Discrete Models

[0119] a) Discretization of the state matrix;

[0120]

[0121] Among them, F k This represents the value of the system state transition matrix at time t, where T is the discretization period, and Φ is the value of the matrix. k+1，k This represents the state transition matrix estimated by the Kalman filter.

[0122] b) Discretization of the system noise matrix;

[0123]

[0124] Among them, G k Let Γ represent the value of matrix G at time t, where T is the discretization period. k+1 This represents the Kalman filter error transfer matrix.

[0125] 3) Time update, represented as follows:

[0126]

[0127] in, P represents the predicted state value at time k. k / k-1 Let P represent the predicted covariance matrix at time k. k-1 This represents the optimal covariance matrix at time k-1. Represents the state transition matrix. Q represents the system noise driving matrix. k-1 This represents the system noise covariance matrix.

[0128] 4) Measurement update, represented as:

[0129]

[0130] P k =(IK k H k )P k / k-1 (20);

[0131] Among them, K k This represents the Kalman gain matrix at time k. R represents the transpose of the observation matrix. k Z represents the observation noise covariance matrix. k H represents the actual observation vector at time k. k This represents the observation matrix at time k.

[0132] The multi-source integrated navigation method of the present invention will be further described in detail below with reference to the embodiments.

[0133] In this embodiment, after the multi-source integrated navigation system completes alignment and enters the navigation working state, it calculates and outputs the information required by the user in real time. In a certain integrated navigation state, the observation equation of the Kalman filter is determined, including the following steps:

[0134] Establish the error state vector:

[0135] X = [φ ENU δV ENU δP ENU ε XYZ ▽ XYZ θ] T ;

[0136] Where, φ ENU Indicates attitude angle error; δV ENU Indicates velocity error; δP ENU These represent position errors; ε XYZ Indicates gyroscope drift; ▽ XYZ θ represents the drift of the celestial navigation system; θ represents the installation error between the celestial navigation system and the inertial navigation system.

[0137] Establish the vector of observed variables:

[0138]

[0139] Where δλ, δL, and δh represent the longitude, latitude, and altitude position errors; ψ x ψ y and ψ z This represents the error angle obtained from the multi-source measurement sensor observation data based on the transformation matrix between the platform coordinate system and the navigation coordinate system.

[0140] The state matrix is ​​calculated as follows:

[0141]

[0142]

[0143] in:

[0144]

[0145]

[0146] The system noise matrix is ​​calculated as follows:

[0147]

[0148] The observation matrix is ​​calculated as follows:

[0149] H1 = [0 3×3 0 3×3 I3×3 0 3×3 0 3×3 0 3×3 ];

[0150]

[0151] Discretize the state matrix:

[0152]

[0153] Discretization of the system noise matrix:

[0154]

[0155] The observation equations of the Kalman filter are obtained after time updates and measurement updates. The time update is expressed as:

[0156] Measurement updates are represented as follows:

[0157]

[0158] P k =(IK k H k )P k / k-1 .

[0159] In this embodiment, a certain measurement information at the current moment is calculated and input into the Kalman filter equation for filtering calculation, which includes time update and measurement update.

[0160] Time update is used to obtain the prior estimate of the current time based on the initial value and the optimal estimate error value of the previous time step; measurement update is used to obtain the optimal estimate error of the current time step.

[0161] The method in this embodiment can realize combined navigation between inertial navigation and various measurement sources such as astronomical navigation and satellite navigation, and can flexibly configure and select different types of measurement sources for combined navigation according to needs.

[0162] On the other hand, the present invention also relates to an electronic device, comprising:

[0163] Processor; and,

[0164] Memory for storing the executable instructions of the processor;

[0165] The processor is configured to execute the multi-source integrated navigation method by executing the executable instructions.

[0166] On the other hand, the present invention also relates to a computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the multi-source integrated navigation method described above.

[0167] The above description is merely a preferred embodiment of the present invention and is not intended to limit the present invention in any way. Any simple modifications or equivalent changes made to the above embodiments based on the technical essence of the present invention shall fall within the protection scope of the present invention.

Claims

1. A multi-source integrated navigation method, characterized in that, Includes the following steps: The Kalman filter state equation is established based on the inertial navigation velocity error equation, position error equation, and attitude error equation. Using multi-source measurement information as the observation, a Kalman filter observation equation is established, and filtering iteration calculations are performed to estimate and compensate for inertial device errors and to solve for navigation errors in velocity, position, attitude, and heading.

2. The multi-source integrated navigation method according to claim 1, characterized in that, The process of determining the observation equations of the Kalman filter, performing filtering iterations, estimating and compensating for inertial device errors, and calculating navigation errors in velocity, position, attitude, and heading includes the following steps: Establish the error state vector: Where, φ ENU Indicates attitude angle error; δV ENU Indicates velocity error; δP ENU These represent position errors; ε XYZ Indicates gyroscope drift; θ represents the drift of the celestial navigation system and the inertial navigation system; Establish the vector of observed variables: Where δλ, δL, and δh represent the longitude, latitude, and altitude position errors; ψ x ψ y and ψ z This represents the error angle obtained from the multi-source measurement sensor observation data based on the transformation matrix between the platform coordinate system and the navigation coordinate system; The state matrix is ​​calculated as follows: in: The system noise matrix is ​​calculated as follows: The observation matrix is ​​calculated as follows: H1=[0 3×3 0 3×3 AND 3×3 0 3×3 0 3×3 0 3×3 ]; Discretize the state matrix: Discretization of the system noise matrix: The observation equations of the Kalman filter are obtained after time updates and measurement updates.

3. The multi-source integrated navigation method according to claim 2, characterized in that, The time update is used to obtain the prior estimate of the current time step based on the initial value and the optimal estimation error value of the previous time step; expressed as:

4. The multi-source integrated navigation method according to claim 3, characterized in that, The measurement update is used to obtain the optimal estimation error at the current moment; it is expressed as: P k =(I-K k H k )P k / k-1 。 5. The multi-source integrated navigation method according to claim 1, characterized in that, The process of coarse alignment and fine alignment is also included before integrated navigation is performed at system startup; The coarse alignment step includes performing coarse alignment using the three-axis angular velocity output by the gyroscope, the specific force output by the accelerometer, internal satellite position information, or binding position information provided by the carrier's VMC system; The fine alignment steps include performing fine alignment using internal satellite navigation position information or position information provided by the carrier's VMC system and multi-source measurement information, and estimating and compensating for errors in inertial devices.

6. The multi-source integrated navigation method according to claim 5, characterized in that, During the navigation calculation process, the device zero bias error estimation results and temperature information are monitored in real time. When the device zero bias error or temperature change exceeds the threshold, the zero bias and temperature coefficient of the inertial device are closed-loop. When the system is started again, the zero bias error and temperature coefficient of the inertial device are automatically compensated.

7. The multi-source integrated navigation method according to claim 5, characterized in that, When satellite navigation information is available, the installation error of the multi-source measurement equipment is estimated. When the estimation result meets the convergence condition, the installation error is automatically compensated, and the compensated installation error is used directly when the system starts up next time.

8. A multi-source integrated navigation system, characterized in that, The multi-source integrated navigation method for executing any one of claims 1-7 includes a power supply module, a main control module, three navigation calculation modules, a satellite navigation module, and a data storage module; The main control module is used to generate time code information and second synchronization signal, output them to the 3-channel navigation calculation module and the 3-channel IMU module, and receive the combined navigation results output by the 3-channel navigation calculation module, select the best output and write them to the data storage module; The navigation calculation module is used to collect data from the three IMU modules, receive data from the three IMU modules and data sent by the main control module, complete the combined navigation calculation and output; The satellite navigation module is used to receive GNSS satellite navigation information and output satellite positioning data and second pulses.

9. An electronic device, characterized in that, include: processor; as well as, Memory for storing the executable instructions of the processor; The processor is configured to execute the multi-source integrated navigation method of any one of claims 1-7 by executing the executable instructions.

10. A computer-readable storage medium having a computer program stored thereon, characterized in that, When the computer program is executed by the processor, it implements the multi-source integrated navigation method according to any one of claims 1-7.

Images