Airborne multi-sensor combined navigation and positioning method, device, equipment and medium

Abstract

The embodiment of the invention provides an airborne multi-sensor integrated navigation and positioning method, device, equipment and medium, and relates to the technical field of aviation navigation, and the method comprises the following steps: a Beidou navigation system, an altimeter and a radio navigation system are respectively combined with an inertial navigation system, each combination outputs an observation value through a sub-filter, and the observation value is calculated through a sub-filter; the observation value output by each sub-filter and the predicted value of the inertial navigation system are fused and then a positioning result is output, and fault detection and repair are respectively carried out on data output by the inertial navigation system, the Beidou navigation system and the altimeter; carrying out anomaly detection on the observation value output by each sub-filter by utilizing the state parameter estimation and the residual vector, and repairing the detected abnormal value by adopting a robust filtering mode; and performing anomaly detection and restoration on the observation value output by each sub-filter, calculating new state parameter estimation, and performing anomaly detection on the dynamic model of the sub-filter based on the new state parameter estimation. The scheme is beneficial to improving the fault tolerance rate of integrated navigation positioning.

Description

Technical Field

[0001] The present invention relates to the field of aviation navigation technology, and in particular to an airborne multi-sensor combined navigation and positioning method, device, equipment and medium. Background Art

[0002] With the development of science and technology, navigation and positioning have become an indispensable and important technical guarantee for vehicles such as cars, airplanes, and ships to smoothly realize cross-domain maneuvers. However, the operating environment of vehicles is often complex and changeable, requiring the navigation and positioning system to have accurate and reliable navigation and positioning capabilities when maneuvering over a large range and for a long time on roads, at sea, and in the air. A single navigation method has one or another disadvantage, either poor accuracy, or susceptibility to interference, or error accumulation over time, and a single navigation technology cannot provide complete navigation reference information. With the rapid development of micro-electromechanical systems, more and more types of sensors can be used for navigation and positioning, and the physical mechanisms of navigation and positioning used are different. Combination positioning using multi-source heterogeneous sensors has become the mainstream trend of navigation and positioning. With the continuous enrichment of the types of sensors in the combined navigation and positioning system, the accuracy, integrity, continuity and availability of the navigation system have been continuously improved. The application field of the combined navigation system is not only limited to guiding the target to maneuver safely and reliably from place A to place B, but can also be applied to dense formations, precise intersections and docking, and other application scenarios. However, multi-sensor integrated navigation still needs to face urgent problems such as the accuracy of sensor raw data, the stability of combined filters and the reliability of combined positioning results.

[0003] Existing technical solutions for the accuracy and integrity of integrated navigation usually use the measurement information in the combined filter to calculate the test statistic, set the test threshold to determine whether the system has problems such as reduced accuracy and excessive integrity risk, and attempt to detect and repair faults at the combined filter level, or use the residual data of each sensor to detect the sensor fault and isolate the faulty sensor. The above methods only consider the combined filter fault or the sensor independent fault model, and their fault tolerance rate is low. When multiple sensors fail at the same time or there are interactive faults in the system, it is often difficult to accurately isolate the real faulty sensor relying solely on the residual data, which seriously affects the availability of the integrated navigation system. In addition, if the system model is inaccurate or there is a large modeling error, the generated residual cannot truly reflect the working status of the sensor, thereby covering up the sensor's fault information, and the reliability of the integrated navigation system cannot be guaranteed. Summary of the invention

[0004] In view of this, an embodiment of the present invention provides an airborne multi-sensor combined navigation and positioning method to solve the technical problem of low fault tolerance of combined navigation in the prior art. A Beidou navigation system and an inertial navigation system form a first combination, an altimeter and the inertial navigation system form a second combination, and a radio navigation system and the inertial navigation system form a third combination. In the first combination, the observation value output by the Beidou navigation system and the first predicted value output by the inertial navigation system are fused and input into a first extended Kalman filter, and the first extended Kalman filter outputs the first observation value of the first combination. In the second combination, the observation value output by the altimeter and the first predicted value output by the inertial navigation system are fused and input into a second extended Kalman filter, and the second extended Kalman filter outputs the second observation value of the second combination. In the third combination, the observation value output by the radio navigation system and the first predicted value output by the inertial navigation system are fused and input into a third extended Kalman filter, and the third extended Kalman filter outputs the third observation value of the third combination. The first observation value, the second observation value, the third observation value and the second predicted value of the inertial navigation system are fused and then the positioning result is output, wherein the method includes: Before the first combination, the second combination, and the third combination output predicted values, respectively performing fault detection and repair on the data output by the inertial navigation system, the data output by the Beidou navigation system, and the data output by the altimeter; In the process of outputting predicted values ​​of the first combination, the second combination and the third combination, the observed values ​​output by the first extended Kalman filter, the second extended Kalman filter and the third extended Kalman filter are respectively tested for abnormalities using state parameter estimation and residual vectors, and the detected abnormal values ​​are repaired by using anti-error filtering; After performing abnormality inspection and repair on the observation values ​​output by the first extended Kalman filter, the second extended Kalman filter, and the third extended Kalman filter, respectively, new state parameter estimates are calculated, and based on the new state parameter estimates, abnormality detection is performed on the Kalman filter dynamics model used to construct the state equation in the extended Kalman filter.

[0005] The embodiment of the present invention also provides an airborne multi-sensor combined navigation and positioning device to solve the technical problem of low fault tolerance of combined navigation in the prior art. The device includes: A source data fault detection module is used to perform fault detection and repair on the data output by the inertial navigation system, the data output by the Beidou navigation system and the data output by the altimeter before outputting the predicted value in the first combination, the second combination and the third combination, wherein the Beidou navigation system and the inertial navigation system form the first combination, the altimeter and the inertial navigation system form the second combination, and the radio navigation system and the inertial navigation system form the third combination. In the first combination, the observation value output by the Beidou navigation system and the first predicted value output by the inertial navigation system are fused and input into a first extended Kalman filter. The first extended Kalman filter Outputting a first observation value of the first combination, fusing the observation value output by the altimeter in the second combination with the first prediction value output by the inertial navigation system and inputting them into a second extended Kalman filter, the second extended Kalman filter outputting a second observation value of the second combination, fusing the observation value output by the radio navigation system in the third combination with the first prediction value output by the inertial navigation system and inputting them into a third extended Kalman filter, the third extended Kalman filter outputting a third observation value of the third combination, and fusing the first observation value, the second observation value, the third observation value and the second prediction value of the inertial navigation system and outputting a positioning result; An observation value abnormality detection module is used to use state parameter estimation and residual vector to perform abnormality detection on the observation values ​​output by the first extended Kalman filter, the second extended Kalman filter and the third extended Kalman filter respectively during the process of outputting predicted values ​​of the first combination, the second combination and the third combination, and repair the detected abnormal values ​​by using anti-error filtering; A model anomaly detection module is used to perform anomaly inspection and repair on the observation values ​​output by the first extended Kalman filter, the second extended Kalman filter and the third extended Kalman filter, respectively, and calculate new state parameter estimates, and perform anomaly detection on the Kalman filter dynamics model used to construct the state equation in the extended Kalman filter based on the new state parameter estimates.

[0006] An embodiment of the present invention also 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 implements any of the above-mentioned airborne multi-sensor combined navigation and positioning methods when executing the computer program, so as to solve the technical problem of low fault tolerance of combined navigation in the prior art.

[0007] An embodiment of the present invention further provides a computer-readable storage medium storing a computer program for executing any of the above-mentioned airborne multi-sensor combined navigation and positioning methods to solve the technical problem of low fault tolerance of combined navigation in the prior art.

