A Robust Adaptive UFastSLAM Autonomous Navigation Method Based on KLD Resampling

Abstract

An anti-robust adaptive UFastSLAM autonomous navigation method based on KLD resampling, comprising the steps of: 1. In the carrier pose estimation stage, an anti-robust adaptive factor is incorporated, and the carrier pose is estimated using the robust adaptive unscented particle filter algorithm (RAUPF); 2. In the feature state estimation stage, the robust adaptive unscented filter algorithm (RAUKF) is used to estimate and update the position information of environmental features; 3. In the particle resampling stage, an adaptive particle resampling method based on KL divergence is adopted to online and real-time adjust the required number of particles, improving the real-time performance of the algorithm while ensuring accuracy. This method can be applied to environments where the measurement noise is time-varying and there are abnormal interferences in the measurement information, improving the problems of decreased carrier positioning accuracy and low real-time performance caused by factors such as abnormal disturbances and inaccurate noise statistical characteristics.

Description

Technical Field

[0001] The invention belongs to the technical field of simultaneous localization and mapping (SLAM) based on environmental features, and in particular relates to a KLD resampling-based robust and adaptive UFastSLAM autonomous navigation method. Background Art

[0002] High-precision navigation and positioning functions are important guarantees for AUVs to perform tasks, and they play a vital role in marine scientific research, resource development, salvage and rescue. In recent years, with the development of artificial intelligence, simultaneous positioning and mapping technology has gradually become a research hotspot in the field of underwater navigation because it does not require a priori information maps, and can achieve the advantages of robot self-positioning and environmental feature map construction by continuously sensing environmental information through its own sensors. The SLAM algorithm based on the extended Kalman filter (EKF-SLAM) has been widely used in different research fields due to its simple structure and easy implementation. However, the EKF-SLAM algorithm will introduce truncation errors when approximating linearization, which makes it difficult to achieve ideal estimation effects in strongly nonlinear robot systems. Moreover, the EKF-SLAM algorithm is sensitive to data association, and even a small amount of erroneous association will lead to a serious decrease in the accuracy of the algorithm or even divergence. In response to the above problems, researchers have successively proposed the RBPF algorithm, FastSLAM algorithm and UFastSLAM algorithm based on EKF. However, in the actual operating environment, the shaking of the underwater carrier, signal disturbances and changes in the environment may cause the noise statistical characteristics of the system to change or the state to mutate, thereby increasing the difference between the proposed distribution function calculated by UKF and the true posterior probability distribution, further aggravating the particle degradation problem and reducing the estimation accuracy of the UFastSLAM algorithm.

[0003] The existing solutions mainly include the following:

[0004] Application date: February 22, 2019; Application number: CN201811354176.1, Patent title: AUV Docking and Recovery Autonomous Navigation Method Based on FMSRUPF Algorithm. This patent discloses an AUV docking and recovery autonomous navigation method based on the FMSRUPF algorithm, that is, for the AUV docking and recovery autonomous navigation based on SINS / USBL / DVL, an attenuated memory square root unscented particle filter algorithm, namely the FMSRUPF algorithm, is provided, and the systematic combined particle resampling method is adopted for its resampling part. The adopted FMSRUPF algorithm uses an attenuation factor to reduce the influence of historical information on filtering, enhances the role of current measurement information in filtering calculation, then uses the square root matrix of the covariance matrix to replace the covariance matrix for filtering solution, and finally improves the resampling process with the systematic combined particle resampling method. The present invention solves the problem of filtering divergence caused by rough or distorted establishment of the AUV docking and recovery system model through the FMSRUPF algorithm and the systematic combined particle resampling method, and combines good numerical characteristics and medium computational burden, effectively improving the positioning accuracy and stability of the AUV docking and recovery autonomous navigation system.

