Robust adaptive unscented kalman filter based slam method and system for multi-sensor information fusion

Abstract

The application discloses a SLAM method and system based on a robust adaptive unscented Kalman filter multi-sensor information fusion, which comprises the following steps: generating symmetric distribution sampling points according to system parameters and calculating corresponding weights, substituting the sampling points into a nonlinear function to complete one-step prediction, obtaining one-step prediction values of states and covariance through weighted summation, generating new sampling points based on the prediction results to perform observation prediction, and synchronously calculating an observation correlation covariance matrix; meanwhile, multi-step sub-processes are performed, including sampling point processing, weight application, operation monitoring and other operations, a dynamic noise estimation and adaptive sensor weight distribution mechanism is designed, a robust adaptive filter framework is combined, multi-source sensor complementary information is fully integrated, and abnormal data interference is effectively resisted, so that the positioning accuracy and robustness of the SLAM system in a complex environment are improved, and the application is suitable for real-time positioning and map construction scenes of autonomous mobile devices.

Description

Technical Field

[0001] This invention relates to the field of complex scene localization technology, and in particular to a SLAM method and system based on robust adaptive unscented Kalman filtering and multi-sensor information fusion. Background Technology

[0002] As autonomous mobile devices are increasingly used in complex scenarios, the requirements for environmental adaptability, positioning accuracy, and robustness in localization and mapping (LSM) technologies continue to rise. Single sensors, limited by their inherent sensing characteristics, are prone to data distortion or information loss in complex environments such as occlusion, lighting changes, and dynamic interference, making it difficult to meet high-precision positioning needs. Multi-sensor information fusion technology, by integrating complementary information from different types of sensors, has become a key approach to improving the performance of Simultaneous Localization and Mapping (SLAM) systems. Unscented Kalman filtering, with its efficient processing capabilities for nonlinear systems, has been widely used in multi-sensor fusion. However, traditional filtering methods lack adaptive adjustment mechanisms in weight allocation and noise processing, making it difficult to adapt to the characteristics of sensor data in complex dynamic environments. Therefore, there is an urgent need to construct a multi-sensor fusion SLAM architecture that combines robustness and adaptability.

[0003] Existing technologies have two significant shortcomings: First, the sampling point weights and filtering parameters of traditional unscented Kalman filtering are mostly fixed and cannot be dynamically adjusted according to the quality of sensor data. When some sensors experience data anomalies or sudden noise changes, the fusion results are easily contaminated, reducing the robustness of the system. Second, the multi-sensor weight allocation lacks a dynamic matching mechanism with real-time noise characteristics and usually adopts an empirical fixed allocation strategy, which makes it difficult to fully leverage the complementary advantages of different sensors in complex environments. This results in the effectiveness of the fused data not being fully explored, which in turn affects the positioning accuracy and environmental adaptability of the SLAM system and fails to meet the reliable operation requirements of autonomous mobile devices in complex scenarios. Summary of the Invention

[0004] To overcome the shortcomings and deficiencies of existing technologies, this invention provides a SLAM method and system based on robust adaptive unscented Kalman filtering and multi-sensor information fusion.

[0005] The technical solution adopted in this invention is a SLAM method based on robust adaptive unscented Kalman filtering and multi-sensor information fusion, comprising the following steps: S1, determining a symmetrically distributed set of sampling points according to the system dimension, the set including sampling points corresponding to the mean and positive and negative symmetrical sampling points obtained by operating on the state mean and covariance matrix; S2, calculating the state weight and covariance weight corresponding to each sampling point according to the scaling factor, adjustment parameters and distribution control parameters set by the system; S3, substituting the sampling points obtained in S1 into the state error nonlinear function to complete one-step prediction calculation for each sampling point; S4, using the weights calculated in S2 to obtain the result from S3. S4. The predicted sampling points are weighted and summed to obtain the one-step state prediction value. At the same time, the one-step state covariance prediction value is calculated by combining the weights, the deviation between the predicted sampling points and the one-step state prediction value, and the system process noise covariance matrix. S5. Based on the one-step state prediction value and the one-step state covariance prediction value obtained in S4, a new set of sampling points is generated by symmetrical distribution sampling. S6. The sampling points obtained in S5 are substituted into the observation nonlinear function to obtain the one-step observation prediction value corresponding to each sampling point. The one-step observation prediction value is then weighted and summed by combining the weights in S2 to obtain the mean of the one-step observation prediction value. At the same time, the observation covariance matrix and the cross-covariance matrix between the state and the observation are calculated.

[0006] Furthermore, the sampling point calculation satisfies: ,in, For the i-th sampling point at time k, Let k be the mean of the state at time k. For the system dimension, Scaling factor Let k be the state covariance matrix at time k. Take an integer from 1 to 2n.

[0007] Furthermore, the calculation of sampling point weights satisfies: , ,in, Let be the state weight of the i-th sampling point at time k. Let the covariance weights of the 0th sampling point at time k be _____. , For scaling parameters, For distributed control parameters, To adjust the parameters, The system dimension.

[0008] Furthermore, the state covariance prediction in one step satisfies: ,in, for One-step prediction of the state covariance at time step. Let be the weight of the i-th sampling point at time k. for The i-th prediction sampling point at time i, for One-step prediction of state at time step. Let k be the system process noise covariance matrix at time k. This is the matrix transpose operation.

[0009] Furthermore, the measurement noise covariance estimate satisfies: ,in, The noise covariance estimate is measured at time k. The length of the sliding window. Let j be the information sequence at time j. Let be the covariance weight of the i-th sampling point at time k. for The predicted value of the i-th observation at time i, for Observe the mean at each step to predict the value.

[0010] Furthermore, the sensor weight allocation satisfies: ,in, For lidar weights, For visual sensor weights, To measure the trace of the noise covariance matrix for lidar, The trace of the noise covariance matrix is ​​measured for a vision sensor.

[0011] Further, step S3 includes the following sub-steps: S31, obtaining all symmetrically distributed sampling points generated in S1, and determining the state dimension and numerical characteristics corresponding to each sampling point; S32, calling a preset state error nonlinear function, inputting the value of each sampling point into the function in sequence, and performing state transformation and error propagation according to the operation logic defined by the function; S33, monitoring the operation process of each sampling point in real time to ensure that the input parameters match the function's domain and avoid operational anomalies; S34, recording the result of each sampling point after the nonlinear function operation, forming a set of predicted sampling points at time k+1, providing basic data for subsequent state estimation.

[0012] Further, step S4 includes the following sub-steps: S41, extracting the weights of each sampling point calculated in S2, confirming the correspondence and numerical validity between the state weights and the covariance weights; S42, performing a weighted product operation on the predicted sampling points obtained in S3 and the corresponding state weights, summing all product results to obtain the one-step predicted state value; S43, calculating the deviation between each predicted sampling point and the one-step predicted state value, transposing the deviation and multiplying it by the original deviation, then combining it with the corresponding covariance weights for weighted summation; S44, adding the weighted summation result to the system process noise covariance matrix to obtain the one-step predicted state covariance value, thus completing the prediction process of state and covariance.

