A slam method based on tag multi-bernoulli alternately updating pose and map

Abstract

The application discloses a SLAM method based on label multi-Bernoulli pose and map alternately updated. In view of the problems of pose drift, data association difficulty and high calculation complexity of the traditional SLAM in a complex noise and clutter environment, the application adopts a pose and map alternately updated framework, decomposes the joint posterior into the marginal posterior of the pose and the map for separate estimation, adopts a label multi-Bernoulli filter to label the map, introduces double-layer Gibbs sampling to reduce the number of data association hypotheses, and adopts covariance cross fusion to integrate multi-hypothesis pose estimation. The application can effectively suppress pose drift, improve mapping accuracy and real-time performance, and is suitable for complex SLAM scenes with high clutter and low detection probability.

Description

Technical Field

[0001] This invention belongs to the field of mobile robot technology and relates to a label-based multi-Bernoulli pose and map alternating update SLAM method. This method improves the problems faced by mobile robots in the SLAM process in complex noise and clutter environments, such as data association difficulties, localization drift and insufficient mapping accuracy, thereby effectively improving the localization accuracy, mapping quality and overall robustness and real-time performance of the robot system. Background Technology

[0002] Simultaneous Localization and Mapping (SLAM) is a key technology for mobile robots to achieve autonomous navigation. It is widely used in various complex work scenarios, and its positioning accuracy is highly dependent on the terrain. Figure 1 Consistency directly affects the reliability of autonomous robot operations. As application scenarios expand to dynamic and high-noise environments, traditional SLAM faces challenges such as complex multi-target associations, pose drift, and inaccurate map building, requiring solutions with higher robustness and real-time performance.

[0003] In the existing SLAM technology system, Bayesian filtering methods based on random finite sets (RFS) have become an important technical approach for SLAM tasks in complex environments because they can directly handle problems such as unknown landmark numbers and uncertain correlations in observation data. Among them, label-based multi-Bernoulli (LMB) filtering and its extended forms are particularly important. -GLMB filtering, by assigning a unique label to each landmark, enables effective management and tracking of multiple targets, demonstrating significant advantages in multi-target data association and gradually being introduced into the SLAM field. Specifically, the paper "The Labeled Multi-Bernoulli SLAM Filter" (LMB-SLAM) introduces LMB random finite sets into the SLAM field. This method achieves effective landmark tracking by assigning a unique label to each landmark, but its pose estimation and map estimation are highly coupled within the RBPF framework. While it uses particle filtering to uniformly propagate joint posterior probabilities, it fails to achieve decoupled estimation of pose and map. The inherent particle degradation and resampling problems of RBPF lead to pose drift, and the computational complexity increases with the product of the number of particles and the number of association hypotheses, making it difficult to meet real-time requirements.

[0004] Furthermore, the paper "A GM-PHD-SLAM algorithm based on pose and map alternating update" discloses a GM-PHD-SLAM method based on the pose and map alternating update (PMAU) framework. This method decomposes the joint posterior into alternating estimations of pose and map edge posteriors, avoiding particle degradation. However, this method uses a probabilistic hypothesis density (PHD) filter with an unlabeled Gaussian mixture weighted sum describing the map intensity function, and each landmark component is updated independently, without handling label-level data association. This leads to a failure to maintain the consistency of landmark identities in the time series, easily resulting in landmark identity loss and trajectory fragmentation.

[0005] However, when considering combining the PMAU framework with the LMB filter, new technical challenges arise: the LMB needs to be expanded into δ-GLMB in the update step to handle label-level associations, and m LMB terms generate 2 m The possible δ-GLMB assumptions lead to an exponential increase in the associated hypothesis space. Furthermore, different δ-GLMB hypothesis sets are independent of each other, and the same physical landmark can be assigned different labels or existence states in different hypothesis sets. If the equal weighted covariance cross (CI) fusion strategy in GM-PHD-PMAU-SLAM is directly applied, it will lead to label ambiguity and over-conservative estimation of covariance.

[0006] Therefore, how to effectively embed LMB's label management advantages into the PMAU alternating update framework while maintaining them, and how to solve the label ambiguity problems caused by the explosion of related hypotheses and the fusion of multiple hypotheses, has become a technical challenge that urgently needs to be solved in this field. Summary of the Invention

[0007] The purpose of this invention is to address the problems of data association difficulties, positioning drift, and insufficient mapping accuracy in complex noise and clutter environments. It proposes a label-based multi-Bernoulli pose and map alternating update SLAM method, abbreviated as LMB-PMAU-SLAM. Under the PMAU-SLAM framework, the robustness and real-time performance of the SLAM system are improved by managing the map through LMB labeling and introducing Gibbs sampling for two-layer reduction δ-GLMB hypothesis association.

