CN114063131A - GNSS/INS/wheel speed combined positioning real-time smoothing method - Google Patents

GNSS/INS/wheel speed combined positioning real-time smoothing method Download PDF

Info

Publication number
CN114063131A
CN114063131A CN202111349078.0A CN202111349078A CN114063131A CN 114063131 A CN114063131 A CN 114063131A CN 202111349078 A CN202111349078 A CN 202111349078A CN 114063131 A CN114063131 A CN 114063131A
Authority
CN
China
Prior art keywords
time
estimation
real
module
smoothing
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
CN202111349078.0A
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.)
Shanghai Gongji Technology Co ltd
Original Assignee
Shanghai Gongji Technology Co ltd
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 Shanghai Gongji Technology Co ltd filed Critical Shanghai Gongji Technology Co ltd
Priority to CN202111349078.0A priority Critical patent/CN114063131A/en
Publication of CN114063131A publication Critical patent/CN114063131A/en
Pending legal-status Critical Current

Links

Images

Classifications

    • 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
    • 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/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G01C21/28—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network with correlation of data from several navigational 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/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
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F30/00—Computer-aided design [CAD]
    • G06F30/20—Design optimisation, verification or simulation

Abstract

The invention discloses a GNSS/INS/wheel speed combined positioning real-time smoothing method which comprises a forward model module, a judgment module, a reverse model module and a smoothing module. The forward model module carries out forward processing and estimation on the acquired data and stores the equivalent of the estimation quantity at the corresponding moment. The judging module is used for judging whether the forward estimation time reaches the set sliding window size, if not, the forward estimation is continued, otherwise, the reverse estimation is carried out. And the inverse model module carries out inverse processing and estimation on the acquired data and stores the equivalent of the estimation quantity at the corresponding moment. And the smoothing module carries out weighted smoothing on the positioning parameters such as the position and the like estimated by the positive and negative modules at the corresponding moment and outputs the positioning parameters, and simultaneously, the smooth parameters such as zero offset and the like are further fed back to the positive model. Compared with the prior art, the method has the advantages of strong real-time performance, high estimation precision and the like.

Description

