CN-122015903-A - Inertial navigation speed correction method, device and system for commercial ultra-ground mobile equipment

Abstract

The invention discloses a method, a device and a system for correcting inertial navigation speed of a commercial super ground mobile device, wherein the method comprises the steps of determining the pixel speed of the ground mobile device according to a last frame image and a current frame image shot by the ground mobile device, and converting the pixel speed into a physical speed; the inertial navigation system comprises an inertial navigation system, an inertial navigation observation state, a current inertial navigation correction state, a current inertial navigation speed and a current inertial navigation speed, wherein the inertial navigation system is installed on the ground, the inertial navigation system is used for outputting inertial navigation data and a last inertial navigation correction state, the inertial navigation prediction state is determined according to the inertial navigation data and the last inertial navigation correction state, the inertial navigation observation state is constructed according to the physical speed of the ground mobile device, the last inertial navigation correction state and the inertial navigation prediction state, the current inertial navigation correction state is determined according to the inertial navigation observation state and the inertial navigation prediction state, and the correction speed in the current inertial navigation correction state is used as the current inertial navigation speed. The invention can restrain the drift of inertial navigation speed and improve the long-term stability of speed estimation.

Inventors

• LIU ZHENG
• ZHONG XI
• LIANG MIN

Assignees

• 汉朔科技股份有限公司

Dates

Publication Date

20260512

Application Date

20251223

Claims (18)