[0013] Further, S5 includes the following sub-steps: S51, obtaining the one-step state prediction value and the one-step state covariance prediction value obtained in S4, and verifying the dimensional consistency and numerical rationality of the two; S52, calculating the square root of the one-step state covariance prediction value, and obtaining the operation coefficients by combining the system dimension and scaling factor; S53, performing addition and subtraction operations on the one-step state prediction value with the product of the operation coefficients and the square root of the covariance, respectively, to generate new symmetric sampling points; S54, sorting and numbering the generated sampling points to ensure that the position of each sampling point matches the corresponding operation logic, forming a complete set of one-step state prediction sampling points.

[0014] A robust adaptive unscented Kalman filter-based multi-sensor information fusion SLAM system is applied to this SLAM method. The system includes: a multi-source sensor data acquisition unit for acquiring triaxial and angular acceleration data from an IMU, point cloud data from a LiDAR, and image data from a vision camera, enabling synchronous acquisition and transmission of multi-sensor data; a sampling point generation and weight calculation unit connected to the multi-source sensor data acquisition unit, which generates symmetrically distributed sampling points based on system parameters and calculates corresponding weights; and a nonlinear prediction calculation unit connected to the sampling points. The generation and weight calculation unit is connected to perform one-step prediction of sampling points and one-step prediction of state and covariance. The observation prediction and covariance calculation unit is connected to the nonlinear prediction calculation unit to complete the generation of observation sampling points, the calculation of observation mean and covariance matrix. The robust adaptive fusion unit is connected to the observation prediction and covariance calculation unit to perform measurement noise estimation, weight allocation and filter gain correction. The state correction and output unit is connected to the robust adaptive fusion unit to correct the displacement change of the IMU and output the corrected positioning data to the SLAM backend. All units interact and work collaboratively in real time through the data bus.

[0015] Beneficial Effects: This invention proposes a robust adaptive unscented Kalman filter-based multi-sensor information fusion SLAM method and system. Addressing the limitations of traditional methods such as fixed sampling point weights and filtering parameters, and insufficient robustness, this invention dynamically generates symmetrically distributed sampling points and calculates state weights and covariance weights in real time based on system parameters. Combined with a sliding window mechanism, it dynamically estimates the measurement noise covariance, accurately adapting to changes in sensor data quality and resisting interference from anomalous data, significantly improving the system's robustness in complex environments. To address the lack of a dynamic matching mechanism in multi-sensor weight allocation, an adaptive weight allocation strategy is designed based on the characteristics of the sensor measurement noise covariance matrix. This fully leverages the complementary information from LiDAR, visual cameras, and IMUs, achieving efficient fusion of multi-source data and significantly improving the positioning accuracy and environmental adaptability of the SLAM system. Furthermore, the modular system units enable seamless collaboration across the entire process of data acquisition, sampling point generation, nonlinear prediction, observation fusion, and state correction, ensuring the continuity and reliability of operations at each stage and providing strong support for the stable operation of autonomous mobile devices in complex scenarios. Attached Figure Description

[0016] Figure 1 This is a flowchart illustrating the overall steps of the method according to an embodiment of the present invention;

[0017] Figure 2 This is a flowchart illustrating the overall steps of a method according to another embodiment of the present invention;

[0018] Figure 3 This is a diagram showing the system unit composition of the present invention. Detailed Implementation

[0019] It should be noted that, unless otherwise specified, the embodiments and features described in this application can be combined with each other. The application will be further described in detail below with reference to the accompanying drawings and specific embodiments.

[0020] Example 1

[0021] like Figure 1As shown, the SLAM method based on robust adaptive unscented Kalman filtering and multi-sensor information fusion includes the following steps: S1, determining a symmetrically distributed set of sampling points according to the system dimension, which includes sampling points corresponding to the mean and positive and negative symmetrical sampling points obtained by operating on the state mean and covariance matrix; S2, calculating the state weight and covariance weight corresponding to each sampling point according to the scaling factor, adjustment parameters, and distribution control parameters set by the system; S3, substituting the sampling points obtained in S1 into the state error nonlinear function to complete one-step prediction calculation for each sampling point; S4, using the weights calculated in S2 to adjust the prediction weights obtained in S3. S1. The sample points are weighted and summed to obtain the one-step state prediction value. At the same time, the one-step state covariance prediction value is calculated by combining the weights, the deviation between the predicted sample points and the one-step state prediction value, and the system process noise covariance matrix. S2. Based on the one-step state prediction value and the one-step state covariance prediction value obtained in S4, a new set of sample points is generated by symmetrical distribution sampling. S3. The sample points obtained in S5 are substituted into the observation nonlinear function to obtain the one-step observation prediction value corresponding to each sample point. The one-step observation prediction value is then weighted and summed by combining the weights in S2 to obtain the mean of the one-step observation prediction value. At the same time, the observation covariance matrix and the cross-covariance matrix between the state and the observation are calculated.

[0022] Step S1 determines a symmetrically distributed set of sampling points based on the key parameter of system dimension. This process must strictly adhere to the sampling principle of unscented transformation. First, the specific value of the system dimension is determined, typically based on the composition of the SLAM system's state vector, including key state variables such as position, attitude, and velocity. Common system dimension values ​​range from 6 to 15, with the specific value determined based on sensor configuration and positioning requirements in the actual application scenario. Based on the determined system dimension, sampling points corresponding to the mean are generated. These sampling points directly correspond to the optimal estimated mean of the system state at the current moment and are the core reference points for the sampling point set. Then, based on the current state covariance matrix, the square root form of the covariance matrix is ​​obtained through matrix square root operations. This, combined with the system dimension and a preset scaling factor, yields the sampling interval coefficient. This coefficient is then multiplied by the square root of the covariance matrix to obtain positive and negative symmetrical offsets. Based on the sampling points corresponding to the mean, the calculated offsets are superimposed and subtracted respectively to generate two sets of symmetrical sampling points of equal quantity, with the number of sampling points in both sets consistent with the system dimension. The final set of sampling points includes one mean sampling point and two symmetrical offset sampling points with twice the system dimension. All sampling points are evenly distributed around the state mean, which can fully cover the probability distribution space of the system state. This provides sufficient sample support for the subsequent mapping operation of nonlinear functions, ensures the integrity and representativeness of the sampling process, and lays the foundation for the accuracy of the entire filtering process.

[0023] Step S2 focuses on the precise calculation of sampling point weights. It requires combining multiple key parameters set by the system and simultaneously solving for state weights and covariance weights according to a preset weight allocation rule. First, the specific values ​​of three core control parameters are determined. The scaling factor typically ranges from 0.0001 to 0.1; this parameter adjusts the distribution range of sampling points, balancing local accuracy and global coverage. The adjustment parameter is generally set between 0 and 3, primarily controlling the sparsity of sampling points to adapt to system models with different degrees of nonlinearity. The distribution control parameter is typically around 2, used to match the probability distribution characteristics of the system state; this parameter is optimal when the system state conforms to a Gaussian distribution. Based on these parameters, the comprehensive scaling coefficient is calculated using a preset computational logic. This coefficient is directly related to the system dimension, scaling factor, and adjustment parameter, and is the core intermediate variable for weight calculation. Subsequently, state weights and covariance weights are calculated for each sampling point. For sampling points corresponding to the mean, the state weights and covariance weights require separate special formulas. The covariance weights also need to be corrected using distribution control parameters to improve the adaptability of the weights to the covariance estimation. For the remaining symmetrically distributed sampling points, a unified weight calculation formula is used, ensuring that the weights of each group of symmetrical sampling points are equal, and that the sum of the state weights of all sampling points satisfies the normalization condition. During the weight calculation process, the numerical validity of each weight must be rigorously verified to ensure that both the state weights and covariance weights are non-negative real numbers and conform to the preset value range. Precise weight allocation allows sampling points at different locations to play their respective roles in subsequent estimation calculations, achieving an accurate representation of the system's state probability distribution.

