CN113124901B - Position correction method and device, electronic device and storage medium - Google Patents

Position correction method and device, electronic device and storage medium Download PDF

Info

Publication number
CN113124901B
CN113124901B CN202110358325.7A CN202110358325A CN113124901B CN 113124901 B CN113124901 B CN 113124901B CN 202110358325 A CN202110358325 A CN 202110358325A CN 113124901 B CN113124901 B CN 113124901B
Authority
CN
China
Prior art keywords
position data
disturbance
difference
determining
variation
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202110358325.7A
Other languages
Chinese (zh)
Other versions
CN113124901A (en
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.)
China Railway Siyuan Survey and Design Group Co Ltd
Original Assignee
China Railway Siyuan Survey and Design Group 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 China Railway Siyuan Survey and Design Group Co Ltd filed Critical China Railway Siyuan Survey and Design Group Co Ltd
Priority to CN202110358325.7A priority Critical patent/CN113124901B/en
Publication of CN113124901A publication Critical patent/CN113124901A/en
Application granted granted Critical
Publication of CN113124901B publication Critical patent/CN113124901B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

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
    • 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
    • G01C25/005—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or starting-up of inertial devices
    • 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/393—Trajectory determination or predictive tracking, e.g. Kalman filtering
    • 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/40—Correcting position, velocity or attitude
    • 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/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
    • G01S19/47—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being an inertial measurement, e.g. tightly coupled inertial

Abstract

The embodiment of the application discloses a position correction method, which comprises the following steps: adding a perturbation to the first location data; and adding a perturbation to the second location data; determining a first position variation according to a difference value between the first position data after disturbance addition and the second position data after disturbance addition; determining third position data according to the first position data before disturbance addition and the first position variation; determining fourth position data according to the second position data before disturbance addition and the first position variation; determining a second position variation according to a difference value between the third position data after disturbance addition and the fourth position data after disturbance addition; and determining the corrected first position data and second position data based on the comparison result of the difference value between the first position variation and the second position variation and a preset threshold value. Thus, the difference between the first position data and the second position data is reduced.

Description