[0008] To achieve the above objectives, the present invention adopts the following technical solution:

[0009] In a first aspect, this invention proposes a SLAM method based on tag-based multi-Bernoulli pose and map alternating updates, comprising the following steps:

[0010] A label-based multi-Bernoulli pose and map alternating update SLAM method includes the following steps:

[0011] Step (1): Establish the platform state model, map model and observation model of the SLAM system, wherein the map model is described by a labeled multi-Bernoulli random finite set, and each landmark is assigned a unique label containing the discovery time index and the sequence number index;

[0012] Step (2): Based on the platform state model, map model and observation model, derive the Bayesian estimation of SLAM under finite set statistics, decompose the joint posterior probability of platform pose and map into pose posterior marginal probability and map posterior marginal probability, and establish an estimation framework for alternating updates of pose and map.

[0013] Step (3): Describe the platform pose estimation as a Gaussian random variable and the map estimation as a labeled multi-Bernoulli random finite set, and establish a description of the SLAM estimation problem based on Gaussian random state and labeled multi-Bernoulli.

[0014] Step (4): Based on the model constructed in step (3), predict the platform pose and map landmarks respectively to obtain the predicted state;

[0015] Step (5): Expand the predicted map landmark set in the field of view into δ-GLMB (δ-generalized label multi-Bernoulli) form, and use two-layer Gibbs sampling to reduce the number of association hypotheses: first, at the existence probability level, independently sample the label multi-Bernoulli components to generate candidate δ-GLMBs and truncate them; then at the observation association level, perform Gibbs iterative sampling on the observation association mapping of each candidate δ-GLMB and truncate it, retaining the set of association hypotheses with the highest likelihood probability, and obtain the reduction result;

[0016] Step (6): Based on the predicted state in step (4) and the reduction results in step (5), the platform pose and map landmarks are updated respectively. The platform pose update replaces the equal weighted covariance cross-fusion with the normalized likelihood weight of each associated hypothesis. The pose estimation results under each associated hypothesis are adaptively weighted and fused by Gaussian mixture moment matching.

[0017] Step (7): Based on the map posterior obtained in step (6), the label multi-Bernoulli random finite set is pruned and regenerated, landmarks with a probability of existence below a set threshold are deleted, and new landmarks are generated for unrelated observation data.

[0018] Preferably, the estimation framework for alternating pose and map updates in step (2) includes:

[0019] Based on the posterior estimation of the map at time k-1, the posterior marginal probability of the platform pose at time k is approximated.

[0020] Based on the posterior estimation result of the platform pose at time k, the posterior edge probability of the map at time k is approximated.

[0021] The platform pose update uses the predicted map as a condition, and the map update uses the posterior estimate of the platform pose as a condition.

[0022] Preferably, the Gibbs sampling reduction of the number of association hypotheses in step (5) includes:

[0023] The predicted set of landmarks within the field of view is divided into a Dobernoli set of labels within the field of view and a Dobernoli set of labels outside the field of view.

[0024] Based on the existence probability of the label multi-Bernoulli set within the field of view, multiple delta-generalized label multi-Bernoulli hypotheses are generated by independent sampling, and the likelihood probability of each hypothesis is recorded.

[0025] Extract the top q delta-generalized label multi-Bernoulli hypotheses with the highest likelihood probabilities to form the reduced set of existence labels;

[0026] For each defined set of existence labels, the Gibbs sampling method is used to generate feasible observation association mappings. The conditional probabilities of each label are traversed and normalized sampling is performed, retaining the association hypothesis with the highest likelihood probability.

[0027] Preferably, the platform pose update in step (6) includes:

[0028] For each feasible association hypothesis, the pose update result corresponding to each non-zero observation association under that association hypothesis is calculated using the extended Kalman filter, including Kalman gain, updated pose state and covariance;

[0029] Using the covariance cross-fusion algorithm, the pose update results corresponding to all non-zero observation associations under each association hypothesis are fused to obtain the pose state and covariance under that association hypothesis.

[0030] Using the normalized likelihood weights of each association hypothesis as natural fusion weights, the pose estimation results under all association hypotheses are adaptively weighted and fused using the Gaussian mixture moment matching formula to obtain the posterior estimate of the platform pose.

[0031] Preferably, when the covariance cross-fusion algorithm fuses the pose update results corresponding to multiple non-zero observation associations under each association hypothesis, the weight coefficients of the covariance inverse matrix corresponding to each observation association are equal, and are all the reciprocal of the number of non-zero observation associations.

[0032] Preferably, the map landmark update in step (6) includes:

[0033] For map landmarks outside the field of view, maintain the predicted state unchanged;

[0034] For map landmarks within the field of view, based on the normalized weights of each association hypothesis, the weighted update results of the existence probability, location mean, and covariance of each landmark are calculated by using the δ-generalized label multi-Bernoulli to label multi-Bernoulli collapse.

[0035] The updated map landmark sets within and outside the viewport are merged to obtain the final map estimate.

[0036] Preferably, in the update of map landmarks within the field of view, the predicted location mean and covariance remain unchanged for landmarks that are missed, while extended Kalman filtering is used to update landmarks that are detected and associated with the observations.

[0037] Preferably, the pruning and regrowth treatment in step (7) includes:

[0038] Set an existence probability threshold, and remove landmarks whose landmark components in the map posterior are lower than the threshold from the label Bernoulli set;

[0039] For each observation data, calculate the probability that it is caused by a known landmark. If the probability is lower than the set threshold for new object determination, the observation is determined to be likely to originate from a new object or clutter.

[0040] Based on the relationship between the expected number of clutter within the field of view and the number of unassociated observations, the number of newly generated targets is estimated, and corresponding label-based Bernoulli terms are generated for eligible newly generated observations, including the probability of existence, the mean of the location, and the covariance.

[0041] Secondly, the present invention provides a label-based multi-Bernoulli pose and map alternating update SLAM system for implementing the above method, comprising:

[0042] The model building module is used to build the platform state model, map model, and observation model of the SLAM system.

[0043] The Bayesian estimation derivation module derives the Bayesian estimation of SLAM under finite set statistics based on the platform state model, map model and observation model. It adopts a strategy of alternating pose and map updates to approximate the posterior marginal probability of the platform pose and the posterior marginal probability of the map, respectively.

[0044] The SLAM model building module is used to build SLAM models based on Gaussian random states and label multi-Bernoulli. It describes the estimation of platform pose as a Gaussian random variable and the estimation of map as a label multi-Bernoulli.

[0045] The prediction module is used to predict the platform pose and map.

[0046] The Gibbs sampling reduction module is used to perform Gibbs sampling reduction on the number of associated hypotheses.

[0047] The update module is used to update the platform's pose and map based on the predicted state and reduction results;

[0048] The pruning and germination module is used to perform multi-Bernoulli pruning and germination of labels based on the obtained map posterior.

[0049] The beneficial effects of this invention are:

[0050] This invention employs a Pose and Map Alternating Update (PMAU) framework, decomposing the joint posterior into separate edge posterior estimations. This avoids pose drift caused by particle degradation in traditional RBPF-SLAM, and pose estimation accuracy is no longer constrained by particle count and resampling quality. A Labeled Multi-Bernoulli (LMB) random finite set is used to assign a unique label to each landmark. Compared to the labelless intensity function representation of the PHD filter, this maintains the consistency of landmark identity in the time series, avoiding landmark identity loss and trajectory fragmentation. To address the problem of exponential growth of association hypotheses after LMB is expanded into δ-GLMB, a two-layer Gibbs sampling reduction is introduced to control the number of association hypotheses at a fixed constant level. This ensures that computational complexity is linearly related to the number of landmarks in the view domain rather than exponentially related, effectively solving pose drift and... To address the inefficiency of the GLMB association assumption, this paper replaces the equal-weighted covariance cross-fusion with the normalized likelihood weight of the association assumption, forming an adaptive weighting mechanism. The high-likelihood assumption contributes more significantly and naturally maintains label consistency during the collapse from δ-GLMB to LMB. Combining clutter Poisson distribution modeling with probability threshold-based adaptive detection of new landmarks, this invention can effectively distinguish between real landmarks and clutter even in high-clutter, low-detection-probability scenarios. Simulation results show that compared to PHD-SLAM3.0, this invention significantly improves upon PHD-SLAM3.0 in three metrics: average pose error, OSPA distance, and single-step running time. Attached Figure Description

[0051] Figure 1 This is a flowchart illustrating the specific implementation of the core part of the method of this invention;

[0052] Figure 2 This is a diagram showing the results of the method of this invention.

[0053] Figure 3 This is a diagram showing the average pose error between the method of this invention and PHD-SLAM3.0.

[0054] Figure 4 This is an OSPA average value chart comparing the method of this invention with PHD-SLAM3.0.

[0055] Figure 5This is a time graph comparing the method of this invention with PHD-SLAM3.0. Detailed Implementation

[0056] The present invention will be further described below with reference to the embodiments and accompanying drawings.

[0057] like Figure 1 As shown, this invention provides a label-based multi-Bernoulli pose and map alternating update SLAM method, comprising:

[0058] Step (1): Establish a model of the SLAM system:

[0059] First, establish the platform's state model, map model, and observation model:

[0060] 1.1 Platform State Model

[0061] Set up the platform The state at time is According to the first-order Markov model, the platform in The state at time is

[0062]

[0063] in, For a one-step state transition function, yes Motion control input at any time, It is process noise, and its distribution is assumed to follow a mean of covariance is A Gaussian random process. It will accumulate to... The platform state vector at time t is denoted as .

[0064] 1.2 Map Model

[0065] Assuming a real map It is a collection of landmarks. ,in, For the number of landmarks, It is the first The location vectors of each landmark are given, assuming all landmarks are stationary. until Map estimation based on the detected time. For the estimated number of landmarks, Is the index as Explored landmarks. At that moment, the platform status was... At that time, only landmarks within its field of view can be seen. These visible landmarks constitute the current visual map, denoted as... ,in, It is the first in the visual map A landmark, It refers to the number of visible landmarks.

[0066] If we disregard clutter and missed detections, and assume the estimation is accurate, then...

[0067]

[0068]

[0069] in, ,in" "" represents the difference operation between sets.

[0070] 1.3 Observation Model

[0071] The set of observation data obtained by the time sensor is ,in If it's the number of observations, then accumulated to... The set of observations at time t is represented as .

[0072] For any observation It could originate from a landmark or clutter. If From landmarks , Then the observation equation is,

[0073]

[0074] in, Represents the observation function, It is the observation error, and its distribution follows a mean of . covariance is A Gaussian random process.

[0075] Assume that the clutter within the sensor's field of view (FoV) is independent and identically distributed, and its quantity follows a parameter... Given a Poisson distribution, and assuming any clutter location follows a uniform distribution, the probability densities of clutter quantity and location can be described by the following two formulas:

[0076]

[0077]

[0078] Where c represents clutter. The number of clutter (false observations) received within the sensor's field of view. It represents the volume of FoV.

[0079] The set of observations from the landmark is denoted as The set of observations from clutter is denoted as ,So

[0080]

[0081] Step (2): Derivation of Bayesian estimation for SLAM under finite set statistics

[0082] If known Posterior probability density of platform pose and map at any given time and Therefore, the goal of Bayesian estimation in SLAM is to obtain... Posterior probability density of platform pose and map at any given time and PMAU-SLAM starts with the posterior probabilities of platform pose and map, employing an alternating estimation approach. First, it approximates the posterior marginal probability of the platform pose based on historical maps. Then, it approximates the posterior marginal probability of the map based on the posterior estimation of the platform pose. The specific process includes two steps: prediction and update.

[0083] 2.1 Prediction

[0084] Known Posterior probabilities of platform pose and map at any given time and Then their predictions are respectively,

[0085]

[0086]

[0087]

[0088] in, It is the one-step pose transition probability of the platform. It represents the probability of a one-step transition based on the map's conditions. It is the one-step transition probability of the map.

[0089] 2.2 Update

[0090] Predicted map Represented as the union of two mutually exclusive sets:

[0091]

[0092] in, It is a set of landmarks outside the current sensor FoV. It is the set of landmarks within the current sensor FoV.

[0093] 1) Posterior probability update of platform pose

[0094]

[0095] in, It is the observation likelihood function. It is the normalization factor.

[0096] 2) Posterior probability update of the map

[0097] First, obtain the posterior mean of the platform pose.

[0098] The approximate map posterior probability update is then:

[0099]

[0100] Step (3): Description of the SLAM problem based on Gaussian random states and label Bernoulli

[0101] This step describes the estimation of the platform pose as a Gaussian random variable, and then uses LMB to describe the estimation of the map, thus leading to a more specific description of the SLAM estimation problem.

[0102] First, the platform pose is described. Assume... The estimation of the platform pose at time step follows a Gaussian random process, where This represents an estimate of the mean. This represents the estimated covariance. Then it will be accumulated to... The set of landmarks estimated at a given time is denoted as . It is a landmark label, among which, This represents the time index of when the landmark was first discovered. This indicates the index of the landmark's discovery time. Representative moment A set of map labels. For any estimated landmark, the labels... It possesses uniqueness. For a given landmark... , This is the probability of the landmark's existence. Assuming the location distribution of the landmarks follows a Gaussian random process, then... and The corresponding mean and covariance are described.

[0103] Based on the above description, the SLAM problem can be described as follows: Given Estimation of platform pose at time and , Estimation of time map When obtained The set of observation data at time How to obtain Estimation of platform pose at time and ,and Estimation of time map .

