A control method and system for a mobile odometry

Abstract

The application relates to a mobile odometer control method and system. The method determines a current working mode based on a preset sensor configuration and a real-time running working condition, collects sensing data of each sensor in an adaptive target sensor combination according to the current working mode, wherein the target sensor combination comprises an inertial measurement sensor and at least one displacement sensor; the sensing data is used to obtain motion state prediction information and displacement observation information of a carrier; the measurement confidence of each displacement sensor is obtained, the motion state prediction information and the displacement observation information are adaptively fused, and state estimation results of the carrier are obtained; position, posture and motion information of the carrier are output based on the state estimation results, and sensor state diagnosis information is output, so that adaptive control of the mobile odometer is realized. Therefore, the hardware implementation cost is reduced, the robustness is enhanced, the environmental adaptability is improved, and non-intervention adaptive control is realized.

Description

Technical Field

[0001] This application is primarily in the field of mobile robot technology, and in particular relates to a control method and system for a mobile odometer. Background Technology

[0002] With the rapid development of smart home technology, intelligent devices with autonomous mobility are gradually entering ordinary households. As a typical household mobile platform, the smart mobile trash can needs accurate location estimation and stable motion control capabilities to achieve functions such as summonable navigation and automatic return. Odometry, as the most basic positioning method for mobile robots, directly affects the device's navigation performance and user experience in terms of accuracy and stability. However, existing odometry technologies face many challenges when applied to low-cost household devices, making it difficult to achieve a good balance between cost, accuracy, and stability.

[0003] Currently, mobile robot odometry solutions on the market mainly adopt the following technical approaches. The first approach uses pure wheel encoder odometry, calculating the distance traveled by measuring the number of wheel rotations. This approach is low-cost and simple to implement, but suffers from cumulative errors due to wheel slippage and uneven ground surfaces. Positioning accuracy drops significantly over long distances, with measured errors reaching 5% to 10% of the traveled distance. The second approach uses visual odometry, acquiring images through cameras and extracting feature points for motion estimation. This approach offers high accuracy, but its computational complexity is high, requiring high-performance processors and resulting in high costs, making it unsuitable for low-cost home devices. The third approach uses LiDAR-SLAM, constructing an environmental map and achieving localization through laser scanning. This approach offers the highest positioning accuracy, but LiDAR typically costs hundreds to thousands of yuan, far exceeding the budget of home devices. The fourth approach fuses IMU and wheel encoders, integrating multi-source data through algorithms such as Kalman filtering. This approach improves positioning accuracy to some extent, but current implementations are mostly based on the ROS framework and high-performance processors, resulting in high algorithm complexity and difficulty in running on low-cost chips like the ESP32.

[0004] The existing technology suffers from the following major problems. First, the hardware cost is too high. Existing high-precision odometer solutions generally rely on LiDAR, depth cameras, or high-performance processors, with the total cost reaching hundreds to thousands of yuan, far exceeding the price range of 50 to 200 yuan that ordinary households can afford for devices such as smart trash cans, making large-scale adoption difficult. Second, the algorithm complexity is high. Existing multi-sensor fusion algorithms are mostly based on extended Kalman filtering or graph optimization methods, which involve large computational loads and require processors with strong floating-point computing power. The 240MHz main frequency and 520KB SRAM of low-cost chips such as the ESP32 are insufficient to run the complete SLAM algorithm in real time, limiting the application of the algorithm in home devices. Third, the environmental adaptability is poor. Single-sensor solutions are prone to failure in specific environments. For example, wheel encoders slip on smooth surfaces, optical flow sensors cannot work on solid-color surfaces or glass surfaces, and IMUs experience severe drift after long-term operation. Existing solutions lack effective sensor adaptive switching mechanisms to cope with complex and changing home environments. Fourth, there is a lack of optimization for low-cost platforms. Existing odometer calculation methods are mostly designed for industrial or service robots and do not take into account the strict limitations of cost, power consumption and size for home devices. When directly ported to platforms such as ESP32, they face problems such as insufficient memory and calculation timeouts. Summary of the Invention

[0005] One objective of this application is to provide a control method and system for a mobile odometer, in order to solve the problems of high hardware cost, high algorithm complexity, poor environmental adaptability, and lack of low-cost platform optimization in the prior art.

[0006] According to one aspect of this application, a control method for a mobile odometer is provided, the method comprising: The current working mode is determined based on the preset sensor configuration and real-time operating conditions. The sensing data of each sensor in the target sensor combination is collected according to the current working mode. The target sensor combination includes an inertial measurement sensor and at least one displacement sensor. Based on the sensing data of each sensor in the target sensor combination, the motion state prediction information and displacement observation information of the carrier are obtained respectively, wherein the displacement observation information includes wheel displacement observation information and visual displacement observation information. The measurement confidence scores of each displacement sensor in the target sensor combination are obtained, and the motion state prediction information and the displacement observation information are adaptively fused to obtain the state estimation result of the carrier. Based on the state estimation results, the position, attitude and motion information of the carrier are output, and sensor state diagnostic information is also output to realize adaptive control of the mobile odometer.

[0007] Optionally, when the displacement sensor includes a wheel-type displacement sensor, the step of acquiring the motion state prediction information and displacement observation information of the carrier based on the sensing data of each sensor in the target sensor combination includes: The motion state prediction information of the carrier is obtained based on the sensing data measured by the inertial measurement sensor. The displacements of the left and right wheels are determined based on the encoder pulse increments of the left and right wheels of the wheel displacement sensor during the sampling period. Based on the respective displacements and wheel spacing of the left and right wheels, the displacement increment and rotation angle increment of the carrier center are determined, and the wheel displacement observation information of the carrier is obtained through position update.

[0008] Optionally, when the displacement sensor includes a visual displacement sensor, the step of acquiring the motion state prediction information and displacement observation information of the carrier based on the sensing data of each sensor in the target sensor combination includes: The motion state prediction information of the carrier is obtained based on the sensing data measured by the inertial measurement sensor. Based on the equivalent focal length, pixel size, and installation height, the pixel displacement output by the visual displacement sensor is converted into the actual physical displacement of the carrier, thereby obtaining the visual displacement observation information of the carrier.

[0009] Optionally, the inertial measurement sensor includes an accelerometer unit and a gyroscope unit, and the step of obtaining motion state prediction information of the carrier based on the sensing data measured by the inertial measurement sensor includes: Zero bias correction and coordinate transformation are performed on the sensor data output by the accelerometer unit and the gyroscope unit respectively to obtain the true acceleration and true angular velocity; The attitude integral is updated by performing attitude integration on the true angular velocity, and the velocity and position integral is performed on the true acceleration to obtain the motion state prediction information of the carrier.