[0024] Step S3 substitutes all the sampling points generated in S1 into the state error nonlinear function to complete one-step prediction calculation for each sampling point. This process is crucial for achieving nonlinear state mapping. First, the complete set of sampling points generated in S1 is extracted from the data storage unit, including the mean sampling points and all symmetrical offset sampling points. These are processed sequentially according to their numbering to avoid omissions or duplicate calculations. Then, the preset state error nonlinear function is called. This function is built based on the kinematic model and error propagation characteristics of the SLAM system and can accurately describe the nonlinear transformation relationship of the system state from the current moment to the next moment, including the influence of sensor noise, motion disturbances, and other factors on state changes. The specific values ​​of each sampling point are input into the nonlinear function sequentially according to the dimension of the state vector. Following the function's defined operational logic, a series of operations are performed, including matrix operations, nonlinear transformations, and error superposition. The calculation process for each sampling point is independent, and parallel computing can improve computational efficiency. During the calculation process, it is necessary to monitor in real time whether the input parameters of each sampling point meet the function's domain requirements, and promptly investigate anomalies such as numerical overflow and parameter dimension mismatch to ensure the stability of the calculation process. For each sampling point, the corresponding predicted sampling point for the next time step is output after the calculation. This predicted sampling point includes the state information after nonlinear transformation and the result of error accumulation. After all sampling points have been calculated, the predicted sampling points are organized and archived according to their original numbering order to form a complete set of predicted sampling points for time step k+1. This set fully preserves the distribution characteristics of the original sampling points and incorporates the influence of the system's nonlinear dynamic characteristics, providing accurate input data for subsequent weighted estimation.

[0025] Step S4 calculates the one-step state prediction and the one-step state covariance prediction through weighted summation and covariance calculation. First, it retrieves all sampling point weights calculated in S2 from the data cache, including the state weight and covariance weight for each sampling point. It verifies the correspondence between weights and sampling points to ensure each predicted sampling point matches the correct weight, while also checking the numerical range and normalization characteristics of the weights to eliminate the influence of invalid weights on the calculation results. Then, it initiates the calculation process for the one-step state prediction. It performs element-wise multiplication of the value of each predicted sampling point obtained in S3 with its corresponding state weight to obtain the weighted contribution value for each sampling point. Finally, it sums the weighted contribution values ​​of all sampling points to obtain the one-step state prediction for the current moment. This value is a preliminary estimate of the system state at the next moment, integrating information from all sampling points. Next, it calculates the one-step state covariance prediction. It first calculates the deviation between each predicted sampling point and the one-step state prediction by subtracting elements to obtain the state deviation vector for each sampling point. Each state deviation vector is transposed, and then the transposed deviation vector is multiplied by the original deviation vector to obtain the deviation covariance matrix. The deviation covariance matrix of each sampling point is multiplied by its corresponding covariance weight to obtain a weighted deviation covariance matrix. All weighted deviation covariance matrices are summed to obtain the total deviation covariance. Finally, this sum is added to a preset system process noise covariance matrix. The value of the system process noise covariance matrix needs to be preset based on the sensor's noise characteristics and the interference intensity of the motion scene. This yields a one-step predicted value of the state covariance, which comprehensively reflects the uncertainty of the system state at the next moment, providing key parameter support for subsequent observation and update processes.

[0026] Step S5 generates a new set of sampling points based on the prediction results obtained in the preceding steps. This is a crucial operation in unscented filtering for implementing sampling during the observation update phase. First, the one-step predicted state value and the one-step predicted state covariance value calculated in S4 are obtained. The dimensions of these two parameters are rigorously verified to ensure that the vector dimension of the one-step predicted state value matches the matrix dimension of the one-step predicted state covariance value, meeting the system dimension requirements. Simultaneously, the numerical rationality of the two parameters is checked, eliminating outliers that exceed the physical range. Next, the calculation of the square root of the covariance matrix is ​​initiated. The Cholesky decomposition algorithm is used to decompose the one-step predicted state covariance value. This algorithm efficiently and stably obtains the lower triangular square root matrix of the covariance matrix, ensuring the positive definiteness and uniqueness of the decomposition result. Based on the square root matrix obtained from the decomposition, combined with the determined system dimension and a preset scaling factor, sampling coefficients are calculated according to a fixed operational logic. These coefficients are used to adjust the offset amplitude of the sampling points, balancing prediction accuracy and computational efficiency. Using the one-step predicted state value as a benchmark, the sampling coefficients are multiplied by the square root of the covariance matrix to obtain the sampling offset vector, whose dimension matches the system dimension. The one-step state prediction value and the sampling offset vector are added and subtracted respectively to generate two sets of symmetrically distributed sampling points, with the number of sampling points in each set equal to the system dimension. Finally, all generated sampling points are systematically sorted and numbered. The sorting rule is determined by the offset direction and magnitude between the sampling point and the one-step state prediction value, and the numbering order is consistent with step S1, ensuring that the positional information of each sampling point corresponds to the computational logic. This ultimately forms a complete set including one mean sampling point and two symmetrical sampling points equal to twice the system dimension, providing suitable input samples for the mapping operation of the observed nonlinear function.

[0027] Step S6, which performs observation prediction and covariance matrix calculation, is the core process for achieving multi-sensor information fusion and observation updates. First, all new sampling points generated in S5 are extracted and processed sequentially according to their numbering, ensuring each sampling point participates in the observation prediction calculation. A preset observation nonlinear function is invoked. This function is built based on a multi-sensor observation model, including the observation characteristics of sensors such as LiDAR, visual cameras, and IMUs. It nonlinearly maps system state variables to sensor observation data formats, accurately describing the correspondence between state variables and observed values. The state value of each sampling point is substituted into the observation nonlinear function, and the conversion from state variable to observed value is completed according to the function's defined calculation flow. Each sampling point generates a one-step prediction value, which simulates the ideal observation result of the sensor in the corresponding state. After completing the observation prediction for all sampling points, the state weights calculated in S2 are extracted. Based on the correspondence between weights and sampling points, each one-step prediction value is multiplied by its corresponding state weight to obtain the weighted contribution of each prediction value. All weighted contributions are summed to obtain the one-step prediction mean, which is the optimal estimate of the sensor observation value at the next time step. Simultaneously, the calculation process for the covariance matrix is ​​initiated. First, the deviation between the predicted value at each observation step and the mean predicted value at the observation step is calculated. After transposing the deviation, matrix multiplication is performed with the original deviation. Then, a weighted sum is calculated using the corresponding covariance weights to obtain the observation covariance matrix. Furthermore, the deviation between each predicted sampling point and the predicted value at the state step, as well as the deviation between the predicted value at the observation step and the mean predicted value at the observation step, are calculated. After transposing and matrix multiplying the two sets of deviations respectively, a weighted sum is calculated using the covariance weights to obtain the cross-covariance matrix between the state and the observations. These two covariance matrices together provide key statistical characteristic parameters for subsequent filter gain calculation and state correction.