Position correction method and device, electronic device and storage medium
Technical Field
The present invention relates to the field of navigation positioning, and in particular, to a position correction method and apparatus, an electronic device, and a storage medium.
Background
At present, in the technology of providing position information by satellite navigation and positioning, for a positioning target in a moving state, such as a train with a high speed, because the train can not only encounter the problems of shielding satellite signals and the like in a tunnel, a bridge, a hill, an urban building and the like, but also be interfered and blocked by electromagnetic waves, the situations of satellite signal receiving difficulty, loss, errors and the like occur, and the reliability and the accuracy of train positioning are reduced. Some researches propose that an Inertial Navigation System (INS) sensor is added to perform multi-source information fusion processing, so that the positioning limitation of a Global Navigation Satellite System (GNSS) is compensated, and an effective combined positioning function whole is formed. The GNSS/INS integrated navigation technology and the Kalman filter form an integrated navigation positioning system, the INS system ensures the high-frequency output of the position of the integrated navigation positioning system, and meanwhile, the error of the INS output position is corrected through the Kalman filter based on the position output by the GNSS. However, in the mode of real-time positioning of the moving target, the position accuracy of the GNSS output is low, and in the case of poor satellite signals, the position of the GNSS output is a floating solution, and the accuracy is poor. At this time, the INS output position cannot be effectively corrected, and the overall positioning effect of the integrated navigation positioning system is further affected, so that the positioning reliability is poor.
Therefore, in order to improve the accuracy of the GNSS output position and improve the reliability of the integrated navigation positioning system, further correction of the positions output by the two navigation systems is required.
Disclosure of Invention
In view of this, embodiments of the present invention provide a position correction method and apparatus, an electronic device, and a storage medium.
The technical scheme of the invention is realized as follows:
in a first aspect, an embodiment of the present invention provides a position correction method, including:
the method comprises the steps that a first navigation system and a second navigation system are obtained to position the same positioning target at the same position, and first position data and second position data are obtained;
adding disturbance to the first position data to obtain the first position data after disturbance addition; adding disturbance to the second position data to obtain second position data after disturbance addition;
determining a first position variation according to a difference value between the first position data after the disturbance addition and the second position data after the disturbance addition and a difference value between the first position data before the disturbance addition and the second position data before the disturbance addition;
determining third position data according to the first position data before disturbance addition and the first position variation; determining fourth position data according to the second position data before disturbance addition and the first position variation;
determining a second position variation according to a difference value between the third position data after the disturbance addition and the fourth position data after the disturbance addition and a difference value between the third position data before the disturbance addition and the fourth position data before the disturbance addition;
and determining the corrected first position data and second position data based on the comparison result of the difference between the first position variation and the second position variation and a preset threshold.
Further, the method further comprises:
respectively determining a first disturbance quantity of the first position data and a second disturbance quantity of the second position data;
adding disturbance to the first position data to obtain disturbance-added first position data; adding disturbance to the second position data to obtain second position data after disturbance addition, wherein the second position data comprises:
adding the first disturbance amount to the first position data to obtain the first position data after disturbance addition; and adding the second disturbance amount to the second position data to obtain the second position data after disturbance addition.
Further, the first disturbance quantity is added to the first position data to obtain the first position data after disturbance addition; and adding the second disturbance amount to the second position data to obtain the second position data after disturbance addition, including:
adding the first disturbance amount to the first position data to respectively obtain first position data under forward disturbance and first position data under reverse disturbance; and adding the second disturbance amount to the second position data to respectively obtain second position data under forward disturbance and second position data under reverse disturbance.
Further, the determining the first position variation according to the difference between the first position data after the disturbance addition and the second position data after the disturbance addition and the difference between the first position data before the disturbance addition and the second position data before the disturbance addition includes:
determining a first position difference between first position data under forward disturbance and second position data under forward disturbance;
determining a second position difference between the first position data under the reverse disturbance and the second position data under the reverse disturbance;
determining a third position difference between the first position data before disturbance addition and the second position data before disturbance addition;
and determining the first position variation according to the first position difference, the second position difference and the third position difference.
Further, the determining the first position variation according to the first position difference, the second position difference and the third position difference includes:
determining a first position difference variation under forward disturbance according to the first position difference and the third position difference;
determining a second position difference variation under reverse disturbance according to the second position difference and the third position difference;
and determining the first position variation according to the first position difference variation and the second position difference variation.
Further, the method further comprises:
acquiring a first learning rate;
determining third position data according to the first position data before disturbance addition and the first position variation; and determining fourth position data according to the second position data before disturbance addition and the first position variation, wherein the fourth position data comprises:
determining third position data according to the first position data before disturbance addition, the first disturbance quantity, the first learning rate and the first position variation; and determining fourth position data according to the second position data before disturbance addition, the second disturbance quantity, the first learning rate and the first position variation.
Further, the determining a second position variation according to a difference between the third position data after the disturbance addition and the fourth position data after the disturbance addition and a difference between the third position data before the disturbance addition and the fourth position data before the disturbance addition includes:
respectively determining a third disturbance quantity of the third position data and a fourth disturbance quantity of the fourth position data;
adding the third disturbance amount to the third position data to respectively obtain third position data under forward disturbance and third position data under reverse disturbance; adding a fourth disturbance amount to the fourth position data to respectively obtain fourth position data under forward disturbance and fourth position data under reverse disturbance;
determining a fourth position difference between the third position data under the forward disturbance and the fourth position data under the forward disturbance;
determining a fifth position difference between the third position data under the reverse disturbance and the fourth position data under the reverse disturbance;
determining a sixth position difference between the third position data before disturbance addition and the fourth position data before disturbance addition;
and determining the second position variation according to the fourth position difference, the fifth position difference and the sixth position difference.
Further, the determining the second position variation according to the fourth position difference, the fifth position difference and the sixth position difference includes:
determining a third position difference variation under the forward disturbance according to the fourth position difference and the sixth position difference;
determining a fourth position difference variation under reverse disturbance according to the fifth position difference and the sixth position difference;
and determining the second position variation according to the third position difference variation and the fourth position difference variation.
Further, the method further comprises:
determining a second learning rate;
determining fifth position data according to third position data before disturbance addition, a third disturbance quantity, a second learning rate and a second position variation; and determining sixth position data according to the fourth position data before disturbance addition, the fourth disturbance quantity, the second learning rate and the second position variation.
Further, the determining a second learning rate includes:
a second learning rate is determined based on the first learning rate.
Further, the determining the corrected first position data and the second position data based on the comparison result of the difference between the first position variation and the second position variation and the preset threshold includes:
if the difference value between the first position variation and the second position variation is smaller than a preset threshold value, determining that the fifth position data is corrected first position data, and determining that the sixth position data is corrected second position data;
if the difference between the first position variation and the second position variation is larger than or equal to a preset threshold, determining the third position variation according to the difference between the fifth position data after disturbance addition and the sixth position data after disturbance addition and the difference between the fifth position data before disturbance addition and the sixth position data before disturbance addition, and determining the corrected first position data and second position data based on the comparison result of the difference between the second position variation and the third position variation and the preset threshold.
In a second aspect, an embodiment of the present invention provides a position correction apparatus, including:
the acquisition unit is used for acquiring the positioning of the first navigation system and the second navigation system on the same position of the same positioning target to obtain first position data and second position data;
the disturbance unit is used for adding disturbance to the first position data to obtain the first position data after disturbance addition; adding disturbance to the second position data to obtain the second position data after disturbance addition;
a determining unit, configured to determine a first position variation according to a difference between the first position data after disturbance addition and the second position data after disturbance addition, and a difference between the first position data before disturbance addition and the second position data before disturbance addition; determining third position data according to the first position data before disturbance addition and the first position variation; determining fourth position data according to the second position data before disturbance addition and the first position variation; determining a second position variation according to a difference value between the third position data after the disturbance addition and the fourth position data after the disturbance addition and a difference value between the third position data before the disturbance addition and the fourth position data before the disturbance addition; and determining the corrected first position data and second position data based on the comparison result of the difference value between the first position variation and the second position variation and a preset threshold value.
In a third aspect, an embodiment of the present invention provides an electronic device, where the electronic device includes: a processor and a memory for storing a computer program capable of running on the processor;
the processor, when running said computer program, performs the steps of one or more of the preceding claims.
In a fourth aspect, an embodiment of the present invention provides a computer-readable storage medium storing computer-executable instructions; the computer-executable instructions, when executed by a processor, are capable of implementing the methods described in one or more of the preceding claims.
The position correction method provided by the embodiment of the invention comprises the following steps: adding disturbance to first position data to obtain the first position data after disturbance addition; adding disturbance to the second position data to obtain the second position data after disturbance addition; determining a first position variation according to a difference value between the first position data after disturbance addition and the second position data after disturbance addition and a difference value between the first position data before disturbance addition and the second position data before disturbance addition; determining third position data according to the first position data before disturbance addition and the first position variation; determining fourth position data according to the second position data before disturbance addition and the first position variation; determining a second position variation according to a difference value between the third position data after the disturbance addition and the fourth position data after the disturbance addition and a difference value between the third position data before the disturbance addition and the fourth position data before the disturbance addition; and determining the corrected first position data and second position data based on the comparison result of the difference value between the first position variation and the second position variation and a preset threshold value. Therefore, the disturbance is added to the position data output by the two navigation systems respectively, and the difference value between the disturbed first position data and the disturbed second position data is adjusted based on the preset threshold value, so that the difference value between the finally corrected first position data and the finally corrected second position data is reduced, the error correction between the first position data and the second position data is realized, and the overall precision of the output position of the combined navigation system is improved.
Drawings
Fig. 1 is a schematic flow chart of a position correction method according to an embodiment of the present invention;
fig. 2 is a schematic flow chart of a position correction method according to an embodiment of the present invention;
fig. 3 is a schematic flow chart of a position correction method according to an embodiment of the present invention;
fig. 4 is a schematic flow chart illustrating a position correction method according to an embodiment of the present invention;
fig. 5 is a schematic flow chart illustrating a position correction method according to an embodiment of the present invention;
fig. 6 is a schematic flow chart illustrating a position correction method according to an embodiment of the present invention;
fig. 7 is a schematic flowchart of a position calibration method according to an embodiment of the present invention;
fig. 8 is a schematic flowchart of a position calibration method according to an embodiment of the present invention;
fig. 9 is a schematic flowchart of a position calibration method according to an embodiment of the present invention;
fig. 10 is a schematic flowchart of a position calibration method according to an embodiment of the present invention;
fig. 11 is a schematic flowchart of a position calibration method according to an embodiment of the present invention;
fig. 12 is a schematic structural diagram of a position calibration device according to an embodiment of the present invention;
fig. 13 is a schematic flowchart of a position calibration method according to an embodiment of the present invention;
fig. 14 is a flowchart illustrating a position correction method according to an embodiment of the present invention.
Detailed Description
In order to make the objects, technical solutions and advantages of the present invention clearer, the present invention will be further described in detail with reference to the accompanying drawings, the described embodiments should not be construed as limiting the present invention, and all other embodiments obtained by a person of ordinary skill in the art without creative efforts shall fall within the protection scope of the present invention.
In the following description, reference is made to "some embodiments" which describe a subset of all possible embodiments, but it is understood that "some embodiments" may be the same subset or different subsets of all possible embodiments, and may be combined with each other without conflict.
In the following description, references to the terms "first \ second \ third" are only to distinguish similar objects and do not denote a particular order, but rather the terms "first \ second \ third" are used to interchange specific orders or sequences, where appropriate, to enable embodiments of the invention described herein to be practiced in other than the order shown or described herein.
Unless defined otherwise, all technical and scientific terms used herein have the same meaning as commonly understood by one of ordinary skill in the art to which this invention belongs. The terminology used herein is for the purpose of describing embodiments of the invention only and is not intended to be limiting of the invention.
As shown in fig. 1, an embodiment of the present invention provides a position correction method, including:
s110: the method comprises the steps that a first navigation system and a second navigation system are obtained to position the same positioning target at the same position, and first position data and second position data are obtained;
s120: adding disturbance to the first position data to obtain the first position data after disturbance addition; adding disturbance to the second position data to obtain the second position data after disturbance addition;
s130: determining a first position variation according to a difference value between the first position data after disturbance addition and the second position data after disturbance addition and a difference value between the first position data before disturbance addition and the second position data before disturbance addition;
s140: determining third position data according to the first position data before disturbance addition and the first position variation; determining fourth position data according to the second position data before disturbance addition and the first position variation;
s150: determining a second position variation according to a difference value between the third position data after the disturbance addition and the fourth position data after the disturbance addition and a difference value between the third position data before the disturbance addition and the fourth position data before the disturbance addition;
s160: and determining the corrected first position data and second position data based on the comparison result of the difference value between the first position variation and the second position variation and a preset threshold value.
Here, the first navigation system and the second navigation system constitute a combined navigation system for locating an object, for example, locating a train running at a high speed. The first navigation system can be a GNSS system, and can also be other positioning systems such as a GPS system and a Beidou satellite navigation system; the second navigation system may be an INS system. The integrated navigation system carries out filtering correction and fusion processing on the position data output by the first navigation system and the position data output by the second navigation system of the same target at the same time through a Kalman filter to obtain accurate positioning information of the current positioning target (such as a train).
In the embodiment of the invention, the integrated navigation system composed of the first navigation system and the second navigation system is applied to positioning of a railway running train, the first position data output by the first navigation system is the position data output by the GNSS, GPS or Beidou satellite navigation system for positioning the train, and the second position data output by the second navigation system is the position data of the train obtained by resolving through the INS inertial navigation system. The position correction method provided by the embodiment of the invention is applied to correcting the first position data and the second position data output by the combined navigation system when the train runs.
The first position data and the second position data may be data capable of characterizing the target position information, such as coordinate information including a target position latitude L, a target position longitude λ, and a target position altitude h, or the like. After the first position data and the second position data output by the navigation system are obtained, disturbance is respectively added to the first position data and the second position data, wherein the disturbance can be random disturbance, or disturbance of a disturbance parameter is determined according to positioning precision required by a positioning target or output precision of a combined navigation system. Here, the disturbance added to the first position data and the second position data may be a bilateral disturbance. The bilateral perturbation herein may include a forward perturbation that may generate a positive incremental data change to the position data and a backward perturbation that may generate a negative incremental data change to the position data.
And performing gradient evaluation on the disturbed first position data and the disturbed second position data, wherein the gradient evaluation process comprises at least twice iterative calculation on the first position data and the second position data.
And determining the position variation obtained by each iterative calculation based on the difference between the disturbed first position data and the disturbed second position data and the variation of the difference between the first position data before disturbance and the second position data before disturbance. And comparing the difference value between the position variation values obtained by two adjacent iterative computations with a preset threshold value, and determining the corrected first position data and second position data obtained by the last iterative computation when the difference value meets a preset condition.
In one embodiment, the first position data and the second position data output by the GNSS/INS integrated navigation system for the same positioning target at the time k are subjected to position correction, wherein the first position data is GNSS position data, and the second position data is INS position data. Here, the time k at which the integrated navigation system outputs the position data may be determined by an output period preset by the integrated navigation system, for example, if the integrated navigation system outputs the position data every 5s, k may be 0, 5, 10 …. Determining a difference between the GNSS position data and the INS position data before disturbance addition:
Figure BDA0003004484730000091
here, p represents position data and the superscript j is the number of iterations in the current gradient estimation processThe subscript G represents GNSS position data, the subscript I represents INS position data, and abs is an absolute value.
Figure BDA0003004484730000092
Calculating the difference between the GNSS position data before disturbance addition and the INS position data for the jth iteration at the moment k;
Figure BDA0003004484730000093
GNSS position data before disturbance addition under jth iterative computation at the moment k, namely GNSS position data obtained by jth-1 iterative computation;
Figure BDA0003004484730000094
and calculating INS position data before disturbance addition for the j-th iteration at the k moment, namely INS position data obtained by the j-1 th iteration.
Figure BDA0003004484730000095
When the j-th iterative computation is performed at the time k, the difference between the previous GNSS position data and the INS position data is added in the L direction, the lambda direction and the h direction in a perturbation mode,
at this time, for the first position data and the second position data output by the system, if the iteration number in the current gradient evaluation process is the first time, j is 1, that is, the first position data output by the integrated navigation system is
Figure BDA0003004484730000101
And second position data
Figure BDA0003004484730000102
The difference before the disturbance addition is
Figure BDA0003004484730000103
Figure BDA0003004484730000104
In another embodiment, the number of GNSS positions is added based on the perturbationBased on the difference between the added INS position data and the perturbation, and
Figure BDA0003004484730000105
the first amount of change in position after the disturbance is added is determined collectively. Based on the first position data and the second position data before disturbance addition, namely the GNSS position data output by the combined navigation system
Figure BDA0003004484730000106
And combining the INS position data output by the navigation system
Figure BDA0003004484730000107
And a first position variation for determining a third position data, namely the GNSS position data output by the first iteration calculation
Figure BDA0003004484730000108
And fourth position data, i.e. INS position data output by the first iteration calculation
Figure BDA0003004484730000109
To pair
Figure BDA00030044847300001010
And
Figure BDA00030044847300001011
performing second iterative calculation according to the added disturbance
Figure BDA00030044847300001012
And after disturbance addition
Figure BDA00030044847300001013
A second amount of change in position is determined.
In another embodiment, the third position data is
Figure BDA00030044847300001014
And fourth location data
Figure BDA00030044847300001015
After the second iterative computation is performed to obtain the second position variation, the fifth position data, that is, the GNSS position data output by the second iterative computation, may be determined according to the third position data, the fourth position data, and the second position variation
Figure BDA00030044847300001016
And sixth position data, i.e. INS position data output by the second iteration
Figure BDA00030044847300001017
Comparing the difference between the first position variation and the second position variation with a preset threshold value delta, wherein the preset threshold value delta can be determined according to the output precision of the first navigation system. For example, δ may be 0.1 m.
If the difference between the first position variation and the second position variation is less than delta, the iterative calculation is performed to output
Figure BDA00030044847300001018
And
Figure BDA00030044847300001019
the first position data and the second position data can be respectively used as corrected first position data and second position data, can be used for being output to a Kalman filter for further position correction, and can also be used for replacing the first position data and the second position data output by the integrated navigation system for other processing operations.
In this way, by adding disturbance to the position data output by the integrated navigation system, the first position data and the second position data generate a position disturbance amount, and then the position variation amount generated after disturbance can be determined. Based on the position variation difference value in two adjacent iterative computations in the gradient evaluation process, evaluation is performed according to a preset threshold value, so that the difference value between the corrected first position data and the corrected second position data is reduced, the precision requirement corresponding to the preset threshold value is met, the error between the output positions of the two navigation systems is effectively corrected, the first position data and the second position data input to the Kalman filter for filtering correction are more reliable, and the positioning precision of the combined navigation system is further improved.
In some embodiments, as shown in fig. 2, the method further comprises:
s170: respectively determining a first disturbance quantity of the first position data and a second disturbance quantity of the second position data;
the S120 includes:
s121: adding the first disturbance amount to the first position data to obtain the first position data after disturbance addition; and adding the second disturbance amount to the second position data to obtain the second position data after disturbance addition.
In the embodiment of the present invention, after adding the disturbance to the first position data and the second position data, a position disturbance amount generated by the first position data, that is, a first disturbance amount, and a position disturbance amount generated by the second position data, that is, a second disturbance amount, are determined, respectively.
In one embodiment, for the GNSS/INS integrated navigation system, at time k, the first position data, i.e., the first disturbance amount of the GNSS position data, may be an absolute value of a position error of the GNSS system:
Figure BDA0003004484730000111
where a subscript G denotes a GNSS navigation system, a superscript k denotes a time at which the combined navigation system outputs the first position data and the second position data, Δ denotes a position error, abs denotes an absolute value function, and d denotes a disturbance amount.
Figure BDA0003004484730000112
A first disturbance quantity representing GNSS position data at time k;
Figure BDA0003004484730000113
a position error representing the GNSS position data at time k;
Figure BDA0003004484730000114
sequentially representing the position errors of the GNSS position data in the L direction, the lambda direction and the h direction at the k moment;
Figure BDA0003004484730000115
the perturbation amounts of the GNSS position data in the L direction, the λ direction, and the h direction at the k time are sequentially represented.
Here, the GNSS system position error may be synchronously output by the GNSS navigation system while outputting the first position data, depending on the accuracy of the GNSS navigation system. Therefore, the first disturbance amount is kept constant during the gradient estimation of the GNSS position data and the INS position data output by the integrated navigation system at a certain time.
In another embodiment, for a GNSS/INS integrated navigation system, the second position data, i.e., the second perturbation amount of the INS position data, may be an absolute value of a difference between the INS position data and the GNSS position data:
Figure BDA0003004484730000121
wherein the content of the first and second substances,
Figure BDA0003004484730000122
representing the disturbance quantity of INS position data during the jth iterative computation at the k moment;
Figure BDA0003004484730000123
representing GNSS position data before disturbance addition in the jth iterative computation at the moment k, namely GNSS position data obtained by the jth-1 iterative computation;
Figure BDA0003004484730000124
and representing INS position data before disturbance addition in the j-th iterative calculation at the k moment, namely INS position data obtained by the j-1 th iterative calculation. When the first iterative calculation is carried out on the first position data and the second position data output by the combined navigation system, j is 1, and the second disturbance quantity of the second position data is output by the navigation systemThe difference between the first position data and the second position data:
Figure BDA0003004484730000125
therefore, in the process of performing gradient estimation on the GNSS position data and the INS position data output by the integrated navigation system at a time, the perturbation amount of the corresponding INS position data in each iterative calculation is different.
Therefore, the disturbance is added to respectively determine the position disturbance amount of the first position data and the position disturbance amount of the second position data, so that the disturbed first position data and the disturbed second position data can be more accurately and respectively determined in the gradient evaluation process, and the first position variation can be further determined.
In some embodiments, as shown in fig. 3, the S121 includes:
s1211: adding the first disturbance amount to the first position data to respectively obtain the first position data under forward disturbance and the first position data under reverse disturbance; and adding the second disturbance amount to the second position data to obtain the second position data under forward disturbance and the second position data under reverse disturbance respectively.
In the embodiment of the invention, the disturbance added to the first position data and the second position data output by the integrated navigation system comprises forward disturbance and reverse disturbance, and the absolute value of the position disturbance quantity generated by the forward disturbance on the position data is the same as the absolute value of the position disturbance quantity generated by the reverse disturbance on the position data.
In one embodiment, for a GNSS/INS integrated navigation system, the first disturbance amount is based on
Figure BDA0003004484730000131
And first position data, which respectively determine the first position data under forward disturbance and the first position data under reverse disturbance:
Figure BDA0003004484730000132
here, the superscript "+" denotes the result under the influence of a forward disturbance, the subscript "-" denotes the result under the influence of a reverse disturbance, pk+,jRepresenting position data under forward disturbance, pk-,jRepresenting position data under reverse perturbation. When the first iterative computation is carried out on the first position data and the second position data output by the combined navigation system, j is 1, and the first position data under the forward disturbance is GNSS position data
Figure BDA0003004484730000133
First position data under reverse disturbance
Figure BDA0003004484730000134
In another embodiment, for a GNSS/INS integrated navigation system, the second disturbance amount is based on
Figure BDA0003004484730000135
And second position data, which respectively determine the second position data under forward disturbance and the second position data under reverse disturbance:
Figure BDA0003004484730000136
when the first iterative computation is carried out on the first position data and the second position data output by the combined navigation system, the second position data under the forward disturbance, namely INS position data
Figure BDA0003004484730000137
Second position data under reverse disturbance
Figure BDA0003004484730000138
Therefore, the first position data and the second position data after the forward disturbance is added and the first position data and the second position data after the reverse disturbance are added can be respectively determined according to the position disturbance amount, and further the position variation amount generated after the disturbance is added can be commonly determined.
In some embodiments, as shown in fig. 4, the S130 includes:
s131: determining a first position difference between the first position data under forward disturbance and the second position data under forward disturbance;
s132: determining a second position difference between the first position data under reverse disturbance and the second position data under reverse disturbance;
s133: determining a third position difference between the first position data before disturbance addition and the second position data before disturbance addition;
s134: and determining a first position variation according to the first position difference, the second position difference and the third position difference.
In the embodiment of the invention, the difference between the first position data and the second position data under forward disturbance and reverse disturbance is respectively determined, namely the difference between the first position data and the second position data after disturbance is added, and the first position variation is determined by combining the difference between the first position data and the second position data before disturbance is added.
In one embodiment, for a GNSS/INS integrated navigation system, the first position difference between the first position data under forward disturbance and the second position data under forward disturbance is:
Figure BDA0003004484730000141
here, the first and second liquid crystal display panels are,
Figure BDA0003004484730000142
and the absolute value of the position difference between the GNSS position data and the INS position data under the positive disturbance at the j th iteration calculation at the k moment is represented. When the GNSS position data and the INS position data output by the integrated navigation system at the time k are subjected to the first iterative computation, j is equal to 1, and the first position difference is
Figure BDA0003004484730000143
In another embodiment, for the GNSS/INS integrated navigation system, the second position difference between the first position data under reverse disturbance and the second position data under reverse disturbance is:
Figure BDA0003004484730000144
when the first iterative computation is performed on the GNSS position data and the INS position data output by the integrated navigation system at the time k, the second position difference is
Figure BDA0003004484730000145
In another embodiment, for the GNSS/INS integrated navigation system, the third position difference between the first position data before the disturbance addition and the second position data before the disturbance addition is:
Figure BDA0003004484730000146
when the first iterative computation is performed on the GNSS position data and the INS position data output by the integrated navigation system at the time k, the third position difference is
Figure BDA0003004484730000151
Based on this, according to the first position difference
Figure BDA0003004484730000152
Second difference in position
Figure BDA0003004484730000153
And a third position difference
Figure BDA0003004484730000154
The amount of change in the first position resulting from the first position data and the second position data before and after the addition of the disturbance may be determined collectively.
In some embodiments, as shown in fig. 5, the S134 includes:
s1341: determining a first position difference variation under forward disturbance according to the first position difference and the third position difference;
s1342: determining a second position difference variation under reverse disturbance according to the second position difference and the third position difference;
s1343: and determining a first position variation according to the first position difference variation and the second position difference variation.
In the embodiment of the invention, the first position variation after disturbance addition is determined jointly according to the difference between the two navigation system output position data after forward disturbance addition and the variation of the difference between the two navigation system output position data before disturbance addition, and the difference between the two navigation system output position data after reverse disturbance addition and the variation of the difference between the two navigation system output position data before disturbance addition.
In one embodiment, for the GNSS/INS integrated navigation system, the variation of the first position difference between the GNSS position data and the INS position data under the forward disturbance at the jth iterative computation at the time k is calculated
Figure BDA0003004484730000155
I.e. the difference between the first position difference and the third position difference;
and when the j-th iterative computation is carried out at the moment k, the variation of a second position difference between the GNSS position data and the INS position data under the reverse disturbance
Figure BDA0003004484730000156
I.e. the difference between the second position difference and the third position difference.
The first position variation is the absolute value of the difference between the first position difference variation and the second position difference variation
Figure BDA0003004484730000157
In another embodiment, for the GNSS/INS integrated navigation system, when performing the first iterative computation on the GNSS position data and the INS position data output by the integrated navigation system at time k, j is 1, and the first position difference variation under the forward disturbance is:
Figure BDA0003004484730000161
the second position difference variation under reverse disturbance is:
Figure BDA0003004484730000162
the first position variation is:
Figure BDA0003004484730000163
therefore, the difference value between the first position data and the second position data can represent the positioning precision of the combined navigation system to the positioning target, and the position variation generated after the bilateral disturbance is added is jointly determined by respectively determining the variation of the difference value between the first position data and the second position data after the forward disturbance and the reverse disturbance are added.
In some embodiments, as shown in fig. 6, the method further comprises:
s180: acquiring a first learning rate;
the S140 includes:
s141: determining third position data according to the first position data before disturbance addition, the first disturbance quantity, the first learning rate and the first position variation; and determining fourth position data according to the second position data before disturbance addition, the second disturbance amount, the first learning rate and the first position variation.
In the embodiment of the present invention, the learning rate is a calculation factor to be determined in each iterative calculation, for example, in the first iterative calculation, the third position data and the fourth position data may be determined based on the first learning rate.
In one embodiment, for a GNSS/INS integrated navigation system, an initial time k is set0Has an initial learning rate of K0E.g. K00.5. When the first position data and the second position data output by the combined navigation system are subjected to position correction at the time k, the first learning rate when the first iterative computation is carried out at the time k is
Figure BDA0003004484730000164
Wherein the content of the first and second substances,
Figure BDA0003004484730000165
and delta is a preset threshold value for the disturbance quantity of the INS navigation system in the h direction when the INS navigation system is subjected to the first iterative computation at the time k.
Here, since the learning rate is related to the INS position disturbance amount, it is based on
Figure BDA0003004484730000171
The INS position perturbation is known to be related to GNSS position data in L, λ, h directions. In positioning of the GNSS navigation system, the position error in the h direction is much larger than those in the L and λ directions, so that the perturbation amount of the INS in the h direction is larger than those in the other two directions. In order to minimize the difference between the iterative computation result of the position data in each direction and the previous iterative computation result in the subsequent iterative computation, the first learning rate should be the minimum value of the learning rates in the three directions. Therefore, according to the calculation formula of the first learning rate, the learning rate corresponding to the direction with the largest disturbance amount in the three directions should be selected, that is, the position disturbance amount d in the h direction of the INS position data is utilizedIk, h1 calculates the learning rate.
In another embodiment, for the GNSS/INS integrated navigation system, the third position data is determined according to the first position data before disturbance addition, the first disturbance amount, the first learning rate and the first position variation amount:
Figure BDA0003004484730000172
where i is 1,2, or 3, which indicates the ith element of the position data matrix; when i is 1,2, or 3, it indicates the L, λ, or h direction in the position data matrix, respectively.
Therefore, when the position data output by the combined navigation system is subjected to the first iteration calculation, j is 1, and the GNSS position data before the disturbance is added is used for calculating the position data
Figure BDA0003004484730000173
A first amount of disturbance
Figure BDA0003004484730000174
First learning rate Kk,1And a first position variation
Figure BDA0003004484730000175
Determining third position data, namely GNSS position data obtained after the first iteration:
Figure BDA0003004484730000176
in another embodiment, for the GNSS/INS integrated navigation system, the fourth position data is determined according to the second position data before disturbance addition, the second disturbance amount, the first learning rate and the first position variation amount:
Figure BDA0003004484730000177
therefore, when the position data output by the integrated navigation system is subjected to the first iterative calculation, j is 1, and the INS position data before adding the disturbance is used as the basis
Figure BDA0003004484730000178
A first amount of disturbance
Figure BDA0003004484730000179
First learning rate Kk,1And a first position variation
Figure BDA00030044847300001710
Determining fourth position data, namely INS position data after the first iteration:
Figure BDA0003004484730000181
in some embodiments, as shown in fig. 7, the S150 includes:
s151: determining a third disturbance amount of the third position data and a fourth disturbance amount of the fourth position data, respectively;
s152: adding the third disturbance amount to the third position data to obtain the third position data under forward disturbance and the third position data under reverse disturbance respectively; adding the fourth disturbance amount to the fourth position data to obtain the fourth position data under forward disturbance and the fourth position data under reverse disturbance respectively;
s153: determining a fourth position difference between the third position data under forward disturbance and the fourth position data under forward disturbance;
s154: determining a fifth position difference between the third position data under reverse disturbance and the fourth position data under reverse disturbance;
s155: determining a sixth position difference between the third position data before disturbance addition and the fourth position data before disturbance addition;
s156: and determining a second position variation according to the fourth position difference, the fifth position difference and the sixth position difference.
In the embodiment of the invention, after the third position data and the fourth position data are obtained by the first iterative computation, the third position data and the fourth position data are subjected to the second iterative computation, wherein the computation logic and the computation method of the second iterative computation are the same as those of the first iterative computation.
In one embodiment, for the GNSS/INS combined navigation system, in the second iterative computation, j is 2, a third disturbance amount of the third position data is determined, that is, a disturbance amount of the GNSS position data obtained after the first iterative computation is the same as the first disturbance amount, and is a position error output by the GNSS navigation system:
Figure BDA0003004484730000182
determining a fourth disturbance quantity of the fourth position data, namely the disturbance quantity of the INS position data obtained after the first iterative computation, as a difference value between the GNSS position data obtained after the first iterative computation and the INS position data:
Figure BDA0003004484730000191
determining third position data under forward disturbance according to the third disturbance quantity:
Figure BDA0003004484730000192
and third position data under reverse disturbance:
Figure BDA0003004484730000193
according to the fourth disturbance quantity, fourth position data under the forward disturbance is determined:
Figure BDA0003004484730000194
and fourth position data under reverse disturbance:
Figure BDA0003004484730000195
in another embodiment, for the GNSS/INS integrated navigation system, the fourth position difference under the forward disturbance is determined in the second iteration:
Figure BDA0003004484730000196
and a fifth position difference under reverse disturbance:
Figure BDA0003004484730000197
and a sixth position difference before disturbance addition:
Figure BDA0003004484730000198
based on a difference in position between the third position data and the fourth position data after the disturbance has been added
Figure BDA0003004484730000199
And
Figure BDA00030044847300001910
and characterizing a difference in position between the third positional data and the fourth positional data before disturbance addition
Figure BDA00030044847300001911
The second position change amount after the disturbance addition in the second iteration calculation can be jointly determined.
In some embodiments, as shown in fig. 8, the S156 includes:
s1561: determining a third position difference variation under positive disturbance according to the fourth position difference and the sixth position difference;
s1562: determining a fourth position difference variation under reverse disturbance according to the fifth position difference and the sixth position difference;
s1563: and determining a second position variation according to the third position difference variation and the fourth position difference variation.
In the embodiment of the present invention, the second position variation after adding disturbance is determined jointly according to the variation of the difference between the third position data and the fourth position data after adding forward disturbance compared with the variation of the difference between the position data before adding disturbance and the variation of the difference between the third position data and the fourth position data after adding reverse disturbance compared with the variation of the difference between the position data before adding disturbance.
In one embodiment, for the GNSS/INS combined navigation system, in the second iteration, j is 2, and the third position difference variation under the forward disturbance is:
Figure BDA0003004484730000201
the fourth position difference variation under reverse disturbance is:
Figure BDA0003004484730000202
the second position variation is:
Figure BDA0003004484730000203
in some embodiments, as shown in fig. 9, the method further comprises:
s157: determining a second learning rate;
s158: determining fifth position data according to the third position data before disturbance addition, the third disturbance quantity, the second learning rate and the second position variation; and determining sixth position data according to the fourth position data before disturbance addition, the fourth disturbance amount, the second learning rate and the second position variation.
In the embodiment of the present invention, the second learning rate is used to determine, in the second iterative computation, GNSS position data, that is, fifth position data, and INS position data, that is, sixth position data, which are obtained after the second iterative computation.
In one embodiment, for the GNSS/INS integrated navigation system, the second iterative computation is performed according to the GNSS position data before the perturbation addition, i.e. the third position data
Figure BDA0003004484730000204
Third amount of disturbance
Figure BDA0003004484730000205
Second learning rate Kk,2And a second position variation
Figure BDA0003004484730000206
Determining GNSS position data obtained after the second iteration, namely fifth position data:
Figure BDA0003004484730000207
in another embodiment, for the GNSS/INS integrated navigation system, the second iterative computation is performed according to the INS position data before the perturbation addition, i.e. the fourth position data
Figure BDA0003004484730000208
A fourth amount of disturbance
Figure BDA0003004484730000209
Second learning rate Kk,2And a second position variation
Figure BDA00030044847300002010
And determining INS position data obtained after the second iteration, namely sixth position data:
Figure BDA00030044847300002011
in some embodiments, as shown in fig. 10, the S157 includes:
s1571: a second learning rate is determined based on the first learning rate.
In the embodiment of the present invention, the second learning rate may be determined by the first learning rate, for example, the second learning rate may be half of the first learning rate.
In one embodiment, the learning rate for the j-th iteration at time K may be based on Kk,j＝Kk,j-1Determination of/2, i.e. starting from the second iteration, the learning rate of each iteration is half of the learning rate of the last iteration, e.g. Kk,2＝Kk,1/2。
In another embodiment, the second learning rate and the learning rate in the subsequent iteration calculation may also be determined exponentially or in a fractional decreasing manner according to the first learning rate. For example, the learning rate calculated by the jth iteration may be K, which is exponentially decreasingk,j＝ajKk,1A is a positive number smaller than 1, j is a positive integer equal to or larger than 2, for example, a may be 0.95, and the second learning rate K isk,2＝0.952Kk,1(ii) a The learning rate of the jth iteration calculation is decreased according to the fraction and can be Kk,j＝Kk,1J, the second learning rate is Kk,2＝Kk,1/2。
Therefore, the second learning rate is determined based on the first learning rate, the fact that the fifth position data and the sixth position data obtained in the second iterative calculation are closer to the optimal value can be guaranteed, the iterative calculation process cannot cause too long gradient evaluation time consumption due to too low learning rate, positioning efficiency is reduced, and the situation that the position data obtained by iterative calculation cannot be closer to the optimal value due to the fact that the learning rate is increased is avoided.
In some embodiments, as shown in fig. 11, the S160 includes:
s161: if the difference value between the first position variation and the second position variation is smaller than a preset threshold value, determining that the fifth position data is corrected first position data, and the sixth position data is corrected second position data;
s162: if the difference between the first position variation and the second position variation is larger than or equal to a preset threshold, determining a third position variation according to a difference between the fifth position data after disturbance addition and the sixth position data after disturbance addition and a difference between the fifth position data before disturbance addition and the sixth position data before disturbance addition, and determining the corrected first position data and second position data based on a comparison result of the difference between the second position variation and the third position variation and the preset threshold.
In the embodiment of the invention, the difference between the first position variation obtained in the first iteration calculation and the second position variation obtained in the second iteration calculation is determined, the difference is compared with the preset threshold delta, and if the difference is smaller than the preset threshold, the change of the difference between the first position data and the second position data in the current iteration calculation is lower than a certain threshold, so that the requirement of correction precision is met. And at the moment, the fifth position data and the sixth position data obtained by the second iterative computation are used as the corrected first position data and the corrected second position data, and are output to a Kalman filter for the next position correction or are output to other modules for processing operation.
In one embodiment, for a GNSS/INS integrated navigation system, a first amount of position change is determined
Figure BDA0003004484730000221
And the second position variation obtained in the second iterative calculation
Figure BDA0003004484730000222
The difference between:
Figure BDA0003004484730000223
will be provided with
Figure BDA0003004484730000224
Comparing with a preset threshold value delta if
Figure BDA0003004484730000225
Then the GNSS position data obtained by the second iterative computation, i.e., the fifth position data, is taken as the corrected first position data, and the INS position data obtained by the second iterative computation, i.e., the sixth position data, is taken as the corrected second position data.
In another embodiment, for a GNSS/INS integrated navigation system, after the second iteration, if
Figure BDA0003004484730000226
And if the difference between the current first position data and the current second position data does not meet the correction precision requirement, performing third iterative computation on the GNSS position data, namely the fifth position data, obtained by the second iterative computation and the INS position data, namely the sixth position data, obtained by the second iterative computation. The calculation logic and method of the third iteration calculation are the same as those of the previous two iteration calculations.
Determining after the third iterative calculation
Figure BDA0003004484730000227
And
Figure BDA0003004484730000228
and comparing the result with a preset threshold value delta. By analogy, in the j iteration calculation, comparison
Figure BDA0003004484730000229
And the magnitude of δ, if
Figure BDA00030044847300002210
J equals j +1, and the next iteration is performed until the next iteration is performed
Figure BDA00030044847300002211
Until the end; if it is not
Figure BDA00030044847300002212
The GNSS position data obtained by the jth iteration is used
Figure BDA00030044847300002213
And INS position data
Figure BDA00030044847300002214
And outputting as the corrected first position data and the corrected second position data.
As shown in fig. 12, an embodiment of the present invention provides a position correction apparatus, including:
the acquiring unit 10 is used for acquiring the positioning of the same positioning target at the same position by the first navigation system and the second navigation system to obtain first position data and second position data;
a perturbation unit 20, configured to add perturbation to the first position data to obtain the first position data after perturbation addition; adding disturbance to the second position data to obtain the second position data after disturbance addition;
a determining unit 30, configured to determine a first position variation according to a difference between the first position data after the disturbance addition and the second position data after the disturbance addition, and a difference between the first position data before the disturbance addition and the second position data before the disturbance addition; determining third position data according to the first position data before disturbance addition and the first position variation; determining fourth position data according to the second position data before disturbance addition and the first position variation; determining a second position variation according to a difference value between the third position data after the disturbance addition and the fourth position data after the disturbance addition and a difference value between the third position data before the disturbance addition and the fourth position data before the disturbance addition; and determining the corrected first position data and second position data based on the comparison result of the difference value between the first position variation and the second position variation and a preset threshold value.
One specific example is provided below in connection with any of the embodiments described above:
the gradient descent evaluation method is a common method for solving the unconstrained optimization problem, and is suitable for the optimization control process with more control variables, more complex controlled systems and incapability of establishing an accurate mathematical model. As shown in FIG. 13, PIINS position data, P, output for a GNSS/INS integrated navigation systemGGNSS position data, P 'output for GNSS/INS integrated navigation system'IIs new INS position data, P ', obtained after a preliminary gradient evaluation'GNew GNSS position data obtained after the pre-gradient estimation. In an embodiment of the invention, the pre-gradient estimation is mainly used to reduce the difference between the GNSS position data and the INS position data. And then, the GNSS position data and the INS position data after the gradient estimation in advance are substituted into a Kalman filter in the GNSS/INS integrated navigation system for processing, so that the reliability of the practical application of the GNSS/INS integrated navigation system is ensured. As shown in fig. 14:
1) and acquiring the disturbance quantity of the output positions of the GNSS and the INS, and determining the gradient evaluation learning rate.
Determination of disturbance quantity: in the gradient evaluation process, the position data of the GNSS and the INS are subjected to iterative computation according to the position disturbance amount and the learning rate of the GNSS position data and the INS position data respectively. Therefore, the learning rate, the GNSS position disturbance amount, and the INS position disturbance amount are predetermined before each gradient estimation. In the embodiment of the present invention, the GNSS position disturbance amount is an absolute value of a GNSS position error, that is:
Figure BDA0003004484730000241
where the subscript G denotes GNSS, the superscript k denotes time, L, λ and h denote latitude, longitude and altitude of the carrier, respectively, Δ denotes position error, abs denotes absolute value function, and d denotes disturbance amount. The position disturbance amount of the INS system is an absolute value of a difference value between the INS position and the GNSS position, that is:
Figure BDA0003004484730000242
where p denotes the position, the index I denotes the INS system, and the superscript j denotes the number of iterations in the gradient estimation process.
Determination of learning rate: since the GNSS position changes every time the computation is iterated in the gradient estimation process, the amount of disturbance of the INS position changes. Moreover, each time the GNSS system outputs a position, the position error thereof changes, and thus the initial learning rate of the gradient estimation at each time and the learning rate at each iterative calculation both change. In the embodiment of the invention, the initial time t is set0The initial learning rate K0 is 0.5; t is tk，k>Initial learning rate at 0
Figure BDA0003004484730000243
Wherein
Figure BDA0003004484730000244
For the disturbance in the initial iterative calculation of the INS in the direction of time k and h,
Figure BDA0003004484730000245
for the perturbation in the h direction of the INS at the j th iteration calculation at the k momentAnd the momentum delta is a preset limit value of the absolute value of the difference between the GNSS position and the INS position after the bilateral disturbance in the adjacent iterative computation. At the j-th iteration calculation, the learning rate is Kk,j＝Kk ,j-1And 2, namely, the learning rate used in the j iteration calculation is half of the learning rate in the j-1 iteration calculation.
2) And calculating the change quantity of the difference absolute value between the GNSS position and the INS position in the forward and reverse disturbance states.
Firstly, calculating the absolute value of the position difference between the GNSS and the INS system under the condition of not being influenced by disturbance momentum:
Figure BDA0003004484730000246
wherein
Figure BDA0003004484730000247
Is the difference between the GNSS position and the INS position at the jth iteration at time k. From the equation (2), the disturbance amount of the INS position can be known
Figure BDA0003004484730000251
And
Figure BDA0003004484730000252
are equal.
Secondly, respectively calculating the absolute value of the position difference between the GNSS and the INS system under the influence of the forward disturbance quantity and the reverse disturbance quantity:
Figure BDA0003004484730000253
Figure BDA0003004484730000254
Figure BDA0003004484730000255
Figure BDA0003004484730000256
in the above formula, the "+" sign indicates the result under the influence of the forward disturbance amount, the "-" sign indicates the result under the influence of the reverse disturbance amount, and pk+,jIndicating the position after application of the positive disturbance quantity, pk-,jIndicating the position after the application of the reverse disturbance amount,
Figure BDA0003004484730000257
and
Figure BDA0003004484730000258
respectively representing the absolute value of the position difference between the GNSS system and the INS system after applying forward disturbance quantity and backward disturbance quantity;
Figure BDA0003004484730000259
and
Figure BDA00030044847300002510
respectively representing the change of the absolute value of the position difference between the GNSS system and the INS system after applying the forward disturbance quantity and the reverse disturbance quantity.
And finally, calculating the variance of the absolute value of the azimuth difference after the bilateral disturbance as follows:
Figure BDA00030044847300002511
3) and updating the positions of the GNSS and INS systems.
Figure BDA00030044847300002512
Figure BDA00030044847300002513
In the above formula, i is 1,2,3, which represents the ith element of the matrix; when i is 1,2, or 3, it represents the values corresponding to the L, λ, or h directions in the matrix.
4) In the process of calculating two adjacent iterative calculations
Figure BDA00030044847300002514
And
Figure BDA00030044847300002515
the absolute value of the difference between.
Figure BDA00030044847300002516
5) Comparison
Figure BDA00030044847300002517
And the magnitude of δ, if
Figure BDA00030044847300002518
J equals j +1 and the steps 1) to 4) are repeated until
Figure BDA0003004484730000261
Until the end; if it is not
Figure BDA0003004484730000262
Then the iteration will obtain
Figure BDA0003004484730000263
And
Figure BDA0003004484730000264
and substituting the acquired position into a Kalman filter for processing to finally obtain the more accurate position of the GNSS/INS integrated navigation system at the moment k.
An embodiment of the present invention further provides an electronic device, where the electronic device includes: a processor and a memory for storing a computer program capable of running on the processor, the computer program when executed by the processor performing the steps of one or more of the methods described above.
An embodiment of the present invention further provides a computer-readable storage medium, where computer-executable instructions are stored in the computer-readable storage medium, and after being executed by a processor, the computer-executable instructions can implement the method according to one or more of the foregoing technical solutions.
The computer storage media provided by the present embodiments may be non-transitory storage media.
In the several embodiments provided in the present application, it should be understood that the disclosed apparatus and method may be implemented in other ways. The above-described device embodiments are merely illustrative, for example, the division of the unit is only a logical functional division, and there may be other division ways in actual implementation, such as: multiple units or components may be combined, or may be integrated into another system, or some features may be omitted, or not implemented. In addition, the coupling, direct coupling or communication connection between the components shown or discussed may be through some interfaces, indirect coupling or communication connection between devices or units, and may be electrical, mechanical or other driving.
The units described as separate parts may or may not be physically separate, and parts displayed as units may or may not be physical units, that is, may be located in one place, or may be distributed on a plurality of network units; some or all of the units can be selected according to actual needs to achieve the purpose of the solution of the embodiment.
In addition, all the functional units in the embodiments of the present invention may be integrated into one processing module, or each unit may be separately used as one unit, or two or more units may be integrated into one unit; the integrated unit can be realized by hardware running or by hardware and software functional units.
In some cases, any two of the above technical features may be combined into a new method solution without conflict.
In some cases, any two of the above technical features may be combined into a new device solution without conflict.
Those of ordinary skill in the art will understand that: all or part of the steps for implementing the method embodiments may be implemented by hardware related to program instructions, and the program may be stored in a computer readable storage medium, and when executed, the program performs the steps including the method embodiments; and the aforementioned storage medium includes: various media capable of storing program codes, such as a removable Memory device, a Read-Only Memory (ROM), a Random Access Memory (RAM), a magnetic disk, and an optical disk.
The above description is only for the specific embodiments of the present invention, but the scope of the present invention is not limited thereto, and any person skilled in the art can easily conceive of the changes or substitutions within the technical scope of the present invention, and all the changes or substitutions should be covered within the scope of the present invention. Therefore, the protection scope of the present invention shall be subject to the protection scope of the appended claims.