[0104] Step (4) Prediction of platform pose and map using the LMB-PMAU-SLAM algorithm

[0105] The PMAU framework used in this invention differs fundamentally from the existing RBPF architecture: PMAU no longer propagates the joint posterior probability, but instead propagates the pose and edge posterior probabilities of the map separately, achieving decoupled estimation through alternating updates. This architectural difference allows PMAU to avoid the particle degradation and resampling problems inherent in particle filtering, reducing computational complexity from "number of particles × number of associated hypotheses" to the "number of associated hypotheses" level. However, existing PMAU framework implementations only describe the map intensity function using an unlabeled Gaussian mixture weighted sum for the Probability Hypothesis Density (PHD) filter, with each landmark component updated independently, without handling label-level data association. When the map description is replaced from PHD to LMB, the map is represented as a set of Bernoulli terms with unique labels, which needs to be expanded into δ-GLMB during updates to handle label-level associations. This introduces an exponential association hypothesis space, i.e., the generation of m LMB terms. This paper describes the possible δ-GLMB and the subsequent reduction, fusion, and collapse requirements. Steps (4) to (7) will explain in detail how the present invention addresses the above problems one by one.

[0106] The prediction of the platform pose can be easily obtained based on the state model described in step 1.1, if The state and covariance of the pose at time step are estimated as follows: and Therefore, their predictions are:

[0107]

[0108]

[0109] in, The Jacobian matrix is ​​represented as:

[0110]

[0111] Landmark prediction is as follows: Landmarks in SLAM problems are generally considered to be stationary, therefore if... ,So ,and,

[0112]

[0113] Landmarks are generally considered not to disappear out of thin air, so it can be assumed that... Alternatively, if it is believed that landmarks may disappear due to movement or other reasons in certain situations, then setting [something] is possible. .

[0114] Step (5) Gibbs sampling reduction of the number of association hypotheses

[0115] This invention introduces Gibbs sampling for two-level reduction: first, candidates are generated by independent sampling at the LMB existence probability level. -GLMB and truncate by likelihood, then in each - GLMB internally performs Gibbs iteration sampling on the observation correlation map and truncates it according to likelihood, thereby keeping the computational cost within a reasonable range. Specifically:

[0116] First, the predicted landmarks are divided into sets. , , , , If the sensor can only see its surrounding radius is Landmarks within the range, then for any... , must meet refer to Positional components.

[0117] first It is an expression of a set of LMBs, so it can be based on Generate a series -GLMB. For a feasible... -GLMB, its set of landmark tags is , It is all feasible -GLMB is a set of existential landmark tags. For any one Its likelihood probability is:

[0118]

[0119] because The set of observation data at time is Therefore, for any There exists a set of observation correlation mappings , and if ,So .if This indicates the target It was missed; if This indicates the target It was detected, and the resulting observation data is For any set of tags, of For GLMB, all possible mappings The set is denoted as Then for any mapping The observation likelihood under the correlation hypothesis is:

[0120]

[0121] in,

[0122]

[0123] in, It is the detection probability. Through The observation data that the mapping points to. It is the observation likelihood function. It appears to be noise.

[0124] If the number of LMB is Then all feasible The number of GLMB is Because each LMB is independent, feasible models can be generated by independently sampling each LMB based on its existence probability. -GLMB, simultaneously records the generated -GLMB likelihood probability. Specifically, it can generate... A different -GLMB, and then extract the one with the highest likelihood probability from it. The reduced composition .

[0125] Next, we consider a defined set of existence labels. of -GLMB, and the observation dataset We use Gibbs sampling to generate feasible observation association hypotheses. First, we generate an initial observation association. For any ,make That is to let If all labels in the dataset are missed, the likelihood probability is:

[0126]

[0127]

[0128] The following Gibbs sampling is performed, and for each hypothesis set, samples are processed in order. Iterate through the tags in the table, except for the fixed ones. Calculate the conditional probability assuming the observed associations with other labels besides those mentioned above. The following formula is used:

[0129]

[0130] Obtain the normalized conditional probability, and sample according to this probability.

[0131]

[0132] Forming the sampled and And record. The value of .

[0133] Finally, based on Gibbs sampling, the sample with the highest likelihood probability is retained. These hypotheses form a set of related hypotheses after sampling.

[0134] Step (6) Platform pose and map update of LMB-PMAU-SLAM algorithm

[0135] 6.1 Pose Update

[0136] Let's consider all feasible options together. -The complete data association hypothesis formed by GLMB and the corresponding observation association mapping, for the specified and The arbitrary association hypothesis constituted Its association weight is:

[0137]

[0138] The weights are normalized and represented as follows:

[0139]

[0140] The next step is to consider how to obtain the association hypothesis. "Pose update of the platform. First, EKF is used to obtain the pose update result corresponding to each observation association from the landmark under this association assumption:"

[0141]

[0142]

[0143]

[0144]

[0145] The following uses the covariance cross-fusion algorithm to obtain the association hypothesis " "Update of pose state and covariance under the condition of ". Assume the number of observations for non-zero mapping is ,So:

[0146]

[0147]

[0148] In GM-PHD-PMAU-SLAM, there are no label constraints between Gaussian components, and equal-weighted covariance cross (CI) fusion can be directly used. However, different... - The GLMB hypothesis sets are independent of each other. The same physical landmark can be assigned different labels or existence states in different hypothesis sets. Directly applying CI fusion will lead to label ambiguity and overly conservative estimation of covariance. To address this problem, this invention proposes to replace equal-weight CI fusion with association hypothesis weighted moment matching: utilizing each - Normalized weights of the GLMB association hypothesis As a natural fusion weight, the global posterior estimate of pose and map is obtained through the Gaussian mixture moment matching formula. Compared with the equal weight strategy of CI, this method constitutes an adaptive weighted fusion mechanism. The higher the likelihood of the association hypothesis, the greater its contribution to the final estimate, and statistically more accurately reflects the confidence distribution of each hypothesis.

[0149] The association hypothesis weighted moment matching is expressed as:

[0150]

[0151]

[0152] in, Let represent the mean of the final posterior estimate of the platform pose at time k. It is a complete association hypothesis. This represents the set of hypotheses indicating which landmarks exist within the field of view. This represents a hypothetical mapping that indicates which observation corresponds to each existing landmark. Represents the normalized weights of the association hypothesis; In this specific association hypothesis The mean pose estimate obtained after updating separately using the Extended Kalman Filter (EKF) is shown below. Let represent the final posterior estimate covariance matrix of the platform pose at time k, and let represent the uncertainty of the final pose estimate. This represents the pose estimation covariance matrix obtained by updating the pose using EKF alone under this specific correlation assumption, and represents the pose uncertainty within this assumption.

[0153] 6.2 Map Updates

[0154] First of all Update and get ,in:

[0155]

[0156] Then to Update accordingly. Using some of the results from step 4.1, all feasible [procedures] have been obtained. - All data association hypotheses constituted by GLMB and the corresponding observation association mapping, for any association hypothesis " "For any" There are two possible scenarios:

[0157] 1) If This indicates that the landmarks are under the current association assumption. The result of the missed detection is:

[0158]

[0159]

[0160] 2) If This indicates that the landmarks are under the current association assumption. Caused observation Therefore, we get:

[0161]

[0162]

[0163]

[0164]

[0165] Because the arbitrary association hypothesis has already been obtained "weight" Therefore, it can be done - The collapse from GLMB to LMB yields the updated LMB values ​​for the map within the viewport. ,

[0166]

[0167]

[0168]

[0169] in,

[0170]

[0171] The final map estimate is obtained by merging the out-of-view and in-view maps (LMB). , .

[0172] Step (7) Pruning and new growth of LMB

[0173] If all accumulated values ​​are retained If the number of LMBs at any given time increases over time, then many of these LMBs may have a very low probability of existence. Retaining and arbitrarily using a large number of low-probability LMBs not only increases computational cost but may also affect the accuracy of the estimation. Therefore, in each posterior estimate of the map... A probability threshold can be set for its existence. ,if Then the corresponding LMB will be transferred from Delete it.

[0174] Because observation data can only be caused by landmarks or clutter, if the probability of a certain observation data being caused by a known landmark is very small, then it is determined that it is caused by clutter or a newly emerging target. For any It can assess the probability that it was caused by a known landmark.

[0175]

[0176]

[0177] Set probability threshold ,if If so, it is determined that the observation data may originate from a potential target or clutter. Further calculations are made to determine the number of observation data that meet the criteria.

[0178]

[0179]

[0180] Because the area of ​​the observation field is Therefore, the expected number of clutter within the field of view is,

[0181]

[0182] Therefore, the number of new targets is approximately considered to be... ,

[0183]

[0184] if ,make =0.

[0185] For all If the conditions are met ,and This will generate a new set of LMBs. ,in:

[0186]

[0187] in, and Based on The given prior mean and covariance.

[0188] To verify the effectiveness of the label-based multi-Bernoulli pose map alternating update SLAM method (LMB-PMAU-SLAM) proposed in this invention, it was compared with the existing PHD-SLAM 3.0 algorithm in a simulation environment.

