Inertial navigation speed correction method, device and system for supermarket and supermarket ground mobile equipment

Abstract

The invention discloses an inertial navigation speed correction method, device and system for a supermarket ground mobile device, and the method comprises the steps: determining the pixel speed of the ground mobile device according to a previous frame image and a current frame image shot by the ground mobile device, 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 mounted on the ground mobile equipment and a previous inertial navigation correction state; constructing an inertial navigation observation state according to the physical speed of the ground mobile equipment, the previous 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 a correction speed in the current inertial navigation correction state as a current inertial navigation speed. Inertial navigation speed drift can be inhibited, and the long-term stability of speed estimation is improved.

Description

Technical Field

[0001] This invention relates to the field of communication technology, and in particular to a method and apparatus for correcting error accumulation in inertial navigation systems. Background Technology

[0002] This section is intended to provide background or context for the embodiments of the invention set forth in the claims. The description herein is not an admission that it is prior art simply because it is included in this section.

[0003] In supermarket indoor environments, ground-based mobile devices such as shopping carts and warehouse sorting carts commonly use inertial navigation systems based on inertial measurement units (IMUs) for velocity estimation. However, inertial navigation systems have an inherent problem: velocity is obtained by integrating acceleration. If the accelerometer has a zero-bias error or long-term slight fluctuations, the velocity estimation will inevitably produce cumulative drift. Over time, this error will continue to amplify, causing the inertial navigation system's judgment of actual velocity and motion trends to deviate from the true state, seriously affecting the reliability of the positioning and control of ground-based mobile devices.

[0004] Therefore, it is necessary to correct the velocity error accumulation of the inertial navigation system. Summary of the Invention

[0005] This invention provides an inertial navigation velocity correction method for commercial and retail ground mobile devices, which can suppress inertial navigation velocity drift and improve the long-term stability of velocity estimation, including:

[0006] Based on the previous frame image and the current frame image captured by the ground mobile device, the pixel velocity of the ground mobile device is determined, and the pixel velocity is converted into physical velocity.

[0007] The inertial navigation prediction state is determined based on the inertial navigation data output by the inertial navigation system installed on the ground mobile device and the previous inertial navigation correction state.

[0008] Based on the physical speed of the ground mobile device, the previous inertial navigation correction state, and the inertial navigation prediction state, the inertial navigation observation state is constructed.

[0009] Based on the inertial navigation observation state and the inertial navigation prediction state, the current inertial navigation correction state is determined, and the correction velocity in the current inertial navigation correction state is taken as the current inertial navigation velocity.

[0010] This invention provides an inertial navigation speed correction device for a commercial and supermarket ground mobile device, which can suppress inertial navigation speed drift and improve the long-term stability of speed estimation. The device includes:

[0011] The physical velocity determination module is used to determine the pixel velocity of the ground mobile device based on the previous frame image and the current frame image captured by the ground mobile device, and convert the pixel velocity into physical velocity.

[0012] The inertial navigation prediction state determination module is used to determine the inertial navigation prediction state based on the inertial navigation data output by the inertial navigation system installed on the ground mobile device and the previous inertial navigation correction state.

[0013] The inertial navigation observation state construction module is used to construct the inertial navigation observation state based on the physical speed of the ground mobile device, the previous inertial navigation correction state, and the inertial navigation prediction state.

[0014] The inertial navigation correction module is used to determine the current inertial navigation correction state based on the inertial navigation observation state and the inertial navigation prediction state, and to use the correction velocity in the current inertial navigation correction state as the current inertial navigation velocity.

[0015] This invention provides an inertial navigation speed correction system for a supermarket ground mobile device, which can suppress inertial navigation speed drift and improve the long-term stability of speed estimation. The system includes: an inertial navigation speed correction device for a supermarket ground mobile device, an inertial navigation system, and a camera.

[0016] The inertial navigation system is used to: calculate inertial navigation data;

[0017] The camera is used to: acquire images within the target shooting area.

[0018] This invention also provides a computer device, including a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the computer program, it implements the above-mentioned inertial navigation speed correction method for supermarket ground mobile devices.

[0019] This invention also provides a computer-readable storage medium storing a computer program that, when executed by a processor, implements the aforementioned inertial navigation speed correction method for commercial and supermarket ground mobile devices.

[0020] This invention also provides a computer program product, which includes a computer program that, when executed by a processor, implements the above-mentioned inertial navigation speed correction method for commercial and supermarket ground mobile devices.

[0021] In this embodiment of the invention, pixel velocities are extracted from the previous and current frames of images captured by the ground mobile device and converted into physical velocities. This provides an external visual reference for the inertial navigation system. Compared to the traditional approach that relies solely on the integral of inertial navigation acceleration, this physical velocity can reflect the actual motion trend of the device in real time, avoiding the amplification of zero-bias errors over time. The constructed inertial navigation observation state integrates physical velocity, the previous inertial navigation correction state, and the inertial navigation prediction state. It can specifically identify deviations in the inertial navigation prediction state and suppress drift from the source of error by updating the current inertial navigation correction state, ensuring that the velocity estimation of the device is always close to the true value during long-term operation. Attached Figure Description

[0022] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort. In the drawings:

[0023] Figure 1 This is a flowchart of the inertial navigation speed correction method for a supermarket ground mobile device in an embodiment of the present invention;

[0024] Figure 2 This is a flowchart illustrating the determination of pixel velocity of a ground-based mobile device in an embodiment of the present invention;

[0025] Figure 3 This is a schematic diagram of the inertial navigation speed correction device for a supermarket ground mobile device in an embodiment of the present invention;

[0026] Figure 4 This is a schematic diagram of the inertial navigation speed correction system for a supermarket ground mobile device in an embodiment of the present invention;

[0027] Figure 5 This is a schematic diagram of a computer device in an embodiment of the present invention. Detailed Implementation

[0028] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the embodiments of the present invention will be further described in detail below with reference to the accompanying drawings. Here, the illustrative embodiments of the present invention and their descriptions are used to explain the present invention, but are not intended to limit the present invention.

[0029] In the application scenario targeted by this invention, the shopping cart is already equipped with a side camera for environmental perception or other tasks. This camera can continuously capture images of the shelves on both sides of the cart, providing rich visual information.

[0030] Based on this premise, this invention proposes to utilize optical flow features in images captured by a camera to assist in correcting the velocity estimation output by an inertial navigation system. Specifically, by tracking and statistically analyzing the displacement (i.e., optical flow) of grayscale feature points in the image across consecutive frames, the pixel velocity corresponding to the current frame and its changing trend can be calculated. The direction of the optical flow reflects the vehicle's motion direction, while the velocity of the optical flow is proportional to the actual velocity; both can form part of the observation state of the inertial navigation system.

[0031] Furthermore, considering the complex states of shopping cart operation, such as frequent stops, slow turns, and backward movement, relying solely on the inertial navigation system or single-frame optical flow is insufficient to accurately determine the overall motion trend. Therefore, this invention further designs a multi-layered, dynamically adjustable fusion mechanism, including: using the consistency of optical flow across consecutive frames to determine whether the cart is in a stationary state; identifying the true velocity reversal based on the directional changes of feature point groups; and fusing the observed states through a Kalman filter structure to correct the velocity of the inertial navigation system in real time.

[0032] In summary, this invention introduces a visual observation mechanism into the original inertial navigation system to construct an optical flow-assisted inertial navigation fusion scheme suitable for ground mobile devices. This scheme can suppress velocity drift and has high robustness and practicality, providing a low-cost, high-precision positioning solution for scenarios such as supermarkets and warehouses.

[0033] Figure 1 The flowchart of the inertial navigation speed correction method for commercial and supermarket ground mobile devices in this embodiment of the invention includes:

[0034] Step 101: Determine the pixel velocity of the ground mobile device based on the previous frame image and the current frame image captured by the ground mobile device, and convert the pixel velocity into physical velocity.

[0035] Step 102: Determine the inertial navigation prediction state based on the inertial navigation data output by the inertial navigation system installed on the ground mobile device and the previous inertial navigation correction state;

[0036] Step 103: Construct the inertial navigation observation state based on the physical speed of the ground mobile device, the previous inertial navigation correction state, and the inertial navigation prediction state;

[0037] Step 104: Determine the current inertial navigation correction state based on the inertial navigation observation state and the inertial navigation prediction state, and use the correction velocity in the current inertial navigation correction state as the current inertial navigation velocity.

[0038] In this embodiment of the invention, pixel velocities are extracted from the previous and current frames of images captured by the ground mobile device and converted into physical velocities. This provides an external visual reference for the inertial navigation system. Compared to the traditional approach that relies solely on the integral of inertial navigation acceleration, this physical velocity can reflect the actual motion trend of the device in real time, avoiding the amplification of zero-bias errors over time. The constructed inertial navigation observation state integrates physical velocity, the previous inertial navigation correction state, and the inertial navigation prediction state. It can specifically identify deviations in the inertial navigation prediction state and suppress drift from the source of error by updating the current inertial navigation correction state, ensuring that the velocity estimation of the device is always close to the true value during long-term operation.

[0039] In this embodiment of the invention, the method is executed by the processor of a ground mobile device.

[0040] In step 101, the pixel velocity of the ground mobile device is determined based on the previous frame image and the current frame image captured by the ground mobile device, and the pixel velocity is converted into physical velocity.

[0041] Figure 2 This is a flowchart illustrating the process of determining the pixel velocity of a ground mobile device in an embodiment of the present invention. In one embodiment, determining the pixel velocity of the ground mobile device based on the previous frame image and the current frame image captured by the ground mobile device includes:

[0042] Step 201: Select the shelf area that can be photographed by the ground mobile device as the target shooting area, and select feature points in the target shooting area;

[0043] Step 202: In the target shooting area, calculate the optical flow component from each feature point of the previous frame image to the corresponding feature point of the current frame image, and calculate the pixel velocity of each feature point based on the optical flow component;

[0044] Step 203: Calculate the pixel velocity of the ground mobile device based on the pixel velocity of all feature points in the current frame image, and convert the pixel velocity into physical velocity.

[0045] In step 201, the ground-based mobile device can use a camera mounted on it or moving synchronously with it to capture the shelf area it can photograph as the target shooting area. Taking a shopping cart as an example, the shelf area that the camera mounted on the cart basket can photograph is selected as the target shooting area. When the right-side camera of the shopping cart is turned on, taking the area in front left of the cart basket as an example, as the shopping cart moves in the shopping aisle, key information of the shelves on the left side of the shopping cart can be captured in this area. Similarly, when the left-side camera of the shopping cart is turned on, the area in front right of the cart basket can be selected as the target shooting area to capture key information of the shelves on the right side of the shopping cart.

[0046] Depending on the actual scenario, you can choose to turn on the left or right camera to calculate optical flow, or turn on both cameras simultaneously to capture the changes in optical flow in different areas. By combining the optical flow data from both shelves, you can make a more comprehensive estimate of the shopping cart's movement posture and trend.

[0047] In one embodiment, feature point selection of the detected feature points includes:

[0048] Feature points are detected in the target shooting area by using preset conditions to obtain the detected feature points;

[0049] Feature points are selected based on the number and distribution requirements of feature points configured in the configuration file.

[0050] In this embodiment of the invention, the preset condition can be the grayscale gradient change value. Within the selected target area, feature point detection is used to obtain points with significant grayscale gradient changes in the target shooting area as feature points. These feature points can be Harris corner points, SIFT feature points, or ORB feature points. Although these can automatically extract relatively obvious texture features, the feature points rely entirely on the local pixel features of the image and do not incorporate the semantics of the actual scene, thus making them susceptible to environmental changes. For example, when a customer passes through the lens, there are changes in light reflection, or the camera captures areas with insufficient texture, such as the ground or blank walls, the number of feature points may be insufficient, cross-frame tracking may fail, or the position may drift randomly. Since optical flow calculation depends on the matching results of the same feature points in consecutive frames, insufficient feature point stability will directly affect the reliability of subsequent pixel velocity estimation. The configuration file can be stored directly in the processor of the ground mobile device or stored in the backend server and retrieved by the processor of the ground mobile device. The configuration file is pre-configured with the number and distribution requirements of feature points, making it easy to modify.

[0051] To improve the quality of feature points and the stability of cross-frame tracking, a shelf area recognition step can be added before feature point detection.

[0052] In one embodiment, before performing feature point detection on the target shooting area according to preset conditions, the method further includes:

[0053] Identify shelf outlines, electronic price tag outlines, and / or product outlines from images of the target shooting area;

[0054] Feature point detection is performed on the target shooting area using preset conditions, including:

[0055] Feature point detection is performed on the areas within the identified shelf outline, electronic price tag outline, and / or product outline using preset conditions.

[0056] In the above embodiments, a lightweight object detection or semantic segmentation model can first be performed on the image of the target shooting area to identify the locations of shelf outlines, electronic price tag outlines, and product outlines that have fixed structures in the real environment. Subsequently, feature point detection is performed only within these regions, rather than indiscriminately searching for corner points throughout the entire image. Because these regions have relatively stable spatial locations and texture features in a supermarket scene, they are more reliable than people, floors, or dynamic backgrounds. Therefore, the generated feature points have higher consistency and continuity in long-sequence tracking. This method can significantly reduce the problem of accidental feature point loss or jumps, making pixel velocity smoother in the time dimension, thereby improving the usability of optical flow estimation in real-world scenes.

[0057] When selecting feature points, the more feature points selected, the more accurate the estimation of the pixel velocity and direction of the subsequent ground mobile device will be. However, this also requires a larger amount of computation and consumes more resources. Due to the inherent edge distortion of cameras, pixels of the same length in world coordinates will appear as different pixel lengths in different areas of the image. Therefore, when selecting feature points, the distribution requirements should be considered. Selecting feature points that are as far away from existing feature points as possible will distribute the selected feature points across different locations in the target shooting area, resulting in a relatively more accurate velocity estimation.

[0058] In step 202, in the target shooting area, the optical flow component from each feature point of the previous frame image to the corresponding feature point of the current frame image is calculated, and the pixel velocity of each feature point is calculated based on the optical flow component.

[0059] The Lucas-Kanade method is used to track feature points in the previous frame. The key is to calculate the optical flow components of these feature points from the previous frame to the current frame, where u represents the optical flow component in the x-direction and v represents the optical flow component in the y-direction. The calculation steps for u and v include:

[0060] For each feature point in the previous frame, select a pixel window of size N×N (such as a 3×3 or 5×5 window) around it, and calculate the following for each pixel within the pixel window:

[0061] Sobel horizontal gradient (reflects the rate of change of gray level of a pixel in the x-direction);

[0062] Sobel vertical gradient (reflects the rate of change of gray level of a pixel in the y direction);

[0063] Based on the Sobel horizontal gradient and Sobel vertical gradient, a linear optical flow equation is constructed. The least squares method is used to solve the linear optical flow equation, and finally the optical flow components u and v of the feature points of the previous frame in the current frame are obtained.

[0064] Using the solved optical flow components u and v, the original positions of feature points from the previous frame are updated to obtain the new positions of those feature points in the current frame. The feature point position update method is as follows: .

[0065] As the shopping cart moves forward, feature points are tracked frame by frame. The displacement of each feature point needs to be converted into its pixel velocity based on the video frame rate. Let the position of the feature point in the i-th frame be denoted as... The position of the feature point in the (i-1)th frame is Then the distance the feature points move between these two frames is , The unit is pixels (px). If the video recording frame rate is F (which can also be expressed as the inter-frame time, in seconds), then the pixel velocity of a single feature point can be obtained as... , The unit is px / s.

[0066] In step 203, the pixel velocity of the ground mobile device is calculated based on the pixel velocity of all feature points in the current frame image, and the pixel velocity is converted into physical velocity.

[0067] The final result of the optical flow velocity calculation is the average pixel velocity of multiple feature points. In other words, the average pixel velocity of multiple feature points can be used as the pixel velocity of the ground mobile device. The direction of optical flow is the direction of movement of the feature points. The change in pixel velocity of the ground mobile device can be calculated based on the change in pixel velocity. The direction of optical flow is opposite to the direction of the ground mobile device, so the direction of travel of the ground mobile device can be calculated.

[0068] In one embodiment, before calculating the optical flow component from each feature point in the previous frame to the corresponding feature point in the current frame, the method further includes:

[0069] Remove selected feature points from the current frame image whose tracking state does not meet the preset tracking state requirements;

[0070] Among the detected feature points, feature points that meet the preset tracking state requirements are reselected to complete the configured number of feature points and obtain updated feature points.

[0071] Calculate the optical flow component from each feature point in the previous frame to the corresponding feature point in the current frame, including:

[0072] Calculate the optical flow component from each updated feature point in the previous frame to the corresponding updated feature point in the current frame.

[0073] In this embodiment of the invention, to improve the stability and accuracy of pixel velocity calculation, before subsequent image information fusion, the feature points extracted from the image need to be screened. Only feature points that appear continuously in multiple frames and have stable trajectories are retained as feature points that meet the preset tracking state requirements for subsequent calculation. This is because feature points that appear briefly or match accidentally often contain high uncertainty and are easily introduced with noise due to factors such as occlusion, blurring, and changes in illumination, thereby interfering with the direction judgment and amplitude estimation of pixel velocity. Therefore, a minimum visible frame threshold T_min (e.g., 3 frames) is set. Let T_i be the number of frames in which the i-th feature point is successfully tracked in the image sequence. If the following conditions are met:

[0074] If T_i ≥ T_min, then the feature point is marked as meeting the preset tracking state requirements. This filtering mechanism ensures that the feature points used have sufficient trajectory continuity, reflecting the actual motion trajectory of the object in real space.

[0075] By eliminating short-term jitter and spurious matching points, smoother and more directional observations can be obtained when calculating the average pixel velocity. This not only improves the accuracy of velocity direction determination but also significantly enhances the reliability of velocity amplitude estimation in subsequent fusion stages.

[0076] After pixel velocity calculation is completed, the pixel velocity (including velocity value and direction) of the feature points calculated in the current frame is recorded. The pixel velocity can be reported once at a frequency of 1 Hz. Then, the pixel velocity of the ground mobile device is calculated, and the operation is performed for the next frame. Each frame returns the velocity of one ground-moving pixel device, which can be reported once at a frequency of 1 Hz.

[0077] In one embodiment, converting the pixel velocity into physical velocity includes:

[0078] Calculate the physical velocity of the ground mobile device based on pixel velocity and scale factor;

[0079] When the physical speed is less than the minimum speed threshold, the physical speed is assigned the minimum speed threshold.

[0080] In this embodiment of the invention, the pixel velocity of the image output, measured in pixels per second (px / s), is essentially a relative velocity in the image plane. Lacking a measurable physical velocity dimension (such as m / s), this invention employs a linear scaling strategy. It assumes a linear relationship between the pixel velocity of the image and the actual physical velocity within a certain observation range and with specific camera parameters, achieved through a scaling factor. This scaling factor can be obtained by calibrating a fixed value during initialization (for example, when the mobile device and the shelf are 2 meters apart, the mobile device moves at a fixed speed, the currently obtained pixel velocity is measured, and the scaling factor is obtained by the ratio of this fixed speed to the pixel velocity). However, when the distance changes significantly (for example, when the distance between the camera of the ground mobile device and the area to be photographed on the shelf changes significantly), this fixed-value approach can lead to some deviation: if the distance is too close, the pixel displacement corresponding to the same physical velocity will increase, potentially misjudging it as high-speed movement; while when the distance increases, the pixel velocity will decrease, leading to an underestimation of the physical velocity or even misjudging it as stationary. Therefore, the scaling factor can be dynamically adjusted according to changes in the actual viewing distance. This invention proposes an adaptive scale factor correction mechanism that can automatically correct the mapping relationship between pixel velocity and physical velocity according to the real-time environment.

[0081] In one embodiment, the method further includes:

[0082] After recording the pixel velocity sequence within the first preset time period, the velocity deviation between the average value of all pixel velocities in the pixel velocity sequence and the preset standard velocity value is calculated.

[0083] If the average value is greater than the preset standard speed value and the speed deviation is greater than the speed deviation threshold, the scaling factor is reduced.

[0084] If the average value is less than the preset standard speed value and the speed deviation is greater than the speed deviation threshold, the scaling factor is increased.

[0085] The method described in the above embodiments does not rely on additional hardware sensors. Instead, it infers the relative distance between the shopping cart and the shelf based on the statistical characteristics of pixel velocity within a continuous time window. During operation, a pixel velocity sequence is recorded within a first preset duration (e.g., 3 seconds). The relative positional relationship between the camera and the shelf is determined by comparing its average value with a preset standard velocity value. If the average value is greater than the preset standard velocity value and the velocity deviation is greater than a velocity deviation threshold, it indicates that the overall pixel velocity within the first preset duration is too high. This suggests that the same motion results in a more significant pixel displacement, implying that the shopping cart is closer to the shelf. In this case, the scale factor needs to be reduced to avoid overestimating the physical velocity. Conversely, if the average value is less than the preset standard velocity value and the velocity deviation is greater than the velocity deviation threshold, it indicates that the overall pixel velocity within the first preset duration is too low. This means that the viewing distance is far, and the scale factor needs to be appropriately increased to compensate for the underestimation effect caused by the smaller pixel displacement. In one embodiment, the ratio of the average value to the pixel velocity corresponding to a fixed scale factor can be calculated. This ratio is used to increase or decrease the scale factor. When the scale factor needs to be decreased: the average pixel velocity > the pixel velocity corresponding to the fixed scale factor, and the ratio is greater than 1. The current scale factor is divided by the ratio to obtain the decreased scale factor. When the scale factor needs to be increased: the average value < the pixel velocity corresponding to the fixed scale factor, and the ratio is less than 1. The current scale factor is divided by the ratio to obtain the increased scale factor. In this way, the statistical characteristics of visual information itself can be utilized to achieve dynamic adjustment of the scale factor without increasing hardware complexity. This method has good adaptability to situations such as changes in channel width and shopping cart angle, and can significantly reduce the accumulation of speed errors caused by changes in viewing distance.

[0086] In one embodiment, the method further includes:

[0087] Identify the tagged area from images of the shelf area captured by ground-based mobile devices;

[0088] Determine the pixel size of the calibration area in the image plane;

[0089] The scale factor is calculated based on the ratio between the pixel size and the actual size of the calibration object.

[0090] In the above embodiments, the calibration object can be anything of known size, such as an electronic shelf label. The principle of calculating the scale factor is to use image recognition to achieve real-time calibration of the scale factor by identifying the pixel size and actual size of the calibration object on the shelf. Since the actual size of the calibration object is usually standardized in actual supermarkets (e.g., the width of an electronic shelf label is about 10 cm), after detecting the calibration object area in the image of the shelf area, its pixel size in the image plane can be measured, and the proportional relationship between the pixel size (e.g., pixel width) and the actual size (e.g., actual width) can be calculated, thereby dynamically obtaining the scale factor. Knowing the pixel size, actual size, and viewing angle parameters of the shooting device, the actual distance between the shooting device and the shelf can be deduced. Therefore, it is reasonable to calculate the scale factor using the above method. The advantage of this method is that it can be directly calculated based on physical reference quantities, avoiding the delay and error caused by relying on statistical estimation. By periodically detecting the size of the calibration object or the size of other shelf elements with known sizes, the scale factor can be continuously updated, ensuring that the accuracy of visual speed conversion remains consistent across different channel widths and positions. This vision-calibrated strategy provides a self-calibration mechanism, using the known structural features of the shelf itself as a reference to automatically adjust the visual scale. By combining it with inertial navigation data, it can maintain consistency and stability in velocity estimation when the shopping cart is close to or far from the shelf, thereby improving the accuracy of overall positioning and motion perception.

[0091] In practical applications, after determining the scale factor, the pixel velocity can be multiplied by the scale factor to obtain the calculated physical velocity.

[0092] Multiply it by the currently known or calculated scale factor (m / px) to obtain the physical velocity, which is then used as part of the observation state in the subsequent inertial navigation velocity correction process.

[0093] In the near-stationary or slow-moving state of ground mobile devices, a minimum speed threshold is introduced to ensure physical consistency in order to avoid loss of direction or instability of magnitude due to excessively low physical speed.