[0010] Optionally, the step of obtaining the measurement confidence levels of each displacement sensor in the target sensor combination and adaptively fusing the motion state prediction information and the displacement observation information includes: Calculate the measurement confidence level corresponding to each motion sensor in the target sensor assembly in real time; The observation weight of the corresponding displacement observation information is dynamically determined based on the confidence level of each measurement. The motion state prediction information and the displacement observation information are adaptively fused based on the observation weights.

[0011] Optionally, the control method further includes: Determine whether the target sensor combination contains a visual displacement sensor. If so, determine the measurement confidence level of the visual displacement sensor based on the surface quality index. When the measurement confidence level is lower than a preset threshold, the observation weights corresponding to the visual displacement observation information are readjusted.

[0012] Optionally, the control method further includes: Determine whether the target sensor assembly contains a wheel displacement sensor; if so, determine the deviation between the measurement speed of the wheel displacement sensor and the integral speed of the inertial measurement sensor. The measurement confidence level of the wheel displacement sensor is determined based on the deviation. When the measurement confidence level is lower than a preset threshold, the observation weights corresponding to the wheel displacement observation information are readjusted.

[0013] Optionally, the adaptive fusion is implemented using an error-state Kalman filter, and the control method further includes: Based on the motion state prediction information and the error state vector, determine the prior error state and the prior error covariance matrix; The observation noise covariance matrix of the error state Kalman filter is dynamically adjusted according to each measurement confidence level. The Kalman gain is determined based on the adjusted observation noise covariance matrix and the prior error covariance matrix. The prior error state is then corrected using the Kalman gain to obtain the posterior error state. Based on the posterior error state, error compensation is performed on the motion state prediction information, and the zero bias error of the inertial measurement sensor is corrected, thereby completing the fusion of the motion state prediction information and the displacement observation information.

[0014] According to another aspect of this application, a control system for a mobile odometer is also provided, the system including a data acquisition module, a state estimation module and an adaptive control module; The data acquisition module is used to acquire the sensing data of each sensor in the adapted target sensor combination according to the current working mode. The target sensor combination includes an inertial measurement sensor and at least one displacement sensor. The current working mode is determined based on the preset sensor configuration and real-time operating conditions. The state estimation module is used to obtain motion state prediction information and displacement observation information of the carrier based on the sensing data of each sensor in the target sensor combination. The displacement observation information includes wheel displacement observation information and visual displacement observation information. The module also obtains the measurement confidence of each displacement sensor in the target sensor combination and adaptively fuses the motion state prediction information and the displacement observation information to obtain the state estimation result of the carrier. The adaptive control module is used to output the position, attitude and motion information of the carrier based on the state estimation results, and output sensor state diagnostic information to realize the adaptive control of the mobile odometer.

[0015] According to another aspect of this application, a computer-readable medium is also provided, on which a computer program is stored, which, when executed by a processor, implements the steps of any of the above methods.

[0016] Compared with existing technologies, this application determines the current working mode based on a preset sensor configuration and real-time operating conditions. It then collects sensing data from each sensor in a suitable target sensor combination, including an inertial measurement sensor and at least one displacement sensor. Based on the sensing data from each sensor in the target sensor combination, it acquires motion state prediction information and displacement observation information for the carrier, including wheel displacement observation information and visual displacement observation information. It obtains the measurement confidence level of each displacement sensor in the target sensor combination, adaptively fuses the motion state prediction information and the displacement observation information to obtain a state estimation result for the carrier. Based on the state estimation result, it outputs the carrier's position, attitude, and motion information, and outputs sensor state diagnostic information, thus realizing adaptive control of the mobile odometer. This reduces hardware implementation costs, enhances robustness, improves environmental adaptability, and achieves non-interventional adaptation. Attached Figure Description

[0017] To make the above-mentioned objectives, features and advantages of this application more apparent and understandable, the specific embodiments of this application will be described in detail below with reference to the accompanying drawings, wherein: Figure 1 A schematic flowchart of a method for controlling a mobile odometer according to one aspect of this application is shown. Figure 2 The flowchart of the error state Kalman filter algorithm in one embodiment of this application is shown; Figure 3 A schematic diagram of a system framework for mobile odometer control according to another aspect of this application is shown; Figure 4 This diagram illustrates the architecture of a low-cost odometry adaptive control system based on the fusion of an IMU and a multi-source displacement sensor, according to one embodiment of this application.

[0018] The same or similar reference numerals in the accompanying drawings represent the same or similar parts. Detailed Implementation

[0019] To make the above-mentioned objectives, features and advantages of this application more apparent and understandable, the specific embodiments of this application will be described in detail below with reference to the accompanying drawings.

[0020] Many specific details are set forth in the following description in order to provide a full understanding of this application. However, this application may also be implemented in other ways different from those described herein, and therefore this application is not limited to the specific embodiments disclosed below.

[0021] As indicated in this application and claims, unless the context clearly indicates otherwise, the words "a," "an," "an," and / or "the" are not specifically singular and may include plural forms. Generally speaking, the terms "comprising" and "including" only indicate the inclusion of explicitly identified steps and elements, which do not constitute an exclusive list, and the method or apparatus may also include other steps or elements.

[0022] Figure 1 The diagram shows a flowchart of a mobile odometer control method according to one aspect of this application, the method comprising steps S11 to S14.

[0023] Step S11: Determine the current working mode based on the preset sensor configuration and real-time operating conditions, and collect the sensing data of each sensor in the adapted target sensor combination according to the current working mode, wherein the target sensor combination includes an inertial measurement sensor and at least one displacement sensor.

[0024] The preset sensor configuration is a pre-configured hardware-supported mode. For example, it can be configured as a dual-sensor mode with a single displacement sensor and an inertial measurement sensor, or a multi-sensor mode with multiple displacement sensors and an inertial measurement sensor, depending on the hardware selection. Real-time operating conditions include current ground environmental conditions and the operating status of each sensor. The optimal current operating mode is determined by matching the preset configuration with the real-time operating conditions, and the corresponding target sensor combination is activated.

[0025] In the target sensor assembly, an inertial measurement unit (IMU) is a mandatory basic sensor, paired with at least one type of displacement sensor. The IMU can be a MEMS-type IMU, integrating an accelerometer and gyroscope, used to collect the linear acceleration and angular velocity of the carrier. The displacement sensor can be a wheel encoder, optical flow sensor, etc., used to collect displacement data of the carrier relative to the ground. The raw data from each sensor can be synchronously acquired through the corresponding communication interface of the microcontroller.

