A simultaneous localization and mapping method based on multiple bernoulli filter

Abstract

The application discloses a simultaneous localization and mapping method based on multiple Bernoulli filters, and specifically comprises the following steps: (1) parameter initialization; (2) obtaining input data; (3) obtaining a robot position prediction value; (4) obtaining an observation set of the robot; (5) obtaining a Bernoulli item of the robot for representing a map feature at the kth moment through a potential balance multiple Bernoulli filtering method; (6) target extraction is performed on the obtained Bernoulli item; (7) the number of map features and the pose obtained at the kth moment are recorded; (8) whether a graph optimization process is executed is judged through an adaptive information control method; (9) the robot pose corresponding to the tth moment is obtained through a graph optimization method, and then step (2) is executed; (10) whether the robot pose and the state estimation value of the map feature are output is determined according to whether the maximum running moment number is reached. According to the method, the robot pose estimation precision in the simultaneous localization and mapping method is improved, and the real-time performance is improved.

Description

Technical Field

[0001] This invention relates to a method for simultaneous localization and mapping, and more particularly to a method for simultaneous localization and mapping based on a multi-Bernoulli filter. Background Technology

[0002] Navigation technology is essential for mobile robots operating in unknown environments, and simultaneous localization and mapping (SMR) has become a major navigation technology in recent years. Continuously improving the accuracy, speed, real-time performance, and stability of navigation algorithms has been a key research focus. In complex environments, such as underwater exploration and indoor fire rescue environments, dense clutter reduces the data association accuracy of traditional SMR algorithms, significantly increasing computational load and leading to a decline in accuracy. To address complex data association problems, one approach is to avoid them altogether; another is to use more efficient algorithms, such as graph-based SMR.

[0003] The multi-objective Bayesian filter based on the Random Finite Set (RFS) avoids data association problems by averaging all possible associations in the measurement update. Since the RFS is a random variable with randomized values, meaning both the number of state vectors and the state vectors themselves are random, RFS naturally introduces map uncertainty. Subsequent research applied RFS to the field of simultaneous localization and mapping (SLAM), proposing the Hypothetical Probability Density Filter-Simultaneous Localization and Mapping (PHD-SLAM) algorithm. This algorithm is implemented using Gaussian mixtures under linear conditions and sequential Monte Carlo methods under nonlinear conditions, but both methods use approximation strategies to calculate particle weights. While avoiding data association, the algorithm suffers from significant estimation errors. Furthermore, traditional SLAM methods based on RFS theory use particle filtering in robot pose estimation, leading to the need for a large number of particles to approximate the posterior distribution of the robot pose in complex indoor environments. This results in high computational cost, poor real-time performance, and low robot pose estimation accuracy. Summary of the Invention

[0004] The purpose of this invention is to solve the problems of low accuracy and poor real-time performance in robot pose estimation, and to provide a method for simultaneous localization and mapping based on pose graph optimization, using potential balance and multiple Bernoulli filtering.

[0005] To achieve the above objectives, the technical solution adopted by the present invention is as follows:

[0006] A simultaneous localization and mapping method based on a multi-Bernoulli filter includes the following steps:

[0007] (1) Initialize the parameters for the number of running times, the number of optimization times, the multi-Bernoulli existence parameter, and the maximum number of running times;

[0008] (2) Obtain the robot's motion speed v and direction angle θ, the direct distance d between the map features and the robot, and the azimuth angle through the sensors carried by the robot. The sensor is an inertial navigation element or a lidar;

[0009] (3) Based on the robot's motion speed v and direction angle θ obtained in step (2), the robot pose prediction value at time k is calculated using the robot's motion equation f(v,θ,k);

[0010] (4) Based on step (3), obtain the robot pose prediction value at time k, and the direct distance d and azimuth angle based on step (2). By observing the equation Obtain the observation set corresponding to the robot at time k;

[0011] (5) Based on the information obtained in steps (3) and (4), the state of the map features is estimated by the potential balance multi-Bernoulli filtering method to obtain the Bernoulli term used by the robot to represent the map features at time k.

[0012] (6) For the Bernoulli term obtained in step (5), target extraction is performed based on the value of its existence probability parameter r. The result is used as the state estimate of the map feature, which includes the number of map features and the pose of the map feature.

[0013] (7) Based on the state estimates of the map features obtained in step (6), record the number of map features and the pose of the map features at time k.

[0014] (8) Based on the map feature state estimate obtained in step (7), determine whether the prior information meets the threshold through the adaptive information control method. If it does not meet the threshold, execute step (2) and let k = k + 1 and t = t + 1. If it meets the threshold, execute step (9).

[0015] (9) Based on step (8), when the conditions of the adaptive information control method are met, the robot pose at time t is estimated by graph optimization method to update the robot pose at time t. Let k = k + 1, t = 0, and execute step (2).

[0016] (10) Based on k in steps (8) and (9), determine whether the maximum number of running time has been reached. If it is, complete the graph optimization process in the last step (9) and output the state estimates of the robot pose and map features. Otherwise, execute step (2).

[0017] Further optimization, step (1) specifically includes the following steps:

[0018] (11) Initialize the runtime parameter, let k = 0;

[0019] (12) Optimize the initialization of the time count parameter, let t = 0;

[0020] (13) Dobernoli existence parameter initialization, let r = 0.99;