[0094] While pixel velocity generally exhibits an approximately linear relationship with true physical velocity, this relationship deviates from linearity in low-speed motion scenarios due to factors such as resolution limitations, feature point jitter, and imaging noise. This leads to a significant decrease in the sensitivity of pixel velocity to changes in true velocity, resulting in nonlinearity. In such cases, if a simple linear mapping is still used to convert pixel velocity to physical velocity, the velocity in the low-speed range will be systematically underestimated, potentially even incorrectly converted to near zero. However, in practical applications such as mobile robots, shopping carts, or handheld devices, the true velocity rarely approaches zero infinitely. Therefore, it is necessary to introduce a minimum velocity threshold to correct for distorted estimations in the low-speed range. This threshold avoids velocity errors caused by nonlinearity in optical flow estimation, improves the physical reliability and algorithmic stability of velocity estimation, and thus enhances the robustness of the system during slow motion.

[0095] In step 102, the inertial navigation prediction state is determined based on the inertial navigation data output by the inertial navigation system installed on the ground mobile device and the previous inertial navigation correction state.

[0096] Traditional inertial navigation systems can output acceleration, velocity, and displacement, but their output is susceptible to the following error sources: accelerometer bias error, cumulative integral drift, orientation estimation deviation, and false motion caused by misjudgment of stationary state.

[0097] The inertial navigation system's predicted state must encompass the current velocity estimate and its error sources. In an inertial navigation system, velocity is obtained by integrating acceleration; if there is a zero-bias error in acceleration, the velocity estimate will drift over time. Therefore, the predicted state at time k... It can be represented as:

[0098]

[0099] in:

[0100] Represents the predicted velocity in the x and y directions, in m / s;

[0101] This represents the prediction zero bias error of the accelerometer in the x and y directions, expressed in m / s².

[0102] The corresponding inertial navigation correction states include the corrected velocities in the x and y directions and the corrected zero bias error of the accelerometer in the x and y directions.

[0103] In this embodiment of the invention, Kalman filtering is used to determine the predicted state of the inertial navigation system.

[0104] In one embodiment, determining the inertial navigation prediction state based on the inertial navigation data output by the inertial navigation system installed on the ground mobile device and the previous inertial navigation correction state includes:

[0105] Based on the inertial navigation data, determine the current net acceleration, which is the value of the acceleration in the inertial navigation data after removing the gravity processing;

[0106] Calculate the inertial navigation predicted state based on the previous inertial navigation correction state, state transition matrix, control input matrix, and current net acceleration.

[0107] The formula corresponding to the above embodiments is:

[0108]

[0109] in, For inertial navigation system predicted state, This is the state from the last inertial navigation correction. Let F be the current net acceleration, F be the state transition matrix, and G be the control input matrix;

[0110] The state transition matrix can be represented as:

[0111]

[0112] in, This is the sampling period of the inertial navigation system.

[0113] The control input matrix can be represented as:

[0114]

[0115] In step 103, the inertial navigation observation state is constructed based on the physical speed of the ground mobile device, the inertial navigation prediction state, and the previous inertial navigation correction state.

[0116] In one embodiment, the inertial navigation observation state includes the observation velocity and the observation zero bias error;

[0117] Based on the physical speed of the ground-based mobile device, the previous inertial navigation correction state, and the inertial navigation prediction state, the inertial navigation observation state is constructed, including:

[0118] If there is a directional conflict between the physical velocity of the ground mobile device and the predicted velocity in the inertial navigation prediction state, the direction of the physical velocity of the ground mobile device will be taken as the direction of the observed velocity, and the observation zero bias error will be set to zero.

[0119] Calculate the difference between the physical velocity amplitude and the predicted velocity amplitude;

[0120] If the difference is less than the difference threshold, the amplitude of the observed velocity is updated using the average of the physical velocity amplitude and the predicted velocity amplitude;

[0121] If the difference is not within the difference threshold range, determine whether the physical velocity amplitude and the predicted velocity amplitude are within the preset physical velocity amplitude range;

[0122] If either the physical velocity amplitude or the predicted velocity amplitude falls within a preset physical velocity amplitude range, the amplitude of the observed velocity is updated using either the physical velocity amplitude or the predicted velocity amplitude that falls within the preset physical velocity amplitude range.

[0123] If neither the physical velocity amplitude nor the predicted velocity amplitude is within the preset physical velocity amplitude range, the amplitude of the observed velocity is updated using the preset velocity amplitude.

[0124] Accelerometer bias is one of the most significant sources of error in inertial navigation systems. Even when the ground mobile device has no acceleration input, the accelerometer may still output a non-zero signal, causing the velocity obtained from acceleration integration to continuously deviate from the true value. Especially in practical applications, this bias can cause a significant lag in the inertial navigation system's perception of motion trends; for example, the ground mobile device may have already begun to decelerate or change direction, while the inertial navigation system maintains its original motion judgment. Therefore, there are at least two situations where the physical velocity of the ground mobile device conflicts with the predicted velocity in the inertial navigation system's predicted state:

[0125] (1) In inertial navigation systems, velocity is usually obtained by integrating acceleration, including amplitude and direction. However, when there is a deviation in the initial direction estimation, or when the zero bias of the accelerometer accumulates over a long period, it can easily lead to deviation or even reversal of the velocity direction. In contrast, the motion direction of feature points is very sensitive to changes in velocity direction, especially in short timescales. The sign of the physical velocity (i.e., orientation) can clearly reflect the actual orientation of the ground mobile device. Even if the velocity amplitude of a physical magnitude cannot be provided, its directional reference value is still reliable. Based on this, this embodiment of the invention introduces a direction-guided pseudo-observation mechanism, which injects the direction information of the physical velocity of the ground mobile device into the inertial navigation prediction state estimation process to assist the inertial navigation system in timely correction when a deviation in direction occurs. This mechanism compares the direction of the physical velocity of the ground mobile device with the direction of the predicted velocity in the inertial navigation prediction state. The direction sign of physical velocity includes forward / backward, etc. This sign is used as a direction constraint to determine the consistency of direction with the predicted velocity in the inertial navigation prediction state. If a direction conflict is detected (e.g., the physical velocity direction is determined to be forward, but the predicted velocity direction in the inertial navigation prediction state still shows backward), the direction of the physical velocity of the ground mobile device is taken as the direction of the observed velocity. Based on the same principle, it is also possible to determine whether the directions of pixel velocity and predicted velocity conflict, since the directions of pixel velocity and physical velocity are consistent. Through continuous guidance by the direction sign of physical velocity or pixel velocity, the direction recognition capability in complex states such as short-distance backward movement can be significantly improved, avoiding path planning, control strategy, or positioning misjudgment due to direction misjudgment.

[0126] When fusing visual and inertial navigation information, it is necessary not only to determine the velocity direction but also the velocity amplitude. This is because the inertial navigation velocity may continuously increase due to zero-bias error after long-term integration, and the pixel velocity used to generate the physical velocity may be underestimated when the number of feature points decreases or the camera distance changes. If the amplitude of the original predicted velocity is used as the amplitude of the observed velocity, it will lead to large fluctuations in the observed velocity.

[0127] To improve the stability of the fusion results, this embodiment of the invention further introduces a "velocity difference determination and confidence interval screening mechanism" based on direction correction. In the current frame, the difference between the physical velocity amplitude and the predicted velocity amplitude is first calculated. If the difference is less than the difference threshold, it indicates that the two are highly consistent, and the two estimates can be considered similar. The average value of the two is used to update the amplitude of the observed velocity, thus taking into account the advantages of both vision and inertial navigation. If the difference is not less than the difference threshold, it indicates that there is a significant inconsistency, and the confidence of each needs to be further determined. This embodiment of the invention proposes a preset physical velocity amplitude range (for example, the typical velocity range of a shopping cart in a passage is 0.5 m / s–1.0 m / s, which will be used as the preset physical velocity amplitude range) and uses it as a reference benchmark. If one of the physical velocity amplitude and the predicted velocity amplitude falls within this range, while the other exceeds it, the one within the preset physical velocity amplitude range is selected as the reliable result. If both exceed the preset physical velocity amplitude range, it indicates an anomaly in the current observation, such as excessive inertial navigation drift or visual velocity estimation distortion. In this case, the observed velocity amplitude is updated using a preset velocity amplitude (e.g., 0.7 m / s) until the new observation returns to normal. Through this dual screening mechanism based on difference and reliability range, the numerical stability and physical rationality of the observed velocity amplitude can be maintained while the predicted velocity direction is adjusted in the opposite direction. This strategy is particularly effective in dynamic environments, significantly reducing velocity abrupt changes and improving the continuity and overall robustness of velocity fusion results.