1. 1. The inertial navigation speed correction method for the commercial ultra-ground mobile equipment is characterized by comprising the following steps of: determining the pixel speed of the ground mobile equipment according to the last frame image and the current frame image shot by the ground mobile equipment, and converting the pixel speed into a physical speed; Determining an inertial navigation prediction state according to inertial navigation data output by an inertial navigation system installed on the ground mobile equipment and the last inertial navigation correction state; constructing an inertial navigation observation state according to the physical speed of the ground mobile equipment, the last inertial navigation correction state and the inertial navigation prediction state; and determining a current inertial navigation correction state according to the inertial navigation observation state and the inertial navigation prediction state, and taking the correction speed in the current inertial navigation correction state as the current inertial navigation speed.
2. 2. The method of claim 1, wherein determining the pixel speed of the ground mobile device from the last frame image and the current frame image taken by the ground mobile device comprises: Taking a goods shelf area which can be shot by the ground mobile equipment as a target shooting area, and selecting characteristic points of the target shooting area; In the target shooting area, calculating an optical flow component from each feature point of the previous frame image to a corresponding feature point of the current frame image, and calculating the pixel speed of each feature point according to the optical flow component; and calculating the pixel speed of the ground mobile equipment according to the pixel speeds of all the characteristic points in the current frame image.
3. 3. The method of claim 2, wherein selecting the feature point for the target capture area comprises: Detecting characteristic points of the target shooting area through preset conditions to obtain detected characteristic points; and selecting the feature points according to the number of the feature points configured in the configuration file and the distribution requirement of the feature points.
4. 4. A method as claimed in claim 3, wherein before calculating the optical flow component of each feature point of the previous frame image to the corresponding feature point of the current frame image, the method further comprises: Removing selected characteristic points of which the tracking state does not meet the preset tracking state requirement in the current frame image; re-selecting the characteristic points meeting the requirement of a preset tracking state from the detected characteristic points to complement the number of the configured characteristic points to obtain updated characteristic points; calculating an optical flow component from each feature point of the previous frame image to a corresponding feature point of the current frame image, including: and calculating optical flow components from each updated characteristic point of the previous frame image to the corresponding updated characteristic point of the current frame image.
5. 5. The method of claim 1, wherein converting the pixel speed to a physical speed comprises: calculating the physical speed of the ground mobile equipment according to the pixel speed and the scale factor; And when the physical speed is smaller than the minimum speed threshold, assigning the physical speed as the minimum speed threshold.
6. 6. The method as recited in claim 5, further comprising: after the pixel speed sequence within the first preset duration is recorded, calculating the speed deviation between the average value of all the pixel speeds in the pixel speed sequence and a preset standard speed value; if the average value is larger than a preset standard speed value and the speed deviation is larger than a speed deviation threshold value, reducing the scale factor; and if the average value is smaller than a preset standard speed value and the speed deviation is larger than a speed deviation threshold value, increasing the scale factor.
7. 7. The method as recited in claim 5, further comprising: identifying a marker area from an image of a shelf area which can be shot by the ground mobile equipment; determining the pixel size of the marker region in an image plane; and calculating a scale factor according to the proportional relation between the pixel size and the actual size of the calibration object.
8. 8. The method of claim 1, wherein determining the inertial navigation prediction state based on inertial navigation data output by an inertial navigation system mounted to the ground mobile device and a last inertial navigation correction state comprises: determining the current net acceleration according to the inertial navigation data, wherein the current net acceleration is a value obtained by adding speed in the inertial navigation data and removing gravity; and calculating an inertial navigation prediction state according to the last inertial navigation correction state, the state transition matrix, the control input matrix and the current net acceleration.
9. 9. The method of claim 1, wherein the inertial navigation observation state includes an observation speed and an observation zero offset error; According to the physical speed, the last inertial navigation correction state and the inertial navigation prediction state of the ground mobile equipment, constructing an inertial navigation observation state, wherein the method comprises the following steps: Taking the physical speed of the ground mobile equipment as an observation speed, and taking the corrected zero offset error in the last inertial navigation correction state as an observation zero offset error; If the physical speed of the ground mobile equipment and the predicted speed in the inertial navigation prediction state have direction conflict, taking the direction of the physical speed of the ground mobile equipment as the direction of the observed speed, and setting the observed zero offset error to be zero; calculating a difference between the physical speed amplitude and the predicted speed amplitude; If the difference value is smaller than the difference value threshold value, updating the amplitude value of the observed speed by using the average value of the physical speed amplitude value and the predicted speed amplitude value; if the difference value is not in the difference value threshold value range, judging whether the physical speed amplitude value and the predicted speed amplitude value are in a preset physical speed amplitude value range or not; If one of the physical speed amplitude and the predicted speed amplitude is in the preset physical speed amplitude range, updating the amplitude of the observed speed by using the physical speed amplitude or the predicted speed amplitude in the preset physical speed amplitude range; if the physical speed amplitude and the predicted speed amplitude are not in the preset physical speed amplitude range, updating the amplitude of the observed speed by using the preset speed amplitude.
10. 10. The method as recited in claim 9, further comprising: If the characteristic points exceeding the preset proportion in the current frame image meet the following conditions, determining that the ground mobile equipment is in a static state, and setting the observation speed to zero, wherein the characteristic points are selected from a target shooting area: the modulo length of the pixel speed of the feature point is below a preset rest threshold.
11. 11. The method as recited in claim 9, further comprising: and if the change rate of the physical speed of the ground mobile equipment has direction conflict with the current net acceleration, correcting the observed zero offset error according to the zero offset correction value, wherein the current net acceleration is a value obtained by processing acceleration and gravity in inertial navigation data.
12. 12. The method of claim 1, wherein determining a current inertial navigation correction state based on the inertial navigation observation state and the inertial navigation prediction state comprises: determining a prediction covariance matrix according to the last optimal covariance matrix, the state transition matrix and the process noise matrix; And performing Kalman filtering update according to the inertial navigation observation state, the inertial navigation prediction state and the prediction covariance matrix to obtain the current inertial navigation correction state.
13. 13. The method as recited in claim 12, further comprising: calculating the average value of the pixel speeds of all the characteristic points according to the pixel speeds of all the characteristic points in the current frame image, wherein the characteristic points are selected from a target shooting area; calculating the variance of the pixel speed of each feature point according to the average value; taking the average value of the variances of the pixel speeds of all the characteristic points as the dispersion of the current frame image; and determining the observation noise parameters of the observation noise matrix in the Kalman filtering updating according to the dispersion.
14. 14. An inertial navigation speed correction device for a commercial ultra-ground mobile device, comprising: the physical speed determining module is used for determining the pixel speed of the ground mobile equipment according to the last frame image and the current frame image shot by the ground mobile equipment and converting the pixel speed into a physical speed; The inertial navigation prediction state determining module is used for determining an inertial navigation prediction state according to inertial navigation data output by an inertial navigation system installed on the ground mobile equipment and the last inertial navigation correction state; the inertial navigation observation state construction module is used for constructing an inertial navigation observation state according to the physical speed of the ground mobile equipment, the last inertial navigation correction state and the inertial navigation prediction state; The inertial navigation correction module is used for determining a current inertial navigation correction state according to the inertial navigation observation state and the inertial navigation prediction state, and taking the correction speed in the current inertial navigation correction state as the current inertial navigation speed.
15. 15. The inertial navigation speed correction system of the commercial ultra-ground mobile equipment is characterized by comprising the inertial navigation speed correction device, the inertial navigation system and a camera of the commercial ultra-ground mobile equipment according to claim 14; The inertial navigation system is used for calculating inertial navigation data; the camera is used for obtaining images in a target shooting area.
16. 16. A computer device comprising a memory, a processor and a computer program stored on the memory and executable on the processor, characterized in that the processor implements the method of any of claims 1 to 13 when executing the computer program.
17. 17. A computer readable storage medium, characterized in that the computer readable storage medium stores a computer program which, when executed by a processor, implements the method of any one of claims 1 to 13.
18. 18. A computer program product, characterized in that the computer program product comprises a computer program which, when executed by a processor, implements the method of any one of claims 1 to 13.

