Self-adaptive UKF fusion positioning method for outdoor cleaning robot

Abstract

The invention discloses a self-adaptive UKF fusion positioning method for an outdoor cleaning robot. The method comprises the following steps: acquiring inertial measurement unit IMU data, point cloud data and RTK positioning data through a sensor; according to the system state of the robot, unscented Kalman filtering (UKF) is adopted for unscented transformation, IMU data serve as control input, a predicted sigma point set is obtained, and sigma points serving as the system state are updated; evaluating the quality of the point cloud data; according to the point cloud data quality evaluation and the RTK positioning data, a current positioning mode is adaptively selected through a state machine, and the positioning mode comprises a first mode with the point cloud data quality as the main mode, a second mode with the RTK positioning data as the main mode and a third mode fusing the point cloud data quality and the RTK positioning data; and according to a corresponding mode, observation updating and time-sharing dimension multiplexing are carried out based on unscented Kalman filter (UKF).

Description

Technical Field

[0001] This invention belongs to the field of autonomous navigation and robotics, specifically relating to an adaptive UKF fusion localization method for outdoor cleaning robots. Background Technology

[0002] With the rapid advancement and application of intelligent robot technology, various outdoor robots, including outdoor cleaning robots, have shown broad application prospects in fields such as logistics and delivery, scenic area guidance, urban cleaning, and security patrol. Outdoor cleaning robots, in particular, have requirements such as edge cleaning, cost reduction, high robustness, and high safety; these requirements also place higher demands on the accuracy, performance, robustness, and versatility of the positioning module.

[0003] Currently, localization algorithms can be broadly categorized into three types: traditional localization methods, machine learning-based methods, and vehicle-to-everything (V2X) solutions. Machine learning-based localization methods rely on training with large amounts of data and are prone to poor generalization, resulting in insufficient versatility. Furthermore, their inference requires significant computational power, leading to poor real-time performance. V2X solutions require the prior construction of network infrastructure, are susceptible to network conditions, and are unsuitable for outdoor cleaning robots that experience frequent contact with people due to network transmission latency and security issues. Therefore, traditional localization methods remain the most suitable solution for outdoor cleaning robots. However, traditional localization methods have significant limitations when using a single observation source, such as laser point clouds or RTK. For example, laser point cloud solutions may fail to provide accurate localization in areas with insufficient point cloud features, such as open fields or tunnels; and due to computational errors, the accuracy may gradually deviate significantly over time. RTK alone is affected by network conditions; in scenarios with obstruction or high cloud thickness, accuracy drops sharply, failing to meet the requirements for high-precision localization. Therefore, observation sources are usually integrated with IMUs, and sometimes multiple observation sources are integrated to handle scenarios that are difficult to solve with a single observation.

[0004] For traditional positioning fusion frameworks, several commonly used methods exist, including Kalman filtering (KF), extended Kalman filtering (EKF), and unscented Kalman filtering (UKF). The KF framework treats linear systems, where both process and observation noise follow a Gaussian distribution, offering advantages such as low computational cost, fast convergence, and high real-time performance; however, it is unsuitable for linear systems. The EKF framework approximates nonlinear systems using Jacobian linearization, resulting in slightly higher computational cost, but still within manageable limits; its drawback is insufficient accuracy due to the use of first-order linearization, especially with strong nonlinearity; and its computational cost increases significantly with increasing dimensionality. The UKF framework uses sigma points instead of Jacobian points, more accurately capturing distributions under nonlinear conditions, making it more suitable for complex nonlinear models and more stable in sharp turns and highly dynamic scenarios; however, it has higher computational cost, is more difficult to tune parameters, and if RTK, laser point cloud, and IMU data are fused simultaneously, the dimensionality of the system state matrix and noise matrix increases significantly, further increasing the computational cost of UKF.

[0005] Existing technologies employ a two-layer filtering architecture using UKF+EKF, where UKF is used to fuse laser, IMU, and odometry data, and EKF is used to fuse UKF results with GNSS absolute position. However, its logic for judging positioning effectiveness relies on 'continuity judgment,' which can be lagging, leading to the inability to detect positioning anomalies in a timely manner and potentially causing accidents.

[0006] Existing technologies include methods that integrate RTK, IMU, and 3D LiDAR to address initialization and positioning issues under occlusion conditions. However, this approach lacks flexibility and is not suitable for situations where open and non-open environments alternate during operation, a scenario commonly encountered in the working areas of cleaning robots, such as plazas.

[0007] Existing technologies also include vehicle positioning by fusing LiDAR, cameras, IMUs, wheel speed sensors, and other technologies. While these technologies employ complex fusion strategies such as Archimedes optimization and the Zebra algorithm to achieve good multimodal fusion, their computational complexity is too high, making them unsuitable for platforms with lower computing power. Furthermore, their use of EKF results in insufficient accuracy when handling nonlinear problems.

[0008] Therefore, in order to address the problems of existing positioning algorithms for outdoor cleaning robots, it is necessary to design a positioning algorithm that can integrate sensor data such as satellite positioning (RTK), laser point cloud (Lidar), and inertial measurement unit (IMU), and provide high accuracy, controllable computational cost, high robustness, and is suitable for outdoor cleaning robots during the positioning process. Summary of the Invention

[0009] To address the shortcomings of existing technologies, improve the adaptability and flexibility of localization algorithms in different scenarios, reduce algorithm overhead to suit the performance of cleaning robots, make the fusion process smoother, and reduce pose jumps during localization, this invention adopts the following technical solution:

[0010] An adaptive UKF fusion localization method for robots includes the following steps:

[0011] Step 1: Acquire inertial measurement unit (IMU) data, point cloud data, and RTK (Real-time kinematic) positioning data through sensors;

[0012] Step 2: Calculate sigma points based on the robot's system state and its covariance. Use unscented Kalman filter (UKF) for unscented transformation. Propagate each sigma point through a nonlinear state transition function f. Use inertial measurement unit (IMU) data as control input to obtain the predicted sigma point set. Perform weighted summation based on the predicted sigma point set to obtain the optimal predicted system state. Obtain the optimal predicted covariance by combining the predicted system state and the sigma point set. Add process noise to update the mean and covariance of the predicted state, which are then used to update the sigma points as the system state.

[0013] Step 3: Evaluate the quality of the point cloud data;

[0014] Step 4: Based on the point cloud data quality assessment and RTK positioning data, the current positioning mode is adaptively selected through a state machine. The positioning modes include a first mode based on point cloud data quality, a second mode based on RTK positioning data, and a third mode that integrates point cloud data quality and RTK positioning data.

[0015] Step 5: Observation update and time-division dimensionality reuse are performed based on Unscented Kalman Filter (UKF). The UKF observation update determines the observation source and observation noise used in this update according to the positioning mode selected in Step 4. The system state and observation noise are merged and subjected to an unscented transformation, and a set of sigma points are calculated. Then, the possible distribution of observation values ​​is calculated through weighted summation. Finally, by calculating the Kalman gain and utilizing the difference between the measured and predicted values, the predicted state and covariance obtained in Step 2 are corrected to obtain the best estimate. Furthermore, dynamic weights are designed for the third mode to achieve smooth fusion of various observation sources.

[0016] Unscented Kalman Filter (UKF) receives data from different observation sources in a time-division multiplexing manner, and enters the filtering process sequentially according to the timestamp of each observation source, while reusing the same filter for computation.

[0017] Further, step 2 includes the following steps:

[0018] Step 2.1: Calculate 2n+1 sigma points based on the current system state and covariance. These sigma points are scaled according to the parameter λ and evenly distributed around the system state.

[0019] Step 2.2: Propagate each sigma point through the nonlinear state transition function f, using inertial measurement unit (IMU) data as control input, to obtain the predicted sigma point set;

[0020] Step 2.3: Based on the predicted sigma point set, the optimal predicted system state and predicted covariance are obtained through unscented transformation and weighted summation. The predicted system state is obtained by weighted summation of sigma points, and the calculation steps are as follows:

[0021]

[0022] The predicted state covariance is obtained based on the sigma point and the corresponding predicted system state, and its calculation steps are as follows:

[0023]

[0024] Where n represents the number of sigma points, This represents the weight of the corresponding sigma point. This represents the sigma point calculated in step 2.2;

[0025] Step 2.4: Add system process noise to the prediction covariance. This value represents the uncertainty of the system model.

[0026] Furthermore, the specific formula for step 2.1 is as follows:

[0027]

[0028] in, Representation matrix The List, Indicates the current system state. The error covariance matrix represents the system state, where n represents the dimension of the state vector and λ represents the scaling parameter used to adjust the distribution of sigma points so that the sigma points are evenly distributed around the system state.

[0029] Furthermore, in step 2.2, the operation process of the state transition function f is as follows:

[0030] Step 2.2.1: The nonlinear state transition function f receives a single sigma point as the system state and inertial measurement unit (IMU) data as the control input vector, where the control input vector is defined as follows:

[0031]

[0032] in, These represent accelerations in three dimensions. These represent angular velocities in three dimensions;

[0033] The system state matrix includes three-dimensional position coordinates and three-dimensional velocities in the world coordinate system, rotation quaternions from the local coordinate system to the world coordinate system, accelerometer bias, and gyroscope bias, totaling 16 dimensions, defined as follows:

[0034]

[0035] in, , representing the three-dimensional position coordinates in the world coordinate system; , representing three-dimensional velocity in the world coordinate system; , represents the rotation quaternion from the local coordinate system to the world coordinate system; This is the accelerometer bias; This is the gyroscope bias;

[0036] Step 2.2.2: Extract the current position, velocity, and quaternion from the system state matrix, and filter out abnormal and noise values. To avoid non-unitization of quaternions due to numerical errors, if a quaternion value is found to be less than the threshold 1e-12, it will be assigned a unit quaternion; otherwise, the quaternion will be normalized, and the influence of gravity will be subtracted from the acceleration. The calculation methods for velocity, position, and quaternion are as follows:

[0037]

[0038]

[0039]

[0040] in, This represents the acceleration in the world coordinate system, obtained by subtracting the accelerometer bias from the acceleration measured by the IMU and then transforming it to the world coordinate system. This represents the acceleration due to gravity, and t represents the time of the last calculation. Indicates time interval, Indicates the current moment. Indicates a rotational increment quaternion. This indicates a normalization operation;

[0041] Step 2.2.3: Perform velocity and position predictions, and remove the influence of gravity; during velocity prediction, the acceleration measurement value will be used first. Subtract bias Then, the acceleration is rotated to the world coordinate system; the formula for this step is as follows:

[0042]

[0043] Next, position prediction is performed. The position prediction is obtained by adding the velocity and time integral to the current position. The calculation formula for this part is as follows:

[0044]

[0045] Step 2.2.4, calculate attitude prediction, first of all, angular velocity Subtract gyroscope bias Next, the compensated angular velocity magnitude and rotation axis are calculated, and the rotation increment quaternion Δq within time Δt is also calculated. To avoid division by zero, this part directly uses the attitude from the previous frame when the angular velocity magnitude or time interval is too small, skipping attitude prediction. The formula for attitude prediction is shown below:

[0046]

[0047] Step 2.2.5: For the accelerometer bias and gyroscope bias, in this invention it is assumed that they are slowly time-varying. Each time a prediction is made, the bias at the current moment will be directly passed to the next moment. That is, in a single running cycle, they will be treated as constants and directly passed to the next state.

[0048]

[0049] Step 2.2.6: To enhance robustness, a final check is added. If the calculated system state contains outliers such as NaN, it will directly revert to the safe system state of the previous frame to ensure overall stability.

[0050] Furthermore, in step 3, the point cloud quality balancing is based on the number of point clouds, the proportion of effective matching points, the registration score, and whether there is a risk of point cloud degradation, to calculate the point cloud quality score, including the following steps:

[0051] Step 3.1, Construct a point cloud quantity penalty term When the number of point clouds is less than the threshold If the current point cloud frames are deemed insufficient to support the positioning requirements, this penalty will be set as a point cloud quantity penalty coefficient. Otherwise, the penalty is 1;

[0052] Step 3.2, calculate the penalty for valid matching points. ;

[0053] First, the map is stored using KDTree. For each point cloud frame, each point is queried. If a point within the vicinity is found in the KDTree, the number of vicinity points is counted. Add 1 to the total number of points in the point cloud frame. This is used to calculate the proportion of effective points, and the calculation method is as follows:

[0054]

[0055] Penalty for the percentage of valid match points The calculation method is as follows:

[0056]

[0057] in, This represents the preset penalty coefficient for valid matching points. The first-stage threshold represents the warning value at which the proportion has decreased. The second-stage threshold represents the dangerous level to which the proportion has decreased.

[0058] Step 3.3, Registration Score: This step involves modeling historical registration score data and calculating the overall variance when new data arrives. If the variance reaches a certain threshold, the registration score is considered unacceptable, and a flag is set. Set to true when the standard is not met, and false otherwise; When true, a registration score penalty is set. To register the score penalty coefficient In other cases, set it to 1;

[0059] Step 3.4: Determine the point cloud degradation risk and set a degradation flag when degradation risk exists. If true, then false; otherwise, set a degradation risk penalty. Penalty coefficient for degradation risk Set to 1 at other times;

[0060] Step 3.5: Based on the point cloud quality indicators from Steps 3.1 to 3.4, calculate the overall point cloud quality score:

[0061]

[0062] in, Indicates the baseline score. This represents the point cloud quantity penalty term, and its value is the point cloud quantity penalty term coefficient. , This represents the penalty for valid matches, and its value is the penalty coefficient based on the proportion of valid matches. , This indicates the registration score penalty item, when When true, the value of the registration score penalty term is the registration score penalty coefficient. Otherwise, it is 1. This indicates a penalty for degradation risk, when and When true, the value of the degradation risk penalty term is the degradation risk penalty coefficient. Otherwise, it is 1.

[0063] Furthermore, in step 3.3, the method for modeling and determining the state of the point cloud registration score includes the following steps:

[0064] Step 3.3.1: Maintain a first-in-first-out (FIFO) sliding window queue to store the error index, i.e. the registration score, calculated recently based on the lidar positioning results;

[0065] Step 3.3.2: Before evaluation, first determine the validity of the point cloud data in the current frame; if the number of point clouds in the current frame is lower than the first preset threshold, or the proportion of valid point clouds is lower than the second preset threshold, then no further calculation will be performed to prevent abnormal registration scores from affecting the modeling data.

[0066] Step 3.3.3: After the sliding window queue is filled, calculate the variance of all error data within the window to quantify the recent fluctuations in lidar positioning performance. The larger the variance, the more unstable the registration accuracy and the worse the reliability.