[0028] Preferably, the sampling point calculation satisfies: ,in, For the i-th sampling point at time k, Let k be the mean of the state at time k. For the system dimension, Scaling factor Let k be the state covariance matrix at time k. Take an integer from 1 to 2n.

[0029] Specifically, the sampling point calculation is based on the sampling principle of unscented transformation to generate symmetrical sampling points that can accurately represent the probability distribution of the system state. Determining the system state mean and covariance matrix are core parameters for describing the state distribution. To ensure that the sampling points cover key areas of the state space, the diffusion range of the state distribution is obtained through the square root operation of the covariance matrix. Simultaneously, a summation term of the system dimension and scaling factor is introduced to scale the square root result, balancing the density and coverage of the sampling points. The formula uses addition and subtraction operations to generate positive and negative symmetrical sampling points because the system state usually conforms to a Gaussian distribution, and symmetrical sampling can comprehensively reflect the symmetry characteristics of the state distribution, avoiding sampling bias. The system dimension is determined based on the composition of the SLAM system state vector, including key state quantities such as position and attitude, and its value ranges from 6 to 15. The scaling factor is used to adjust the distance between the sampling point and the mean, with a value range from 0.0001 to 0.1. Smaller values ​​improve local sampling accuracy, while larger values ​​enhance global coverage. The sampling point index is a positive integer, and the number is set to twice the system dimension to ensure that the number of sampling points is sufficient to represent the state distribution. The formula is based on the idea of ​​"approximating nonlinear distributions with finite sampling points" in unscented filtering. Through mathematical operations, the statistical information of the state mean and covariance matrix is ​​transformed into specific sampling points. In implementation, the system state mean and covariance matrix at the current moment are obtained first, and the matrix square root operation and scaling factor related calculations are completed. Then, a symmetric sampling point set is generated through addition and subtraction operations, which provides comprehensive sample support for subsequent nonlinear function mapping and ensures the scientificity and effectiveness of the sampling process.

[0030] Preferably, the calculation of sampling point weights satisfies: , ,in, Let be the state weight of the i-th sampling point at time k. Let the covariance weights of the 0th sampling point at time k be _____. , For scaling parameters, For distributed control parameters, To adjust the parameters, The system dimension.

[0031] Specifically, the sampling point weight calculation is based on the weight allocation criterion of unscented transformation, assigning reasonable weights to different sampling points so that the weighted sampling points can accurately reconstruct the mean and covariance of the system state. First, a comprehensive scaling coefficient is constructed by introducing scaling parameters, adjustment parameters, and distributed control parameters. This coefficient integrates the influence of system dimension and various control parameters to achieve flexible adjustment of weight allocation. The derivation of the state weight formula follows the principle of "unbiased mean reconstruction." The ratio of the comprehensive scaling coefficient to the summation term of the system dimension and scaling factor ensures that the sum of all state weights satisfies the normalization condition. The covariance weight formula introduces a correction term related to the distributed control parameters based on the state weights because covariance estimation is more sensitive to the marginal distribution of sampling points. The correction term is needed to improve the accuracy of covariance estimation, especially for sampling points corresponding to the mean, where a separate correction logic needs to be designed to strengthen their contribution to covariance estimation. The scaling parameter ranges from 0.01 to 1, the adjustment parameter ranges from 0 to 3, and the distributed control parameter is typically set to 2 to adapt to a Gaussian distribution. The formula is based on the logic of "matching the weights with the contribution of the sampling points to the state distribution." The closer a sampling point is to the mean, the higher its contribution to the state distribution, and the larger its weight. During implementation, control parameters are first set according to system requirements, and the comprehensive scaling factor is calculated. Then, these factors are substituted into the state weight and covariance weight formulas to generate the corresponding weight for each sampling point. Simultaneously, the non-negativity and normalization characteristics of the weights are verified to ensure that the weights accurately reflect the importance of the sampling points, providing a reliable basis for subsequent weighted estimation.

[0032] Preferably, the one-step prediction of state covariance satisfies: ,in, for One-step prediction of the state covariance at time step. Let be the weight of the i-th sampling point at time k. for The i-th prediction sampling point at time i, for One-step prediction of state at time step. Let k be the system process noise covariance matrix at time k. This is the matrix transpose operation.

[0033] Specifically, the one-step prediction of state covariance is based on the weighted least squares principle and the covariance propagation law. It is used to calculate the one-step predicted value of state covariance, integrating the statistical information of the predicted sampling points and the system process noise. First, it is determined that the one-step predicted value of state covariance must reflect the uncertainty of the state at the next moment. This uncertainty comes from two parts: the dispersion of the predicted sampling points and the system process noise. For the calculation of the dispersion of the sampling points, the uncertainty of a single sampling point is characterized by the deviation between each predicted sampling point and the one-step predicted value of state. The product of the transpose of the deviation and the original deviation is the deviation covariance matrix. Then, a weighted sum is performed in combination with the covariance weights to achieve the synthesis of the uncertainty of all sampling points. The system process noise covariance matrix is ​​introduced because there are unavoidable interferences such as motion disturbances and sensor noise during the operation of the SLAM system. This matrix is ​​needed to quantify the impact of such interferences on the state uncertainty. Finally, the two parts of uncertainty are integrated through addition to form the complete one-step predicted value of state covariance. Regarding parameter values, the covariance weights are calculated in accordance with the sampling point weights. The system process noise covariance matrix is ​​set based on the sensor noise characteristics and motion scenario, with matrix elements being non-negative real numbers and diagonal elements representing the noise intensity of each state dimension. The formula is based on the statistical law that "total uncertainty equals the sum of individual uncertainties," and mathematical operations are used to quantify and integrate the statistical characteristics of sampling points with the influence of external noise. During implementation, the predicted sampling points, one-step predicted state values, covariance weights, and process noise covariance matrix are first obtained. Then, the deviation calculation, deviation covariance matrix solution, weighted summation, and matrix addition operations are performed sequentially to finally obtain the one-step predicted state covariance value. This value provides a key uncertainty parameter for subsequent observation and update stages, ensuring the accuracy of the filtering process.

[0034] Preferably, the measurement noise covariance estimate satisfies: ,in, The noise covariance estimate is measured at time k. The length of the sliding window. Let j be the information sequence at time j. Let be the covariance weight of the i-th sampling point at time k. for The predicted value of the i-th observation at time i, for Observe the mean at each step to predict the value.