[0026] By flexibly selecting the combination of sensors to be enabled based on hardware configuration and real-time operating status, it can adapt to hardware solutions of different cost levels and changing operating environments.

[0027] Step S12: Obtain motion state prediction information and displacement observation information of the carrier based on the sensing data of each sensor in the target sensor combination. The displacement observation information includes wheel displacement observation information and visual displacement observation information.

[0028] The raw data from different sensors are independently processed to obtain the carrier's motion state prediction information and displacement observation information, providing input for subsequent adaptive fusion. Specifically, the raw data collected by the inertial measurement sensor is preprocessed and processed to obtain the carrier's motion state prediction information. Preprocessing steps may include zero-bias correction, coordinate transformation, etc., and the motion state prediction information may include predicted values ​​of the carrier's attitude, velocity, and position.

[0029] The raw data collected by various displacement sensors currently in use are converted into physical quantities to obtain corresponding displacement observation information. Specifically, when the displacement sensor is a wheel encoder, wheel displacement observation information is obtained; when the displacement sensor is a vision-based displacement sensor such as an optical flow sensor, visual displacement observation information is obtained. Depending on the current operating mode, the displacement observation information may include one or more types of observation results.

[0030] Step S13: Obtain the measurement confidence of each displacement sensor in the target sensor combination, and adaptively fuse the motion state prediction information and the displacement observation information to obtain the state estimation result of the carrier.

[0031] For each type of displacement sensor currently in use, its corresponding measurement confidence level is calculated in real time. The measurement confidence level is used to characterize the reliability of the sensor's current measurement results. For example, for vision-type displacement sensors, the measurement confidence level can be calculated based on the surface texture quality index of its output; for wheel encoder-type displacement sensors, slippage detection can be performed based on speed deviation, and then the measurement confidence level can be calculated.

[0032] Based on the measurement confidence of each displacement sensor, the weights of motion state prediction information and displacement observation information in the fusion process are dynamically adjusted, and the optimal carrier state estimation result is obtained through multi-sensor fusion algorithm.

[0033] By evaluating the measurement reliability of displacement sensors in real time and dynamically adjusting the fusion weights, robustness and positioning accuracy in complex environments can be improved.

[0034] Step S14: Based on the state estimation results, output the position, attitude and motion information of the carrier, and output sensor state diagnostic information to realize adaptive control of the mobile odometer.

[0035] The output of carrier motion-related information includes, but is not limited to, the carrier's planar position coordinates, heading attitude, linear velocity and angular velocity, etc. The output of sensor status diagnostic information includes, but is not limited to, the working status of each sensor, the current measurement confidence level, fault warning information, etc.

[0036] Based on real-time state estimation results and sensor diagnostic information, the working mode and fusion strategy can be dynamically adjusted, and reliable sensor combinations can be automatically switched under adverse working conditions to maintain a stable odometer output, thereby achieving adaptive control of the mobile odometer.

[0037] In one embodiment of this application, the inertial measurement sensor includes an accelerometer unit and a gyroscope unit. The sensor data output by the accelerometer unit and the gyroscope unit are respectively subjected to zero bias correction and coordinate transformation to obtain the true acceleration and the true angular velocity. The true angular velocity is updated by attitude integration, and the true acceleration is integrated by velocity and position to obtain the motion state prediction information of the carrier.

[0038] Inertial measurement sensors can use low-cost MEMS-type IMUs for attitude estimation, such as the MPU6050, which integrates an accelerometer unit and a gyroscope unit. The gyroscope measures angular velocity and the accelerometer measures linear acceleration.

[0039] Due to limitations in the manufacturing process and inherent characteristics of MEMS devices, the raw measurement values ​​directly output by the sensor contain systematic and random errors, and cannot be directly used for motion calculation. Therefore, it is necessary to establish an IMU measurement model and perform error correction and coordinate transformation processing. The IMU measurement model is defined as follows: Gyroscope measurement values Compared with true angular velocity The relationship is:

[0040] in, To achieve zero bias in the gyroscope, The noise measured by the gyroscope follows a Gaussian distribution with a mean of zero. The gyroscope's zero bias is an inherent, constant offset error of the device, while the gyroscope measurement noise is a random measurement error. By eliminating the systematic error caused by the gyroscope's zero bias through zero bias correction, and combined with noise suppression processing, the true angular velocity of the carrier is obtained.

[0041] Accelerometer measurements The relationship with the actual linear acceleration 'a' of the carrier is as follows:

[0042] Among them, R wb Let g be the rotation matrix from the world coordinate system to the body coordinate system, and g be the gravitational acceleration vector. To achieve zero bias in the accelerometer, To measure noise for accelerometers.

[0043] After completing data correction and coordinate transformation, the attitude integral is updated based on the true angular velocity, and the velocity and position are integrated based on the true acceleration to obtain the predicted motion state information of the carrier.

[0044] Attitude integration is implemented using quaternion representation, which avoids the gimbal lock problem present in Euler angle calculation. A unit quaternion is defined. Used to describe the carrier's attitude, where Let be the real part of the quaternion. , , Let be the imaginary part of the quaternion. The quaternion update equation in the continuous domain is:

[0045] in, This represents the true angular velocity of the carrier after correction.

[0046] To adapt to the discrete computational characteristics of digital systems, the continuous update equation is discretized to obtain a discretized quaternion update formula:

[0047] in, The sampling period is set to 5ms in this scheme to ensure the real-time output of the odometer.

[0048] Based on the rotation matrix obtained from the attitude calculation, and using the true linear acceleration in the world coordinate system, the vehicle's velocity is obtained by performing a first integration, and the vehicle's position information is obtained by performing a second integration.

[0049] The carrier's attitude, velocity, and position information obtained from the above calculations are used to determine the carrier's motion state prediction information. This information can be used as the nominal state in the subsequent error state Kalman filter and participate in multi-sensor adaptive fusion calculation.

[0050] In one embodiment of this application, when the displacement sensor includes a visual displacement sensor, motion state prediction information of the carrier is obtained based on the sensing data measured by the inertial measurement sensor; based on the equivalent focal length, pixel size and installation height, the pixel displacement output by the visual displacement sensor is converted into the actual physical displacement of the carrier to obtain visual displacement observation information of the carrier.

[0051] When the displacement sensor in the current working mode includes a visual displacement sensor, the data processing and calculation process of the inertial measurement sensor remains consistent with other working modes. That is, the raw data collected is corrected and transformed, and the motion state prediction information of the carrier is obtained through attitude integration, velocity integration and position integration.

[0052] The visual displacement sensor can be a low-cost optical flow sensor, which can be installed in the center of the mobile platform chassis, near the casters / driven wheels, or near both sides of the drive wheel axis, with an installation height h of 10 to 20 mm from the ground. It can communicate with the main controller via SPI or I2C interface.

[0053] The raw data directly output by the optical flow sensor is the pixel displacement in the image coordinate system. It cannot be directly used for odometer positioning and needs to be converted into actual physical displacement. The conversion formula is:

[0054] Where f is the equivalent focal length of the optical flow sensor, s is the pixel size, and h is the mounting height. The actual physical displacement obtained through the above calculation is the visual displacement observation information of the carrier.

[0055] In one embodiment of this application, when the displacement sensor includes a wheel displacement sensor, motion state prediction information of the carrier is obtained based on the sensing data measured by the inertial measurement sensor; the displacement of the left and right wheels is determined based on the encoder pulse increment of the left and right wheels of the wheel displacement sensor within the sampling period; based on the displacement of the left and right wheels and the wheel distance, the displacement increment and rotation angle increment of the center of the carrier are determined, and the wheel displacement observation information of the carrier is obtained through position update.

[0056] When the displacement sensor in the current working mode includes a wheel displacement sensor, the data processing and calculation process of the inertial measurement sensor remains consistent with other working modes, and will not be elaborated here.

[0057] Wheel displacement sensors can be implemented using wheel encoders. This embodiment is based on a wheel encoder odometer using a differential drive model, but it can also be extended to support other drive models. First, the displacement of each wheel is calculated based on the encoder pulse increment within the sampling period. In a single sampling period... Inside, the pulse increments of the encoders for the left and right wheels are collected. and By combining the number of pulses per encoder revolution N with the wheel radius r, the actual displacement of the left and right wheels is calculated respectively:

[0058] Based on the individual displacements of the left and right wheels and the wheelbase, the motion increment of the carrier center is derived. Combining the mean and difference of the displacements of the left and right wheels, and the wheelbase L, the displacement increment of the carrier center is calculated. With heading angle increment :

[0059] Finally, the wheel displacement observation information in the global coordinate system is obtained through position update. This is based on the vehicle's heading angle at the current moment. The displacement and rotation increments of the carrier center are transformed to the world coordinate system to complete the iterative update of position and heading. Position updates employ median integrals to improve the solution accuracy within short cycles.

[0060] in, This represents the angle between the main forward direction of the carrier's (e.g., a mobile trash can) coordinate system and the positive X-axis direction of the global world coordinate system (e.g., a Cartesian coordinate system); , These are the position coordinates of the carrier's center in the world coordinate system at the previous moment; , These are the updated position coordinates at the current moment. The position and heading information obtained through the above calculations determine the wheel displacement observation information of the vehicle.

[0061] In one embodiment of this application, it is determined whether the target sensor combination contains a visual displacement sensor. If so, the measurement confidence of the visual displacement sensor is determined based on the surface quality index. When the measurement confidence is lower than a preset threshold, the observation weights corresponding to the visual displacement observation information are readjusted.

[0062] In the multi-sensor adaptive fusion mechanism of this scheme, the measurement reliability assessment of displacement sensors and the dynamic adjustment of fusion weights are crucial steps. First, it is determined whether the current target sensor combination includes a visual displacement sensor. If it is determined to include a visual displacement sensor, the measurement confidence level of the visual displacement sensor is determined based on surface quality indicators.

[0063] In this embodiment, the visual displacement sensor is an optical flow sensor, and the surface quality index is the SQUAL index output by the optical flow sensor. This index is used to characterize the richness of the texture of the currently detected ground. The richer the texture information, the higher the SQUAL value, and the stronger the reliability of the optical flow displacement measurement results.

[0064] The measurement confidence level of a visual displacement sensor can be calculated using a piecewise function. For example, when the surface quality index SQUAL ≥ 100, the measurement confidence level is 1.0; when 30 ≤ SQUAL < 100, the measurement confidence level is SQUAL / 100; and when SQUAL < 30, the measurement confidence level is 0.

[0065] When the measurement confidence level falls below a preset threshold, the observation weights corresponding to the visual displacement observation information are readjusted. For example, the preset threshold can be initially set to 0.3, which is a test verification value and can be adaptively adjusted based on operational feedback from actual application scenarios. When the measurement confidence level falls below this preset threshold, it indicates that the current environment is unfavorable to the operation of the optical flow sensor, such as a solid-color ground or glass surface, and the reliability of the optical flow measurement results is insufficient.

[0066] In the multi-sensor fusion mode that includes both wheel displacement sensors and visual displacement sensors, the system can automatically switch to wheel displacement sensors as the main displacement measurement source, thereby reducing the observation weight of visual displacement observation information. In the mode that includes only visual displacement sensors and inertial measurement sensors, the observation weight of visual displacement observation information will be reduced, while the fusion weight of motion state prediction information from inertial measurement sensors will be increased simultaneously to compensate for the uncertainty of optical flow measurement and ensure the stability and continuity of the odometer output.

[0067] In one embodiment of this application, it is determined whether the target sensor combination contains a wheel displacement sensor. If so, the deviation between the measurement speed of the wheel displacement sensor and the integral speed of the inertial measurement sensor is determined. Based on the deviation, the measurement confidence of the wheel displacement sensor is determined. When the measurement confidence is lower than a preset threshold, the observation weights corresponding to the wheel displacement observation information are readjusted.

[0068] When the current target sensor combination includes a wheeled displacement sensor, determine the deviation between the measured velocity of the wheeled displacement sensor and the integrated velocity of the inertial measurement sensor. Specifically, the measured velocity of the wheeled displacement sensor... The integral velocity of the inertial measurement sensor is obtained by calculating the encoder pulse increment. The velocity calculation results are taken from the motion state prediction information. The velocity deviation between the two is... The expression, calculated by taking the absolute difference, is:

[0069] Based on the aforementioned speed deviation values, the measurement confidence level of the wheel displacement sensor is calculated. The measurement confidence level of the wheel displacement sensor is calculated using a piecewise function; the larger the speed deviation, the lower the corresponding measurement confidence level, indicating a higher probability of wheel slippage and poorer reliability of the displacement measurement results. Wheel encoder confidence level. The calculation formula is:

[0070] when When the value falls below a preset threshold, the observation weights corresponding to the wheel displacement observation information are readjusted. For example, the preset threshold can be set to 0.3. When this occurs, it indicates that the wheel may be slipping, and the reliability of the wheel displacement measurement results decreases significantly.

