CN117367430A - Vehicle-mounted positioning method for improved factor graph - Google Patents

Vehicle-mounted positioning method for improved factor graph Download PDF

Info

Publication number
CN117367430A
CN117367430A CN202311357210.1A CN202311357210A CN117367430A CN 117367430 A CN117367430 A CN 117367430A CN 202311357210 A CN202311357210 A CN 202311357210A CN 117367430 A CN117367430 A CN 117367430A
Authority
CN
China
Prior art keywords
gnss
imu
odo
vehicle
factor graph
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.)
Pending
Application number
CN202311357210.1A
Other languages
Chinese (zh)
Inventor
胡慧珠
权思航
周一览
陈少华
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Zhejiang University ZJU
Original Assignee
Zhejiang University ZJU
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Zhejiang University ZJU filed Critical Zhejiang University ZJU
Priority to CN202311357210.1A priority Critical patent/CN117367430A/en
Publication of CN117367430A publication Critical patent/CN117367430A/en
Pending legal-status Critical Current

Links

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/01—Satellite radio beacon positioning systems transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/13—Receivers
    • G01S19/23—Testing, monitoring, correcting or calibrating of receiver elements
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/48—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system
    • G01S19/49—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system whereby the further system is an inertial position system, e.g. loosely-coupled
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Navigation (AREA)

Abstract

The invention discloses a vehicle-mounted positioning method for an improved factor graph. The invention comprises the following steps: firstly, acquiring motion information data acquired by a sensor on a vehicle; then, constructing a factor graph model according to the acquired motion information data; then, according to the factor graph model, combining the acquired motion information data, performing fault detection on the GNSS measurement value by utilizing a multi-condition analysis fault detection method to obtain fault GNSS measurement information; isolating the fault GNSS measurement information by deleting the corresponding factor nodes, and updating the GNSS measurement information added into the factor graph model; and finally, carrying out optimization solution on all navigation state quantities in the sliding window based on the current factor graph model to obtain navigation information of the vehicle carrier, thereby realizing fusion positioning of the vehicle carrier. The invention effectively suppresses the influence of GNSS measurement signal abnormality on navigation precision and effectively reduces the system false alarm rate.

Description