[0035] Specifically, the measurement noise covariance estimation is based on the statistical characteristics of the information sequence and the observation prediction error. It is used to dynamically estimate the measurement noise covariance matrix, improving the robustness and adaptability of the filtering. The sample covariance is calculated using the information sequence within a sliding window. The information sequence is the difference between the observed value and the observed predicted value; its statistical characteristics directly reflect the changes in measurement noise. The sliding window design captures the time-varying characteristics of noise, and the window length needs to be set according to the noise variation frequency. Subsequently, the deviation between the observed predicted value and the mean of the one-step prediction is calculated, and a weighted sum is obtained by combining the covariance weights to obtain the theoretical covariance of the observed prediction. This part characterizes the uncertainty of the observation prediction process itself. The measurement noise covariance estimate is obtained by the difference between the sample covariance and the theoretical covariance because the sample covariance includes both the observation prediction uncertainty and the measurement noise. Removing the theoretical covariance separates the pure measurement noise information. The sliding window length ranges from 5 to 20. A longer window length results in more stable noise estimation but reduces real-time performance; a balance between stability and real-time performance needs to be struck based on the application scenario. The formula is based on "data-driven noise estimation," which dynamically adjusts the noise matrix using real-time data from the information sequence, overcoming the limitations of traditional fixed noise matrices. During implementation, the sliding window length is first set, the information sequence within the window is collected, and the sample covariance and the observation-prediction theoretical covariance are calculated. Then, the estimated measurement noise covariance is obtained through difference calculation. During implementation, the information sequence within the window needs to be updated in real time to ensure that the noise estimation can track actual noise changes, providing accurate noise parameters for adaptive weight allocation and filter gain correction.

[0036] Preferably, the sensor weight allocation satisfies: ,in, For lidar weights, For visual sensor weights, To measure the trace of the noise covariance matrix for lidar, The trace of the noise covariance matrix is ​​measured for a vision sensor.

[0037] Specifically, the sensor weight allocation calculation is based on the statistical characteristics of sensor measurement noise to achieve adaptive weight allocation for multiple sensors, fully leveraging the complementary advantages of different sensors. First, the principle for sensor weight allocation is determined as "the lower the noise, the greater the weight." The trace of the measurement noise covariance matrix comprehensively reflects the overall noise level of the sensor; the smaller the trace value, the higher the sensor's measurement accuracy, and the greater the weight should be allocated. By taking the reciprocal of the trace of the noise covariance matrix, the noise level is converted into a weight contribution. The reciprocal operation achieves the mapping relationship of "the lower the noise, the greater the contribution." Subsequently, the sum of the weight contributions of two sensors is used as a normalization factor to normalize the contribution of a single sensor, ensuring that the sum of the weights of the two sensors is 1, satisfying the normalization requirement for weight allocation. The weight of the other sensor is obtained by subtracting the weight of the previous sensor from 1, simplifying the calculation process while ensuring the integrity of the weight allocation. The traces of the measurement noise covariance matrices of both the LiDAR and vision sensors are positive real numbers. These trace values ​​are obtained through sensor calibration experiments and statistical analysis of actual test data. The trace value of the LiDAR is typically smaller than that of the vision sensor, especially under varying lighting conditions, where the visual sensor's trace value increases significantly. The formula is based on "adaptive allocation logic based on noise level," abandoning the traditional empirical fixed weight strategy to achieve dynamic matching between weights and the real-time performance of the sensors. During implementation, the measurement noise covariance matrices of the two sensors are first obtained through calibration and real-time monitoring. The traces of these matrices are calculated, and their reciprocals are taken. These reciprocals are then substituted into the formula to calculate the weights. Throughout the implementation process, the traces of the noise covariance matrix need to be updated in real time to ensure that the weights can be dynamically adjusted according to the actual operating state of the sensors. This allows the LiDAR and vision sensors to perform optimally in different environments, improving the effectiveness of multi-sensor fusion.

[0038] Preferably, step S3 includes the following sub-steps: S31, obtaining all symmetrically distributed sampling points generated in S1, and determining the state dimension and numerical characteristics corresponding to each sampling point; S32, calling a preset state error nonlinear function, inputting the value of each sampling point into the function in sequence, and performing state transformation and error propagation according to the operation logic defined by the function; S33, monitoring the operation process of each sampling point in real time to ensure that the input parameters match the function's domain and avoid operational anomalies; S34, recording the result of each sampling point after the nonlinear function operation, forming a set of predicted sampling points at time k+1, providing basic data for subsequent state estimation.

[0039] Specifically, step S3 performs nonlinear prediction calculations for the sampling points through four sub-steps, ensuring that each sampling point accurately completes state transitions and error propagation. Step S31 first extracts the complete set of symmetrically distributed sampling points generated in S1 from the data storage module. This set includes one mean sampling point and symmetrically offset sampling points with twice the system dimension. The system dimension typically ranges from 6 to 15. It is necessary to determine the state dimension (such as position, attitude, velocity, etc.) and specific numerical characteristics corresponding to each sampling point to ensure the integrity and identifiability of the sampling point data, laying the foundation for subsequent calculations. Step S32 calls a preset state error nonlinear function. This function is constructed based on the kinematic model of the SLAM system and the error propagation law, including influencing factors such as sensor noise and motion disturbances. The values ​​of each sampling point are sequentially input into the function according to the sampling point number order, strictly following the matrix operations, nonlinear transformations, and error superposition logic defined in the function definition to achieve a nonlinear mapping of the state from the current moment to the next moment, ensuring the standardization of the calculation logic. Step S33 monitors the computation process for each sampling point in real time, focusing on verifying whether the input parameters meet the function's domain requirements and investigating anomalies such as numerical overflow and dimensional mismatch. If a problem is found, the computation is immediately paused and feedback is provided to ensure the stability and accuracy of the computation process and prevent abnormal data from entering subsequent stages. Step S34 records the results of each sampling point after nonlinear function computation, and organizes and archives the results according to the original sampling point numbering order to form a set including all predicted sampling points. This set retains the distribution characteristics of the original sampling points and the nonlinear dynamic information of the system, providing accurate and complete input data for the weighted estimation in step S4, ensuring the consistency and effectiveness of the entire prediction process.

[0040] Preferably, step S4 includes the following sub-steps: S41, extracting the weights of each sampling point calculated in S2, and confirming the correspondence and numerical validity between the state weights and the covariance weights; S42, performing a weighted product operation on the predicted sampling points obtained in S3 and the corresponding state weights, summing all product results to obtain the one-step predicted state value; S43, calculating the deviation between each predicted sampling point and the one-step predicted state value, transposing the deviation and multiplying it by the original deviation, and then performing a weighted summation with the corresponding covariance weights; S44, adding the weighted summation result to the system process noise covariance matrix to obtain the one-step predicted state covariance value, thus completing the prediction process of state and covariance.