[0071] In the multi-sensor fusion mode that includes both wheel displacement sensors and visual displacement sensors, the observation weight of wheel displacement observation information is automatically reduced while the observation weight of visual displacement observation information is increased simultaneously. In the mode that includes only wheel displacement sensors and inertial measurement sensors, the observation weight of wheel displacement observation information is reduced while the fusion weight of motion state prediction information from inertial measurement sensors is increased simultaneously to compensate for the uncertainty of wheel measurement and ensure the stability of the odometer output under slippage conditions.

[0072] In one embodiment of this application, the measurement confidence level corresponding to each displacement sensor in the target sensor combination is calculated in real time; the observation weight of the corresponding displacement observation information is dynamically determined based on each measurement confidence level; and the motion state prediction information and the displacement observation information are adaptively fused based on the observation weight.

[0073] The measurement confidence level of each displacement sensor in the target sensor array is calculated in real time. For example, the confidence level of a visual displacement sensor is calculated based on surface texture quality indicators, and the confidence level of a wheel displacement sensor is calculated based on slippage detection results. This quantitatively characterizes the current reliability of each displacement observation result, with higher confidence levels corresponding to higher fusion weights. This weight is mapped to the error state Kalman filter, manifesting as a dynamic adjustment of the observation noise covariance matrix; lower confidence levels correspond to higher observation noise values.

[0074] Based on the determined observation weights, adaptive fusion calculations are performed on motion state prediction information and displacement observation information from various sources. This allows the system to prioritize the use of highly reliable observation data and automatically reduce the fusion ratio when the operating conditions of a single sensor degrade, thus ensuring the stability of the odometer output and the positioning accuracy.

[0075] In one embodiment of this application, the adaptive fusion is implemented using an error state Kalman filter. Based on the motion state prediction information and the error state vector, a prior error state and a prior error covariance matrix are determined. The observation noise covariance matrix of the error state Kalman filter is dynamically adjusted according to each measurement confidence level. The Kalman gain is determined based on the adjusted observation noise covariance matrix and the prior error covariance matrix. The prior error state is corrected using the Kalman gain to obtain the posterior error state. Error compensation is performed on the motion state prediction information based on the posterior error state, and the zero bias error of the inertial measurement sensor is corrected, thus completing the fusion of the motion state prediction information and the displacement observation information.

[0076] exist Figure 2 The illustrated embodiment demonstrates the algorithm flow of the Error-State Kalman Filter (ESKF). ESKF is a filtering algorithm used to fuse IMU data with external observation data, improving numerical stability by estimating the error state rather than the complete state. To adapt to different low-cost products, this solution proposes an adaptive fusion algorithm based on sensor confidence, which can automatically adjust the fusion strategy according to the actual sensor type and number configured.

[0077] The complete operation flow of the error state Kalman filter includes four stages: initialization, state prediction, observation update, and error state correction. In stage 1, initialization, parameter configuration can be completed based on the initial static condition of the carrier. For example, the initial values ​​of position, velocity, and attitude errors can be set to zero for state vector initialization, and the initial values ​​of zero bias errors of gyroscopes and accelerometers can be the factory calibration values ​​or the mean of static sampling. The covariance matrix initialization can set corresponding diagonal elements according to the initial uncertainty of each state variable. The noise covariance can be set according to the process noise reference value based on the noise level of the IMU device and the observation noise reference value based on the measurement accuracy of the displacement sensor.

[0078] In stage 2 state prediction, based on motion state prediction information and the error state vector, the prior error state and the prior error covariance matrix are calculated. The motion state prediction information serves as the nominal state of the filter, providing a benchmark for estimating the error state. The error state vector describes the deviation between the nominal and true states. The standard error state vector comprises 15 dimensions: position error, velocity error, attitude error, gyroscope bias error, and accelerometer bias error, expressed as:

[0079] in, For positional error, For speed error, For attitude error, For gyroscope zero bias error, This refers to the zero bias error of the accelerometer.

[0080] For planar motion scenarios on home mobile platforms, the vertical motion component can be ignored. To adapt to the computing power and memory limitations of low-cost hardware such as the ESP32, the 15-dimensional standard state vector is simplified to 9 dimensions, significantly reducing the amount of matrix operations. The simplified error state vector expression is as follows:

[0081] in, , For planar position error, , For planar velocity error, , , For the zero bias error of the three-axis gyroscope, This represents the zero bias error of the vertical accelerometer.

[0082] Using the motion state prediction information as the nominal state of the filter, and combining it with the error state vector, state prediction is performed to complete the time-domain propagation of the error state and uncertainty, resulting in the prior error state and the prior error covariance matrix. The state prediction equation is:

[0083] in, This is the prior error state. Let be the prior error covariance matrix. Let be the state transition matrix, and Q be the process noise covariance. The state transition matrix is... It is a 9×9 time-varying matrix used to describe the propagation relationship of error states between adjacent sampling periods. Its matrix elements are jointly determined by the nominal values ​​of angular velocity and acceleration at the current moment, as well as the sampling period. The nominal values ​​of angular velocity and acceleration are the angular velocity and linear acceleration in the predicted carrier motion state information obtained after zero-bias correction of the inertial measurement sensor, coordinate transformation, and attitude integration.

[0084] Specifically, the state transition matrix The construction process is as follows: First, based on the two-dimensional planar motion characteristics of the carrier, an error state propagation equation is established that includes only planar position error, planar velocity error, and gyroscope zero bias error. This equation describes the drift and coupling relationship of each error component over time when there is no external observation input. The nominal values ​​of angular velocity and acceleration at the current moment are used as linearization parameters of the error state propagation equation, and substituted with the sampling period. The Euler discretization method is used to transform the continuous-domain error differential equation into a discrete form. Finally, the partial derivatives of the error state with respect to itself are extracted from the discretized error state propagation equation to obtain the state transition matrix at the current moment. Since the nominal values ​​of angular velocity and acceleration change with the motion state of the carrier in each sampling period, the state transition matrix... The calculation and update must be performed in each sampling period.

[0085] In Phase 3 observation updates, the observation noise covariance matrix of the error state Kalman filter is dynamically adjusted based on the measurement confidence levels of each displacement sensor, achieving adaptive adjustment of the fusion weights. The value of the observation noise covariance matrix is ​​negatively correlated with the sensor measurement confidence levels, and the adjustment formula is as follows:

[0086] in, The baseline observation noise covariance matrix, The confidence weights for the visual displacement sensors. The confidence weights are for the wheel-type displacement sensor. It is a very small constant. When the confidence level of sensor measurements decreases, the value of the observation noise covariance matrix increases synchronously. The filter's confidence in the displacement observation results decreases, and it relies more on the prediction results of the inertial measurement sensor, thereby achieving adaptive adjustment of the fusion weights.

[0087] The Kalman gain is calculated based on the adjusted observation noise covariance matrix and the prior error covariance matrix. The prior error state is then corrected using the Kalman gain to obtain the posterior error state. Displacement observation information is used as the observation input, and the observation update is completed by combining the observation matrix. The observation update equation is as follows:

[0088] in, Here, H is the Kalman gain, H is the observation matrix, and R is the observation noise covariance. These are the observed values ​​corresponding to the displacement observation information. This represents the posterior error state. Let be the posterior error covariance matrix. The Kalman gain determines the magnitude of the correction of the error state by the observation residuals. After the observation is updated, the optimal error state estimate and its corresponding covariance are obtained.

[0089] In Stage 4, error state correction, error compensation is performed on the motion state prediction information based on the posterior error state, and the zero-bias error of the inertial measurement sensor is corrected, thus completing the fusion of motion state prediction information and displacement observation information. Specifically, the position error, velocity error, and attitude error in the posterior error state are superimposed onto the motion state prediction information, which serves as the nominal state, to correct the position, velocity, and attitude of the carrier. Simultaneously, the zero-bias error term in the posterior error state is used to correct the zero bias of the gyroscope and accelerometer online, suppressing the drift error of the inertial devices. After correction, the final carrier state estimation result is obtained, completing one full filtering and fusion iteration.

[0090] In one embodiment of this application, taking a real-world home operation scenario of a smart mobile trash can as an example, when the trash can moves from the living room into the kitchen, crossing an area of ​​highly reflective, textureless white polished tiles, the following adaptive control process is triggered. During the condition recognition phase, the optical flow sensor cannot properly calculate displacement due to ground reflection and the lack of effective texture features. Its output surface quality index (SQUAL) drops sharply to 20, far below the effective working threshold of 30. According to the aforementioned formula for calculating the confidence level of the optical flow sensor, when SQUAL < 30, the optical flow confidence level is 0, lower than the preset confidence threshold of 0.3. Simultaneously, the friction between the wheels and the tiled surface is sufficient, and the wheel encoder, after speed deviation detection, determines that no slippage has occurred, maintaining an encoder confidence level of C. enc =1.0.

[0091] When the confidence level of the optical flow sensor is determined to be below a threshold, a single-source degradation adaptive mechanism is automatically triggered. In multi-sensor fusion mode, the system dynamically increases the noise weight corresponding to the optical flow observation in the observation noise covariance matrix, simultaneously increasing the observation proportion of the wheel encoder, and automatically switching to using the wheel encoder as the main displacement measurement source. If in optical flow + IMU-only working mode, the weight proportion of the IMU prediction end is increased to compensate for the observation uncertainty caused by optical flow measurement failure, ensuring that the odometer continues to output stably during the switching of working conditions.

[0092] Figure 3The diagram illustrates a framework of a mobile odometer control system according to another aspect of this application. The system includes a data acquisition module 10, a state estimation module 20, and an adaptive control module 30. The data acquisition module 10 acquires sensing data from each sensor in an adapted target sensor combination according to a current operating mode. The target sensor combination includes an inertial measurement sensor and at least one displacement sensor. The current operating mode is determined based on a preset sensor configuration and real-time operating conditions. The state estimation module 20 obtains motion state prediction information and displacement observation information of the carrier based on the sensing data from each sensor in the target sensor combination. The displacement observation information includes wheel displacement observation information and visual displacement observation information. It also obtains the measurement confidence level of each displacement sensor in the target sensor combination and adaptively fuses the motion state prediction information and the displacement observation information to obtain a state estimation result for the carrier. The adaptive control module 30 outputs the position, attitude, and motion information of the carrier based on the state estimation result and outputs sensor state diagnostic information to achieve adaptive control of the mobile odometer.

[0093] The data acquisition module 10 is the system's data input unit, corresponding to the working mode matching and sensor data acquisition steps in the method. This module determines the current working mode based on the preset sensor hardware configuration and real-time operating conditions, activates the matched target sensor combination, and synchronously acquires the raw sensor data from the inertial measurement sensor and various displacement sensors.

[0094] The state estimation module 20 corresponds to the state calculation, confidence assessment, and adaptive fusion steps in the method. Based on the collected sensor data, this module calculates the predicted motion state information and multi-channel displacement observation information of the carrier, evaluates the measurement confidence of each displacement sensor in real time, and completes the adaptive weighted fusion of multi-source data through an error state Kalman filter to output the optimal state estimation result of the carrier.

[0095] The adaptive control module 30 outputs motion information such as the position, attitude, and velocity of the carrier based on the state estimation results. At the same time, it outputs diagnostic information such as the working status of the sensors and the confidence level, supporting the upper-level motion control and navigation functions and ensuring the stable operation of the system under different working conditions.

[0096] Figure 4 The illustrated embodiment demonstrates the architecture of a low-cost odometry adaptive control system based on the fusion of IMU and multi-source displacement sensors. It uses a low-cost microcontroller (such as ESP32) as the core scheduling unit, adopts a hierarchical modular design, supports three sensor configuration modes, and can be adapted to different application scenarios and cost requirements.

[0097] The sensor input layer includes three types of sensing units, which can be flexibly selected according to the operating mode. The IMU sensor (MPU6050) is a mandatory unit in all modes, outputting raw data of triaxial acceleration and triaxial angular velocity, which is connected to the main controller via I2C / SPI interface, providing basic data for attitude calculation and inertial navigation prediction. The optical flow sensor is an optional unit, corresponding to mode 2 and mode 3 configurations, outputting XY plane displacement data and SQUAL surface quality index. The displacement data is transmitted via I2C / SPI interface, and the SQUAL index is used for confidence assessment. The wheel encoder is an optional unit, corresponding to mode 1 and mode 3 configurations, outputting the pulse increment (Δn) of the left and right wheels. L , Δn R It is connected to the main controller via the GPIO interface and is used for wheel odometer calculation and slip detection.

