CN111060133A - Integrated navigation integrity monitoring method for urban complex environment - Google Patents
Integrated navigation integrity monitoring method for urban complex environment Download PDFInfo
- Publication number
- CN111060133A CN111060133A CN201911225922.1A CN201911225922A CN111060133A CN 111060133 A CN111060133 A CN 111060133A CN 201911225922 A CN201911225922 A CN 201911225922A CN 111060133 A CN111060133 A CN 111060133A
- 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.)
- Granted
Links
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, X is performed2During 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 position
Centering 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 position
Clustering 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; gamma-shapedk-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, whereinδkjAs a function of kronecker, delta being only when k is jkjValue 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 the content of the first and second substances,
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 moment to the next momentkRepresenting a system noise driving matrix from a previous time to a next time;
then will be at each moment
As a result of output by fusion of inertial and visual information, according to the state
Reference 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 fusion
Making 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 k
Then according to state Xg,kOutputting position, velocity and attitude information, wherein Pg,kAfter information fusion, outputting a covariance matrix of the state; xg,kFor information fusion, inputThe state matrix at the output k moment; 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 D
As 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 be
k 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 step
Centering 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 fusion
Making 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 position
Clustering 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: firstly, marking all objects in the fault candidate point set D as unprocessed states, then starting from the starting point as the circle center, and judging whether a circle with the radius of R exists or notAnd marking the circle center as a core point if the other fault candidate points are more than 3, and starting from the next fault candidate point in the circle with the radius of R according to the same principle until no other fault candidate points with more than 3 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 countedsvAre arranged in sequence from big to small.
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 satellites
k 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 (9)
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;
and 2, detecting fault data aiming at the preprocessed satellite data, and removing the fault data.
2. The method of claim 1, wherein step 1 comprises: 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 for each subset compares each position accuracy factor to a reliability threshold, and incorporates subsets less than the reliability threshold into a subsequent set S for fault data monitoring.
3. The method of claim 2, wherein step 2 comprises:
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 position
Centering 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 position
Clustering 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.
4. The method of claim 3, 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, taking the difference between the relative position and the attitude calculated by inertial navigation and the relative position and the attitude obtained by a visual system as an external quantity measurement Z, and filtering by a Kalman filter which comprises two equations: equation of state 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-1A state transition matrix from the system time k-1 to the time k; gamma-shapedk-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 relationship, where δkjAs a function of kronecker, delta being only when k is jkjValue 1, other values 0:
E[ωk]＝0，E[vk]＝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 the content of the first and second substances,
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 moment to the next momentkRepresenting a system noise driving matrix from a previous time to a next time;
then the state of each time is determined
As a result of output by fusion of inertial and visual information, according to the state
Outputting the reference position
。
5. The method of claim 4, 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 fusion
Making 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 k
Then 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.
6. The method of claim 5, wherein steps 2-5 comprise: using the central point and the reference position point in the satellite data fault candidate point set D
As 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.
7. The method of claim 6, 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.
8. The method of claim 7, 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.
9. The method of claim 8, 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 true CN111060133A (en) | 2020-04-24 |
| CN111060133B 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) |
Cited By (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111337963A (en) * | 2020-05-21 | 2020-06-26 | 蓝箭航天空间科技股份有限公司 | Method and device for determining reference information for integrated navigation, and storage medium |
| CN112147647A (en) * | 2020-08-11 | 2020-12-29 | 徐州徐工挖掘机械有限公司 | Method and system for monitoring and predicting GPS terminal fault |
| CN112230247A (en) * | 2020-09-25 | 2021-01-15 | 南京航空航天大学 | GNSS integrity monitoring method used in urban complex environment |
| CN112833919A (en) * | 2021-03-25 | 2021-05-25 | 成都纵横自动化技术股份有限公司 | Management method and system for redundant inertial measurement data |
| CN113483759A (en) * | 2021-06-29 | 2021-10-08 | 北京航空航天大学 | Integrity protection level calculation method for GNSS, INS and Vision integrated navigation system |
| CN113932820A (en) * | 2020-06-29 | 2022-01-14 | 杭州海康威视数字技术股份有限公司 | Object detection method and device |
| CN114112125A (en) * | 2021-11-12 | 2022-03-01 | 北京自动化控制设备研究所 | Data fusion processing method of redundant pressure sensor |
| CN115979509A (en) * | 2022-12-13 | 2023-04-18 | 国家石油天然气管网集团有限公司 | Pressure transmitter fault detection method and device and storage medium |
| RU2803185C1 (en) * | 2022-11-07 | 2023-09-11 | Федеральное государственное казенное военное образовательное учреждение высшего образования "Военный учебно-научный центр Военно-воздушных сил "Военно-воздушная академия имени профессора Н.Е. Жуковского и Ю.А. Гагарина" (г. Воронеж) Министерства обороны Российской Федерации | Method for determining spatial coordinates of aircraft |
| CN116736358A (en) * | 2023-08-09 | 2023-09-12 | 北京理工大学 | Long baseline carrier phase differential positioning method suitable for satellite navigation |
Citations (18)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101908065A (en) * | 2010-07-27 | 2010-12-08 | 浙江大学 | On-line attribute abnormal point detecting method for supporting dynamic update |
| CN102654407A (en) * | 2012-04-17 | 2012-09-05 | 南京航空航天大学 | Multiple-fault detecting device and detecting method for tightly-integrated inertial satellite navigation system |
| CN102819030A (en) * | 2012-08-13 | 2012-12-12 | 南京航空航天大学 | Method for monitoring integrity of navigation system based on distributed sensor network |
| CN103454650A (en) * | 2013-08-20 | 2013-12-18 | 北京航空航天大学 | Method for monitoring satellite integrity with vision as auxiliary |
| WO2014026074A2 (en) * | 2012-08-09 | 2014-02-13 | Bae Systems Information And Electronic Systems Integration Inc. | Integrated data registration |
| WO2014088783A2 (en) * | 2012-12-06 | 2014-06-12 | Qualcomm Incorporated | Determination of position, velocity and/or heading by simultaneous use of on-device and on-vehicle information |
| US20160040992A1 (en) * | 2014-08-08 | 2016-02-11 | Stmicroelectronics S.R.L. | Positioning apparatus and global navigation satellite system, method of detecting satellite signals |
| CN105758427A (en) * | 2016-02-26 | 2016-07-13 | 南京航空航天大学 | Monitoring method for satellite integrity based on assistance of dynamical model |
| CN106483533A (en) * | 2015-09-01 | 2017-03-08 | 北京自动化控制设备研究所 | A kind of Inertia information assists RAIM detection method |
| CN108020847A (en) * | 2017-11-27 | 2018-05-11 | 南京航空航天大学 | For the definite method of fault mode in senior receiver Autonomous Integrity Monitoring |
| CN108089210A (en) * | 2016-11-23 | 2018-05-29 | 北京自动化控制设备研究所 | A kind of Inertia information aids in RAIM detection methods |
| US20180292212A1 (en) * | 2017-04-05 | 2018-10-11 | Novatel Inc. | Navigation system utilizing yaw rate constraint during inertial dead reckoning |
| CN109085619A (en) * | 2017-06-14 | 2018-12-25 | 展讯通信（上海）有限公司 | Localization method and device, storage medium, the receiver of multimode GNSS system |
| CN109490916A (en) * | 2019-01-21 | 2019-03-19 | 南京航空航天大学 | A kind of GNSS receiver autonomous integrity monitoring method |
| CN109581445A (en) * | 2018-11-01 | 2019-04-05 | 北京航空航天大学 | A kind of ARAIM subset selection method and system based on Beidou constellation |
| CN110007317A (en) * | 2019-04-10 | 2019-07-12 | 南京航空航天大学 | A kind of senior receiver autonomous integrity monitoring method for selecting star to optimize |
| CN110196434A (en) * | 2019-03-29 | 2019-09-03 | 南京航空航天大学 | A kind of constellation dynamic selection method of senior receiver autonomous integrity monitoring |
| CN110260885A (en) * | 2019-04-15 | 2019-09-20 | 南京航空航天大学 | A kind of satellite/inertia/visual combination navigation system integrity appraisal procedure |
-
2019
- 2019-12-04 CN CN201911225922.1A patent/CN111060133B/en active Active
Patent Citations (18)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101908065A (en) * | 2010-07-27 | 2010-12-08 | 浙江大学 | On-line attribute abnormal point detecting method for supporting dynamic update |
| CN102654407A (en) * | 2012-04-17 | 2012-09-05 | 南京航空航天大学 | 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 |
| CN102819030A (en) * | 2012-08-13 | 2012-12-12 | 南京航空航天大学 | Method for monitoring integrity of navigation system based on distributed sensor network |
| WO2014088783A2 (en) * | 2012-12-06 | 2014-06-12 | Qualcomm Incorporated | Determination of position, velocity and/or heading by simultaneous use of on-device and on-vehicle information |
| CN103454650A (en) * | 2013-08-20 | 2013-12-18 | 北京航空航天大学 | Method for monitoring satellite integrity with vision as auxiliary |
| US20160040992A1 (en) * | 2014-08-08 | 2016-02-11 | Stmicroelectronics 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 |
| US20180292212A1 (en) * | 2017-04-05 | 2018-10-11 | Novatel Inc. | Navigation system utilizing yaw rate constraint during inertial dead reckoning |
| CN109085619A (en) * | 2017-06-14 | 2018-12-25 | 展讯通信（上海）有限公司 | Localization method and device, storage medium, the receiver of multimode GNSS system |
| CN108020847A (en) * | 2017-11-27 | 2018-05-11 | 南京航空航天大学 | For the definite method of fault mode in senior receiver Autonomous Integrity Monitoring |
| CN109581445A (en) * | 2018-11-01 | 2019-04-05 | 北京航空航天大学 | A kind of ARAIM subset selection method and system based on Beidou constellation |
| CN109490916A (en) * | 2019-01-21 | 2019-03-19 | 南京航空航天大学 | A kind of GNSS receiver autonomous integrity monitoring method |
| CN110196434A (en) * | 2019-03-29 | 2019-09-03 | 南京航空航天大学 | A kind of constellation dynamic selection method of senior receiver autonomous integrity monitoring |
| CN110007317A (en) * | 2019-04-10 | 2019-07-12 | 南京航空航天大学 | A kind of senior receiver autonomous integrity monitoring method for selecting star to optimize |
| CN110260885A (en) * | 2019-04-15 | 2019-09-20 | 南京航空航天大学 | A kind of satellite/inertia/visual combination navigation system integrity appraisal procedure |
Non-Patent Citations (7)
| Title |
|---|
| JIANG LIU 等: "Integrity of GNSS-based Train Positioning: From GNSS to Sensor Integration", 《2017 EUROPEAN NAVIGATION CONFERENCE》 * |
| LIVIO MARRADI 等: "GNSS FOR ENHANCED ODOMETRY:THE GRAIL-2 RESULTS", 《2012 6TH ESA WORKSHOP ON SATELLITE NAVIGATION TECHNOLOGIES & EUROPEAN WORKSHOP ON GNSS SIGNALS AND SIGNAL PROCESSING》 * |
| NADER JALILI: "《基于压电材料的振动控制--从宏观系统到微纳米系统》", 31 March 2017, 国防工业出版社 * |
| 冀捐灶 等: "三种卫星故障检测策略的对比研究与仿真", 《计算机仿真》 * |
| 孙建军 等: "GPS空间位置精度因子模糊分析", 《全球定位系统》 * |
| 王尧: "民用飞机IRS_GNSS紧组合运动对准及完好性技术研究", 《中国优秀硕士学位论文全文数据库工程科技Ⅱ辑》 * |
| 苏先礼: "GNSS完好性监测体系及辅助性能增强技术研究", 《中国博士学位论文全文数据库基础科学辑》 * |
Cited By (16)
| 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 |
| CN111337963A (en) * | 2020-05-21 | 2020-06-26 | 蓝箭航天空间科技股份有限公司 | 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 |
| CN112230247A (en) * | 2020-09-25 | 2021-01-15 | 南京航空航天大学 | GNSS integrity monitoring method used in urban complex environment |
| CN112230247B (en) * | 2020-09-25 | 2021-07-27 | 南京航空航天大学 | GNSS integrity monitoring method used in urban complex environment |
| CN112833919A (en) * | 2021-03-25 | 2021-05-25 | 成都纵横自动化技术股份有限公司 | Management method and system for redundant inertial measurement data |
| 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 |
| CN113483759A (en) * | 2021-06-29 | 2021-10-08 | 北京航空航天大学 | Integrity protection level calculation method for GNSS, INS and Vision integrated navigation system |
| CN114112125A (en) * | 2021-11-12 | 2022-03-01 | 北京自动化控制设备研究所 | Data fusion processing method of redundant pressure sensor |
| CN114112125B (en) * | 2021-11-12 | 2023-08-15 | 北京自动化控制设备研究所 | Data fusion processing method of redundant pressure sensor |
| RU2803185C1 (en) * | 2022-11-07 | 2023-09-11 | Федеральное государственное казенное военное образовательное учреждение высшего образования "Военный учебно-научный центр Военно-воздушных сил "Военно-воздушная академия имени профессора Н.Е. Жуковского и Ю.А. Гагарина" (г. Воронеж) Министерства обороны Российской Федерации | Method for determining spatial coordinates of aircraft |
| CN115979509A (en) * | 2022-12-13 | 2023-04-18 | 国家石油天然气管网集团有限公司 | Pressure transmitter fault detection method and device and storage medium |
| CN116736358A (en) * | 2023-08-09 | 2023-09-12 | 北京理工大学 | Long baseline carrier phase differential positioning method suitable for satellite navigation |
| CN116736358B (en) * | 2023-08-09 | 2023-11-03 | 北京理工大学 | Long baseline carrier phase differential positioning method suitable for satellite navigation |
Also Published As
| Publication number | Publication date |
|---|---|
| CN111060133B (en) | 2020-10-20 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN111060133B (en) | Integrated navigation integrity monitoring method for urban complex environment | |
| JP4446604B2 (en) | GPS signal fault isolation monitor | |
| EP2598912B1 (en) | Method for determining a protection space in the event of two simultaneous satellite failures | |
| EP1714166B1 (en) | Device for monitoring the integrity of information delivered by a hybrid ins/gnss system | |
| CN103097911B (en) | Method and device for detecting and excluding multiple satellite failures in a GNSS system | |
| CN110007317B (en) | Star-selection optimized advanced receiver autonomous integrity monitoring method | |
| US9983314B2 (en) | System for excluding a failure of a satellite in a GNSS system | |
| WO2008040658A1 (en) | Method and device for mojnitoringthe integrity of information provided by a hybrid ins/gnss system | |
| CN109471143B (en) | Self-adaptive fault-tolerant train combined positioning method | |
| CN114545454A (en) | Fusion navigation system integrity monitoring method for automatic driving | |
| CN110260885B (en) | Satellite/inertia/vision combined navigation system integrity evaluation method | |
| CN110531383A (en) | Abnormal satellite elimination method in a kind of satellite navigation and positioning | |
| CN109308518A (en) | A kind of monitoring system and its smoothing parameter optimization method based on probabilistic neural network | |
| CN102135621B (en) | Fault recognition method for multi-constellation integrated navigation system | |
| CN115420284B (en) | Fault detection and identification method for combined navigation system | |
| CN115047496A (en) | Synchronous multi-fault detection method for GNSS/INS combined navigation satellite | |
| US11733397B2 (en) | System and method for computing positioning protection levels | |
| CN112230247A (en) | GNSS integrity monitoring method used in urban complex environment | |
| US11977150B2 (en) | Vehicle localization precision enhancement via multi-sensor fusion | |
| CN115561782B (en) | Satellite fault detection method in integrated navigation based on odd-even vector projection | |
| CN116481525A (en) | MHSS FDE method based on inter-satellite differential GPS/BDS/INS tight integrated navigation | |
| 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 | |
| Chen et al. | A factor set-based GNSS fault detection and exclusion for vehicle navigation in urban environments |
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 |