[0041] Specifically, step S4, through four sub-steps, accurately calculates the one-step predicted value of the state and the one-step predicted value of the state covariance, and is the core link in achieving state estimation. Step S41 extracts all the sampling point weights calculated in S2 from the data cache, including the state weight and covariance weight corresponding to each sampling point. It verifies the correspondence between the weights and sampling points one by one to ensure no mismatches, and verifies the validity of the weight values. The sum of the state weights must meet the normalization condition, and the covariance weights must be non-negative real numbers to eliminate the interference of invalid weights on the calculation results and ensure the reliability of the weight data. Step S42 performs element-wise multiplication of the predicted sampling points obtained in S3 with the corresponding state weights. The weighted contribution value of each sampling point is obtained by multiplying the value with the weight. Then, the weighted contribution values ​​of all sampling points are summed to obtain the one-step predicted value of the state. This value comprehensively reflects the state information of all sampling points and is a preliminary estimate of the system state at the next moment. Step S43 first calculates the deviation between each predicted sampling point and the one-step predicted state value. This deviation vector is obtained by element-wise subtraction. The deviation vector is then transposed, and the transposed vector is multiplied by the original deviation vector to obtain the deviation covariance matrix. This matrix is ​​then multiplied by the corresponding covariance weights, and all weighted deviation covariance matrices are summed to achieve a comprehensive calculation of the sampling point uncertainty. Step S44 performs matrix addition on the weighted summation result and a preset system process noise covariance matrix. This system process noise covariance matrix is ​​set based on sensor noise characteristics and the intensity of interference from the motion scene, with matrix elements being non-negative real numbers. Finally, the one-step predicted state covariance value is obtained. This value comprehensively characterizes the degree of uncertainty in the system state at the next moment, providing key parameter support for subsequent observation and update stages and ensuring the accuracy and rationality of the filtering estimation.

[0042] Preferably, step S5 includes the following sub-steps: S51, obtaining the one-step state prediction value and the one-step state covariance prediction value obtained in S4, and verifying the dimensional consistency and numerical rationality of the two; S52, calculating the square root of the one-step state covariance prediction value, and obtaining the operation coefficient by combining the system dimension and scaling factor; S53, performing addition and subtraction operations on the one-step state prediction value with the product of the operation coefficient and the square root of the covariance, respectively, to generate new symmetrical sampling points; S54, sorting and numbering the generated sampling points to ensure that the position of each sampling point matches the corresponding operation logic, forming a complete set of one-step state prediction sampling points.

[0043] Specifically, step S5 generates a new set of symmetrically distributed sampling points through four sub-steps, providing qualified samples for the nonlinear mapping in the observation update phase. Step S51 first obtains the one-step predicted values ​​of the state and the one-step predicted values ​​of the state covariance calculated in step S4. The dimensions of the two parameters are rigorously verified to ensure that the vector dimension of the one-step predicted values ​​of the state and the matrix dimension of the one-step predicted values ​​of the state covariance are consistent and match the system dimension (6 to 15). Simultaneously, the parameter values ​​are checked to ensure they are within the physically permissible range, eliminating outliers and guaranteeing the rationality and validity of the input parameters. Step S52 uses the Cholesky decomposition algorithm to perform square root calculation on the one-step predicted values ​​of the state covariance. This algorithm can efficiently and stably obtain a positive definite lower triangular square root matrix. Combined with the determined system dimension and a preset scaling factor (ranging from 0.0001 to 0.1), sampling coefficients are calculated according to a fixed operational logic. These coefficients are used to adjust the offset of the sampling points, balancing prediction accuracy and computational efficiency. Step S53 uses the one-step state prediction as a benchmark, multiplying the sampling coefficients with the square root of the covariance matrix to obtain a sampling offset vector with the same dimension as the system. Then, through addition and subtraction operations, the one-step state prediction is combined with the sampling offset vector to generate two sets of symmetrically distributed sampling points. The number of sampling points in each set is equal to the system dimension, ensuring the symmetry and reasonable distribution of the sampling points. Step S54 systematically sorts and numbers all generated sampling points. The sorting rules are determined based on the offset direction and magnitude between the sampling points and the one-step state prediction, and the numbering order is consistent with step S1, ensuring that the positional information of each sampling point corresponds to the computational logic. Finally, a complete set is formed, including one mean sampling point and two symmetrical sampling points with twice the system dimension. This provides comprehensive and standardized input samples for the observation nonlinear function mapping in step S6, ensuring the smooth progress of the observation and prediction process.

[0044] Example 2

[0045] Figure 2This is a complete execution flowchart of another embodiment of the present invention. The flowchart takes unscented Kalman filtering (UKF) as the core framework and integrates key technologies such as robust estimation, adaptive adjustment, dynamic weight allocation of sensors, soft handover and dynamic optimization of computation. The whole process revolves around the fusion of multi-source information from IMU, LiDAR and vision sensors. It aims to solve the problems of decreased positioning accuracy, insufficient robustness and even divergence of SLAM system caused by sudden changes in sensor measurement noise in extreme environments. The process starts from the algorithm start and goes through all stages such as sampling calculation, prediction update, noise estimation, anomaly judgment, weight correction and failure handover. Finally, it ends after completing state estimation and covariance update. It is a visual representation of the core execution logic of the robust adaptive UKF fusion algorithm of the present invention. After the process starts, it first enters the basic sampling and prediction stage of unscented Kalman filtering, executing the core basic calculation steps S1 to S7 in sequence: S1 calculates symmetrically distributed sampling points, generating a set of 2n+1 Sigma sampling points based on the system dimension n, and completing symmetrical sampling through the state mean and covariance matrix to provide basic sampling data for nonlinear state estimation; S2 then calculates the weights corresponding to each sampling point, solving for the mean weight, covariance weight, and unified weights of the state sampling points, and combining the scaling factor λ, adjustment parameter κ, and parameters α and β to complete the accurate calculation of weights, control the distribution of sampling points, and reduce prediction error; S3 performs one-step prediction of the sampling points, substituting the sampling point set generated in S1 into the state error nonlinear function. S2 completes the recursive prediction of the state of the sampling points; S4 performs one-step prediction of the state and covariance. Based on the weights of S2 and the predicted sampling points of S3, the weighted sum is used to obtain the one-step predicted state value, and the system process noise covariance matrix Qk is superimposed to obtain the one-step predicted covariance value; S5 performs symmetrical distribution sampling again based on the one-step state prediction results to generate a set of sampling points for the one-step state prediction; S6 substitutes this set of sampling points into the observation nonlinear function to complete the calculation of the sampling points for the one-step observation prediction; S7 combines the sampling point weights to complete the calculation of the mean, covariance matrix, and cross-covariance matrix of the state and observation for the one-step observation prediction. This completes the sampling, prediction, and observation pre-calculation steps of the UKF algorithm, laying the data foundation for subsequent measurement noise estimation and adaptive correction.