[0098] The main control core processing layer includes a microcontroller, which integrates four core functional units to collaboratively complete data processing and fusion calculations. The data acquisition and attitude estimation unit uniformly collects and synchronizes data from various sensors, performs attitude calculations based on raw IMU data, and outputs predicted carrier motion state information. The confidence assessment module receives sensor data and calculates the optical flow confidence C for each sensor. of confidence level C of wheel encoder enc Simultaneously complete wheel encoder slippage detection and output confidence weight w of w enc The adaptive fusion module employs an ESKF filter at its core, dynamically allocating fusion weights based on confidence levels to achieve adaptive fusion of inertial navigation predictions and displacement observations, outputting the fused pose result (x, y, θ, v, ω). The motion control and diagnostic module generates feedback control signals based on the fused pose and simultaneously outputs sensor status diagnostic information, realizing closed-loop control and status monitoring of the system.

[0099] The output and execution layer transmits the fused pose results to the motor drive unit, generating PWM control signals to achieve closed-loop control of the carrier's speed and angular velocity. Simultaneously, the system outputs position coordinates (x, y), heading angle θ, linear velocity v, angular velocity ω, and status diagnostic information, completing all the functions of the odometer. The system supports three operating modes: IMU-wheel encoder fusion, IMU-optical flow sensor fusion, and IMU-optical flow + wheel encoder multi-sensor fusion, which can automatically switch according to preset hardware configuration and real-time operating conditions.

[0100] The sensor confidence evaluation model proposed in this application dynamically calculates the confidence of each sensor by real-time monitoring of the SQUAL value of the optical flow sensor and the slippage detection results of the wheel encoder, and adaptively adjusts the fusion weights accordingly. This technical solution significantly improves environmental adaptability; the system can automatically identify adverse conditions such as smooth surfaces, solid-color carpets, and wheel slippage and make corresponding adjustments, maintaining stable positioning accuracy in various home environments. It is highly robust; when a single sensor fails, the system can automatically switch to other sensors without completely losing the positioning function. No manual intervention is required; confidence evaluation and weight adjustment are fully automated, and users do not need to understand the underlying technical details.

[0101] To address the hardware limitations of low-cost microcontrollers such as the ESP32, the standard ESKF algorithm is optimized for lightweight design, including state vector dimensionality reduction, fixed-point arithmetic substitution, and dual-core parallel processing. This significantly reduces hardware costs, enabling multi-sensor fusion without a high-performance processor, and lowering the main control chip cost from hundreds of yuan to 15 yuan. Real-time performance is guaranteed; the optimized algorithm runs for less than 10ms on the ESP32, meeting real-time control requirements. Power consumption is reduced; the ESP32 consumes only one-tenth the power of a high-performance processor, extending battery life. Mass production is facilitated; the ESP32 has a mature supply chain and comprehensive development tools, making productization and mass production easy.

[0102] The proposed solution employs a low-cost MEMS IMU combined with an adaptive fusion architecture using optical flow sensors or wheel encoders. This, along with a lightweight ESKF algorithm optimized for ESP32 and an adaptive weight adjustment mechanism based on sensor confidence, achieves centimeter-level positioning accuracy and millisecond-level response speed on a hardware platform with a total cost below 50 yuan. The proposed solution also includes a sensor confidence evaluation model and an adaptive switching strategy, enabling the system to automatically select the optimal sensor combination based on environmental conditions. This maintains stable odometer output in various home floor environments, providing a low-cost, high-reliability positioning control solution for home mobile devices such as smart mobile trash cans.

[0103] This application also provides a computer-readable medium having computer instructions stored thereon, which can be executed by a processor to implement a mobile odometer control method as described above.

[0104] When a mobile odometer control method is implemented as a computer program, it can also be stored as an article of manufacture in a computer-readable storage medium. For example, a computer-readable storage medium may include, but is not limited to, magnetic storage devices (e.g., hard disks, floppy disks, magnetic stripes), optical discs (e.g., compact discs (CDs), digital multifunction discs (DVDs)), smart cards, and flash memory devices (e.g., electrically erasable programmable read-only memory (EPROM), cards, sticks, key drives). Furthermore, the various storage media described herein can represent one or more devices and / or other machine-readable media for storing information. The term "machine-readable medium" may include, but is not limited to, wireless channels and various other media (and / or storage media) capable of storing, containing, and / or carrying code and / or instructions and / or data.

[0105] It should be understood that the embodiments described above are merely illustrative. The embodiments described herein may be implemented in hardware, software, firmware, middleware, microcode, or any combination thereof. For hardware implementation, the processor may be implemented within one or more application-specific integrated circuits (ASICs), digital signal processors (DSPs), digital signal processing devices (DSPDs), programmable logic devices (PLDs), field-programmable gate arrays (FPGAs), processors, controllers, microcontrollers, microprocessors, and / or other electronic units designed to perform the functions described herein, or combinations thereof.

[0106] Some aspects of this application can be executed entirely by hardware, entirely by software (including firmware, resident software, microcode, etc.), or by a combination of hardware and software. The aforementioned hardware or software may be referred to as a "data block," "module," "engine," "unit," "component," or "system." The processor may be one or more application-specific integrated circuits (ASICs), digital signal processors (DSPs), digital signal processing devices (DAPDs), programmable logic devices (PLDs), field-programmable gate arrays (FPGAs), processors, controllers, microcontrollers, microprocessors, or combinations thereof. Furthermore, aspects of this application may manifest as computer products residing in one or more computer-readable media, including computer-readable program code. For example, computer-readable media may include, but are not limited to, magnetic storage devices (e.g., hard disks, floppy disks, magnetic tapes, etc.), optical discs (e.g., compressed CDs, digital multifunction DVDs, etc.), smart cards, and flash memory devices (e.g., cards, sticks, key drives, etc.).

[0107] A computer-readable medium may contain a propagated data signal containing computer program code, for example, on baseband or as part of a carrier wave. This propagated signal may take various forms, including electromagnetic, optical, and so on, or suitable combinations thereof. A computer-readable medium can be any computer-readable medium other than a computer-readable storage medium, which can be connected to an instruction execution system, apparatus, or device to enable communication, propagation, or transmission of a program for use. The program code located on the computer-readable medium can be propagated through any suitable medium, including radio, cable, fiber optic cable, radio frequency signals, or similar media, or any combination of the above media.

[0108] The basic concepts have been described above. Obviously, for those skilled in the art, the above disclosure is merely illustrative and does not constitute a limitation of this application. Although not explicitly stated herein, those skilled in the art may make various modifications, improvements, and corrections to this application. Such modifications, improvements, and corrections are suggested in this application, and therefore remain within the spirit and scope of the exemplary embodiments of this application.