[0021] (14) Initialize the maximum running time parameter, let k max =500.

[0022] Further preferred, in step (2), the robot's motion speed v and direction angle θ, the direct distance d between the map features and the robot, and the azimuth angle are obtained through the sensors carried by the robot. The methods and steps are:

[0023] (21) Obtain the robot's motion speed v and direction angle θ through the sensors carried by the robot;

[0024] (22) Obtain the distance d between the map features and the robot and the azimuth angle through the sensors carried by the robot.

[0025] Further preferably, the method and steps for predicting the robot's pose using the robot's motion equation f(v,θ,k) in step (3) are as follows:

[0026] (31) Take the pose at the initial time, i.e., k=0, as the origin;

[0027] (32) Establish a Cartesian coordinate system with the initial motion direction as the y-axis;

[0028] (33) The robot's motion equation f(v,θ,t) is determined by formula (1);

[0029]

[0030] Among them, v k Let θ represent the robot's speed at time k. k R represents the robot's forward angle at time k. f,k This refers to the process noise during robot operation.

[0031] Further preferred, the observation set obtained by the robot at time k in step (4) is derived from the observation equation. We obtain, i.e., formula (2):

[0032]

[0033] In the above formula, It is the robot's observation set. X represents the distance and angle between the map features observed by the sensor and the mobile robot itself. l and Y l These are the X-axis and Y-axis coordinates of the l-th map feature, R. z,k It is the observation noise covariance matrix.

[0034] Further preferred, the method and steps described in step (5) for estimating the state of map features using the potential balance multi-Bernoulli filtering method to obtain the Bernoulli term used by the robot to represent map features at time k are as follows:

[0035] (51) Based on the robot pose prediction value obtained in step (3), the map features observed by the robot at time k are described, and the form is represented by a random finite set. The random finite set model is obtained through formula (3).

[0036]

[0037] in, M represents a random finite set of map features of the robot from time 0 to k. k-1 Let X represent a random finite set of map features from time 0 to k-1, and M represent a random finite set of all map features; k This represents the robot's pose at time k. This represents the newborn map features of the robot at time k;

[0038] (52) Based on the observation set of the robot on the map features at time k obtained in step (4), it is represented in the form of a random finite set, and its random finite set model is obtained by formula (4).

[0039]

[0040] Among them, set Z k Let X represent the robot's observation set at time k. k For the robot pose, D k (m,X k ) indicates that the robot is in X k C is a true observation of map features. k (X k ) indicates that the robot is in pose X k The observed clutter error observation set;

[0041] (53) Using the conditional Bayes formula, we construct the simultaneous localization and mapping problem, i.e., the robot is in the observation set Z. kEstimating map features M under certain conditions k And robot pose X 1:k The process of obtaining the joint posterior probability density; the simultaneous localization and mapping problem is represented by formula (5);

[0042] π k|k (M k ,X 1:k |Z 1:k ,u 1:k ,X0) (5)

[0043] Among them, set Z 1:k Let X represent the robot's observation set from time 1 to time k. 1:k Let M be the robot's pose from time 1 to time k. k X represents the map features around the robot at time k, and X0 represents the pose at the initial time.

[0044] (54) For convenience, the decomposition form of formula (4) is obtained by using the conditional probability formula and factorization, namely formula (6).