[0128] (2) In typical usage scenarios of ground mobile devices such as shopping carts and logistics vehicles, turning operations are frequent and unavoidable. Especially in narrow passages such as supermarkets and warehouses, ground mobile devices often need to make slow turns, U-turns, or fine-tuning of their paths. In these scenarios, relying solely on the velocity direction calculated by acceleration integral can easily lead to directional drift due to attitude estimation errors or zero acceleration bias, resulting in incorrect motion path judgment. Although inertial navigation systems are usually equipped with gyroscopes to sense angular velocity and thus detect rotation, their response to directional changes may be delayed or even misjudged in low-speed or gently changing angular velocity situations. This lag problem will be further amplified, especially when the inertial navigation system has already experienced slight drift or error accumulation. Therefore, this embodiment of the invention proposes to use the motion trend of feature points extracted from the image to help identify whether the velocity direction in the inertial navigation system's predicted state has deviated from the true direction when the ground mobile device is rotating, and guide it to automatically correct itself when necessary. In specific implementation, the gyroscope can be used to determine whether the current state is rotating. When the angular velocity of any axis exceeds the set threshold (e.g., 0.3 rad / s), it can be assumed that the ground mobile device is turning. When the turning is determined, after the above-mentioned direction and amplitude updates of the observed velocity are performed, a zero-bias update operation will be performed, and the observation zero-bias error will be set to zero to correct the velocity judgment error caused by the delay.

[0129] In one embodiment, the method further includes:

[0130] If the rate of change of the physical velocity of the ground mobile device conflicts with the current net acceleration in direction, the observation zero bias error is corrected according to the zero bias correction value. The current net acceleration is the value of acceleration in the inertial navigation data after removing gravity processing.

[0131] If the rate of change of the physical velocity of a ground-based mobile device conflicts in direction with the current net acceleration, a typical scenario is when the mobile device switches from acceleration to deceleration, or vice versa. In this case, the observed rate of change of physical velocity (i.e., physical acceleration) will flip. Because of the presence of zero bias in the inertial navigation system, it may continue to output incorrect acceleration after the trend reverses. By monitoring the trend of physical velocity changes in consecutive frames of images and combining this with the direction of the net acceleration obtained from the current acceleration output by the inertial navigation system, an inconsistency can be determined. Once a directional conflict is detected, a correction to the zero-bias estimation of acceleration is triggered. Specifically, a zero-bias correction value is introduced to ensure that there is no over-adjustment due to short-term image fluctuations. Furthermore, the correction is only executed when a clear contradiction is detected between the rate of change of physical velocity in the image and the current net acceleration, ensuring that it intervenes only when necessary. The step of correcting the observation zero-bias error based on the zero-bias correction value is to use the product of the zero-bias correction value and the observation zero-bias error as the new observation zero-bias error. Specifically, the sign of the zero-bias correction value can be determined by detecting the rate of change of physical velocity. When the rate of change of physical velocity is positive, the zero-bias correction value is also positive; when the rate of change of physical velocity is negative, the zero-bias correction value is also negative. The specific value of the zero-bias correction value can be preset based on empirical values, such as 0.5. Based on the same principle, the sign of the zero-bias correction value can also be determined by detecting the rate of change of pixel velocity in the image. When the rate of change of pixel velocity is positive, the zero-bias correction value is also positive; when the rate of change of pixel velocity is negative, the zero-bias correction value is also negative.

[0132] In one embodiment, the method further includes:

[0133] If more than a preset proportion of feature points in the current frame image meet the following conditions, the ground mobile device is determined to be stationary, and the observation speed is set to zero. These feature points are selected from the target shooting area:

[0134] The magnitude of the pixel velocity of the feature point is lower than the preset static threshold.

