Search

CN-115307645-B - Simultaneous positioning and mapping method based on Bernoulli filter

CN115307645BCN 115307645 BCN115307645 BCN 115307645BCN-115307645-B

Abstract

The invention discloses a simultaneous positioning and mapping method based on a Bernoulli filter, which comprises the specific steps of (1) initializing parameters, (2) obtaining input data, (3) obtaining a robot position predicted value, (4) obtaining an observation set of a robot, (5) obtaining Bernoulli items of the robot used for representing map features at a kth moment through a potential equalization Bernoulli filtering method, (6) extracting targets of the Bernoulli items, 7) recording the number and the pose of the map features obtained at the k moment, (8) judging whether to execute a map optimizing process through a self-adaptive information control method, (9) judging whether to execute the pose of the robot corresponding to t moments of the map optimizing method, and then executing the step (2), and (10) judging whether to reach the maximum running moment number to determine whether to output the pose of the robot and the state estimated value of the map features. The method improves the estimation precision of the robot pose in the simultaneous positioning and mapping method and improves the instantaneity.

Inventors

  • ZHANG FEI
  • ZHANG ZIJING
  • JI CHUANTANG

Assignees

  • 江苏科技大学

Dates

Publication Date
20260512
Application Date
20220829

Claims (10)

  1. 1. The simultaneous localization and mapping method based on the multiple Bernoulli filter is characterized by comprising the following steps of: (1) Initializing a running time number parameter, an optimizing time number parameter, a multiple Bernoulli existence parameter and a maximum running time number parameter; (2) The motion speed v and the direction angle theta of the robot, the direct distance d between the map feature and the robot and the azimuth angle are obtained through sensors carried by the robot The sensor is an inertial navigation element or a laser radar; (3) Calculating and obtaining a predicted value of the robot pose at the kth moment through a motion equation f (v, theta, k) of the robot based on the motion speed v and the direction angle theta of the robot during operation obtained in the step (2); (4) Obtaining a predicted value of the pose of the robot at the kth moment based on the step (3), and based on the direct distance d and the azimuth angle of the step (2) By observing equations Obtaining an observation set corresponding to the kth moment of the robot; (5) Based on the information obtained in the step (3) and the step (4), carrying out state estimation on the map features by a potential equalization multiple Bernoulli filtering method to obtain Bernoulli items of the robot for representing the map features at the kth moment; (6) Performing target extraction on the Bernoulli term obtained in the step (5) according to the value of the existence probability parameter r, wherein the result is used as a state estimation value of map features, and the state estimation value comprises the number of the map features and the pose of the map features; (7) Recording the number of map features and the pose of the map features obtained at the moment k based on the state estimation value of the map features obtained in the step (6); (8) Judging whether the prior information meets a threshold value or not through a self-adaptive information control method based on the map feature state estimation value obtained in the step (7), and executing the step (2) when the prior information does not meet the threshold value, wherein k=k+1 and t=t+1; (9) Based on the step (8), when the condition of the self-adaptive information control method is met, estimating the pose of the robot at the t moments by a graph optimization method so as to update the pose of the robot corresponding to the t moments, and executing the step (2) by enabling k=k+1 and t=0; (10) And (3) judging whether the maximum running time number is reached based on k of the step (8) and the step (9), if yes, completing the map optimization process of the last step (9), outputting the state estimation values of the pose and the map features of the robot, and ending, otherwise, executing the step (2).
  2. 2. The method for simultaneous localization and mapping based on a bernoulli filter according to claim 1, wherein the method and the step for initializing the running time number parameter, the optimization time number parameter, the bernoulli presence parameter, and the maximum running time number parameter in the step (1) are as follows: (11) Initializing a running time number parameter, and enabling k=0; (12) Initializing an optimization time parameter, wherein t=0; (13) Initializing a bernoulli presence parameter, and enabling r=0.99; (14) The maximum runtime number parameter is initialized, let k max =500.
  3. 3. The method for simultaneous localization and mapping based on a Bernoulli filter according to claim 1, wherein in the step (2), the motion velocity v and the direction angle θ of the robot, the direct distance d and the azimuth angle of the map feature and the robot are obtained by the sensor carried by the robot The method and the steps of (1) are as follows: (21) The motion speed v and the direction angle theta of the robot are obtained through a sensor carried by the robot; (22) Obtaining the distance d and azimuth angle between the map feature and the robot directly through a sensor carried by the robot
  4. 4. The method for simultaneous localization and mapping based on a bernoulli filter according to claim 1, wherein the method for predicting the pose of the robot by the motion equation f (v, θ, k) of the robot in the step (3) comprises the following steps: (31) Taking the initial moment, namely the pose of k=0 as an origin; (32) Establishing a Cartesian coordinate system by taking the motion direction at the initial moment as a y axis; (33) The robot motion equation f (v, θ, t) is determined by equation (1); Where v k denotes a moving speed of the robot at the kth time, θ k denotes a forward angle of the robot at the kth time, and R f,k denotes a process noise of the robot operation process.
  5. 5. The method for simultaneous localization and mapping based on a Bernoulli filter according to claim 1, wherein in the step (4), the method is performed by observing equation The method for obtaining the observation set obtained by the robot at the kth moment is that Observation equation Determined by equation (2): In the above-mentioned method, the step of, Is an observation set of the robot, d k , Representing the distance and direction included angles between the map features observed by the sensor and the mobile robot, respectively, X l and Y l are the X-axis and Y-axis coordinates of the first map feature, and R z,k is the observed noise covariance matrix.
  6. 6. The method for simultaneous localization and mapping based on a Bernoulli filter according to claim 1, wherein in the step (5), the state estimation is performed on the map feature by using the potential equalization Bernoulli filter method to obtain a Bernoulli term of the robot for representing the map feature at the kth time The method and the steps of (1) are as follows: (51) Describing map features in an observed range of the robot at time k based on the predicted value of the robot pose obtained in the step (3), wherein the map features are expressed in a random finite set form, and a random finite set model is obtained through a formula (3); Wherein, the A random finite set representing map features of robots 0 through k, M k-1 representing a random finite set of map features of robots 0 through k-1, M representing a random finite set of overall map features, X k representing the pose of the robot at k, Representing the new map features of the robot at the moment k; (52) Based on the observation set of the map features of the robot at the kth moment obtained in the step (4), the observation set is represented in a random finite set form, and a random finite set model is obtained through a formula (4); Wherein, the set Z k represents the observation set of the robot at the kth moment, X k is the pose of the robot, D k (m,X k ) represents the real observation of the robot on the map feature at the position X k , and C k (X k ) represents the clutter error observation set observed by the robot at the pose X k ; (53) Constructing a simultaneous localization and mapping problem through a conditional Bayesian formula, namely estimating the joint posterior probability density of the map feature M k and the robot pose X 1:k of the robot under the condition of an observation set Z k , wherein the simultaneous localization and mapping problem is represented through a formula (5); π k|k (M k ,X 1:k |Z 1:k ,u 1:k ,X 0 ) (5) Wherein Z 1:k represents the observation set of the robot from the 1 st time to the kth time, X 1:k represents the pose of the robot from the 1 st time to the kth time, M k represents the map features around the robot at the k time, and X 0 represents the pose at the initial time; (54) For convenience of implementation, the decomposed form of formula (4), i.e., formula (6), is obtained by a conditional probability formula and factorization; π k (M k ,X 1:k |Z 0:k ,u 0:k-1 ,X 0:k )＝π k (X 1:k |Z 0:k ,u 0:k-1 ,X 0 )π k (M k |Z 0:k ,X 0:k ) (6) Pi k (X 1:k |Z 0:k ,u 0:k-1 ,X 0 ) represents the joint posterior estimation of the robot under the conditions of observed quantity at the kth moment, control quantity at the k-1 moment and initial pose quantity, pi k (M k |Z 0:k ,X 0:k ) represents the joint posterior estimation of map features under the condition that the robot obtains an observation set Z 0:k when the pose is X 0:k ; (55) Obtaining M k-t:k by step (7); (56) Establishing a multi-Bernoulli random finite set form of new map features observed by the robot at the kth moment based on the step (55), wherein the multi-Bernoulli random finite set form can be obtained through a formula (7); Wherein, the The presence probability parameters of a random finite set of bernoulli representing the nascent map features, The new map feature b (m|X k ) at the moment comprises observed map features of a single robot per se at the moment k-1 and state estimated values of the map features obtained at t moments before the moment k which are added as a part of the new map feature set; (57) Based on the step (56), after the multi-Bernoulli random finite set form of the new map features is obtained, a Gaussian mixture specific implementation form of probability density parameters p is obtained through a formula (8); Wherein, the The number of Gaussian items corresponding to Bernoulli items of the new map feature at the moment k is represented, The bernoulli term representing the new map feature at time k corresponds to the weight of its gaussian term, The bernoulli term representing the new map feature at time k corresponds to the mean value of its gaussian term, A covariance matrix of a Gaussian term corresponding to a Bernoulli term representing the new map feature at the moment k; (58) Determining a multiple Bernoulli random finite set representation of a priori map features obtained by the robot at the k-1 time, wherein the multiple Bernoulli random finite set representation comprises a presence probability parameter r and a probability density parameter p and can be obtained through a formula (9); Wherein, the A probability of existence parameter of a multiple bernoulli random finite set representing the L k-1 a priori map features obtained at time k-1, A probability density parameter of presence of a multiple Bernoulli random finite set representing the L k-1 a priori map features obtained at time k-1; (59) Based on step (58) Which is determined by formula (10); Wherein, the The map feature weights, means and covariance at time k-1 are represented respectively, The number of Gaussian items; (510) Establishing a Bernoulli random finite set form of a predicted value of a map feature state estimated value obtained by a robot at a kth moment based on the novel map feature Bernoulli random finite set form in the step (57) and the surviving map feature Bernoulli random finite set form in the step (59), and obtaining the predicted value through a formula (11); Wherein, the Initial value of 0.99; The predicted value of the presence probability parameter r of the bernoulli term is determined by the formula (12); Wherein p S,k is 0.95 as a survival probability; the predicted value of probability density parameter p of the bernoulli term is represented by a gaussian mixture, determined by equation (13); Wherein the predicted value of the mean value of the gaussian term is determined by formula (14); the predicted value of the covariance matrix of the gaussian term can be determined by equation (15); The initial value of the weight of the Gaussian term is 0.1; (511) Obtaining an updated value of the state estimate of the map feature based on the predicted value of the state estimate of the map feature of step (510), the form of the limited set of the bernoulli random values being determined by equation (16), comprising the limited set of the bernoulli random values of the missed portion of the map feature and the limited set of the bernoulli random values of the updated portion of the map feature resulting from the observation; the bernoulli probability density parameter is obtained by a form of gaussian mixture, determined by equation (17); Wherein, p D,k is the detection probability, The weight of the j-th gaussian term of the l-th bernoulli term; Obtaining a weight, a mean value and a covariance matrix of a Gaussian item corresponding to the p parameter of each Bernoulli item through extended Kalman filtering, and obtaining the Gaussian item through formulas (18) - (21); the updated value of the bernoulli parameters of the robot is determined by formulas (22) - (25); and updating the map feature pose according to the formula, namely updating parameters r and p.
  7. 7. The method for simultaneous localization and mapping based on a bernoulli filter according to claim 1, wherein the method and the step for extracting the target by obtaining the bernoulli term in the step (5) according to the value of the existence probability parameter r in the step (6) are as follows: (61) Setting an existence probability threshold T r ＝10 -3 , and eliminating Bernoulli items with the existence probability smaller than a threshold value after each update; (62) Summing the existence probability parameters of the surviving Bernoulli terms in the step (61) to obtain integers, wherein the obtained integers are the number of map features, and the number is recorded as N map_k ; (63) Setting a weight threshold T p ＝10 -5 , pruning Gaussian components of the rest Bernoulli terms in the step (61), and pruning Gaussian components with weights smaller than the threshold; (64) Based on the pruning result in the step (63), setting a gaussian item distance threshold T m , and combining gaussian items with position distances smaller than the threshold T m =1 m to form a gaussian component; (65) Based on the step (61) and the step (64), taking out the Gaussian item with the largest weight from the Gaussian items corresponding to the surviving Bernoulli items obtained by the robot at the kth moment; (66) And (3) taking the mean value of the Gaussian item obtained in the step (65) as the state estimation value of the map feature corresponding to the surviving Bernoulli item.
  8. 8. The method for simultaneous localization and mapping based on a bernoulli filter according to claim 1, wherein in the step (8), based on the map feature state estimation value obtained in the step ((7), the method for controlling the adaptive information is used to determine whether the prior information satisfies the threshold, if not, the step (2) is executed, and if not, k=k+1, t=t+1, and if yes, the specific content and method for executing the step (9) include the following steps: (81) Setting a distance screening threshold, determining by formula (26), d T =R+v×dt (26) (82) Setting a priori information filtering threshold based on the estimated value of the map feature obtained in the step (7) and the distance threshold obtained in the step (81), determining by a formula (27), Wherein N z (X i ) represents the number of map features observed when the robot pose is X i and the observation range is d T , N f represents the number of sides composed of motion information, and a is an information control value; (83) Based on the step (82), whether a is smaller than or equal to a T is satisfied or not is judged, if not, the step (2) is executed, and if satisfied, the step (8) is executed, so that k=k+1 and t=t+1.
  9. 9. The method for simultaneous localization and mapping based on a bernoulli filter according to claim 1, wherein in the step (9), if the condition of the adaptive information control method is satisfied in the step (8), estimating the pose of the robot at the t moments by a graph optimization method to update the pose of the robot corresponding to the t moments, so that k=k+1, t=0, and executing the specific content and method of the step (2) includes the following steps: (91) The observed error of the graph optimization process is determined, by equation (28), e k,z ＝z k -h(x k )≈0 (28) (92) Determining a process error of the motion process of the graph optimization process, determined by equation (29), e k,f ＝f k -g(x k ) (29) (93) Based on step (92), a graph optimization objective function is determined by equation (30), (94) Estimating the pose of the robot at the t moments through a graph optimization method based on the step (93) and the step (8) to obtain updated pose estimated values of the robot corresponding to the t moments, enabling t=0, and executing the step (2).
  10. 10. The method for simultaneous localization and mapping based on a bernoulli filter according to claim 1, wherein in the step (10), based on k in the step (8) and the step (9), judging whether the maximum running time number is reached, if yes, completing the last time of map optimization in the step (9), and outputting the state estimation values of the pose and the map features of the robot, and if not, executing the specific content and the method in the step (2) comprises the following steps: (101) Based on the step (8), judging whether k is more than or equal to k max or not, if yes, executing the step (9) once, and ending after finishing; (102) Based on step (101), when not satisfied, step (2) is performed.

Description

Simultaneous positioning and mapping method based on Bernoulli filter Technical Field The invention relates to a simultaneous localization and mapping method, in particular to a simultaneous localization and mapping method based on a Bernoulli filter. Background Navigation techniques are essential for mobile robots to work in unknown environments, while synchronous positioning and mapping (simultaneous positioning and mapping) are the main navigation techniques in recent years. The continuous improvement of the accuracy, speed, real-time performance and stability of navigation algorithms is always the focus of research. In some complex environments, such as underwater exploration environments, indoor firefighting rescue environments and the like, due to clutter concentration, the data association precision of the traditional simultaneous localization and mapping algorithm can be reduced, the calculated amount can be greatly increased, and the accuracy of the traditional simultaneous localization and mapping method is reduced. To solve the complex data association problem, one approach is to avoid the data association problem, and the other approach is to use more efficient algorithms, such as simultaneous mapping and mapping based on the graph. A multi-objective bayesian filter based on a Random Finite Set (RFS) avoids the problem of data correlation by averaging all possible correlations in the measurement update. RFS is a random variable of a set value, i.e. the number of state vectors and the state vectors themselves are random, so RFS naturally obtains map uncertainty. In subsequent studies, the application of RFS to the field of simultaneous localization and mapping, a hypothetical probability density filter-simultaneous localization and mapping (PHD-SLAM) algorithm was proposed. Assuming probability density filters-simultaneous localization and mapping algorithms are implemented by gaussian mixture under linear conditions and sequential monte carlo under nonlinear conditions, both methods use approximation strategies to calculate particle weights. Although data correlation is avoided, its algorithm estimation error is large. And because the traditional simultaneous localization and mapping method based on the RFS theory uses a particle filtering method in a robot pose estimation part, the problem that a large number of particles are required to approximate posterior distribution of the robot pose in a complex indoor environment, so that the calculated amount is large, the instantaneity is not high and the robot pose estimation precision is low is caused. Disclosure of Invention The invention aims to solve the problems of low pose estimation precision and poor real-time performance of a robot, and provides a potential equalization Bernoulli filtering simultaneous positioning and mapping method based on pose graph optimization. In order to achieve the above purpose, the invention adopts the following technical scheme: a simultaneous localization and mapping method based on a Bernoulli filter comprises the following steps: (1) Initializing a running time number parameter, an optimizing time number parameter, a multiple Bernoulli existence parameter and a maximum running time number parameter; (2) The motion speed v and the direction angle theta of the robot, the direct distance d between the map feature and the robot and the azimuth angle are obtained through sensors carried by the robot The sensor is an inertial navigation element or a laser radar; (3) Calculating and obtaining a predicted value of the robot pose at the kth moment through a motion equation f (v, theta, k) of the robot based on the motion speed v and the direction angle theta of the robot during operation obtained in the step (2); (4) Obtaining a predicted value of the pose of the robot at the kth moment based on the step (3), and based on the direct distance d and the azimuth angle of the step (2) By observing equationsObtaining an observation set corresponding to the kth moment of the robot; (5) Based on the information obtained in the step (3) and the step (4), carrying out state estimation on the map features by a potential equalization multiple Bernoulli filtering method to obtain Bernoulli items of the robot for representing the map features at the kth moment; (6) Performing target extraction on the Bernoulli term obtained in the step (5) according to the value of the existence probability parameter r, wherein the result is used as a state estimation value of map features, and the state estimation value comprises the number of the map features and the pose of the map features; (7) Recording the number of map features and the pose of the map features obtained at the moment k based on the state estimation value of the map features obtained in the step (6); (8) Judging whether the prior information meets a threshold value or not through a self-adaptive information control method based on the map feature state estimation value obtained