[0046] After completing the basic prediction calculations in S1-S7, the process enters the core intermediate stage of online measurement noise estimation, dynamic optimization of computational load, and routine state updates. It then executes key steps S8 to S11 sequentially, while embedding the first conditional judgment branch—whether the algorithm's running time exceeds the set computational cycle threshold Tmax—to achieve a dynamic balance between computational efficiency and system stability. S8 is for measurement noise covariance estimation. It uses a sliding window method combined with the innovation sequence to estimate the sensor measurement noise covariance matrix Rk online. The real-time measurement noise covariance estimate R̂k is obtained by subtracting the mean of the outer product of historical innovation sequences within the window from the observed prediction covariance. This is used to determine whether the sensor measurement noise deviates from its original statistical characteristics. This step is the core basis for identifying sensor distortion and triggering adaptive correction. After noise estimation, the process enters the first key judgment: checking whether the current algorithm running time exceeds the preset maximum time threshold Tmax. If the judgment result is "yes," then to ensure system real-time performance and stability, it sequentially executes S11 to reduce the sliding window length (halving the original window length L) and S12 to reduce the number of Sigma sampling points (reducing from 2n+1 to n+1 symmetrical sampling points). (Sample points) By reducing computational complexity, the algorithm is ensured to complete execution within a limited time. If the judgment result is "no", it directly proceeds to the subsequent judgment of changes in sensor statistical characteristics. Under the premise of not exceeding the calculation cycle, the process judges whether the sensor measurement noise statistical characteristics have changed, that is, compares the real-time estimated R̂k with the original Rk. If there is no change, it is determined that the sensor is working normally. S9 is executed to calculate the standard Kalman filter gain, and S10 is executed to update the state estimate and covariance matrix. After completing the conventional UKF state update, S10 is executed to process the covariance matrix based on SVD decomposition. The singular value decomposition solves the problem that the state covariance matrix is ​​singular and cannot be decomposed, and optimizes the calculation accuracy of sampling points. If the sensor statistical characteristics change, it is determined that the sensor has been distorted, and it proceeds to the subsequent adaptive correction and weight allocation stage.

[0047] When the process determines that the statistical characteristics of sensor measurement noise have changed, it enters the robust adaptive core link of sensor distortion detection, dynamic weight allocation, adaptive correction and long-term failure soft switching. Steps S13 to S21 are executed in sequence, and dual condition judgments of continuous failure time and sensor type failure are embedded. Finally, the algorithm ends after the entire process correction is completed. First, S13 calculates the dynamic weights of the LiDAR and vision sensor. A weight allocation formula is constructed based on the trace of the measurement noise covariance matrix, ensuring the sum of the weights of the two sensors is 1. Higher noise corresponds to lower weights, achieving dynamic matching of sensor confidence. S14 reconstructs the observation matrix based on the dynamic weights, fusing the predicted observations from the LiDAR and vision sensor according to their weights to correct the observation input data. S15 calculates the adaptive fading factor matrix, generating a diagonal fading matrix by the ratio of the real-time noise covariance to the original noise covariance, preventing system divergence caused by abnormal noise estimation. S16 combines the adaptive fading factor with correction of the measurement prediction covariance matrix, while simultaneously introducing an adaptive adjustment factor to correct the state prediction covariance matrix, achieving dual robust correction of the covariance. S17 adaptively corrects the Kalman filter gain based on the corrected covariance matrix, obtaining an optimized filter gain to improve state estimation accuracy. After completing the adaptive correction, the process enters the second key judgment: detecting whether the sensor is continuously distorted within the continuous time threshold Tfail. If it is a short-term distortion, the correction is completed and the state update is directly connected. If it is determined to be a long-term failure, the process enters the third condition judgment—distinguishing whether the failed sensor type is LiDAR. If the failed sensor is not LiDAR (i.e., the vision sensor fails), S19 is executed to decrease the smoothing factor γ, and the LiDAR weight is gradually increased through a smooth transition mechanism. If the failed sensor is LiDAR, S21 is executed to increase the smoothing factor γ, and the LiDAR weight is gradually decreased and the vision sensor weight is increased. The smooth weight switching follows the weighted recursive formula of S20 to avoid system divergence caused by sudden weight changes and ensure a smooth transition of the weights of the two sensors. After all correction and switching steps are completed, the algorithm completes the information fusion calculation of this robust adaptive unscented Kalman filter, and the process finally ends. The entire flowchart fully realizes the robust adaptive logic of the entire link through multi-condition branch judgment, multi-step progressive calculation, dynamic parameter adjustment and sensor soft switching: "basic UKF calculation → online noise estimation → dynamic optimization of computational load → sensor distortion discrimination → adaptive weight allocation → covariance and gain correction → smooth switching after long-term failure". It accurately matches the robustness improvement requirements of multi-sensor SLAM system in extreme environment, and is the core process carrier of the algorithm from theory to execution.

[0048] like Figure 3As shown, a robust adaptive unscented Kalman filter-based multi-sensor information fusion SLAM system is applied to the SLAM method based on robust adaptive unscented Kalman filter multi-sensor information fusion. The system includes: a multi-source sensor data acquisition unit, used to acquire triaxial and angular acceleration data from an IMU, point cloud data from a LiDAR, and image data from a vision camera, performing synchronous acquisition and transmission of multi-sensor data; a sampling point generation and weight calculation unit, connected to the multi-source sensor data acquisition unit, generating symmetrically distributed sampling points based on system parameters and calculating corresponding weights; and a nonlinear prediction calculation unit, connected to the sampling point generation and weight calculation unit. The sample point generation and weight calculation unit is connected to perform one-step prediction of sample points and one-step prediction of state and covariance. The observation prediction and covariance calculation unit is connected to the nonlinear prediction calculation unit to complete the generation of observation sample points, the calculation of observation mean and covariance matrix. The robust adaptive fusion unit is connected to the observation prediction and covariance calculation unit to perform measurement noise estimation, weight allocation and filter gain correction. The state correction and output unit is connected to the robust adaptive fusion unit to correct the displacement change of the IMU and output the corrected positioning data to the SLAM backend. All units interact and work collaboratively in real time through the data bus.

[0049] The SLAM method and system based on robust adaptive unscented Kalman filtering and multi-sensor information fusion achieves a closed-loop processing from multi-source data input to positioning result output by completing key steps such as sampling point generation, weight calculation, nonlinear prediction, and covariance estimation in stages. Each step is closely linked and logically coherent, ensuring the standardization of the calculation process. Simultaneously, dedicated weight allocation logic is designed for the characteristics of different sensors, fully leveraging the sensing advantages of various sensors to achieve complementary data fusion, significantly improving the comprehensiveness and accuracy of positioning data. At the system level, a modular design is adopted to divide functional units, each performing its own function and achieving real-time collaboration through a data bus. This ensures both the focus of individual functions and the efficient linkage of the overall system, significantly enhancing the feasibility and stability of the technology implementation.

[0050] This method and system address the insufficient robustness of traditional methods due to fixed weights and parameters. By dynamically generating symmetrically distributed sampling points, calculating adaptive weights in real time, and dynamically estimating measurement noise characteristics using a sliding window mechanism, it can flexibly adjust computational parameters according to changes in sensor data quality, effectively resisting interference from abnormal data and ensuring the stability of fusion results in complex environments. Addressing the lack of dynamic matching in multi-sensor weight allocation, it abandons empirical fixed allocation modes and designs an adaptive weight strategy based on real-time sensor noise characteristics. This allows different sensors to play their optimal role in different environmental scenarios, fully exploring the complementary value of multi-source data. Furthermore, by refining the sub-processes of key steps, it further improves computational accuracy and reliability, ultimately achieving a dual improvement in positioning accuracy and environmental adaptability of the SLAM system in complex scenarios, providing solid technical support for the stable operation of autonomous mobile devices.

[0051] In the description of this invention, it should be noted that, unless otherwise specified and limited, the terms "set," "install," "connect," "link," and "fix" should be interpreted broadly. For example, they can refer to a fixed connection, a detachable connection, or an integral connection; they can refer to a mechanical connection or an electrical connection; they can refer to a direct connection or an indirect connection through an intermediate medium; and they can refer to the internal connection of two components. Those skilled in the art will understand the specific meaning of the above terms in this invention based on the specific circumstances.