[0109] Furthermore, this application uses specific terms to describe embodiments of the application. For example, "an embodiment," "one embodiment," and / or "some embodiments" refer to a particular feature, structure, or characteristic related to at least one embodiment of the application. Therefore, it should be emphasized and noted that "an embodiment," "one embodiment," or "an alternative embodiment" mentioned twice or more in different locations in this specification do not necessarily refer to the same embodiment. In addition, certain features, structures, or characteristics in one or more embodiments of the application can be appropriately combined.

[0110] In some embodiments, numbers describing the quantity of components and attributes are used. It should be understood that such numbers used in the description of embodiments are modified in some examples with the terms "approximately," "approximately," or "generally." Unless otherwise stated, "approximately," "approximately," or "generally" indicates that the numbers are allowed to vary by ±20%. Accordingly, in some embodiments, the numerical parameters used in the specification and claims are approximate values, which may be changed depending on the characteristics required by individual embodiments. In some embodiments, numerical parameters should take into account specified significant digits and employ a general method of digit reservation. Although the numerical ranges and parameters used to confirm their breadth of scope in some embodiments of this application are approximate values, in specific embodiments, such values ​​are set as precisely as feasible.

Claims

1. A control method for a mobile odometer, characterized in that, The control method includes: The current working mode is determined based on the preset sensor configuration and real-time operating conditions. The sensing data of each sensor in the target sensor combination is collected according to the current working mode. The target sensor combination includes an inertial measurement sensor and at least one displacement sensor. Based on the sensing data of each sensor in the target sensor combination, the motion state prediction information and displacement observation information of the carrier are obtained respectively, wherein the displacement observation information includes wheel displacement observation information and visual displacement observation information. The measurement confidence scores of each displacement sensor in the target sensor combination are obtained, and the motion state prediction information and the displacement observation information are adaptively fused to obtain the state estimation result of the carrier. Based on the state estimation results, the position, attitude and motion information of the carrier are output, and sensor state diagnostic information is also output to realize adaptive control of the mobile odometer.

2. The control method according to claim 1, characterized in that, When the displacement sensor includes a wheel-type displacement sensor, the step of acquiring the carrier's motion state prediction information and displacement observation information based on the sensing data of each sensor in the target sensor assembly includes: The motion state prediction information of the carrier is obtained based on the sensing data measured by the inertial measurement sensor. The displacements of the left and right wheels are determined based on the encoder pulse increments of the left and right wheels of the wheel displacement sensor during the sampling period. Based on the respective displacements and wheel spacing of the left and right wheels, the displacement increment and rotation angle increment of the carrier center are determined, and the wheel displacement observation information of the carrier is obtained through position update.

3. The control method according to claim 1, characterized in that, When the displacement sensor includes a visual displacement sensor, the step of acquiring the carrier's motion state prediction information and displacement observation information based on the sensing data of each sensor in the target sensor combination includes: The motion state prediction information of the carrier is obtained based on the sensing data measured by the inertial measurement sensor. Based on the equivalent focal length, pixel size, and installation height, the pixel displacement output by the visual displacement sensor is converted into the actual physical displacement of the carrier, thereby obtaining the visual displacement observation information of the carrier.

4. The control method according to claim 2 or 3, characterized in that, The inertial measurement sensor includes an accelerometer unit and a gyroscope unit. The step of obtaining motion state prediction information of the carrier based on the sensing data measured by the inertial measurement sensor includes: Zero bias correction and coordinate transformation are performed on the sensor data output by the accelerometer unit and the gyroscope unit respectively to obtain the true acceleration and true angular velocity; The attitude integral is updated by performing attitude integration on the true angular velocity, and the velocity and position integral is performed on the true acceleration to obtain the motion state prediction information of the carrier.

5. The control method according to claim 1, characterized in that, The step of obtaining the measurement confidence levels of each displacement sensor in the target sensor combination and adaptively fusing the motion state prediction information with the displacement observation information includes: Calculate the measurement confidence level corresponding to each motion sensor in the target sensor assembly in real time; The observation weight of the corresponding displacement observation information is dynamically determined based on the confidence level of each measurement. The motion state prediction information and the displacement observation information are adaptively fused based on the observation weights.

6. The control method according to claim 5, characterized in that, The control method further includes: Determine whether the target sensor combination contains a visual displacement sensor. If so, determine the measurement confidence level of the visual displacement sensor based on the surface quality index. When the measurement confidence level is lower than a preset threshold, the observation weights corresponding to the visual displacement observation information are readjusted.

7. The control method according to claim 5, characterized in that, The control method further includes: Determine whether the target sensor assembly contains a wheel displacement sensor; if so, determine the deviation between the measurement speed of the wheel displacement sensor and the integral speed of the inertial measurement sensor. The measurement confidence level of the wheel displacement sensor is determined based on the deviation. When the measurement confidence level is lower than a preset threshold, the observation weights corresponding to the wheel displacement observation information are readjusted.

8. The control method according to claim 5, characterized in that, The adaptive fusion is implemented using an error-state Kalman filter, and the control method further includes: Based on the motion state prediction information and the error state vector, determine the prior error state and the prior error covariance matrix; The observation noise covariance matrix of the error state Kalman filter is dynamically adjusted according to each measurement confidence level. The Kalman gain is determined based on the adjusted observation noise covariance matrix and the prior error covariance matrix. The prior error state is then corrected using the Kalman gain to obtain the posterior error state. Based on the posterior error state, error compensation is performed on the motion state prediction information, and the zero bias error of the inertial measurement sensor is corrected, thereby completing the fusion of the motion state prediction information and the displacement observation information.

9. A control system for a mobile odometer, characterized in that, The control system includes a data acquisition module, a state estimation module, and an adaptive control module; The data acquisition module is used to acquire the sensing data of each sensor in the adapted target sensor combination according to the current working mode. The target sensor combination includes an inertial measurement sensor and at least one displacement sensor. The current working mode is determined based on the preset sensor configuration and real-time operating conditions. The state estimation module is used to obtain motion state prediction information and displacement observation information of the carrier based on the sensing data of each sensor in the target sensor combination. The displacement observation information includes wheel displacement observation information and visual displacement observation information. The module also obtains the measurement confidence of each displacement sensor in the target sensor combination and adaptively fuses the motion state prediction information and the displacement observation information to obtain the state estimation result of the carrier. The adaptive control module is used to output the position, attitude and motion information of the carrier based on the state estimation results, and output sensor state diagnostic information to realize the adaptive control of the mobile odometer.

10. A computer-readable medium having a computer program stored thereon, characterized in that, When the computer program is executed by the processor, it implements the steps of the control method as described in any one of claims 1 to 8.

Images