[0008] Compared with the prior art, the beneficial effects that can be achieved by at least one of the above-mentioned technical solutions adopted in the embodiments of this specification include at least: it is proposed to adopt the Beidou navigation system, altimeter, and radio navigation system to form different combinations with the inertial navigation system respectively, and the observation values ​​output by the sensors in each combination are combined with the predicted values ​​of the inertial navigation system and the extended Kalman filter is fused to output the state estimation value. After completing the measurement update of each sub-filter, the reset federal Kalman filter is used to perform optimal information fusion, and then the optimal state estimation value is obtained, thereby realizing the construction of an expandable multi-sensor combined navigation and positioning fusion framework, each sub-filter (i.e., the extended Kalman filter) is independent of each other, and it is convenient to add other new navigation and positioning sensors as needed, thereby improving the scalability; at the same time, it is proposed to respectively output the data of the inertial navigation system, the data output by the Beidou navigation system, and the data output by the altimeter. The method can detect and repair faults in the data, thereby eliminating or reducing the adverse effects of errors at the original data source level on the positioning results, and can also detect the problem of simultaneous failure of multiple sensors; it also proposes to use state parameter estimation and residual vector to perform abnormality checks on the observation values ​​output by the first extended Kalman filter, the second extended Kalman filter, and the third extended Kalman filter, and use anti-error filtering to repair the detected abnormal values, thereby reducing the adverse effects of abnormalities in the output observation values ​​of each combination on the positioning results, and further testing and controlling the Kalman filter dynamics model on the basis of testing and eliminating the observation anomalies, thereby improving the stability of the filter and the estimation accuracy of the sub-filter, which is beneficial to reducing the adverse effects of inaccurate system models or large modeling errors on the positioning results, and further beneficial to improving the fault tolerance rate of combined navigation positioning. BRIEF DESCRIPTION OF THE DRAWINGS

[0009] In order to more clearly illustrate the technical solutions of the embodiments of the present application, the drawings required for use in the embodiments will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present application. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative work.

[0010] Figure 1 is a flow chart of an airborne multi-sensor combined navigation and positioning method provided by an embodiment of the present invention; Figure 2 It is a schematic diagram of the principle of an airborne multi-sensor combined navigation and positioning method provided by an embodiment of the present invention; Figure 3 is a flow chart of inertial navigation data detection and repair provided by an embodiment of the present invention; Figure 4It is a flowchart of Beidou data quality fault detection and repair provided by an embodiment of the present invention; Figure 5 is a flow chart of a carrier initial alignment heading calculation provided by an embodiment of the present invention; Figure 6 is a flow chart of an inertial navigation machinery arrangement calculation provided by an embodiment of the present invention; Figure 7 This is a data error fault detection and repair simulation comparison diagram provided by an embodiment of the present invention; Figure 8 It is a comparison diagram of a filtering error fault detection and repair simulation provided by an embodiment of the present invention; Fig. 9 It is a result error fault detection and repair simulation comparison diagram provided by an embodiment of the present invention; Fig.10 is a structural block diagram of a computer device provided by an embodiment of the present invention; Fig.11 It is a structural block diagram of an airborne multi-sensor combined navigation and positioning device provided by an embodiment of the present invention. DETAILED DESCRIPTION

[0011] The embodiments of the present application are described in detail below with reference to the accompanying drawings.

[0012] The following describes the implementation methods of the present application through specific examples, and those skilled in the art can easily understand other advantages and effects of the present application from the contents disclosed in this specification. Obviously, the described embodiments are only a part of the embodiments of the present application, rather than all the embodiments. The present application can also be implemented or applied through other different specific implementation methods, and the details in this specification can also be modified or changed in various ways based on different viewpoints and applications without departing from the spirit of the present application. It should be noted that, in the absence of conflict, the following embodiments and the features in the embodiments can be combined with each other. Based on the embodiments in the present application, all other embodiments obtained by ordinary technicians in the field without making creative work belong to the scope of protection of the present application.

[0013] In an embodiment of the present invention, an airborne multi-sensor combined navigation and positioning method is provided, such as Figure 1 , Figure 2As shown, a BeiDou navigation system (BDS, such as a BDS terminal, a receiver module or a base station) and an inertial navigation system (INS, such as an inertial navigation sensor IMU module) form a first combination, an altimeter and the inertial navigation system form a second combination, and a radio navigation system (VOR) and the inertial navigation system form a third combination. In the first combination, the observation value (such as a fixed phase observation value and a pseudorange observation value) output by the BeiDou navigation system and the first prediction value (i.e., the prediction value output by the inertial navigation system after mechanical arrangement) output by the inertial navigation system are fused and input into a first extended Kalman filter. The first extended Kalman filter outputs the first observation value of the first combination. In the second combination, the observation value (such as a fixed phase observation value and a pseudorange observation value) output by the BeiDou navigation system and the first prediction value (i.e., the prediction value output by the inertial navigation system after mechanical arrangement) are fused and input into a first extended Kalman filter. The first extended Kalman filter outputs the first observation value of the first combination. The observation value output by the altimeter in the combination (such as the elevation information innovation vector) is fused with the first prediction value output by the inertial navigation system and then input into a second extended Kalman filter, the second extended Kalman filter outputs the second observation value of the second combination, the observation value output by the radio navigation system in the third combination (such as the position velocity innovation vector) is fused with the first prediction value output by the inertial navigation system and then input into a third extended Kalman filter, the third extended Kalman filter outputs the third observation value of the third combination, the first observation value, the second observation value, the third observation value and the second prediction value of the inertial navigation system are fused and then a positioning result is output, the method comprising: Step S101: Before the first combination, the second combination, and the third combination output predicted values, respectively, fault detection and repair are performed on the data output by the inertial navigation system, the data output by the Beidou navigation system, and the data output by the altimeter; Step S102: in the process of outputting predicted values ​​of the first combination, the second combination, and the third combination, abnormality inspection is performed on the observed values ​​output by the first extended Kalman filter, the second extended Kalman filter, and the third extended Kalman filter respectively by using state parameter estimation and residual vector, and the detected abnormal values ​​are repaired by using anti-error filtering; Step S103: After performing abnormality inspection and repair on the observation values ​​output by the first extended Kalman filter, the second extended Kalman filter, and the third extended Kalman filter, respectively, new state parameter estimates are calculated, and based on the new state parameter estimates, abnormality detection is performed on the Kalman filter dynamics model used to construct the state equation in the extended Kalman filter.

[0014] Depend on Figure 1As can be seen from the process shown, in an embodiment of the present invention, it is proposed to use the Beidou navigation system, the altimeter, and the radio navigation system to form different combinations with the inertial navigation system respectively. The observation values ​​output by the sensors in each combination are combined with the predicted values ​​of the inertial navigation system and the state estimation values ​​are output after fusion by the extended Kalman filter. After the measurement and update of each sub-filter are completed, the reset federal Kalman filter is used to perform optimal information fusion, and then the optimal state estimation value is obtained, thereby realizing the construction of an expandable multi-sensor combined navigation and positioning fusion framework. Each sub-filter (i.e., the extended Kalman filter) is independent of each other, and it is convenient to add other new navigation and positioning sensors as needed, thereby improving the scalability. At the same time, it is proposed to perform fault detection and repair on the data output by the inertial navigation system, the data output by the Beidou navigation system, and the data output by the altimeter respectively, so that The method can eliminate or reduce the adverse effects of errors at the level of the original data source on the positioning results, and can also detect the problem of simultaneous failure of multiple sensors; it also proposes to use state parameter estimation and residual vectors to perform abnormality checks on the observation values ​​output by the first extended Kalman filter, the second extended Kalman filter, and the third extended Kalman filter, and use anti-error filtering to repair the detected abnormal values, thereby reducing the adverse effects of abnormalities in the output observation values ​​of each combination on the positioning results. After checking and eliminating the observation anomalies, the Kalman filter dynamics model is further checked and controlled, which improves the stability of the filter and the estimation accuracy of the sub-filter, which is beneficial to reducing the adverse effects of inaccurate system models or large modeling errors on the positioning results, and further helps to improve the fault tolerance rate of combined navigation positioning.