[0052] Although embodiments of the invention have been shown and described, it will be understood by those skilled in the art that various equivalent changes, modifications, substitutions and alterations can be made to these embodiments without departing from the principles and spirit of the invention, the scope of which is defined by the appended claims and their equivalents.

Claims

1. A SLAM method based on robust adaptive unscented Kalman filtering and multi-sensor information fusion, characterized in that, Includes the following steps: S1. Determine a symmetrically distributed set of sampling points based on the system dimension. This set includes sampling points corresponding to the mean and positive and negative symmetrical sampling points obtained based on the state mean and covariance matrix. S2. Calculate the state weight and covariance weight corresponding to each sampling point according to the scaling factor, adjustment parameters, and distribution control parameters set by the system. S3. Substitute the sampling points obtained in S1 into the state error nonlinear function to complete the one-step prediction calculation for each sampling point. S4. Use the weights calculated in S2 to perform a weighted summation of the predicted sampling points obtained in S3 to obtain the one-step state prediction value. Simultaneously, [the calculation is repeated in the original text]. S4: Combine weights, the deviation between the predicted sampling points and the state one-step predicted value, and the system process noise covariance matrix to calculate the state covariance one-step predicted value; S5: Based on the state one-step predicted value and the state covariance one-step predicted value obtained in S4, generate a new set of sampling points through symmetrical distribution sampling; S6: Substitute the sampling points obtained in S5 into the observation nonlinear function to obtain the observation one-step predicted value corresponding to each sampling point, and then combine the weights in S2 to perform a weighted summation of the observation one-step predicted values ​​to obtain the observation one-step predicted mean. At the same time, calculate the observation covariance matrix and the cross-covariance matrix between the state and the observation.

2. The SLAM method based on robust adaptive unscented Kalman filtering and multi-sensor information fusion according to claim 1, characterized in that, The sampling point calculation satisfies: ,in, For the i-th sampling point at time k, Let k be the mean of the state at time k. For the system dimension, Scaling factor Let k be the state covariance matrix at time k. Take an integer from 1 to 2n.

3. The SLAM method based on robust adaptive unscented Kalman filtering and multi-sensor information fusion according to claim 1, characterized in that, The calculation of sampling point weights satisfies: , ,in, Let be the state weight of the i-th sampling point at time k. Let the covariance weights of the 0th sampling point at time k be _____. , For scaling parameters, For distributed control parameters, To adjust the parameters, The system dimension.

4. The SLAM method based on robust adaptive unscented Kalman filtering and multi-sensor information fusion according to claim 1, characterized in that, State covariance one-step prediction satisfies: ,in, for One-step prediction of the state covariance at time step. Let be the weight of the i-th sampling point at time k. for The i-th prediction sampling point at time i, for One-step prediction of state at time step. Let k be the system process noise covariance matrix at time k. This is the matrix transpose operation.

5. The SLAM method based on robust adaptive unscented Kalman filtering and multi-sensor information fusion according to claim 1, characterized in that, The measurement noise covariance estimate satisfies: ,in, The noise covariance estimate is measured at time k. The length of the sliding window. Let j be the information sequence at time j. Let be the covariance weight of the i-th sampling point at time k. for The predicted value of the i-th observation at time i, for Observe the mean at each step to predict the value.

6. The SLAM method based on robust adaptive unscented Kalman filtering and multi-sensor information fusion according to claim 1, characterized in that, The sensor weight allocation satisfies: ,in, For lidar weights, For visual sensor weights, To measure the trace of the noise covariance matrix for lidar, The trace of the noise covariance matrix is ​​measured for a vision sensor.

7. The SLAM method based on robust adaptive unscented Kalman filtering and multi-sensor information fusion according to claim 1, characterized in that, S3 includes the following sub-steps: S31, obtain all symmetrically distributed sampling points generated in S1, and determine the state dimension and numerical characteristics corresponding to each sampling point; S32, call the preset state error nonlinear function, input the value of each sampling point into the function in sequence, and perform state transformation and error propagation according to the operation logic defined by the function; S33, monitor the operation process of each sampling point in real time to ensure that the input parameters match the function's domain and avoid operation abnormalities; S34, record the result of each sampling point after the nonlinear function operation to form the predicted sampling point set at time k+1, providing basic data for subsequent state estimation.

8. The SLAM method based on robust adaptive unscented Kalman filtering and multi-sensor information fusion according to claim 1, characterized in that, S4 includes the following steps: S41, extract the weights of each sampling point calculated in S2, and confirm the correspondence between the state weights and the covariance weights and the validity of the values. S42, perform a weighted product operation on the predicted sampling points obtained in S3 and the corresponding state weights, and sum all the product results to obtain the one-step predicted state value; S43, calculate the deviation between each predicted sampling point and the one-step predicted state value, perform a transpose operation on the deviation and multiply it with the original deviation, and then perform a weighted summation in combination with the corresponding covariance weights; S44, add the weighted summation result to the system process noise covariance matrix to obtain the one-step predicted state covariance value, thus completing the prediction process of state and covariance.

9. The SLAM method based on robust adaptive unscented Kalman filtering and multi-sensor information fusion according to claim 1, characterized in that, S5 includes the following sub-steps: S51, obtaining the one-step state prediction value and the one-step state covariance prediction value obtained in S4, and verifying the dimensional consistency and numerical rationality of the two; S52, calculating the square root of the one-step state covariance prediction value, and obtaining the operation coefficients by combining the system dimension and scaling factor; S53, performing addition and subtraction operations on the one-step state prediction value with the product of the operation coefficients and the square root of the covariance, respectively, to generate new symmetrical sampling points; S54, sorting and numbering the generated sampling points to ensure that the position of each sampling point matches the corresponding operation logic, forming a complete set of one-step state prediction sampling points.

10. A SLAM system based on robust adaptive unscented Kalman filtering and multi-sensor information fusion, characterized in that, This system is applied to the SLAM method based on robust adaptive unscented Kalman filtering multi-sensor information fusion as described in claim 1, comprising: a multi-source sensor data acquisition unit for acquiring triaxial and angular acceleration data from an IMU, point cloud data from a lidar, and image data from a vision camera, performing synchronous acquisition and transmission of multi-sensor data; a sampling point generation and weight calculation unit connected to the multi-source sensor data acquisition unit, generating symmetrically distributed sampling points and calculating corresponding weights based on system parameters; a nonlinear prediction operation unit connected to the sampling point generation and weight calculation unit, performing one-step prediction of sampling points and one-step prediction of state and covariance; an observation prediction and covariance calculation unit connected to the nonlinear prediction operation unit, completing the generation of observation sampling points, calculation of observation mean and covariance matrix; a robust adaptive fusion unit connected to the observation prediction and covariance calculation unit, performing measurement noise estimation, weight allocation, and filter gain correction; and a state correction and output unit connected to the robust adaptive fusion unit, correcting the displacement change of the IMU and outputting the corrected positioning data to the SLAM backend. Each unit interacts and collaborates in real time via a data bus.

Images