Claims (14)

1. A position correction method, characterized in that the method comprises:
the method comprises the steps that a first navigation system and a second navigation system are obtained to position the same positioning target at the same position, and first position data and second position data are obtained;
adding disturbance to the first position data to obtain the first position data after disturbance addition; adding disturbance to the second position data to obtain the second position data after disturbance addition;
determining a first position variation according to a difference value between the first position data after disturbance addition and the second position data after disturbance addition and a difference value between the first position data before disturbance addition and the second position data before disturbance addition;
determining third position data according to the first position data before disturbance addition and the first position variation; determining fourth position data according to the second position data before disturbance addition and the first position variation;
determining a second position variation according to a difference value between the third position data after the disturbance addition and the fourth position data after the disturbance addition and a difference value between the third position data before the disturbance addition and the fourth position data before the disturbance addition;
and determining the corrected first position data and second position data based on the comparison result of the difference value between the first position variation and the second position variation and a preset threshold value.
2. The method of claim 1, further comprising:
respectively determining a first disturbance quantity of the first position data and a second disturbance quantity of the second position data;
adding disturbance to the first position data to obtain the first position data after disturbance addition; adding disturbance to the second position data to obtain the second position data after disturbance addition, including:
adding the first disturbance amount to the first position data to obtain the first position data after disturbance addition; and adding the second disturbance amount to the second position data to obtain the second position data after disturbance addition.
3. The method according to claim 2, wherein the adding the first disturbance amount to the first position data results in disturbance-added first position data; and adding the second disturbance amount to the second position data to obtain the second position data after disturbance addition, including:
adding the first disturbance amount to the first position data to respectively obtain the first position data under forward disturbance and the first position data under reverse disturbance; and adding the second disturbance amount to the second position data to obtain the second position data under forward disturbance and the second position data under reverse disturbance respectively.
4. The method of claim 3, wherein determining the first amount of change in position based on the difference between the first position data after adding disturbance and the second position data after adding disturbance and the difference between the first position data before adding disturbance and the second position data before adding disturbance comprises:
determining a first position difference between the first position data under forward disturbance and the second position data under forward disturbance;
determining a second position difference between the first position data under reverse disturbance and the second position data under reverse disturbance;
determining a third position difference between the first position data before disturbance addition and the second position data before disturbance addition;
and determining a first position variation according to the first position difference, the second position difference and the third position difference.
5. The method of claim 4, wherein determining a first amount of change in position based on the first difference in position, the second difference in position, and the third difference in position comprises:
determining a first position difference variation under forward disturbance according to the first position difference and the third position difference;
determining a second position difference variation under reverse disturbance according to the second position difference and the third position difference;
and determining a first position variation according to the first position difference variation and the second position difference variation.
6. The method according to any one of claims 2 to 5, further comprising:
acquiring a first learning rate;
determining third position data according to the first position data before disturbance addition and the first position variation; and determining fourth position data according to the second position data before disturbance addition and the first position variation, wherein the fourth position data comprises:
determining third position data according to the first position data before disturbance addition, the first disturbance quantity, the first learning rate and the first position variation; and determining fourth position data according to the second position data before disturbance addition, the second disturbance amount, the first learning rate and the first position variation.
7. The method according to claim 1, wherein determining a second position change amount according to a difference between the third position data after the disturbance addition and the fourth position data after the disturbance addition and a difference between the third position data before the disturbance addition and the fourth position data before the disturbance addition comprises:
determining a third disturbance amount of the third position data and a fourth disturbance amount of the fourth position data, respectively;
adding the third disturbance amount to the third position data to obtain the third position data under forward disturbance and the third position data under reverse disturbance respectively; adding the fourth disturbance amount to the fourth position data to obtain the fourth position data under forward disturbance and the fourth position data under reverse disturbance respectively;
determining a fourth position difference between the third position data under forward disturbance and the fourth position data under forward disturbance;
determining a fifth position difference between the third position data under reverse disturbance and the fourth position data under reverse disturbance;
determining a sixth position difference between the third position data before disturbance addition and the fourth position data before disturbance addition;
and determining a second position variation according to the fourth position difference, the fifth position difference and the sixth position difference.
8. The method of claim 7, wherein determining a second amount of change in position based on the fourth difference in position, the fifth difference in position, and the sixth difference in position comprises:
determining a third position difference variation under positive disturbance according to the fourth position difference and the sixth position difference;
determining a fourth position difference variation under reverse disturbance according to the fifth position difference and the sixth position difference;
and determining a second position variation according to the third position difference variation and the fourth position difference variation.
9. The method of claim 7, further comprising:
determining a second learning rate;
determining fifth position data according to the third position data before disturbance addition, the third disturbance quantity, the second learning rate and the second position variation; and determining sixth position data according to the fourth position data before disturbance addition, the fourth disturbance amount, the second learning rate and the second position variation.
10. The method of claim 9, wherein determining the second learning rate comprises:
a second learning rate is determined based on the first learning rate.
11. The method according to claim 9, wherein determining the corrected first and second position data based on a comparison of a difference between the first and second amount of position change with a preset threshold comprises:
if the difference value between the first position variation and the second position variation is smaller than a preset threshold value, determining that the fifth position data is corrected first position data, and the sixth position data is corrected second position data;
if the difference between the first position variation and the second position variation is larger than or equal to a preset threshold, determining a third position variation according to a difference between the fifth position data after disturbance addition and the sixth position data after disturbance addition and a difference between the fifth position data before disturbance addition and the sixth position data before disturbance addition, and determining the corrected first position data and second position data based on a comparison result of the difference between the second position variation and the third position variation and the preset threshold.
12. A position correction apparatus, characterized in that the apparatus comprises:
the acquisition unit is used for acquiring the positioning of the first navigation system and the second navigation system on the same position of the same positioning target to obtain first position data and second position data;
the disturbance unit is used for adding disturbance to the first position data to obtain the first position data after disturbance addition; adding disturbance to the second position data to obtain the second position data after disturbance addition;
a determining unit, configured to determine a first position variation according to a difference between the first position data after disturbance addition and the second position data after disturbance addition, and a difference between the first position data before disturbance addition and the second position data before disturbance addition; determining third position data according to the first position data before disturbance addition and the first position variation; determining fourth position data according to the second position data before disturbance addition and the first position variation; determining a second position variation according to a difference value between the third position data after the disturbance addition and the fourth position data after the disturbance addition and a difference value between the third position data before the disturbance addition and the fourth position data before the disturbance addition; and determining the corrected first position data and second position data based on the comparison result of the difference value between the first position variation and the second position variation and a preset threshold value.
13. An electronic device, characterized in that the electronic device comprises: a processor and a memory for storing a computer program capable of running on the processor; wherein the content of the first and second substances,
the processor, when executing the computer program, performs the steps of the position correction method of any one of claims 1 to 11.
14. A computer-readable storage medium having stored thereon computer-executable instructions; the computer-executable instructions, when executed by a processor, enable the position correction method of any one of claims 1 to 11.
CN202110358325.7A 2021-04-01 2021-04-01 Position correction method and device, electronic device and storage medium Active CN113124901B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110358325.7A CN113124901B (en) 2021-04-01 2021-04-01 Position correction method and device, electronic device and storage medium

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110358325.7A CN113124901B (en) 2021-04-01 2021-04-01 Position correction method and device, electronic device and storage medium