[0067] Step 3.3.4: Compare the calculated variance with the third preset threshold; if the variance is greater than the threshold, it indicates that the lidar positioning accuracy is fluctuating drastically or deteriorating, and it is determined that there is a risk of losing positioning, and a setting is made. If the variance is less than or equal to the threshold, it indicates that the lidar positioning accuracy is stable and reliable, and the setting is... It is false;

[0068] Step 3.3.5, Window Update: After completing an evaluation and decision, add the latest error data to the window and remove the oldest historical data to achieve sliding window updates and ensure that the evaluation basis is always the latest data sequence.

[0069] Furthermore, the method for determining the point cloud degradation risk in step 3.4 includes the following steps:

[0070] Step 3.4.1: Preprocess the input point cloud frame and use KDTree to perform spatial management of the point cloud;

[0071] Step 3.4.2: Perform Principal Component Analysis (PCA) within the local neighborhood; for each point in the point cloud... Search for K nearest neighbors within its local neighborhood to form a local point set. Then, the three-dimensional covariance matrix of this local point set is calculated. :

[0072]

[0073] in, Represents a local point set The geometric center (centroid) of the . Represents points within a neighborhood;

[0074] For covariance matrix Eigenvalue decomposition yields three eigenvalues. ,and ; Calculate points using three eigenvalues Local curvature :

[0075]

[0076] The curvature value reflects the degree of curvature of a local surface at that point; a larger value indicates greater curvature. The curvature value is then compared with a preset curvature threshold. If a comparison is made, Then the calculation point It was identified as a corner feature;

[0077] Step 3.4.3: Count the number of all points identified as corners in the entire point cloud. And calculate its relationship with the total number of point clouds. Corner ratio:

[0078]

[0079] The ratio reflects the richness of corner features in the point cloud;

[0080] Step 3.4.4: Perform global principal component analysis (PCA) on the entire point cloud frame, calculate its covariance matrix, and perform eigenvalue decomposition to obtain three eigenvalues. ,and ; where the smallest eigenvalue This reflects the variance of the point cloud along the direction of the most concentrated distribution. A small value indicates that the point cloud lacks distribution in one or more dimensions, directly reflecting the occurrence of point cloud degradation; the ratio of the intermediate eigenvalue to the minimum eigenvalue is calculated. A large ratio indicates that the point cloud is distributed in a linear or planar manner, rather than a healthy clumped distribution;

[0081] Step 3.4.5: Based on the indicators described in steps 3.5.2 to 3.5.4, comprehensively determine whether the point cloud is at risk of degradation.

[0082]

[0083] in, These represent the corner ratio threshold, minimum eigenvalue threshold, and ratio threshold of intermediate eigenvalue to minimum eigenvalue, respectively set based on prior knowledge and / or actual application scenarios. That is, when the corner ratio is low or the minimum eigenvalue is small, and the eigenvalue ratio is too large, the point cloud is deemed to have a risk of degradation. If degradation is determined, [the following will be implemented / removed / removed]. Set to true.

[0084] Furthermore, step 4 includes the following steps:

[0085] Step 4.1, the state machine performs a mode switch; the criteria for mode switching include point cloud quality scores. Longitude covariance of RTK positioning data and latitude covariance ; and a pre-set threshold for point cloud quality warning. Threshold for poor point cloud quality and RTK positioning data covariance threshold ,and If the mean covariance of RTK positioning data is If the current RTK positioning data is deemed unavailable, the state machine is prohibited from switching to any mode other than the second and third modes.

[0086] Step 4.2: If the current mode is the first mode and it is possible to switch to the second mode, then the point cloud quality score will be calculated. Threshold for point cloud quality warning In comparison, if less than If the degradation counter is less than the threshold value for poor point cloud quality, then increment the degradation counter. If so, the degradation counter is increased by a larger step size; if Higher than the If the value of the degradation counter is higher than the degradation threshold, the degradation counter will be reset to zero and the system will switch to the second mode.

[0087] Step 4.3: If the current mode is the second mode, and it is possible to switch to the first or third mode, then the point cloud quality score will be calculated. Threshold for point cloud quality warning In comparison, if less than If the degradation counter is incremented and the improvement counter is reset to zero, then if it is still less than the threshold for poor point cloud quality... Then increase the degradation counter with a large step size; if Higher than If the value of the degradation counter is higher than the degradation threshold, the degradation counter and the improvement counter are reset to zero and the system switches to the third mode. If the value of the improvement counter is higher than the improvement threshold, the degradation counter and the improvement counter are reset to zero and the system switches to the first mode.

[0088] Step 4.4: If the current mode is the third mode, and it is possible to switch to the second mode; if the point cloud quality score... The threshold is higher than the point cloud quality warning threshold. If the value of the improvement counter is higher than the improvement threshold, then the degradation counter and the improvement counter are reset to zero and the system switches to the second mode.

[0089] Furthermore, in step 5, the observation update of the unscented Kalman filter (UKF) and the time-division multiplexing of the filter include the following steps:

[0090] Step 5.1: Based on the state machine switching result, determine the observation source data to be entered into the filter and the observation covariance to be used; if the current mode is the first mode, then receive the seven-dimensional observation matrix obtained from point cloud registration. With the corresponding covariance matrix :

[0091]

[0092] in, This represents the world coordinates obtained after point cloud registration; This represents the rotation quaternion obtained after point cloud registration.

[0093]

[0094] These represent the covariance of the laser point cloud along the x-axis, y-axis, and z-axis, respectively, in the first mode. The default value is the same, which corresponds to the covariance of the quaternion in the first mode. The value of the covariance is obtained based on the point cloud quality score.

[0095] If the current mode is the third mode, then RTK positioning data and point cloud observations are used in a time-division manner, and the same filter is used in the calculation, reusing the observation matrix and covariance matrix:

[0096]

[0097]

[0098] exist At any given moment, execute observation updates using point cloud data; Continuously perform observation updates using RTK positioning data; This represents the three-dimensional coordinates in the world coordinate system observed based on RTK positioning data. This represents the rotation quaternion observed based on RTK positioning data;

[0099] The covariance matrix will be overwritten with the covariance of the corresponding observation source at the corresponding time. When RTK positioning data is used as the current observation source, only the covariance in the x and y directions is replaced with the covariance on the x and y axes corresponding to the RTK positioning data, while other positions remain unchanged.

[0100] If the current mode is the second mode, then the observations from RTK positioning data will be used, and the corresponding observation matrix is ​​as follows:

[0101]

[0102] Step 5.2, in the third mode, to make the filtering of RTK positioning data and laser point cloud smoother and prevent pose jumps, the mean covariance of the RTK positioning data is used as the basis. Design RTK positioning data weights Based on point cloud quality score Design point cloud quality weights ;

[0103] The calculation method is as follows:

[0104]

[0105] in, This represents the preset point cloud quality threshold, which must meet the following requirements. ;in The threshold representing the point cloud quality warning; This indicates that the covariance threshold of RTK positioning data is lower than [a certain threshold]. Then it is assumed that the quality of the current RTK positioning data meets the requirements;

[0106] The calculation method is as follows:

[0107]

[0108] The state machine switching mechanism ensures that if the point cloud quality score is... Greater than the point cloud quality threshold Furthermore, when the RTK positioning data quality meets the requirements, it is not in the third mode. and None of them are taken as 1;

[0109] Based on the current observation source type, apply the corresponding weighting factor to the observation noise matrix; if the current observation source is RTK positioning data, then update the covariance matrix. If the current observation source is point cloud data, then update the covariance matrix. ;

