CN115575996A - Navigation correction method and system in static state based on static judgment - Google Patents
Navigation correction method and system in static state based on static judgment Download PDFInfo
- Publication number
- CN115575996A CN115575996A CN202211194301.3A CN202211194301A CN115575996A CN 115575996 A CN115575996 A CN 115575996A CN 202211194301 A CN202211194301 A CN 202211194301A CN 115575996 A CN115575996 A CN 115575996A
- Authority
- CN
- China
- Prior art keywords
- static
- data
- result
- state
- judgment result
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
- G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
- G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
- G01S19/42—Determining position
- G01S19/48—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system
- G01S19/49—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system whereby the further system is an inertial position system, e.g. loosely-coupled
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
- G01C21/1652—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments with ranging devices, e.g. LIDAR or RADAR
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/183—Compensation of inertial measurements, e.g. for temperature effects
- G01C21/188—Compensation of inertial measurements, e.g. for temperature effects for accumulated errors, e.g. by coupling inertial systems with absolute positioning systems
-
- 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
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Computer Networks & Wireless Communication (AREA)
- Automation & Control Theory (AREA)
- Navigation (AREA)
Abstract
The invention relates to the technical field of navigation, and provides a navigation correction method and a navigation correction system based on static judgment in a static state. The method comprises the following steps: acquiring first data output by an attitude sensor and second data output by a laser radar, and respectively resolving the first data and the second data to obtain first position change data and second position change data; taking the first position change data as a pre-measurement and the second position change data as an observation, performing extended Kalman filter operation to obtain an error amount of the second data, and correcting the second data to obtain third data; and respectively carrying out static judgment based on the first data and the third data, and locking the satellite navigation information and executing zero-speed correction if the satellite navigation information is determined to be in a static state. The invention can eliminate accumulated errors and obtain more accurate and effective navigation output.
Description
Technical Field
The invention relates to the technical field of navigation, in particular to a static judgment-based navigation correction method and system in a static state.
Background
A typical design principle block diagram of a satellite-inertial combined device in the prior art is shown in fig. 2, and the current satellite-inertial combined principle is that navigation information output by inertial navigation and navigation information output by a satellite card are subjected to combined kalman filtering, when a satellite navigation signal is affected, the satellite-inertial combined device loses the satellite navigation information and cannot be combined, and the device enters a pure inertial operation mode.
The satellite-inertial navigation combination equipment receives external satellite navigation information and inertial navigation information to carry out combined filtering, and when the satellite navigation signal is shielded or interfered, the combined filtering cannot be carried out, and the equipment can only carry out navigation by means of pure inertial updating. In the pure inertia mode, errors of components of an IMU (Inertial Measurement Unit) are accumulated along with integration, so that the precision of the device is reduced. Under the application scenes of urban road vehicle-mounted, port and wharf and the like, factors influencing the satellite navigation signal exist, so that the use effect of the satellite navigation combination equipment is influenced.
Therefore, how to provide a more effective navigation correction method and system becomes a technical problem to be solved urgently in the industry.
Disclosure of Invention
The invention provides a navigation correction method and a navigation correction system based on static judgment in a static state, which are used for solving the defect of equipment precision reduction caused by inertial navigation error accumulation in the prior art and realizing more effective navigation correction.
The invention provides a static judgment-based navigation correction method in a static state, which comprises the following steps:
acquiring first data output by an attitude sensor and second data output by a laser radar, and respectively resolving the first data and the second data to obtain first position change data and second position change data; the output frequency of the attitude sensor is greater than that of the laser radar;
performing extended Kalman filtering operation by taking the first position change data as a pre-measurement and the second position change data as an observation to obtain an error amount of the second data, and correcting the second data according to the error amount of the second data to obtain third data;
and performing static judgment based on the first data and the third data respectively to obtain a first static judgment result and a second static judgment result, and locking the satellite navigation information and executing zero-speed correction if the first static judgment result and the second static judgment result are both in a static state.
According to the navigation correction method based on static determination in the static state provided by the invention, the step of respectively performing static determination based on the first data and the third data to obtain a first static determination result and a second static determination result comprises the following steps:
inputting the first data into an acceleration mean square error detector, an acceleration amplitude detector and an angular velocity energy detector respectively to obtain an acceleration mean square error detection result, an acceleration amplitude detection result and an angular velocity energy detection result;
if the acceleration mean square error detection result is smaller than a set mean square error threshold value, the acceleration amplitude detection result is smaller than a set amplitude threshold value, and the angular velocity energy detection result is smaller than a set energy threshold value, outputting a first static judgment result of a static state; otherwise, outputting a first static judgment result in a non-static state.
According to the navigation correction method under the static state based on the static judgment, the acceleration mean square error detector, the acceleration amplitude detector and the angular velocity energy detector are all detection probability maximum likelihood detectors based on the set false alarm probability; the false alarm probability refers to the probability that the output of the acceleration mean square error detector, the acceleration amplitude detector or the angular velocity energy detector is in a static state when the attitude sensor is in a motion state; the detection probability refers to the probability that the output of the acceleration mean square error detector, the acceleration amplitude detector or the angular velocity energy detector is in a static state.
According to the navigation correction method based on static determination in the static state provided by the invention, the step of respectively performing static determination based on the first data and the third data to obtain a first static determination result and a second static determination result comprises the following steps:
judging laser radar position information corresponding to the third data according to obstacle and/or marker information in a preset scene map to obtain a second static judgment result in a static state or a non-static state; and/or the presence of a gas in the gas,
and judging the position change information of the laser radar relative to the obstacle according to the third data to obtain a second static judgment result in a static state or a non-static state.
According to the navigation correction method under the static state based on the static state determination, if the first static state determination result and the second static state determination result are both in the static state, the steps of locking the satellite navigation information and executing the zero-speed correction comprise:
performing static judgment according to fourth data output by the odometer to obtain a third static judgment result;
and if the first static judgment result, the second static judgment result and the third static judgment result are all in a static state, locking the satellite navigation information and executing zero-speed correction.
According to the navigation correction method under the static state based on the static state judgment, if the first static state judgment result and the second static state judgment result are both in the static state, the steps of locking the satellite navigation information and executing the zero speed correction comprise:
determining that the first static judgment result and the second static judgment result are both in a static state, setting a speed value serving as an observed quantity to be zero, and locking satellite navigation information;
inputting the reference course value in a static state and a speed value serving as an observed quantity into a navigation filter for standard Kalman filtering to obtain a navigation correction result;
the state vector of the navigation filter comprises any one or any combination of longitude, latitude, altitude, northbound speed, eastern speed, astronomical speed, pitching angle, roll angle, course angle, gyroscope triaxial zero offset, accelerometer triaxial zero offset, and odometer scale.
The invention also provides a navigation correction system based on static judgment in a static state, which comprises the following components:
the acquisition module is used for acquiring first data output by the attitude sensor and second data output by the laser radar, and respectively resolving the first data and the second data to obtain first position change data and second position change data; the output frequency of the attitude sensor is greater than that of the laser radar;
the extended Kalman filter module is used for performing extended Kalman filter operation by taking the first position change data as a prediction quantity and the second position change data as an observation quantity to obtain an error quantity of the second data, and correcting the second data according to the error quantity of the second data to obtain third data;
and the correction module is used for performing static judgment based on the first data and the third data respectively to obtain a first static judgment result and a second static judgment result, and locking the satellite navigation information and executing zero-speed correction if the first static judgment result and the second static judgment result are both in a static state.
The invention also provides an electronic device, which comprises a memory, a processor and a computer program stored on the memory and capable of running on the processor, wherein the processor executes the program to realize the steps of the navigation correction method based on the static determination in the static state.
The present invention also provides a non-transitory computer readable storage medium having stored thereon a computer program which, when being executed by a processor, carries out the steps of the static decision based navigation correction method in a stationary state as described in any of the above.
The present invention also provides a computer program product comprising a computer program which, when being executed by a processor, carries out the steps of the static decision-based navigation correction method in a stationary state as described in any of the above.
According to the navigation correction method and system based on static judgment in the static state, the high-frequency attitude sensor data, namely the first data are used for predicting and correcting the low-frequency laser radar data, namely the second data, in an interpolation manner, so that the third data which is free of errors and more accurate is obtained; and on the basis, static judgment is carried out by combining the third data and the first data, and the satellite navigation signal is locked in a static state for zero-speed correction, so that accumulated errors are eliminated, and more accurate and effective navigation output is obtained.
Drawings
In order to more clearly illustrate the technical solutions of the present invention or the prior art, the drawings needed for the description of the embodiments or the prior art will be briefly described below, and it is obvious that the drawings in the following description are some embodiments of the present invention, and those skilled in the art can also obtain other drawings according to the drawings without creative efforts.
FIG. 1 is a flow chart of a static decision-based navigation correction method in a static state according to the present invention;
FIG. 2 is a schematic block diagram of a prior art satellite inertial measurement unit;
FIG. 3 is a flow chart of the navigation state lock zero speed correction in the static determination and the static state according to the embodiment of the present invention;
FIG. 4 is a schematic flow chart of static determination of an attitude sensor according to an embodiment of the present invention;
FIG. 5 is a schematic diagram of the zero-speed correction principle provided by the embodiment of the present invention;
FIG. 6 is a flow chart illustrating a static state determination provided by an embodiment of the present invention;
FIG. 7 is a schematic flow chart illustrating a process for modifying lidar data using IMU data according to an embodiment of the present invention;
FIG. 8 is a schematic structural diagram of an electronic device provided by the present invention;
fig. 9 is a schematic structural diagram of a navigation correction device in a static state based on static determination according to the present invention.
Reference numerals:
810: a processor;
820: a communication interface;
830: a memory;
840: a communication bus;
901: an acquisition module;
902: an extended Kalman module;
903: and a correction module.
Detailed Description
In order to make the objects, technical solutions and advantages of the present invention more apparent, the technical solutions of the present invention will be clearly and completely described below with reference to the accompanying drawings, and it is obvious that the described embodiments are some, but not all embodiments of the present invention. All other embodiments, which can be derived by a person skilled in the art from the embodiments given herein without making any creative effort, shall fall within the protection scope of the present invention.
The static decision-based navigation correction method in a stationary state according to the present invention is described below with reference to fig. 1 to 7.
As shown in fig. 1, an embodiment of the present invention provides a static decision-based navigation correction method in a stationary state, including:
102, acquiring first data output by an attitude sensor and second data output by a laser radar, and respectively resolving the first data and the second data to obtain first position change data and second position change data; the output frequency of the attitude sensor is greater than that of the laser radar;
104, taking the first position change data as a prediction quantity and the second position change data as an observation quantity, performing extended kalman filter operation to obtain an error quantity of the second data, and correcting the second data according to the error quantity of the second data to obtain third data;
and 106, performing static judgment respectively based on the first data and the third data to obtain a first static judgment result and a second static judgment result, and locking the satellite navigation information and executing zero-speed correction if the first static judgment result and the second static judgment result are both in a static state.
In this embodiment, the first static determination result is a determination result of whether or not the vehicle is in a stationary state based on the attitude sensor; the second static determination result is a determination result of whether or not the laser radar is in a static state based on the laser radar
In a preferred embodiment, the first static determination result of step 106 is performed based on whether the magnitude of each of the accelerations of the first data is smaller than a set threshold, whether the mean square error of each of the accelerations is smaller than the set threshold, and whether the energy value of the angular velocity is smaller than the set threshold, if all three are smaller than the set threshold, the first static determination result of the static state is output, otherwise, the first static determination result of the non-static state is output;
similarly, the second static determination result of step 106 is performed based on whether the position variation of the third data is smaller than a set threshold, if the position variation is smaller than the set threshold, the second static determination result of the static state is output, otherwise, the second static determination result of the non-static state is output;
the angular velocity energy value refers to kinetic energy corresponding to the angular velocity of the attitude sensor obtained based on the gyroscope.
In an alternative embodiment, the precondition performed by step 102 is:
determining that the satellite navigation signal is in a set intensity interval, and performing inertial navigation based on an attitude sensor and a laser radar; the set intensity interval refers to the condition that the intensity of the satellite navigation signal is smaller than a set threshold value;
fig. 3 shows a more complete flow schematic based on the above optional embodiment, and in the static determination and navigation state locking technology in the static state provided in this embodiment, the navigation information of the device in the static state is locked, and meanwhile, zero-speed correction is performed to estimate and compensate a zero offset error of the IMU, thereby effectively suppressing divergence of the device error and improving the use accuracy of the device.
Specifically, referring to fig. 3, during the use of the device, the motion state of the device is always determined, and the navigation information locking and the zero speed correction are performed in the static state. In the process, errors of navigation information (information such as position and attitude) output by the equipment are effectively inhibited, zero-speed correction can estimate and compensate zero offset errors of the IMU, and the accuracy of the system is further improved.
An alternative device start-up procedure is as follows.
After the power supply of the equipment is turned on, the equipment starts to receive the satellite guide signal, the quality and the effectiveness of the satellite guide signal in a period of time are judged, if the satellite guide signal quality is good, the reliability is high, the equipment enters an initial alignment stage, the initial alignment has no requirement on the motion state of the vehicle at the moment, and the equipment can be performed statically and dynamically.
After the initial alignment is completed, the combination solution is started.
The initial alignment condition is relaxed, the use scenes of the equipment can be greatly enriched, and the commercial value of the equipment is improved.
The beneficial effect of this embodiment lies in:
predicting and correcting low-frequency laser radar data, namely second data, by interpolation of high-frequency attitude sensor data, namely first data, so as to obtain more accurate third data with errors eliminated; and on the basis, static judgment is carried out by combining the third data and the first data, and the satellite navigation signal is locked in a static state to carry out zero-speed correction, so that accumulated errors are eliminated, and more accurate and effective navigation output is obtained.
According to the above embodiment, in the present embodiment:
the step of performing static determination based on the first data and the third data, respectively, to obtain a first static determination result and a second static determination result includes:
inputting the first data into an acceleration mean square error detector, an acceleration amplitude detector and an angular velocity energy detector respectively to obtain an acceleration mean square error detection result, an acceleration amplitude detection result and an angular velocity energy detection result;
if the acceleration mean square error detection result is smaller than a set mean square error threshold value, the acceleration amplitude detection result is smaller than a set amplitude threshold value, and the angular velocity energy detection result is smaller than a set energy threshold value, outputting a first static judgment result of a static state; otherwise, outputting a first static judgment result in a non-static state.
In a preferred embodiment, the acceleration mean square error detector, the acceleration amplitude detector and the angular velocity energy detector are all detection probability maximum likelihood detectors based on a set false alarm probability; the false alarm probability refers to the probability that the output of the acceleration mean square error detector, the acceleration amplitude detector or the angular velocity energy detector is in a static state when the attitude sensor is in a motion state; the detection probability refers to the probability that the output of the acceleration mean square error detector, the acceleration amplitude detector or the angular velocity energy detector is in a static state.
An alternative zero-velocity detection method and three detectors based on the zero-velocity detection method will be described below, taking an IMU as an attitude sensor as an example.
First, the zero-speed detection method will be described.
The output of the IMU is written as follows:
wherein the content of the first and second substances,
is the specific force vector at time k,
is the angular velocity vector at time k. The zero-speed detection is to judge the motion state of the IMU when an observation sequence is given in a time period N to N + N-1 containing N observation values. In addition, to ensure that the probability of false detection (i.e., misjudging the motion state of the IMU) is as low as possible, a fixed false alarm probability is given (when the IM is used)U is a moving state and the detector determines the probability of being stationary), the probability that the detector detects a stationary state (detection probability) should be maximized. The detector can be under two assumptions H 0 And H 1 Selecting:
H 0 : IMU in motion
H 1 : IMU is in static state
The performance of the detector is determined by the false alarm probability P FA And a detection probability P D And (6) determining. For a given P FA Choose hypothesis H 1 Can make P D The conditions for maximization are:
wherein L (z) n ) For any z, as likelihood ratio n It represents H 1 Suppose relative to H 0 The assumed likelihood. The test method of the above equation is called likelihood ratio test. The threshold γ is determined by the following equation:
the output of the IMU may be expressed as:
y k ＝s k (θ)+v k
wherein
And
the specific force and angular velocity, respectively, and theta represents a series of unknown parameters required to describe the signal.
And
accelerometer and gyro noise, respectivelyThe noise is mutually independent zero mean Gaussian white noise, and the variance matrix of the noise is as follows:
at H 0 And H 1 In two hypothetical cases, the sensor signal satisfies the following condition:
Wherein the content of the first and second substances,
is the magnitude of the local gravity vector,
thus, in both hypothetical cases, the unknown parameters in the signal can be described as follows:
i.e. at H 0 Lower, s k (theta) is completely unknown, however in H 1 Next, only the direction of the specific force vector is unknown.
Since the signal parameter θ cannot be completely described, the probability density functions of the observed quantities under two assumptions cannot be accurately obtained. However, from the sensor model:
wherein:
after the unknown parameters are replaced by corresponding maximum likelihood estimation, generalized likelihood ratio test can be obtained, if the hypothesis is selected, the following requirements are met:
wherein, the first and the second end of the pipe are connected with each other,
is to assume H 1 The maximum likelihood estimate of the unknown parameter under,
is to assume H 0 Maximum likelihood estimation of the unknown parameter.
For unknown parameters, a maximum likelihood estimate may be obtained. Under the assumption of H 0 In the lower, the signal is completely unknown,
so that:
under the assumption of H 1 Next, maximum likelihood estimation of unknown parameters is enabledBy maximizing the formula:
the generalized likelihood ratio test becomes: if the hypothesis H is selected 1 The requirements are met,
the above formula is simplified to
Then there is
Wherein γ' = - (2/N) ln (γ). The above formula shows that there is a vector v with a magnitude g, the direction pointing in the direction of the mean of the specific force vectors. And if the mean square error of the accelerometer observed quantity and the vector v and the weighted average value of the gyro signal energy value are smaller than a threshold value gamma', judging that the IMU is in a static state through generalized likelihood ratio test.
The following describes a detector based on the zero-velocity detection method.
1) Acceleration mean square error detector
The acceleration mean square error detector determines H using only accelerometer data 1 It holds that:
2) Acceleration amplitude detector
The acceleration magnitude detector is then used to supplement the acceleration mean square error detector: if the acceleration amplitude detector detects a specific forceIf the specific force magnitude and g are very close to each other in the magnitude of the vector, the IMU can be judged to be in a stationary state at this time. Determination hypothesis H 1 And, it is satisfied that:
3) Angular velocity energy detector
An angular velocity energy detector may be used when only the energy value of the gyro signal can be used for zero velocity detection. Judgment H 1 It holds that:
wherein, the first and the second end of the pipe are connected with each other,
on the basis of the previous embodiment, the beneficial effect of this embodiment is to use three different zero-speed detectors at the same time to reduce the false detection rate.
According to any of the embodiments described above, in this embodiment:
the step of performing static determination based on the first data and the third data, respectively, to obtain a first static determination result and a second static determination result includes:
judging laser radar position information corresponding to the third data according to obstacle and/or marker information in a preset scene map to obtain a second static judgment result in a static state or a non-static state; and/or the presence of a gas in the gas,
and judging the position change information of the laser radar relative to the obstacle according to the third data to obtain a second static judgment result in a static state or a non-static state.
In a preferred embodiment, if it is determined that the first static determination result and the second static determination result are both in a static state, the step of locking the satellite navigation information and performing the null velocity correction includes:
performing static judgment according to fourth data output by the odometer to obtain a third static judgment result;
and if the first static judgment result, the second static judgment result and the third static judgment result are all in a static state, locking the satellite navigation information and executing zero-speed correction.
Further, if it is determined that the first static determination result and the second static determination result are both in a static state, the step of locking the satellite navigation information and performing the zero-velocity correction includes:
if the first static judgment result and the second static judgment result are both in a static state, setting a speed value serving as an observed quantity to be zero and locking satellite navigation information;
inputting the reference course value in a static state and a speed value serving as an observed quantity into a navigation filter for standard Kalman filtering to obtain a navigation correction result;
the state vector of the navigation filter comprises any one or any combination of longitude, latitude, altitude, northbound speed, eastern speed, skyward speed, pitching angle, roll angle, course angle, gyroscope triaxial zero offset, accelerometer triaxial zero offset, and odometer scale.
It is worth to be noted that the navigation filter state vector is 17-dimensional, and the input quantity is the position (longitude, latitude, altitude), speed (north, east, and sky), attitude (pitch, roll, and heading angle), gyroscope triaxial zero offset, accelerometer triaxial zero offset, and odometer scale factor of the carrier where the device is located.
The state vector setting of the navigation filter can be correspondingly adjusted along with different use scenes, so that the combined filter can achieve the best use effect.
In an alternative embodiment, the static determination and the zero-speed correction in the satellite navigation signal lock state (hereinafter referred to as state lock) may be implemented by a counter as follows.
Referring to fig. 4, when the device is started, the data of the IMU is collected, copied and stored in a specific array for static detection, and the counter starts counting while the copied data is stored. When the counter is less than 50, adding one to each group of copy data, when the counter reaches 50, leading the data stored in the groups into an acceleration mean square error detector, an acceleration amplitude detector and an angular velocity energy detector in sequence for zero-speed detection, and not resetting the counter data; after that, each time a new group of data is copied, the oldest stored data in the array is emptied, the storage positions of the existing data are all moved forward by 1 bit, the newest data is stored in the last bit of the array, the total number of the array is kept at 50 through the above operation, and the data stored in the array is subjected to zero-speed detection in the same way. And if the detection results of the three zero-speed detectors all judge that the equipment is in a static state, the detection result mark is given with 1, and if one or more judgment equipment in the three zero-speed detectors is in a motion state, the detection result mark is given with 0.
When the result flag is 1, the static flag ZuptCounter is increased by 1 (if the static flag ZuptCounter exceeds 2, the value is assigned to 2), when the result flag is 0, the static flag ZuptCounter is decreased by 2 (if the static flag ZuptCounter is less than 0, the value is assigned to 0), and when the static flag ZuptCounter is 2, the system determines that the device is in a static state.
The capacity of the array can be set by itself, and in the present embodiment, the output frequency of the IMU data is 50HZ, and the array is set to store 1 second of data.
Fig. 5 is a schematic flow chart of zero-speed correction in the state locking section based on the above-described static determination flow, and the state locking section is specifically described as follows.
The static mark ZuppCounter value is detected to be 2 for the first time, the course data output by the current satellite inertial combination is stored as a reference course value, and the course angle attitude angle error is used as an external observed quantity during zero-speed correction and is combined and filtered together with the speed error, so that the navigation information precision output by the satellite inertial combination equipment in the current environment is kept stable.
In the process, if the static flag ZuptCounter value is detected to be 0, exiting the zero-speed correction, and the current reference course value is invalid, the system enters the filtering updating mode in the normal motion mode until the static flag ZuptCounter value is detected to be 2 next time, and then entering the state locking and the zero-speed correction again.
And zero-speed correction, namely performing Kalman filtering on the reference course value and speed stored in a static state and a pure inertia updating result by taking the reference course value and the speed as observed quantities, wherein the speed value as the observed quantities is 0 because the equipment is in a static state.
In the present embodiment, 8-dimensional state quantities are selected:
X＝[δV E δV N φ E β N φ U ε x ε y ε z ]
wherein, δ V E 、δV N Velocity errors in east and sky directions, respectively, phi E 、φ N And phi U Attitude angle errors, ε, of pitch angle, roll angle and course angle, respectively x 、ε y And ε z The expression of the state equation of the Kalman filter for zero offset and zero speed correction of the gyroscope is shown as follows
The state transition matrix F (t) is represented as follows:
wherein, the first and the second end of the pipe are connected with each other,
where T is a strapdown matrix, ω ie The input quantity of the gyroscope is the rotational angular velocity of the earth, L is the local latitude, R M And R N The curvature radius of the prime meridian and the curvature radius of the prime circle are represented, f is specific force for measuring acceleration, V E 、V N 、V U East, north and sky speeds, respectively, and h is height.
At the time of carrier parking, the theoretical values of the east-direction speed and the north-direction speed are 0, so the east-direction speed and the north-direction speed obtained through navigation calculation are represented as the corresponding speed errors: delta V E 、δV N Selecting two horizontal velocity errors deltaV E 、δV N And heading angle attitude error phi U For the observed quantity, the measurement equation of the system is
Measuring matrix
Measuring noise
v E And v N Is a white noise component in the east and north accelerometer error outputs,
and outputting a white noise component in the course angle attitude error.
Further, on the basis of the above-mentioned embodiment of static determination based on the attitude sensor, fig. 6 shows a more complete static determination flow, that is, static determination is performed by comprehensively considering output data of the attitude sensor (such as IMU), the lidar and the odometer.
Specifically, the method comprises the following steps:
1. odometer data-assisted static testing
Receiving odometer information from a vehicle, wherein the odometer information comprises motion information of the vehicle where equipment is located, and performing static judgment by identifying the motion information and combining an IMU data static judgment calculation result and a laser radar static judgment test result;
2. laser radar assisted static testing
There are two types of static test modes assisted by laser radar:
1) The laser radar is used for measuring and drawing a high-precision map of an equipment-mounted vehicle operation scene in advance, mainly marking obstacles and markers in the scene, judging and identifying the position of the operation vehicle and surrounding scenes by combining position information output by the satellite-inertial combined equipment, and assisting in judging a static state according to the judgment and identification result of the position information;
2) The laser radar can continuously emit radar signals to the periphery, the distance between the working vehicle and surrounding obstacles is judged according to the reflection result of the radar signals, and the static state judgment can be assisted by identifying the reflection result.
More critically, the working principle of error compensation and calibration of the laser radar data by the IMU inertial measurement unit output data is shown in FIG. 7
The laser radar data frequency is slow and is output at 10HZ, the position information obtained by resolving is position increment relative to the initial position, the position result can generate distortion in the resolving process, the positioning effect is inaccurate, the IMU resolving frequency is high and is 50-200HZ, the position increment data can be obtained by resolving the gyroscope and accelerometer data output by the IMU of the inertial navigation system, the resolving position of the laser radar is continuously interpolated by the inertial navigation resolving result output at high frequency, the resolving result of the inertial navigation is used as a prediction quantity, the resolving position of the laser radar is used as an observed quantity, kalman filtering operation is expanded, the original data error of the laser radar is estimated, and the original data error is compensated and corrected, so that the output result is more accurate.
The beneficial effect of this embodiment lies in:
the embodiment provides a static determination and navigation state locking method in a static state, which effectively inhibits error divergence of equipment by locking navigation information output by static equipment in a pure inertia working mode, and meanwhile, estimates and compensates IMU zero offset errors by zero speed correction, thereby effectively improving the use precision of the equipment.
1. The static state detection method can set the array capacity of static detection according to the requirement, the static detection of the array passes, the result mark is 1, the static mark ZuppCounter adds 1 (if the static mark ZuppCounter exceeds 2, the value is assigned to 2), when the result mark is 0, the static mark ZuppCounter subtracts 2 (if the static mark ZuppCounter is less than 0, the value is assigned to 0), and when the static mark ZuppCounter is 2, the system judges that the equipment is in the static state.
2. When the static mark ZuppCounter is detected to be 2 for the first time, storing the course data output by the current satellite inertial combination as a reference course value, and performing combined filtering on the course angle attitude angle error and the speed error together to ensure that the accuracy of the navigation information output by the satellite inertial combination equipment under the current condition is kept stable.
3. And meanwhile, a plurality of zero-speed detectors are used for detecting the motion state of the equipment, so that the misjudgment rate is effectively reduced.
The technology of the embodiment enables the equipment in a static state to output stable navigation information (including position, speed, attitude and the like), simultaneously uses three different zero-speed detectors to reduce the false detection rate, and the zero-speed correction can estimate and compensate IMU zero offset errors by using the time, thereby effectively inhibiting the error divergence of the equipment and improving the use precision of the equipment.
The navigation correction device in a static state based on static determination provided by the present invention is described below, and the navigation correction device in a static state based on static determination described below and the navigation correction method in a static state based on static determination described above may be referred to in correspondence with each other.
As shown in fig. 9, an embodiment of the present invention further provides a navigation correction system in a static state based on static determination, including:
an obtaining module 901, configured to obtain first data output by an attitude sensor and second data output by a laser radar, and respectively calculate the first data and the second data to obtain first position change data and second position change data; the output frequency of the attitude sensor is greater than that of the laser radar;
an extended kalman module 902, configured to perform an extended kalman filtering operation to obtain an error amount of the second data by using the first position change data as a predictor and the second position change data as an observer, and correct the second data according to the error amount of the second data to obtain third data;
a correcting module 903, configured to perform static determination based on the first data and the third data, respectively, to obtain a first static determination result and a second static determination result, and if it is determined that the first static determination result and the second static determination result are in a static state, lock the satellite navigation information and perform zero-speed correction.
Further, the modification module 903 comprises:
the detector unit is used for inputting the first data into the acceleration mean square error detector, the acceleration amplitude detector and the angular velocity energy detector respectively to obtain an acceleration mean square error detection result, an acceleration amplitude detection result and an angular velocity energy detection result;
the first static judgment unit is used for outputting a first static judgment result of a static state if the acceleration mean square error detection result is smaller than a set mean square error threshold, the acceleration amplitude detection result is smaller than a set amplitude threshold, and the angular velocity energy detection result is smaller than a set energy threshold; otherwise, outputting a first static judgment result of the non-static state.
The acceleration mean square error detector, the acceleration amplitude detector and the angular velocity energy detector are all detection probability maximum likelihood detectors based on set false alarm probability; the false alarm probability refers to the probability that the output of the acceleration mean square error detector, the acceleration amplitude detector or the angular velocity energy detector is in a static state when the attitude sensor is in a motion state; the detection probability refers to the probability that the output of the acceleration mean square error detector, the acceleration amplitude detector or the angular velocity energy detector is in a static state.
The preset second static judgment unit is used for judging the laser radar position information corresponding to the third data according to the obstacle and/or marker information in a preset scene map to obtain a second static judgment result in a static state or a non-static state; and/or the presence of a gas in the gas,
and the real-time second static judgment unit is used for judging the position change information of the laser radar relative to the obstacle according to the third data to obtain a second static judgment result in a static state or a non-static state.
The third static judgment unit is used for performing static judgment according to fourth data output by the odometer to obtain a third static judgment result;
and a lock correction unit configured to lock the satellite navigation information and perform null correction if the first static determination result, the second static determination result, and the third static determination result are all in a stationary state.
A state locking unit, configured to set a velocity value as an observed quantity to zero and lock satellite navigation information if it is determined that the first static determination result and the second static determination result are both in a static state;
the zero-speed correction unit is used for inputting a reference course value in a static state and a speed value serving as an observed quantity into a navigation filter for standard Kalman filtering to obtain a navigation correction result;
the state vector of the navigation filter comprises any one or any combination of longitude, latitude, altitude, northbound speed, eastern speed, skyward speed, pitching angle, roll angle, course angle, gyroscope triaxial zero offset, accelerometer triaxial zero offset, and odometer scale.
The beneficial effect of this embodiment lies in:
predicting and correcting low-frequency laser radar data, namely second data, by interpolation of high-frequency attitude sensor data, namely first data, so as to obtain more accurate third data with errors eliminated; and on the basis, static judgment is carried out by combining the third data and the first data, and the satellite navigation signal is locked in a static state for zero-speed correction, so that accumulated errors are eliminated, and more accurate and effective navigation output is obtained.
Fig. 8 illustrates a physical structure diagram of an electronic device, which may include, as shown in fig. 8: a processor (processor) 810, a communication interface 820, a memory 830 and a communication bus 840, wherein the processor 810, the communication interface 820 and the memory 830 communicate with each other via the communication bus 840. The processor 810 may call logic instructions in the memory 830 to perform a static decision-based navigation fix-up method in a quiescent state, the method comprising: acquiring first data output by an attitude sensor and second data output by a laser radar, and respectively resolving the first data and the second data to obtain first position change data and second position change data; the output frequency of the attitude sensor is greater than that of the laser radar; performing extended Kalman filtering operation by taking the first position change data as a pre-measurement and the second position change data as an observation to obtain an error amount of the second data, and correcting the second data according to the error amount of the second data to obtain third data; and performing static judgment based on the first data and the third data respectively to obtain a first static judgment result and a second static judgment result, and locking the satellite navigation information and executing zero-speed correction if the first static judgment result and the second static judgment result are both in a static state.
In addition, the logic instructions in the memory 830 can be implemented in the form of software functional units and stored in a computer readable storage medium when the software functional units are sold or used as independent products. Based on such understanding, the technical solution of the present invention may be embodied in the form of a software product, which is stored in a storage medium and includes instructions for causing a computer device (which may be a personal computer, a server, or a network device) to execute all or part of the steps of the method according to the embodiments of the present invention. And the aforementioned storage medium includes: a U-disk, a removable hard disk, a Read-only memory (ROM), a Random Access Memory (RAM), a magnetic disk, an optical disk, or other various media capable of storing program codes.
In another aspect, the present invention also provides a computer program product, where the computer program product includes a computer program, the computer program can be stored on a non-transitory computer readable storage medium, and when the computer program is executed by a processor, a computer can execute the method for navigation modification in a static state based on static determination provided by the above methods, where the method includes: acquiring first data output by an attitude sensor and second data output by a laser radar, and respectively resolving the first data and the second data to obtain first position change data and second position change data; the output frequency of the attitude sensor is greater than that of the laser radar; performing extended Kalman filter operation by taking the first position change data as a predictor and the second position change data as an observer to obtain an error amount of the second data, and correcting the second data according to the error amount of the second data to obtain third data; and performing static judgment based on the first data and the third data respectively to obtain a first static judgment result and a second static judgment result, and locking the satellite navigation information and executing zero-speed correction if the first static judgment result and the second static judgment result are both in a static state.
In still another aspect, the present invention also provides a non-transitory computer-readable storage medium, on which a computer program is stored, the computer program being implemented by a processor to execute a static determination-based navigation correction method provided by the above methods in a static state, the method including: acquiring first data output by an attitude sensor and second data output by a laser radar, and respectively resolving the first data and the second data to obtain first position change data and second position change data; the output frequency of the attitude sensor is greater than that of the laser radar; performing extended Kalman filter operation by taking the first position change data as a predictor and the second position change data as an observer to obtain an error amount of the second data, and correcting the second data according to the error amount of the second data to obtain third data; and performing static judgment based on the first data and the third data respectively to obtain a first static judgment result and a second static judgment result, and locking the satellite navigation information and executing zero-speed correction if the first static judgment result and the second static judgment result are both in a static state.
The above-described embodiments of the apparatus are merely illustrative, and 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, may be located in one place, or may be distributed on a plurality of network units. Some or all of the modules may be selected according to actual needs to achieve the purpose of the solution of the present embodiment. One of ordinary skill in the art can understand and implement it without inventive effort.
Through the above description of the embodiments, those skilled in the art will clearly understand that each embodiment can be implemented by software plus a necessary general hardware platform, and certainly can also be implemented by hardware. Based on the understanding, the above technical solutions substantially or otherwise contributing to the prior art may be embodied in the form of a software product, which may be stored in a computer-readable storage medium, such as ROM/RAM, magnetic disk, optical disk, etc., and includes several instructions for causing a computer device (which may be a personal computer, a server, or a network device, etc.) to execute the method according to the various embodiments or some parts of the embodiments.
Finally, it should be noted that: the above examples are only intended to illustrate the technical solution of the present invention, but not to limit it; although the present invention has been described in detail with reference to the foregoing embodiments, it will be understood by those of ordinary skill in the art that: the technical solutions described in the foregoing embodiments may still be modified, or some technical features may be equivalently replaced; and such modifications or substitutions do not depart from the spirit and scope of the corresponding technical solutions of the embodiments of the present invention.
Claims (10)
1. A navigation correction method in a static state based on static judgment is characterized by comprising the following steps:
acquiring first data output by an attitude sensor and second data output by a laser radar, and respectively resolving the first data and the second data to obtain first position change data and second position change data; the output frequency of the attitude sensor is greater than that of the laser radar;
performing extended Kalman filtering operation by taking the first position change data as a pre-measurement and the second position change data as an observation to obtain an error amount of the second data, and correcting the second data according to the error amount of the second data to obtain third data;
and performing static judgment respectively based on the first data and the third data to obtain a first static judgment result and a second static judgment result, and locking the satellite navigation information and executing zero-speed correction if the first static judgment result and the second static judgment result are both in a static state.
2. The static determination-based navigation correction method in a stationary state according to claim 1, wherein the step of performing static determination based on the first data and the third data, respectively, to obtain a first static determination result and a second static determination result comprises:
inputting the first data into an acceleration mean square error detector, an acceleration amplitude detector and an angular velocity energy detector respectively to obtain an acceleration mean square error detection result, an acceleration amplitude detection result and an angular velocity energy detection result;
if the acceleration mean square error detection result is smaller than a set mean square error threshold value, the acceleration amplitude detection result is smaller than a set amplitude threshold value, and the angular velocity energy detection result is smaller than a set energy threshold value, outputting a first static judgment result of a static state; otherwise, outputting a first static judgment result in a non-static state.
3. The static state based navigation correction method of claim 2, wherein the acceleration mean square error detector, the acceleration amplitude detector and the angular velocity energy detector are all detection probability maximum likelihood detectors based on a set false alarm probability; the false alarm probability refers to the probability that the output of the acceleration mean square error detector, the acceleration amplitude detector or the angular velocity energy detector is in a static state when the attitude sensor is in a motion state; the detection probability refers to the probability that the output of the acceleration mean square error detector, the acceleration amplitude detector or the angular velocity energy detector is in a static state.
4. The static determination-based navigation correction method in a stationary state according to claim 1, wherein the step of performing static determination based on the first data and the third data, respectively, to obtain a first static determination result and a second static determination result comprises:
judging laser radar position information corresponding to the third data according to obstacle and/or marker information in a preset scene map to obtain a second static judgment result in a static state or a non-static state; and/or the presence of a gas in the atmosphere,
and judging the position change information of the laser radar relative to the obstacle according to the third data to obtain a second static judgment result in a static state or a non-static state.
5. The static decision-based navigation correction method in a stationary state according to claim 1, wherein if it is determined that the first static decision result and the second static decision result are both in a stationary state, the step of locking the satellite navigation information and performing the zero-velocity correction comprises:
performing static judgment according to fourth data output by the odometer to obtain a third static judgment result;
and if the first static judgment result, the second static judgment result and the third static judgment result are all in a static state, locking the satellite navigation information and executing zero-speed correction.
6. The static decision-based navigation correction method in a stationary state according to claim 1 or 5, wherein if it is determined that the first static decision result and the second static decision result are both in a stationary state, the step of locking the satellite navigation information and performing the null speed correction comprises:
if the first static judgment result and the second static judgment result are both in a static state, setting a speed value serving as an observed quantity to be zero and locking satellite navigation information;
inputting the reference course value in a static state and a speed value serving as an observed quantity into a navigation filter for standard Kalman filtering to obtain a navigation correction result;
the state vector of the navigation filter comprises any one or any combination of longitude, latitude, altitude, northbound speed, eastern speed, astronomical speed, pitching angle, roll angle, course angle, gyroscope triaxial zero offset, accelerometer triaxial zero offset, and odometer scale.
7. A navigation correction system in a stationary state based on a static determination, comprising:
the acquisition module is used for acquiring first data output by the attitude sensor and second data output by the laser radar, and respectively resolving the first data and the second data to obtain first position change data and second position change data; the output frequency of the attitude sensor is greater than that of the laser radar;
the extended Kalman filter module is used for performing extended Kalman filter operation by taking the first position change data as a prediction quantity and the second position change data as an observation quantity to obtain an error quantity of the second data, and correcting the second data according to the error quantity of the second data to obtain third data;
and the correction module is used for performing static judgment respectively based on the first data and the third data to obtain a first static judgment result and a second static judgment result, and locking the satellite navigation information and executing zero-speed correction if the first static judgment result and the second static judgment result are both in a static state.
8. An electronic device comprising a memory, a processor and a computer program stored on the memory and executable on the processor, wherein the processor when executing the program implements the steps of the static determination based navigation correction method in a static state according to any of claims 1 to 6.
9. A non-transitory computer readable storage medium, on which a computer program is stored, wherein the computer program, when being executed by a processor, implements the steps of the static determination-based navigation correction method in a stationary state according to any one of claims 1 to 6.
10. A computer program product comprising a computer program, wherein the computer program when executed by a processor implements the steps of the static decision based navigation correction method in a stationary state according to any of claims 1 to 6.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211194301.3A CN115575996A (en) | 2022-09-28 | 2022-09-28 | Navigation correction method and system in static state based on static judgment |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211194301.3A CN115575996A (en) | 2022-09-28 | 2022-09-28 | Navigation correction method and system in static state based on static judgment |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN115575996A true CN115575996A (en) | 2023-01-06 |
Family
ID=84582650
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202211194301.3A Pending CN115575996A (en) | 2022-09-28 | 2022-09-28 | Navigation correction method and system in static state based on static judgment |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN115575996A (en) |
-
2022
- 2022-09-28 CN CN202211194301.3A patent/CN115575996A/en active Pending
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN111156994B (en) | INS/DR & GNSS loose combination navigation method based on MEMS inertial component | |
| US8510044B2 (en) | Position sensing device and method | |
| EP2095148B1 (en) | Arrangement for and method of two dimensional and three dimensional precision location and orientation determination | |
| CN110031019B (en) | Slip detection processing method for automatic driving vehicle | |
| CN109343095B (en) | Vehicle-mounted navigation vehicle combined positioning device and combined positioning method thereof | |
| CN108931244B (en) | Inertial navigation error suppression method and system based on train motion constraint | |
| CN112507281B (en) | Method for estimating SINS/DVL tight combination system based on double-state multi-factor robust difference | |
| CN107247275B (en) | Urban GNSS vulnerability monitoring system and method based on bus | |
| US20230358541A1 (en) | Inertial navigation system capable of dead reckoning in vehicles | |
| CN110057356B (en) | Method and device for positioning vehicles in tunnel | |
| CN112432642B (en) | Gravity beacon and inertial navigation fusion positioning method and system | |
| US20140267686A1 (en) | System and method for augmenting a gnss/ins navigation system of a low dynamic vessel using a vision system | |
| CN108974054B (en) | Seamless train positioning method and system | |
| CN113029139B (en) | Airport flight area vehicle differential Beidou/SINS combined navigation method based on motion detection | |
| CN114415224A (en) | Vehicle fusion positioning system and method in complex limited environment | |
| Gao et al. | Development of precise GPS/INS/wheel speed sensor/yaw rate sensor integrated vehicular positioning system | |
| CN115060257B (en) | Vehicle lane change detection method based on civil-grade inertia measurement unit | |
| CN112212862A (en) | GPS/INS integrated navigation method for improving particle filtering | |
| CN113341443A (en) | Processing method of positioning track information and vehicle-mounted navigation device | |
| CN111443363A (en) | Satellite navigation deception identification method and device | |
| CN114074693A (en) | Train positioning method, device and system with multiple sensors integrated and train | |
| CN115575996A (en) | Navigation correction method and system in static state based on static judgment | |
| CN113985466A (en) | Combined navigation method and system based on pattern recognition | |
| US5928295A (en) | Method and apparatus for automatic calibration of the wheel track of a movable vehicle | |
| CN115839718A (en) | Fusion positioning method and device based on motion constraint |
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 |