Publications (2)

Publication Number Publication Date
CN113124901A CN113124901A (en) 2021-07-16
CN113124901B true CN113124901B (en) 2022-03-11

Family

ID=76775458

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110358325.7A Active CN113124901B (en) 2021-04-01 2021-04-01 Position correction method and device, electronic device and storage medium

Country Status (1)

Country Link
CN (1) CN113124901B (en)

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
DE60134556D1 (en) * 2000-07-28 2008-08-07 Honeywell Int Inc COMPLEMENTARY MIXING FILTER OF SECOND ORDER FOR A GPS INERTIAL NAVIGATION SYSTEM
CN103941271A (en) * 2014-03-11 2014-07-23 哈尔滨工程大学 Time-space difference GPS/SINS supercompact integrated navigation method
CN105004354A (en) * 2015-06-19 2015-10-28 北京航空航天大学 Unmanned aerial vehicle visible light and infrared image target positioning method under large squint angle
CN111337056A (en) * 2020-05-19 2020-06-26 北京数字绿土科技有限公司 Optimization-based LiDAR motion compensation position and attitude system alignment method
CN111854793A (en) * 2019-04-29 2020-10-30 北京初速度科技有限公司 Calibration method and device for lever arm between inertial measurement unit and global navigation system
CN111947681A (en) * 2020-06-24 2020-11-17 中铁第四勘察设计院集团有限公司 Filtering correction method for GNSS and inertial navigation combined navigation position output
CN112577521A (en) * 2020-11-26 2021-03-30 北京邮电大学 Combined navigation error calibration method and electronic equipment

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
DE60134556D1 (en) * 2000-07-28 2008-08-07 Honeywell Int Inc COMPLEMENTARY MIXING FILTER OF SECOND ORDER FOR A GPS INERTIAL NAVIGATION SYSTEM
CN103941271A (en) * 2014-03-11 2014-07-23 哈尔滨工程大学 Time-space difference GPS/SINS supercompact integrated navigation method
CN105004354A (en) * 2015-06-19 2015-10-28 北京航空航天大学 Unmanned aerial vehicle visible light and infrared image target positioning method under large squint angle
CN111854793A (en) * 2019-04-29 2020-10-30 北京初速度科技有限公司 Calibration method and device for lever arm between inertial measurement unit and global navigation system
CN111337056A (en) * 2020-05-19 2020-06-26 北京数字绿土科技有限公司 Optimization-based LiDAR motion compensation position and attitude system alignment method
CN111947681A (en) * 2020-06-24 2020-11-17 中铁第四勘察设计院集团有限公司 Filtering correction method for GNSS and inertial navigation combined navigation position output
CN112577521A (en) * 2020-11-26 2021-03-30 北京邮电大学 Combined navigation error calibration method and electronic equipment

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
A New Three-Dimensional Noise Modeling Method Based on Singular Value Decomposition and Its Application to CMONOC GPS Network;Ma Jun,et.al;《EARTH AND SPACE SCIENCE》;20210228;第8卷(第2期);第1-21页 *
Research on an Improved Method for Foot-Mounted Inertial/Magnetometer Pedestrian-Positioning Based on the Adaptive Gradient Descent Algorithm;Wang Qiuying,et.al;《SENSORS》;20181231;第18卷(第12期);第1-18页 *
SINS/GNSS组合导航系统中激光多普勒测速仪参数估计;马俊等;《激光杂志》;20210331;第42卷(第3期);第47-51页 *