[0110] Step 5.3: Before updating the unscented Kalman filter (UKF) observations, the filter is locked to prevent multi-threaded conflicts when RTK positioning data and point cloud data are available simultaneously. Then, the augmented state and noise matrices are constructed; Sigma points are generated and unscented transformations are performed, and the data is propagated to the observation space; the mean and covariance of the predicted observations are calculated; the cross-covariance matrix of the state and observations and the Kalman gain are calculated; the system state mean and covariance matrix are updated using the actual observations; and the filter is unlocked.

[0111] An adaptive UKF fusion localization method for robots is provided, wherein an outdoor cleaning robot is subjected to adaptive UKF fusion localization according to the aforementioned adaptive UKF fusion localization method.

[0112] The advantages and beneficial effects of this invention are as follows:

[0113] 1. This invention uses UKF filters instead of commonly used KF or EKF filters, eliminating linearity errors and improving positioning accuracy.

[0114] 2. This invention integrates data from multiple sensors such as RTK, laser point cloud, and IMU, which solves the limitations of algorithm application caused by the original single observation source, enabling the cleaning robot to operate in more scenarios.

[0115] 3. This invention analyzes the point cloud quality status from multiple perspectives and combines it with the point cloud quality of high-precision maps to comprehensively calculate the point cloud quality score. It replaces the commonly used fixed threshold method for judging point cloud registration scores and introduces a registration consistency judgment based on sliding window variance. This effectively solves the problem in the prior art that the overall positioning trend degradation cannot be identified due to accidental success of single-frame matching, thus providing early warning before positioning failure.

[0116] 4. This invention uses a time-division multiplexing of a single UKF filter supplemented by a dynamic weighting strategy, which significantly reduces the system's computing resources and memory usage while ensuring positioning accuracy, and solves the resource conflicts and synchronization problems caused by parallel operation of multiple filters or high-dimensional matrix calculations.

[0117] 5. This invention achieves multi-localization mode adaptation through a state machine, and combines point cloud quality scoring, RTK covariance, hysteresis mechanism and dynamic weights to reduce interference between various observation sources and achieve smoother localization with fewer jumps.

[0118] 6. This invention improves the overall robustness of the localization program, reduces the impact of noise, and improves the accuracy of the prediction update stage by optimizing and modifying the state transition function of the UKF filter prediction update stage. Attached Figure Description

[0119] Figure 1 This is a framework diagram of the fusion positioning method in an embodiment of the present invention.

[0120] Figure 2 This is a flowchart of the fusion positioning method in an embodiment of the present invention. Detailed Implementation

[0121] The specific embodiments of the present invention will be described in detail below with reference to the accompanying drawings. It should be understood that the specific embodiments described herein are for illustration and explanation only and are not intended to limit the present invention.

[0122] like Figure 1 As shown, an adaptive UKF fusion localization method for outdoor cleaning robots first receives raw data from external sensors such as RTK, LiDAR, and IMU. Upon receiving the corresponding data, the RTK data undergoes quality filtering as preprocessing; the LiDAR point cloud undergoes downsampling, removal of invalid or noisy points, and cropping to remove points outside a specific sensing range as preprocessing; the IMU data undergoes format conversion and is stored in a data queue. Next, UKF state prediction is performed using the IMU, and the prediction results are stored in the system state matrix and covariance matrix. Then, a comprehensive quality assessment of the point cloud is performed, and the resulting point cloud quality assessment score is fed into the state machine for state switching judgment. After both state machine switching and UKF state prediction are completed, the values ​​of the observation matrix and noise matrix are adjusted according to the state machine's judgment, and the UKF observation update step begins. After the observation update is completed, the pose is published to downstream nodes.

[0123] This invention achieves smoother adaptive state switching by combining point cloud quality assessment with RTK quality assessment and a hysteresis mechanism. In the UKF state prediction stage, the state transition function is optimized, noise is filtered out, and a safe backoff mechanism is added to improve overall robustness. In the UKF observation update stage, a time-sharing dimension reuse scheme is used to reduce the matrix dimension of the fusion stage to reduce overall overhead. In the fusion localization mode, a dynamic weighting system based on point cloud quality score and RTK covariance is designed to reduce interference between observation sources during fusion localization.

[0124] like Figure 2 As shown, the present invention proposes an adaptive UKF fusion localization method for outdoor cleaning robots, comprising the following steps:

[0125] Step 1, Sensor Data Reception and Preprocessing: Real-time reception of laser point cloud, IMU and RTK data, preprocessing and filtering to obtain sensor data that meets the requirements; and storage of RTK, IMU and laser point cloud data that meet the requirements.

[0126] 1.1) Data reception and buffering: Real-time reception and buffering of RTK positioning data, IMU inertial data and LiDAR point cloud frame data respectively; among them, IMU data is stored in the first-in-first-out IMU data queue according to its timestamp;

[0127] 1.2) Laser point cloud preprocessing: The laser point cloud frame data in the cache is preprocessed, including downsampling, removal of invalid or noisy points, and cropping to remove point clouds outside a specific sensing range;

[0128] 1.3) RTK data preprocessing: RTK data in the cache is filtered based on its positioning status, and only valid RTK data whose positioning accuracy meets the preset requirements is retained;

[0129] 1.4) IMU data preprocessing: Preprocessing the IMU data in the cache, including one or more of format conversion, coordinate system conversion, and zero bias compensation.

[0130] Step 2, Unscented Kalman Filter (UKF) State Prediction: A set of sigma points is generated by inputting to the IMU and unscented transformation, and the predicted state mean and covariance are updated by weighted summation and superimposing process noise. The obtained state mean and covariance are used to update the system state.

[0131] 2.1) Calculate 2n+1 sigma points based on the current system state and covariance. These sigma points are scaled according to the parameter λ and uniformly distributed around the system state. The specific formula is as follows:

[0132]

[0133] in, Pointer matrix The List, Refers to the current system state. Then it refers to the error covariance matrix of the system state, n refers to the dimension of the state vector, and λ refers to the scaling parameter, which can be used to adjust the distribution of sigma points. In this way, the sigma points will be evenly distributed around the system state.

[0134] 2.2) Propagate each sigma point obtained in step 2.1) through the nonlinear state transition function f to obtain the predicted sigma point set; the operation process of the state transition function f is as follows:

[0135] 2.2.1) The nonlinear state transition function f receives the system state (a single sigma point) and the control input vector (IMU data), where the control input vector is defined as follows:

[0136]

[0137] The first three dimensions represent acceleration, in m / s². The last three digits represent the angular velocity, in rad / s;

[0138] The system state matrix has 16 dimensions and is defined as follows:

[0139]

[0140] in, , representing the three-dimensional position coordinates in the world coordinate system; , representing three-dimensional velocity in the world coordinate system; , represents the rotation quaternion from the local coordinate system to the world coordinate system; This is the accelerometer bias; This is the gyroscope bias;

[0141] 2.2.2) Next, the current position, velocity, and quaternion are extracted from the system state matrix, and abnormal and noise values ​​are filtered out. To avoid non-unitization of quaternions due to numerical errors, if a quaternion value is found to be less than the threshold 1e-12, it will be assigned a unit quaternion; otherwise, the quaternion is normalized, and the influence of gravity is subtracted from the acceleration. The calculation methods for velocity, position, and quaternion are as follows:

[0142]

[0143]

[0144]

[0145] 2.2.3) After that, velocity prediction and position prediction are performed, and the influence of gravity is removed; among them, velocity prediction will first be performed on the acceleration measurement value. Subtract bias Then, the acceleration is rotated to the world coordinate system; the formula for this step is as follows:

[0146]

[0147] Next, position prediction is performed. The position prediction is obtained by adding the velocity and time integral to the current position. The calculation formula for this part is as follows:

[0148]

[0149] After 2.2.4), attitude prediction is calculated, starting with angular velocity. Subtract gyroscope bias Next, the compensated angular velocity magnitude and rotation axis are calculated, and the rotation increment quaternion Δq within time Δt is also calculated. To avoid division by zero, this part directly uses the attitude from the previous frame when the angular velocity magnitude or time interval is too small, skipping attitude prediction. The formula for attitude prediction is shown below:

[0150]

[0151] 2.2.5) For accelerometer bias and gyroscope bias, this invention assumes they are slowly time-varying. Each prediction will directly pass the current bias to the next time step, that is, treat them as constants in a single running cycle and pass them directly to the next state.

[0152]

[0153] 2.2.6) To enhance robustness, a final check has been added. If the calculated system state contains outliers such as NaN, it will directly revert to the safe system state of the previous frame to ensure overall stability.

[0154] 2.3) Then, through unscented transformation and weighted summation, the optimal predicted system state and predicted covariance are obtained. The predicted system state is obtained through weighted summation, and the calculation steps are as follows:

[0155]

[0156] in The weights are the corresponding sigma points. The sigma point calculated in step 2.2);

[0157] The steps for calculating the predicted state covariance are as follows:

[0158]

[0159] in and Consistent with the above, This is the result of the previous calculation;

[0160] 2.4) Finally, the system process noise is added to the prediction covariance, which represents the uncertainty of the system model.

[0161] Step 3: Evaluate the point cloud quality score for the laser point cloud data; The point cloud quality is scored based on the various indicator data obtained from the laser point cloud analysis; The point cloud quality score is based on the number of points, the proportion of effective matching points, the registration score, and whether there is a risk of point cloud degradation. These parameters are used to calculate the point cloud quality score using a designed formula.

[0162] 3.1) Number of point clouds Penalty: When the number of point clouds is less than the threshold If the current point cloud frame is deemed insufficient to support the positioning requirements, this penalty term will be set as a penalty coefficient. Otherwise, the penalty is 1;

[0163] 3.2) Number of valid matching points The calculation is performed as follows: First, the map is stored using KDTree. For each point cloud frame, each point is queried. If a point within the vicinity is found in the KDTree, then... Add 1; and use the total number of points in the point cloud frame as... This is used to calculate the proportion of effective points, and the calculation method for this proportion is as follows:

[0164]

[0165] Penalty for the percentage of valid match points The calculation method is as follows:

[0166]

[0167] in, This is the preset penalty coefficient; This is the first-stage threshold, indicating that the proportion has dropped to the warning level; This is the second-stage threshold, indicating that the proportion has dropped to a dangerous level.

[0168] 3.3) The registration score is not calculated using the commonly used fixed threshold scheme. Instead, it is calculated by modeling historical registration score data and calculating the overall variance when new data arrives. When the variance reaches a certain threshold, the registration score is considered unacceptable, and a flag is set. Set to true when the standard is not met, and false otherwise; when true, set a registration score penalty. Penalty coefficient In other cases, set it to 1;

[0169] The method for modeling and determining the state of point cloud registration scores in step 3.3) is as described in sections 3.3.1 to 3.3.5:

[0170] 3.3.1) Maintain a first-in-first-out (FIFO) sliding window queue to store the error index, i.e. the registration score, calculated recently based on the lidar positioning results;

[0171] 3.3.2) Before evaluation, the validity of the point cloud data in the current frame is first determined; if the number of point clouds in the current frame is lower than the first preset threshold, or the proportion of valid point clouds is lower than the second preset threshold, subsequent calculations are not performed to prevent abnormal registration scores from affecting the modeling data.

[0172] 3.3.3) Once the sliding window queue is full, calculate the variance of all error data within the window; the variance is used to quantify the recent fluctuations in lidar positioning performance; the larger the variance, the more unstable the registration accuracy and the worse the reliability.

[0173] 3.3.4) Compare the calculated variance with the third preset threshold; if the variance is greater than the threshold, it indicates that the lidar positioning accuracy is fluctuating drastically or deteriorating, and it is determined that there is a risk of losing positioning, and a setting is made. If the variance is less than or equal to the threshold, it indicates that the lidar positioning accuracy is stable and reliable, and the setting is... It is false;

[0174] 3.3.5) Window update: After completing an evaluation and decision, the latest error data is added to the window and the oldest historical data is removed to achieve sliding updates of the window and ensure that the evaluation basis is always the latest data sequence.

[0175] 3.4) Degradation risk assessment of point cloud is achieved by comprehensively assessing degradation risk through feature point extraction and global geometric feature analysis, and a degradation flag is set when degradation risk is present. If true, then false; otherwise, set a degradation risk penalty. Penalty coefficient Set to 1 at other times;

[0176] In step 3.4), the point cloud degradation risk is assessed by comprehensively evaluating the degradation risk through feature point extraction and global geometric feature analysis, and a degradation flag is set when degradation risk is present. If true, then false; otherwise, set a degradation risk penalty. Penalty coefficient The value is set to 1 at other times; the comprehensive judgment method for point cloud degradation risk is as follows:

[0177] 3.4.1) First, the input point cloud frame is preprocessed, mainly including removing invalid points and noise; and KDTree is used to perform spatial management of the point cloud for subsequent operations.

[0178] 3.4.2) Then perform PCA analysis within the local neighborhood; for each point in the point cloud Search for K nearest neighbors within its local neighborhood to form a local point set. Then, the three-dimensional covariance matrix of this local point set is calculated. :

[0179]

[0180] in, For local point sets The geometric center (centroid) of the . It is a point within the neighborhood;

[0181] Then, the covariance matrix Eigenvalue decomposition yields three eigenvalues. ,and Then, the points are calculated using these three eigenvalues. Local curvature :

[0182]

[0183] This curvature value reflects the degree of curvature of the local surface at that point; a larger value indicates greater curvature. This curvature is then compared with a preset curvature threshold. If a comparison is made, If so, the point is determined to be a corner feature;

[0184] 3.4.3) Then count the number of all points in the entire point cloud that are identified as corner points. And calculate its relationship with the total number of point clouds. The ratio:

[0185]

[0186] This ratio reflects the richness of corner features in the point cloud;

[0187] 3.4.4) Then, perform global PCA on the entire point cloud frame, calculate its covariance matrix, and perform eigenvalue decomposition to obtain three eigenvalues. ,and ; where the smallest eigenvalue This reflects the variance of the point cloud along the direction of the most concentrated distribution. A value that is too small indicates that the point cloud lacks distribution in one or more dimensions, and this feature directly reflects the occurrence of point cloud degradation. Then, the ratio of the intermediate eigenvalue to the minimum eigenvalue is calculated. If this ratio is too large, it indicates that the point cloud is distributed in a linear or planar manner, rather than a healthy clumped distribution.

[0188] 3.4.5) Finally, based on the indicators described in steps 3.5.2) to 3.5.4), a comprehensive judgment is made as to whether the point cloud is at risk of degradation:

[0189]

[0190] in, The threshold is set based on prior knowledge or actual application scenarios; that is, when (the proportion of corner points is too low or the minimum feature value is too small) and the feature value ratio is too large, the point cloud is judged to have a risk of degradation; if it is judged to be degraded, it will be... Set to true.