[0135] In this embodiment of the invention, the modulus is defined as the positive square root of the sum of the squares of the components along the X and Y axes, and the pixel velocity of a single feature point is ( , The modulus is the magnitude of the pixel velocity, which is its length. In a two-dimensional plane, it is calculated using the Euclidean distance formula: Modulus = |V| = sqrt( 2 + 2 ).

[0136] Taking the above condition as condition 1, this embodiment of the invention also proposes the following two conditions:

[0137] Condition 2: The orientation of the feature point remains unchanged across multiple consecutive frames;

[0138] Condition 3: The increase in the pixel velocity of the feature point is less than the amplitude threshold.

[0139] Determining that the ground mobile device is stationary can satisfy any one of the above three conditions. To improve the accuracy of the judgment, it can also satisfy two or all of the above three conditions.

[0140] If a feature point satisfies the above conditions, then it is determined that the feature point maintains stable orientation and velocity amplitude within multiple consecutive frames.

[0141] In inertial navigation systems, even when the ground-based mobile device is completely stationary, the accelerometer may still output a weak but persistent error signal due to zero bias. This error, after integration, manifests as a non-zero velocity estimate. Over time, this spurious velocity accumulates, causing significant drift in the inertial navigation system. To address this issue, this invention employs a stationary state determination mechanism based on fused images. It actively identifies the stationary state of the inertial navigation system and sets the observed velocity in the inertial navigation observation state to zero (forcibly converging to zero), thereby effectively suppressing the accumulation of spurious velocity. The preset ratio can be two-thirds, and the preset stationary threshold can be 0.2 px / s, which can be determined according to actual conditions.

[0142] Completed inertial navigation observation state It can be represented as:

[0143]

[0144] in, For observation speed, To observe the zero bias error.

[0145] In step 104, the current inertial navigation correction state is determined based on the inertial navigation observation state and the inertial navigation prediction state, and the correction velocity in the current inertial navigation correction state is taken as the current inertial navigation velocity.

[0146] In one embodiment, determining the current inertial navigation correction state based on the inertial navigation observation state and the inertial navigation prediction state includes:

[0147] The prediction covariance matrix is ​​determined based on the previous optimal covariance matrix, state transition matrix, and process noise matrix.

[0148] Based on the inertial navigation observation state, the inertial navigation prediction state, and the prediction covariance matrix, Kalman filtering is performed to update and obtain the current inertial navigation correction state.

[0149] Predicting covariance matrix The calculation formula is as follows:

[0150]

[0151] in, The process noise matrix can be represented as:

[0152]

[0153] in, These are process noise parameters;

[0154] This is the previous optimal covariance matrix. To predict the covariance matrix.

[0155] In this embodiment of the invention, the Kalman filter update formula includes:

[0156]

[0157]

[0158]

[0159]

[0160]

[0161] in, The residual represents the predicted state of the inertial navigation system. The difference between the observed state and the inertial navigation system. The optimal covariance matrix for this operation is given by the acceleration output by the inertial navigation system. , Let be the residual covariance, representing the uncertainty of the residuals. For Kalman gain, This is the current inertial navigation correction state;

[0162] H is the observation matrix, which can be represented as:

[0163] ;

[0164] The observation noise matrix can be represented as:

[0165]

[0166] in, and To observe noise parameters;

[0167] Optimal covariance matrix initialization:

[0168]

[0169] in, and is the covariance parameter.

[0170] After using the corrected velocity in the current inertial navigation correction state as the current inertial navigation velocity, the next correction begins. The current inertial navigation correction state at this point is used as the "previous inertial navigation correction state" in the next correction.

[0171] In one embodiment, the method further includes:

[0172] Calculate the average pixel velocity of all feature points in the current frame image based on the pixel velocity of all feature points, where the feature points are selected from the target shooting area;

[0173] Based on the average value, calculate the variance of the pixel velocity for each feature point;

[0174] The average of the variances of the pixel velocities of all feature points is used as the discreteness of the current frame image.

[0175] The observation noise parameters of the observation noise matrix in the Kalman filter update are determined based on the aforementioned discreteness.

[0176] In practical applications, although the average pixel velocity of feature points can roughly reflect the overall motion trend of ground-based mobile devices, some abnormal feature points inevitably exist in the image (such as points affected by occlusion, noise, blurring, or mismatch). These feature points often deviate from the main direction or even oppose the main trend. If they are directly included in velocity calculation without screening, it can easily lead to observational distortion. To improve the robustness of velocity calculation, a dispersion index is introduced to measure the consistency between the velocity of each feature point in the current frame and the overall motion trend. This dispersion index is calculated based on the variance of the feature point velocity vector, reflecting whether the velocity field is concentrated and whether the direction is uniform.

[0177] During operation, the deviation of each feature point in each frame from the average pixel velocity of all feature points is calculated. If the overall deviation is small, it indicates that the optical flow direction is concentrated and the calculation is reliable; if the deviation is large, it may indicate that there is dynamic interference, sudden background changes, or unstable tracking in the image. This dispersion is used to dynamically adjust the confidence of the inertial navigation observation state in the fusion process. The adjustment method is to use the dispersion as the observation noise parameter of the observation noise matrix in the Kalman filter update. Specifically, the smaller the dispersion, the higher the observation consistency and the larger the observation noise parameter; the larger the dispersion, the more chaotic the image state and the smaller the observation noise parameter. This actively reduces the influence of the current frame on the fusion result of the Kalman filter and avoids amplifying errors from erroneous information.

[0178] This invention also proposes an inertial navigation speed correction device for a shopping mall ground mobile device, the principle of which is similar to the inertial navigation speed correction method for shopping mall ground mobile devices, and will not be described in detail here.

[0179] Figure 3 This is a schematic diagram of the inertial navigation speed correction device for a commercial and supermarket ground mobile device in an embodiment of the present invention, including:

[0180] The physical velocity determination module 301 is used to determine the pixel velocity of the ground mobile device based on the previous frame image and the current frame image captured by the ground mobile device, and convert the pixel velocity into physical velocity.

[0181] The inertial navigation prediction state determination module 302 is used to determine the inertial navigation prediction state based on the inertial navigation data output by the inertial navigation system installed on the ground mobile device and the previous inertial navigation correction state.

[0182] The inertial navigation observation state construction module 303 is used to construct the inertial navigation observation state based on the physical speed of the ground mobile device, the previous inertial navigation correction state, and the inertial navigation prediction state.

[0183] The inertial navigation correction module 304 is used to determine the current inertial navigation correction state based on the inertial navigation observation state and the inertial navigation prediction state, and to use the correction velocity in the current inertial navigation correction state as the current inertial navigation velocity.

[0184] In one embodiment, the physical velocity determination module is used to:

[0185] The shelf area that can be captured by the ground mobile device is taken as the target shooting area, and feature points are selected in the target shooting area;

[0186] In the target shooting area, the optical flow component from each feature point of the previous frame image to the corresponding feature point of the current frame image is calculated, and the pixel velocity of each feature point is calculated based on the optical flow component.

[0187] The pixel velocity of the ground mobile device is calculated based on the pixel velocity of all feature points in the current frame image, and the pixel velocity is converted into physical velocity.

[0188] In one embodiment, the physical velocity determination module is used to:

[0189] Feature points are detected in the target shooting area by using preset conditions to obtain the detected feature points;

[0190] Feature points are selected based on the number and distribution requirements of feature points configured in the configuration file.

[0191] In one embodiment, the physical velocity determination module is used to:

[0192] Remove selected feature points from the current frame image whose tracking state does not meet the preset tracking state requirements;

[0193] Among the detected feature points, feature points that meet the preset tracking state requirements are reselected to complete the configured number of feature points and obtain updated feature points.

[0194] Calculate the optical flow component from each updated feature point in the previous frame to the corresponding updated feature point in the current frame.

[0195] In one embodiment, the physical velocity determination module is used to:

[0196] Calculate the physical velocity of the ground mobile device based on pixel velocity and scale factor;

[0197] When the physical speed is less than the minimum speed threshold, the physical speed is assigned the minimum speed threshold.

[0198] In one embodiment, the physical velocity determination module is used to:

[0199] After recording the pixel velocity sequence within the first preset time period, the velocity deviation between the average value of all pixel velocities in the pixel velocity sequence and the preset standard velocity value is calculated.

[0200] If the average value is greater than the preset standard speed value and the speed deviation is greater than the speed deviation threshold, the scaling factor is reduced.

[0201] If the average value is less than the preset standard speed value and the speed deviation is greater than the speed deviation threshold, the scaling factor is increased.

[0202] In one embodiment, the physical velocity determination module is used to:

[0203] Identify the tagged area from images of the shelf area captured by ground-based mobile devices;

[0204] Determine the pixel size of the calibration area in the image plane;

[0205] The scale factor is calculated based on the ratio between the pixel size and the actual size of the calibration object.

[0206] In one embodiment, the inertial navigation prediction state determination module is used for:

[0207] Based on the inertial navigation data, determine the current net acceleration, which is the value of the acceleration in the inertial navigation data after removing the gravity processing;

[0208] Calculate the inertial navigation predicted state based on the previous inertial navigation correction state, state transition matrix, control input matrix, and current net acceleration.

[0209] In one embodiment, the inertial navigation observation state includes the observation velocity and the observation zero bias error;

[0210] The inertial navigation observation state construction module is used for:

[0211] The physical speed of the ground mobile device is used as the observation speed, and the correction zero bias error in the previous inertial navigation correction state is used as the observation zero bias error.

[0212] If there is a directional conflict between the physical velocity of the ground mobile device and the predicted velocity in the inertial navigation prediction state, the direction of the physical velocity of the ground mobile device will be taken as the direction of the observed velocity, and the observation zero bias error will be set to zero.

[0213] Calculate the difference between the physical velocity amplitude and the predicted velocity amplitude;

[0214] If the difference is less than the difference threshold, the amplitude of the observed velocity is updated using the average of the physical velocity amplitude and the predicted velocity amplitude;

[0215] If the difference is not within the difference threshold range, determine whether the physical velocity amplitude and the predicted velocity amplitude are within the preset physical velocity amplitude range;

[0216] If either the physical velocity amplitude or the predicted velocity amplitude falls within a preset physical velocity amplitude range, the amplitude of the observed velocity is updated using either the physical velocity amplitude or the predicted velocity amplitude that falls within the preset physical velocity amplitude range.

[0217] If neither the physical velocity amplitude nor the predicted velocity amplitude is within the preset physical velocity amplitude range, the amplitude of the observed velocity is updated using the preset velocity amplitude.

[0218] In one embodiment, the inertial navigation prediction state determination module is used for:

[0219] If more than a preset proportion of feature points in the current frame image meet the following conditions, the ground mobile device is determined to be stationary, and the observation speed is set to zero. These feature points are selected from the target shooting area:

[0220] The magnitude of the pixel velocity of the feature point is lower than the preset static threshold.

[0221] In one embodiment, the inertial navigation prediction state determination module is used for:

[0222] If the rate of change of the physical velocity of the ground mobile device conflicts with the current net acceleration in direction, the observation zero bias error is corrected according to the zero bias correction value. The current net acceleration is the value of acceleration in the inertial navigation data after removing gravity processing.

[0223] In one embodiment, the inertial navigation correction module is used for:

[0224] The prediction covariance matrix is ​​determined based on the previous optimal covariance matrix, state transition matrix, and process noise matrix.

[0225] Based on the inertial navigation observation state, the inertial navigation prediction state, and the prediction covariance matrix, Kalman filtering is performed to update and obtain the current inertial navigation correction state.

[0226] In one embodiment, the inertial navigation correction module is used for:

[0227] Calculate the average pixel velocity of all feature points in the current frame image based on the pixel velocity of all feature points, where the feature points are selected from the target shooting area;

[0228] Based on the average value, calculate the variance of the pixel velocity for each feature point;

[0229] The average of the variances of the pixel velocities of all feature points is used as the discreteness of the current frame image.

[0230] The observation noise parameters of the observation noise matrix in the Kalman filter update are determined based on the aforementioned discreteness.

[0231] Figure 4 This is a schematic diagram of the structure of the inertial navigation speed correction system for a supermarket ground mobile device in an embodiment of the present invention, including: an inertial navigation speed correction device 401, an inertial navigation system 402, and a camera 403 for the supermarket ground mobile device;

[0232] The inertial navigation system is used to: calculate inertial navigation data;

[0233] The camera is used to: acquire images within the target shooting area.

[0234] The aforementioned inertial navigation system includes a low-power three-axis accelerometer and a three-axis gyroscope combination module; the camera type can be a low-power wide-angle industrial camera, installed on the front left and right sides of the vehicle body, facing the shopping aisle shelves.

[0235] The proposed solution is applicable to various ground mobile devices, especially those operating in environments with complex indoor structures, limited space, and no GNSS signals, such as supermarket shopping carts and warehouse picking carts. In these scenarios, ground mobile devices often face challenges such as frequent starts and stops, direction changes, obstruction interference, and low-speed movement. Traditional inertial navigation systems have difficulty estimating speed and direction stably over a long period of time.

[0236] For example, the system could be deployed in a large retail supermarket where shopping carts need to navigate through multiple aisles. The space is compact, shelves are high and densely packed, and people and other shopping carts pass by frequently, making it impossible to use high-cost positioning technologies such as GPS and UWB, and also lacking the conditions for deploying additional base stations or landmarks. Shopping carts in actual use exhibit the following movement characteristics:

[0237] They move slowly (0.3–1.5 m / s) and often remain stationary for short periods.

[0238] Frequently change direction when encountering the end of a passage, a fork in the road, or when a U-turn is required;

[0239] There is a need to switch between forward and backward directions, and there are occasional rapid deceleration / acceleration;

[0240] Users may linger in front of the shelves for extended periods, and the system still needs to maintain static recognition and stability.

[0241] By deploying the system proposed in the embodiments of the invention, user needs can be met. In practical applications, the camera and the inertial navigation system collect data synchronously. The system detects feature points and performs calculations in real time. It can also trigger a calculation once per second based on the image. Based on the calculation results, the inertial navigation observation state is generated and the Kalman filter state is updated. In this way, the fused inertial navigation speed can be output every second.

[0242] In summary, the methods, apparatus, and systems proposed in the embodiments of the present invention can achieve the following beneficial effects:

[0243] I. Suppressing inertial navigation velocity drift and improving the long-term stability of velocity estimation

[0244] Pixel velocities are extracted from the previous and current frames of images captured by ground-based mobile devices and converted into physical velocities. This provides an external visual reference for the inertial navigation system. Compared to traditional schemes that rely solely on inertial navigation acceleration integration, this physical velocity can reflect the actual motion trend of the device in real time, avoiding the amplification of zero-bias errors over time. The constructed inertial navigation observation state integrates physical velocity, the previous inertial navigation correction state, and the inertial navigation prediction state. It can specifically identify deviations in the inertial navigation prediction state and suppress drift from the source of error by updating the current inertial navigation correction state, ensuring that the velocity estimation of the device always closely matches the true value during long-term operation.

[0245] II. Enhancing the ability to identify speed and direction in complex motion scenarios

[0246] This solution is highly adaptable to complex scenarios commonly encountered by ground-based mobile devices, such as low-speed turning, short-distance reversing, and frequent start-stop operations.

[0247] Pixel velocity extraction relies on feature point tracking within the target shooting area. This area has rich and stable features. Even if the device turns at low speed, it can still accurately capture the speed direction change by the change in the movement direction of the feature points, avoiding the direction misjudgment caused by the lag in the angular velocity response of the inertial navigation system.

[0248] The inertial navigation observation status is simultaneously associated with physical velocity (quantized amplitude) and physical velocity (reflecting direction). When the device reverses a short distance (such as a shopping cart reversing to avoid a pedestrian), the real-time feedback of the physical velocity direction sign can quickly correct any directional deviations that may occur in the inertial navigation, ensuring that the velocity direction is consistent with the actual movement of the device and preventing path planning and control strategies from failing due to directional misjudgment.

[0249] Third, no additional hardware costs are required, and it is compatible with existing device architectures.

[0250] This solution makes full use of the existing hardware resources of ground-based mobile devices, and has the advantage of low-cost deployment:

[0251] Pixel speed extraction relies on the camera already equipped on the device, eliminating the need for additional high-precision sensors such as LiDAR and UWB, thus reducing hardware deployment costs;

[0252] The construction of the inertial navigation prediction state only requires the inertial navigation data output by the existing inertial navigation system and the previous correction state. No hardware modification of the inertial navigation system is required. It can be directly adapted to mainstream low-cost IMUs (inertial measurement units), has strong compatibility, and is easy to promote quickly in existing equipment in scenarios such as supermarkets and warehouses.

[0253] IV. Optimize state fusion logic to improve system robustness

[0254] By constructing a hierarchical prediction-observation-correction closed loop, the system's resilience to disturbances from complex environments is significantly improved.

[0255] The inertial navigation predicted state is calculated based on the previous inertial navigation correction state (the optimal state that has been fused with historical visual observations) and inertial navigation data. Compared with directly using the original inertial navigation output, it offsets some of the zero bias error in advance, providing a more reliable benchmark for subsequent corrections.

[0256] Introducing physical velocity into the inertial navigation observation state allows for dynamic assessment of the reliability of visual observations, ensuring that the fusion results remain stable and reliable even under environmental interference.

[0257] 5. Achieve precise switching between dynamic and static states, avoiding the accumulation of false motion.

[0258] To address the problem of inertial navigation systems outputting false velocities due to zero bias even when stationary, this solution achieves accurate identification and correction of stationary states through visual observation:

[0259] Pixel velocity can directly reflect the stationary state of the device (e.g., the pixel velocity magnitude of most feature points is below the threshold). Combined with the characteristic that physical velocity approaches zero, the constructed inertial navigation observation state can accurately determine whether the device is stationary.

[0260] When the device is determined to be stationary, the correction velocity can be forced to converge to zero by updating the current inertial navigation correction state, completely eliminating the accumulation of false velocity caused by the zero bias of the inertial navigation. This ensures that the velocity estimation is drift-free when the device is restarted after a long period of inactivity (such as a shopping cart parked in front of a shelf), thus improving the reliability of positioning and control.

[0261] This invention also provides a computer device. Figure 5 This is a schematic diagram of a computer device in an embodiment of the present invention. The computer device 500 includes a memory 510, a processor 520, and a computer program 530 stored in the memory 510 and executable on the processor 520. When the processor 520 executes the computer program 530, it implements the above-mentioned inertial navigation speed correction method for supermarket ground mobile equipment.

[0262] This invention also provides a computer-readable storage medium storing a computer program that, when executed by a processor, implements the aforementioned inertial navigation speed correction method for commercial and supermarket ground mobile devices.

[0263] This invention also provides a computer program product, which includes a computer program that, when executed by a processor, implements the above-mentioned inertial navigation speed correction method for commercial and supermarket ground mobile devices.

[0264] Those skilled in the art will understand that embodiments of the present invention can be provided as methods, systems, or computer program products. Therefore, the present invention can take the form of a completely hardware embodiment, a completely software embodiment, or an embodiment combining software and hardware aspects. Furthermore, the present invention can take the form of a computer program product embodied on one or more computer-usable storage media (including, but not limited to, disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0265] This invention is described with reference to flowchart illustrations and / or block diagrams of methods, apparatus (systems), and computer program products according to embodiments of the invention. It will be understood that each block of the flowchart illustrations and / or block diagrams, and combinations of blocks in the flowchart illustrations and / or block diagrams, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, special-purpose computer, embedded processor, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing apparatus, generate instructions for implementing the flowchart illustrations and / or block diagrams. Figure 1 One or more processes and / or boxes Figure 1 A device that provides the functions specified in one or more boxes.

[0266] These computer program instructions may also be stored in a computer-readable storage medium that can direct a computer or other programmable data processing device to function in a particular manner, such that the instructions stored in the computer-readable storage medium produce an article of manufacture including instruction means, which are implemented in a process Figure 1 One or more processes and / or boxes Figure 1 The function specified in one or more boxes.

[0267] These computer program instructions may also be loaded onto a computer or other programmable data processing equipment to cause a series of operational steps to be performed on the computer or other programmable equipment to produce a computer-implemented process, thereby providing instructions that execute on the computer or other programmable equipment for implementing the process. Figure 1 One or more processes and / or boxes Figure 1 The steps of the function specified in one or more boxes.

[0268] The specific embodiments described above further illustrate the purpose, technical solution, and beneficial effects of the present invention. It should be understood that the above descriptions are merely specific embodiments of the present invention and are not intended to limit the scope of protection of the present invention. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the scope of protection of the present invention.

Claims

1. A method for correcting the inertial navigation speed of a ground-based mobile device in a shopping mall, characterized in that, include: Based on the previous frame image and the current frame image captured by the ground mobile device, the pixel velocity of the ground mobile device is determined, and the pixel velocity is converted into physical velocity. The inertial navigation prediction state is determined based on the inertial navigation data output by the inertial navigation system installed on the ground mobile device and the previous inertial navigation correction state. Based on the physical speed of the ground mobile device, the previous inertial navigation correction state, and the inertial navigation prediction state, the inertial navigation observation state is constructed. Based on the inertial navigation observation state and the inertial navigation prediction state, the current inertial navigation correction state is determined, and the correction velocity in the current inertial navigation correction state is taken as the current inertial navigation velocity.

2. The method as described in claim 1, characterized in that, Based on the previous frame and the current frame image captured by the ground mobile device, determine the pixel velocity of the ground mobile device, including: The shelf area that can be captured by the ground mobile device is taken as the target shooting area, and feature points are selected in the target shooting area; In the target shooting area, the optical flow component from each feature point of the previous frame image to the corresponding feature point of the current frame image is calculated, and the pixel velocity of each feature point is calculated based on the optical flow component. Calculate the pixel velocity of the ground mobile device based on the pixel velocity of all feature points in the current frame image.

3. The method as described in claim 2, characterized in that, Feature point selection for the target imaging area includes: Feature points are detected in the target shooting area by using preset conditions to obtain the detected feature points; Feature points are selected based on the number and distribution requirements of feature points configured in the configuration file.

4. The method as described in claim 3, characterized in that, Before calculating the optical flow component from each feature point in the previous frame to the corresponding feature point in the current frame, the method further includes: Remove selected feature points from the current frame image whose tracking state does not meet the preset tracking state requirements; Among the detected feature points, feature points that meet the preset tracking state requirements are reselected to complete the configured number of feature points and obtain updated feature points. Calculate the optical flow component from each feature point in the previous frame to the corresponding feature point in the current frame, including: Calculate the optical flow component from each updated feature point in the previous frame to the corresponding updated feature point in the current frame.

5. The method as described in claim 1, characterized in that, Converting the pixel velocity to physical velocity includes: Calculate the physical velocity of the ground mobile device based on pixel velocity and scale factor; When the physical speed is less than the minimum speed threshold, the physical speed is assigned the minimum speed threshold.

6. The method as described in claim 5, characterized in that, Also includes: After recording the pixel velocity sequence within the first preset time period, the velocity deviation between the average value of all pixel velocities in the pixel velocity sequence and the preset standard velocity value is calculated. If the average value is greater than the preset standard speed value and the speed deviation is greater than the speed deviation threshold, the scaling factor is reduced. If the average value is less than the preset standard speed value and the speed deviation is greater than the speed deviation threshold, the scaling factor is increased.

7. The method as described in claim 5, characterized in that, Also includes: Identify the tagged area from images of the shelf area captured by ground-based mobile devices; Determine the pixel size of the calibration area in the image plane; The scale factor is calculated based on the ratio between the pixel size and the actual size of the calibration object.

8. The method as described in claim 1, characterized in that, Based on the inertial navigation data output by the inertial navigation system installed on the ground mobile device and the previous inertial navigation correction state, the inertial navigation prediction state is determined, including: Based on the inertial navigation data, determine the current net acceleration, which is the value of the acceleration in the inertial navigation data after removing the gravity processing; Calculate the inertial navigation predicted state based on the previous inertial navigation correction state, state transition matrix, control input matrix, and current net acceleration.

9. The method as described in claim 1, characterized in that, Inertial navigation observation status includes observation velocity and observation zero bias error; Based on the physical speed of the ground-based mobile device, the previous inertial navigation correction state, and the inertial navigation prediction state, the inertial navigation observation state is constructed, including: The physical speed of the ground mobile device is used as the observation speed, and the correction zero bias error in the previous inertial navigation correction state is used as the observation zero bias error. If there is a directional conflict between the physical velocity of the ground mobile device and the predicted velocity in the inertial navigation prediction state, the direction of the physical velocity of the ground mobile device will be taken as the direction of the observed velocity, and the observation zero bias error will be set to zero. Calculate the difference between the physical velocity amplitude and the predicted velocity amplitude; If the difference is less than the difference threshold, the amplitude of the observed velocity is updated using the average of the physical velocity amplitude and the predicted velocity amplitude; If the difference is not within the difference threshold range, determine whether the physical velocity amplitude and the predicted velocity amplitude are within the preset physical velocity amplitude range; If either the physical velocity amplitude or the predicted velocity amplitude falls within a preset physical velocity amplitude range, the amplitude of the observed velocity is updated using either the physical velocity amplitude or the predicted velocity amplitude that falls within the preset physical velocity amplitude range. If neither the physical velocity amplitude nor the predicted velocity amplitude is within the preset physical velocity amplitude range, the amplitude of the observed velocity is updated using the preset velocity amplitude.

10. The method as described in claim 9, characterized in that, Also includes: If more than a preset proportion of feature points in the current frame image meet the following conditions, the ground mobile device is determined to be stationary, and the observation speed is set to zero. These feature points are selected from the target shooting area: The magnitude of the pixel velocity of the feature point is lower than the preset static threshold.

11. The method as described in claim 9, characterized in that, Also includes: If the rate of change of the physical velocity of the ground mobile device conflicts with the current net acceleration in direction, the observation zero bias error is corrected according to the zero bias correction value. The current net acceleration is the value of acceleration in the inertial navigation data after removing gravity processing.

12. The method as described in claim 1, characterized in that, Based on the inertial navigation observation state and the inertial navigation prediction state, determine the current inertial navigation correction state, including: The prediction covariance matrix is ​​determined based on the previous optimal covariance matrix, state transition matrix, and process noise matrix. Based on the inertial navigation observation state, the inertial navigation prediction state, and the prediction covariance matrix, Kalman filtering is performed to update and obtain the current inertial navigation correction state.

13. The method as described in claim 12, characterized in that, Also includes: Calculate the average pixel velocity of all feature points in the current frame image based on the pixel velocity of all feature points, where the feature points are selected from the target shooting area; Based on the average value, calculate the variance of the pixel velocity for each feature point; The average of the variances of the pixel velocities of all feature points is used as the discreteness of the current frame image. The observation noise parameters of the observation noise matrix in the Kalman filter update are determined based on the aforementioned discreteness.

14. An inertial navigation speed correction device for a commercial and supermarket ground mobile device, characterized in that, include: The physical velocity determination module is used to determine the pixel velocity of the ground mobile device based on the previous frame image and the current frame image captured by the ground mobile device, and convert the pixel velocity into physical velocity. The inertial navigation prediction state determination module is used to determine the inertial navigation prediction state based on the inertial navigation data output by the inertial navigation system installed on the ground mobile device and the previous inertial navigation correction state. The inertial navigation observation state construction module is used to construct the inertial navigation observation state based on the physical speed of the ground mobile device, the previous inertial navigation correction state, and the inertial navigation prediction state. The inertial navigation correction module is used to determine the current inertial navigation correction state based on the inertial navigation observation state and the inertial navigation prediction state, and to use the correction velocity in the current inertial navigation correction state as the current inertial navigation velocity.

15. An inertial navigation speed correction system for a commercial and supermarket ground mobile device, characterized in that, include: The inertial navigation speed correction device, inertial navigation system, and camera for a supermarket ground mobile device as described in claim 14; The inertial navigation system is used to: calculate inertial navigation data; The camera is used to: acquire images within the target shooting area.

16. A computer device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that, When the processor executes the computer program, it implements the method according to any one of claims 1 to 13.

17. A computer-readable storage medium, characterized in that, The computer-readable storage medium stores a computer program that, when executed by a processor, implements the method according to any one of claims 1 to 13.

18. A computer program product, characterized in that, The computer program product includes a computer program that, when executed by a processor, implements the method according to any one of claims 1 to 13.

Images