Also Published As

Publication number Publication date
CN113124901A (en) 2021-07-16

Similar Documents

Publication Publication Date Title
CN109373999B (en) Integrated navigation method based on fault-tolerant Kalman filtering
US20230194265A1 (en) Square-Root Multi-State Constraint Kalman Filter for Vision-Aided Inertial Navigation System
CN111721289A (en) Vehicle positioning method, device, equipment, storage medium and vehicle
US8543266B2 (en) Modified Kalman filter for generation of attitude error corrections
CN105136145A (en) Kalman filtering based quadrotor unmanned aerial vehicle attitude data fusion method
CN103217175A (en) Self-adaptive volume Kalman filtering method
CN109059907A (en) Track data processing method, device, computer equipment and storage medium
CN113551666A (en) Automatic driving multi-sensor fusion positioning method and device, equipment and medium
US20200126429A1 (en) Method, device, and computer-readable storage medium with instructions for estimating the pose of a transportation vehicle
CN110395297B (en) Train positioning method
CN114370878B (en) Multi-AUV (autonomous Underwater vehicle) co-location method based on STACKF (space-time adaptive filter)
CN111103566A (en) Course angle determining method, course angle determining system, electronic equipment and storage medium
CN113124901B (en) Position correction method and device, electronic device and storage medium
CN103123487A (en) Spacecraft attitude determination method
CN105180955A (en) Real-time precise positioning method and real-time precise positioning device of motor vehicles
CN109459769B (en) Autonomous positioning method and system
CN114019954B (en) Course installation angle calibration method, device, computer equipment and storage medium
CN112986977B (en) Method for overcoming radar extended Kalman track filtering divergence
CN115993132A (en) Visual inertial odometer initialization method and device and aerocar
CN116917771A (en) Method for determining at least one system state by means of a Kalman filter
US20230078005A1 (en) Navigation assistance method for a mobile carrier
CN114255616A (en) Unpowered ship track prediction method, device, equipment and storage medium
CN114323007A (en) Carrier motion state estimation method and device
CN113008245B (en) Positioning information fusion method and device and computer server
US11892296B2 (en) Method for determining the position and the orientation of a vehicle

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