[0005] It provides an attenuated memory square root unscented particle filter algorithm for the autonomous navigation of SINS / USBL / DVL, and adopts the systematic combined particle resampling method for its resampling part, solving the problem of filtering divergence caused by rough or distorted establishment of the AUV docking and recovery system model. This paper proposes an anti-robust adaptive UFastSLAM autonomous navigation method based on KLD resampling for SLAM positioning and navigation. This method uses the anti-robust adaptive unscented particle filter algorithm and the anti-robust adaptive unscented filter algorithm to estimate and update the pose and feature position information of the carrier respectively, and adopts the adaptive particle resampling method based on KL divergence in the particle resampling stage, solving the problems of decreased positioning accuracy and low real-time performance of the carrier caused by factors such as abnormal disturbances and inaccurate noise statistical characteristics.

[0006] Application date: January 16, 2018; Application number: CN201710717024.2, Patent title: AUV autonomous navigation method based on Unscented FastSLAM algorithm. This patent discloses an AUV autonomous navigation method based on Unscented FastSLAM algorithm, including the following steps: 1) The AUV obtains initial pose information through GPS and navigation sensors on the water surface; 2) Use unscented particle filter to predict the AUV pose and environmental landmarks according to the latest control quantity and sensor observation quantity input to the AUV; 3) Use fading adaptive unscented particle filter to generate a proposal distribution function with parameter adaptive adjustment and sample from it; 4) According to each particle associated with the latest observed environmental information, use unscented Kalman filter to update the estimation of each feature; 5) Use adaptive partial systematic resampling method to resample the particle set; 6) Perform AUV positioning and map construction. By improving the proposal distribution function and resampling process of the Unscented FastSLAM algorithm, the present invention can improve the particle sampling efficiency of the Unscented FastSLAM algorithm, reduce the degradation degree of particles, and greatly improve the consistency of AUV pose estimation and the accuracy of autonomous navigation.

[0007] It uses fading adaptive unscented particle filter to generate a proposal distribution function with parameter adaptive adjustment and sample from it. At the same time, it uses adaptive partial systematic resampling method to resample the particle set, improving the particle sampling efficiency of the Unscented FastSLAM algorithm and reducing the degradation degree of particles. This paper uses robust adaptive unscented particle filter algorithm to estimate the pose information of the vehicle, and uses adaptive particle resampling method based on KL divergence in the particle resampling stage. In addition, this paper uses robust adaptive unscented filter algorithm to estimate and update the feature position information, improving the problems of vehicle positioning accuracy decline and low real-time performance caused by factors such as abnormal disturbance and inaccurate noise statistical characteristics. Summary of the Invention

[0008] To solve the problems existing in the prior art, the present invention proposes a robust adaptive UFastSLAM autonomous navigation method based on KLD resampling. Aiming at the problems of performance degradation of the UFastSLAM algorithm and low real-time performance of the algorithm caused by fixed number of particles due to abnormal disturbance and inaccurate noise statistical characteristics during the navigation process of underwater vehicles, a RAUFastSLAM algorithm based on improved particle proposal distribution estimation and adaptive KLD resampling is proposed.

[0009] The present invention provides a robust adaptive UFastSLAM autonomous navigation method based on KLD resampling, including the following steps:

[0010] (1) In the carrier pose estimation stage, a robust adaptive factor is incorporated, and the carrier pose is estimated using the robust adaptive unscented particle filter algorithm;

[0011] (2) In the feature state estimation stage, the robust adaptive unscented filter algorithm is used to estimate and update the position information of environmental features;

[0012] (3) In the particle resampling stage, an adaptive particle resampling method based on KL divergence is adopted to adjust the required number of particles online in real time, improving the real-time performance of the algorithm while ensuring accuracy.

[0013] As a further improvement of the present invention, the specific method of step (1) is as follows:

[0014] (1-1) Carrier pose prediction stage:

[0015] In the carrier pose prediction stage, the state vector after particle augmentation and the covariance matrix can be respectively expressed as:

[0016]

[0017]

[0018] Among them, and are respectively the state vector and covariance matrix at time t-1, and Q t is the control noise covariance.

[0019] Perform Cholesky decomposition on , then the decomposed covariance matrix is expressed as:

[0020]

[0021] Among them, chol represents Cholesky decomposition;

[0022] According to the Sigma point symmetric sampling strategy, 2L+1 Sigma points are extracted from the robot pose state, where L is the dimension of the augmented state vector, then the Sigma point set is:

[0023]

[0024]

[0025]

[0026] Among them, i represents the i th th column of the matrix, and λ = α 2(L + κ) - L, where α, 0 < α < 1 is a very small number used to avoid non - local effects that occur when sampling a strongly non - linear system. κ is called the Sigma scaling parameter, and here the default value κ = 0 is taken;

[0027] Then, the sampled points are transformed through the non - linear motion function f(·).

[0028]

[0029] Among them, and are the vehicle state component and the control component in the augmented state vector respectively;

[0030] By performing linear weighted processing on the state prediction vector and state prediction covariance matrix of the system are obtained as:

[0031]

[0032]

[0033] Among them, and are the weights of the mean and covariance respectively, and are calculated by the following formula:

[0034]

[0035]

[0036]

[0037] In the formula, β is a parameter that combines the high - order moment information term of the posterior probability distribution, and the ideal value is 2;

[0038] (1 - 2) Vehicle pose update stage:

[0039] In the vehicle pose update stage, when a certain feature is observed by the vehicle again, the measurement value Sigma point set generated by the measurement equation h(·) is:

[0040]

[0041] Among them, represents the position estimated value of the k th th feature at time t - 1;

[0042] According to the measurement Sigma point set, the mean of the predicted measurement value is calculated as:

[0043]

[0044] Thus, the innovation covariance and the cross-covariance between the vehicle state and the observables are expressed as:

[0045]

[0046]

[0047] where the adaptive robust factor is calculated by the following formula:

[0048]

[0049] where c is a constant, and generally c = 1.0 - 2.5, and ΔS t is calculated by the following formula:

[0050]

[0051] where tr(·) is the matrix inversion operator;

[0052] Therefore, the innovation covariance matrix is expressed as:

[0053]

[0054] According to the innovation covariance and the cross-covariance matrix with the Kalman gain matrix is calculated as:

[0055]

[0056] Therefore, the posterior estimation mean and covariance matrix of the vehicle pose state are respectively expressed as:

[0057]

[0058]

[0059] When multiple feature points are observed at the same moment, the calculation is repeated to obtain the final estimated mean and covariance of the vehicle pose state.

[0060] Finally, the results of the above formula are used as the statistics of the particle proposal distribution, and at the same time, a new generation of particles are generated:

[0061]

[0062] (1 - 3) Importance weight calculation:

[0063] The importance weights of the RAUFastSLAM algorithm are also in the form of a Gaussian distribution and are calculated by the following formula:

[0064]

[0065] Among them,

[0066] As a further improvement of the present invention, the specific method of step (2) is as follows:

[0067] (2-1) Initialization of the new feature state:

[0068] In the initialization stage of the new feature state, if the position information of each feature point in the underwater environment is represented by a 3×1 column vector, that is, the dimension of the feature state is l = 3, then according to the symmetry sampling principle, 2l + 1 Sigma points need to be sampled for each feature. Therefore, from the current observation z t and the measurement noise covariance R t the Sigma point set is:

[0069] ψ [0][m] = z t

[0070]

[0071]

[0072] Perform a non-linear transformation on the constructed Sigma points:

[0073]

[0074] Then the mean and covariance of the new feature are respectively expressed as:

[0075]

[0076]

[0077] (3-2) Update the position of the existing feature;

[0078] When the observed feature is associated with the existing feature data in the state quantity, the position information of this feature needs to be updated. First, before sampling the Sigma points of this feature, the state vector and covariance of the feature are expanded, and the expanded state vector and covariance are respectively expressed as:

[0079]

[0080]

[0081] At the same time, construct a 2n + 1 Sigma point set as:

[0082]

[0083]

[0084]