[0191] 3.5) Based on the point cloud quality indicators mentioned in steps 3.1) to 3.4), the overall point cloud quality score is calculated as follows:

[0192]

[0193] in As the baseline score, The point cloud quantity penalty coefficient is determined by step 3.2). The effective point ratio penalty coefficient is determined by step 3.3). To determine the registration consistency penalty coefficient, when Its value is when it is true. Otherwise, it is 1; The environmental degradation penalty coefficient, when and Its value is when it is true. Otherwise, it is 1.

[0194] Step 4, adaptive switching of the state machine; based on the point cloud quality score and the RTK status, the current positioning mode is adaptively selected through the state machine; the positioning modes include at least: a first mode based on LiDAR data, a second mode based on RTK data, and a third mode that fuses LiDAR and RTK data;

[0195] 4.1) The state machine execution switching modes include a first mode based on LiDAR data, a second mode based on RTK data, and a third mode that fuses LiDAR and RTK data; the judgment data used includes point cloud quality scores. and the longitude covariance of RTK and latitude covariance ; and pre-set (Threshold for point cloud quality warning). (Threshold for poor point cloud quality) and (RTK covariance threshold), and If the mean covariance of RTK is determined... If the current RTK is deemed unavailable, the state machine will be prevented from switching to a mode other than the first mode.

[0196] 4.2) If the current mode is the first mode, it can switch to the second mode; first, [the following text is missing from the original] and In comparison, if it is less than If the value is less than 1, then increment the degenerate counter; if the value is less than 1, then increment the degenerate counter. If the degradation counter is increased by a larger step size; Higher than the If the degradation counter is higher than the degradation threshold after the above logic is completed, the degradation counter will be reset to zero and the system will switch to the second mode.

[0197] 4.3) If the current mode is the second mode, it can switch to the first or third mode; first, it will... and In comparison, if it is less than If the degenerate counter is incremented and the improvement counter is reset to zero, then if it is still less than zero, the degenerate counter will be incremented and the improvement counter will be reset to zero. If the degradation counter is increased by a larger step size; Higher than the If the degradation counter is higher than the degradation threshold, the degradation counter and the improvement counter are reset to zero and the system switches to the third mode. If the improvement counter is higher than the improvement threshold, the degradation counter and the improvement counter are reset to zero and the system switches to the first mode.

[0198] 4.4) If the current mode is third, it can switch to second mode; if Higher than the If the value of the improvement counter is higher than the improvement threshold after the above logic is completed, the degradation counter and the improvement counter will be reset to zero and the system will switch to the second mode.

[0199] Step 5: Unscented Kalman Filter (UKF) observation update and Unscented Kalman Filter (UKF) time-division dimensionality reuse.

[0200] Unscented Kalman Filter (UKF) Observation Update: Based on the positioning mode selected in step 4), the observation source and observation noise used in this update are determined; the system state and observation noise are combined and an unscented transformation is applied to generate a set of sigma points; then, the possible distribution of observation values ​​is calculated by weighted summation; finally, by calculating the Kalman gain, the predicted state and covariance obtained in step 2 are corrected using the difference between the measured and predicted values, thereby obtaining the best estimate; in addition, dynamic weights are designed for the third mode to achieve smooth fusion of various observation sources;

[0201] Unscented Kalman Filter (UKF) Time-Division Dimension Multiplexing: It accepts data from different observation sources and enters the filtering process in a time-division manner according to the timestamp of each observation source, and reuses the same filter for calculation.

[0202] The UKF observation update and filter time-division dimensionality reuse are as follows:

[0203] 5.1) First, based on the state machine switching result, determine the observation source data and the observation covariance to be used in the current filter; if the current mode is the first mode, then receive the seven-dimensional observation matrix obtained from the laser point cloud registration. With the corresponding covariance matrix :

[0204]

[0205] in, , representing the world coordinate system coordinates obtained after laser point cloud registration; , representing the rotation quaternion obtained after laser point cloud registration;

[0206]

[0207] The above In the first mode, each dimension represents the covariance of the laser point cloud along the x-axis, y-axis, and z-axis, respectively. The last four dimensions are assumed to have the same value, corresponding to the covariance of the quaternion. These covariance values ​​are based on the point cloud quality score.

[0208] If the current mode is the third mode, the observations from RTK and laser point clouds will be used in a time-sharing manner, and the same filter will be used in the calculations, reusing the observation matrix and covariance matrix:

[0209]

[0210]

[0211] exist At any given moment, observation updates are performed using laser point cloud data; Continuously execute observation updates using RTK; , refers to the three-dimensional coordinates observed by RTK in the world coordinate system. , refers to the rotation quaternion of RTK observations;

[0212] The covariance matrix will also be overwritten with the covariance of the corresponding observation source at the corresponding time. When RTK is the current observation source, only the covariance in the x and y directions will be replaced with the covariance on the corresponding axis of RTK, while other positions remain unchanged.

[0213] If the current mode is RTK, then the observations will be used, and the corresponding observation matrix is ​​as follows:

[0214]

[0215] The meaning of the relevant values ​​is consistent with that of RTK when it is involved in filtering in the third mode;

[0216] 5.2) In the third mode, to make the filtering of RTK and laser point clouds smoother and prevent pose jumps, RTK weights were designed. With laser point cloud weights Both weights are based on the mean of the RTK covariance. Point cloud quality score Calculated; for The calculation method is as follows:

[0217]

[0218] in, The preset threshold must meet the following requirements. ;in These are the parameters mentioned above; This is the RTK covariance threshold; if the covariance is below this threshold, the current RTK quality is considered to meet the requirements.

[0219] for The calculation method is as follows:

[0220]

[0221] The state machine switching mechanism ensures that when both the point cloud quality and RTK quality are good, the system will not be in fusion localization mode, thus preventing issues such as... and The case where both are 1;

[0222] Depending on the current observation source type, a corresponding weighting factor will be applied to its observation noise matrix: if the current observation source is RTK, then let If the current observation source is a laser point cloud, then let ;

[0223] 5.3) Before performing UKF observation updates, the filter needs to be locked to prevent multi-threaded conflicts when RTK and laser point cloud data are available simultaneously. Then, the standard procedure is executed, including: constructing augmented state and noise matrices; generating Sigma points and performing unscented transformations, then propagating them to the observation space; calculating the mean and covariance of the predicted observations; calculating the cross-covariance matrix of the state and observations and the Kalman gain; updating the system state mean and covariance matrix using the actual observations; and unlocking the filter.

[0224] After completing the above steps, the current system state will be published as the robot pose for use by other nodes in the future.

[0225] In summary, this invention utilizes the UKF framework to fuse data from multiple sensors, improving the usability and operational stability of the cleaning robot during outdoor operation. Furthermore, by employing a time-division multiplexing filter, the resource overhead of UKF filtering is reduced, enabling it to operate on a lower-cost platform. A comprehensive point cloud quality assessment algorithm and state machine adaptation are used to achieve seamless switching between various positioning modes. Moreover, dynamic weight calculation during the fusion of multiple data sources yields a more stable positioning output, effectively solving the problem of interference from multiple observation sources.

[0226] The above embodiments are only used to illustrate the technical solutions of the present invention, and are not intended to limit it. Although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some or all of the technical features therein. Such modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the scope of the technical solutions of the embodiments of the present invention.

Claims