Description

Inertial navigation speed correction method, device and system for commercial ultra-ground mobile equipment Technical Field The present invention relates to the field of communications technologies, and in particular, to a method and an apparatus for correcting error accumulation in an inertial navigation system. Background This section is intended to provide a background or context to the embodiments of the invention that are recited in the claims. The description herein is not admitted to be prior art by inclusion in this section. In super-home environments, ground mobile devices such as shopping carts, warehouse sorting carts, and the like commonly employ inertial navigation systems based on Inertial Measurement Units (IMUs) for velocity estimation. However, inertial navigation systems have a congenital problem in that the velocity is obtained by integrating the acceleration, and if there is a zero bias error or a long-term weak fluctuation of the accelerometer, the velocity estimation will inevitably produce cumulative drift. Over time, the error is continuously amplified, so that the judgment of the inertial navigation system on the actual speed and the motion trend deviates from the actual state, and the positioning and control reliability of the ground mobile equipment is seriously affected. Therefore, correction of the speed error accumulation of the inertial navigation system is required. Disclosure of Invention The embodiment of the invention provides an inertial navigation speed correction method for a commercial ultra-ground mobile device, which can inhibit the drift of the inertial navigation speed and improve the long-term stability of speed estimation, and comprises the following steps: determining the pixel speed of the ground mobile equipment according to the last frame image and the current frame image shot by the ground mobile equipment, and converting the pixel speed into a physical speed; Determining an inertial navigation prediction state according to inertial navigation data output by an inertial navigation system installed on the ground mobile equipment and the last inertial navigation correction state; constructing an inertial navigation observation state according to the physical speed of the ground mobile equipment, the last inertial navigation correction state and the inertial navigation prediction state; and determining a current inertial navigation correction state according to the inertial navigation observation state and the inertial navigation prediction state, and taking the correction speed in the current inertial navigation correction state as the current inertial navigation speed. The embodiment of the invention provides an inertial navigation speed correction device of a commercial ultra-ground mobile device, which can inhibit the drift of the inertial navigation speed and improve the long-term stability of speed estimation, and comprises the following components: the physical speed determining module is used for determining the pixel speed of the ground mobile equipment according to the last frame image and the current frame image shot by the ground mobile equipment and converting the pixel speed into a physical speed; The inertial navigation prediction state determining module is used for determining an inertial navigation prediction state according to inertial navigation data output by an inertial navigation system installed on the ground mobile equipment and the last inertial navigation correction state; the inertial navigation observation state construction module is used for constructing an inertial navigation observation state according to the physical speed of the ground mobile equipment, the last inertial navigation correction state and the inertial navigation prediction state; The inertial navigation correction module is used for determining a current inertial navigation correction state according to the inertial navigation observation state and the inertial navigation prediction state, and taking the correction speed in the current inertial navigation correction state as the current inertial navigation speed. The embodiment of the invention provides an inertial navigation speed correction system of a commercial ultra-ground mobile device, which can inhibit the drift of the inertial navigation speed and improve the long-term stability of speed estimation, and comprises an inertial navigation speed correction device of the commercial ultra-ground mobile device, an inertial navigation system and a camera; The inertial navigation system is used for calculating inertial navigation data; the camera is used for obtaining images in a target shooting area. The embodiment of the invention also provides computer equipment, which comprises a memory, a processor and a computer program stored on the memory and capable of running on the processor, wherein the processor realizes the inertial navigation speed correction method of the commercial off-the-ground mobile equipment