GNSS/INS/wheel speed combined positioning real-time smoothing method
Technical Field
The invention relates to the technical field of navigation, in particular to a GNSS/INS/wheel speed combined positioning real-time smoothing method.
Background
The GNSS/INS integrated navigation system is one of the most widely used vehicle navigation positioning systems at present, along with the development of intelligent vehicles, the requirement on positioning performance is increasingly strict, the INS cannot be measured and updated due to the fact that the GNSS is easily shielded by shielding objects to cause signal lock loss, and pose errors estimated by the INS are accumulated continuously, so that partial scholars consider fusing vehicle chassis information such as wheel speed to assist the INS, but the accuracy still needs to be improved, and therefore the post-processing is considered on the basis of the GNSS/INS/wheel speed. Although the positioning accuracy can be greatly improved after the post-processing technology is introduced, the technology does not have real-time performance, and the real-time high-accuracy positioning is needed in the aspects of planning and decision making of the existing intelligent vehicle.
Disclosure of Invention
The present invention is directed to provide a method for performing real-time smoothing on GNSS/INS/wheel speed combined positioning, so as to solve the problems mentioned in the background art.
In order to achieve the purpose, the invention provides the following technical scheme:
a GNSS/INS/wheel speed combined positioning real-time smoothing method comprises the following steps:
the forward model module is used for performing Kalman filtering estimation and feeding back to the INS for correction, and then sequentially storing parameters such as state estimators, covariance and the like at corresponding moments;
the judging module judges whether the time reaches the size of the sliding window, if not, the forward model module is continued, and if the time just reaches the size of the sliding window, the reverse model module is started;
the reverse model module is used for performing reverse recursion by taking the value of the last moment of the forward model module as the initial value of the reverse model module when the judging module meets the condition;
and the smoothing module is used for weighting and smoothing the initial value of the last moment of the reverse model module and the initial value of the forward model module to obtain an optimal solution close to real time, meanwhile, considering that parameters such as zero offset and the like are invariant in a short-term range, assigning the smoothed zero offset to the forward moment, then continuing to carry out forward filtering, improving the real-time estimation precision of the forward model and outputting the optimal solution in real time.
As a further technical solution of the present invention, the state estimator is written as:
Figure DEST_PATH_1
the forward model is:
Figure 273342DEST_PATH_IMAGE002
real-time fixed lag smoothing requires first building an inverse model:
Figure 100002_DEST_PATH_IMAGE003
wherein
Figure 502329DEST_PATH_IMAGE004
，
Figure 100002_DEST_PATH_IMAGE005
。
As a further technical scheme of the invention, a formula for performing weighted smoothing on the state estimators of the positive and negative modules is as follows:
Figure 408843DEST_PATH_IMAGE006
where the subscript s denotes the smoothed value, the subscript f denotes the value of the forward filtered estimate, and the subscript b denotes the value of the backward filtered estimate.
As a further technical scheme of the invention, the forward and reverse Kalman filtering specifically comprises the following steps:
system state vector:
Figure 100002_DEST_PATH_IMAGE007
calculating a state prediction:
Figure 748688DEST_PATH_IMAGE008
state one-step prediction mean square error matrix:
Figure 100002_DEST_PATH_IMAGE009
filtering gain vector:
Figure 794398DEST_PATH_IMAGE010
state estimation vector update:
Figure 100002_DEST_PATH_IMAGE011
state estimation mean square error update:
Figure 100002_DEST_PATH_IMAGE013
wherein
Figure 588096DEST_PATH_IMAGE014
Representing the variance of the noise of the equation of state process,
Figure 100002_DEST_PATH_IMAGE015
the predicted value of the state is represented,
Figure 731633DEST_PATH_IMAGE016
in order to be a system state transition matrix,
Figure 100002_DEST_PATH_IMAGE017
in order to be the system state at the last moment,
Figure 395964DEST_PATH_IMAGE018
a prediction value of the covariance matrix is represented,
Figure 100002_DEST_PATH_IMAGE019
in order to obtain the gain of the kalman filter,
Figure 468218DEST_PATH_IMAGE020
which represents the variance of the noise in the measurement process,
Figure 100002_DEST_PATH_IMAGE021
representing the current observed quantity measured by the sensor, I being the identity matrix.
As a further technical scheme of the invention, the smoothed positioning parameters such as the position and the like are output in a near real-time manner, and the corrected zero offset is further fed back to the forward model as the updated feedback quantity, so that the next real-time estimation is more accurate, the optimal solution is output in a real-time manner, and the rapid divergence of the recursive errors of the INS system can be reduced during the loss period of the GNSS signals.
Compared with the prior art, the invention has the beneficial effects that: the method integrates fixed lag smoothing on the basis of GNSS/INS/wheel speed, on one hand, the positioning accuracy after smoothing can be output in real time, on the other hand, the zero offset estimated by forward filtering can be smoothed, and the later stage estimation accuracy is improved.
Drawings
Fig. 1 is a frame diagram of the present invention.
In the figure, 1 is a forward model module; 2 is a judging module; 3 is a reverse model module; and 4, a smoothing module.
Detailed Description
The technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the drawings in the embodiments of the present invention, and it is obvious that the described embodiments are only a part of the embodiments of the present invention, and not all of the embodiments. All other embodiments, which can be derived by a person skilled in the art from the embodiments given herein without making any creative effort, shall fall within the protection scope of the present invention.
Referring to fig. 1, a GNSS/INS/wheel speed combined positioning real-time smoothing method is implemented by the following technical solutions:
building a reverse model on the basis of a GNSS/INS/wheel speed forward model, wherein the forward model is recorded as follows:
Figure 585210DEST_PATH_IMAGE002
the reverse model is:
Figure 827710DEST_PATH_IMAGE003
wherein
Figure 674443DEST_PATH_IMAGE004
，
Figure 947293DEST_PATH_IMAGE005
Wherein the positive and negative models are independent of each other.
The state analysis and fusion Kalman filtering technology specifically comprises the following steps:
system state vector:
Figure 613898DEST_PATH_IMAGE007
calculating a state prediction:
Figure 895974DEST_PATH_IMAGE008
state one-step prediction mean square error matrix:
Figure 833100DEST_PATH_IMAGE022
filtering gain vector:
Figure 542430DEST_PATH_IMAGE010
state estimation vector update:
Figure 696331DEST_PATH_IMAGE011
state estimation mean square error update:
Figure 570800DEST_PATH_IMAGE013
wherein
Figure 716611DEST_PATH_IMAGE014
Representing the variance of the noise of the equation of state process,
Figure DEST_PATH_IMAGE023
the predicted value of the state is represented,
Figure 295491DEST_PATH_IMAGE016
in order to be a system state transition matrix,
Figure 919370DEST_PATH_IMAGE024
in order to be the system state at the last moment,
Figure 842807DEST_PATH_IMAGE018
a prediction value of the covariance matrix is represented,
Figure 893939DEST_PATH_IMAGE019
in order to obtain the gain of the kalman filter,
Figure 22432DEST_PATH_IMAGE020
which represents the variance of the noise in the measurement process,
Figure 184423DEST_PATH_IMAGE021
representing the current observed quantity measured by the sensor, I being the identity matrix.
Assuming that the current time is t and the size of a sliding window is q, if mod (t, q) =0, performing backward filtering estimation from the time t to the time t-q, smoothing the estimated value of forward and backward filtering at the time t-q, assigning a slow-varying parameter such as zero offset to the time t (the zero offset is considered to be constant from the time t-q to the time q), simultaneously outputting parameters such as the position after smoothing at the time t-q, deleting the measured value at the time t-q from the window, increasing the measured value at the time t-q +1, and performing smooth estimation of the next round, so as to improve the precision under the condition of ensuring real time.
It will be evident to those skilled in the art that the invention is not limited to the details of the foregoing illustrative embodiments, and that the present invention may be embodied in other specific forms without departing from the spirit or essential attributes thereof. The present embodiments are therefore to be considered in all respects as illustrative and not restrictive, the scope of the invention being indicated by the appended claims rather than by the foregoing description, and all changes which come within the meaning and range of equivalency of the claims are therefore intended to be embraced therein. Any reference sign in a claim should not be construed as limiting the claim concerned.
Furthermore, it should be understood that although the present description refers to embodiments, not every embodiment may contain only a single embodiment, and such description is for clarity only, and those skilled in the art should integrate the description, and the embodiments may be combined as appropriate to form other embodiments understood by those skilled in the art.