[0015] When implementing it, Figure 2 As shown, in order to eliminate or reduce the adverse effects of errors at the level of the original data source on the positioning results, and also to facilitate the elimination of the adverse effects of multiple sensor failures, it is proposed that before the first combination, the second combination, and the third combination output predicted values, the data output by the inertial navigation system, the data output by the Beidou navigation system, and the data output by the altimeter are respectively detected and repaired for faults. Specifically, after the observation data of the inertial navigation system, the observation data of the Beidou navigation system, the observation data of the radio navigation system, and the observation data of the altimeter are respectively checked (i.e., fault detection and repair), the observation values ​​or predicted values ​​are outputted separately. For example, Figure 3 As shown, the process of detecting and repairing faults in the data output by the inertial navigation system includes: Fault detection is performed on the data output by the inertial navigation system in combination with the inertial navigation time series from various aspects such as continuity (i.e., whether there is data loss), non-zero property (i.e., detecting zero values of data), non-repetitiveness (i.e., detecting repeated data), and finite value property (i.e., detecting whether the data exceeds the limit). For the missing data of the inertial navigation data fault with few epochs (i.e., moments) detected, the historical data replacement method is used for repair. For example, the historical epoch data adjacent to the missing data is used to replace the missing data for repair. For the data of the inertial navigation data fault with multiple epochs detected, the polynomial function fitting prediction method is used for repair. For example, the adjacent epoch data is used for polynomial fitting function, and the data predicted by the fitting function is used for repair.

[0016] During specific implementation, after the receiver module or the reference station receives the observation data of Beidou satellites, it performs fault detection and repair according to the Figure 4 process shown to prevent satellite positioning anomalies caused by poor data quality of individual satellites. After loading the Beidou satellite data and navigation data, RAIM (Receiver Autonomous Integrity Monitoring) satellite fault detection and exclusion are performed. It is judged whether i < ns. If so, it is judged whether the elevation angle is greater than the threshold. If not, the satellite is excluded. If so, it is judged whether the signal-to-noise ratio is greater than the threshold. If not, the satellite is excluded. If so, jump to i++. If it is judged that i < ns is false, the DOP value of the satellites participating in positioning is calculated, and it is judged whether the DOP value is less than the threshold. If so, the detection ends. If not, the check fails.

[0017] During specific implementation, the process of performing fault detection and repair on the data output by the altimeter includes: An error correction model is obtained through model training based on historical elevation data (this error correction model can be a non-linear function model or a neural network model). The error correction model includes the functional relationship between elevation error and elevation (for example, the functional relationship of using the least squares to solve the optimal solution of parameters for elevation error and elevation). During the positioning process, the elevation error of the elevation output by the altimeter is predicted through the error correction model. If the predicted elevation error is inconsistent with the actual elevation error, the actual elevation error is replaced with the predicted elevation error to achieve error prediction and repair of the elevation output by the altimeter and improve the altitude measurement accuracy.

[0018] During specific implementation, during the initialization process of the inertial navigation system, in order to improve the accuracy of initial alignment, the following method is proposed in this embodiment: During the initialization process of the inertial navigation system, when the carrier is in a static condition, the output data of the Beidou navigation system is used to locate the carrier, and the positioning result is used as the initial position of the inertial navigation system; for example, the carrier is first placed in a static condition, and the Beidou data is used to locate the carrier, and the positioning result is used as the initial position of the inertial navigation system. And\or When the carrier is in a static condition, the static zero bias of the inertial device is calculated according to the acceleration and angular velocity output by the inertial navigation system, and the static zero bias is subtracted from the observed value of the inertial navigation system and then output; for example, using the static condition of the carrier, the acceleration and angular velocity data output by the inertial navigation system are subjected to noise reduction processing, and then the static zero bias of the inertial device is calculated. AND\OR In order to eliminate the noise influence of inertial device data, the acceleration and angular velocity data of its own inertial output are denoised using the online smoothing method, and then the initial pitch angle is calculated in combination with the local gravity information. and roll angle , the calculation formula is:

[0019] In the formula, is the accelerometer specific force value in the three-axis direction measured by the inertial device. and\or When the moving speed of the carrier reaches the speed threshold, the first heading angle is calculated using the positioning results of the Beidou navigation system at both the previous and next epochs, the second heading angle is calculated using the speed determination result of the Beidou navigation system, a first difference between the first heading angle and the second heading angle at the previous epoch is calculated, if the first difference is less than a preset threshold, the first heading angle or the second heading angle is determined as an alternative heading angle for the previous epoch, the second difference between the first heading angle and the second heading angle at the next epoch is calculated, if the second difference is less than the preset threshold, the first heading angle or the second heading angle is determined as an alternative heading angle for the next epoch, if the difference between the alternative heading angle of the previous epoch and the alternative heading angle of the next epoch is less than the preset threshold, the average of the alternative heading angle of the previous epoch and the alternative heading angle of the next epoch is determined as the initial heading angle of the inertial navigation system.

[0020] For example, Figure 5 As shown, the initial heading angle is calculated and cross-checked using the positioning and speed results of the Beidou navigation system. Since the commonly used inertial device is a low-cost inertial navigation, it is necessary to use other sensors to provide the initial heading angle to complete the initial alignment. A speed threshold is set for the speed of the carrier. When the speed of the carrier reaches the speed threshold, the heading angle is calculated using the positioning and speed results of the Beidou navigation system, as shown in Figure 5As shown, the first heading angle is calculated using the positioning result and the second heading angle is calculated using the constant speed result at both the previous and next epochs. For example, the first heading angle is calculated using the positioning result and the second heading angle is calculated using the constant speed result at one epoch by the following formula:

[0021]

[0022] In the formula, is the speed value in the east and north directions calculated based on the position, is the difference between the carrier position and the starting reference position in the local coordinate system, is the speed value in the east and north directions based on the speed calculation, is the speed value solved by Beidou, is the first heading angle, is the second heading angle, is the time difference. The calculated first heading angle and second heading angle are cross-checked. In the first stage, each of the previous and next epochs is regarded as the current epoch, and the difference between the first heading angle and the second heading angle is calculated. If the difference is less than the preset threshold, the heading angle calculated in the current epoch is valid, and the first heading angle or the second heading angle is selected as the candidate heading angle of the current epoch. At the same time, the heading angle tested in the first stage is tested between the previous and next epochs, and the difference between the candidate heading angle of the previous epoch and the candidate heading angle of the next epoch is calculated. If the difference is less than the preset threshold, the average of the candidate heading angle of the previous epoch and the candidate heading angle of the next epoch is determined as the initial heading angle of the inertial navigation system.

[0023] In specific implementation, after initializing the inertial navigation system, the inertial navigation mechanical arrangement can be performed, such as Figure 6 As shown, the acceleration and angular velocity data of the inertial navigation system are integrated to obtain the speed, position, and attitude of the carrier. The calculation formula is as follows:

[0024]

[0025]

[0026]

[0027]

[0028] In the formula, is the rotation matrix from the carrier coordinate system to the navigation coordinate system; is the angular velocity of the Earth's rotation in the navigation coordinate system; is the angular velocity of the navigation coordinate system relative to the earth coordinate system; is the angular velocity of the carrier coordinate system relative to the inertial coordinate system; is the local gravity vector; is the radius of curvature of the ellipsoid meridian; is the radius of the ellipsoidal unitary circle; is the height from the ellipsoid; is the rotation angular velocity vector The antisymmetric matrix of ; is the differential of velocity, Acceleration data in inertial coordinate system, is the differential of latitude, is the differential of longitude, is the differential of the elevation, is the speed in the N direction, is the speed in the E direction, is the secant function of latitude, is the velocity in the D direction, . represents the derivative, and × represents the antisymmetric matrix.