[0085] Among them, n is the dimension of the augmented matrix, and λ = α 2 (n + κ) - n, where α = 0.01 and κ = 0 are taken;

[0086] By transforming the Sigma point set according to the current pose information of the carrier and the nonlinear measurement model h(·), we can obtain:

[0087]

[0088] Therefore, the predicted measurement value and the corresponding covariance matrix are respectively expressed as:

[0089]

[0090]

[0091] Similarly, to suppress the influence of environmental uncertainty interference on the feature estimation accuracy, an adaptive robust factor is introduced into the measurement innovation covariance matrix. Among them, the adaptive robust factor is calculated by the following formula:

[0092]

[0093] Among them, c is a constant, usually 1.0 to 2.5, and is calculated by the following formula:

[0094]

[0095] Then the measurement innovation covariance matrix and the cross-covariance matrix are respectively expressed as:

[0096]

[0097]

[0098] From this, its filtering gain can be obtained as:

[0099]

[0100] Therefore, the mean and covariance matrix of the feature are respectively expressed as:

[0101]

[0102]

[0103] As a further improvement of the present invention, the specific method of step (3) is as follows:

[0104] To avoid particle degeneracy, when the number of effective particles N in the particle set eff is less than a certain threshold, where the threshold is taken as 75% of the total number of particles, a resampling operation needs to be performed on it. Among them, the number of effective particles is obtained by the following formula:

[0105]

[0106] where M is the total number of particles, is the normalized weight of the m th -th particle;

[0107] The KL divergence between the true posterior probability distribution p(x k ) of the carrier pose state and its particle-based approximate distribution q(x k ) is defined as:

[0108]

[0109] Whenever new particles are resampled, in order to make the KL divergence less than a pre-given error e with probability 1 - σ, the minimum number of particles N required k is calculated by the following formula:

[0110]

[0111] where B represents the number of non-empty subspaces, and Z 1-σ represents the 1 - σ upper quantile of the standard normal distribution.

[0112] Beneficial effects:

[0113] The present invention discloses a robust adaptive UFastSLAM autonomous navigation method based on KLD resampling. This method aims at the problem that the accuracy of the SLAM filtering algorithm decreases due to factors such as environmental changes and abnormal disturbances during the navigation of an underwater vehicle. It incorporates a robust adaptive factor and suppresses the influence of factors such as state disturbances and inaccurate noise statistical characteristics on the positioning accuracy of the filtering algorithm by controlling the filtering gain, thereby improving the robustness and adaptability of the algorithm to environmental disturbances. At the same time, to reduce the increasing computational amount caused by the increase in the number of particles, during the particle resampling process, the minimum number of particles required is dynamically determined through the KL divergence between the posterior distribution of the carrier pose state and its approximate distribution represented by particles, improving the real-time performance of the algorithm. Description of the drawings

[0114] Figure 1 is a simulation environment designed based on the SLAM simulator for the disclosed method of the present invention;

[0115] Figure 2 is the overall flowchart of the disclosed method of the present invention. Detailed implementation manners

[0116] The present invention will be further described in detail below in conjunction with the accompanying drawings and specific embodiments:

[0117] The present invention discloses a robust adaptive UFastSLAM autonomous navigation method based on KLD resampling. Its overall flowchart is as Figure 2 shown, and the simulation environment designed by its SLAM simulator is as Figure 1 shown, including the following steps:

[0118] Step 1: In the carrier pose estimation stage, a robust adaptive factor is incorporated, and the RAUPF algorithm is used to estimate the carrier pose. It is mainly divided into the carrier pose prediction stage, the carrier pose update stage, and the importance weight calculation. First, in the carrier pose prediction stage, the state vector and covariance matrix after particle expansion can be respectively expressed as:

[0119]

[0120]

[0121] Among them, and are the state vector and covariance matrix at time t - 1 respectively, and Q t is the control noise covariance.

[0122] Perform Cholesky decomposition on , then the decomposed covariance matrix can be expressed as:

[0123]

[0124] Among them, chol represents Cholesky decomposition.