1. An adaptive UKF fusion positioning method for a robot, characterized in that The method comprises the following steps: Step 1, obtaining inertial measurement unit (IMU) data, point cloud data and RTK positioning data through a sensor; Step 2, performing unscented transformation on the system state of the robot by using an unscented Kalman filter (UKF), taking the IMU data as a control input, obtaining a set of predicted sigma points, performing weighted summation based on the set of predicted sigma points to obtain an optimal predicted system state, obtaining an optimal predicted covariance through the predicted system state and the set of sigma points, adding a system process noise to update the mean value and the covariance of the predicted state, and updating the sigma points as the system state; Step 3, evaluating the quality of the point cloud data; Step 4, adaptively selecting a current positioning mode through a state machine according to the quality evaluation of the point cloud data and the RTK positioning data, wherein the positioning mode comprises a first mode mainly based on the quality of the point cloud data, a second mode mainly based on the RTK positioning data, and a third mode fusing the quality of the point cloud data and the RTK positioning data; Step 5, performing observation updating and time-dimension multiplexing based on the UKF; the observation updating of the UKF is determined by the positioning mode selected in step 4, and the observation source and the observation noise used in the updating are determined; the system state and the observation noise are combined to perform unscented transformation, and a set of sigma points are generated; then the possible observation value distribution is calculated by weighted summation; finally, the predicted state and the covariance obtained in step 2 are corrected by calculating the Kalman gain and using the difference between the measurement value and the predicted value to obtain the best estimation; The time-dimension multiplexing of the UKF receives data from different observation sources, and sequentially enters the filtering process according to the time stamps of the observation sources, and performs operation by using the same filter. 2.The adaptive UKF fusion positioning method for robots of claim 1, wherein: The step 2 comprises the following steps: Step 2.1, calculating 2n+1 sigma points according to the current system state and the covariance, wherein the sigma points are uniformly distributed around the system state after being scaled according to a parameter λ, Step 2.2, propagating each sigma point through a nonlinear state transition function f to obtain a set of predicted sigma points, wherein the IMU data is taken as a control input; Step 2.3, obtaining the optimal predicted system state and the predicted covariance by using unscented transformation and weighted summation based on the set of predicted sigma points, wherein the predicted system state is obtained by weighted summation of the sigma points; Step 2.4, adding a system process noise to the predicted covariance, wherein the value represents the uncertainty of the system model. 3.The adaptive UKF fusion positioning method for robots of claim 2, wherein: The specific formula of step 2.1 is as follows: wherein represents the matrix of the column, represents the current system state, represents the error covariance matrix of the system state, n represents the dimension of the state vector, and λ represents a scaling parameter for adjusting the distribution of the sigma points so that the sigma points are uniformly distributed around the system state. 4.The adaptive UKF fusion positioning method for robots of claim 2, wherein: In step 2.2, the operation process of the state transition function f is as follows: Step 2.2.1, the nonlinear state transition function f receives a single sigma point as the system state and the IMU data as a control input vector; The system state matrix comprises three-dimensional position coordinates and three-dimensional velocities in a world coordinate system, a rotation quaternion from a local coordinate system to a world coordinate system, an accelerometer bias and a gyroscope bias; Step 2.2.2, the current position, velocity and quaternion are extracted from the system state matrix, and abnormal values and noise values are filtered, and in order to avoid non-unitization of the quaternion due to value error, if the quaternion value is found to be less than a threshold value 1e-12, the value is assigned to a unit quaternion, otherwise the quaternion is normalized, the gravity effect is subtracted from the acceleration, and the calculation methods of the velocity, position and quaternion are as follows: wherein, denotes the acceleration in the world coordinate system, denotes the gravitational acceleration, t denotes the time of the last operation, denotes the time interval, denotes the current time, denotes the rotation delta quaternion, denotes the normalization operation; Step 2.2.3, perform velocity prediction and position prediction and remove the effect of gravity; where velocity prediction is performed by first subtracting the bias in the acceleration measurements from the acceleration measurements, and then rotating the acceleration to the world coordinate frame; this step is formulated as follows:​ Then the position prediction is performed, and the position prediction is obtained by integrating the current position with the product of the velocity and the time, and the calculation formula of this part is as follows: Step 2.2.4, computing the pose prediction, first subtracts the gyroscope bias from the angular velocity Then computes the compensated angular velocity magnitude and rotation axis, and computes the rotation increment quaternion Δq in time Δt; this part avoids the zero division problem by directly using the last frame pose when the angular velocity magnitude or time interval is too small, and skips the pose prediction; the calculation formula of the pose prediction is as follows:​ Step 2.2.5, for the accelerometer bias and the gyroscope bias, it is assumed in the application that it is slowly time-varying, and the bias at the current time will be directly transmitted to the next time in each prediction, that is, it is regarded as a constant in a single running cycle and is directly transmitted to the next state: 。 5. The adaptive UKF fusion positioning method for robots of claim 1, wherein: In the step 3, the point cloud quality evaluation is based on the point cloud quantity, the proportion of valid matching points, the registration score, and whether there is a point cloud degradation risk, and the point cloud quality score is calculated, including the following steps: Step 3.1, Constructing the point cloud number penalty term When the point cloud number is less than the threshold , it is determined that the current point cloud frame is not sufficient to support the positioning requirement, and the penalty term will be set to the point cloud number penalty coefficient , otherwise the penalty term is 1; Step 3.2, Calculate the number of valid matching points penalty term ; Firstly, the map is stored by KDTree, and for each point cloud frame, the query is carried out point by point, if the point in the KDTree is inquired to be in the nearby range, then the number of nearby points Add 1, the total number of point cloud frames The effective point ratio is calculated, and the ratio is calculated as follows: a penalty term for the proportion of valid matching points The calculation is as follows: wherein, represents a preset effective matching point penalty coefficient, is a first stage threshold value, representing an alarm value to which the ratio is reduced, is a second stage threshold value, representing a danger value to which the ratio is reduced; Step 3.3, the registration score is considered not to be in compliance when the overall variance reaches a certain threshold, a flag is set true when not in compliance, false otherwise; when true, a registration score penalty term is set is the registration score penalty coefficient , otherwise it is set to 1; Step 3.

4. Determine the risk of point cloud degeneration and set a degeneration flag if there is a risk of degeneration is true, otherwise false; when it is true, a degeneration risk penalty term is set is the degeneration risk penalty coefficient , otherwise it is set to 1; Step 3.5, according to the point cloud quality indicators of steps 3.1 to 3.4, the overall point cloud quality score is calculated: wherein, represents the reference score, represents the point cloud number penalty term, whose value is the point cloud number penalty term coefficient , represents the valid matching point number penalty term, whose value is the valid point proportion penalty coefficient , represents the registration score penalty term, when is true, the value of the registration score penalty term is the registration score penalty coefficient , otherwise 1, represents the degeneration risk penalty term, when and are true, the value of the degeneration risk penalty term is the degeneration risk penalty coefficient , otherwise 1. 6.The adaptive UKF fusion positioning method for robots of claim 5, wherein: In the step 3.3, the modeling of the point cloud registration score and the judgment of the state include the following steps: Step 3.3.1, a first-in-first-out sliding window queue is maintained for storing the error indicators calculated based on the laser radar positioning results in recent period, i.e. the registration score; Step 3.3.2, before evaluation, the validity of the current frame of point cloud data is first judged; if the point cloud quantity of the current frame is less than a first preset threshold, or the proportion of valid point clouds is less than a second preset threshold, the subsequent calculation is not performed; Step 3.3.3, when the sliding window queue is filled, the variance of all error data in the window is calculated to quantify the fluctuation of the recent laser radar positioning performance; Step 3.3.4, compare the calculated variance with a third preset threshold value; if the variance is greater than the threshold value, determine that there is a risk of losing positioning, and set to true; if the variance is less than or equal to the threshold value, set to false. Step 3.3.5, window update; after completing the evaluation and decision, the latest error data is added to the window, and the oldest historical data is removed to realize the sliding update of the window.