Claims (5)

1. A GNSS/INS/wheel speed combined positioning real-time smoothing method is characterized by comprising the following steps:
the forward model module is used for performing Kalman filtering estimation and feeding back to the INS for correction, and then sequentially storing parameters such as state estimators, covariance and the like at corresponding moments;
the judging module judges whether the time reaches the size of the sliding window, if not, the forward model module is continued, and if the time just reaches the size of the sliding window, the reverse model module is started;
the reverse model module is used for performing reverse recursion by taking the value of the last moment of the forward model module as the initial value of the reverse model module when the judging module meets the condition;
and the smoothing module is used for weighting and smoothing the initial value of the last moment of the reverse model module and the initial value of the forward model module to obtain an optimal solution close to real time, meanwhile, considering that parameters such as zero offset and the like are invariant in a short-term range, assigning the smoothed zero offset to the forward moment, then continuing to carry out forward filtering, improving the real-time estimation precision of the forward model and outputting the optimal solution in real time.
2. The method of claim 1, wherein the GNSS/INS/wheel speed combined positioning real-time smoothing method,
the state estimator is written as:
Figure 1
the forward model is:
Figure 477382DEST_PATH_IMAGE002
real-time fixed lag smoothing requires first building an inverse model:
Figure DEST_PATH_IMAGE003
wherein
Figure 639110DEST_PATH_IMAGE004
，
Figure DEST_PATH_IMAGE005
。
3. The method of claim 2, wherein the equation for performing weighted smoothing on the state estimator of the positive and negative module is as follows:
Figure 150994DEST_PATH_IMAGE006
where the subscript s denotes the smoothed value, the subscript f denotes the value of the forward filtered estimate, and the subscript b denotes the value of the backward filtered estimate.
4. The GNSS/INS/wheel speed combined positioning real-time smoothing method as claimed in claim 2, wherein the forward and reverse Kalman filtering specifically comprises:
system state vector:
Figure DEST_PATH_IMAGE007
calculating a state prediction:
Figure 38179DEST_PATH_IMAGE008
state one-step prediction mean square error matrix:
Figure DEST_PATH_IMAGE009
filtering gain vector:
Figure 707451DEST_PATH_IMAGE010
state estimation vector update:
Figure DEST_PATH_IMAGE011
state estimation mean square error update:
Figure DEST_PATH_IMAGE013
wherein
Figure 39261DEST_PATH_IMAGE014
Representing the variance of the noise of the equation of state process,
Figure DEST_PATH_IMAGE015
the predicted value of the state is represented,
Figure 300609DEST_PATH_IMAGE016
in order to be a system state transition matrix,
Figure DEST_PATH_IMAGE017
in order to be the system state at the last moment,
Figure 253915DEST_PATH_IMAGE018
a prediction value of the covariance matrix is represented,
Figure DEST_PATH_IMAGE019
in order to obtain the gain of the kalman filter,
Figure 321228DEST_PATH_IMAGE020
which represents the variance of the noise in the measurement process,
Figure DEST_PATH_IMAGE021
representing the current observed quantity measured by the sensor, I being the identity matrix.
5. The method as claimed in claim 2, wherein the positioning parameters such as the smoothed position are output in a near real-time manner, and the corrected zero offset is further fed back to the forward model as the updated feedback quantity, so that the next real-time estimation is more accurate, the optimal solution is output in a real-time manner, and the recursive error of the INS system can be reduced to be rapidly dispersed during the loss of GNSS signals.
CN202111349078.0A 2021-11-15 2021-11-15 GNSS/INS/wheel speed combined positioning real-time smoothing method Pending CN114063131A (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111349078.0A CN114063131A (en) 2021-11-15 2021-11-15 GNSS/INS/wheel speed combined positioning real-time smoothing method

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111349078.0A CN114063131A (en) 2021-11-15 2021-11-15 GNSS/INS/wheel speed combined positioning real-time smoothing method

Publications (1)

Publication Number Publication Date
CN114063131A true CN114063131A (en) 2022-02-18

Family

ID=80272355

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111349078.0A Pending CN114063131A (en) 2021-11-15 2021-11-15 GNSS/INS/wheel speed combined positioning real-time smoothing method

Country Status (1)

Country Link
CN (1) CN114063131A (en)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114894222A (en) * 2022-07-12 2022-08-12 深圳元戎启行科技有限公司 External parameter calibration method of IMU-GNSS antenna and related method and equipment
CN114897942A (en) * 2022-07-15 2022-08-12 深圳元戎启行科技有限公司 Point cloud map generation method and device and related storage medium

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114894222A (en) * 2022-07-12 2022-08-12 深圳元戎启行科技有限公司 External parameter calibration method of IMU-GNSS antenna and related method and equipment
CN114897942A (en) * 2022-07-15 2022-08-12 深圳元戎启行科技有限公司 Point cloud map generation method and device and related storage medium
CN114897942B (en) * 2022-07-15 2022-10-28 深圳元戎启行科技有限公司 Point cloud map generation method and device and related storage medium

Similar Documents

Publication Publication Date Title
CN114063131A (en) GNSS/INS/wheel speed combined positioning real-time smoothing method
CN109521454B (en) GPS/INS integrated navigation method based on self-learning volume Kalman filtering
WO2019114757A1 (en) Optimization method and apparatus for multi-sensor target information fusion, computer device, and recording medium
CN110689576A (en) Automatic ware-based dynamic 3D point cloud normal distribution AGV positioning method
CN109059911B (en) Data fusion method of GNSS, INS and barometer
CN108508471A (en) A kind of automatic driving vehicle localization method and device
CN104021289A (en) Non-Gaussian unsteady-state noise modeling method
CN111784784B (en) IMU internal reference calibration method and device, electronic equipment and storage medium
CN112578419A (en) GPS data reconstruction method based on GRU network and Kalman filtering
CN110875054B (en) Far-field noise suppression method, device and system
CN112051569A (en) Radar target tracking speed correction method and device
CN110572139A (en) fusion filtering implementation method and device for vehicle state estimation, storage medium and vehicle
CN114295126A (en) Fusion positioning method based on inertial measurement unit
CN112305418A (en) Motor system fault diagnosis method based on mixed noise double filtering
CN111399021A (en) Navigation positioning method
CN112097772B (en) Robot and map construction method and device thereof
CN103530857B (en) Based on multiple dimensioned Kalman filtering image denoising method
CN117392215A (en) Mobile robot pose correction method based on improved AMCL and PL-ICP point cloud matching
CN113654554A (en) Fast self-adaptive dynamic inertial navigation real-time resolving denoising method
CN112880659A (en) Fusion positioning method based on information probability
US20220413050A1 (en) Complicated system fault diagnosis method and system based on multi-stage model
JPH0797136B2 (en) Multi-target tracking method and apparatus
CN117541655B (en) Method for eliminating radar map building z-axis accumulated error by fusion of visual semantics
CN115128655B (en) Positioning method and device for automatic driving vehicle, electronic equipment and storage medium
CN115451945A (en) Combined navigation post-processing method and system based on Kalman filtering combined graph optimization

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