[0125] According to the Sigma point symmetric sampling strategy, extract 2L + 1 Sigma points for the robot pose state. L is the dimension of the augmented state vector, then the Sigma point set is:

[0126]

[0127]

[0128]

[0129] Among them, i represents the i th th column of the matrix, and λ = α 2(L + κ) - L, where α (0 < α < 1) is a very small number used to avoid non - local effects that occur when sampling a strongly non - linear system. κ is called the Sigma scaling parameter, and here the default value κ = 0 is taken.

[0130] Then, the sampled points are transformed through the non - linear motion function f(·).

[0131]

[0132] Among them, and are the vehicle state component and the control component in the augmented state vector respectively.

[0133] By linearly weighting the state prediction vector and state prediction covariance matrix of the system can be obtained as:

[0134]

[0135]

[0136] Among them, and are the weights of the mean and covariance respectively, which can be calculated by the following formula:

[0137]

[0138]

[0139]

[0140] In the formula, β is a parameter that combines the high - order moment information term of the posterior probability distribution, and the ideal value is 2.

[0141] In the vehicle pose update stage, when a certain feature is observed by the vehicle again, the measurement value Sigma point set can be generated by the measurement equation h(·) as:

[0142]

[0143] Among them, represents the position estimated value of the k th th feature at time t - 1.

[0144] According to the measurement Sigma point set, the mean of the predicted measurement value is calculated as:

[0145]

[0146] From this, the innovation covariance and the cross - covariance between the vehicle state and the observable can be expressed as:

[0147]

[0148]

[0149] When the vehicle is navigating underwater, the system is vulnerable to environmental influences, such as state disturbances, inaccurate noise statistical characteristics, and gross measurement errors, which will affect the positioning accuracy of the filtering algorithm. Therefore, an adaptive robust factor is introduced in this paper to adjust the covariance matrix in real time, and then the influence of factors such as state disturbances and inaccurate noise statistical characteristics on the positioning accuracy of the filtering algorithm is suppressed by controlling the filtering gain, so as to improve the fault tolerance and robustness of the system. Among them, the adaptive robust factor can be calculated by the following formula:

[0150]

[0151] where c is a constant, and generally c = 1.0 - 2.5. ΔS t can be calculated by Equation (14):

[0152]

[0153] where tr(·) is the matrix inversion operator.

[0154] Therefore, the innovation covariance matrix can be expressed as:

[0155]

[0156] According to the innovation covariance and cross-covariance matrices with the Kalman gain matrix is calculated as:

[0157]

[0158] Therefore, the mean and covariance matrix of the posterior estimation of the vehicle pose state can be expressed as:

[0159]

[0160]

[0161] When multiple feature points are observed at the same time, the calculation is repeated to obtain the final mean and covariance of the vehicle pose state estimation.

[0162] Finally, the results of the above two formulas are used as the statistics of the particle proposal distribution, and a new generation of particles are generated at the same time:

[0163]

[0164] The importance weight of the RAUFastSLAM algorithm also follows a Gaussian distribution and can be calculated using the following formula:

[0165]

[0166] where

[0167] Step 2: In the feature state estimation stage, the RAUKF algorithm is used to estimate and update the position information of environmental features. It is mainly divided into the initialization of new feature states and the position update of existing features. First, in the initialization stage of new feature states, if the position information of each feature point in the underwater environment is represented by a 3×1 column vector, that is, the dimension of the feature state is l = 3, then according to the symmetry sampling principle, 2l + 1 Sigma points need to be sampled for each feature. Therefore, from the current measurement z t and the measurement noise covariance R t the Sigma point set can be obtained as:

[0168] ψ [0][m] = z t

[0169]

[0170]

[0171] Perform a non-linear transformation on the constructed Sigma points:

[0172]

[0173] Then the mean and covariance of the new feature can be expressed as:

[0174]

[0175]

[0176] When the observed feature is associated with the existing feature data in the state quantity, the position information of this feature needs to be updated. First, before sampling the Sigma points of this feature, the state vector and covariance of the feature are expanded, and the expanded state vector and covariance can be expressed as:

[0177]

[0178]

[0179] At the same time, construct a 2n + 1 Sigma point set as:

[0180]

[0181]

[0182]

[0183] where n is the dimension of the augmented matrix, and λ = α 2 (n + κ) - n, where α = 0.01 and κ = 0 are taken.

[0184] By transforming the Sigma point set using the current pose information of the carrier and the non - linear measurement model h(·), we can obtain:

[0185]

[0186] Therefore, the predicted measurement value and the corresponding covariance matrix can be expressed as:

[0187]

[0188]

[0189] Similarly, to suppress the influence of environmental uncertainty interference on the feature estimation accuracy, an adaptive robust factor is introduced into the measurement innovation covariance matrix. Among them, the adaptive robust factor can be calculated by the following formula:

[0190]

[0191] where c is a constant, usually 1.0 - 2.5. And can be calculated by the following formula:

[0192]

[0193] Then the measurement innovation covariance matrix and the cross - covariance matrix can be expressed as:

[0194]

[0195]

[0196] From this, its filtering gain can be obtained as:

[0197]

[0198] Therefore, the mean and covariance matrix of the feature can be expressed as:

[0199]

[0200]

[0201] Step 3: In the particle resampling stage, an adaptive particle resampling method based on KL divergence is used to adjust the required number of particles online in real time, improving the real-time performance of the algorithm while ensuring accuracy. To avoid particle degradation, when the number of effective particles N in the particle set eff is less than a certain threshold (usually taking 50% and 75% of the total number of particles), resampling operation needs to be performed on it. Among them, the number of effective particles can be obtained by the following formula:

[0202]

[0203] where M is the total number of particles, is the normalized weight of the m th -th particle.

[0204] To update the required number of particles in the sampling process in real time and balance the relationship between estimation accuracy and time complexity, this paper adopts an adaptive particle resampling method based on KL divergence. Usually, the KL divergence between the true posterior probability distribution p(x k ) of the carrier pose state and its particle-based approximate distribution q(x k ) is defined as:

[0205]

[0206] Whenever new particles are resampled, to make the KL divergence less than a pre-given error e with probability 1 - σ, the minimum number of particles N k required can be calculated by the following formula:

[0207]

[0208] where B represents the number of non-empty subspaces, and Z 1-σ represents the 1 - σ upper quantile of the standard normal distribution.

[0209] The above is only a preferred embodiment of the present invention, and it is not any other form of limitation to the present invention. Any modification or equivalent change made according to the technical essence of the present invention still belongs to the scope protected by the present invention.

Claims