Vehicle-mounted positioning method for improved factor graph
Technical Field
The invention relates to a positioning method in the technical field of vehicle-mounted integrated navigation, in particular to a vehicle carrier positioning method for improving factor graphs.
Background
Global navigation positioning systems (GNSS) can provide all-weather accurate position and velocity information in outdoor environments, which are a major source of Intelligent Transportation System (ITS) global positioning references. However, the positioning accuracy of GNSS is greatly affected by the environment, and interference due to factors such as multipath and non-line of sight is likely to occur, so that the navigation accuracy is affected. In contrast, an Inertial Navigation System (INS) can autonomously provide pose information, and has high output frequency and little dependence on external environment, but navigation errors can accumulate with time, and can only provide high-precision positioning information in a short time. The GNSS/INS integrated navigation system combines the advantages of the GNSS and the INS integrated navigation system, avoids the defect of a single navigation mode, and can provide a navigation result with high precision and high frequency.
The INS/GNSS integrated navigation method based on EKF has been widely used, but EKF can only iterate in a single step based on the first order markov assumption, and history information is not fully utilized. In addition, the addition and deletion of sensors in the traditional integrated navigation method requires the reconstruction of the system, and the complexity and time consumption of the method are increased. The recently proposed factor graph scheme provides a new idea for multi-sensor fusion. Compared with an EKF one-time iteration method, the factor graph method utilizes a plurality of pieces of measurement information at historical moments to estimate state quantities at the same time so as to obtain a global optimal solution. Because of the multiple iteration and re-linearization properties, the solving accuracy of the system to the nonlinear problem is effectively improved, and the plug-and-play characteristic of the system is convenient for adding and deleting the sensor factors.
Although the potential of the factor graph method in positioning has been explored, research has shown that the improved method based on the factor graph can better adapt to the error distribution of multi-sensor fusion and has certain robustness to outlier measurement. However, in complex environments such as urban canyons, the integrity of GNSS positioning presents significant challenges. Multipath effects and non-line-of-sight interference are the main sources of GNSS coarse and faults have a large impact on the accuracy of integrated navigation. Many scholars propose an abnormal value detection method using a residual mahalanobis distance, and the fault is judged by setting a threshold value of chi-square test, and the method is simple to operate and small in calculated amount, but has higher omission ratio for a slow-change fault.
At the same time, the conventional factor graph method requires optimization of all variables. As new variables continue to increase, the information matrix of the system becomes larger, resulting in an increasing amount of computation. The sliding window performs marginalization on the past state by using marginal probability, and improves the operation efficiency under the condition of keeping certain historical information. Research also shows that multiple iterative calculations of the sliding window are beneficial to assisting in improving the accuracy of fault detection.
Disclosure of Invention
In a vehicle-mounted integrated navigation system, GNSS signals are easy to be interfered by multipath and non-line-of-sight, and positioning accuracy is affected by rough difference, no model error and the like in observed data. Meanwhile, in the practical use process of the GNSS, especially in the single-point positioning process, the STD of the GNSS receiver can have abnormal conditions, so that the alarm leakage occurs. Therefore, aiming at the defects of the existing factor graph method, the invention introduces a sliding window and provides a fault detection method of multi-condition analysis, the calculation speed is improved through the sliding window, and error measurement information is timely isolated by combining the proposed fault detection method, so that the accuracy of the integrated navigation system is improved, and the system alarm missing rate is reduced.
In order to achieve the above purpose, the present invention provides the following technical solutions:
1) Acquiring motion information data acquired by a sensor on a vehicle;
2) According to the acquired motion information data, an IMU-ODO/GNSS factor graph model is constructed;
3) According to the IMU-ODO/GNSS factor graph model, combining the acquired motion information data, and performing fault detection on the GNSS measurement value by utilizing a multi-condition analysis fault detection method to obtain fault GNSS measurement information;
4) After isolating the fault GNSS measurement information by deleting the corresponding factor nodes, updating the GNSS measurement information added into the IMU-ODO/GNSS factor graph model;
5) And carrying out optimization solution on all navigation state quantities in the sliding window based on the current IMU-ODO/GNSS factor graph model to obtain navigation information of the vehicle carrier, thereby realizing fusion positioning of the vehicle carrier.
The 2) is specifically:
and constructing an IMU-ODO pre-integration model, an IMU-ODO factor node and a GNSS measurement model and a GNSS factor node according to the acquired motion information data, so as to form an IMU-ODO/GNSS factor graph model.
The formula of the IMU-ODO pre-integration model is as follows:
wherein b k The carrier coordinate system b is the k moment;is a rotation matrix from b-system to reference coordinate system w-system at time k-1; g w Is the gravity in a w coordinate system; Δt (delta t) k A time interval of k-1 to k; /> B is the velocity projection and position projection of the w system relative to the w system at time k,/->B is the speed and position projection of w relative to w at time k-1; />For the mounting angle of IMU, +.>For wheel speed increment +.>Lever arm for wheel speed>The gesture quaternions at the moment k and the moment k-1 are respectively; />B is a rotation in the time k-1 to k; />The pose and velocity increment from time k-1 to time k.
The IMU-ODO/GNSS factor map model comprises an IMU-ODO pre-integration factor and a GNSS factor, and specifically:
the IMU-ODO pre-integration factor satisfies the following formula:
f bias (k k ，k k-1 )＝d(k k -u(k k ))
wherein f IMU-ODO () Is a factor node X of an IMU-ODO pre-integration model constructed by adding ODO into a pre-integration process k+1 、X k The navigation state variables at time k-1 and time k respectively,zero bias of gyro->Zero offset for accelerometer, c k Is the wheel speed scale factor, z k For the sensor measurement information, d () is the mahalanobis distance, h () is the observation equation, f bias () Is a bias node; u (k) k ) For the bias parameter k k Random walk models of (a);
the GNSS factor satisfies the following formula:
wherein f GNSS () Is a GNSS factor node;for the GNSS position in the w coordinate system, l b For lever arm error of GNSS and IMU, +.>A rotation matrix from b series to w series; />The b series at time k is projected with respect to the position of the w series under w.
In the 3), the criteria in the multi-condition analysis fault detection method comprise residual error criteria, STD criteria and incremental error criteria.
The STD criterion is formulated as follows:
wherein STD is the standard deviation of position, d 1 And F represents that the detection result is a fault, and T represents that the detection result is a normal condition.
The incremental error criteria are formulated as follows:
wherein,is the difference between the GNSS position increment and the IMU-ODO pre-integration mileage increment in adjacent time, s 1 And F represents that the detection result is a fault, and T represents that the detection result is a normal condition.
In the step 5), all navigation state quantities in the sliding window are solved by using a Gaussian Newton nonlinear optimization method, and after solving is completed once, the sliding window is moved backwards by one length until all sensor data are solved and calculated, so that navigation information of the vehicle carrier is obtained.
The invention has the beneficial effects that:
the invention provides an IMU-ODO/GNSS integrated navigation frame suitable for a vehicle-mounted condition based on a factor graph, and solves the problem of navigation accuracy reduction caused by GNSS measurement faults in the traditional factor graph method by combining a sliding window with the proposed fault detection method based on multi-condition analysis. Meanwhile, the fault detection method can effectively reduce the false alarm rate and improve the integrity of the system.
The invention firstly provides a combined navigation fault detection method based on factor graph, which is suitable for vehicle-mounted conditions, and effectively solves the problem that the existing method has larger delay for slow fault detection.
Along with the development of intelligent traffic systems, the vehicle-mounted integrated navigation system is gradually perfected, the factor graph technology can effectively fuse data of a plurality of sensors, and the intelligent vehicle-mounted integrated navigation system has the characteristics of plug and play and has a large application potential in the integrated navigation system. The invention introduces a sliding window into the traditional model, reduces the operation amount of the system while retaining the history information through the marginalization strategy of the sliding window, and also provides a fault detection method based on multi-condition analysis, which can timely identify and isolate faults and effectively reduce the influence of the faults on the precision of the integrated navigation system.
It should be noted that not all the above effects can be achieved by all the technical solutions of the present invention.
Drawings
FIG. 1 is a diagram of the overall framework of a vehicle-mounted positioning method of the improved factor graph of the present invention.
FIG. 2 is a factor graph model of a vehicle-mounted positioning method of the improved factor graph of the present invention.
FIG. 3 is an exploded view of the GNSS and IMU-ODO incremental error vectors of an improved factor graph of the present invention.
FIG. 4 is a diagram showing a sliding window of an on-vehicle positioning method according to an embodiment of the present invention.
FIG. 5 is a flowchart showing the steps of a method for locating a vehicle with an improved factor graph according to the present invention.
FIG. 6 is a diagram of the results of the fault detection simulation of the vehicle-mounted positioning method of the improved factor graph of the present invention.
Detailed Description
The present invention will be described in further detail with reference to the following brief description of the drawings and the accompanying drawings, in which the invention is further described in conjunction with the specific embodiments and the related formula deductions.
As shown in fig. 1 and 5, the present invention includes the steps of:
1) Acquiring motion information data acquired by a sensor on a vehicle; the motion data comprise acceleration, angular velocity, position, standard deviation of position (STD), speed and the like, wherein the standard deviation of position and position (STD) is acquired through a Global Navigation Satellite System (GNSS), the three-axis angular velocity and the three-axis acceleration are acquired through an Inertial Measurement Unit (IMU), and the wheel speed is acquired through a wheel speed odometer (ODO). In practice, both system noise and system state quantity need to be initialized. The navigation system state quantity includes: carrier position, carrier velocity, carrier attitude, gyro and zero bias with meter, wheel speed scale factor. The system noise is selected according to the characteristics of the sensor, and the longitude and latitude height of the geographic position of the carrier position information carrier is initialized to be the GNSS initial longitude and latitude height value; the speed information is east speed, north speed and sky speed, and is initialized to 0 m/s; the attitude information is pitch angle, roll angle and course angle, and is initialized to the initial aligned attitude angle.
2) According to the acquired motion information data, an IMU-ODO/GNSS factor graph model is constructed;
2) The method comprises the following steps: according to the acquired motion information data, an IMU-ODO pre-integration model is built, an IMU-ODO factor node is built, a GNSS measurement model and a GNSS factor node are built, and therefore an IMU-ODO/GNSS factor graph model is formed; as shown in FIG. 2, when GNSS measurement information arrives, the corresponding factor node is connected with the factor node constructed by the IMU-ODO. The IMU-ODO pre-integration model is obtained based on angular velocity, acceleration and velocity information obtained by the IMU and the ODO, and the GNSS measurement model is obtained based on position information obtained by the GNSS.
The formula of the IMU-ODO pre-integration model is as follows:
wherein b k The carrier coordinate system b is the k moment;is a rotation matrix from b-system to reference coordinate system w-system at time k-1; g w Is the gravity in a w coordinate system; Δt (delta t) k A time interval of k-1 to k; /> B is the velocity projection and position projection of the w system relative to the w system at time k,/->B is the speed and position projection of w relative to w at time k-1; />For the mounting angle of IMU, +.>For wheel speed increment +.>Lever arm for wheel speed>The gesture quaternions at the moment k and the moment k-1 are respectively; />B is a rotation in the time k-1 to k; />The pose and velocity increment from time k-1 to time k.
Attitude and velocity delta from time k-1 to time kThe following formula is satisfied:
wherein the method comprises the steps of
Wherein,f b 、ω b for the specific force and angular velocity measurements output by the IMU under the b-series, the velocity and angle, respectively, are the integral of the specific force and angular velocity, respectively.
When introducing OD observation data, taking NHC constraint into consideration, the speed from b system to vehicle coordinate system v systemCan be expressed as:
the mileage increment derived from the speed is:
wherein,for the mounting angle of IMU, +.>Is the wheel speed increment, expressed as +.> Is the output of the wheel speed odometer.
Thus, the location update of the IMU and odometer is expressed as:
the IMU-ODO/GNSS factor map model comprises an IMU-ODO pre-integration factor and a GNSS factor, and specifically:
the IMU-ODO pre-integration factor satisfies the following formula:
f bias (k k ，k k-1 )＝d(k k -u(k k ))
wherein f IMU-ODO () Is a factor node X of an IMU-ODO pre-integration model constructed by adding ODO into a pre-integration process k+1 、X k The navigation state variables at time k-1 and time k respectively,zero bias of gyro->Zero offset for accelerometer, c k Is the wheel speed scale factor, z k For the sensor measurement information, d () is the mahalanobis distance, h () is the observation equation, f bias () Is a bias node; u (k) k ) For the bias parameter k k Random walk models of (a);
the GNSS positioning results are obtained from the GNSS receiver and the measurement equation taking into account the lever arm effect can be expressed as:
for the GNSS position in the w coordinate system, n k To measure position noiseSound.
The GNSS factor satisfies the following formula:
wherein f GNSS () Is a GNSS factor node;for the GNSS position in the w coordinate system, l b For lever arm error of GNSS and IMU, +.>A rotation matrix from b series to w series; />The b series at time k is projected with respect to the position of the w series under w.
3) According to the IMU-ODO/GNSS factor graph model, combining the position standard deviation in the acquired motion information data, performing fault detection on the GNSS measurement value by utilizing a multi-condition analysis fault detection algorithm to obtain fault GNSS measurement information; if the fault is detected, the corresponding node factor is directly deleted to isolate the fault measurement information.
3) The criteria in the multi-condition analysis fault detection algorithm comprise residual error criteria, STD criteria and incremental error criteria.
The formula of the STD criterion is as follows:
wherein STD is the standard deviation of position, d 1 And F represents that the detection result is a fault, and T represents that the detection result is a normal condition.
The formula of the delta error criterion is as follows:
wherein,is the difference between the GNSS position increment and the IMU-ODO pre-integration mileage increment in adjacent time, s 1 And F represents that the detection result is a fault, and T represents that the detection result is a normal condition.
The specific derivation of the residual criterion and the error increment criterion is as follows:
according to the GNSS factor model, the residual equation of the GNSS positioning factor can be obtained as follows:
assuming that the observation equation model and the IMU-ODO model parameters do not contain abnormal errors, the measurement noise obeys the expectation of 0, and the variance is a constant value sigma 0 Can obtain z i Probability density distribution of (c):
if the GNSS device fails, the statistical properties of the measurement noise will change, and the residual error will be foundThe average value of r (k) hereinafter will not be equal to 0, and is described in detail as follows:
making binary assumption for r (k), H 0 Indicating no failure:
E{r(k)}＝0，E{r(k)r(k) T }＝∑ 0
H 1 indicating that there is a fault:
E{r(k)}＝μ，E{(r(k)-μ)(r(k)-μ) T }＝∑ 0
the residual r (k) criterion is thus derived:
wherein r is 1 Is the first error threshold.
Within two adjacent GNSS epochs, the position increment of the GNSS isAt the same time, IMU-ODO pre-integration is recursively calculated in the form of increment, and the mileage increment is +.>X in FIG. 3 k And X k-1 Is the navigation state quantity before the GNSS fusion. The position change of the system from k-1 to k time can be reflected by both the GNSS position increment and the IMU-ODO pre-integrated mileage increment. When the GNSS measurement is normal, the difference between the position increment of the GNSS and the mileage increment of the IMU-ODO is small, otherwise, if the GNSS measurement information is wrong, the increment error of the position increment and the mileage increment of the IMU-ODO is obviously increased. Based on this, it can be determined whether the GNSS measurement information is faulty. Since the increment is a vector, we use vector decomposition under the carrier coordinate system in order to accurately reflect the change of the increment.
Lateral delta errorCan be expressed as:
longitudinal incremental errorThe method comprises the following steps:
thus the IMU-ODO pre-integration and GNSS delta errorThe method comprises the following steps:
the incremental error criteria of the wheel speed and the GNSS are as follows:
the fault detection method based on multi-condition analysis judges whether the own STD information is normal or not according to the comprehensive criteria of the combination of the residual criteria, the STD criteria, the GNSS and the IMU-ODO incremental error criteria. Specifically, the classification into four categories is shown in table 1:
table 1 results Table of criteria
In the 1 st or 3 rd case, when the detection results of the residual method and the STD are the same, the result is judged as normal, i.e., the result is consistent with the STD judgment. In case 2, the residual error is judged to be normal but the STD judges that the abnormal situation belongs to false alarm, and the probability of occurrence in practice is low. In the 4 th case, the residual method shows that the GNSS signal is normal at this time, and the STD shows an abnormal situation, i.e. a false alarm phenomenon frequently occurring in the navigation system. Since the residual error is more sensitive to faults, additional time is required for the system to recover to normal when the faults accumulate. Thus, we introduce incremental errors in the GNSS and IMU-ODO to assist in determining the end of the fault. If the incremental errors of the GNSS and the IMU-ODO are judged to be normal at the moment, the system fault is considered to be ended and the system is restored to the normal state.
4) By utilizing the plug and play characteristic of the factor graph, after isolating fault GNSS measurement information by deleting corresponding factor nodes, updating GNSS measurement information added into an IMU-ODO/GNSS factor graph model;
5) Based on the current IMU-ODO/GNSS factor graph model, the size of a sliding window is selected, all navigation state quantities in the sliding window are optimized and solved, and navigation information of the vehicle carrier is obtained, so that fusion positioning of the vehicle carrier is achieved.
5) The navigation state quantity of the carrier comprises carrier position, carrier speed, carrier posture, gyro zero bias and meter adding zero bias and wheel speed scale factors. Solving all navigation state quantities in the sliding window by using a Gaussian Newton (G-N) nonlinear optimization method, and after solving once, moving the sliding window backwards by one length until all sensor data are solved and calculated to obtain navigation information of the vehicle carrier, namely a navigation result of the carrier.
And the sliding window optimizing part utilizes the marginalization characteristic of the sliding window to perform optimization solution in the sliding window and marginalize data outside the sliding window. And after each optimization is finished, the sliding window moves backwards to perform the next optimization until all data are optimized, and a final navigation result is obtained. Fig. 4 shows a schematic diagram of a sliding window method, assuming that the data update frequency of the sensor is 10Hz, the optimal calculation frequency is 5Hz, and the size of the sliding window is 4. It can be seen that the sliding window is still in the initial phase for the first second, only two variables are contained within the window, two past variables are deleted and two new variables are added from each update after the fifth second, and so on.
The effectiveness of the provided fault detection method is verified through fault simulation. The zero bias stability of the gyroscope is set to be 0.9 degrees/h, the zero bias stability of the accelerometer is set to be 0.01mg, and the GNSS positioning accuracy is set to be 1.5m. The simulation time was 800s. And adding a gradual fault to the GNSS in the longitude and latitude directions within the total time of 180s from 400s to 580s, wherein the maximum fault size can reach 25m. Because the fault and the missing alarm are simulated, the STD test is normal at this time, the detection effects of three different detection methods, namely, the residual error test, the GNSS and IMU-ODO incremental error test and the comprehensive test are compared, the fault detection simulation result is shown in FIG. 6, a of FIG. 6 is statistics of residual error detection, b of FIG. 6 is judgment result of residual error detection, c of FIG. 6 is statistics of incremental error detection, d of FIG. 6 is judgment result of incremental error criterion, e of FIG. 6 is statistics of comprehensive detection, and f of FIG. 6 is statistics of comprehensive detection. And (5) synthesizing a judgment result of the detection.
For the fault curve, a value of 1 represents that the detection method detects that the fault is considered, and a value of 0 represents that the fault is normal. The fault detection delays for the different detection methods are shown in table 2. As shown in e of fig. 6, the proposed method can effectively distinguish between two adjacent different faults, and the residual error check rule considers that this is a whole-segment fault, resulting in serious delay. The detection results shown in table 2 show that all methods can detect the fault start faster, but compared with the residual error detection method, the method can effectively detect the fault end, and the fault end detection time of the method is reduced by 90%.
Table 2 is a comparison table of fault detection delays for different detection methods
The foregoing is only a preferred embodiment of the present invention, but the scope of the present invention is not limited thereto, and any changes or substitutions easily contemplated by those skilled in the art within the scope of the present invention should be covered by the present invention.