[0029] During the specific implementation, in the process of inertial navigation mechanical arrangement output prediction value of the inertial navigation system, in the process of extended Kalman sub-filter, it is proposed to increase observation anomaly detection and dynamic model verification, and introduce anti-error repair. After the observation values ​​output by the first extended Kalman filter, the second extended Kalman filter and the third extended Kalman filter are inspected and eliminated for anomalies, the dynamic model is further inspected and controlled, thereby improving the stability of the filtering.

[0030] For example, using state parameter estimation and residual vectors to perform abnormality checks on the observation values ​​output by the first extended Kalman filter, the second extended Kalman filter, and the third extended Kalman filter, respectively, and repairing the detected abnormal values ​​by using anti-error filtering, including: For each of the first extended Kalman filter, the second extended Kalman filter and the third extended Kalman filter, establish a state equation of the extended Kalman filter with an inertial navigation error equation as a reference, and calculate a state parameter estimate based on the state equation; Establishing an observation equation for each of the first extended Kalman filter, the second extended Kalman filter, and the third extended Kalman filter, and outputting an observation value based on the observation equation; For each extended Kalman filter, the statistics of the state parameter estimates are calculated by the following formula:

[0031]

[0032] in, is the statistic of the state parameter estimate, is the covariance matrix, is the observed residual vector, is the variance, is the chi-square distribution, is the number of observations, is the number of state parameters, is the observation matrix, is the estimate of the state parameters, is the observation vector; If the statistic , then determine the observation vector contains outliers, and the observation vector Perform gross error positioning; The following robustness model is used to calculate the robustness factor :

[0033] in, is the standardized parameter, and is a constant; By taking the observation variance matrix and dividing it by the robustness factor way to eliminate gross errors.

[0034] In specific implementation, the Kalman filter state equation is established with the inertial guidance error equation as reference:

[0035] In the formula, is the identity matrix; is the sampling interval; is the state vector; is the optimal estimate at the previous moment; is the system noise matrix; is the process noise vector; is the observation epoch; It is the system dynamic matrix, which is derived through the physical model of the system.

[0036] The selection of the state quantity of the system equation depends on the rotation of the error state of the inertial device and the error state of the Beidou navigation system, which can be expressed as:

[0037] In the formula, is the position error in the three-axis direction; is the velocity error in the three-axis direction, is the attitude error in the three-axis direction; is the added zero bias error in the three-axis direction; is the error of the added scale factor in the three-axis direction; is the gyro bias error in the three-axis direction; is the gyro scale factor error in the three-axis direction; is the gravity acceleration error in the three-axis direction.

[0038] In the specific implementation, the observation equation of the extended Kalman subfilter based on Beidou / INS integrated navigation is established. Combined with the carrier phase observation value, the observation equation is specifically expressed as:

[0039] In the formula, is the carrier phase observation value; The carrier phase observation value predicted by the inertial navigation; is the observation coefficient matrix; is the transformation matrix from the navigation coordinate system to the earth coordinate system; is the transformation matrix from the carrier coordinate system to the earth coordinate system; is the three-axis position error in the navigation coordinate system; is the lever arm value between the inertial navigation and the satellite antenna; is the carrier wavelength of the BeiDou system; is the whole-cycle ambiguity; is the observation noise; represents the double difference operator.

[0040] In the specific implementation, the observation equation of the extended Kalman subfilter based on INS / altimeter combined navigation is established. Combined with the altimeter observation value, the observation equation is specifically expressed as:

[0041] Where Z represents the altitude observation of INS and altimeter; Indicates the altitude measured by the altimeter; Indicates the altitude calculated by the INS mechanical arrangement; Indicates elevation error.

[0042] Rewrite the above formula as:

[0043] In the formula, represents the measurement matrix; is the measurement noise error, is the state prediction value.

[0044] In the specific implementation, the observation equation of the extended Kalman subfilter based on INS / VOR integrated navigation is established, the position and velocity output by INS and VOR are respectively subtracted, and the difference is used as the measurement input of the system, thereby constructing the measurement equation. is the observation vector, , and , are the position and velocity information output by INS and VOR respectively, then the observation equation can be expressed as follows:

[0045] The error equation of the system can be expressed as:

[0046] In the formula, represents the residual vector, is the observation matrix, is the parameter state vector.

[0047] In specific implementation, during the combined filtering process, the observation information in the measurement update can be expressed as:

[0048] Observation innovation variance Combination of the observed value noise variance and the predicted state variance:

[0049] Under ideal filtering conditions, the innovation sequence should be a white noise sequence. Therefore, innovation filtering can be used to detect gross errors in the observation model. The standardized innovation vector can be expressed as:

[0050] In the formula, To standardize the parameters, the following robustness model can be used to calculate the robustness factor: :

[0051] Updated observation variance matrix It can be expressed as:

[0052] In the formula, is the observation noise matrix at time k, is the noise matrix of the observation value at time k.

[0053] In a specific implementation, a new state parameter estimate is calculated, and an abnormality detection is performed on a Kalman filter dynamics model used to construct a state equation in an extended Kalman filter based on the new state parameter estimate, including: Calculate the predicted value of the state parameter vector , calculate the new state parameter estimate With the predicted value The difference , ; The difference is calculated by the following formula Statistics :

[0054] in, for The mean square error of is a normal distribution.

[0055] According to the given significance level , determine the rejection region; if the calculated statistic W falls into the rejection region, it is judged that there is an abnormality in the Mann filter dynamics model.

[0056] In specific implementation, after obtaining the combined positioning solution, the trajectory model prediction method and the observation value combination detection method are used to perform fault detection on the positioning solution, and the positioning solution with faults is repaired. Use any one or any combination of the following methods to judge the correctness of the positioning result and repair incorrect fault positioning results: Fitting a trajectory prediction model according to the historical combined positioning solution data sequence, using the trajectory prediction model to predict the positioning result of the carrier's current epoch, comparing the difference between the positioning result solved in real time by the combined navigation positioning and the predicted positioning result, and judging whether the positioning result of the current epoch is correct according to the size of the difference; Using the fixed solution of the wide lane combination method to determine whether the positioning result of the integrated navigation positioning is correct; The fixed solution of the narrow lane combination method is used to determine whether the positioning result of the integrated navigation positioning is correct.

[0057] In specific implementation, the process of performing integrated navigation positioning using the above-mentioned airborne multi-sensor integrated navigation positioning method may include the following steps: Step 1: Detect and repair faults in sensor data; Step 1.1: Detect and repair faults in inertial navigation data After the equipment is turned on, the inertial navigation data is tested for faults in four aspects: continuity, non-zero, non-repetitive, and finite value, in combination with the inertial navigation time series. Figure 3 As shown, the inertial navigation data failure with few epochs and the inertial navigation data failure with many epochs are repaired by using the historical epoch replacement method and the polynomial fitting function prediction method respectively.

[0058] Step 1.2: Detect and repair Beidou data faults Receive Beidou satellite observation data, according to Figure 4 As shown in the figure, the satellite observation data are detected and eliminated from four aspects, namely, signal-to-noise ratio, altitude angle, DOP value and RAIM, to prevent satellite positioning anomalies caused by poor quality of individual satellite data.

[0059] Step 1.3, fault detection and repair of altimeter data Prior model training is performed on elevation data. The height measurement data and residual values ​​are combined. The nonlinear function model is used to apply the least square method to the elevation error and elevation to obtain the optimal solution of the parameters, and a nonlinear function error correction model is constructed. During the positioning process, the error of the elevation output by the altimeter is predicted and repaired to improve the height measurement accuracy.