[0189] The simulation experimental environment is set as follows:

[0190] A 1200m × 1200m square region was set up in a two-dimensional plane, containing 160 landmark points. The platform made multiple turns within the region to fully verify the localization and mapping performance of the algorithm under complex maneuvering conditions. Meanwhile, considering missed detections and clutter interference, the detection probability was set. and clutter quantity To simulate perceptual uncertainty in real-world environments.

[0191] The evaluation metrics are set as follows: Absolute Localization Error (ALE), Generalized Optimal Sub-Pattern Assignment (GOSPA), and Average Execution Time (AET).

[0192] The results are as follows Figures 2-5 As shown. Figure 2 The pose and map estimation results for the two methods are shown in the following specific metrics: Figures 3-5 As shown, by Figure 3 The proposed method has a smaller average pose error, indicating that it can effectively suppress pose drift and improve robot localization accuracy through alternating pose and map updates and a multi-hypothesis fusion mechanism. Figure 4The OSPA metric can comprehensively evaluate the errors in both the number and location of landmarks. Therefore, a lower average OSPA indicates that the proposed method has better performance in terms of map construction accuracy and data association accuracy, and can estimate the environmental map more accurately. Figure 5 The average execution time indicates that the proposed method effectively reduces computational complexity, ensuring both estimation accuracy and real-time performance, thus meeting the requirements of online SLAM for mobile robots.

[0193] The above description is merely a preferred embodiment of the present invention and is not intended to limit the invention. For those skilled in the art, the present invention can have various modifications and variations, such as adjustments to the proportions of the components and reaction conditions. Any modifications, equivalent substitutions, or improvements made within the spirit and principles of the present invention should be included within the scope of protection of the present invention.

Claims

1. A label-based multi-Bernoulli pose and map alternating update SLAM method, characterized in that, Includes the following steps: Step (1): Establish the platform state model, map model and observation model of the SLAM system, wherein the map model is described by a labeled multi-Bernoulli random finite set, and each landmark is assigned a unique label containing the discovery time index and the sequence number index; Step (2): Based on the platform state model, map model and observation model, derive the Bayesian estimation of SLAM under finite set statistics, decompose the joint posterior probability of platform pose and map into pose posterior marginal probability and map posterior marginal probability, and establish an estimation framework for alternating updates of pose and map. Step (3): Describe the platform pose estimation as a Gaussian random variable and the map estimation as a labeled multi-Bernoulli random finite set, and establish a description of the SLAM estimation problem based on Gaussian random state and labeled multi-Bernoulli. Step (4): Based on the model constructed in step (3), predict the platform pose and map landmarks respectively to obtain the predicted state; Step (5): Expand the predicted map landmark set in the field of view into the form of δ-GLMB, and use double-layer Gibbs sampling to reduce the number of association hypotheses: first, at the existence probability level, independently sample the multi-Bernoulli components of the labels to generate candidate δ-GLMBs and truncate them; then at the observation association level, perform Gibbs iterative sampling on the observation association mapping of each candidate δ-GLMB and truncate it, retaining the set of association hypotheses with the highest likelihood probability to obtain the reduction result; Step (6): Based on the predicted state in step (4) and the reduction results in step (5), the platform pose and map landmarks are updated respectively. The platform pose update replaces the equal weighted covariance cross-fusion with the normalized likelihood weight of each associated hypothesis. The pose estimation results under each associated hypothesis are adaptively weighted and fused by Gaussian mixture moment matching. Step (7): Based on the map posterior obtained in step (6), the label multi-Bernoulli random finite set is pruned and regenerated, landmarks with a probability of existence below a set threshold are deleted, and new landmarks are generated for unrelated observation data.

2. The SLAM method based on tag-based multi-Bernoulli pose and map alternating updates according to claim 1, characterized in that, The estimation framework for alternating pose and map updates described in step (2) includes: Based on the posterior estimation of the map at time k-1, the posterior marginal probability of the platform pose at time k is approximated. Based on the posterior estimation result of the platform pose at time k, the posterior edge probability of the map at time k is approximated. The platform pose update uses the predicted map as a condition, and the map update uses the posterior estimate of the platform pose as a condition.