1. A robust adaptive UFastSLAM autonomous navigation method based on KLD resampling, characterized in that: It includes the following steps: (1) In the carrier pose estimation stage, a robust adaptive factor is incorporated, and the carrier pose is estimated using the robust adaptive unscented particle filter algorithm; The specific method of step (1) is as follows: (1-1) Carrier pose prediction stage: In the carrier pose prediction stage, the state vector after particle expansion and the covariance matrix are respectively expressed as: ； ； Among them, and are respectively the state vector and covariance matrix at the moment, is the control noise covariance; If a Cholesky decomposition is performed, the covariance matrix after decomposition is expressed as: ； Among them, represents Cholesky decomposition; According to the Sigma point symmetric sampling strategy, 2L + 1 Sigma points are extracted from the robot pose state, where L is the dimension of the augmented state vector, and the Sigma point set is: ； Among them, represents the th column of the matrix, , is a very small number used to avoid non-local effects that occur when sampling a strongly non-linear system, is called the Sigma scaling parameter, and the default value here is ; Then, the sampled points are transformed through a non-linear motion function for conversion; ； wherein, and are the carrier state component and the control component in the augmented state vector, respectively; By performing linear weighting on the state prediction vector and state prediction covariance matrix of the system are obtained as follows: ； ； wherein, and are the weights of the mean value and the covariance respectively, and are calculated by the following formula: ； In the formula, is the parameter that combines the high-order moment information term of the posterior probability distribution, and the ideal value is 2; (1-2) Carrier pose update stage: During the carrier pose update phase, when a certain feature is observed again by the carrier, the measurement value Sigma point set is generated by the measurement equation as follows: ； Among them, represents the th estimated value of the position of the feature at the The mean value of the predicted measurement is calculated according to the measured Sigma point set as: ； From this, the innovation covariance and the cross-covariance between the carrier state and the observable are expressed as: ； ； Among them, the adaptive robust factor is calculated by the following formula: ； wherein, is a constant, and generally , is calculated by the following formula: ； Among them, is the matrix inversion operator; Therefore, the innovation covariance matrix is expressed as: ； Calculate the Kalman gain matrix according to the innovation covariance and cross-covariance matrices with as follows: ； Therefore, the posterior estimation mean value and covariance matrix of the carrier pose state are respectively expressed as: ； ； When multiple feature points are observed at the same time, repeated calculations are performed to obtain the final estimated mean value and covariance of the carrier pose state; Finally, the result of the above formula is used as the statistic of the particle proposal distribution, and at the same time, a new generation of particles is generated: ； (1-3) Importance weight calculation: The importance weight of the RAUFastSLAM algorithm is also in the form of a Gaussian distribution and is calculated by the following formula: ； Among them, : (2) In the feature state estimation stage, the robust adaptive unscented filter algorithm is used to estimate and update the position information of the environmental features; The specific method of step (2) is as follows: (2-1) New feature state initialization: In the initialization stage of the new feature state, if the position information of each feature point in the underwater environment is represented by the column vector of , that is, the dimension of the feature state is , then according to the symmetry sampling principle, Sigma points need to be sampled for each feature. Therefore, from the current observation and the measurement noise covariance , the Sigma point set is obtained as follows: ； Nonlinear transformation is performed on the constructed Sigma points: ； Then the mean value and covariance of the new feature are respectively expressed as: ； ； (3-2) Position update of existing features; When the observed feature is associated with the existing feature data in the state quantity, the position information of this feature needs to be updated. First, before sampling the Sigma points of this feature, the state vector and covariance of the feature are expanded, and the expanded state vector and covariance are respectively expressed as: ； ； Meanwhile, construct sigma point sets as follows: ； Among them, is the dimension of the augmented matrix, , where is taken; From the current pose information of the carrier and the non-linear measurement model By transforming the Sigma point set, we can obtain: ； Therefore, the predicted measurement value and the corresponding covariance matrix are respectively expressed as: ； ； Similarly, in order to suppress the influence of environmental uncertainty interference on the feature estimation accuracy, an adaptive robust factor is introduced into the measurement innovation covariance matrix, where the adaptive robust factor is calculated by the following formula: ； wherein, is a constant, usually , and is calculated by the following formula: ； Then the measurement innovation covariance matrix and the cross-covariance matrix are respectively expressed as: ； ； From this, its filtering gain is: ； Therefore, the mean value and covariance matrix of the feature are respectively expressed as: ； ； (3) In the particle resampling stage, an adaptive particle resampling method based on KL divergence is used to online and real-time adjust the required number of particles, and the real-time performance of the algorithm is improved while ensuring the accuracy; The specific method of step (3) is as follows: To avoid particle degeneracy, when the number of effective particles in the particle set is less than a certain threshold, where the threshold is selected as 75% of the total number of particles, it is necessary to perform a resampling operation on it. Among them, the number of effective particles is obtained by the following formula: ； Among them, is the total number of particles, is the normalized weight of the th particle; True posterior probability distribution of the carrier pose state and its particle-based approximate distribution The KL divergence between them is defined as: ; Whenever a new particle is resampled, in order to make the KL divergence less than a pre-given error with probability less than a pre-given error , the minimum number of particles required is calculated by the following formula: ； Among them, represents the number of non-empty subspaces, represents the upper quantile of the standard normal distribution.

Images