[0060] Step 2, after initializing the inertial navigation system using the inertial measurement data, integrate to obtain the position, velocity, and attitude results predicted by the inertial navigation system; Step 2.1: Initially align the INS based on static conditions and Beidou positioning results First, the carrier is placed in a static condition and positioned using Beidou data. The positioning result is used as the initial position of the inertial navigation system. At the same time, the acceleration and angular velocity data output by the inertial navigation system are subjected to noise reduction using the static condition of the carrier, and then the static zero bias of the inertial device is calculated. The measured data of the inertial navigation system is subtracted from the static zero bias and then output.

[0061] In order to eliminate the noise influence of inertial device data, the acceleration and angular velocity data of its own inertial output are denoised using the online smoothing method, and then the pitch angle is calculated in combination with the local gravity information. and roll angle , the calculation formula is:

[0062] In the formula, It is the accelerometer specific force value in the three-axis direction measured by the inertial device.

[0063] Step 2.2: Calculate the initial heading angle using the cross detection method Since the inertial device in the present invention is a low-cost inertial navigation system, it is necessary to use other sensors to provide an initial heading angle to complete the initial alignment. A threshold is set for the speed of the carrier movement. When the carrier movement speed reaches the threshold, the Beidou positioning result and the constant speed result are used to calculate the heading angle respectively. , ,like Figure 5 As shown, the heading angles calculated by the two methods are checked against each other, and the heading angle that meets the requirements is used as the initial heading angle of the inertial navigation system. The calculation formula is:

[0064]

[0065] In the formula, is the speed value in the east and north directions calculated based on the position, is the difference between the carrier position and the starting reference position in the local coordinate system, is the speed value in the east and north directions based on the speed calculation, They are the speed values ​​solved by Beidou respectively. is the first heading angle, is the second heading angle, The time difference between before and after.

[0066] like Figure 5 The steps shown in the figure use the Beidou positioning information of the current epoch and the positioning information of the previous epoch to calculate the fixed solution position difference between the two in the local coordinate system, and then find the speed values ​​in the northeast and northeast directions. The speed value is combined with trigonometric function to calculate the first heading angle. , and determine whether the heading angle meets the speed threshold. If so, output the first heading angle ; Use Beidou differential velocity information combined with trigonometric function to calculate the second heading angle , and determine whether the heading angle meets the speed threshold, if so, output the second heading angle ; The calculated heading angle and Perform a cross check, calculate a first difference between the first heading angle and the second heading angle at a previous epoch, if the first difference is less than a preset threshold, determine the first heading angle or the second heading angle as an alternative heading angle for the previous epoch, calculate a second difference between the first heading angle and the second heading angle at a subsequent epoch, if the second difference is less than the preset threshold, determine the first heading angle or the second heading angle as an alternative heading angle for the subsequent epoch, if the difference between the alternative heading angle of the previous epoch and the alternative heading angle of the subsequent epoch is less than the preset threshold, determine the average of the alternative heading angle of the previous epoch and the alternative heading angle of the subsequent epoch as the initial heading angle of the inertial navigation system.

[0067] Step 2.3, perform inertial navigation mechanical arrangement, integrate the acceleration and angular velocity data to obtain the carrier's speed, position, and attitude, such as Figure 6As shown, first use the angular velocity output by the gyroscope, the angular velocity of the navigation coordinate system relative to the earth coordinate system, and the angular velocity of the earth's rotation to calculate the attitude matrix through the attitude matrix (i.e., rotation matrix) update algorithm, and then infer the attitude angle of the carrier from the attitude matrix through Euler angle calculation. Then use the updated attitude matrix to convert the acceleration in the inertial coordinate system to the navigation coordinate system, and then solve the velocity (i.e., earth velocity and carrier velocity calculation) and position differential equations (i.e., position velocity integral) combined with Coriolis correction and local gravity compensation to obtain the position and velocity results of the carrier. The calculation formula is as follows:

[0068]

[0069]

[0070]

[0071]

[0072] In the formula, is the rotation matrix from the carrier coordinate system to the navigation coordinate system; is the angular velocity of the Earth's rotation in the navigation coordinate system; is the angular velocity of the navigation coordinate system relative to the earth coordinate system; is the angular velocity of the carrier coordinate system relative to the inertial coordinate system; is the local gravity vector; is the radius of curvature of the ellipsoid meridian; is the radius of the ellipsoidal unitary circle; is the height from the ellipsoid; is the rotation angular velocity vector The antisymmetric matrix of is the differential of velocity, Acceleration data in inertial coordinate system, is the differential of latitude, is the differential of longitude, is the differential of the elevation, is the speed in the N direction, is the speed in the E direction, is the secant function of latitude, is the speed in the D direction.

[0073] Step 3: Establish an extended Kalman filter architecture based on multiple sensors such as Beidou / INS / altimeter / VOR, and establish state equations and measurement equations; Step 3.1: Establish the Kalman filter state equation with the inertial guidance error equation as reference

[0074] In the formula, is the identity matrix; is the sampling interval; is the state vector; is the system noise matrix; is the process noise vector; is the observation epoch; It is the system dynamic matrix, which is derived through the physical model of the system; is the optimal estimate at the previous moment.

[0075] The selection of the state quantity of the system equation depends on the rotation of the error state of the inertial device and the error state of the Beidou system, which can be expressed as:

[0076] In the formula, is the position error in the three-axis direction; is the velocity error in the three-axis direction, is the attitude error in the three-axis direction; is the added zero bias error in the three-axis direction; is the error of the added scale factor in the three-axis direction; is the gyro bias error in the three-axis direction; is the gyro scale factor error in the three-axis direction; is the gravity acceleration error in the three-axis direction.

[0077] Step 3.2: Establish the extended Kalman subfilter observation equation based on Beidou / INS integrated navigation Combined with the carrier phase observation value, the observation equation of BeiDou / INS is specifically expressed as:

[0078] In the formula, is the carrier phase observation value; The carrier phase observation value predicted by the inertial navigation; is the observation coefficient matrix; is the transformation matrix from the navigation coordinate system to the earth coordinate system; is the transformation matrix from the carrier coordinate system to the earth coordinate system; is the lever arm value between the inertial navigation and the satellite antenna; is the carrier wavelength of the BeiDou system; is the whole-cycle ambiguity; is the observation noise; represents the double difference operator; is the three-axis position error in the navigation coordinate system.

[0079] Step 3.3, establish the observation equation based on INS / altimeter extended Kalman subfilter Combined with the altimeter observations, the INS / altimeter observation equation is specifically expressed as:

[0080] Where Z represents the altitude observation of INS and altimeter; Indicates the altitude measured by the altimeter; Indicates the altitude calculated by the INS mechanical arrangement; Indicates elevation error.

[0081] Rewrite the above formula as:

[0082] In the formula, represents the measurement matrix; is the measurement noise error; is the state prediction value.

[0083] Step 3.4: Establish observation equation based on INS / VOR extended Kalman subfilter The position and velocity output by INS and VOR are subtracted and the difference is used as the measurement input of the system to construct the measurement equation. is the observation vector, , and , are the position and velocity information output by INS and VOR respectively, then the observation equation can be expressed as follows:

[0084] The error equation of the system can be expressed as:

[0085] In the formula, represents the residual vector, is the observation matrix, is the parameter state vector.

[0086] Step 4: Use state parameter estimation and residual vector to check and repair observation anomalies, perform robust repair on faulty sub-filters, and then perform anomaly detection on the Kalman filter dynamics model based on reliable observations; Step 4.1: Use state parameter estimation and residual vector to check and repair the observed anomalies of each sub-filter At time k, the estimate of the state parameters can be obtained:

[0087] The constructed statistics are:

[0088]

[0089] if , then the observation vector Contains outliers, is the covariance matrix, is the observation residual vector.

