CN111060133B - Integrated navigation integrity monitoring method for urban complex environment - Google Patents
Integrated navigation integrity monitoring method for urban complex environment Download PDFInfo
- Publication number
- CN111060133B CN111060133B CN201911225922.1A CN201911225922A CN111060133B CN 111060133 B CN111060133 B CN 111060133B CN 201911225922 A CN201911225922 A CN 201911225922A CN 111060133 B CN111060133 B CN 111060133B
- Authority
- CN
- China
- Prior art keywords
- satellite
- state
- fault
- data
- matrix
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 50
- 238000012544 monitoring process Methods 0.000 title claims abstract description 13
- 230000008569 process Effects 0.000 claims abstract description 5
- 239000011159 matrix material Substances 0.000 claims description 45
- 238000005259 measurement Methods 0.000 claims description 25
- 230000000007 visual effect Effects 0.000 claims description 24
- 230000004927 fusion Effects 0.000 claims description 13
- 238000004364 calculation method Methods 0.000 claims description 9
- 238000001914 filtration Methods 0.000 claims description 9
- 230000003044 adaptive effect Effects 0.000 claims description 8
- 238000007781 pre-processing Methods 0.000 claims description 5
- 230000001133 acceleration Effects 0.000 claims description 4
- 230000007704 transition Effects 0.000 claims description 4
- 238000006467 substitution reaction Methods 0.000 claims description 2
- 238000001514 detection method Methods 0.000 abstract description 20
- 238000013461 design Methods 0.000 abstract description 7
- 238000003745 diagnosis Methods 0.000 abstract description 7
- 239000000284 extract Substances 0.000 abstract description 2
- 238000005516 engineering process Methods 0.000 description 8
- 238000013459 approach Methods 0.000 description 6
- 238000013528 artificial neural network Methods 0.000 description 4
- 230000006872 improvement Effects 0.000 description 3
- 230000000694 effects Effects 0.000 description 2
- 230000007717 exclusion Effects 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 239000000243 solution Substances 0.000 description 2
- 238000004458 analytical method Methods 0.000 description 1
- 238000012937 correction Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000010790 dilution Methods 0.000 description 1
- 239000012895 dilution Substances 0.000 description 1
- 230000008030 elimination Effects 0.000 description 1
- 238000003379 elimination reaction Methods 0.000 description 1
- 238000007689 inspection Methods 0.000 description 1
- 238000002955 isolation Methods 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 238000013178 mathematical model Methods 0.000 description 1
- 238000003062 neural network model Methods 0.000 description 1
- 239000002245 particle Substances 0.000 description 1
- 230000035945 sensitivity Effects 0.000 description 1
- 238000012549 training Methods 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
Landscapes
- Engineering & Computer Science (AREA)
- Manufacturing & Machinery (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Navigation (AREA)
Abstract
The invention provides a method for monitoring the integrity of integrated navigation in a complex urban environment. Meanwhile, the invention designs a self-adaptive threshold, extracts a dynamic clustering radius by using the related information of the fault candidate point, and adopts the advantages of the dynamic clustering radius: firstly, whether a fault occurs or not is judged quickly and reliably in fault detection by determining the dynamic clustering radius, and the real-time performance is improved. And secondly, after the dynamic clustering radius is determined, determining candidate points in the fault data candidate points as fault data in the subsequent clustering process, thereby eliminating the fault data, realizing the diagnosis of the fault, and having simple design and clear thought.
Description
Technical Field
The invention belongs to the technical field of navigation positioning, and particularly relates to a combined navigation integrity monitoring method for a complex urban environment.
Background
The method is used for monitoring the integrity of the multi-sensor fused combined navigation fault condition in the urban complex environment, wherein the most critical problem is that fault detection and diagnosis links can provide accurate and timely fault information. At present, the main methods adopted at home and abroad for the Fault Detection and Elimination (FDE) of the navigation system are as follows:
1) a hardware redundancy based approach. The hardware redundancy based approach is the earliest fault detection and diagnosis approach. In the 60's of the 20 th century, this approach was widely used to develop high performance aircraft. Because the means investment is large, the fault detection and diagnosis method based on software redundancy is gradually developed, and a fault diagnosis method combining software redundancy with the software redundancy is formed;
2) based on x2The detection method comprises the following steps: chi shape2The method of checking is widely used in fault detection of combined navigation, and it uses the state provided by Kalman filter to compare with the state of another auxiliary filter which does not accept measurement update, and judges the validity of the filter output in real time, but its effect is not satisfactory. In 1992, Ren Da proposed an improvement: the method adopts 2 auxiliary filters to carry out periodical exchange correction, so as to avoid the increasing error between the state recursor and the real state; in addition, inCarry out chi2During inspection, the method for independently detecting 2 state variables instead of the whole vector is adopted, and chi is increased2The sensitivity to failure is checked. Aiming at the defect that the error is accumulated along with the time in the single-state recursion, a state recursion device updating judgment criterion taking the speed error as an updating basis is provided.
3) Parity space based approach: the basic idea of the parity space method is to construct a parity matrix according to hardware redundancy or analytic redundancy equation of the system, check the parity of a mathematical model of the system by using actual observed quantity, find a parity equation decoupled with a fault, construct a parity vector and a fault detection function, and thus, can carry out fault detection and diagnosis;
4) data-driven methods: represented typically as a neural network, it is common practice for fault detection to: selecting variables capable of reflecting system faults as input quantity of a neural network, and selecting output quantity according to fault types needing to be judged; selecting an appropriate neural network model; training a neural network according to historical or empirical data of input and output information, and determining a weight and a threshold; when new input information is applied to the neural network and the output of the network exceeds a given threshold, the system may be considered to have failed.
The method based on hardware redundancy is generally used in the high-precision aviation field in the early stage, and is high in manufacturing cost and large in investment. Based on x2In the actual application of the detection method, the threshold for judging the occurrence of the fault is too single, the threshold cannot be divided according to different application scenes, if the threshold is too small, data without the fault is judged to be the fault, so that the false alarm rate is increased, the calculated amount is increased, and the real-time performance of the algorithm is reduced. If the threshold value is too large, the faulty data can escape fault detection, the missing detection rate is increased, and the positioning result is greatly influenced. In the actual application, the method based on the odd-even space and the data driving method are influenced by various factors such as uncertainty, external interference and the like of a system model, so that a plurality of models are adopted in algorithm design, the design is complex, and the calculated amount is large.
Interpretation of terms: GNSS: global Navigation Satellite System (Global Navigation Satellite System);
GPS: global Positioning System (Global Positioning System);
INS: inertial Navigation systems (Inertial Navigation System);
an IMU: an Inertial Measurement Unit (Inertial Measurement Unit);
RAIM: receiver Autonomous Integrity Monitoring;
FDE: error detection and exclusion (failure detection and exclusion).
PDOP: position precision factor (position dilution of precision)
Disclosure of Invention
The purpose of the invention is as follows: the invention aims to solve the technical problem of the prior art, and provides a method for monitoring the integrity of integrated navigation in a complex urban environment, which comprises the following steps:
step 1, preprocessing satellite data received by a satellite receiver;
and 2, detecting fault data aiming at the preprocessed satellite data, and removing the fault data.
The step 1 comprises the following steps: according to satellite data received by a receiver, randomly grouping the satellite data corresponding to N visible satellites by four satellites to obtain N subsets, wherein the calculation formula of N is as follows:
calculating a position accuracy factor (PDOP), i.e. an open root number value of the sum of squared errors such as latitude, longitude and elevation, of each subset, since the PDOP generally has a value ranging from 0.5 to 99.9, and then comparing each position accuracy factor with a reliability threshold of 99.9, and including the subset smaller than the reliability threshold into a subsequent set S for fault data monitoring.
The step 2 comprises the following steps:
step 2-1, four-star grouping subset S in set SjCan be combined into a 4 x 1 linear matrix ZjAnd a linear matrix H of order 4 × 4jWherein Z isjIs composed of four satellite pseudoranges, HjThe element in (1) is the linear relationship between the calculated receiver position and the receiver clock error and the satellite pseudorange; then, the position of each subset in the set S, i-th subset S is calculated by using a least square methodiIn the position of
Step 2-2, outputting the reference position with inertia and visual information fused by passing the acceleration data and angular velocity data output by the inertial measurement unit IMU and the velocity data output by the visual odometer through a Kalman filter
Step 2-3, with reference to the positionCentering on each subset S obtained in step 2-1iPosition X iniAll the satellite data failure points are regarded as satellite data failure points and are all put in a set to form a satellite data failure candidate point set D;
step 2-4, judging whether a fault occurs;
step 2-5, based on the determined clustering radius R and the reference positionClustering is carried out to find out outliers;
step 2-6, obtaining a normal satellite and a candidate fault satellite according to the result of the step 2-5;
step 2-7, removing the satellite data with faults based on the self-adaptive threshold;
step 2-8, judging whether the residual satellite data is available;
and 2-9, updating the time and returning to the step 1.
Step 2-2 comprises: taking the error of the navigation parameter of the combined system as a state quantity X, establishing a state model on the basis of an inertial navigation error equation, taking the difference between the relative position and the attitude calculated by inertial navigation and the relative position and the attitude obtained by a vision system as an external quantity measurement Z, passing through a Kalman filter,
(Note: the Kalman filter filtering technique is a linear minimum variance estimation, and is mainly divided into two equations, namely a state equation xk=Fk,k-1xk-1+k-1ωk-1And measurement equation zk=Hkxk+vk)
Wherein xkAnd zkRespectively measuring the estimated state and external quantity at the moment k; fk,k-1A state transition matrix from the system time k-1 to the time k;k-1driving a matrix for system noise; hkIs a measurement matrix; omegak-1、vkRespectively, system noise and measurement noise sequence, QkAnd RkCovariance matrices of system noise and measurement noise, respectively, are known, and satisfy the following relationships, whereinkjFor the kronecker function, only if k equals j,kjvalue 1, other values 0: (Note: E [ X ]]Mathematical expectation of X)
At this point the initial state x of the system0And its covariance matrix P0When the covariance matrix Q of the system noise and the covariance matrix R of the measurement noise are known, according to the external measurement value Z, the Kalman filtering step is as follows:
step 2-2-1, initialization, substitution into initial state x0And its covariance matrix P0,P0,-1=P0,For an initial state after initialization, P0,-1A covariance matrix of the initialized initial state;
step 2-2-2, measurement updating:
Pk=Pk,k-1-KkHkPk,k-1,
wherein,representing the state matrix, P, at the moment k obtained by calculationkRepresenting the calculated state covariance matrix at time k, Pk,k-1Representing the state covariance matrix at time K-1, KkIn order to filter the gain matrix of the filter,
step 2-2-3, time updating:
wherein, FkRepresenting the state transition matrix from the previous time to the next time,krepresenting a system noise driving matrix from a previous time to a next time;
then will be at each momentAs a result of output by fusion of inertial and visual information, according to the stateReference position of output carrier
The steps 2-4 comprise: satellite data fault candidate points in the satellite data fault candidate point set D and reference positions based on inertia and visual information fusionMaking a difference, and recording each difference value delta XiEach difference Δ XiIn order of magnitude, starting with the third difference, if a sudden adjacent interval difference (Δ X) is encounteredk+1-ΔXk) Before (Δ X)k-ΔXk-1,…,ΔX2-ΔX1) Is larger, i.e. (Δ X)k+1-ΔXk) Greater than (Δ X)k-ΔXk-1,…,ΔX2-ΔX1) Three times the maximum value of (1), it is determined that faulty data has occurred while taking the difference Δ Xk+1Previous difference (Δ X)k,…,ΔX1) The average value of (a) is used as a dynamic clustering radius R; if (Δ X)n-ΔXn-1,…,ΔX2-ΔX1) If the difference value of each adjacent satellite is close, namely the difference value of the adjacent interval with larger jump cannot be found, judging that the satellite data without faults occur;
after judging that no fault exists, each navigation sensor calculates the position, speed and attitude information of each navigation sensor, the satellite receiver and the visual odometer are respectively combined with the inertial sensor, meanwhile, the inertial sensor is used as a reference to respectively form a subsystem 1 and a subsystem 2, namely, the subsystem 1 consists of a satellite receiver and an inertial sensor, the subsystem 1 consists of a visual odometer and an inertial sensor, each subsystem then combines the errors of the system navigation parameters into a state quantity X in the same way as step 2-2, establishing a state model on the basis of an inertial navigation error equation, taking the difference between the solved relative position and posture of inertial navigation and the relative position and posture obtained by other navigation systems as an external measurement value Z, passing through a Kalman filter, namely, the first stage filtering is performed to obtain the state X of the subsystem 1 and the subsystem 2 at the time k respectively.1,k,X2,kAnd a corresponding covariance matrix P1,k,P2,kThen, information fusion is performed in the following manner: pg,k=(P1,k -1+P2,k -1)-1 ;Xg,k=Pg,k((P1,k -1X1,k+P2,k -1X2,k) I.e. second stage filtering, and dividing the fused result by Xi,k=Xg,k,The state of the subsystem is reset, i is 1,2, time updating is carried out, and the state X at each moment is simultaneously updatedg,kAs the result of fused output of the satellite, vision and inertial navigation information at the time kThen according to state Xg,kOutputting position, velocity and attitude information, wherein Pg,kAfter information fusion, outputting a covariance matrix of the state; xg,kOutputting a k moment state matrix after information fusion; xi,kResetting the state matrix of the subsystem i at the time k to perform state calculation at the next time; pi,kThe state covariance matrix at time k for subsystem i is reset for the next state covariance calculation.
The steps 2-5 comprise: using the central point and the reference position point in the satellite data fault candidate point set DAs a starting point, clustering by using the determined dynamic clustering radius R, wherein the specific process is as follows: firstly, marking all objects in the set D in an unprocessed state, starting from a starting point as a circle center, judging whether more than 3 other fault candidate points exist in a circle with the radius of R, if so, marking the circle center as a core point, starting from a next fault candidate point in the circle with the radius of R according to the same principle until more than 3 other fault candidate points do not exist in the circle with the radius of R, and finally obtaining three types of points:
the first method comprises the following steps: core points, i.e. points that meet the conditions;
and the second method comprises the following steps: boundary points are as follows: boundary points are not core points but fall within a circle of a core point;
and the third is that: outliers: any point that is neither a core point nor a boundary point.
The steps 2-6 comprise: the satellite numbers in the four-star packet subset where the three types of points are located are recorded. Recording the number of each satellite corresponding to the core point as a normal satellite; recording each satellite number corresponding to the outlier as a candidate fault satellite; then normal satellite numbers contained in the candidate fault satellites are eliminated, the remaining candidate fault satellite numbers are recorded, and the frequency f of each candidate fault satellite number is countedsvAre arranged in sequence from big to small.
The steps 2-7 comprise: determining an adaptive threshold (one of which is based on the number k of candidate failed satellites and the number n of all visible satellites in steps 2-6. the threshold is taken to bek is generally less than four), the frequency of each candidate failed satellite number is compared with a determined adaptive threshold, if the frequency is greater than the threshold, the satellite data output by the candidate failed satellite number is unreliable, and then the system excludes the data of the satellite.
Judging whether the number of the remaining satellites is more than 4, if so, fusing the information of the obtained satellite data and the data obtained by the inertial sensor and the visual sensor according to the same method in the step 2-4, respectively calculating the position, the speed and the attitude information of a carrier by each navigation sensor according to the state quantity in the system state model, respectively combining the satellite navigation system and the visual navigation system with the inertial navigation system, simultaneously forming a subsystem 1 and a subsystem 2 by taking the inertial navigation system as a reference system, then establishing a state model by taking the error of the navigation parameters of the combined system as the state quantity X by each subsystem according to the same method as the step 2-2 on the basis of an inertial navigation error equation, and taking the difference of the calculated relative position and attitude of the inertial navigation system and the relative position and attitude obtained by other navigation systems as an external measurement value Z, after twice filtering, the position, speed and attitude information of the carrier with the fused satellite/inertial and visual information is finally output; if the number of the remaining satellites is less than 4, fusing the inertial sensor and the visual sensor according to the same method in the step 2-2, and then outputting the position, the speed and the attitude information of the carrier.
Has the advantages that: the invention carries out fault detection based on software redundancy, avoids arranging a plurality of instruments on a carrier and reduces the cost. Meanwhile, the invention designs a self-adaptive threshold, extracts a dynamic clustering radius by using the related information of the fault candidate point, and adopts the advantages of the dynamic clustering radius: firstly, whether a fault occurs or not is judged quickly and reliably in fault detection by determining the dynamic clustering radius, and the real-time performance is improved. And secondly, after the dynamic clustering radius is determined, determining candidate points in the fault data candidate points as fault data in the subsequent clustering process, thereby eliminating the fault data and realizing the diagnosis of the fault. Finally, the design of the invention leads the candidate fault data points which meet the same condition to be gathered together to form a cluster, thereby isolating the data points which are not under the same cluster. Finally, fault analysis is carried out through the isolation points (outliers), meanwhile, data preprocessing is carried out at the beginning, the calculated amount is reduced, subsequent operation is carried out more quickly, and the real-time performance of fault detection is improved.
Drawings
The foregoing and/or other advantages of the invention will become further apparent from the following detailed description of the invention when taken in conjunction with the accompanying drawings.
FIG. 1 is a flow chart of the present invention.
Detailed Description
With the rapid development of the navigation technology, the application prospect of the navigation technology has been generally acknowledged by countries in the world, the application field of the navigation technology tends to be diversified, and the current navigation technology is required to have higher precision and higher reliability. Global navigation satellite technology (GNSS) is a major part of navigation technology because of real-time and relative reliability. However, under urban complex conditions, such as urban canyon environments, the accuracy and reliability of global satellite navigation technology are greatly reduced due to the influence of buildings, overpasses and trees. In this context, navigation techniques of multi-sensor fusion are a necessary trend. Meanwhile, in the multi-sensor fusion combined navigation, in order to solve the problem that the reliability and the precision of satellite data information in the global satellite navigation technology are reduced under the urban complex condition, the invention designs a satellite/inertia/vision combined navigation integrity monitoring method for the urban complex environment, which is used for carrying out fault detection on satellite data in the satellite navigation and outputting an optimal position solution according to a fault detection result.
As shown in fig. 1, the method of the present invention is mainly divided into two parts, the first part is a data preprocessing part:
according to satellite data received by a receiver, randomly grouping the satellite data corresponding to N visible satellites by four satellites to obtain N subsets, wherein the calculation formula of N is as follows:
calculating a position accuracy factor (PDOP), i.e. an open root number value of the sum of squared errors such as latitude, longitude and elevation, of each subset, since the PDOP generally has a value ranging from 0.5 to 99.9, and then comparing each position accuracy factor with a reliability threshold of 99.9, and including the subset smaller than the reliability threshold into a subsequent set S for fault data monitoring.
The second part is to detect and eliminate fault data, and the specific steps are as follows:
step 1: four-star grouping subset S in set SjVector matrix Z composed of 4 pseudo rangesjAnd a linear matrix H of order 4 × 4jThen, the position of each subset in the set S, i-th subset S, is calculated by using a least square methodiIn the position of
Step 2: the acceleration data and the angular velocity data output by the inertial measurement unit IMU and the velocity data output by the visual odometer pass through a Kalman filter, and finally, a reference position with inertial and visual information fused is output
And 3, step 3: because the satellite data information is subjected to multipath effect under the complex urban environment, the reliability of the satellite data information is reduced, and therefore the set of candidate satellite data fault points is calculated. I.e. the reference position output in the previous stepCentering on each subset S obtained in step 1iPosition X iniAll the satellite data failure points are regarded as satellite data failure points and are all put in a set to form a satellite data failure candidate point set D;
and 4, step 4: judging whether a fault occurs: satellite data fault candidate points in the satellite data fault candidate point set D and reference positions based on inertia and visual information fusionMaking a difference, and recording each difference value delta XiEach difference Δ XiIn order of magnitude, starting with the third difference, if a sudden adjacent interval difference (Δ X) is encounteredk+1-ΔXk) Before (Δ X)k-ΔXk-1,…,ΔX2-ΔX1) Is larger, i.e. (Δ X)k+1-ΔXk) Greater than (Δ X)k-ΔXk-1,…,ΔX2-ΔX1) Three times the maximum value of (1), it is determined that faulty data has occurred while taking the difference Δ Xk+1Previous difference (Δ X)k,…,ΔX1) The average value of (a) is used as a dynamic clustering radius R; if (Δ X)n-ΔXn-1,…,ΔX2-ΔX1) If the difference value of each adjacent satellite is close, namely the difference value of the adjacent interval with larger jump cannot be found, judging that the satellite data without faults occur;
and after no fault is judged, performing information fusion on the vision, the inertia and the satellite data to serve as input information of Kalman filtering, and outputting position, speed and attitude information of the information fusion of the inertia, the vision and the satellite after passing through a Kalman filter.
And 5, step 5: based on the determined cluster radius R and the reference positionClustering to find out outlier, namely collecting the central point (reference position point) in the satellite data failure candidate point set D in the step 3) And as a starting point, clustering by using the determined dynamic clustering radius R. The specific process is as follows: marking all objects in the failure candidate point set D in an unprocessed state, starting from a starting point as a circle center, judging whether more than 3 other failure candidate points exist in a circle with the radius of R, if so, marking the circle center as a core point, and starting from a next failure candidate point in the circle with the radius of R according to the same principle until more than 3 other failure candidate points do not exist in the circle with the radius of R. There are thus three types of points:
the first method comprises the following steps: core points, i.e. points that meet the conditions;
and the second method comprises the following steps: boundary points are as follows: boundary points are not core points, but fall within a circle of a certain core point;
and the third is that: outliers: any point that is neither a core point nor a boundary point;
and 6, step 6: and recording relevant information of the core points and the outliers, namely recording the satellite numbers in the four-star set where each point is located. And recording each satellite number corresponding to the core point as a normal satellite. And recording each satellite number corresponding to the outlier as a candidate fault satellite. Then normal satellite numbers contained in the candidate fault satellites are eliminated, the remaining candidate fault satellite numbers are recorded, and the frequency f of each candidate fault satellite number is countedsvAccording to the order from big to smallAnd (5) secondary arrangement.
And 7, step 7: the method comprises the following steps of removing fault satellite data based on an adaptive threshold, specifically, determining the adaptive threshold based on the number of observed satellites, and increasing the threshold correspondingly when the number of the observed satellites is large. (one method for taking the adaptive threshold is to take the threshold as k according to the number of candidate fault satellites in the step 6 and n according to the number of all visible satellitesk is generally less than four) and then compares the frequency of each candidate failed satellite number to a determined adaptive threshold, and if greater than the threshold, the satellite data output by that satellite number is unreliable, and the system then rejects the data for that satellite.
And 8, step 8: after the satellite data with faults is eliminated, whether the residual satellite data is available or not is judged, namely whether the satellite number is more than 4 or not, if so, the obtained satellite data and data obtained by other sensors (namely an inertial sensor and a visual sensor) are subjected to information fusion, namely, the method is similar to the step 2. And finally outputting the position, speed, attitude and other information of the carrier fused with satellite, inertia and visual information by passing the acceleration data and angular velocity data output by an Inertial Measurement Unit (IMU) and the speed data output by the visual odometer and the pseudo-range data in the satellite data through a Kalman filter. If the number of the particles is less than 4, the previous inertial sensor and the visual sensor are fused, and then information such as the position, the speed, the attitude and the like of the carrier is output.
Step 9: and updating the time and returning to the first step of data preprocessing. The time update is to calculate the state of the next time by starting from the current time and setting the state of the current time as the state of the previous time.
The present invention provides a method for monitoring the integrity of integrated navigation for urban complex environments, and a plurality of methods and approaches for implementing the technical solution, and the above description is only a preferred embodiment of the present invention, and it should be noted that, for those skilled in the art, a plurality of improvements and modifications can be made without departing from the principle of the present invention, and these improvements and modifications should also be regarded as the protection scope of the present invention. All the components not specified in the present embodiment can be realized by the prior art.
Claims (7)
1. A method for monitoring the integrity of integrated navigation in a complex urban environment is characterized by comprising the following steps:
step 1, preprocessing satellite data received by a satellite receiver;
step 2, detecting fault data aiming at the preprocessed satellite data, and eliminating the fault data;
the step 1 comprises the following steps: according to satellite data received by a receiver, randomly grouping the satellite data corresponding to N visible satellites by four satellites to obtain N subsets, wherein the calculation formula of N is as follows:
calculating the position precision factor of each subset, comparing each position precision factor with a reliable threshold value, and incorporating the subset smaller than the reliable threshold value into a set S for subsequent fault data monitoring;
the step 2 comprises the following steps:
step 2-1, four-star grouping subset S in set SjCan be combined into a 4 x 1 linear matrix ZjAnd a linear matrix H of order 4 × 4jWherein Z isjIs composed of four satellite pseudoranges, HjThe element in (1) is the linear relationship between the calculated receiver position and the receiver clock error and the satellite pseudorange; then, the position of each subset in the set S, i-th subset S is calculated by using a least square methodiIn the position of
Step 2-2, outputting the reference position with inertia and visual information fused by passing the acceleration data and angular velocity data output by the inertial measurement unit IMU and the velocity data output by the visual odometer through a Kalman filter
Step 2-3, with reference to the positionCentering on each subset S obtained in step 2-1iPosition X iniAll the satellite data failure points are regarded as satellite data failure points and are all put in a set to form a satellite data failure candidate point set D;
step 2-4, judging whether a fault occurs;
step 2-5, based on the determined clustering radius R and the reference positionClustering is carried out to find out outliers;
step 2-6, obtaining a normal satellite and a candidate fault satellite according to the result of the step 2-5;
step 2-7, removing the satellite data with faults based on the self-adaptive threshold;
step 2-8, judging whether the residual satellite data is available;
and 2-9, updating the time and returning to the step 1.
2. The method of claim 1, wherein step 2-2 comprises: taking the error of the navigation parameter of the combined system as a state quantity X, establishing a state model on the basis of an inertial navigation error equation, and calculating the relative position of inertial navigation,
Taking the difference of the relative position and the attitude obtained by the attitude and the vision system as external quantity Z, and measuring the external quantity Z by a Kalman filter, wherein the Kalman filter comprises two equations, namely a state equation xk=Fk,k-1xk-1+k-1ωk-1And measurement equation zk=Hkxk+vkWherein x iskAnd zkRespectively measuring the estimated state and external quantity at the moment k; fk,k-1For the state transition from the system time k-1 to the time kA matrix;k-1driving a matrix for system noise; hkIs a measurement matrix; omegak-1、vkRespectively, system noise and measurement noise sequence, QkAnd RkCovariance matrices of system noise and measurement noise, respectively, are known, and satisfy the following relationships, whereinkjFor the kronecker function, only if k equals j,kjvalue 1, other values 0:
E[X]is the mathematical expectation of X, at which point the initial state X of the system0And its covariance matrix P0When the covariance matrix Q of the system noise and the covariance matrix R of the measurement noise are known, according to the external measurement value Z, the Kalman filtering step is as follows:
step 2-2-1, initialization, substitution into initial state x0And its covariance matrix P0,P0,-1=P0,For an initial state after initialization, P0,-1A covariance matrix of the initialized initial state;
step 2-2-2, measurement updating:
Pk=Pk,k-1-KkHkPk,k-1,
wherein,representing the state matrix, P, at the moment k obtained by calculationkRepresenting the calculated state covariance matrix at time k, Pk,k-1Representing the state covariance matrix at time K-1, KkIn order to filter the gain matrix of the filter,
step 2-2-3, time updating:
wherein, FkRepresenting the state transition matrix from the previous time to the next time,krepresenting a system noise driving matrix from a previous time to a next time;
3. The method of claim 2, wherein steps 2-4 comprise: satellite data fault candidate points in the satellite data fault candidate point set D and reference positions based on inertia and visual information fusionMaking a difference, and recording each difference value delta XiEach difference Δ XiArranged in the order from small to large, from the thirdThe difference starts, if a sudden adjacent interval difference (Δ X) is encounteredk+1-ΔXk) Before (Δ X)k-ΔXk-1,…,ΔX2-ΔX1) Is larger, i.e. (Δ X)k+1-ΔXk) Greater than (Δ X)k-ΔXk-1,…,ΔX2-ΔX1) Three times the maximum value of (1), it is determined that faulty data has occurred while taking the difference Δ Xk+1Previous difference (Δ X)k,…,ΔX1) The average value of (a) is used as a dynamic clustering radius R; if (Δ X)n-ΔXn-1,…,ΔX2-ΔX1) If the difference value of each adjacent satellite is close, namely the difference value of the adjacent interval with larger jump cannot be found, judging that the satellite data without faults occur;
after judging that no fault exists, each navigation sensor calculates the position, speed and attitude information of each navigation sensor, the satellite receiver and the visual odometer are respectively combined with the inertial sensor, meanwhile, the inertial sensor is used as a reference to respectively form a subsystem 1 and a subsystem 2, namely, the subsystem 1 consists of a satellite receiver and an inertial sensor, the subsystem 2 consists of a visual odometer and an inertial sensor, each subsystem then combines the errors of the system navigation parameters into a state quantity X in the same way as step 2-2, establishing a state model on the basis of an inertial navigation error equation, taking the difference between the solved relative position and posture of inertial navigation and the relative position and posture obtained by other navigation systems as an external measurement value Z, passing through a Kalman filter, namely, the first stage filtering is performed to obtain the state X of the subsystem 1 and the subsystem 2 at the time k respectively.1,k,X2,kAnd a corresponding covariance matrix P1,k,P2,kThen, information fusion is performed in the following manner: pg,k=(P1,k -1+P2,k -1)-1;Xg,k=Pg,k(P1,k -1X1,k+P2,k -1X2,k) I.e. second stage filtering, and dividing the fused result by Xi,k=Xg,k,The state of the subsystem is reset, i is 1,2, time updating is carried out, and the state X at each moment is simultaneously updatedg,kAs the result of fused output of the satellite, vision and inertial navigation information at the time kThen according to state Xg,kOutputting position, velocity and attitude information, wherein Pg,kAfter information fusion, outputting a covariance matrix of the state; xg,kOutputting a k moment state matrix after information fusion; xi,kResetting the state matrix of the subsystem i at the time k to perform state calculation at the next time; pi,kThe state covariance matrix at time k for subsystem i is reset for the next state covariance calculation.
4. The method of claim 3, wherein steps 2-5 comprise: using the central point and the reference position point in the satellite data fault candidate point set DAs a starting point, clustering by using the determined dynamic clustering radius R, wherein the specific process is as follows: firstly, marking all objects in the set D in an unprocessed state, starting from a starting point as a circle center, judging whether more than 3 other fault candidate points exist in a circle with the radius of R, if so, marking the circle center as a core point, starting from a next fault candidate point in the circle with the radius of R according to the same principle until more than 3 other fault candidate points do not exist in the circle with the radius of R, and finally obtaining three types of points:
the first method comprises the following steps: core points, i.e. points that meet the conditions;
and the second method comprises the following steps: boundary points are as follows: boundary points are not core points but fall within a circle of a core point;
and the third is that: outliers: any point that is neither a core point nor a boundary point.
5. The method of claim 4, wherein steps 2-6 comprise: recording satellite numbers in the four-satellite grouping subset where the three types of points are located, and recording each satellite number corresponding to the core point as a normal satellite; recording each satellite number corresponding to the outlier as a candidate fault satellite; then normal satellite numbers contained in the candidate fault satellites are eliminated, the remaining candidate fault satellite numbers are recorded, and the frequency f of each candidate fault satellite number is countedsvAre arranged in sequence from big to small.
6. The method of claim 5, wherein steps 2-7 comprise: and determining an adaptive threshold, comparing the frequency of each candidate failed satellite number with the determined adaptive threshold, if the frequency is greater than the threshold, the satellite data output by the candidate failed satellite number is unreliable, and then the system excludes the data of the satellite.
7. The method of claim 6, wherein steps 2-8 comprise: and judging whether the number of the remaining satellites is more than 4, if so, fusing the obtained satellite data with data line information obtained by an inertial sensor and a visual sensor, and outputting the position, the speed and the posture information of the carrier.
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201911225922.1A CN111060133B (en) | 2019-12-04 | 2019-12-04 | Integrated navigation integrity monitoring method for urban complex environment |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201911225922.1A CN111060133B (en) | 2019-12-04 | 2019-12-04 | Integrated navigation integrity monitoring method for urban complex environment |
Publications (2)
Publication Number | Publication Date |
---|---|
CN111060133A CN111060133A (en) | 2020-04-24 |
CN111060133B true CN111060133B (en) | 2020-10-20 |
Family
ID=70299438
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201911225922.1A Active CN111060133B (en) | 2019-12-04 | 2019-12-04 | Integrated navigation integrity monitoring method for urban complex environment |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN111060133B (en) |
Families Citing this family (9)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN111337963B (en) * | 2020-05-21 | 2020-08-25 | 蓝箭航天空间科技股份有限公司 | Method and device for determining reference information for integrated navigation, and storage medium |
CN113932820A (en) * | 2020-06-29 | 2022-01-14 | 杭州海康威视数字技术股份有限公司 | Object detection method and device |
CN112147647A (en) * | 2020-08-11 | 2020-12-29 | 徐州徐工挖掘机械有限公司 | Method and system for monitoring and predicting GPS terminal fault |
CN112230247B (en) * | 2020-09-25 | 2021-07-27 | 南京航空航天大学 | GNSS integrity monitoring method used in urban complex environment |
CN112833919B (en) * | 2021-03-25 | 2023-11-03 | 成都纵横自动化技术股份有限公司 | Management method and system for redundant inertial measurement data |
CN113483759B (en) * | 2021-06-29 | 2023-10-17 | 北京航空航天大学 | Integrity protection level calculation method for GNSS, INS, vision integrated navigation system |
CN114112125B (en) * | 2021-11-12 | 2023-08-15 | 北京自动化控制设备研究所 | Data fusion processing method of redundant pressure sensor |
CN115979509B (en) * | 2022-12-13 | 2024-07-26 | 国家石油天然气管网集团有限公司 | Pressure transmitter fault detection method, device and storage medium |
CN116736358B (en) * | 2023-08-09 | 2023-11-03 | 北京理工大学 | Long baseline carrier phase differential positioning method suitable for satellite navigation |
Family Cites Families (18)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN101908065B (en) * | 2010-07-27 | 2012-05-23 | 浙江大学 | On-line attribute abnormal point detecting method for supporting dynamic update |
CN102654407B (en) * | 2012-04-17 | 2014-07-02 | 南京航空航天大学 | Multiple-fault detecting device and detecting method for tightly-integrated inertial satellite navigation system |
WO2014026074A2 (en) * | 2012-08-09 | 2014-02-13 | Bae Systems Information And Electronic Systems Integration Inc. | Integrated data registration |
CN102819030B (en) * | 2012-08-13 | 2013-11-06 | 南京航空航天大学 | Method for monitoring integrity of navigation system based on distributed sensor network |
US10041798B2 (en) * | 2012-12-06 | 2018-08-07 | Qualcomm Incorporated | Determination of position, velocity and/or heading by simultaneous use of on-device and on-vehicle information |
CN103454650B (en) * | 2013-08-20 | 2015-06-24 | 北京航空航天大学 | Method for monitoring satellite integrity with vision as auxiliary |
US10234292B2 (en) * | 2014-08-08 | 2019-03-19 | Stmicroelefctronics S.R.L. | Positioning apparatus and global navigation satellite system, method of detecting satellite signals |
CN106483533A (en) * | 2015-09-01 | 2017-03-08 | 北京自动化控制设备研究所 | A kind of Inertia information assists RAIM detection method |
CN105758427A (en) * | 2016-02-26 | 2016-07-13 | 南京航空航天大学 | Monitoring method for satellite integrity based on assistance of dynamical model |
CN108089210A (en) * | 2016-11-23 | 2018-05-29 | 北京自动化控制设备研究所 | A kind of Inertia information aids in RAIM detection methods |
US10533856B2 (en) * | 2017-04-05 | 2020-01-14 | Novatel Inc. | Navigation system utilizing yaw rate constraint during inertial dead reckoning |
CN109085619B (en) * | 2017-06-14 | 2020-09-25 | 展讯通信(上海)有限公司 | Positioning method and device of multimode GNSS system, storage medium and receiver |
CN108020847B (en) * | 2017-11-27 | 2021-05-28 | 南京航空航天大学 | Method for determining fault mode in advanced receiver autonomous integrity monitoring |
CN109581445B (en) * | 2018-11-01 | 2021-03-19 | 北京航空航天大学 | ARAIM subset selection method and system based on Beidou constellation |
CN109490916B (en) * | 2019-01-21 | 2020-01-17 | 南京航空航天大学 | GNSS receiver autonomous integrity monitoring method |
CN110196434B (en) * | 2019-03-29 | 2022-05-20 | 南京航空航天大学 | Constellation dynamic selection method for autonomous integrity monitoring of advanced receiver |
CN110007317B (en) * | 2019-04-10 | 2022-06-17 | 南京航空航天大学 | Star-selection optimized advanced receiver autonomous integrity monitoring method |
CN110260885B (en) * | 2019-04-15 | 2020-06-30 | 南京航空航天大学 | Satellite/inertia/vision combined navigation system integrity evaluation method |
-
2019
- 2019-12-04 CN CN201911225922.1A patent/CN111060133B/en active Active
Also Published As
Publication number | Publication date |
---|---|
CN111060133A (en) | 2020-04-24 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN111060133B (en) | Integrated navigation integrity monitoring method for urban complex environment | |
JP4446604B2 (en) | GPS signal fault isolation monitor | |
US9983314B2 (en) | System for excluding a failure of a satellite in a GNSS system | |
CN107132558A (en) | The multi-frequency multi-mode GNSS cycle slip rehabilitation methods and system of inertia auxiliary | |
CN110007317B (en) | Star-selection optimized advanced receiver autonomous integrity monitoring method | |
CN101629997A (en) | Detection device and detection method of navigation integrity of inertia subsatellite | |
CN114545454A (en) | Fusion navigation system integrity monitoring method for automatic driving | |
Zhang et al. | Improved fault detection method based on robust estimation and sliding window test for INS/GNSS integration | |
CN110531383A (en) | Abnormal satellite elimination method in a kind of satellite navigation and positioning | |
US20230341563A1 (en) | System and method for computing positioning protection levels | |
CN109308518A (en) | A kind of monitoring system and its smoothing parameter optimization method based on probabilistic neural network | |
CN115420284B (en) | Fault detection and identification method for combined navigation system | |
CN115047496A (en) | Synchronous multi-fault detection method for GNSS/INS combined navigation satellite | |
CN112230247A (en) | GNSS integrity monitoring method used in urban complex environment | |
CN112130177A (en) | Foundation reinforcement system integrity monitoring method based on stable distribution | |
US10649093B2 (en) | Method for determining protection levels of navigation solutions, associated computer program product and receiver | |
CN115561782B (en) | Satellite fault detection method in integrated navigation based on odd-even vector projection | |
CN112198533A (en) | System and method for evaluating integrity of foundation enhancement system under multiple hypotheses | |
CN116481525A (en) | MHSS FDE method based on inter-satellite differential GPS/BDS/INS tight integrated navigation | |
CN116466374A (en) | On-orbit anti-interference satellite-borne navigation receiver processing method | |
EP4214532B1 (en) | Method for checking the integrity of calibration on a plurality of landmarks, associated computer program product and associated integrity checking device | |
CN113376664B (en) | Unmanned bee colony collaborative navigation multi-fault detection method | |
CN111999750B (en) | Real-time single-station cycle slip detection improvement method aiming at inaccurate lever arm | |
CN115540907A (en) | Multi-fault detection and elimination method based on GPS/BDS/INS tightly-combined navigation facing inter-satellite difference | |
CN115291253A (en) | Vehicle positioning integrity monitoring method and system based on residual error detection |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |