A multi-sensor fusion positioning method based on extended Kalman filter

Abstract

The present application relates to the technical field of fusion positioning, and more particularly to a multi-sensor fusion positioning method based on extended Kalman filtering, which proposes the following scheme: by fusing the motion information obtained by a wheel speed encoder and an inertial measurement unit, a slip indicator reflecting the change of wheel-ground adhesion is constructed, and a plurality of motion modes and mode probabilities are introduced accordingly; a slip extension state is introduced in a unified state vector, and prediction updating and observation consistency evaluation are respectively performed under different motion modes; the observation noise parameters are adaptively adjusted in combination with observation innovation, and the multi-mode estimation results are fused and output based on the mode probabilities. The present application can improve the positioning stability and robustness under the conditions of load change and adhesion fluctuation.

Description

Technical Field

[0001] This invention relates to the field of fusion positioning technology, and in particular to a multi-sensor fusion positioning method based on extended Kalman filtering. Background Technology

[0002] In smart factories and smart agriculture, handling robots typically need to perform continuous walking, turning, and load handling operations in complex environments. Their positioning accuracy and stability directly affect path planning, obstacle avoidance, and task execution. Current technologies often employ multi-source information such as wheel speed encoders, inertial measurement units, and lidar or vision sensors, using Kalman filtering or extended Kalman filtering to achieve multi-sensor fusion positioning.

[0003] However, in practical applications, the aforementioned existing technologies still have significant shortcomings under complex working conditions. First, when the transport robot operates under different ground conditions or load states, the wheeled odometer estimation error is prone to fluctuation, and the positioning results accumulate offsets after long-term operation, making it difficult to maintain stable accuracy. Second, under conditions of changing adhesion conditions, local slippage, or sudden load changes, existing multi-sensor fusion methods often fail to reflect changes in operating status in a timely manner, leading to jitter or jumps in the positioning results and affecting trajectory continuity. Third, some existing technologies are highly dependent on external observation information; when the quality of observation fluctuates or fails temporarily, the positioning results are prone to abnormal corrections, reducing overall robustness.

[0004] To address the above issues, this application proposes a multi-sensor fusion positioning method based on extended Kalman filtering. Summary of the Invention

[0005] The technical problem this invention aims to solve is to address the shortcomings of existing technologies by providing a multi-sensor fusion localization method based on extended Kalman filtering. This method fuses motion information acquired from wheel speed encoders and inertial measurement units to construct a slip indication reflecting changes in wheel-ground adhesion, and introduces multiple motion modes and their probabilities accordingly. A slip extension state is introduced into a unified state vector, and prediction updates and observation consistency assessments are performed under different motion modes. Furthermore, observation noise parameters are adaptively adjusted based on observation innovation, and the multi-mode estimation results are fused and output based on the mode probabilities. This solves the problem of fluctuating wheel mileage estimation errors when a transport robot operates under different ground conditions or load states.

[0006] To achieve the above objectives, the present invention provides the following technical solution:

[0007] A multi-sensor fusion localization method based on extended Kalman filtering is applied to a handling robot. The handling robot includes a sensor assembly for acquiring motion and observation information of the handling robot. The method includes:

[0008] Based on the motion information, a slip indication is calculated, and the mode probability of multiple preset motion patterns is determined according to the slip indication, wherein the slip indication is used to characterize the degree of inconsistency between the motion information and the robot's short-term dynamic response;

[0009] For each motion mode, an extended Kalman filter is used to update the prediction, resulting in the predicted state and prediction covariance for each motion mode.

[0010] After acquiring the observation information, for each motion mode, the observation prediction value is calculated based on the corresponding prediction state to obtain the observation innovation and innovation covariance, and the mode probability of the motion mode is updated according to the observation innovation and innovation covariance, wherein the innovation covariance is determined by the prediction covariance and the observation noise parameter of the extended Kalman filter.

[0011] The observation noise parameters are adjusted based on the updated mode probabilities and the observation innovations. An extended Kalman filter observation update is performed for each motion mode, and the fusion localization result of the transport robot is output.

[0012] The motion information includes wheel speed information collected by the wheel speed encoder and angular velocity and linear acceleration information collected by the inertial measurement unit. The observation information includes pose observation information output by the lidar positioning module, pose observation information output by the visual positioning module, and position observation information output by the positioning device. The method further includes preprocessing the raw data collected by the sensor components to obtain the motion information and the observation information. The preprocessing includes time alignment based on timestamps, coordinate system transformation, and removal of abnormal data.

[0013] The method further includes constructing a unified state vector for extended Kalman filtering. The unified state vector includes at least a pose state characterizing the pose of the handling robot and a slip extension state characterizing the wheel motion error due to load changes. The initial value of the pose state is determined based on a preset initial pose and an initial pose obtained from the observation information. The initial value of the slip extension state is set to a preset nominal value, and the initial variance corresponding to the preset nominal value is greater than or equal to the initial variance corresponding to the pose state. A state covariance is defined and initialized as a diagonal matrix composed of the initial variances corresponding to each state component of the unified state vector.

[0014] Based on the motion information, the slip indication is calculated, including:

[0015] Based on the motion information, the short-term speed and displacement increment calculated from the wheel mileage are determined to obtain the first short-term motion amount;

[0016] Based on the motion information, the short-term linear acceleration and angular velocity calculated by inertia are determined, and the short-term linear acceleration and angular velocity are integrated to obtain the second short-term motion quantity;

[0017] The difference between the first short-term motion quantity and the second short-term motion quantity is calculated as the original inconsistency quantity, and the original inconsistency quantity is normalized and low-pass filtered to obtain the slip indication quantity, wherein the normalization is based on the amplitude of the second short-term motion quantity.

[0018] Determine the mode probabilities of multiple preset motion patterns based on the slip indication, including:

[0019] A preset mode set includes multiple motion modes, the mode set including at least a low slip mode and a high slip mode, and a corresponding slip threshold range is set for each motion mode. The low slip mode indicates that the handling robot is in a stable wheel-to-ground attachment state, and the high slip mode indicates that the handling robot is in an unstable wheel-to-ground attachment state. The slip threshold range includes an overlapping range, which is the overlapping part of the slip threshold range of the low slip mode and the slip threshold range of the high slip mode.

[0020] The slip indication is matched with the slip threshold range of each motion mode to obtain the original weight of each motion mode;

[0021] The original weights of each motion pattern are normalized to obtain the pattern probability of the motion pattern.

[0022] The steps for constructing the overlapping interval include:

[0023] Acquire historical motion data of the handling robot in a preset calibration scenario. The historical motion data includes at least the wheel speed signal output by the wheel speed encoder, the linear acceleration signal output by the inertial measurement unit, and the angular velocity signal.

[0024] Frequency domain analysis of wheel speed signal line, acceleration signal and angular velocity signal is performed using a sliding time window, and energy characteristics within a preset frequency band are extracted as adhesion degradation characteristics.

[0025] Based on the historical motion data, the historical slip indication for the corresponding time window is calculated, and the historical slip indication is statistically correlated with the adhesion degradation characteristics to obtain the distribution parameters of the slip indication under different adhesion degradation levels.

[0026] The slip threshold ranges for the low slip mode and the high slip mode are determined based on the distribution parameters, and an overlapping range is determined through a critical partitioning strategy. The critical partitioning strategy includes: dividing the adhesion degradation features into at least two distinct adhesion degradation levels, including a critical adhesion degradation level; obtaining preset quantile thresholds for historical slip indicator samples belonging to the critical adhesion degradation level in the distributions corresponding to the low slip mode and the high slip mode, respectively; using the upper quantile threshold of the low slip mode as the upper bound of the low slip mode threshold range; using the lower quantile threshold of the high slip mode as the lower bound of the high slip mode threshold range; and using the overlap between the upper and lower bounds as the overlapping range.

[0027] The extended Kalman filter prediction update for each motion mode, to obtain the prediction state and prediction covariance for each motion mode, includes:

[0028] Determine a nonlinear state transition function for each motion mode, including a slip extension state, wherein the slip extension state is updated according to the slip indication;

[0029] Substitute the motion information into the nonlinear state transition function to update the unified state vector of the previous moment, and obtain the predicted state under the corresponding motion mode.

[0030] The nonlinear state transition function is linearized to the unified state vector at the previous time step to obtain the corresponding state transition Jacobian matrix.

[0031] Based on the state transition Jacobian matrix, the process noise parameters corresponding to the motion mode, and the state covariance of the previous moment, the state covariance is propagated and updated to obtain the prediction covariance corresponding to the motion mode.

[0032] The slip extension state participates in the time update of the nonlinear state transition function and the propagation update of the state covariance, and the process noise parameters corresponding to different motion modes are adjusted according to the slip extension state.

[0033] Calculate the observed predicted value based on the corresponding predicted state, obtain the observed innovation and innovation covariance, and update the mode probability of the motion pattern according to the observed innovation and innovation covariance, including:

[0034] For each motion mode, a nonlinear observation function corresponding to the observation information is determined, and the predicted state corresponding to the motion mode is substituted into the nonlinear observation function to obtain the corresponding observation prediction value.

[0035] Calculate the observation innovation corresponding to the motion pattern, wherein the observation innovation is the difference between the observation information and the observation prediction value;

[0036] The nonlinear observation function is linearized to the first order at the predicted state of the corresponding motion mode to obtain the observation Jacobian matrix corresponding to the motion mode.

[0037] Based on the observed Jacobian matrix, the predicted covariance corresponding to the motion pattern, and the observed noise parameters, calculate the innovative covariance corresponding to the motion pattern.

[0038] The observation consistency metric corresponding to the motion pattern is calculated based on the observation innovation and the innovation covariance, and the likelihood weight of the motion pattern is determined by the observation consistency metric.

[0039] The prior mode probability of the motion pattern is normalized and updated based on the likelihood weight to obtain the posterior mode probability of the motion pattern.

[0040] The adjustment of the observation noise parameters based on the updated model probability and the observation innovation includes:

[0041] Based on the posterior mode probability, a comprehensive observation innovation weighted by mode probability is determined, wherein the comprehensive observation innovation is obtained by weighting the observation innovation corresponding to each motion mode with the corresponding posterior mode probability.

[0042] The observation noise scaling factor is calculated based on the comprehensive observation innovation, and the observation noise parameters are scaled according to the observation noise scaling factor to obtain the adjusted observation noise parameters.

[0043] The extended Kalman filter observation update for each motion mode outputs the fused localization result of the handling robot, including:

[0044] Based on the adjusted observation noise parameters, observation innovation, and innovation covariance, calculate the Kalman gain corresponding to the motion mode;

[0045] The predicted state corresponding to the motion pattern is corrected and updated based on the Kalman gain to obtain the updated state;

[0046] Based on the posterior mode probability of each motion mode, the updated states of each motion mode are weighted and fused to obtain the fused localization result of the handling robot.

[0047] Compared with the prior art, the beneficial effects of the present invention are:

[0048] This invention introduces a slip indication and a multi-motion mode probability mechanism to explicitly model wheel motion errors within a unified state vector. It also adaptively adjusts the filtering process by combining prediction consistency and observation consistency, enabling the positioning results to continuously evolve with changes in load and attachment conditions. By setting a mode probability determination method that includes overlapping intervals, it effectively avoids positioning jitter and jumps during critical state transitions. Furthermore, by innovatively adjusting observation noise parameters based on observations, it reduces the impact of observation quality fluctuations or short-term failures on the positioning results. This improves the stability, continuity, and robustness of positioning under complex conditions, making it suitable for the high-reliability positioning requirements of handling robots in smart factories and smart agriculture scenarios. Attached Figure Description

[0049] Other features, objects, and advantages of the invention will become more apparent from the following detailed description of non-limiting embodiments with reference to the accompanying drawings:

[0050] Figure 1 An exemplary application scenario diagram provided for an embodiment of the present invention;

[0051] Figure 2 This is a schematic diagram of the structure of the handling robot provided in an embodiment of the present invention;

[0052] Figure 3 A flowchart illustrating a multi-sensor fusion localization method based on extended Kalman filtering, provided in an embodiment of the present invention;

[0053] The figure shows the robot body 100, chassis 101, wheels 102, sensor assembly 103, and processor 104. Detailed Implementation

[0054] The technical solutions of the present invention will be clearly and completely described below with reference to the accompanying drawings of the embodiments of the present invention. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments.

[0055] The term "embodiment" as used herein means that a particular feature, structure, or characteristic described in connection with an embodiment may be included in at least one embodiment of this application. The appearance of this phrase in various places throughout the specification does not necessarily refer to the same embodiment, nor is it a separate or alternative embodiment mutually exclusive with other embodiments. It will be explicitly and implicitly understood by those skilled in the art that the embodiments described herein can be combined with other embodiments.

[0056] In the factory-style production system of smart agriculture, material flow often exhibits typical characteristics of high frequency, small batch, and strong timeliness: seedling trays, nutrient solution tanks, harvesting turnover boxes, packaging pallets, and supporting consumables need to be continuously moved between the seedling area, cultivation area, sorting and packaging area, and cold storage buffer zone.

[0057] To adapt to greenhouse corridors, vertical cultivation racks, narrow work passages, and mixed personnel and equipment organization, handling robots typically adopt sensor configurations with limited external viewing angles and chassis structures close to the ground, achieving continuous positioning and path tracking without adding large-scale infrastructure.

[0058] In actual operation, the interference faced by the positioning system does not come from the insufficient accuracy of a single sensor, but from the coupled changes in operating conditions: the wheel-ground adhesion conditions and dynamic response will be significantly deviated under different humidity, cleanliness and load conditions on the same channel, which will cause the traditional positioning link of "mainly based on wheel speed and odometer, supplemented by external observation and correction" to be distorted in stages.

[0059] The ground and environment of smart agricultural factories exhibit a combination of characteristics drastically different from traditional warehousing: the ground may have damp water films, localized low-adhesion areas formed by nutrient solution drips, as well as drainage ditches, ramp transitions, ground seams, and film edges; the sides of the passageways are often lined with dense cultivation racks or crop walls, and structural obstruction causes the effective field of view for external observation to intermittently degrade; the glass / film structure and metal supports of greenhouses produce strong reflections and repetitive textures, causing pose observations based on vision or laser matching to drift in local areas in a way that appears reasonable but is actually biased; at the same time, handling tasks naturally involve load mutation processes, such as picking up and placing seedling trays, stacking turnover boxes, and moving liquid barrels, etc. The robot's center of mass position and equivalent inertia change in a short period of time, causing the wheel motion error during acceleration, deceleration, and turning phases to transform from an approximately random disturbance into an error term with bias and mutation characteristics.

[0060] The direct consequence of the combined effect of the above factors is that the wheel speed odometer will slip significantly under low adhesion and load changes, and the external observation will be intermittently unstable under obstructed and reflective environments. The errors of the two are often not exposed on the same time scale, which means that the fusion positioning may have cumulative drift or jitter caused by excessive pull-back when the external observation is restored, which further affects the safety and efficiency of the handling task.

[0061] Existing technologies typically employ two engineering approaches to address this type of problem: one is to empirically tune process or observation noise within the extended Kalman filter framework, or to directly eliminate observations to avoid divergence when innovation is too significant; the other is to introduce additional scene-dependent constraints, such as relying on ground markers, QR codes, magnetic nails, or high-density base stations to achieve absolute correction. The former is usable under stable operating conditions, but in the aforementioned typical agricultural transport conditions, the error morphology is not simply amplified Gaussian noise: wheel-related errors may manifest as short-term abrupt changes or persistent biases, while external observation errors may manifest as intermittent deviations rather than outliers. While the latter can improve absolute accuracy, it faces practical constraints in terms of greenhouse or factory modification costs, maintenance costs, and operational flexibility, and it is still difficult to completely avoid observational instability under conditions of partial shading, reflection, and high humidity.

[0062] It is understandable that the multi-sensor fusion positioning approach proposed in this application is not based on adding more external benchmarks, but rather on establishing a more realistic fusion method targeting the most decisive source of error in smart agriculture handling scenarios. The starting point is that, within a short time scale, the displacement / velocity reflected by the wheel speed calculation should be consistent with the dynamic response reflected by the inertial measurement unit. When the attachment drops, the load is disturbed, or a transient impact occurs, an observable inconsistency will appear between the two. This inconsistency is not equivalent to an increase in sensor noise, but rather more like entering different motion conditions.

[0063] Therefore, this embodiment generates a slip indication from motion information during operation to characterize the degree of inconsistency between wheel-based calculations and short-term dynamic responses. Based on this, the robot's motion is divided into at least two preset modes: low slip and high slip, expressing the current working condition as a probability. Considering the large number of critical attachment regions in smart agriculture scenarios, where the slip degree fluctuates frequently around the threshold, this embodiment further introduces overlapping intervals within the mode threshold range. This prevents hard switching at the critical stage and avoids jitter in the fusion output by using continuous weight transitions. Furthermore, the overlapping interval is not given empirically but can be constructed offline using historical motion data under preset calibration scenarios. By extracting the frequency domain energy characteristics of wheel speed and inertial signals to form attachment degradation characteristics, and combining the statistical distribution of the slip indication, the overlapping boundary is determined at the critical attachment level, thus making the threshold setting reproducible and interpretable.

[0064] This embodiment employs a unified state representation using extended Kalman filtering, incorporating both the pose state and the slip extension state (used to characterize the evolution of wheel errors caused by load changes) into the filtered state vector. This prevents slip from being equated to unmodelable random noise, instead treating it as a state variable that can evolve with the operating conditions and participate in prediction and covariance propagation. For each motion mode, prediction updates and observation updates are performed in parallel. Upon obtaining observation information from laser, vision, or positioning devices, the interpretability of each mode to the current observation is evaluated based on observation innovation and its covariance, thereby updating the mode probability. This achieves a closed loop between motion-side prior identification and observation-side consistency correction.

[0065] Furthermore, considering that external observations may experience short-term instability due to greenhouse reflections, shading, and dynamic backgrounds, this embodiment can also scale and adjust the observation noise parameters based on posterior mode probability and comprehensive observation innovation, so that the observation update exhibits a more robust weight allocation during high slip or observation instability phases, thereby improving the stability and usability of the positioning results without sacrificing continuity.

[0066] It should be emphasized that this embodiment does not rely on pre-partitioning modeling of a fixed passage, a fixed ground material, or a fixed crop rack layout within a smart agricultural factory, nor does it require the handling robot to obtain stable external observation throughout the process; application scenarios include, but are not limited to: handling and return of seedling trays between vertical cultivation lines, cross-zone transfer of turnover boxes in sorting and packaging areas, pallet switching and handling between refrigerated buffer zones and ambient temperature zones, and multi-robot collaborative material flow under conditions of slippery ground and narrow passages.

[0067] refer to Figure 1 , Figure 1 This is an exemplary application scenario diagram provided for an embodiment of this application.

[0068] Figure 1 The illustration depicts a transport robot used to carry and transport loads along a transport corridor. These loads can be seedling trays, turnover boxes, packaging containers, or other materials that need to be transferred between different work areas during agricultural production. The transport corridor is a typical work corridor within an agricultural factory, whose width, structure, and ground conditions are limited by the production layout and environmental conditions, typically exhibiting characteristics of narrow corridors, continuous structures, and easily changing ground adhesion conditions.

[0069] Understandably, when performing transport tasks, the handling robot needs to continuously travel along the transport channel and complete pick-up, placement, turning, or path switching operations at predetermined locations. To ensure the safety and efficiency of the transport process, the handling robot needs to continuously acquire its own positional information within the transport channel to facilitate path tracking, motion control, and collaborative operation with other equipment or robots in the working environment. Especially in smart agricultural factories, transport tasks are typically characterized by high frequency and continuity. Single positioning errors or short-term positioning failures can be amplified in subsequent movements, thereby affecting the stability of the overall handling process.

[0070] In the specific embodiment not shown in the figure, the transport load may experience weight changes, center of gravity shifts, or differences in loading status during handling. Furthermore, the ground conditions of the transport corridor may change due to humidity, cleanliness, or work residue. Consequently, the handling robot is prone to changes in wheel-to-ground adhesion during movement, leading to deviations in motion estimation based on wheel mileage. Simultaneously, external observation information may be unstable or less accurate in some sections due to factors such as obstruction by the working corridor structure, environmental reflections, and limited viewing angles. Under the combined effect of these factors, if the handling robot lacks robust positioning capabilities, it is prone to positioning drift, positioning jitter, or discontinuous positioning problems.

[0071] refer to Figure 2 , Figure 2 This is a schematic diagram of the structure of the handling robot provided in an embodiment of this application.

[0072] Figure 2 The robot shown includes a robot body 100, a chassis 101, wheels 102, a sensor assembly 103, and a processor 104, wherein:

[0073] The robot body 100 serves as the overall load-bearing structure of the handling robot, housing and supporting the chassis 101, wheels 102, sensor assembly 103, and processor 104, and bearing the transport load to complete material handling operations. The specific structural form, dimensions, and material composition of the robot body 100 can be set according to actual application requirements, and this application does not limit them.

[0074] The chassis 101 supports the robot body 100 and provides a basic mechanical structure and motion mounting platform for the handling robot. The chassis 101 can be an integral structure or a split structure, and its structural form does not constitute a limitation of this application.

[0075] The walking wheels 102 are installed in the chassis 101 and are used to drive the handling robot to move in the working environment. The walking wheels 102 can be driving wheels or driven wheels, and their number, arrangement and driving form can be configured according to the specific movement requirements of the handling robot. This application does not make specific limitations in this regard.

[0076] The sensor assembly 103 is used to collect information related to the motion state and environmental perception of the handling robot. The sensor assembly 103 is at least used to acquire motion information and observation information. Exemplarily, the motion information may include wheel speed information, angular velocity information, and linear acceleration information, and the observation information may include pose or position information output by a lidar, vision sensor, or positioning device. It should be understood that the sensor assembly 103 may include one or more sensors, and their type, number, and installation method do not constitute a limitation of this application.

[0077] The processor 104 is used to communicate with the sensor assembly 103 and to execute the multi-sensor fusion positioning method described in the embodiments of this application. The processor 104 can be a single processing unit or a combination of multiple processing units, and its specific implementation can be a general-purpose processor, a dedicated processor, or an embedded controller, etc., without affecting the implementation of the technical solution of this application.

[0078] It should be noted that, Figure 2 The illustrated transport robot structure is merely an example to illustrate the application of the multi-sensor fusion positioning method in the embodiments of this application. It is not intended to limit the specific structural composition or component connection relationship of the transport robot. Based on the core concept of this application, those skilled in the art can adjust or replace the structural form of the transport robot.

[0079] Next, combined Figure 3 This paper further describes a multi-sensor fusion localization method based on extended Kalman filtering provided in the embodiments of this application. Figure 3 The method shown is applied to a handling robot, which includes a sensor assembly for collecting motion and observation information of the handling robot. The method includes:

[0080] S1: Based on the motion information, calculate the sliding indication quantity, and determine the mode probability of multiple preset motion modes according to the sliding indication quantity;

[0081] The slip indication is used to characterize the degree of inconsistency between the motion information and the robot's short-term dynamic response;

[0082] In the material handling scenarios of smart agricultural factories, the transport load may change during loading, stacking, or transfer, and the transport channel floor may have dampness, water film, or local low-adhesion areas. Therefore, wheeled motion errors often no longer exhibit stable random disturbance characteristics, but rather manifest as slippage behavior related to the operating conditions. Based on these practical problems, this embodiment does not directly equate slippage with noise amplification. Instead, it quantifies the current motion state through slippage indication and maps the motion state of the handling robot to multiple preset motion modes accordingly.

[0083] S2: Perform extended Kalman filter prediction update for each motion mode to obtain the prediction state and prediction covariance for each motion mode;

[0084] In this embodiment, an extended Kalman filter prediction update process is performed for each motion mode obtained in step S1.

[0085] It is understandable that load changes and ground adhesion states often have a certain degree of persistence, and correcting motion errors only for a short period of time is insufficient to fully reflect the evolution trend of the errors. Therefore, this embodiment introduces slip-related error terms into the state vector, allowing them to participate in the state transition and covariance propagation process, rather than treating them merely as static parameters or noise terms. For different motion modes, different process noise parameters can be used to model the slip extension state to reflect the differences in the evolution rate and uncertainty of wheel-type errors under low-slip and high-slip conditions.

[0086] S3: After obtaining the observation information, for each motion mode, calculate the observation prediction value based on the corresponding prediction state to obtain the observation innovation and innovation covariance, and update the mode probability of the motion mode according to the observation innovation and innovation covariance.

[0087] The innovative covariance is determined by the prediction covariance and the observation noise parameters of the extended Kalman filter.

[0088] In this embodiment, considering factors such as structural obstruction, highly reflective materials, and dynamic changes in crops within the smart agricultural factory, external observation information may exhibit accuracy fluctuations or intermittent degradation in some operating areas. Relying solely on a single observation result to correct the predicted state can easily introduce new positioning errors. Therefore, this embodiment does not simply use the observation result to correct the predicted state, but rather evaluates the consistency between the predicted state and the observation information under each motion mode through observation innovation and its covariance, and uses this consistency result as the basis for updating the motion mode probability.

[0089] S4: Adjust the observation noise parameters according to the updated mode probability and the observation innovation, perform extended Kalman filter observation update for each motion mode, and output the fusion localization result of the handling robot;

[0090] In this embodiment, when the transport robot is in a high-slip condition or a complex observation environment, the external observation information is not completely invalid, but its reliability is relatively reduced. If fixed observation noise is still used for updating, the positioning results are prone to over-correction in a short period of time.

[0091] This embodiment adaptively adjusts the observation noise parameters to enable the extended Kalman filter to exhibit different observation fusion intensities under various operating conditions, thereby improving overall robustness while ensuring positioning continuity. Extended Kalman filter observation updates are performed separately for each motion mode, and the positioning results for each mode are weighted and fused based on the updated motion mode probabilities to obtain the final fused positioning result. Through these steps, stable and reliable pose estimation results can be continuously output for the handling robot under complex agricultural operation conditions such as load variations, attachment state changes, and unstable observations.

[0092] Before delving into the specific technical details of the steps, the embodiments of this application need to be emphasized again.

[0093] For wheeled platforms, wheel speed and inertial information have a natural physical consistency constraint on a short timescale: under the condition that no significant slippage or abrupt change in contact conditions occurs, the displacement increment calculated from wheel speed and the dynamic response derived from inertial measurement should be within a mutually verifiable range. However, once the wheel-ground contact changes from rolling to slip-dominant, this consistency relationship is often disrupted before external observation anomalies occur, and this disruption is directional and persistent, easily accumulating into systematic deviations in subsequent movements. Traditional implementations often equate these deviations to noise amplification or occasional outliers, and then use fixed threshold elimination or global parameter tuning to handle them. However, in the rapidly changing operating sequence, errors do not always appear with the same statistical characteristics: some stages exhibit smooth drift, some stages exhibit instantaneous jumps, and others exhibit bias amplification for specific motion directions (such as acceleration, braking, or steering). If a single noise model or a single filter configuration is used throughout the entire process, there will often be over-reliance on motion prediction or over-response to external observations, resulting in discontinuities or instability in the positioning results.

[0094] This application explicitly identifies the non-negligible differences in operating conditions during operation, enabling them to participate in subsequent filter updates rather than passively absorbing them as sources of error. Crucially, the characterization of the operating state does not rely on external environmental structural partitions, map priors, or fixed site markers, but is built upon repeatable and verifiable constraints within the motion side: the degree of inconsistency between wheel-based estimation and short-term dynamic response can be quantified as a slip indicator, thereby forming a probabilistic representation of the current operating state. This probabilistic representation is not equivalent to scene classification, but rather a basis for selecting the evolutionary law by which the current motion error should be modeled, thus ensuring structural consistency in subsequent filtering processes under the same set of state representations, while allowing different error evolution assumptions to be adopted for different operating states. In other words, this embodiment emphasizes that the error model switches with the operating state, rather than the filter structure switching with the scene, thereby avoiding the introduction of rigid constraints for specific scenarios or specific task paths.

[0095] Taking wheel speed encoders, inertial measurement units (IMUs), and external pose observations (such as laser-matched poses or visual odometry poses) as examples, a common approach in exemplary technologies is as follows: A state vector containing only the robot's pose and velocity is constructed. A motion model is built using wheel speed and the IMU for time updates to obtain the predicted state and prediction covariance. When external pose observations are present, innovation and innovation covariance are calculated according to a fixed observation model. Then, observation updates are completed based on pre-defined observation noise parameters, resulting in a fused pose result under a single filter. To address measurement anomalies, some implementations set innovation thresholds (e.g., discarding an observation when the innovation amplitude or its normalized metric exceeds a threshold), or empirically increase process noise / observation noise to reduce the probability of divergence.

[0096] Those skilled in the art will understand that the above scheme can obtain usable positioning output under the conditions of stable working conditions, good ground adhesion, and continuous and reliable external observation.

[0097] The method in this application differs substantially in its filtering organization and error modeling path, at least in the following aspects:

[0098] First, this application does not treat wheel error as merely a noise term, but introduces a slip extension state in the unified state vector to characterize the evolution of wheel motion error caused by load changes. This allows slip error to participate in time updates and covariance propagation in the form of state variables, thereby enabling the formation of a propagable estimate for bias / abrupt error, rather than passively absorbing it through noise amplification.

[0099] Secondly, this application does not use a single filter configuration throughout the entire process. Instead, it constructs a slip indicator based on motion information and determines the mode probabilities of at least several motion modes, such as low slip and high slip. For each motion mode, it performs extended Kalman filter prediction updates and observation updates. After obtaining the observation information, it uses observation innovation and innovation covariance to evaluate the consistency of each mode and updates the mode probabilities using likelihood weights. This makes the judgment of the running state subject to dual constraints from both the motion side and the observation side.

[0100] Furthermore, this application sets an overlapping interval within the mode threshold range and allows the overlapping interval boundary to be constructed offline through the frequency domain attachment degradation characteristics of historical calibration data and the statistical distribution of the slip indication, so that the mode probability transition in the critical slip stage is smooth and reproducible, avoiding the filter jitter caused by frequent switching near the threshold in the prior art.

[0101] Finally, this application further scales and adjusts the observation noise parameters based on posterior mode probability and integrated observation innovation, so that the observation update exhibits adaptive fusion intensity under external observation degradation or slip conditions, thereby achieving a more reasonable trade-off between positioning continuity and stability.

[0102] In one example, the motion information described in this application includes wheel speed information collected by a wheel speed encoder and angular velocity and linear acceleration information collected by an inertial measurement unit. The observation information includes pose observation information output by a lidar positioning module, pose observation information output by a visual positioning module, and position observation information output by a positioning device. The method further includes preprocessing the raw data collected by the sensor components to obtain the motion information and the observation information. The preprocessing includes time alignment based on timestamps, coordinate system transformation, and removal of abnormal data.

[0103] It should be noted that the types of motion information, the sources of observation information, and the corresponding preprocessing methods mentioned above are all commonly used implementation methods in the field of multi-sensor fusion positioning. Those skilled in the art can configure and implement the relevant sensor types, data formats, and preprocessing processes according to specific application scenarios. The technical principles and implementation methods are all existing technical content, and this application will not elaborate further on them here.

[0104] Next, we will further elaborate on the technical aspects of the unified state vector in this application.

[0105] In one example, the method further includes constructing a unified state vector of an extended Kalman filter and initializing the state covariance. The unified state vector includes at least a pose state characterizing the pose of the transport robot and a slip extended state characterizing the wheel motion error of the load change. The initial value of the pose state is determined based on a preset initial pose and an initial pose obtained from the observation information. The initial value of the slip extended state is set to a preset nominal value. The state covariance is initialized as a diagonal matrix composed of the initial variances corresponding to each state component of the unified state vector, and the initial variance corresponding to the preset nominal value is greater than or equal to the initial variance corresponding to the pose state.

[0106] Understandably, the construction of a unified state vector aims to provide a consistent estimation carrier for multi-source motion and observation information, enabling the geometric state related to pose and the operational state related to wheel motion errors to evolve collaboratively within the same filtering framework. Unlike conventional state vectors that only contain pose or velocity states, this embodiment explicitly introduces a slip extension state in the state space to characterize the evolution of wheel motion errors. This allows the influence of factors such as load changes and attachment state changes on the motion estimation results to participate in the prediction and update process in the form of state variables. In specific implementations, the pose state can include at least position and attitude components, and its dimension can be determined according to the motion model adopted. The slip extension state is used to characterize the systematic deviations that evolve over time in wheel odometer estimation, and its physical meaning can correspond to proportional error, bias error, or equivalent slip factor.

[0107] Specifically, regarding the initial state setting, the initial value of the pose state does not solely rely on a single information source, but is determined by combining a preset initial pose with the initial pose obtained from observation information. Specifically, during the robot's startup or repositioning phase, if a preset initial pose exists based on manual settings, task planning, or historical records, it can be used as a reference for the initial pose value. Simultaneously, the observed pose output by the LiDAR positioning module, visual positioning module, or other positioning devices during the startup phase is used to correct or verify the preset initial pose, thereby reducing the impact of initial positioning errors on subsequent estimation processes. In engineering implementation, the initial pose from the observation information can be directly selected as the initial pose state value, or a weighted method can be used to fuse the preset initial pose and the observed initial pose. This embodiment does not limit this, as long as a reasonable and consistent initial value for the pose state can be provided at the initial filtering time.

[0108] Furthermore, for the slip extension state, since it is used to describe the evolution trend of wheel motion error, and there is usually insufficient information to accurately estimate this error in the initial stage of filtering, this embodiment sets its initial value to a preset nominal value. This nominal value can correspond to the ideal state under conditions of no significant slip or nominal attachment, such as a scaling factor of 1, an offset of 0, or other equivalent representations. Its specific numerical form can be determined according to the modeling method of the slip extension state.

[0109] Those skilled in the art will understand that this initial value is not required to accurately reflect the true error, but rather serves as a neutral assumption when the filter is started, to be gradually corrected in conjunction with subsequent time updates and observation updates.

[0110] Regarding the initialization of state covariance, this embodiment uses a diagonal matrix constructed from the initial variances corresponding one-to-one with each state component of the unified state vector to reflect the uncertainty level of each state component at the initial moment. Specifically, the initial variance corresponding to the pose state can be set according to the preset confidence level of the initial pose and the accuracy of the initial observation information. For example, in the case of high-confidence external positioning observations, a smaller initial variance can be selected. However, for the slip extension state, since its initial value is only a nominal assumption and the actual error level has significant uncertainty, its corresponding initial variance is set to be greater than or equal to the initial variance corresponding to the pose state.

[0111] Next, we will further elaborate on the technical content of the method of this application regarding the sliding indication quantity.

[0112] In this embodiment, the construction of the slip indication is based on the following basic understanding: within a short time scale, when the wheeled handling robot is in a normal attachment state, the motion result calculated by the wheel speed encoder should be consistent with the short-time dynamic response reflected by the inertial measurement unit. The two have a mutually verifiable physical constraint relationship in terms of speed change trend and displacement increment. However, when the wheel-ground attachment conditions deteriorate or the load change causes slip behavior, this consistency relationship will be broken, and this break is often first reflected in the deviation between short-time motion quantities.

[0113] In the specific implementation, firstly, based on the wheel speed information collected by the wheel speed encoder and combined with the wheel kinematic model of the handling robot, the corresponding linear velocity change and displacement increment are calculated within a preset short time window to form the first short-term motion quantity. The length of this time window can be set according to the robot control cycle or sensor sampling frequency. Those skilled in the art will understand that the time window should not be too long to avoid the accumulation of integral drift, nor too short to avoid the influence of instantaneous noise; it only needs to be able to characterize the short-term motion trend. At the same time, based on the linear acceleration and angular velocity information collected by the inertial measurement unit, the linear acceleration is integrated within the same time window to obtain the velocity change, and the angular velocity is integrated to reflect the influence of attitude change on the motion direction, thereby obtaining the second short-term motion quantity. Since the inertial calculation has a good response capability to dynamic changes in a short time, this second short-term motion quantity can better reflect the transient characteristics of the actual motion.

[0114] Furthermore, the first short-term motion quantity is compared with the second short-term motion quantity, and the difference is used as the original inconsistency quantity to characterize the degree of deviation between the wheel mileage estimation result and the inertial estimation result. It should be noted that this difference can be calculated in the velocity domain, the displacement domain, or a combination of both in the state space. This embodiment does not limit this, as long as it can reflect the inconsistency between the two types of estimation results on a short time scale. To avoid the problem of inconsistent dimensions or scale imbalance of the difference under different motion amplitude conditions, this embodiment further normalizes the original inconsistency quantity based on the amplitude of the second short-term motion quantity, so that the slip indication quantities obtained in low-speed and high-speed operation stages and under different travel directions are comparable.

[0115] Furthermore, considering that factors such as sensor noise, minor ground unevenness, and actuator vibration may introduce high-frequency disturbances, this embodiment performs low-pass filtering on the normalized inconsistency quantity to suppress the influence of instantaneous noise on slip judgment. The result after low-pass filtering is used as the final slip indication quantity to reflect the degree of consistency disruption between wheel motion and dynamic response at the current moment.

[0116] Next, we will further elaborate on the technical aspects of the pattern probability in this application.

[0117] In some optional embodiments, the motion patterns in this application are not used to classify work scenarios or environment types, but rather to characterize the subjective behavioral characteristics followed by the wheeled motion error of the handling robot at the current moment. In other words, the motion patterns reflect "what assumptions should be used to model and propagate the wheeled motion error under the current operating state," rather than direct labeling of ground type, load type, or task type. Those skilled in the art will understand that this type of motion pattern is an abstract expression of the operating state, and its purpose is to provide candidate interpretation paths under different error evolution assumptions for the filtering process.

[0118] In this specific implementation, the motion mode is divided into at least a low-slip mode and a high-slip mode. The low-slip mode represents a relatively stable operating state with stable wheel-to-ground adhesion. In this state, the wheel speed encoder's calculation results and the inertial calculation results maintain a high degree of consistency over a short time scale, and the wheel motion error mainly manifests as small random disturbances with relatively stable statistical characteristics. The high-slip mode represents an operating state where the wheel-to-ground adhesion deteriorates significantly or the load change has a significant impact on the motion calculation. In this state, the inconsistency between the wheel speed calculation results and the inertial calculation results increases significantly, and the wheel motion error exhibits characteristics of increased bias or abrupt amplification.

[0119] It should be noted that the above two types of motion modes do not exhaust all possible operating states, but rather abstract the two most representative error-dominant forms in actual operation. This embodiment can meet the requirement of improving positioning robustness by at least adopting the above classification method.

[0120] It is important to emphasize that this embodiment does not assume that the motion patterns exhibit discrete jumps in time. Instead, it continuously characterizes the credibility of different motion patterns through pattern probabilities. The pattern probability is used to characterize the relative rationality of each motion pattern as an error evolution hypothesis at the current moment. Its value is not directly equivalent to the judgment result of whether slippage has occurred, but serves as an important basis for weight allocation in subsequent prediction and observation updates.

[0121] In one example, the mode probabilities of multiple preset motion patterns are determined based on the slip indication, including:

[0122] S1.1: A preset mode set including multiple motion modes is provided. The mode set includes at least a low-slip mode and a high-slip mode. A corresponding slip threshold range is set for each motion mode. The low-slip mode indicates that the handling robot is in a stable wheel-to-ground attachment state, and the high-slip mode indicates that the handling robot is in an unstable wheel-to-ground attachment state. The slip threshold range includes an overlapping range, which is the overlapping part of the slip threshold range of the low-slip mode and the slip threshold range of the high-slip mode.

[0123] Specifically, during the continuous operation of a wheeled transport robot, the wheel-ground contact state does not always occur in a completely normal or completely slippery manner. More commonly, it exhibits inconsistent fluctuations caused by short-term adhesion decreases, load changes, or drive disturbances. If a single threshold is used to rigidly divide the slip indication into two states, the natural fluctuations of the slip indication near the threshold will cause the motion mode to switch frequently between adjacent time points, resulting in jumps in subsequent prediction updates and noise configurations based on the motion mode. Based on this objective law, this embodiment adopts a threshold interval expression method that includes overlapping intervals, allowing low slip mode and high slip mode to simultaneously exist within the critical range and maintain a continuous transition. This makes the mode discrimination and probability update more consistent with the gradual characteristics of wheel-ground adhesion changes.

[0124] In one example, the steps for constructing the overlapping intervals include:

[0125] Acquire historical motion data of the handling robot in a preset calibration scenario. The historical motion data includes at least the wheel speed signal output by the wheel speed encoder, the linear acceleration signal output by the inertial measurement unit, and the angular velocity signal.

[0126] Frequency domain analysis of wheel speed signal line, acceleration signal and angular velocity signal is performed using a sliding time window, and energy characteristics within a preset frequency band are extracted as adhesion degradation characteristics.

[0127] Based on the historical motion data, the historical slip indication for the corresponding time window is calculated, and the historical slip indication is statistically correlated with the adhesion degradation characteristics to obtain the distribution parameters of the slip indication under different adhesion degradation levels.

[0128] The slip threshold ranges for the low slip mode and the high slip mode are determined based on the distribution parameters, and an overlapping range is determined through a critical partitioning strategy. The critical partitioning strategy includes: dividing the adhesion degradation features into at least two distinct adhesion degradation levels, including a critical adhesion degradation level; obtaining preset quantile thresholds for historical slip indicator samples belonging to the critical adhesion degradation level in the distributions corresponding to the low slip mode and the high slip mode, respectively; using the upper quantile threshold of the low slip mode as the upper bound of the low slip mode threshold range; using the lower quantile threshold of the high slip mode as the lower bound of the high slip mode threshold range; and using the overlap between the upper and lower bounds as the overlapping range.

[0129] It is understandable that in scenarios where both wheel speed encoders and inertial measurement units are involved in motion estimation, adhesion degradation typically manifests as increased wheel speed fluctuations, increased high-frequency acceleration components, or amplified angular velocity disturbances within specific frequency bands. Therefore, this embodiment first collects historical motion data covering different adhesion conditions under a preset calibration scenario, ensuring that the wheel speed signal, linear acceleration signal, and angular velocity signal can fully reflect the entire process of transitioning from stable adhesion to unstable adhesion. The calibration scenario can include different ground materials, different load conditions, and different acceleration and deceleration conditions. Those skilled in the art will understand that it is sufficient to ensure that the collected data includes typical processes of adhesion state changes; this application does not further limit the specific form of the calibration scenario.

[0130] In the specific processing, this embodiment analyzes historical motion data using a sliding time window approach, allowing signal characteristics to be locally characterized over time rather than being globally averaged. For each time window, frequency domain analysis is performed on wheel speed, linear acceleration, and angular velocity signals, and energy features are extracted within a preset frequency band as adhesion degradation features. This preset frequency band is not used to describe specific vibration modes, but rather to capture energy redistribution phenomena commonly observed in motion signals due to changes in wheel-ground adhesion, such as increased non-stationary components in wheel speed signals or a rise in the proportion of high-frequency disturbances in inertial signals.

[0131] Furthermore, after completing the sample construction, this embodiment does not directly classify the slip indication based on a single threshold. Instead, it characterizes the distribution behavior of the slip indication under different adhesion degradation conditions by dividing the adhesion degradation characteristics into multiple adhesion degradation levels. Especially in the transition stage from stable to unstable adhesion, the corresponding adhesion degradation characteristics are at an intermediate level, and the slip indication distribution often exhibits the characteristic of simultaneously covering both low slip and high slip intervals. Based on this observation, this embodiment defines this transition stage as the critical adhesion degradation level and separately calculates the quantile thresholds of historical slip indication samples belonging to this level in the distributions corresponding to the low slip mode and the high slip mode. By selecting the upper quantile threshold of the low slip distribution and the lower quantile threshold of the high slip distribution as the threshold interval boundaries, the threshold setting can reflect the maximum range of low slip that can still be reasonably interpreted under critical adhesion conditions, and the minimum range that has already exhibited high slip characteristics under critical adhesion conditions. When the above two types of thresholds overlap numerically, the overlapping part is determined as the overlapping interval.

[0132] S1.2: Match the slip indication with the slip threshold range of each motion mode to obtain the original weight of each motion mode;

[0133] In this embodiment, when matching the slide indicator with the threshold range of each motion mode, the original weights are generated as follows: when the slide indicator falls into the non-overlapping part of the low slide mode, the original weight of the low slide mode is set to a preset larger value, and the original weight of the high slide mode is set to a preset smaller value or zero; when the slide indicator falls into the non-overlapping part of the high slide mode, the original weights of the two modes are set in reverse; when the slide indicator falls into the overlapping range, a weight allocation rule that monotonically changes with the slide indicator is adopted, so that the original weight of the low slide mode decreases as the slide indicator increases, while the original weight of the high slide mode increases as the slide indicator increases.

[0134] The above monotonic variation rule can be implemented by linear interpolation, piecewise interpolation or other continuous mapping methods. Those skilled in the art will understand that it is only necessary to ensure that the original weights change continuously within the overlapping interval and that the original weights of the two modes maintain the same comparable dimension.

[0135] To improve noise immunity, the sliding indicator can be smoothed within the sliding time window before participating in matching, so as to avoid instantaneous shift of the original weights caused by single sampling anomalies.

[0136] S1.3: Normalize the original weights of each motion pattern to obtain the pattern probability of the motion pattern.

[0137] For example, the following numerical example is provided to illustrate how to obtain the original weights. This example is only used to explain the relationship between the calculation link and the dimensions. The selected parameters and values ​​are illustrative and do not represent the actual calibration results or engineering recommendations.

[0138] In this example, the unified state vector is a combination of planar pose and slip extension state. The state components include: position x, y (unit: meters), heading angle θ (unit: radians), and slip extension state S (dimensionless, which can be understood as an equivalent scaling factor for wheel mileage increments). The time step is 0.1 seconds.

[0139] The initial state at time k is taken as: The initial state covariance is illustrated using a diagonal matrix (example only).

[0140] In this example, two motion modes are set: low slip mode and high slip mode, and the slip threshold range is preset to include an overlapping range, as shown in the following settings: Low slip mode threshold range: 0.00, 0.60; High slip mode threshold range: 0.40, 1.00; Overlapping range: 0.40, 0.60 (the overlapping part of the two).

[0141] The example of observation information uses external pose observation (which can correspond to lidar positioning or visual positioning output). The observation noise parameters are set as follows: standard deviation of position observation noise is 0.10 m (variance 0.010), and standard deviation of heading observation noise is 0.05 rad (variance 0.0025).

[0142] During the period from time k to k+1, the motion information collected by the sensor components is as follows: the short-term displacement increment calculated from the wheel speed is approximately 0.100 m (which can be understood as the wheel speed being approximately 1.0 m / s, integrated over 0.1 s); the displacement increment calculated from the short-term dynamic response given by the inertial measurement unit is approximately 0.070 m (illustrated to indicate that the short-term effective displacement obtained by inertial integration is even smaller); the heading increment obtained by the angular velocity integration of the inertial measurement unit is approximately 0.005 rad.

[0143] An example of external observation information obtained at time k+1 is as follows:

[0144] Observation location Observe the heading .

[0145] In this example, the original inconsistency is taken as the difference between the wheel-type short-time displacement increment and the inertial short-time displacement increment, i.e., 0.03m. Normalization uses the amplitude of the second short-time motion (here, the inertial calculated displacement increment) as the scale, approximately 0.429. A low-pass filter is used to suppress instantaneous fluctuations. For demonstration purposes, it is assumed that the filtered output is close to the normalized inconsistency, resulting in a slip indication of 0.5.

[0146] Since the slip indicator is located within the overlapping interval, this example uses a continuous matching method that is easy to understand: the left end of the overlapping interval is biased towards the low slip mode, and the right end is biased towards the high slip mode. That is, when the slip indicator is 0.40, the original weight of low slip is 1 and the original weight of high slip is 0. When the slip indicator is 0.60, the original weight of low slip is 0 and the original weight of high slip is 1. When it is at the midpoint of the interval, the original weights of low slip and high slip are both 0.5.

[0147] Next, we will further elaborate on the technical content of the method in this application regarding predictive updates.

[0148] In one example, the extended Kalman filter prediction update for each motion mode, to obtain the prediction state and prediction covariance for each motion mode, includes:

[0149] S2.1: Determine a nonlinear state transition function for each motion mode, including a slip extension state, wherein the slip extension state is updated according to the slip indication quantity;

[0150] Specifically, during the time update phase of a wheeled platform, the evolution of its pose state is typically driven by both wheel speed estimation and inertial information. However, when wheel-ground adhesion conditions fluctuate, the wheel speed estimation error exhibits a bias or proportional deviation that varies with the operating conditions. If this type of error is still treated as fixed noise, it can only be removed by increasing process noise, making it difficult to form a sustainable compensation at the state level. Therefore, a slip extension state is introduced into the prediction model, allowing wheel errors to participate in the evolution as state variables. In engineering implementation, this enables changes in the reliability of motion estimation to be reflected through state and covariance propagation, rather than simply remaining a passive correction at the observation update stage.

[0151] In this embodiment, for each motion mode, a nonlinear state transition function consistent with the unified state vector is constructed. The nonlinear state transition function includes at least the kinematic update relationship of the pose state and the evolution relationship of the slip extension state.

[0152] The attitude state update is achieved by combining wheel kinematics and inertial constraints: within a control cycle, the linear velocity and angular velocity components calculated by the wheel are determined using wheel speed information, the angular velocity of the inertial measurement unit is used to constrain the heading change, and the integral result of linear acceleration is compensated for short time when necessary to obtain the attitude increment within the cycle; this attitude increment is used to update the position and attitude during state transition.

[0153] The update of the slip extension state is achieved by a state evolution method modulated by the slip indicator: when the slip indicator is in the low value range, the slip extension state is maintained near the nominal value and allowed to change slowly; when the slip indicator enters the high value range or the overlapping range is close to the high slip side, the sensitivity of the slip extension state is increased so that it can reflect the offset trend of the wheel error more quickly.

[0154] It should be noted that the nonlinear state transition function can be understood as: within a discrete time step, the mapping relationship that advances the unified state vector of the previous moment to the next moment under the combined action of wheel kinematic constraints and inertial dynamic constraints. It is used to describe both the temporal evolution of the pose state and the evolution law of the slip extension state under different motion modes. Furthermore, the slip extension state enters the pose update process through scaling or bias compensation of the wheel odometer extrapolation increment.

[0155] In one optional specific implementation, the unified state vector is expressed in planar pose and includes position components, heading angle components, and slip extended state components.

[0156] The pose update part of the nonlinear state transition function is implemented according to differential wheel kinematics: the linear velocities of the left and right wheels are calculated from the wheel speed information of the wheel speed encoder, and then the forward velocity and angular velocity at the center of gravity of the chassis are obtained; within one control cycle, the forward velocity is projected onto two orthogonal directions of the world coordinate system according to the heading angle at the previous moment to obtain the position increment, and the heading angle increment is obtained by integrating the angular velocity, thereby updating the position and heading angle.

[0157] The slip extension state is entered into the pose update in the form of a scaling factor: the forward velocity or the displacement increment obtained therefrom is multiplied by the scaling factor corresponding to the slip extension state, so that when the slip extension state deviates from the nominal value, the contribution of wheel odometer calculation to the pose update is automatically adjusted to express the effective travel reduction or offset drift caused by changes in wheel-ground adhesion.

[0158] In another optional implementation, the slip extension state is introduced into the pose update in the form of a bias term: a bias compensation related to the slip extension state is superimposed on the forward velocity or displacement increment calculated from the wheel speed, so that the pose update process can absorb systematic errors introduced by load changes, equivalent wheel diameter changes, or local slippage. Correspondingly, the evolution of the slip extension state is implemented using a random walk modulated by the slip indicator: when the slip indicator is in a low value range, the change of the slip extension state is restricted to a small range and maintained with slow drift; when the slip indicator is in a high value range or falls into an overlapping range near the high slip side, the evolution intensity of the slip extension state is increased, so that it can converge to a new bias level or proportional level in a short time. The modulation of the evolution intensity can be achieved by segmenting the slip indicator, that is, using different update step sizes or different process noise intensities in different intervals, so that the state transition functions corresponding to the low slip mode and the high slip mode exhibit different response characteristics in the slip extension state update term.

[0159] S2.2: Substitute the motion information into the nonlinear state transition function to update the unified state vector of the previous moment in time, and obtain the predicted state under the corresponding motion mode;

[0160] Specifically, the predicted state provided during the time update phase serves as a priori benchmark for subsequent observation updates, and the rationality of the predicted state determines whether the amount of innovation can truly reflect the consistency of observations. In wheeled transport scenarios, load changes and ground adhesion changes often occur within short periods of acceleration, deceleration, turning, or passing through ground transition zones. If the predicted state still uses a single model output, it is easy for the predicted trajectory to deviate and then be forcibly pulled back when the observation arrives, resulting in trajectory jitter or local discontinuities. Substituting the motion information at the same moment into the state transition functions corresponding to different motion modes allows each mode to provide prediction results that conform to its own error assumptions, providing differentiated candidate interpretations for subsequent mode probability updates.

[0161] In this embodiment, when updating the unified state vector of the previous moment, wheel speed information and inertial measurement information are read according to a preset sampling period, and after completing the necessary time alignment, they are used as inputs to the state transition function.

[0162] For the pose state part, the planar displacement and heading change increments for that period are calculated based on wheel kinematics, and the heading change is corrected by combining the inertial angular velocity, making the prediction process traceable to short-term steering responses. For the slip extension state part, an appropriate evolution strategy is selected according to the interval in which the slip indication is located: in the low slip mode, a strong holding constraint is imposed on the slip extension state, causing it to change slowly with process noise only within a small range; in the high slip mode, the slip extension state is allowed to respond to changes in the slip indication over a larger range, reflecting the possibility of wheel error evolving from random disturbances to biases or abrupt changes. The resulting predicted state is not a repeated calculation of the same input, but rather provides different prior expressions of how wheel error affects pose increments under different mode assumptions, making the predicted states of each mode exhibit distinguishable trajectory deviations and uncertainty levels when slip occurs.

[0163] S2.3: Perform first-order linearization on the nonlinear state transition function at the unified state vector of the previous time step to obtain the corresponding state transition Jacobian matrix;

[0164] Specifically, since the state transition function includes the kinematic relationship of pose and the influence of the slip extension state on the pose increment, it is a nonlinear mapping as a whole. The propagation of the state covariance needs to be linearized to obtain a locally approximate state transition relationship. If the coupling term between the slip extension state and the pose state is ignored, the covariance propagation will not be able to reflect "how the uncertainty of slip error is transmitted to the uncertainty of pose", and the subsequent calculation of the innovative covariance will also lose its sensitivity to the differences in working conditions.

[0165] In this embodiment, the state transition Jacobian matrix is ​​calculated using the unified state vector of the previous moment as the linearization point, and the local sensitivity between each state component is obtained based on the composition of the state transition function. For the pose state part, the linearization result reflects the dependence of the pose increment on the attitude, velocity, or related state components of the previous moment; for the slip extension state part, the linearization result reflects the degree of influence of the slip extension state on the displacement increment or velocity increment, and this influence is reflected in the Jacobian matrix through the corresponding partial derivative terms. Since different motion modes have different evolution strengths and constraints on the slip extension state, the state transition functions corresponding to each mode differ in the slip-related terms, and the Jacobian matrix obtained by linearization also varies with the mode, so that the covariance propagation can reflect the different assumptions about the evolution of wheel error under different modes.

[0166] S2.4: Based on the state transition Jacobian matrix, the process noise parameters corresponding to the motion mode, and the state covariance of the previous moment, the state covariance is propagated and updated to obtain the predicted covariance corresponding to the motion mode; wherein, the slip extension state participates in the time update of the nonlinear state transition function and the propagation update of the state covariance, and the process noise parameters corresponding to different motion modes are adjusted according to the slip extension state;

[0167] Specifically, the prediction covariance is used in filtering to characterize the uncertainty level of the predicted state, determining the magnitude of the Kalman gain and the scale of the innovation covariance in subsequent observation updates. Under the complex operating conditions of wheeled platforms, the main sources of process noise include not only sensor measurement noise but also unmodeled dynamics caused by changes in wheel-ground adhesion and load, as well as error terms resulting from wheel kinematic simplification. If the process noise is always configured in a fixed way, it will lead to an overly large prediction covariance at low slip, resulting in excessive reliance on external observations for observation updates; or an underly small prediction covariance at high slip, leading to over-reliance on prediction results. Correlating process noise with the slip extension state allows the process noise to adjust according to the degree of wheel error evolution, which helps to keep the prediction covariance consistent with the actual operating conditions.

[0168] In this embodiment, during covariance propagation and update, the state covariance of the previous time step is propagated to the current time step based on the state transition Jacobian matrix, and the process noise parameters of the corresponding motion mode are superimposed to form the predicted covariance.

[0169] The process noise parameters are configured using a multi-mode setting method: in low slip mode, the wheel error related component in the process noise is set to a smaller range, so that the prediction covariance mainly reflects the random disturbance under normal rolling conditions; in high slip mode, the wheel error related component in the process noise is set to a larger range, so that the prediction covariance can cover the pose drift risk caused by slip.

[0170] Furthermore, the process noise parameters are not statically selected, but are adjusted in conjunction with the slip extension state: when the slip extension state deviates from the nominal value and continues to increase, the intensity of the process noise related to the wheel error is increased, so that the contribution of covariance propagation to slip uncertainty is increased; when the slip extension state returns to near the nominal value, the intensity of the corresponding process noise is reduced, so that the predicted covariance gradually converges to the level under stable operating conditions.

[0171] This adjustment method ensures that the change in predicted covariance is consistent with the evolution of the state itself, avoiding covariance jitter caused by directly amplifying noise based solely on the instantaneous value of the sliding indicator.

[0172] Next, we will further elaborate on the technical aspects of the observation update method in this application.

[0173] Understandably, during the continuous positioning process of a wheeled transport robot, observation updates are not simply superimposed on the predicted state from external observations; rather, they serve a dual purpose: verifying and correcting the rationality of the prediction model. Since the aforementioned prediction update stage has already provided differentiated predicted states and prediction covariances under different motion modes, these prediction results inherently contain different assumptions about the current wheel-ground adhesion state and the evolution of wheel errors. Therefore, the observation update stage needs to evaluate the prediction results for each motion mode separately while maintaining filtering consistency, rather than mixing or directly merging all prediction results. Otherwise, the observation information will lose its ability to discern "which motion mode is more consistent with the current operating state," and the motion mode probability will be difficult to effectively correct.

[0174] In this embodiment, the observation update uses a mode-by-mode consistency evaluation as its basic processing logic. For each motion mode, based on the predicted state under that mode, the predicted observation value is calculated using the corresponding nonlinear observation function, making the predicted state a comparable reference quantity in the observation space. The nonlinear observation function is used to describe the mapping relationship between the pose component in the state vector and the observation information. Its specific form can be determined according to the type of observation information. For example, when the observation information is a pose observation, the observation function can directly extract the position and attitude components in the predicted state; when the observation information is a position observation, only the position-related components are extracted.

[0175] Those skilled in the art will understand that the construction of the observation function follows conventional methods of existing multi-sensor fusion localization, and this application does not impose additional limitations on its specific form. By comparing the observation information with the observation prediction value, the observation innovation in the corresponding motion mode is obtained, which reflects the degree to which the prediction result of the mode explains the current observation.

[0176] Furthermore, while calculating the observed innovation, the observation function is linearized to the first order at the predicted state to obtain the corresponding observation Jacobian matrix, and the innovation covariance is calculated by combining the prediction covariance and the observation noise parameter.

[0177] In this embodiment, the innovation covariance is used not only for the subsequent calculation of Kalman gain, but also for scale constraint on observation innovation, so that observation innovation under different motion modes is comparable.

[0178] Because the prediction covariance differs across different motion modes, the same observation information may produce normalized innovations of varying magnitudes under different modes, thus reflecting "under which motion mode assumption the observation is more reasonable." Based on this, this embodiment utilizes observation innovation and its innovation covariance to construct an observation consistency metric, enabling this metric to simultaneously reflect the combined impact of prediction uncertainty and observation bias.

[0179] Furthermore, based on the observation consistency measure, the pattern probabilities of each motion mode are updated so that the pattern probabilities not only originate from prior judgments based on motion information but are also subject to posterior corrections constrained by observation information. When the predicted state under a certain motion mode can well explain the current observation information, its corresponding observational innovation is smaller and its innovation covariance matching degree is higher, thus gaining greater weight in the pattern probability update; conversely, when the predicted result under a certain motion mode deviates significantly from the observation information, its pattern probability will be suppressed accordingly. Through this update mechanism, motion mode probabilities gradually converge towards modes that "simultaneously meet the constraints of both the motion side and the observation side," rather than making judgments based solely on a single information source.

[0180] It should be noted that the observation update does not assume that external observation information is always reliable, but rather allows the observation consistency evaluation results to fluctuate over different time periods. When the quality of observation information declines, the overall observation innovation in each motion model may increase, but because the prediction covariance and innovation covariance are both involved in the evaluation, the model probability update can still remain relatively smooth and will not experience drastic jumps due to a single observation anomaly.

[0181] It should be emphasized that after calculating the updated pattern probability and the observation innovation, the positioning method of this application does not immediately inject the observation information into the prediction state with fixed weights, but enters an observation update stage based on adaptive adjustment of the running state.

[0182] Understandably, the model probabilities obtained at this point comprehensively reflect the joint judgment of the current operating state by both the motion information side and the observation information side. They not only characterize the relative reliability of different motion modes but also indirectly reflect the level of consistency between the current prediction model and external observations. Based on this, if fixed observation noise parameters are still used for updates, unnecessary state correction biases are easily introduced when the operating state changes or observation quality fluctuates, thereby weakening the discriminative significance brought about by the aforementioned model probability updates.

[0183] In this embodiment, the updated mode probability and observation innovation are further used to adjust the observation noise parameter, enabling the observation update process to adaptively adjust to changes in operating status and observation consistency. When the mode probability of high-slip mode increases, or when the comprehensive observation innovation reflects a large deviation between the predicted state and the observation information, the observation noise parameter is increased to suppress the correction magnitude of the predicted state during this stage, thereby avoiding excessive pose correction when the wheel error has not yet stabilized or the observation information is uncertain. Conversely, when the mode probability of low-slip mode dominates and the observation innovation remains within a reasonable range, the observation noise parameter is reduced to allow the observation information to participate more fully in the state update, thereby improving positioning accuracy.

[0184] The above adjustment is not a switch-on control of a single observation channel, but rather a scaling of observation noise in a continuous numerical space, so that the intensity of observation updates changes smoothly with the model probability and innovation level.

[0185] Furthermore, after adjusting the observation noise parameters, extended Kalman filtering observation updates are performed for each motion mode. After the updates are completed, the updated states are weighted and fused based on the posterior mode probabilities of each motion mode to obtain the final fused localization result. In this way, the localization result is not derived from the estimation of a single motion mode, but is determined probabilistically by the estimation results of multiple modes. This allows the output pose to simultaneously inherit the stability of the low-slip mode and the risk coverage capability of the high-slip mode. This processing logic enables the localization method to maintain the continuity and rationality of the output even under complex conditions such as load changes, attachment condition changes, and observation quality fluctuations, providing a reliable pose input basis for subsequent motion control and task execution.

[0186] In one example, adjusting the observation noise parameter based on the updated mode probability and the observation innovation includes:

[0187] S4.1: Based on the posterior mode probability, determine the comprehensive observation innovation weighted by mode probability, wherein the comprehensive observation innovation is obtained by weighting the observation innovation corresponding to each motion mode with the corresponding posterior mode probability;

[0188] Specifically, observational innovation is the deviation between observed information and predicted values ​​under each motion mode assumption. It includes the influence of both observation and prediction errors. When in the critical stage where low slip and high slip coexist, the observational innovations of different motion modes are often inconsistent. If the observational innovation of a certain mode is directly selected as the basis for subsequent noise adjustment, it is easy to mistake "differences in mode assumptions" for "changes in observation quality," thus leading to instability in the direction of observational noise adjustment.

[0189] By weighting and summing the observational innovations from various models according to their posterior model probabilities, the interpretative power of different models for the same observation can be incorporated into the same scale, making subsequent noise adjustments more consistent with the comprehensive judgment of the current operating status, rather than relying on the accidental results of a single model.

[0190] In this embodiment, the construction of comprehensive observation innovation is weighted by the posterior mode probabilities obtained in the claims, and the observation innovations corresponding to each motion mode are weighted and superimposed.

[0191] To facilitate implementation, the observation innovations of each motion mode maintain the same dimension and the same observation definition before participating in the weighting. For example, when the observation information is a pose observation, the observation innovation includes the deviation components of position and heading; when the observation information is a position observation, it only includes the position deviation component. During the weighting process, the observation innovation of each motion mode is multiplied by the corresponding posterior mode probability and then summed to obtain the comprehensive observation innovation.

[0192] S4.2: Calculate the observation noise scaling factor based on the comprehensive observation innovation, and scale the observation noise parameter according to the observation noise scaling factor to obtain the adjusted observation noise parameter;

[0193] Specifically, the observation noise parameter is used to characterize the uncertainty level of observation information and plays a crucial role in the calculation of innovation covariance and Kalman gain. During operation, changes in observation error do not necessarily manifest as obvious outliers; more commonly, they include increased deviation, decreased stability, or enhanced short-term fluctuations within a road segment. If the observation noise parameter remains constant, the observation update intensity will remain constant, making it difficult to account for reliability differences at different stages.

[0194] In this embodiment, the noise scaling factor is calculated based on a monotonic mapping relationship constructed according to the magnitude of the comprehensive observation innovation: the larger the comprehensive observation innovation, the larger the scaling factor; the smaller the comprehensive observation innovation, the closer the scaling factor is to the baseline value.

[0195] To facilitate implementation, a standardized index can be calculated as input for the comprehensive observation innovation. For example, the weighted sum of the absolute values ​​of each component, the square root of the sum of squares of each component, or the index can be calculated separately for the position component and the heading component and then combined.

[0196] Those skilled in the art will understand that it is sufficient to ensure that the scalarization index increases with the degree of observation inconsistency. The mapping relationship can be set in a piecewise continuous manner: when the comprehensive observation innovation is below the first threshold, the scaling factor remains near the baseline; when the comprehensive observation innovation is in the middle range, the scaling factor increases smoothly; when the comprehensive observation innovation is above the second threshold, the scaling factor is increased to the preset upper limit to avoid the observation noise being infinitely amplified, rendering the observation update ineffective. When the adjusted observation noise parameters are obtained after scaling, the original observation noise parameters can be scaled at the same scale, or different scaling ratios can be used for different observation components. For example, a different scaling factor can be used for the heading angle observation noise than for the position observation noise, thereby adapting to the error characteristics of different observation components.

[0197] In yet another example, the extended Kalman filter observation update for each motion pattern, outputting the fused localization result of the handling robot, includes:

[0198] S4.3: Calculate the Kalman gain corresponding to the motion mode based on the adjusted observation noise parameters, observation innovation, and innovation covariance;

[0199] Specifically, the Kalman gain determines the strength of the correction of the predicted state by the observation information, and its magnitude depends on the relative relationship between the prediction uncertainty and the observation uncertainty. In the preceding steps, the adjusted observation noise parameters have dynamically characterized the observation uncertainty, allowing the observation update strength to adjust with changes in consistency. If the Kalman gain calculation still uses the unadjusted observation noise parameters, the effect of noise adjustment cannot be reflected in the update results. Therefore, it is necessary to use the adjusted observation noise parameters in the construction of the innovation covariance for each motion mode, and calculate the Kalman gain accordingly.

[0200] In this embodiment, for each motion mode, the prediction covariance, observation Jacobian matrix, and corresponding observation innovation for that motion mode are read, and the adjusted observation noise parameters are used as the observation noise input to form the innovation covariance for that motion mode. Subsequently, the Kalman gain is calculated for that motion mode based on the matching relationship between the prediction covariance and the innovation covariance.

[0201] To ensure a consistent processing flow across different observation sources, when the observation information is a pose observation, the Kalman gain corresponds to the joint update of the position and heading components; when the observation information is a position observation, the Kalman gain corresponds to the update of the position component, while the heading component can retain the result obtained from the prediction update or be indirectly constrained through other observation sources. Those skilled in the art will understand that the dimension of the Kalman gain is consistent with the output dimension of the observation function; it is only necessary to ensure that the observation Jacobian and covariance matrix match in dimension.

[0202] S4.4: Correct and update the predicted state corresponding to the motion mode according to the Kalman gain to obtain the updated state;

[0203] In this embodiment, for each motion mode, the predicted state and the observation innovation of that mode are input into the correction and update process. The Kalman gain is used to correct the predicted state to obtain the updated state. During the correction process, the pose-related components are adjusted according to the direction of the observation innovation, so that the position and heading are closer to the observation direction. The slip extension state components can also be corrected in the observation update, depending on whether the observation function has a sensitive term for the slip extension state or whether indirect correction is achieved through state coupling.

[0204] Those skilled in the art will understand that even if the observation function does not directly observe the slip extension state, the slip extension state can still obtain a correction path during covariance propagation and updating through its correlation with the pose components, thereby achieving indirect correction of the observation-pose-slip error. After the update is completed, the state covariance of this motion mode can be updated synchronously to reflect the degree of convergence of uncertainty introduced by the observation, and the updated state and updated covariance are saved as the posterior estimate of this motion mode at the current time.

[0205] S4.5: Based on the posterior mode probability of each motion mode, the updated state of each motion mode is weighted and fused to obtain the fused localization result of the handling robot;

[0206] In this embodiment, the fused positioning result is generated using a component-wise weighted method: the updated states of each motion mode are weighted and summed according to the posterior mode probability to obtain the final pose output. Position components can be directly weighted and fused; for periodic quantities such as heading angle, they can be converted into continuous representations before weighting to avoid discontinuities in the average result caused by crossing angle boundaries. Those skilled in the art will understand that this continuous representation can be implemented using common methods such as direction vectorization, and this application does not further limit it.

[0207] Whether to output the slip-extended state depends on the application requirements: when the slip-extended state is only used for internal estimation, it need not be output externally; when it needs to provide a reference for upper-level control or state diagnosis, the mode probability-weighted slip-extended state estimate can be output synchronously. After fusion, the fused positioning result can be used as the initial value for filtering or control input for the next moment, and the time index used for time alignment can be updated synchronously to ensure the consistency of subsequent loop processing.

[0208] Although embodiments of the present invention have been shown and described above, it is understood that the above embodiments are exemplary and should not be construed as limiting the present invention. Those skilled in the art can make changes, modifications, substitutions and variations to the above embodiments within the scope of the present invention.

Claims

1. A multi-sensor fusion localization method based on extended Kalman filtering, applied to a handling robot, characterized in that, The transport robot includes a sensor assembly for collecting motion and observation information of the transport robot. The method includes: Based on the motion information, a slip indication is calculated, and the mode probability of multiple preset motion patterns is determined according to the slip indication, wherein the slip indication is used to characterize the degree of inconsistency between the motion information and the robot's short-term dynamic response; For each motion mode, an extended Kalman filter is used to update the prediction, resulting in the predicted state and prediction covariance for each motion mode. After acquiring the observation information, for each motion mode, the observation prediction value is calculated based on the corresponding prediction state to obtain the observation innovation and innovation covariance, and the mode probability of the motion mode is updated according to the observation innovation and innovation covariance, wherein the innovation covariance is determined by the prediction covariance and the observation noise parameter of the extended Kalman filter. The observation noise parameters are adjusted based on the updated mode probabilities and the observation innovations. An extended Kalman filter observation update is performed for each motion mode, and the fusion localization result of the transport robot is output.

2. The multi-sensor fusion localization method based on extended Kalman filtering according to claim 1, characterized in that, The motion information includes wheel speed information collected by the wheel speed encoder and angular velocity and linear acceleration information collected by the inertial measurement unit; The observation information includes pose observation information output by the lidar positioning module, pose observation information output by the visual positioning module, and position observation information output by the positioning device. The method further includes: preprocessing the raw data collected by the sensor components to obtain the motion information and the observation information, wherein the preprocessing includes time alignment based on timestamps, coordinate system transformation, and removal of abnormal data.

3. The multi-sensor fusion localization method based on extended Kalman filtering according to claim 1, characterized in that, The method further includes: constructing a unified state vector for extended Kalman filtering; the unified state vector includes at least a pose state characterizing the pose of the handling robot and a slip extension state characterizing the wheel motion error due to load variation; wherein, the initial value of the pose state is determined based on a preset initial pose and an initial pose obtained from the observation information; the initial value of the slip extension state is set to a preset nominal value; the initial variance corresponding to the preset nominal value is greater than or equal to the initial variance corresponding to the pose state. Define the state covariance and initialize it as a diagonal matrix composed of the initial variances of each state component of the unified state vector.

4. The multi-sensor fusion localization method based on extended Kalman filtering according to claim 2, characterized in that, Based on the motion information, the slip indication is calculated, including: Based on the motion information, the short-term speed and displacement increment calculated from the wheel mileage are determined to obtain the first short-term motion amount; Based on the motion information, the short-term linear acceleration and angular velocity calculated by inertia are determined, and the short-term linear acceleration and angular velocity are integrated to obtain the second short-term motion quantity; The difference between the first short-term motion quantity and the second short-term motion quantity is calculated as the original inconsistency quantity, and the original inconsistency quantity is normalized and low-pass filtered to obtain the slip indication quantity, wherein the normalization is based on the amplitude of the second short-term motion quantity.

5. The multi-sensor fusion localization method based on extended Kalman filtering according to claim 1, characterized in that, Determine the mode probabilities of multiple preset motion patterns based on the slip indication, including: A preset mode set includes multiple motion modes, the mode set including at least a low slip mode and a high slip mode, and a corresponding slip threshold range is set for each motion mode. The low slip mode indicates that the handling robot is in a stable wheel-to-ground attachment state, and the high slip mode indicates that the handling robot is in an unstable wheel-to-ground attachment state. The slip threshold range includes an overlapping range, which is the overlapping part of the slip threshold range of the low slip mode and the slip threshold range of the high slip mode. The slip indication is matched with the slip threshold range of each motion mode to obtain the original weight of each motion mode; The original weights of each motion pattern are normalized to obtain the pattern probability of the motion pattern.

6. The multi-sensor fusion localization method based on extended Kalman filtering according to claim 5, characterized in that, The steps for constructing the overlapping interval include: Acquire historical motion data of the handling robot in a preset calibration scenario. The historical motion data includes at least the wheel speed signal output by the wheel speed encoder, the linear acceleration signal output by the inertial measurement unit, and the angular velocity signal. Frequency domain analysis of wheel speed signal line, acceleration signal and angular velocity signal is performed using a sliding time window, and energy characteristics within a preset frequency band are extracted as adhesion degradation characteristics. Based on the historical motion data, the historical slip indication for the corresponding time window is calculated, and the historical slip indication is statistically correlated with the adhesion degradation characteristics to obtain the distribution parameters of the slip indication under different adhesion degradation levels. The slip threshold ranges for the low slip mode and the high slip mode are determined based on the distribution parameters, and the overlapping range is determined through a critical division strategy. The critical partitioning strategy includes: dividing the adhesion degradation features into at least two distinct adhesion degradation levels, including a critical adhesion degradation level; obtaining preset quantile thresholds for historical slip indicator samples belonging to the critical adhesion degradation level in the distributions corresponding to low slip mode and high slip mode, respectively; using the upper quantile threshold of the low slip mode as the upper bound of the low slip mode threshold interval; using the lower quantile threshold of the high slip mode as the lower bound of the high slip mode threshold interval; and using the overlap between the upper and lower bounds as the overlapping interval.

7. The multi-sensor fusion localization method based on extended Kalman filtering according to claim 3, characterized in that, The extended Kalman filter prediction update for each motion mode, to obtain the prediction state and prediction covariance for each motion mode, includes: Determine a nonlinear state transition function for each motion mode, including a slip extension state, wherein the slip extension state is updated according to the slip indication; Substitute the motion information into the nonlinear state transition function to update the unified state vector of the previous moment, and obtain the predicted state under the corresponding motion mode. The nonlinear state transition function is linearized to the unified state vector at the previous time step to obtain the corresponding state transition Jacobian matrix. Based on the state transition Jacobian matrix, the process noise parameters corresponding to the motion mode, and the state covariance of the previous moment, the state covariance is propagated and updated to obtain the prediction covariance corresponding to the motion mode. The slip extension state participates in the time update of the nonlinear state transition function and the propagation update of the state covariance, and the process noise parameters corresponding to different motion modes are adjusted according to the slip extension state.

8. The multi-sensor fusion localization method based on extended Kalman filtering according to claim 1, characterized in that, Calculate the observed predicted value based on the corresponding predicted state, obtain the observed innovation and innovation covariance, and update the mode probability of the motion pattern according to the observed innovation and innovation covariance, including: For each motion mode, a nonlinear observation function corresponding to the observation information is determined, and the predicted state corresponding to the motion mode is substituted into the nonlinear observation function to obtain the corresponding observation prediction value. Calculate the observation innovation corresponding to the motion pattern, wherein the observation innovation is the difference between the observation information and the observation prediction value; The nonlinear observation function is linearized to the first order at the predicted state of the corresponding motion mode to obtain the observation Jacobian matrix corresponding to the motion mode. Based on the observed Jacobian matrix, the predicted covariance corresponding to the motion pattern, and the observed noise parameters, calculate the innovative covariance corresponding to the motion pattern. The observation consistency metric corresponding to the motion pattern is calculated based on the observation innovation and the innovation covariance, and the likelihood weight of the motion pattern is determined by the observation consistency metric. The prior mode probability of the motion pattern is normalized and updated based on the likelihood weight to obtain the posterior mode probability of the motion pattern.

9. The multi-sensor fusion localization method based on extended Kalman filtering according to claim 8, characterized in that, The adjustment of the observation noise parameters based on the updated model probability and the observation innovation includes: Based on the posterior mode probability, a comprehensive observation innovation weighted by mode probability is determined, wherein the comprehensive observation innovation is obtained by weighting the observation innovation corresponding to each motion mode with the corresponding posterior mode probability. The observation noise scaling factor is calculated based on the comprehensive observation innovation, and the observation noise parameters are scaled according to the observation noise scaling factor to obtain the adjusted observation noise parameters.

10. The multi-sensor fusion localization method based on extended Kalman filtering according to claim 9, characterized in that, The extended Kalman filter observation update for each motion mode outputs the fused localization result of the handling robot, including: Based on the adjusted observation noise parameters, observation innovation, and innovation covariance, calculate the Kalman gain corresponding to the motion mode; The predicted state corresponding to the motion pattern is corrected and updated based on the Kalman gain to obtain the updated state; Based on the posterior mode probability of each motion mode, the updated states of each motion mode are weighted and fused to obtain the fused localization result of the handling robot.

Images