[0090] Then the gross error of the observed residual vector is located, that is,

[0091] In the formula, is the statistic of the observed residual vector; is the i-th row vector of the observed residual vector; (3) is the mean square error of the observed residual vector, and Correspondingly, the statistic W of the observed residual vector is used to determine whether there is a gross error. is a normal distribution.

[0092] Step 4.2: Construct an observation robustness filtering model to detect and eliminate gross errors in observation information. In the combined filtering process, the observation information in the measurement update can be expressed as:

[0093] In the formula, is the observation value innovation vector; is the observation vector; is the observation coefficient matrix; is the state prediction value.

[0094] Observation innovation variance Combination of the observed value noise variance and the predicted state variance

[0095] Under ideal filtering conditions, the innovation sequence should be a white noise sequence. Therefore, innovation filtering can be used to detect gross errors in the observation model. The standardized innovation vector can be expressed as:

[0096] In the formula, is the standardized parameter, is the i-th row vector of the observation innovation vector at time k, is the variance of the observation value in the i-th row and i-th column at time k. The following robustness model can be used to calculate the robustness factor :

[0097] The observed value variance matrix after updating the observed value noise matrix at time k It can be expressed as:

[0098] In the formula, is the observation noise matrix at time k, is the noise matrix of the observation value at time k. Step 4.3: Based on the reliable observation, anomaly detection is performed on the Kalman filter dynamics model used to construct the state equation In the first two steps, the observed anomalies were checked and eliminated, and the geometric solution of the state parameters was recalculated after the inspection. , then the newly obtained Relatively reliable. The predicted value of the state parameter vector is calculated by the dynamic model , calculate the difference:

[0099] It mainly reflects the error of the dynamic model, and its statistics are calculated as:

[0100] In the formula, for The mean square error of is a normal distribution; The expression is:

[0101] Assume that there is no gross error in the data, that is, the statistic W of the observed residual vector conforms to the normal distribution (0, 1); Calculate the statistic W using the statistic W calculation formula; According to the given significance level , determine the rejection region (usually obtained by looking up the normal distribution table). If the calculated statistic W falls into the rejection region, it is considered that there is an anomaly in the Kalman filter dynamics model.

[0102] Step 5: Use reset federated Kalman filter for optimal information fusion and information allocation After completing the measurement update of each sub-filter and repairing the faulty filter, the reset federated Kalman filter is used for optimal information fusion.

[0103] 1) Optimal Information Fusion

[0104]

[0105]

[0106] In the formula, is the covariance matrix of the federated main filter; is the covariance matrix of the nth sub-filter; N represents the total number of sub-filters; represents the optimal state estimate of the federated main filter; represents the measurement update estimate of the nth sub-filter; Main filter system noise covariance matrix; represents the system noise covariance matrix of the nth sub-filter.

[0107] 2) Information distribution The fused optimal estimation result is distributed to each sub-filter according to the following formula as the initial value of the next epoch sub-filter:

[0108]

[0109]

[0110] In the formula, represents the initial prediction value of the next epoch of the subfilter; represents the initial covariance matrix of the next epoch of the sub-filter; Represents the initial system noise matrix for the next epoch.

[0111] Step 6: perform fault detection and repair on the correctness of the integrated navigation positioning solution; After obtaining the combined positioning solution, the trajectory model prediction method and the observation value combination detection method are used to detect faults in the positioning solution, and the positioning solution with faults is repaired.

[0112] Step 6.1: Fault detection and repair using trajectory prediction model A trajectory prediction model is fitted according to the historical combined positioning solution data sequence, and the positioning result of the carrier's current epoch is predicted using the trajectory prediction model. The difference between the positioning result solved in real time by the combined navigation positioning and the predicted positioning result is compared, and whether the positioning result of the current epoch is correct is determined based on the size of the difference.

[0113] Step 6.2: Fault detection and repair using wide lane combination method The wide lane combination is a combination of the carrier phase observation equation by selecting parameters from the Beidou dual-frequency phase measurement, which is defined as:

[0114] in, is the wide-lane combined carrier observation value, is the frequency 1 carrier observation value, is the frequency 2 carrier observation value.

[0115] The corresponding combined wavelength ,frequency and combined fuzziness for:

[0116]

[0117]

[0118] in, Frequency 1 wavelength, Frequency 2 wavelength, C represents the speed of light, is frequency 1, is frequency 2, is the frequency 1 ambiguity, is the frequency 2 ambiguity.

[0119] After wide-lane combination, the wavelength is longer than the original observation value, and the combined wavelength can more easily determine the ambiguity , use the wide lane fixed solution to test the combined solution and determine whether the combined solution is correct.

[0120] Step 6.3: Fault detection and repair using the narrow lane combination method The principle of narrow lane combination is the same as that of wide lane combination. Its combination coefficient is the sum of two carrier phase observation equations, which can be expressed as:

[0121] The corresponding wavelength ,frequency and integer ambiguity for:

[0122]

[0123]

[0124] After the narrow lanes are combined, the corresponding wavelength becomes shorter. The shorter wavelength is not conducive to calculating the ambiguity problem of the narrow lane, but the root mean square error of the measurement value will be reduced, which can be applied to precise positioning. The residual of the narrow lane fixed solution is used to detect the combined solution to determine whether the solution is correct.

[0125] In the specific implementation, in order to verify the performance of fault detection and fault repair of the above-mentioned airborne multi-sensor combined navigation and positioning method, a positioning simulation experiment was carried out, and faults such as raw data error, filter residual error, and combined positioning result error were simulated respectively. The errors of no fault, traditional fault repair method, and the fault repair method of the present invention (i.e., the above-mentioned airborne multi-sensor combined navigation and positioning method) were compared and analyzed. The position error comparison results are as follows: Figure 7 , 8 As shown in FIG9 . As can be seen from the figure, the airborne multi-sensor combined navigation and positioning method of the present application realizes accurate fault detection and repair and is superior to the traditional fault repair method, the positioning error is greatly improved, and the overall fault tolerance and positioning accuracy of the navigation system are improved.

[0126] In this embodiment, a computer device is provided, such as Fig.10 As shown, it includes a memory 1001, a processor 1002, and a computer program stored in the memory and executable on the processor, and when the processor executes the computer program, any of the above-mentioned airborne multi-sensor combined navigation and positioning methods is implemented.

[0127] Specifically, the computer device may be a computer terminal, a server or a similar computing device.

[0128] In this embodiment, a computer-readable storage medium is provided, wherein the computer-readable storage medium stores a computer program for executing any of the above-mentioned airborne multi-sensor combined navigation and positioning methods.

[0129] Specifically, computer-readable storage media include permanent and non-permanent, removable and non-removable media that can be used to store information by any method or technology. Information can be computer-readable instructions, data structures, program modules or other data. Examples of computer-readable storage media include, but are not limited to, phase change memory (PRAM), static random access memory (SRAM), dynamic random access memory (DRAM), other types of random access memory (RAM), read-only memory (ROM), electrically erasable programmable read-only memory (EEPROM), flash memory or other memory technology, compact disk read-only memory (CD-ROM), digital versatile disk (DVD) or other optical storage, magnetic cassettes, tape disk storage or other magnetic storage devices or any other non-transmission media that can be used to store information that can be accessed by a computing device. As defined herein, computer-readable storage media does not include temporary computer-readable media (transitory media), such as modulated data signals and carrier waves.

[0130] Based on the same inventive concept, an airborne multi-sensor combined navigation and positioning device is also provided in the embodiments of the present invention, as described in the following embodiments. Since the principle of solving the problem by the airborne multi-sensor combined navigation and positioning device is similar to that of the airborne multi-sensor combined navigation and positioning method, the implementation of the airborne multi-sensor combined navigation and positioning device can refer to the implementation of the airborne multi-sensor combined navigation and positioning method, and the repeated parts will not be repeated. As used below, the term "unit" or "module" can be a combination of software and / or hardware that implements a predetermined function. Although the device described in the following embodiments is preferably implemented in software, the implementation of hardware, or a combination of software and hardware, is also possible and conceived.