3. The SLAM method based on tag-based multi-Bernoulli pose and map alternating updates according to claim 1, characterized in that, The double-layer Gibbs sampling described in step (5) includes: The predicted set of landmarks within the field of view is divided into a Dobernoli set of labels within the field of view and a Dobernoli set of labels outside the field of view. Based on the existence probability of the label multi-Bernoulli set within the field of view, multiple delta-generalized label multi-Bernoulli hypotheses are generated by independent sampling, and the likelihood probability of each hypothesis is recorded. Extract the top q delta-generalized label multi-Bernoulli hypotheses with the highest likelihood probabilities to form the reduced set of existence labels; For each defined set of existence labels, the Gibbs sampling method is used to generate feasible observation association mappings. The conditional probabilities of each label are traversed and normalized sampling is performed, retaining the association hypothesis with the highest likelihood probability.

4. The SLAM method based on tag-based multi-Bernoulli pose and map alternating updates according to claim 1, characterized in that, The platform pose update in step (6) includes: For each feasible association hypothesis, the pose update result corresponding to each non-zero observation association under that association hypothesis is calculated using extended Kalman filtering, including Kalman gain, updated pose state and covariance; Using the covariance cross-fusion algorithm, the pose update results corresponding to all non-zero observation associations under each association hypothesis are fused to obtain the pose state and covariance under that association hypothesis. Using the normalized likelihood weights of each association hypothesis as natural fusion weights, the pose estimation results under all association hypotheses are adaptively weighted and fused using the Gaussian mixture moment matching formula to obtain the posterior estimate of the platform pose.

5. The SLAM method based on tag-based multi-Bernoulli pose and map alternating updates according to claim 4, characterized in that, The Gaussian mixture moment matching is expressed as: in, Let represent the mean of the final posterior estimate of the platform pose at time k. It is a complete association hypothesis. This represents the set of hypotheses indicating which landmarks exist within the field of view. This represents a hypothetical mapping that indicates which observation corresponds to each existing landmark. Represents the normalized weights of the association hypothesis; In this specific association hypothesis The mean pose estimate obtained after updating individually using extended Kalman filtering is shown below. Let represent the final posterior estimate covariance matrix of the platform pose at time k, and let represent the uncertainty of the final pose estimate. This represents the pose estimation covariance matrix obtained by updating the pose independently using EKF under this specific association assumption, and represents the pose uncertainty within this assumption. Represents any set of tags as of -All feasible mappings for GLMB A set; This represents the set of predicted landmark labels within the current field of view.

6. The SLAM method based on tag-based multi-Bernoulli pose and map alternating updates according to claim 1, characterized in that, The map landmark update described in step (6) includes: For map landmarks outside the field of view, maintain the predicted state unchanged; For map landmarks within the field of view, based on the normalized weights of each association hypothesis, the weighted update results of the existence probability, location mean, and covariance of each landmark are calculated by using the δ-generalized label multi-Bernoulli to label multi-Bernoulli collapse. The updated map landmark sets within and outside the viewport are merged to obtain the final map estimate.

7. The SLAM method based on tag-based multi-Bernoulli pose and map alternating updates according to claim 6, characterized in that, In the update of map landmarks within the field of view, the predicted location mean and covariance remain unchanged for landmarks that are missed, while extended Kalman filtering is used to update landmarks that are detected and associated with observations.

8. The SLAM method based on tag-based multi-Bernoulli pose and map alternating updates according to claim 1, characterized in that, The pruning and regrowth treatment described in step (7) includes: Set an existence probability threshold, and remove landmarks whose landmark components in the map posterior are lower than the threshold from the label Bernoulli set; For each observation data, calculate the probability that it is caused by a known landmark. If the probability is lower than the set threshold for new object determination, the observation is determined to be likely to originate from a new object or clutter. Based on the relationship between the expected number of clutter within the field of view and the number of unassociated observations, the number of newly generated targets is estimated, and corresponding label-based Bernoulli terms are generated for eligible newly generated observations, including the probability of existence, the mean of the location, and the covariance.

9. A label-based multi-Bernoulli pose and map alternating update SLAM system implementing the method as described in any one of claims 1-8, characterized in that, include: The model building module is used to build the platform state model, map model, and observation model of the SLAM system. The Bayesian estimation derivation module derives the Bayesian estimation of SLAM under finite set statistics based on the platform state model, map model and observation model. It adopts a strategy of alternating pose and map updates to approximate the posterior marginal probability of the platform pose and the posterior marginal probability of the map, respectively. The SLAM model building module is used to build SLAM models based on Gaussian random states and label multi-Bernoulli. It describes the estimation of platform pose as a Gaussian random variable and the estimation of map as a label multi-Bernoulli. The prediction module is used to predict the platform pose and map. The Gibbs sampling reduction module is used to perform Gibbs sampling reduction on the number of associated hypotheses. The update module is used to update the platform's pose and map based on the predicted state and reduction results; The pruning and germination module is used to perform multi-Bernoulli pruning and germination of labels based on the obtained map posterior.

Images