[0045] π 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 ,X0)π k (M k |Z 0:k ,X 0:k (6)

[0046] Where, π k (X 1:k |Z 0:k ,u 0:k-1 X0) represents the joint posterior estimate of the robot's observations at time k, control parameters at time k-1, and initial pose; π k (M k |Z 0:k ,X 0:k ) indicates that the robot is in pose X 0:k When the observation set Z is obtained 0:k Joint posterior estimation of map features under the given conditions;

[0047] (55) Obtain M through step (7) k-t:k ;

[0048] (56) Based on step (55), the form of the multi-Bernoulli random finite set of the features of the newly formed map observed by the robot at time k can be obtained by formula (7);

[0049]

[0050] in, The probability parameter representing the existence of a Dobernuli random finite set that describes the features of a newly generated map. M represents the existence probability density parameter of a multi-Bernoulli random finite set representing the features of a newly generated map. k-t:k This represents the Gaussian term information closest to the respective pose among the Gaussian terms obtained by filtering all robot platforms; the newly generated map feature b(m|X) at this time k The map features include not only the map features observed by the robot itself at time k-1, but also the state estimates of the map features obtained t times before time k, which are added as part of the new map feature set.

[0051] (57) Based on step (56), after obtaining the multi-Bernoulli random finite set form of the new map features, the Gaussian mixture implementation of its probability density parameter p is obtained through formula (8).

[0052]

[0053] in, This represents the number of Gaussian terms corresponding to the Bernoulli terms of the newly generated map features at time k. This represents the weight of the Gaussian term corresponding to the Bernoulli term of the newly generated map features at time k. Let represent the mean of the Gaussian terms corresponding to the Bernoulli terms of the features of the newly formed map at time k. The covariance matrix of the Gaussian terms corresponding to the Bernoulli terms of the features of the newly generated map at time k;

[0054] (58) Determine the multi-Bernoulli random finite set representation of the prior map features obtained by the robot at time k-1, which includes the existence probability parameter r and the probability density parameter p, which can be obtained by formula (9);

[0055]

[0056] in, Represents the Lth time obtained at time k-1. k-1 The existence probability parameter of a multi-Bernoulli random finite set of prior map features. Represents the Lth time obtained at time k-1. k-1 The existence probability density parameter of a multi-Bernoulli random finite set of prior map features;

[0057] (59) Based on step (58) It is determined by formula (10);

[0058]

[0059] in, These represent the map feature weights, mean, and covariance at time k-1, respectively. The number of Gaussian terms;

[0060] (510) Based on the multi-Bernoulli random finite set form of the new map features in step (57) and the multi-Bernoulli random finite set form of the map features at time k-1 in step (59), establish the multi-Bernoulli random finite set form of the predicted value of the map feature state estimate obtained by the robot at time k, and obtain it through formula (11).

[0061]

[0062] in, The initial value is 0.99;

[0063] The predicted value of the existence probability parameter r of the multiple Bernoulli term is determined by formula (12);

[0064]

[0065] Where p S,k The probability of survival is 0.95;

[0066] The predicted value of the probability density parameter p of the multi-Bernoulli term is expressed in Gaussian mixture form and is determined by formula (13);

[0067]

[0068] The predicted value of the mean of the Gaussian term is determined by formula (14);

[0069]

[0070] The predicted value of the covariance matrix of the Gaussian term can be determined by formula (15);

[0071]

[0072] The initial weight of the Gaussian term is 0.1;

[0073] (511) Based on the predicted value of the map feature state estimate in (510), the updated value of the map feature state estimate is obtained. Its multi-Bernoulli random finite set form is determined by formula (16), which includes the multi-Bernoulli random finite set of the missed part of the map feature and the multi-Bernoulli random finite set of the updated part of the map feature obtained from the observation.

[0074]

[0075] The Dobernoli probability density parameter is obtained through Gaussian mixture and is determined by formula (17).

[0076]

[0077] Where, p D,k For detection probability, The weight of the j-th Gaussian term of the l-th Bernoulli term.

[0078] The weights, mean, and covariance matrix of the Gaussian term corresponding to the p-parameter of each Bernoulli term are obtained by extended Kalman filtering. These are then obtained using formulas (18)-(21).

[0079]

[0080]

[0081]

[0082]

[0083] The updated values ​​of the robot's Dobernuli parameters are determined by formulas (22)-(25).

[0084]

[0085]

[0086]

[0087]

[0088] The map feature pose is updated according to the above formula, that is, the parameters r and p are updated.

[0089] Further preferred, the method and steps for target extraction based on the existence probability parameter r of the Bernoulli term obtained in step (5) as described in step (6) are as follows:

[0090] (61) Set an existence probability threshold T r =10 -3After each update, Bernoulli terms with a probability less than the threshold value are removed.

[0091] (62) Sum the existence probability parameters of the surviving Bernoulli terms from step (61) and take the integer part. The result is the number of map features, denoted as N. map_k ;

[0092] (63) Set a weight threshold T p =10 -5 , trim the Gaussian components of the remaining Bernoulli terms in (61) and trim away the Gaussian components with weights less than the threshold.

[0093] (64) Based on the pruning results of step (63), set a Gaussian term distance threshold T. m The positions that are less than the threshold T m =1 meter Gaussian terms are combined into one Gaussian component;

[0094] (65) Based on steps (61) and (64), extract the Gaussian term with the largest weight among the Gaussian terms corresponding to the surviving Bernoulli terms obtained by the robot at time k.

[0095] (66) The mean of the Gaussian terms obtained in step (65) is used as the state estimate of the map features corresponding to the surviving Bernoulli terms.

[0096] Further optimization involves step (7) recording the number and pose of map features obtained at time k based on the state estimates of the map features obtained in step (6).

[0097] Further preferred, in step (8), based on the map feature state estimate obtained in step (7), the adaptive information control method is used to determine whether the prior information meets the threshold. If it does not meet the threshold, step (2) is executed, letting k = k + 1 and t = t + 1; if it meets the threshold, step (9) is executed. The specific content and method of this step include the following steps:

[0098] (81) Set the distance filtering threshold, which is determined by formula (26).

[0099] d T =R+v×dt (26)

[0100] (82) Based on the estimated values ​​of map features obtained in step (7) and the distance threshold obtained in step (81), a priori information filtering threshold is set and determined by formula (27).

[0101]

[0102] Where, n z (X i ) indicates that the robot's pose is Xi And the observation range is d T The number of map features observed at that time, N f It represents the number of edges composed of motion information, and A is the information control value;

[0103] (83) Based on step (82), determine A≤A T If the conditions are not met, proceed to step (2); if the conditions are met, proceed to step (8). Let k = k + 1 and t = t + 1.

[0104] Further optimization, in step (9), if the conditions of the adaptive information control method are met based on step (8), the robot pose at time t is estimated by graph optimization method to update the robot pose at time t. Let k = k + 1, t = 0, the specific content and method of executing step (2) include the following steps:

[0105] (91) The observation error of the graph optimization process can be determined by formula (28).

[0106] e k,z =z k -h(x k )≈0 (28)

[0107] (92) The process error of the motion process in the graph optimization process is determined by formula (29).

[0108] e k,f =f k -g(x k (29)

[0109] (93) Based on (92), the objective function for graph optimization is determined by formula (30).

[0110]

[0111] (94) Based on (93) and step (8), estimate the robot pose at time t using graph optimization to obtain the updated robot pose estimate corresponding to time t. Let t = 0 and execute step (2).

[0112] Further optimization involves determining whether the maximum number of running times has been reached based on k from steps (8) and (9) in step (10). If the maximum number of running times has been reached, the final graph optimization in step (9) is completed, and the state estimates of the robot pose and map features are output before the process ends. Otherwise, the specific content and method of step (2) include the following steps:

[0113] (101) Based on (8), determine k≥k maxIf the condition is met, then execute step (9) once, and then end the process.

[0114] (102) Based on (101), if not satisfied, proceed to step (2).

[0115] Beneficial effects: Compared with the prior art, the present invention has the following significant advantages:

[0116] (1) A bit-and mapping method based on multi-Bernoulli filter is proposed to improve the low accuracy of the traditional simultaneous localization and mapping method based on RFS. Under the same experimental conditions, it can improve the robot pose estimation accuracy by 63.1% compared with the traditional simultaneous localization and mapping method based on RFS.

[0117] (2) An adaptive information control method (AIC) is proposed, which improves the real-time performance of the traditional RFS-based simultaneous localization and mapping method. Under the same experimental conditions, it can improve the robot's real-time performance by 17.4% compared with the traditional RFS-based simultaneous localization and mapping method. Attached Figure Description

[0118] Figure 1 This is a flowchart of the simultaneous localization and mapping method based on a multi-Bernoulli filter according to the present invention. Detailed Implementation

[0119] The technical solution of the present invention will be further described in detail below with reference to the accompanying drawings.

[0120] Depend on Figure 1 As shown, the present invention provides a method for simultaneous localization and mapping of potential-equalized multi-Bernoulli filters based on pose graph optimization, comprising the following steps:

[0121] (1) Specific content and steps for initializing the running time parameter, optimization time parameter, multi-Bernoulli existence parameter, and maximum running time parameter:

[0122] (11) Initialize the runtime parameter by setting k = 0.

[0123] (12) Optimize the initialization of the time parameter by setting t = 0.

[0124] (13) The Dobernoli existence parameter is initialized by setting r = 0.99.

[0125] (14) Initialize the maximum running time parameter, let k max =500.

[0126] (2) Obtain the robot's motion speed v and direction angle θ, the direct distance d between the map features and the robot, and the azimuth angle through the sensors carried by the robot (such as inertial navigation elements, lidar, etc.). Specific content and steps:

[0127] (21) Obtain the robot's motion speed v and direction angle θ through the sensors carried by the robot.

[0128] (22) Obtain the direct distance d between the map features and the robot and the azimuth angle through the sensors carried by the robot.

[0129] (3) Predicting the robot's pose using the robot's motion equation f(v,θ,k), where v is the robot's velocity, θ is the robot's direction angle, and k is the running time. Specific details and steps:

[0130] (31) Take the pose at the initial time, i.e., k=0, as the origin.

[0131] (32) Establish a Cartesian coordinate system with the initial direction of motion as the y-axis.

[0132] (33) The robot's motion equation f(v,θ,t) is determined by formula (1).

[0133]

[0134] Among them, v k Let θ represent the robot's speed at time k. k R represents the robot's forward angle at time k. f,k This refers to the process noise during robot operation.

[0135] (4) Based on step (3), obtain the robot pose prediction value at time k, and the direct distance d and azimuth angle based on step (2). By observing the equation Obtain the observation set acquired by the robot at time k.

[0136] Observation equations Formula (2) determines:

[0137]

[0138] In the above formula, It is the robot's observation set. X represents the distance and angle between the map features observed by the sensor and the mobile robot itself. l and Y l These are the X-axis and Y-axis coordinates of the l-th map feature, R. z,k It is the observation noise covariance matrix.

[0139] (5) Based on the information obtained in steps (3) and (4), the map features are estimated using the potential balance multi-Bernoulli filtering method to obtain the specific content and steps of the Bernoulli term used by the robot to represent the map features at time k:

[0140] (51) Based on the robot pose prediction value obtained in step (3), the map features observed by the robot at time k are described, and the form is represented by a random finite set. The random finite set model is obtained by formula (3).

[0141]

[0142] in, M represents a random finite set of map features of the robot from time 0 to k. k-1 Let X represent a random finite set of map features from time 0 to k-1, and M represent a random finite set of all map features; k This represents the robot's pose at time k. This represents the newborn map features of the robot at time k;

[0143] (52) Based on the observation set of the robot on the map features at time k obtained in step (4), it is represented in the form of a random finite set, and its random finite set model is obtained by formula (4).

[0144]

[0145] Among them, set Z k Let X represent the robot's observation set at time k. k For the robot pose, D k (m,X k ) indicates that the robot is in X k C is a true observation of map features. k (X k ) indicates that the robot is in pose X k The observed clutter error set.

[0146] (53) Using the conditional Bayes formula, we construct the simultaneous localization and mapping problem, i.e., the robot is in the observation set Z. k Estimating map features M under certain conditions k And robot pose X 1:k The process of obtaining the joint posterior probability density. This simultaneous localization and mapping problem is represented by formula (5).

[0147] π k|k (M k ,X 1:k |Z 1:k ,u 1:k,X0) (5)

[0148] Among them, set Z 1:k Let X represent the robot's observation set from time 1 to time k. 1:k Let M be the robot's pose from time 1 to time k. k X represents the map features around the robot at time k, and X0 represents the pose at the initial time.

[0149] (54) For convenience, the decomposition form of formula (4) is obtained by using the conditional probability formula and factorization, which is formula (6).

[0150] π 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 ,X0)π k (M k |Z 0:k ,X 0:k (6)

[0151] Where, π k (X 1:k |Z 0:k ,u 0:k-1 X0) represents the joint posterior estimate of the robot's observations at time k, control parameters at time k-1, and initial pose; π k (M k |Z 0:k ,X 0:k ) indicates that the robot is in pose X 0:k When the observation set Z is obtained 0:k Joint posterior estimation of map features under given conditions.

[0152] (55) Obtain M through step (7) k-t:k .

[0153] (56) Based on step (55), the form of the multi-Bernoulli random finite set of the features of the new map observed by the robot at time k can be obtained by formula (7).

[0154]

[0155] in, The probability parameter representing the existence of a Dobernuli random finite set that describes the features of a newly generated map. M represents the existence probability density parameter of a multi-Bernoulli random finite set representing the features of a newly generated map.k-t:k This represents the Gaussian term information closest to the respective pose among the Gaussian terms obtained by filtering all robot platforms; the newly generated map feature b(m|X) at this time k The map features include not only the map features observed by the robot itself at time k-1, but also the state estimates of the map features obtained t times before time k, which are added as part of the new map feature set.

[0156] (57) Based on step (56), after obtaining the multi-Bernoulli random finite set form of the new map features, the Gaussian mixture implementation of its probability density parameter p is obtained through formula (8).

[0157]

[0158] in, This represents the number of Gaussian terms corresponding to the Bernoulli terms of the newly generated map features at time k. This represents the weight of the Gaussian term corresponding to the Bernoulli term of the newly generated map features at time k. Let represent the mean of the Gaussian terms corresponding to the Bernoulli terms of the features of the newly formed map at time k. Let represent the covariance matrix of the Gaussian term corresponding to the Bernoulli term of the newly generated map feature at time k.

[0159] (58) Determine the multi-Bernoulli random finite set representation of the prior map features obtained by the robot at time k-1, which includes the existence probability parameter r and the probability density parameter p, and can be obtained by formula (9).

[0160]

[0161] in, Represents the Lth time obtained at time k-1. k-1 The existence probability parameter of a multi-Bernoulli random finite set of prior map features. Represents the Lth time obtained at time k-1. k-1 The existence probability density parameter of a multi-Bernoulli random finite set of prior map features.

[0162] (59) Based on step (58) It is determined by formula (10).

[0163]

[0164] in, These represent the map feature weights, mean, and covariance at time k-1, respectively. The number of Gaussian terms.

[0165] (510) Based on the multi-Bernoulli random finite set form of the new map features in step (57) and the multi-Bernoulli random finite set form of the map features at time k-1 in step (59), the multi-Bernoulli random finite set form of the predicted value of the map feature state estimate obtained by the robot at time k is established and obtained by formula (11).

[0166]

[0167] in, The initial value is 0.99.

[0168] The predicted value of the probability parameter r of the existence of the multiple Bernoulli term is determined by formula (12).

[0169]

[0170] Where p S,k The probability of survival is 0.95.

[0171] The predicted value of the probability density parameter p of the multi-Bernoulli term is expressed in Gaussian mixture form and is determined by formula (13).

[0172]

[0173] The predicted value of the mean of the Gaussian term is determined by formula (14).

[0174]

[0175] The predicted value of the covariance matrix of the Gaussian term can be determined by formula (15).

[0176]

[0177] The initial value of the Gaussian term is 0.1.

[0178] (511) Based on the predicted value of the map feature state estimate in step (510), the updated value of the map feature state estimate is obtained. Its multi-Bernoulli random finite set form is determined by formula (16), which includes the multi-Bernoulli random finite set of the missed part of the map feature and the multi-Bernoulli random finite set of the updated part of the map feature obtained from the observation.

[0179]

[0180] The Dobernoli probability density parameter is obtained through Gaussian mixture and is determined by formula (17).

[0181]

[0182] Where, p D,k For detection probability, The weight of the j-th Gaussian term of the l-th Bernoulli term.

[0183] The weights, mean, and covariance matrix of the Gaussian term corresponding to the p-parameter of each Bernoulli term are obtained by extended Kalman filtering. These are then obtained using formulas (18)-(21).

[0184]

[0185]

[0186]

[0187]

[0188] The updated values ​​of the robot's Dobernuli parameters are determined by formulas (22)-(25).

[0189]

[0190]

[0191]

[0192]

[0193] The map feature pose is updated according to the above formula, that is, the parameters r and p are updated.

[0194] (6) The method and steps for extracting the target based on the value of the existence probability parameter r of the Bernoulli term obtained in step (5) are as follows:

[0195] (61) Set an existence probability threshold T r =10 -3 After each update, Bernoulli terms with a probability less than the threshold value are removed.

[0196] (62) Sum the existence probability parameters of the surviving Bernoulli terms from step (61) and take the integer part. The result is the number of map features, denoted as N. map_k .

[0197] (63) Set a weight threshold T p =10 -5 The Gaussian components of the remaining Bernoulli terms in step (61) are trimmed, and the Gaussian components with weights less than the threshold are trimmed.

[0198] (64) Based on the pruning results of step (63), set a Gaussian term distance threshold T. m The positions that are less than the threshold T m=1 meter Gaussian terms are combined into one Gaussian component.

[0199] (65) Based on steps (61) and (64), extract the Gaussian term with the largest weight among the surviving Bernoulli terms obtained by the robot at time k.

[0200] (66) The mean of the Gaussian terms obtained in step (65) is used as the state estimate of the map features corresponding to the surviving Bernoulli terms.

[0201] (7) Based on the state estimate of the map features obtained in step (6), record the number of map features and pose at time k, and use this information for the adaptive information control method in step (8).

[0202] (8) Based on the map feature state estimate obtained in step (7), determine whether the prior information meets the threshold using the adaptive information control method. If not, proceed to step (2), setting k = k + 1 and t = t + 1; if it meets the threshold, proceed to step (9). This includes the following steps:

[0203] (81) Set the distance filtering threshold, which is determined by formula (26).

[0204] d T =R+v×dt (26)

[0205] (82) Based on the estimated values ​​of map features obtained in step (7) and the distance threshold obtained in step (81), a priori information filtering threshold is set and determined by formula (27).

[0206]

[0207] Where, n z (X i ) indicates that the robot's pose is X i And the observation range is d T The number of map features observed at that time, N f Let A represent the number of edges formed by motion information, and let A be the information control value.

[0208] (83) Based on step (82), determine A≤A T If the conditions are not met, proceed to step (2); if the conditions are met, proceed to step (8). Let k = k + 1 and t = t + 1.

[0209] (9) Based on step (8), if the conditions of the adaptive information control method are met, the robot pose at time t is estimated using the graph optimization method to update the robot pose at time t. Let k = k + 1, t = 0, and execute step (2). This includes the following steps:

[0210] (91) The observation error of the graph optimization process can be determined by formula (28).

[0211] e k,z =z k -h(x k )≈0 (28)

[0212] (92) The process error of the motion process in the graph optimization process can be determined by formula (29).

[0213] e k,f =f k -g(x k (29)

[0214] (93) Based on step (92), the objective function for graph optimization can be determined by formula (30).

[0215]

[0216] (94) Based on steps (93) and (8), estimate the robot pose at time t using graph optimization to obtain the updated robot pose estimate at time t. Let t = 0 and execute step (2).

[0217] (10) Based on k from steps (8) and (9), determine whether the maximum number of running times has been reached. If so, complete the final graph optimization in step (9), output the state estimates of the robot pose and map features, and then end the process. Otherwise, execute step (2). This includes the following steps:

[0218] (101) Based on step (8), determine k≥k max If the condition is met, then execute step (9) once, and then end the process.

[0219] (102) If step (101) is not satisfied, then step (2) is executed.

Claims

1. A method for simultaneous localization and mapping based on a multi-Bernoulli filter, characterized in that: Includes the following steps: (1) Initialize the parameters for the number of running times, the number of optimization times, the multi-Bernoulli existence parameter, and the maximum number of running times; (2) Obtain the robot's motion speed v and direction angle θ, the direct distance d between the map features and the robot, and the azimuth angle through the sensors carried by the robot. The sensor is an inertial navigation element or a lidar; (3) Based on the robot's motion speed v and direction angle θ obtained in step (2), the robot pose prediction value at time k is calculated using the robot's motion equation f(v,θ,k); (4) Based on step (3), obtain the robot pose prediction value at time k, and the direct distance d and azimuth angle based on step (2). By observing the equation Obtain the observation set corresponding to the robot at time k; (5) Based on the information obtained in steps (3) and (4), the state of the map features is estimated by the potential balance multi-Bernoulli filtering method to obtain the Bernoulli term used by the robot to represent the map features at time k. (6) For the Bernoulli term obtained in step (5), target extraction is performed based on the value of its existence probability parameter r. The result is used as the state estimate of the map feature, which includes the number of map features and the pose of the map feature. (7) Based on the state estimates of the map features obtained in step (6), record the number of map features and the pose of the map features at time k. (8) Based on the map feature state estimate obtained in step (7), determine whether the prior information meets the threshold through the adaptive information control method. If it does not meet the threshold, execute step (2) and let k = k + 1 and t = t + 1. If it meets the threshold, execute step (9). (9) Based on step (8), when the conditions of the adaptive information control method are met, the robot pose at time t is estimated by graph optimization method to update the robot pose at time t. Let k = k + 1, t = 0, and execute step (2). (10) Based on k in steps (8) and (9), determine whether the maximum number of running time has been reached. If it is, complete the graph optimization process in the last step (9) and output the state estimates of the robot pose and map features. Otherwise, execute step (2).

2. The simultaneous localization and mapping method based on a multi-Bernoulli filter according to claim 1, characterized in that: The method and steps for initializing the running time parameter, optimization time parameter, multi-Bernoulli existence parameter, and maximum running time parameter as described in step (1) are as follows: (11) Initialize the runtime parameter, let k = 0; (12) Optimize the initialization of the time count parameter, let t = 0; (13) Dobernoli existence parameter initialization, let r = 0.99; (14) Initialize the maximum running time parameter, let k max =500.

3. The simultaneous localization and mapping method based on a multi-Bernoulli filter according to claim 1, characterized in that: In step (2), the robot's motion speed v and direction angle θ, the direct distance d between the map features and the robot, and the azimuth angle are obtained through the sensors carried by the robot. The methods and steps are: (21) Obtain the robot's motion speed v and direction angle θ through the sensors carried by the robot; (22) Obtain the distance d between the map features and the robot and the azimuth angle through the sensors carried by the robot.

4. The simultaneous localization and mapping method based on a multi-Bernoulli filter according to claim 1, characterized in that: The method and steps for predicting the robot's pose using the robot's motion equation f(v,θ,k) as described in step (3) are as follows: (31) Take the pose at the initial time, i.e., k=0, as the origin; (32) Establish a Cartesian coordinate system with the initial motion direction as the y-axis; (33) The robot's motion equation f(v,θ,t) is determined by formula (1); Among them, v k Let θ represent the robot's speed at time k. k R represents the robot's forward angle at time k. f,k This refers to the process noise during robot operation.

5. The simultaneous localization and mapping method based on a multi-Bernoulli filter according to claim 1, characterized in that: The step (4) described in the observation equation The method for obtaining the observation set acquired by the robot at time k is... Observation equations Determined by formula (2): In the above formula, It is the robot's observation set, d k , X represents the distance and angle between the map features observed by the sensor and the mobile robot itself. l and Y l These are the X-axis and Y-axis coordinates of the l-th map feature, R. z,k It is the observation noise covariance matrix.

6. The simultaneous localization and mapping method based on a multi-Bernoulli filter according to claim 1, characterized in that: In step (5), the potential balance multi-Bernoulli filtering method is used to estimate the state of the map features, and the Bernoulli term used by the robot to represent the map features at time k is obtained. The methods and steps are: (51) Based on the robot pose prediction value obtained in step (3), the map features observed by the robot at time k are described, and the form is represented by a random finite set. The random finite set model is obtained through formula (3). in, M represents a random finite set of map features of the robot from time 0 to k. k-1 Let X represent a random finite set of map features from time 0 to k-1, and M represent a random finite set of all map features; k This represents the robot's pose at time k. This represents the newborn map features of the robot at time k; (52) Based on the observation set of the robot on the map features at time k obtained in step (4), it is represented in the form of a random finite set, and its random finite set model is obtained by formula (4). Among them, set Z k Let X represent the robot's observation set at time k. k For the robot pose, D k (m,X k ) indicates that the robot is in X k C is a true observation of map features. k (X k ) indicates that the robot is in pose X k The observed clutter error observation set; (53) Using the conditional Bayes formula, we construct the simultaneous localization and mapping problem, i.e., the robot is in the observation set Z. k Estimating map features M under certain conditions k And robot pose X 1:k The process of obtaining the joint posterior probability density; the simultaneous localization and mapping problem is represented by formula (5); π k|k (M k ,X 1:k |Z 1:k ,u 1:k ,X0) (5) Among them, Z 1:k Let X represent the robot's observation set from time 1 to time k. 1:k Let M be the robot's pose from time 1 to time k. k X represents the map features around the robot at time k, and X0 represents the pose at the initial time. (54) For convenience, the decomposition form of formula (4) is obtained by using the conditional probability formula and factorization, namely formula (6). π 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 ,X0)π k (M k |Z 0:k ,X 0:k ) (6) Where, π k (X 1:k |Z 0:k ,u 0:k-1 X0) represents the joint posterior estimate of the robot's observations at time k, control parameters at time k-1, and initial pose; π k (M k |Z 0:k ,X 0:k ) indicates that the robot is in pose X 0:k When the observation set Z is obtained 0:k Joint posterior estimation of map features under the given conditions; (55) Obtain M through step (7) k-t:k ; (56) Based on step (55), the form of the multi-Bernoulli random finite set of the features of the newly formed map observed by the robot at time k can be obtained by formula (7); in, The probability parameter representing the existence of a Dobernuli random finite set that describes the features of a newly generated map. M represents the existence probability density parameter of a multi-Bernoulli random finite set representing the features of a newly generated map. k-t:k This represents the Gaussian term information closest to the respective pose among the Gaussian terms obtained by filtering all robot platforms; the newly generated map feature b(m|X) at this time k The map features include not only the map features observed by the robot itself at time k-1, but also the state estimates of the map features obtained t times before time k, which are added as part of the new map feature set. (57) Based on step (56), after obtaining the multi-Bernoulli random finite set form of the new map features, the Gaussian mixture implementation of its probability density parameter p is obtained through formula (8). in, This represents the number of Gaussian terms corresponding to the Bernoulli terms of the newly generated map features at time k. This represents the weight of the Gaussian term corresponding to the Bernoulli term of the newly generated map features at time k. Let represent the mean of the Gaussian terms corresponding to the Bernoulli terms of the features of the newly formed map at time k. The covariance matrix of the Gaussian terms corresponding to the Bernoulli terms of the features of the newly generated map at time k; (58) Determine the multi-Bernoulli random finite set representation of the prior map features obtained by the robot at time k-1, which includes the existence probability parameter r and the probability density parameter p, which can be obtained by formula (9); in, Represents the Lth time obtained at time k-1. k-1 The existence probability parameter of a multi-Bernoulli random finite set of prior map features. Represents the Lth time obtained at time k-1. k-1 The existence probability density parameter of a multi-Bernoulli random finite set of prior map features; (59) Based on step (58) It is determined by formula (10); in, These represent the map feature weights, mean, and covariance at time k-1, respectively. The number of Gaussian terms; (510) Based on the multi-Bernoulli random finite set form of the new map features in step (57) and the multi-Bernoulli random finite set form of the surviving map features in step (59), establish the multi-Bernoulli random finite set form of the predicted value of the map feature state estimate obtained by the robot at time k, and obtain it through formula (11). in, The initial value is 0.99; The predicted value of the existence probability parameter r of the multiple Bernoulli term is determined by formula (12); Where p S,k The probability of survival is 0.95; The predicted value of the probability density parameter p of the multi-Bernoulli term is expressed in Gaussian mixture form and is determined by formula (13); The predicted value of the mean of the Gaussian term is determined by formula (14); The predicted value of the covariance matrix of the Gaussian term can be determined by formula (15); The initial weight of the Gaussian term is 0.1; (511) Based on the predicted value of the map feature state estimate in step (510), the updated value of the map feature state estimate is obtained. Its multi-Bernoulli random finite set form is determined by formula (16), which includes the multi-Bernoulli random finite set of the missed part of the map feature and the multi-Bernoulli random finite set of the updated part of the map feature obtained from the observation. The Dobernouri probability density parameter is obtained by Gaussian mixture and is determined by formula (17); Where, p D,k For detection probability, The weight of the j-th Gaussian term of the l-th Bernoulli term; The weights, mean, and covariance matrix of the Gaussian terms corresponding to the p-parameters of each Bernoulli term are obtained by extended Kalman filtering and are then obtained by formulas (18)-(21). The updated values ​​of the robot's Dobernuli parameters are determined by formulas (22)-(25); The map feature pose is updated according to the above formula, that is, the parameters r and p are updated.

7. The simultaneous localization and mapping method based on a multi-Bernoulli filter according to claim 1, characterized in that: The method and steps for target extraction based on the existence probability parameter r of the Bernoulli term obtained in step (5), as described in step (6), are as follows: (61) Set an existence probability threshold T r =10 -3 After each update, Bernoulli terms with a probability less than the threshold value are removed. (62) Sum the existence probability parameters of the surviving Bernoulli terms from step (61) and take the integer part. The result is the number of map features, denoted as N. map_k ; (63) Set a weight threshold T p =10 -5 The Gaussian components of the remaining Bernoulli terms in step (61) are trimmed, and the Gaussian components with weights less than the threshold are trimmed. (64) Based on the pruning results of step (63), set a Gaussian term distance threshold T. m The positions that are less than the threshold T m =1 meter Gaussian terms are combined into one Gaussian component; (65) Based on steps (61) and (64), extract the Gaussian term with the largest weight among the Gaussian terms corresponding to the surviving Bernoulli terms obtained by the robot at time k. (66) The mean of the Gaussian terms obtained in step (65) is used as the state estimate of the map features corresponding to the surviving Bernoulli terms.

8. The simultaneous localization and mapping method based on a multi-Bernoulli filter according to claim 1, characterized in that: In step (8), based on the map feature state estimate obtained in step (7), the adaptive information control method is used to determine whether the prior information meets the threshold. If it does not meet the threshold, step (2) is executed, setting k = k + 1 and t = t + 1. If it meets the threshold, step (9) is executed. The specific content and method of this step include the following steps: (81) Set the distance filtering threshold, which is determined by formula (26). d T =R+v×dt (26) (82) Based on the estimated values ​​of map features obtained in step (7) and the distance threshold obtained in step (81), a priori information filtering threshold is set and determined by formula (27). Where, n z (X i ) indicates that the robot's pose is X i And the observation range is d T The number of map features observed at that time, N f It represents the number of edges composed of motion information, and A is the information control value; (83) Based on step (82), determine A≤A T If the conditions are not met, proceed to step (2); if the conditions are met, proceed to step (8). Let k = k + 1 and t = t + 1.

9. The simultaneous localization and mapping method based on a multi-Bernoulli filter according to claim 1, characterized in that: In step (9), if the conditions of the adaptive information control method are met based on step (8), the robot pose at time t is estimated by graph optimization method to update the robot pose at time t. Let k = k + 1, t = 0. The specific content and method of executing step (2) include the following steps: (91) Determine the observation error of the graph optimization process using formula (28). e k,z =z k -h(x k )≈0 (28) (92) The process error of the motion process in the graph optimization process is determined by formula (29). e k,f =f k -g(x k ) (29) (93) Based on step (92), the objective function for graph optimization is determined by formula (30). (94) Based on steps (93) and (8), estimate the robot pose at time t using graph optimization to obtain the updated robot pose estimate at time t. Let t = 0 and execute step (2).

10. The simultaneous localization and mapping method based on a multi-Bernoulli filter according to claim 1, characterized in that: In step (10), based on k from steps (8) and (9), it is determined whether the maximum number of running times has been reached. If so, the final graph optimization in step (9) is completed, and the state estimates of the robot pose and map features are output, and the process ends. Otherwise, the specific content and method of step (2) include the following steps: (101) Based on step (8), determine k≥k max If the condition is met, then step (9) is executed once, and the process ends after completion. (102) Based on step (101), if the condition is not met, then step (2) is executed.

Images