[0131] Fig.11 is a structural block diagram of an airborne multi-sensor combined navigation and positioning device according to an embodiment of the present invention. Fig.11 As shown, including: The source data fault detection module 1101 is used to perform fault detection and repair on the data output by the inertial navigation system, the data output by the Beidou navigation system and the data output by the altimeter before outputting the predicted value in the first combination, the second combination and the third combination, wherein the Beidou navigation system and the inertial navigation system form a first combination, the altimeter and the inertial navigation system form a second combination, and the radio navigation system and the inertial navigation system form a third combination. In the first combination, the observation value output by the Beidou navigation system and the first predicted value output by the inertial navigation system are fused and input into a first extended Kalman filter, and the first extended Kalman filter outputs The first observation value of the first combination, the observation value output by the altimeter in the second combination and the first prediction value output by the inertial navigation system are fused and input into a second extended Kalman filter, the second extended Kalman filter outputs the second observation value of the second combination, the observation value output by the radio navigation system in the third combination and the first prediction value output by the inertial navigation system are fused and input into a third extended Kalman filter, the third extended Kalman filter outputs the third observation value of the third combination, and the first observation value, the second observation value, the third observation value and the second prediction value of the inertial navigation system are fused and then a positioning result is output; An observation value abnormality detection module 1102 is used to perform abnormality detection on the observation values ​​output by the first extended Kalman filter, the second extended Kalman filter and the third extended Kalman filter respectively by using state parameter estimation and residual vector in the process of outputting predicted values ​​of the first combination, the second combination and the third combination, and repair the detected abnormal values ​​by using anti-error filtering; The model anomaly detection module 1103 is used to perform anomaly inspection and repair on the observation values ​​output by the first extended Kalman filter, the second extended Kalman filter and the third extended Kalman filter, respectively, and calculate new state parameter estimates, and perform anomaly detection on the Kalman filter dynamics model used to construct the state equation in the extended Kalman filter based on the new state parameter estimates.

[0132] In one embodiment, the above device further comprises: An initial module, used to locate the carrier using the output data of the Beidou navigation system when the carrier is in a static condition during the initialization process of the inertial navigation system, and use the positioning result as the initial position of the inertial navigation system; and\or When the carrier is in a static condition, the static zero bias of the inertial device is calculated according to the acceleration and angular velocity output by the inertial navigation system, and the static zero bias is subtracted from the observed value of the inertial navigation system and then output; and\or When the moving speed of the carrier reaches the speed threshold, the first heading angle is calculated using the positioning results of the Beidou navigation system at both the previous and next epochs, the second heading angle is calculated using the speed determination result of the Beidou navigation system, the first difference between the first heading angle and the second heading angle at the previous epoch is calculated, if the first difference is less than a preset threshold, the first heading angle or the second heading angle is determined as the candidate heading angle of the previous epoch, the second difference between the first heading angle and the second heading angle at the next epoch is calculated, if the second difference is less than the preset threshold, the first heading angle or the second heading angle is determined as the candidate heading angle of the next epoch, if the difference between the candidate heading angle of the previous epoch and the candidate heading angle of the next epoch is less than the preset threshold, the average of the candidate heading angle of the previous epoch and the candidate heading angle of the next epoch is determined as the initial heading angle of the inertial navigation system.

[0133] In one embodiment, the above device further comprises: A repair module is used to judge the correctness of the positioning result and repair incorrect fault positioning results by using any one or any combination of the following methods: fitting a trajectory prediction model according to a historical combined positioning solution data sequence, using the trajectory prediction model to predict the positioning result of the carrier at the current epoch, comparing the difference between the positioning result solved in real time by the combined navigation positioning and the predicted positioning result, and judging whether the positioning result of the current epoch is correct according to the size of the difference; judging whether the positioning result of the combined navigation positioning is correct by using the fixed solution of the wide lane combination method; judging whether the positioning result of the combined navigation positioning is correct by using the fixed solution of the narrow lane combination method.

[0134] The embodiments of the present invention achieve the following technical effects: it proposes to use the Beidou navigation system, altimeter, and radio navigation system to form different combinations with the inertial navigation system respectively, and the observation values ​​output by the sensors in each combination are combined with the predicted values ​​of the inertial navigation system and the extended Kalman filter is fused to output the state estimation value. After completing the measurement update of each sub-filter, the reset federal Kalman filter is used to perform optimal information fusion, and then the optimal state estimation value is obtained, thereby realizing the construction of an expandable multi-sensor combined navigation and positioning fusion framework, each sub-filter (i.e., the extended Kalman filter) is independent of each other, and it is convenient to add other new navigation and positioning sensors as needed, thereby improving the scalability; at the same time, it is proposed to respectively detect and repair the faults of the data output by the inertial navigation system, the data output by the Beidou navigation system, and the data output by the altimeter, so that The method can eliminate or reduce the adverse effects of errors at the level of the original data source on the positioning results, and can also detect the problem of simultaneous failure of multiple sensors; it also proposes to use state parameter estimation and residual vectors to perform abnormality checks on the observation values ​​output by the first extended Kalman filter, the second extended Kalman filter, and the third extended Kalman filter, and use anti-error filtering to repair the detected abnormal values, thereby reducing the adverse effects of abnormalities in the output observation values ​​of each combination on the positioning results. After checking and eliminating the observation anomalies, the Kalman filter dynamics model is further checked and controlled, which improves the stability of the filter and the estimation accuracy of the sub-filter, which is beneficial to reducing the adverse effects of inaccurate system models or large modeling errors on the positioning results, and further helps to improve the fault tolerance rate of combined navigation positioning.

[0135] Obviously, those skilled in the art should understand that the modules or steps of the above-mentioned embodiments of the present invention can be implemented by a general computing device, they can be concentrated on a single computing device, or distributed on a network composed of multiple computing devices, and optionally, they can be implemented by a program code executable by a computing device, so that they can be stored in a storage device and executed by the computing device, and in some cases, the steps shown or described can be executed in a different order from that here, or they can be made into individual integrated circuit modules, or multiple modules or steps therein can be made into a single integrated circuit module for implementation. In this way, the embodiments of the present invention are not limited to any specific combination of hardware and software.

[0136] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. For those skilled in the art, the embodiments of the present invention may have various modifications and variations. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention shall be included in the protection scope of the present invention.

Claims

1. An airborne multi-sensor combined navigation and positioning method, characterized in that: A Beidou navigation system and an inertial navigation system form a first combination, an altimeter and the inertial navigation system form a second combination, and a radio navigation system and the inertial navigation system form a third combination. In the first combination, an observation value output by the Beidou navigation system and a first prediction value output by the inertial navigation system are fused and input into a first extended Kalman filter, and the first extended Kalman filter outputs a first observation value of the first combination. In the second combination, an observation value output by the altimeter and a first prediction value output by the inertial navigation system are fused and input into a second extended Kalman filter, and the second extended Kalman filter outputs a second observation value of the second combination. In the third combination, an observation value output by the radio navigation system and a first prediction value output by the inertial navigation system are fused and input into a third extended Kalman filter, and the third extended Kalman filter outputs a third observation value of the third combination. The first observation value, the second observation value, the third observation value and the second prediction value of the inertial navigation system are fused and then a positioning result is output. The method comprises: Before the first combination, the second combination, and the third combination output predicted values, respectively performing fault detection and repair on the data output by the inertial navigation system, the data output by the Beidou navigation system, and the data output by the altimeter; In the process of outputting predicted values ​​of the first combination, the second combination and the third combination, the observed values ​​output by the first extended Kalman filter, the second extended Kalman filter and the third extended Kalman filter are respectively tested for abnormalities using state parameter estimation and residual vectors, and the detected abnormal values ​​are repaired by using anti-error filtering; After performing abnormality inspection and repair on the observation values ​​output by the first extended Kalman filter, the second extended Kalman filter, and the third extended Kalman filter, respectively, new state parameter estimates are calculated, and based on the new state parameter estimates, abnormality detection is performed on the Kalman filter dynamics model used to construct the state equation in the extended Kalman filter.