7. The adaptive UKF fusion positioning method for robots of claim 5, wherein: The judgment method of the point cloud degradation risk in the step 3.4 includes the following steps: Step 3.4.1, the input point cloud frame is preprocessed, and the KDTree is used for spatial management of the point cloud; Step 3.4.2, perform Principal Component Analysis, PCA, within the local neighborhood; for each point in the point cloud search K nearest neighbors within its surrounding local neighborhood, constituting a local point set ; then compute the 3D covariance matrix of the local point set : wherein, denotes the geometric center of the local point set denotes the geometric center of the local point set denotes a point within the neighborhood; Eigenvalue decomposition of the covariance matrix results in three eigenvalues , and ; the local curvature of the point is calculated using the three eigenvalues The curvature value reflects the bending degree of the local surface at the point, and the greater the value, the greater the curvature; the curvature value is compared with a preset curvature threshold If , the calculation point is determined as a corner point feature. Step 3.4.3, count the number of all the corner points in the whole point cloud and calculate the corner point ratio with the total number of point cloud : The ratio reflects the richness of the corner feature in the point cloud; Step 3.4.

4. Global principal component analysis (PCA) is performed on the entire point cloud frame, the covariance matrix is calculated and eigenvalue decomposition is performed to obtain three eigenvalues , and ; wherein the minimum eigenvalue reflects the variance of the point cloud in the direction of the most concentrated distribution, and a small value indicates that the point cloud lacks distribution in one or more dimensions, directly reflecting the occurrence of point cloud degradation; the ratio of the intermediate eigenvalue to the minimum eigenvalue , a large ratio indicates that the point cloud is linearly or planarly distributed, rather than a healthy cluster distribution; Step 3.4.5, according to the indicators in steps 3.5.2 to 3.5.4, it is comprehensively judged whether the point cloud has a degradation risk: in, These represent the set threshold values ​​for the corner ratio, minimum eigenvalue, and the ratio of the intermediate eigenvalue to the minimum eigenvalue, respectively. If it is determined to be degenerate, it will... Set to true. 8.The adaptive UKF fusion positioning method for robots of claim 1, wherein: The step 4 includes the following steps: Step 4.1, the state machine performs mode switching; the basis for judging mode switching includes point cloud quality score , longitude covariance of RTK positioning data , and latitude covariance of RTK positioning data; and preset threshold values of point cloud quality warning , point cloud quality difference , and RTK positioning data covariance threshold , and ; If the RTK positioning data covariance mean value If the RTK positioning data covariance mean value is determined that the current RTK positioning data is unavailable, the state machine is prohibited from switching to the second mode and the third mode. Step 4.2, if current is first mode and can switch to second mode; then score the point cloud quality threshold value for point cloud quality alert if less than then increment the degradation counter; if less than a threshold value for point cloud quality difference then increase the degradation counter with a larger step; if higher than the then reset the degradation counter to zero; finally, if the value of the degradation counter is higher than a degradation threshold value, then reset the degradation counter to zero and switch to the second mode; Step 4.3, if the current is the second mode and can switch to the first mode or the third mode; then score the point cloud quality with the threshold of the point cloud quality alert In contrast, if less than then increment the degradation counter and reset the improvement counter to zero; if also less than the threshold of the point cloud quality difference then increase the degradation counter with a large step; if higher than then reset the degradation counter to zero and increment the improvement counter; finally, if the value of the degradation counter is higher than the degradation threshold, then reset the degradation counter and the improvement counter to zero and switch to the third mode; if the value of the improvement counter is higher than the improvement threshold, then reset the degradation counter and the improvement counter to zero and switch to the first mode; Step 4.4, if the current mode is the third mode and it is possible to switch to the second mode; if the point cloud quality score is higher than the threshold of the point cloud quality warning then the improvement counter is incremented; finally, if the value of the improvement counter is higher than the improvement threshold, the deterioration counter and the improvement counter are reset to zero and the second mode is switched to. 9.The adaptive UKF fusion positioning method for robots of claim 8, wherein: In the step 5, the observation update of the unscented Kalman filter UKF and the time-sharing dimension multiplexing of the filter include the following steps: Step 5.1, according to the state machine switching result, decide the current entering filter observation source data and the used observation covariance; if the current is the first mode, receive the seven-dimensional observation matrix obtained by point cloud registration and the corresponding covariance matrix : wherein, represents the world coordinate system coordinate obtained after point cloud registration; represents the rotation quaternion obtained after point cloud registration; respectively represent the covariance of the x-axis direction, the covariance of the y-axis direction, and the covariance of the z-axis direction of the laser point cloud in the first mode in sequence, are the same value by default, corresponding to the covariance of the quaternion in the first mode, and the value of the covariance is obtained based on the point cloud quality score; If the current is the third mode, the RTK positioning data and the observation value of the point cloud are used in time-sharing, and the same filter is used in the calculation, and the observation matrix and the covariance matrix are multiplexed: At a time instant, performing an observation update with point cloud data; a time instant, performing an observation update with RTK positioning data; represents a three-dimensional coordinate in the world coordinate system observed from the RTK positioning data, represents a rotation quaternion observed from the RTK positioning data; The covariance matrix is covered as the covariance of the corresponding observation source at the corresponding time, wherein when the RTK positioning data is used as the current observation source, only the covariance of the x and y directions is replaced by the covariance of the x and y axes corresponding to the RTK positioning data, and the other positions remain unchanged; If the current is the second mode, the observation value of RTK positioning data is used, and the corresponding observation matrix is: Step 5.2, in the third mode, RTK positioning data covariance mean based Designing RTK positioning data weights , based on point cloud quality score Designing point cloud quality weights ; The calculation is as follows: wherein, represents a preset point cloud quality threshold, which needs to meet ; wherein represents a point cloud quality warning threshold; represents an RTK positioning data covariance threshold, and if the RTK positioning data covariance is lower than the current RTK positioning data quality is considered to meet the requirements. The calculation is as follows: if the point cloud quality score is greater than a point cloud quality threshold and the RTK positioning data quality meets the requirements, not in the third mode, and none of them is 1; According to the current observation source type, a corresponding weight factor is applied to the observation noise matrix; if the current observation source is RTK positioning data, the covariance matrix is updated ; if the current observation source is point cloud data, the covariance matrix is updated ; Step 5.3, before the observation update of the unscented Kalman filter UKF, the filter is locked, then the augmented state and noise matrix are constructed, the Sigma point is generated and the unscented transformation is carried out, and it is propagated to the observation space; the mean and covariance of the predicted observation value are calculated; the mutual covariance matrix of the state and the observation and the Kalman gain are calculated; the actual observation value is used to update the mean and covariance matrix of the system state; the filter is unlocked.

10. An adaptive UKF fusion positioning method for a robot, characterized in that: The adaptive UKF fusion positioning method for robots according to any one of claims 1 to 9 is used for adaptive UKF fusion positioning of an outdoor cleaning robot.

Images