Claims (8)

1. An on-vehicle positioning method for improving factor graph is characterized by comprising the following steps:
1) Acquiring motion information data acquired by a sensor on a vehicle;
2) According to the acquired motion information data, an IMU-ODO/GNSS factor graph model is constructed;
3) According to the IMU-ODO/GNSS factor graph model, combining the acquired motion information data, and performing fault detection on the GNSS measurement value by utilizing a multi-condition analysis fault detection method to obtain fault GNSS measurement information;
4) After isolating the fault GNSS measurement information by deleting the corresponding factor nodes, updating the GNSS measurement information added into the IMU-ODO/GNSS factor graph model;
5) And carrying out optimization solution on all navigation state quantities in the sliding window based on the current IMU-ODO/GNSS factor graph model to obtain navigation information of the vehicle carrier, thereby realizing fusion positioning of the vehicle carrier.
2. The vehicle-mounted positioning method of the improved factor graph according to claim 1, wherein the 2) specifically comprises:
and constructing an IMU-ODO pre-integration model, an IMU-ODO factor node and a GNSS measurement model and a GNSS factor node according to the acquired motion information data, so as to form an IMU-ODO/GNSS factor graph model.
3. The method for vehicle-mounted positioning of an improved factor graph according to claim 2, wherein the IMU-ODO pre-integration model is formulated as follows:
wherein b k The carrier coordinate system b is the k moment;is a rotation matrix from b-system to reference coordinate system w-system at time k-1; g w Is the gravity in a w coordinate system; Δt (delta t) k A time interval of k-1 to k; /> For time kb is the velocity projection and position projection under w is the relative to w, < >>B is the speed and position projection of w relative to w at time k-1; />For the mounting angle of IMU, +.>For wheel speed increment +.>Lever arm for wheel speed>The gesture quaternions at the moment k and the moment k-1 are respectively; />B is a rotation in the time k-1 to k; />The pose and velocity increment from time k-1 to time k.
4. The vehicle-mounted positioning method of an improved factor graph according to claim 1, wherein the IMU-ODO/GNSS factor graph model comprises IMU-ODO pre-integration factors and GNSS factors, in particular:
the IMU-ODO pre-integration factor satisfies the following formula:
f bias (k k ，k k-1 )＝d(k k -u(k k ))
wherein f IMU-ODO () Is a factor node X of an IMU-ODO pre-integration model constructed by adding ODO into a pre-integration process k+1 、X k The navigation state variables at time k-1 and time k respectively,zero bias of gyro->Zero offset for accelerometer, c k Is the wheel speed scale factor, z k For the sensor measurement information, d () is the mahalanobis distance, h () is the observation equation, f bias () Is a bias node; u (k) k ) For the bias parameter k k Random walk models of (a);
the GNSS factor satisfies the following formula:
wherein f GNSS () Is a GNSS factor node;for the GNSS position in the w coordinate system, l b For lever arm error of GNSS and IMU, +.>A rotation matrix from b series to w series; />The b series at time k is projected with respect to the position of the w series under w.
5. The vehicle-mounted positioning method of an improved factor graph according to claim 1, wherein in said 3), the criteria in the multi-condition analysis fault detection method include residual criteria, STD criteria, incremental error criteria.
6. The method for vehicle-mounted positioning of an improved factor graph of claim 5, wherein said STD criterion is formulated as follows:
wherein STD is the standard deviation of position, d 1 And F represents that the detection result is a fault, and T represents that the detection result is a normal condition.
7. The method for vehicle-mounted positioning of an improved factor graph of claim 5, wherein said incremental error criteria is formulated as follows:
wherein,is the difference between the GNSS position increment and the IMU-ODO pre-integration mileage increment in adjacent time, s 1 And F represents that the detection result is a fault, and T represents that the detection result is a normal condition.
8. The vehicle-mounted positioning method of the improved factor graph according to claim 1, wherein in the step 5), all navigation state quantities in the sliding window are solved by using a Gaussian Newton nonlinear optimization method, and after solving once, the sliding window is moved backwards by one length until all sensor data are solved and calculated, so that navigation information of the vehicle carrier is obtained.
CN202311357210.1A 2023-10-17 2023-10-17 Vehicle-mounted positioning method for improved factor graph Pending CN117367430A (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202311357210.1A CN117367430A (en) 2023-10-17 2023-10-17 Vehicle-mounted positioning method for improved factor graph

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202311357210.1A CN117367430A (en) 2023-10-17 2023-10-17 Vehicle-mounted positioning method for improved factor graph

Publications (1)

Publication Number Publication Date
CN117367430A true CN117367430A (en) 2024-01-09

Family

ID=89401856

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202311357210.1A Pending CN117367430A (en) 2023-10-17 2023-10-17 Vehicle-mounted positioning method for improved factor graph

Country Status (1)

Country Link
CN (1) CN117367430A (en)

Similar Documents

Publication Publication Date Title
CN105509738B (en) Vehicle positioning orientation method based on inertial navigation/Doppler radar combination
CN110779521A (en) Multi-source fusion high-precision positioning method and device
US20100019963A1 (en) Vehicular navigation and positioning system
CN109870173A (en) A kind of track correct method of the submarine pipeline inertial navigation system based on checkpoint
CN104019828A (en) On-line calibration method for lever arm effect error of inertial navigation system in high dynamic environment
CN103822633A (en) Low-cost attitude estimation method based on second-order measurement update
CN112505737A (en) GNSS/INS combined navigation method based on Elman neural network online learning assistance
CN114545472B (en) Navigation method and device of GNSS/INS combined system
CN115060257B (en) Vehicle lane change detection method based on civil-grade inertia measurement unit
WO2024041156A1 (en) Vehicle positioning calibration method and apparatus, computer device, and storage medium
CN114002725A (en) Lane line auxiliary positioning method and device, electronic equipment and storage medium
Gao et al. Development of precise GPS/INS/wheel speed sensor/yaw rate sensor integrated vehicular positioning system
CN111380516A (en) Inertial navigation/odometer vehicle combined navigation method and system based on odometer measurement information
US20230341563A1 (en) System and method for computing positioning protection levels
WO2021221989A1 (en) Navigation apparatus and method in which measurement quantization errors are modeled as states
US12019170B1 (en) GNSS and INS integrated navigation positioning method and system thereof
CN113008229B (en) Distributed autonomous integrated navigation method based on low-cost vehicle-mounted sensor
CN113074757A (en) Calibration method for vehicle-mounted inertial navigation installation error angle
CN117268408A (en) Laser slam positioning method and system
CN116576849A (en) Vehicle fusion positioning method and system based on GMM assistance
CN112923934A (en) Laser SLAM technology suitable for combining inertial navigation in unstructured scene
CN116594000A (en) Laser Doppler velocimeter online calibration method and device based on position observation
Atia et al. A novel systems integration approach for multi-sensor integrated navigation systems
CN117367430A (en) Vehicle-mounted positioning method for improved factor graph
CN112083465A (en) Position information acquisition system and method

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