2. The airborne multi-sensor combined navigation and positioning method according to claim 1, characterized in that: Performing fault detection and repair on the data output by the inertial navigation system, including: In combination with the inertial navigation time series, fault detection is performed on the data output by the inertial navigation system from various aspects such as continuity, non-zero, non-repeatability and finite value. For the missing data of the detected inertial navigation data fault with a few epochs, the historical epoch data adjacent to the missing data are used to replace the missing data for repair. For the data of the detected inertial navigation data fault with multiple epochs, a polynomial fitting function is performed using adjacent epoch data, and the data predicted by the fitting function is used for repair.

3. The airborne multi-sensor combined navigation and positioning method according to claim 1, characterized in that: Performing fault detection and repair on the data output by the altimeter, including: The error correction model is obtained by model training based on historical elevation data. The error correction model includes a functional relationship between elevation error and elevation. During the positioning process, the elevation error output by the altimeter is predicted through the error correction model. If the predicted elevation error is inconsistent with the actual elevation error, the actual elevation error is replaced by the predicted elevation error.

4. The airborne multi-sensor combined navigation and positioning method according to claim 1, characterized in that: In the process of outputting predicted values ​​of the first combination, the second combination, and the third combination, abnormality inspection is performed on the observed values ​​output by the first extended Kalman filter, the second extended Kalman filter, and the third extended Kalman filter respectively using state parameter estimation and residual vectors, and the detected abnormal values ​​are repaired by anti-error filtering, including: For each of the first extended Kalman filter, the second extended Kalman filter and the third extended Kalman filter, establish a state equation of the extended Kalman filter with an inertial navigation error equation as a reference, and calculate a state parameter estimate based on the state equation; Establishing an observation equation for each of the first extended Kalman filter, the second extended Kalman filter, and the third extended Kalman filter, and outputting an observation value based on the observation equation; For each extended Kalman filter, the statistics of the state parameter estimates are calculated by the following formula: in, is the statistic of the state parameter estimate, is the covariance matrix, is the observed residual vector, is the variance, is the chi-square distribution, is the number of observations, is the number of state parameters, is the observation matrix, is the estimate of the state parameters, is the observation vector; If the statistic , then determine the observation vector contains outliers, and the observation vector To locate the gross error, is the significance level; The following robustness model is used to calculate the robustness factor : in, is the standardized parameter, and is a constant; By taking the observation variance matrix and dividing it by the robustness factor way to eliminate gross errors.

5. The airborne multi-sensor combined navigation and positioning method according to claim 1, characterized in that: Calculating a new state parameter estimate, and performing anomaly detection on a Kalman filter dynamics model used to construct a state equation in an extended Kalman filter based on the new state parameter estimate, including: Calculate the predicted value of the state parameter vector , calculate the new state parameter estimate and the predicted value The difference ; The difference is calculated by the following formula Statistics : in, for The mean square error of is a normal distribution; According to the given significance level , determine the rejection region, if the statistic W falls into the rejection region, it is judged that there is an abnormality in the Kalman filter dynamics model.

6. The airborne multi-sensor combined navigation and positioning method according to any one of claims 1 to 5, characterized in that: Also includes: During the initialization process of the inertial navigation system, when the carrier is in a static condition, the carrier is positioned using the output data of the Beidou navigation system, and the positioning result is used as the initial position of the inertial navigation system; and\or When the carrier is in a static condition, the static zero bias of the inertial device is calculated according to the acceleration and angular velocity output by the inertial navigation system, and the static zero bias is subtracted from the observed value of the inertial navigation system and then outputted; and\or When the moving speed of the carrier reaches the speed threshold, the first heading angle is calculated using the positioning results of the Beidou navigation system at both the previous and next epochs, the second heading angle is calculated using the speed determination result of the Beidou navigation system, the first difference between the first heading angle and the second heading angle at the previous epoch is calculated, if the first difference is less than a preset threshold, the first heading angle or the second heading angle is determined as the candidate heading angle of the previous epoch, the second difference between the first heading angle and the second heading angle at the next epoch is calculated, if the second difference is less than the preset threshold, the first heading angle or the second heading angle is determined as the candidate heading angle of the next epoch, if the difference between the candidate heading angle of the previous epoch and the candidate heading angle of the next epoch is less than the preset threshold, the average of the candidate heading angle of the previous epoch and the candidate heading angle of the next epoch is determined as the initial heading angle of the inertial navigation system.

7. The airborne multi-sensor combined navigation and positioning method according to any one of claims 1 to 5, characterized in that: Also includes: Use any one or any combination of the following methods to judge the correctness of the positioning result and repair incorrect fault positioning results: Fitting a trajectory prediction model according to the historical combined positioning solution data sequence, using the trajectory prediction model to predict the positioning result of the carrier's current epoch, comparing the difference between the positioning result solved in real time by the combined navigation positioning and the predicted positioning result, and judging whether the positioning result of the current epoch is correct according to the size of the difference; Using the fixed solution of the wide lane combination method to determine whether the positioning result of the integrated navigation positioning is correct; The fixed solution of the narrow lane combination method is used to determine whether the positioning result of the integrated navigation positioning is correct.

8. An airborne multi-sensor combined navigation and positioning device, characterized in that: include: A source data fault detection module is used to perform fault detection and repair on the data output by the inertial navigation system, the data output by the Beidou navigation system and the data output by the altimeter before outputting the predicted value in the first combination, the second combination and the third combination, wherein the Beidou navigation system and the inertial navigation system form the first combination, the altimeter and the inertial navigation system form the second combination, and the radio navigation system and the inertial navigation system form the third combination. In the first combination, the observation value output by the Beidou navigation system and the first predicted value output by the inertial navigation system are fused and input into a first extended Kalman filter. The first extended Kalman filter Outputting a first observation value of the first combination, fusing the observation value output by the altimeter in the second combination with the first prediction value output by the inertial navigation system and inputting them into a second extended Kalman filter, the second extended Kalman filter outputting a second observation value of the second combination, fusing the observation value output by the radio navigation system in the third combination with the first prediction value output by the inertial navigation system and inputting them into a third extended Kalman filter, the third extended Kalman filter outputting a third observation value of the third combination, and fusing the first observation value, the second observation value, the third observation value and the second prediction value of the inertial navigation system and outputting a positioning result; An observation value abnormality detection module is used to use state parameter estimation and residual vector to perform abnormality detection on the observation values ​​output by the first extended Kalman filter, the second extended Kalman filter and the third extended Kalman filter respectively during the process of outputting predicted values ​​of the first combination, the second combination and the third combination, and repair the detected abnormal values ​​by using anti-error filtering; A model anomaly detection module is used to perform anomaly inspection and repair on the observation values ​​output by the first extended Kalman filter, the second extended Kalman filter and the third extended Kalman filter, respectively, and calculate new state parameter estimates, and perform anomaly detection on the Kalman filter dynamics model used to construct the state equation in the extended Kalman filter based on the new state parameter estimates.

9. 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 computer program, the airborne multi-sensor combined navigation and positioning method according to any one of claims 1 to 7 is implemented.

10. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores a computer program for executing the airborne multi-sensor combined navigation and positioning method according to any one of claims 1 to 7.

Images