CN104597900A - Electromagnetism-like mechanism optimization based FastSLAM method - Google Patents

Abstract

Provided is an electromagnetism-like mechanism optimization based FastSLAM method. According to the electromagnetism-like mechanism optimization based FastSLAM method, when a single robot executes a FastSLAM task, an electromagnetism-like mechanism optimization thought is introduced in the re-sampling process, an electromagnetism-like mechanism optimization based FastSLAM algorithm is established, a sampling particle is regarded as an electron, particle distribution is optimized and particle quality is improved according to the principle that particles with large weights attract particles with small weights and the particles with small weights repel the particles with large weights, and accordingly the problems of particle degeneracy and particle diversity insufficiency of a traditional FastSLAM method are solved. By means of the method, the attraction and repelling mechanism of charged particles in an electromagnetic field is simulated to improve particle distribution, each robot observing the same road sign is regarded as a node when multiple robots observe a common road sign, a Kalman consistency algorithm is utilized to correct estimated values of the robots to the road sign, and accordingly the positioning and image composition accuracy of the robots is further improved. The electromagnetism-like mechanism optimization based FastSLAM method can improve the estimation accuracy of self positioning and map establishment of the robot in an unknown environment.

Description

A kind of FastSLAM method optimized based on electromagnetism-like mechanism

Technical field

The present invention relates to a kind of FastSLAM method optimized based on electromagnetism-like mechanism, belong to the navigation of intelligent robot movable independent and automatic control technology field.

Background technology

The application level of mobile robot technology and research degree represent the height of a National Industrial automation development, have important strategic importance to national defence, society, science and technology.Independent navigation is difficult point and the focus of mobile robot's area research always, and navigation problem can be summarized as three basic problems: and 1) " Wheream I? (I am where ?) ", this is robot self-localization problem; 2) " Where do I want to go? (where I think ?) ", this is robot task planning problem; 3) " How do I get there? (how I arrive that ?) " this is robot path planning's problem, in the middle of current location and target location, namely select a path that is suitable, that optimize, to guarantee that mobile robot can arrive at target location safely smoothly.Wherein robot self-localization problem is the most important, is to solve the precondition of latter two problems, because mobile robot has the position known in external environment residing for self only, it is just more meaningful to answer two problems below.Mobile robot's simultaneous localization and mapping (SLAM) refers to and robot is placed in completely unknown environment, observed by the sensors towards ambient of himself configuration, the continuous map of creation environment of increment type, uses existing cartographic information synchronously to estimate self-position simultaneously.SLAM versatility answers " I where? " this problem, is generally considered the key link that mobile robot realizes real independent navigation, and the target mankind being realized to complete autonomous mobile robot has very important theory and using value.

Owing to being controlled information and sensors observe noise in robot kinematics, the general probabilistic method that adopts solves SLAM problem.Wherein propose the earliest, to use be SLAM algorithm (EKF-SLAM) based on EKF the most widely, estimated the joint posterior distribution of mobile robot's position and posture and map mark information by Kalman's iterative step.But the calculated amount of EKF-SLAM algorithm can increase along with map and exponentially increase, and when the higher order term of nonlinear function Taylor expansion cannot be ignored, the precision of state estimation of system just there will be comparatively big error, even filtering divergence, these shortcomings all seriously limit the development of EKF-SLAM technology.The FastSLAM algorithm based on Rao-Blackwellized particle filter of rising in recent years is applied to the particle filter algorithm being applicable to nonlinear and non-Gaussian noise factor in higher-dimension SLAM, and associating SLAM spatiality is estimated to be divided into sampling section and parsing part to reduce sample space, robot position and posture is estimated with particle filter, by EKF recursive estimation map state, solve the deficiency of EKF-SLAM very well, but due to the method for sampling that FastSLAM is based on sequential importance, sample degeneracy and particle diversity scarcity are the subject matter of the method.Can obtain more accurately based on multirobot, environmental information faster, also there is better self-align ability simultaneously, multirobot SLAM method is subject to increasing attention and research gradually, but single robot SLAM method all mainly expands in the situation of multirobot by existing method.

Summary of the invention

The object of the invention is, in circumstances not known, locating the estimated accuracy with map structuring to improve robot, the invention provides a kind of multirobot FastSLAM method (MBLCC-FastSLAM) based on road sign consistent correction.

Realizing technical scheme of the present invention is, a kind of FastSLAM method optimized based on electromagnetism-like mechanism, described method is when single robot performs FastSLAM task, in resampling process, introduce electromagnetism-like mechanism optimize thought, set up the FastSLAM algorithm that electromagnetism-like mechanism is optimized, sampling particle is regarded as an electronics, the particle large according to weights attracts the particle that weights are little, the particle that weights are little repels the large particle of weights, optimize distribution of particles, improve mass particle, thus improve the problem of sample degeneracy and particle diversity scarcity in traditional F astSLAM method;

Rejection mechanism is attracted to improve distribution of particles between charged particle in described method analog electromagnetic field, then when multirobot observes common road sign, observing that each robot of same road sign is as a node, utilize the estimated value of Kalman's consistency algorithm correction robot road markings, thus further increase each robot localization and pattern accuracy.

FastSLAM algorithm based on electromagnetism-like mechanism optimization regards an electronics as each sampling particle, and its electric charge computing formula is as follows:

$Q_i=\exp \{-n\frac{f\left(x^{\mathrm{best}}\right)-f\left(x^i\right)}{\underset{k=1}{\overset{N}{\mathrm{\Σ}}}(f\left(x^{\mathrm{best}}\right)-f\left(x^k\right))}\}---\left(1\right)$

In examination, n is the dimension of independent variable pose x, and N represents the number of particle, and f (x) represents the computing formula of weights, x ^bestrepresent the position of robot optimum, as can be seen from formula (1), the weights of particle are larger, then the quantity of electric charge is larger.After obtaining the electric charge of each particle, calculate each particle and be subject to class electromagnetism with joint efforts, formula is as follows:

$\left\{\begin{array}{cc}{F}_{\mathrm{ij}}=(x^j-x^i)Q_i{Q}_j,& f\left(x^j\right)>f\left(x^i\right);\\ F_{\mathrm{ij}}=(x^i-x^j)Q_i{Q}_j,& f\left(x^j\right)\≤f\left(x^i\right);\end{array}\right.---\left(2\right)$

F _i＝ΣF _ij,j＝1,2,...,N,j≠i. (3)

In formula, F _ijrepresent that a jth particle is to the acting force of i-th particle, F _irepresent that particle i is subject to making a concerted effort of other all particles; x ⁱrepresent the position of i-th particle; x ^jrepresent the position of a jth particle; Q _irepresent the electricity of i-th particle; Q _jrepresent the electricity of a jth particle.From formula (2), between particle, class electromagnetic force follows the little particle of the large particle attraction weights of weights, the particle that weights are little repels the large particle of weights, particle is moved towards high likelihood region by interparticle attraction in the process of motion thus, solve sample degeneracy problem, mutually pin down to maintain a certain distance again when particle collection is assembled near actual value under repulsive force effect simultaneously and ensure that diversity.Particle upgrades oneself position under the effect of making a concerted effort, and update rule is as follows:

x ⁱ＝x ⁱ+λRF _i/||F _i|| (4)

In formula, step-length λ is [01] upper equally distributed random number, and R is transport coefficient.The position of each particle through type (4) Policy Updates oneself, obtain new a collection of high-quality particle collection, electromagnetism-like mechanism iteration optimization terminates.

The FastSLAM2.0 algorithm specific implementation flow process optimized based on electromagnetism-like mechanism is as follows:

Step 1 is sampled: consider control variable u _kwith with Current observation value z _k, first adopt EKF to estimate that robot position and posture obtains average x _kand variance p _k, build with this N (x that distributes just very much _k, p _k) as proposal distribution, therefrom take particle collection

Step 2 weighting: the weights utilizing each particle of weights computing formula.

Step 3 electromagnetism-like mechanism iteration optimization distribution of particles: first, each particle through type (1) calculates its quantity of electric charge; Then, through type (2), (3) calculate each particle and are subject to making a concerted effort of other particle class electromagnetic forces; Finally, through type (4) rule is improved, the particle assembly after being optimized, and re-uses weight computing formulae discovery particle weights and normalization weights in formula, with represent respectively the k moment from the importance density function stochastic sampling ⁱthe weights that the system state of individual particle is corresponding with it.

Step 4 resampling: if N _effbe less than given threshold values, then resampling carried out to particle collection and obtain new particle collection otherwise do not carry out resampling.

Step 5 environmental characteristic upgrades: with the current pose filter value obtained and the map created before, upgrades with EKF the feature road sign that each particle Current observation arrives.

Step 6 repeats above step, till not having new observation.

When robot exploration zone of ignorance obtains the relative observation of feature road sign, the estimation of all road sign position is all relevant, because they are based on common robot pose evaluated error, the accuracy that road sign is estimated directly affects the positioning precision of SLAM and the Uniform estimates of map.In traditional F astSLAM2.0 algorithm, each particle adopt one independently EKF algorithm to upgrade road sign estimate.The present invention adopts three robots to work simultaneously, each robot performs an EM-FastSLAM algorithm separately, when observing common road sign, share the priori estimates separately to same road sign, each robot regards a node as, by the estimated value of Kalman's consistency algorithm correction road markings separately, thus improve the precision of location and composition further, its algorithm schematic diagram as shown in Figure 1.

The core procedure framework of Kalman's consistance filtering algorithm is shown below:

${\hat{x}}_{k|k}^i={\stackrel{\‾}{x}}_{k|k-1}^i+K(z_k^i-H{\stackrel{\‾}{x}}_{k|k-1}^i)+C_i\mathrm{\Σ}({\stackrel{\‾}{x}}_{k|k-1}^j-{\stackrel{\‾}{x}}_{k|k-1}^i)---\left(5\right)$

Wherein, represent i node partial estimation value, represent i node priori estimates, K represents Kalman filtering gain, represent k moment observed reading, H represents observing matrix, C _irepresent consistance gain.From formula (5), algorithm mainly comprises two parts: a part is Kalman filtering, and a part is multiple agent consistance.Kalman filtering part utilizes observed reading to upgrade and corrects the priori estimates of node for target, simultaneously due to the conforming capacity of many intelligence body, can, in the evaluated error to a certain degree reducing node, make all nodes obtain approximately uniform estimated value simultaneously.Based on this thought, each robot as a node, by the consistance Kalman filtering of road markings, merge the observation correction estimated value of other robot, introduce road sign Kalman consistent correction implementation procedure below:

First each moment each robot is calculated for the observed reading of road sign position:

${\hat{z}}_{j,k}^{t,i}=h({\mathrm{\μ}}_{j,k-1}^{t,i},x_k^{t,i})---\left(6\right)$

Wherein, represent the observed reading of k moment t robot i-th particle road markings j, h () represents observation equation.Calculate Jacobian matrix:

$H_{j,k}^{t,i}=h^{\′}({\mathrm{\μ}}_{j,k-1}^{t,i},x_k^{t,i})---\left(7\right)$

Calculate kalman gain:

$K_{j,k}^{t,i}={\mathrm{\Σ}}_{j,k-1}^{t,i}{\left(H_j^{t,i}\right)}^T{(H_j^{t,i}{\mathrm{\Σ}}_{j,k-1}^{t,i}{\left(H_j^{t,i}\right)}^T+R_k^t)}^{-1}---\left(8\right)$

Wherein, represent a jth road sign position average of k-1 moment t robot i-th particle, represent observation noise; represent the observing matrix of t robot i-th particle road markings j.Suppose that road sign is static, namely its priori estimates was exactly the estimated value in a upper moment, and therefore road sign position consistent correction equation is described below:

${\mathrm{\μ}}_{j,k}^{t,i}={\mathrm{\μ}}_{j,k-1}^{t,i}+K_{j,k}^{t,i}(z_{j,k}^{t,i}-{\hat{z}}_{j,k}^{t,i})+\mathrm{\ϵ}\underset{n\∈N_{j,k-1}}{\mathrm{\Σ}}({\mathrm{\μ}}_{j,k-1}^{n,\max }-{\mathrm{\μ}}_{j,k-1}^{t,i})---\left(9\right)$

${\mathrm{\Σ}}_{j,k}^{t,i}={\mathrm{\Σ}}_{j,k-1}^{t,i}-{\mathrm{\Σ}}_{j,k-1}^{t,i}{\left(H_j^{t,i}\right)}^T{(H_j^{t,i}{\mathrm{\Σ}}_{j,k-1}^{t,i}{\left(H_j^{t,i}\right)}^T+R^i)}^{-1}{H}_j^{t,i}{\mathrm{\Σ}}_{j,k-1}^{t,i}---\left(10\right)$

Wherein, ε is step-length, N _{j, k-1}expression observes the number of road sign j robot jointly in the k-1 moment, represent the particle at k-1 moment maximum weights; R ⁱrepresent i-th particle measurement noise covariance matrix; represent a jth road sign position observed reading of k-1 moment t robot i-th particle.

From formula (9), here the priori estimates of robot to same road sign is only shared, because in actual applications, consider the interference such as, packet loss limited in communication network broadband, delay, if exchange too many information between robot, may there is poor anti jamming capability and the weak problem of robustness in algorithm.

The beneficial effect of the present invention and existing SLAM Technical comparing is, can improve robot and position oneself the estimated accuracy with map structuring in circumstances not known, and have better stability and robustness under strong noise environment.

Accompanying drawing explanation

Fig. 1 is MBLCC-FastSLAM algorithm schematic diagram;

In Fig. 1: I is robot 1; II is robot 2; III is robot 3;

represent the road sign observed reading that first robot obtains;

represent the road sign observed reading that second robot obtains;

represent the road sign observed reading that the 3rd robot obtains;

represent the road sign position variance that first robot obtains;

represent the road sign position variance that second robot obtains;

represent the road sign position variance that the 3rd robot obtains;

Fig. 2 is the simulated environment of the present embodiment test experiments;

Fig. 3 is MBLCC-FastSLAM estimated result figure;

Fig. 4 is the pose evaluated error of robot 1; Fig. 5 is the road sign evaluated error of robot 1;

Fig. 6 is the pose evaluated error of robot 2; Fig. 7 is the road sign evaluated error of robot 2;

Fig. 8 is the pose evaluated error of robot 3; Fig. 9 is the road sign evaluated error of robot 3;

Figure 10 is each algorithm ARMSE curve under different population.

Embodiment

The invention process adopts three robots to work simultaneously, and the kinematic system of robot is:

$X_k^v=\left[\begin{array}{c}{x}_k^v\\ y_k^v\\ {\mathrm{\Φ}}_k^v\end{array}\right]=\left[\begin{array}{c}{x}_{k-1}^v+\mathrm{\ΔT}\·\cos ({\mathrm{\Φ}}_{k-1}^v+a_k)\\ y_k^v+\mathrm{\ΔT}\·\sin ({\mathrm{\Φ}}_{k-1}^v+a_k)\\ {\mathrm{\Φ}}_{k-1}^v+\frac{\mathrm{\ΔT}\·V_k\·\sin \left(a_k\right)}{D}\end{array}\right]+\left[\begin{array}{c}{\mathrm{\υ}}_x\\ {\mathrm{\υ}}_y\\ {\mathrm{\υ}}_{\mathrm{\Φ}}\end{array}\right]---\left(11\right)$

Wherein, refer to the pose of robot in global coordinate system, the crab angle of robot, V _krepresent robot movement velocity, a _kfor robot angle of deviation, Δ T is the odometer sampling time, υ _x, υ _y, υ _Φfor noise item, D represents two distance between axles.

Observation model is:

$Z_k=\left[\begin{array}{c}r\\ \mathrm{\θ}\end{array}\right]=\left[\begin{array}{c}\sqrt{{(x_i-x_k^v)}^2+{(y_i-y_k^v)}^2}\\ \arctan \frac{y_i-y_k^v}{x_i-x_k^v}-{\mathrm{\Φ}}_k^v\end{array}\right]+{\mathrm{\ω}}_k---\left(12\right)$

Wherein, r and θ refers to the Distance geometry angular separation of the environmental characteristic that sensor measurement obtains and robot respectively, (x _i, y _i) refer to the position coordinates of i-th feature observed, ω _krepresent observation noise.

Based on above robot model, in the process implemented, respectively to the present invention propose MBLCC-FastSLAM method and existing Multi-robot SLAM (known initial position) method, single robot EM-FastSLAM, traditional F astSLAM2.0 carry out SLAM emulation experiment, and be analyzed in estimated accuracy, error, stability.

Fig. 2 is the simulated environment of the present embodiment test experiments, and wherein asterisk represents the road sign of random setting, and three curves represent three robot predetermined movement paths, and robot goes to terminal and stops from initial point along this path, robot speed is set to V ₁=4m/s, V ₂=3.8m/s, V ₃=3.5m/s, the effective observed range of sensor is 30m, tentation data association is known, under this simulated environment, adopt estimated result that MBLCC-FastSLAM algorithm obtains as shown in Figure 3, wherein triangle represents the position estimating road sign, dotted line represents the path locus of estimation, and process noise is (σ _v=0.3m/s, σ _g=3 °), observation noise is (σ _r=0.2m, σ _θ=2 °), transport coefficient R is [0.008,0.005,0.001] ^t, road sign upgrades step-length ε=0.1, and population N is 10, N _efflower than total number of particles 75% time then carry out resampling.

Fig. 3 is MBLCC-FastSLAM estimated result figure.As seen from Figure 3 MBLCC-FastSLAM algorithm estimate robot path and road sign and actual value substantially identical, illustrate that it has higher estimated capacity.

Three robots are respectively to Multi-robot SLAM under the same conditions, and EM-FastSLAM, FastSLAM2.0 algorithm independently carries out emulation experiment.Fig. 4 ~ Fig. 9 gives three robots pose and road sign evaluated error comparison diagram under algorithms of different.Fig. 4 and Tu5Shi robot 1 is pose and road sign evaluated error comparison diagram under algorithms of different; Fig. 6 and Tu7Shi robot 2 is pose and road sign evaluated error comparison diagram under algorithms of different; Fig. 8 and Tu9Shi robot 3 is pose and road sign evaluated error comparison diagram under algorithms of different.

As can be seen from the figure no matter 3 robots are that pose evaluated error or road sign evaluated error MBLCC-FastSLAM algorithm all will be starkly lower than other three kinds of algorithms, and the estimated performance of EM-FastSLAM algorithm is significantly better than FastSLAM2.0 algorithm, illustrate that in resampling, introduce electromagnetism-like mechanism optimization thought improves estimated performance, the consistent correction of road markings further increases the estimated accuracy of location and composition.

The each robot of table 1 four kinds of algorithm evaluated errors

In order to eliminate randomness, more directly find out result, table 1 gives utilization four kinds of algorithms and carries out 50 independent emulation experiments, 3 robot SLAM estimation error statistics results.As can be seen from table also, MBLCC-FastSLAM algorithm estimated accuracy and stability are all better than other three kinds of algorithms.Figure 10 provides four kinds of algorithms and independently emulates the average (average RMSE, ARMSE) of 30 RMSE with N change curve.As seen from the figure, when population increases, the estimated accuracy of four kinds of algorithms all increases, population is increased little on the impact of algorithm estimated performance when population is greater than 150, under given noise level, the ARMSE of MBLCC-FastSLAM is minimum, even if still can keep higher estimated accuracy when N is very little, in this example, MBLCC-FastSLAM gets 10 particles and FastSLAM2.0 to get 100 particle filter precision suitable.

Claims (4)

1. the multirobot FastSLAM method based on road sign consistent correction, it is characterized in that, described method is when single robot performs FastSLAM task, in resampling process, introduce electromagnetism-like mechanism optimize thought, set up the FastSLAM algorithm that electromagnetism-like mechanism is optimized, sampling particle is regarded as an electronics, the particle large according to weights attracts the particle that weights are little, the particle that weights are little repels the large particle of weights, optimize distribution of particles, improve mass particle, thus improve the problem of sample degeneracy and particle diversity scarcity in traditional F astSLAM method;

Rejection mechanism is attracted to improve distribution of particles between charged particle in described method analog electromagnetic field, then when multirobot observes common road sign, observing that each robot of same road sign is as a node, utilize the estimated value of Kalman's consistency algorithm correction robot road markings, thus further increase each robot localization and pattern accuracy.

2. the multirobot FastSLAM method based on road sign consistent correction according to claim 1, is characterized in that, the FastSLAM algorithm of described electromagnetism-like mechanism optimization regards an electronics as each sampling particle, and its electric charge computing formula is as follows:

$Q_i=\exp \{-n\frac{f\left(x^{\mathrm{best}}\right)-f\left(x^i\right)}{\underset{k=1}{\overset{N}{\mathrm{\Σ}}}(f\left(x^{\mathrm{best}}\right)-f\left(x^k\right))}\}---\left(1\right)$

In formula, n is the dimension of independent variable pose x, and N represents the number of particle, and f (x) represents the computing formula of weights, x ^bestrepresent the position of robot optimum;

After obtaining the electric charge of each particle, calculate each particle and be subject to class electromagnetism with joint efforts, formula is as follows:

$\left\{\begin{array}{cc}{F}_{\mathrm{ij}}=(x^j-x^i)Q_i{Q}_j,& f\left(x^j\right)>f\left(x^i\right);\\ F_{\mathrm{ij}}=(x^i-x^j)Q_i{Q}_j,& f\left(x^j\right)\≤f\left(x^i\right);\end{array}\right.---\left(2\right)$

F _i＝ΣF _ij,j＝1,2,...,N,j≠i. (3)

In formula, F _ijrepresent that a jth particle is to the acting force of i-th particle, F _irepresent that particle i is subject to making a concerted effort of other all particles; x ⁱrepresent the position of i-th particle; x ^jrepresent the position of a jth particle; Q _irepresent the electricity of i-th particle; Q _jrepresent the electricity of a jth particle;

Particle upgrades oneself position under the effect of making a concerted effort, and update rule is as follows:

x ⁱ＝x ⁱ+λRF _i/||F _i|| (4)

In formula, step-length λ is [01] upper equally distributed random number; R is transport coefficient.

3. the multirobot FastSLAM method based on road sign consistent correction according to claim 1, is characterized in that, the FastSLAM2.0 algorithm specific implementation flow process of described electromagnetism mechanism optimization is as follows:

Step 1 is sampled: consider control variable u _kwith with Current observation value z _k, first adopt EKF to estimate that robot position and posture obtains average x _kand variance p _k, build with this N (x that distributes just very much _k, p _k) as proposal distribution, therefrom take particle collection

Step 2 weighting: the weights utilizing each particle of weights computing formula;

Step 3 electromagnetism-like mechanism iteration optimization distribution of particles: first, each particle through type (1) calculates its quantity of electric charge; Then, through type (2), (3) calculate each particle and are subject to making a concerted effort of other particle class electromagnetic forces; Finally, through type (4) rule is improved, the particle assembly after being optimized, and re-uses weight computing formulae discovery particle weights and normalization weights in formula, with represent the weights that the system state of k moment i-th particle of stochastic sampling from the importance density function is corresponding with it respectively;

Step 4 resampling: if N _effbe less than given threshold values, then resampling carried out to particle collection and obtain new particle collection otherwise do not carry out resampling;

Step 5 environmental characteristic upgrades: with the current pose filter value obtained and the map created before, upgrades with EKF the feature road sign that each particle Current observation arrives;

Step 6 repeats above step, till not having new observation.

4. the multirobot FastSLAM method based on road sign consistent correction according to claim 1, is characterized in that, the step of described Kalman's consistency algorithm correction robot road markings observation procedure is:

(1) calculate the observed reading of each robot for road sign position, each robot obtains the priori estimates of road sign by observation equation:

${\hat{z}}_{j,k}^{t,i}=h({\mathrm{\μ}}_{j,k-1}^{t,i},x_k^{t,i})$

Wherein, represent the observed reading of k moment t robot i-th particle road markings j, h () represents observation equation;

Calculate Jacobian matrix:

$H_{j,k}^{t,i}=h^{\′}({\mathrm{\μ}}_{j,k-1}^{t,i},x_k^{t,i})$

Calculate kalman gain:

$K_{j,k}^{t,i}={\mathrm{\Σ}}_{j,k-1}^{t,i}{\left(H_j^{t,i}\right)}^T{(H_j^{t,i}{\mathrm{\Σ}}_{j,k-1}^{t,i}{\left(H_j^{t,i}\right)}^T+R_k^t)}^{-1}$

Wherein, represent a jth road sign position average of k-1 moment t robot i-th particle, represent observation noise; represent the observing matrix of t robot i-th particle road markings j;

(2) when robot observes same road sign, road sign position is by its priori estimates of Kalman's coherence method correction:

${\mathrm{\μ}}_{j,k}^{t,i}={\mathrm{\μ}}_{j,k-1}^{t,i}+K_{j,k}^{t,i}(z_{j,k}^{t,i}-{\hat{z}}_{j,k}^{t,i})+\mathrm{\ϵ}\underset{n\∈N_{j,k-1}}{\mathrm{\Σ}}({\mathrm{\μ}}_{j,k-1}^{n,\max }-{\mathrm{\μ}}_{j,k-1}^{t,i})$

${\mathrm{\Σ}}_{j,k}^{t,i}={\mathrm{\Σ}}_{j,k-1}^{t,i}-{\mathrm{\Σ}}_{j,k-1}^{t,i}{\left(H_j^{t,i}\right)}^T{(H_j^{t,i}{\mathrm{\Σ}}_{j,k-1}^{t,i}{\left(H_j^{t,i}\right)}^T+R^i)}^{-1}{H}_j^{t,i}{\mathrm{\Σ}}_{j,k-1}^{t,i}$

In formula, ε is step-length, N _{j, k-1}expression observes the number of road sign j robot jointly in the k-1 moment, represent the particle at k-1 moment maximum weights; R ⁱrepresent i-th particle measurement noise covariance matrix; represent a jth road sign position observed reading of k-1 moment t robot i-th particle.