CN103487047A - Improved particle filter-based mobile robot positioning method - Google Patents

Abstract

The invention provides an improved particle filter-based mobile robot positioning method. The improved particle filter-based mobile robot positioning method comprises the following steps: establishing a motion equation and a road sign calculation equation of a robot; optimizing a particle set by using a multi-agent particle swarm optimization algorithm, wherein the obtained optimal value is estimation of a pose; estimating an environmental road sign by using Kalman filtering algorithm; updating and normalizing the weight and resampling. The positioning method is accurate in positioning and easy to implement; the pose estimation and the environmental road sign estimation of the mobile robot are more accurate in a simulation process of the mobile robot.

Description

A kind of method for positioning mobile robot based on improving particle filter

Technical field

The present invention relates to the mobile robot and locate simultaneously and build the diagram technology field, a kind of method of improving the localization for Mobile Robot error specifically is provided.

Background technology

The mobile robot has intelligent, independence, can replace the task of people in various the unknowns or hazardous environment, and often be full of various probabilistic factors in these unknown or dangerous environment, therefore the mobile robot must possess autonomous homing capability and work capacity, the mobile robot explores the unknown, the key of hazardous environment is to set up the local environment map, the local environment map of all foundation carries out data fusion, the overall environmental map of final structure, and being the mobile robot, mobile robot's location technology sets up the basis of local environment map, also can be described as the basis to other aspect researchs of mobile robot.The accurate location of only having the mobile robot, the local environment map that the mobile robot sets up is just more accurate, thus mobile robot's independent navigation ability, work capacity are just stronger.

For now, existing localization method, mainly for known environment, has been set barrier in environment, be called " road sign ", in the situation that given road sign and Mobile Robotics Navigation track, asks for the error of robot location and road sign.Method commonly used has the robot of ekf-slam(based on Kalman filtering locate simultaneously and build figure) and the robot of pf-slam(based on particle filter locate simultaneously and build figure).Locate simultaneously and build drawing method at existing two kinds, method calculated amount based on Kalman filtering is large, and the deficiency that error must Gaussian distributed is arranged, therefore, both at home and abroad to localization for Mobile Robot with build the research of figure mainly for particle filter method.And the pf-slam of standard there will be the problem of the poor and a large amount of particles of needs of particle, particle is poor to be referred in particle filter algorithm, and along with renewal and the iteration of particle, the weight of a lot of particles can diminish, even, after resampling, there will be the monistic phenomenon of particle.So it is significant to study the localization method of the enough less particles of a kind of energy and solution " particle is poor " problem.

Summary of the invention

For above deficiency of the prior art, the object of the present invention is to provide a kind of mobile robot of making can set up more efficiently, more accurately the method for improving the localization for Mobile Robot error of environmental map, this method is mainly for wheeled robot.For achieving the above object, technical scheme of the present invention is:

A kind of method for positioning mobile robot based on improving particle filter, it comprises the following steps:

101, initialization mobile robot's condition of work, comprise motion path, move mode, the reference position moved and the target location of setting the mobile robot, the positional information of road sign, obtain number of revolutions n in photoelectric coded disk △ t time on wheels of mobile robot and the resolution p of photoelectric coded disk, try to achieve the displacement d of mobile robot within the △ t time according to formula d=2 π rn/p, mobile robot's the left and right displacement of taking turns is set to respectively d ₁and d ₂, according to formula $\left\{\begin{array}{c}\mathrm{\&Delta;D}=\frac{{\mathrm{<figure-callout\; id="1"\; label="d"\; filenames="HDA00003627573800021.png"\; state="\{\{state\}\}">d</figure-callout>}}_1+d_2}{2}\\ \mathrm{\&Delta;\&theta;}=\frac{d_1-d_2}{l}\end{array}\right.$ Try to achieve displacement increment △ D and the rotating angle increment △ θ of robot, wherein r is radius of wheel, and l is the two-wheeled wheelspan, according to formula

try to achieve the moving radius R of mobile robot within the △ t time.After obtaining above parameter the prediction mobile robot at t+1 pose constantly:

$X(t+1)=\left\{\begin{array}{c}{x}_t+\frac{\mathrm{\&Delta;}{D}_t}{\mathrm{\&Delta;}{\mathrm{\&theta;}}_t}\cos {\mathrm{\&theta;}}_t\\ y_t+\frac{{\mathrm{\&Delta;D}}_t}{{\mathrm{\&Delta;\&theta;}}_t}\sin {\mathrm{\&theta;}}_t\\ {\mathrm{\&theta;}}_t+{\mathrm{\&theta;}}_t\end{array}\right\}+N\left(t\right)---\left(I\right)$

X in formula _t, y _t, θ _tbe respectively robot t horizontal ordinate, ordinate and attitude angle constantly under the planimetric coordinates system; attitude angle is the angle of robot and horizontal ordinate; N (t) is because the process noise of wheel distortion, ground friction formation; obeying average is zero Gaussian distribution; the value of t is [1n], wherein N (1)+N (2)+... + N (n)=0;

102, according to the estimation equation of road sign coordinate

the road markings coordinate estimated, wherein the t of robot pose constantly is X _t=(x _t, y _t, θ _t), x _t, y _t, θ _tbe respectively robot t horizontal ordinate, ordinate and attitude angle constantly under the planimetric coordinates system, the coordinate of road sign M is (x _m, y _m), sensor is positioned at the front end of robot, and center sensor is not when the robot geometric center overlaps, and the center sensor coordinate is (x _s, y _s), center sensor and robot geometric center distance are L, according to formula $\left[\begin{array}{c}{x}_s\\ y_s\end{array}\right]=\left\{\begin{array}{c}{x}_t+L{\cos \mathrm{\&theta;}}_t\\ y_t+L\sin {\mathrm{\&theta;}}_t\end{array}\right\}$ Try to achieve the observed reading of road sign coordinate

ρ _tfor the distance between road sign coordinate and center sensor coordinate, scan angle while for sensor, observing road sign, the line that scan angle is road sign coordinate and robot geometric center and the angle of horizontal ordinate positive dirction; The estimate equation of observed reading is:

103, a certain moment pose of robot is considered as to several particles and is combined to form the particle collection, release next particle collection constantly according to the mobile robot's who obtains in step 101 the equation of motion (I), with the MAPSO optimized algorithm, the particle collection is optimized, and the renewal weights W that sets the particle collection establishes, the value after optimizing is as the final estimated value to the robot pose;

104, according to the final estimated value of the robot pose obtained in the robot road sign coordinate estimate equation drawn in step 102 and step 103, adopt Kalman filtering algorithm to carry out the iteration renewal to the road sign coordinate of robot, draw the optimal value of road sign coordinate;

Right value update formula W according to the particle collection _t+1=W _t* exp (0.5* ρ _t ²), ρ _tfor the distance between road sign coordinate and center sensor coordinate, when weights are less than the renewal weights W set in step 103 and establish, return to step 101, until the final estimated value of the optimal value of the road sign coordinate of robot and pose determines, robot finishes location.

Further, in step 103, the optimization method to the particle collection comprises step:

A, according to fitness function formula F itness=exp{-0.5*sqrt[(Z _t-Z _tp) ^tr ^-1(Z _t-Z _tp)/2] } calculate the fitness function value of particle, mean the degree of optimization to the particle collection, when tried to achieve fitness function value global optimum, the optimization of particle collection is finished, wherein Z _tfor observed reading, Z _tpfor the observation predicted value that the pose according to the road sign that observed and prediction calculates, R is the observation noise covariance;

B, initialization agent, in position and the speed of solution space, ask for the neighbours agent of each agent, the individual extreme value of initialization and global optimum's adaptive value, definition initial inertia factor w ₀, stop inertial factor w ₁, definition maximum iteration time N, current iteration frequency n, define initial self study factor c _1i, stop self study factor c _1f, the initial factor c of social learning _2i, stop the factor c of social learning _2f, number of particles is L _size* L _size, L wherein _sizefor positive integer.；

Each particle collection sample constantly of C, optimization particle filter, the particle collection sample of particle filter was that the particle collection sample evidence moveable robot movement prediction equation in a upper moment obtains.In MAPSO, each agent neighbours optimum with it are at war with, and obtain local optimum P _pbest, then with the P of global optimum _gbestcompare, if be better than P _gbest, becoming new global optimum, the position of particle and speed more new formula are as follows:

V _i,j=W*V _i,j+C ₁*rand(1,d)*(P _pbest-P _i,j)+C ₂*rand(1,d)*(P _gbest-P _i,j)

P _i,j=P _i,j+ V _i,jv wherein _i,jthat horizontal ordinate is i, the particle rapidity that ordinate is j, P _i,jfor position, W is that the particle weight is inertial factor, asks for particle weights W=(w ₀-w ₁) * (N-n)/N+w ₁, C ₁and C ₂be respectively the self study factor and close social learning's factor, according to self study factor computing formula, try to achieve C ₁=(c _1f-c _1i) * n/N+c _1i, according to social learning's factor computing formula, try to achieve C ₂=(c _2f-c _2i) * n/N+c _2i; When the fitness function value Fitness of particle is less than or equal to the threshold value of setting, optimized algorithm stops optimizing.

Further, in step 104, the more new formula of weight is W=W*exp (0.5* ρ _t ²), ρ _tfor the distance between road sign coordinate and center sensor coordinate.

Advantage of the present invention and beneficial effect are as follows:

The present invention optimizes with many agent particle swarm optimization algorithm the centralized procurement of particle filter particle, with optimizing, obtains to such an extent that optimal value replaces the particle collection average in the standard particle filtering algorithm, with the road sign based on Kalman filtering, estimates to set up associating posterior probability density function.The present invention simultaneously also considers particle degeneration and the poor problem of particle, and the particle weight is constantly upgraded and resampled.The present invention is simple and be easy to realization, and in mobile robot's simulation process, mobile robot's pose is estimated and the environment road sign is estimated more accurate.Adopt MAPSO to optimize the method for positioning mobile robot of particle filter, although the place that positioning error is larger is arranged local, but the overall situation, positioning error is than the particle filtering method of basic particle filtering method and particle swarm optimization algorithm optimization, and error is less.

the accompanying drawing explanation

A kind of method for positioning mobile robot process flow diagram based on improving particle filter that Fig. 1 is the embodiment of the present invention;

Fig. 2 is the mobile schematic diagram of embodiment of the present invention robot under coordinate system.

Embodiment

The invention will be further elaborated to provide the embodiment of an indefiniteness below in conjunction with accompanying drawing.

(1) mobile robot generally calculates displacement with odometer, and the principle of work of odometer is to utilize to be arranged on the radian that the photoelectric coded disk on wheel turns within a certain period of time and to calculate displacement.Suppose that radius of wheel is r, the two-wheeled wheelspan is l, photoelectric coded disk is output as n time/△ t, resolution is p line/turn, mobile robot's displacement computing formula is d=2 π rn/p (rad), according to this formula, the mobile robot is carved into t+1 during from t constantly, and two-wheeled displacement is respectively d because differential drives ₁and d ₂, can obtain displacement increment, rotating angle increment and the moving radius of robot by above parameter.Can release the mobile robot t+1 equation of motion of pose constantly according to mobile robot t moment pose and angle.In the present invention, in the equation of motion of asking for mobile robot's pose, added because wheel distortion, ground friction etc. form process noise, it is zero Gaussian distribution that this noise is obeyed average.

Referring to Fig. 2, asking for road sign according to observed reading and mobile robot's pose is the basis that robot pose and road sign are estimated.If the t of robot pose constantly is X _t=(x _t, y _t, θ _t), x _t, y _t, θ _tbe respectively robot t geometric center horizontal ordinate, ordinate and attitude angle constantly under the planimetric coordinates system, the road sign coordinate is (x _m, y _m), sensor is positioned at the front end of robot, if, while robot not being considered as to a point, the geometric center of robot does not overlap with center sensor, the center sensor coordinate is (x _s, y _s), center sensor and robot geometric center distance are L, observed reading ρ _tfor the distance between road sign coordinate and center sensor coordinate,

scan angle while for sensor, observing road sign.The estimate equation of road markings coordinate is:

(2) location of robot is to set up the model P (x that asks for robot pose and environment road sign with the core of building figure _t, x _m| Z _t, u, n), u wherein, n means respectively control vector and data correlation here.Robot localization method based on particle filter can be divided into two independent processes of road sign estimation of pose based on particle filter location and Kalman filtering, that is:

$P(x_t,x_m|Z_t,u,n)=P\left(X_t\right|Z_t,u,n)\underset{i=1}{\overset{n}{\mathrm{\&Pi;}}}P\left(x_{m_i}\right|X_t,Z_t,u,n)---\left(2\right)$

Wherein, X _tfor the pose sequence vector.

In the pose position fixing process, adopt MAPSO to being optimized based on particle filter particle collection in the particle filtering method.MAPSO is the combination of particle swarm optimization algorithm (PSO) and many agent system (MAS), and MAPSO is considered as one " particle " by each agent, so total L of agent mono- _size* L _sizeindividual, L _size* L _sizefor the environmental map of definition, L _sizeit is a positive integer.The action strategy of MAPSO is that each agent and its neighbours are at war with and operate with cooperation, first ask for four neighbours' adaptive value, get its optimum neighbours, each agent neighbours optimum with it are at war with, if be not defeated by optimum neighbours, its invariant position in solution space, if be defeated by optimum neighbours, its position in solution space will be upgraded.

If the Spatial Dimension of search is d, the solution space position of agent is P _i,j, its optimum neighbours' solution space position is Q _i,j, i, j are for being not more than L _sizepositive integer, more new formula is:

(P _i,j)'=Q _i,j+(2*rand(1,d)-1)*(Q _i,j-P _i,j) （3）

(P _i,j) ' must be between the bound of agent solution space position.

As follows to particle filter particle collection Optimization Steps:

1) define the fitness function of MAPSO, be used for meaning the degree of optimization of robot predicting position (the particle collection in particle filter algorithm), the fitness function computing formula is as follows:

Fitness=exp{-0.5*sqrt[(Z _t-Z _tp) ^TR ^-1(Z _t-Z _tp)/2]} （4）

Z wherein _tpfor the observation predicted value that the pose according to the road sign that observed and prediction calculates, R is the observation noise covariance relevant with sensor observation scope error, vehicle wheel rotational speed, observed reading error.

2) initialization agent, in position and the speed of solution space, asks for the neighbours agent of each agent, the individual extreme value of initialization and global optimum's adaptive value, definition initial inertia factor w ₀, stop inertial factor w ₁, definition maximum iteration time N, current iteration frequency n, define initial self study factor c _1i, stop self study factor c _1f, the initial factor c of social learning _2i, stop the factor c of social learning _2f, number of particles is L _size* L _size.

3) optimize each particle collection sample constantly of particle filter.The particle collection sample of particle filter was that the particle collection sample evidence moveable robot movement prediction equation in a upper moment obtains.In MAPSO, each agent neighbours optimum with it are at war with, and obtain local optimum P _pbest, then with the P of global optimum _gbestcompare, if be better than P _gbest, become new global optimum.The position of particle and speed more new formula are as follows:

V _i,j=W*V _i,j+C ₁*rand(1,d)*(P _pbest-P _i,j)+C ₂*rand(1,d)*(P _gbest-P _i,j) （5）

P _i,j=P _i,j+V _i,j （6）

W is that the particle weight is inertial factor, C ₁and C ₂for the study factor, ask for formula as follows respectively:

W=(w ₀-w ₁)*(N-n)/N+w ₁ （7）

C ₁=(c _1f-c _1i)*n/N+c _1i （8）

C ₂=(c _2f-c _2i)*n/N+c _2i （9）

Particle collection in particle filter algorithm constantly upgrades position and the speed of each particle by many agent particle swarm optimization algorithm, constantly the actual value to location approaches, when the fitness function value of particle meets the threshold values of setting, show that the particle collection has divided near actual value, particle swarm optimization algorithm stops optimizing.

(3) utilize the Kalman filtering road markings to be estimated

Robot based on particle filter locates with the research of building figure simultaneously can be divided into two parts: the road sign that the pose of robot is estimated and movement-based robot pose is estimated is estimated two processes.The pose of robot is estimated as previously mentioned, and the road sign of robot estimates to adopt Kalman filtering algorithm.In the pose estimation procedure of robot, each particle will generate local road sign estimation separately, and therefore, m particle and n road sign are by corresponding m * n stand-alone card Thalmann filter.The estimation of road markings just is based on the renewal iterative process of Kalman filtering.

When the mobile robot observes the road sign in the sensor scan scope for the first time, by formula (1), obtain the initial estimate of some road signs, suppose that the road sign in environmental map is constant, that is to say that the road markings predicted value is constant, be initial estimate.Afterwards each of robot constantly, introduce estimation pose and up-to-date observed reading that MAPSO algorithm optimization particle filter obtains, again by formula (1) road markings, estimated, resulting value is called measured value, with Kalman filtering algorithm, these two values are upgraded to iteration, the optimal value finally obtained is the final estimated value of road markings, and the renewal process that each road sign is estimated is as follows:

1) set up state equation and the calculating covariance P that road sign is estimated

For the estimate equation of road sign, can use function f (X _t, Z _t-1)+v _tmean v _tfor the white Gaussian noise that average in the road markings estimation procedure is zero, its covariance is Q.If m particle estimated some road signs, generate a sample m=(m ₁, m ₂... .m _m), initial variance P ₁=Var (m)+Q.Introduce the pose (X after optimizing when for the first time _t) ' and up-to-date observed reading Z _tafter, function f (X _t, Z _t-1)+v _tin X _tby (X _t) ' institute replaces, and the white Gaussian noise that different obedience averages is zero is arranged, and the R of covariance now for mentioning above, road sign estimated value now, use X _1mmean, m particle reappraises and generates new sample M, variance P now this road sign ₂=Var (M)+R.

2) calculate kalman gain K _gwith road sign, estimate and the covariance renewal

K _gby initial variance and measurement variance, obtain, initial kalman gain is

The estimated value that road markings is new is X _2m, can be by kalman gain and predicted value X _mand X _1mobtain new estimated value X _2m=X _m+ K _g(X _1m-X _m), the covariance P that this is stylish ₃=(1-K _g) P ₁.So constantly carry out the renewal iteration that kalman gain, covariance, road sign are estimated, the continuous approaching to reality value of road sign estimation meeting obtained, complete the road sign estimation procedure of Kalman filtering.

(5) weight normalization and resampling.Locate in robot and build in the research of figure, after iteration of the particle filter particle collection of optimizing based on MAPSO is upgraded, the weight of particle can change, and the renewal of weight and normalization formula are as follows:

W _t+1=W _t*exp(-0.5*ρ _t ²) （10）

$W_{t+1}=W_t/\underset{n=1}{\overset{100}{\mathrm{\&Sigma;}}}W---\left(11\right)$

ρ _tfor the road sign coordinate mentioned above and the distance between the center sensor coordinate.

(5) weight normalization and resampling.Locate in robot and build in the research of figure, after iteration of the particle filter particle collection of optimizing based on MAPSO is upgraded, the weight of particle can change, and the renewal of weight and normalization formula are as follows:

W _i=W _i-1*exp(-0.5*ρ _t ²) （10）

$W_i=W_i/\underset{n=1}{\overset{100}{\mathrm{\&Sigma;}}}{W}_i---\left(11\right)$

W _ibe the weight of particle after the i time iteration, ρ _tfor the t that mentions the above distance between road sign coordinate and center sensor coordinate constantly.The number that particle is chosen is 100.

In the continuous renewal of particle weight, the particle weight often had can become very little, the robot that Here it is based on particle filter locates simultaneously and builds the particle degenerate problem there will be in drawing method, in this case, must be resampled, retain the larger particle of weights, remove the little particle of weights.Use N _effthe degree of degeneration that means particle.W establishes corresponding minimum number of particles N _min

$N_{\mathrm{eff}}=\frac{1}{\underset{n=1}{\overset{100}{\mathrm{\&Sigma;}}}{W_i}^2}---\left(12\right)$

Work as N _effbe less than the minimum number of particles N allowed that in particle swarm optimization algorithm, particle is set after degenerating _minthe time, the particle collection is resampled.The particle collection is after being obtained new weights, being resampled, and robot, according to optimal landmark and new weights, is estimated the pose in next moment by the equation of motion of robot.Until complete all road signs and each optimal estimation of robot pose constantly, whole robot localization method just finishes.

In the continuous renewal of particle weight, the particle weight often had can become very little, the robot that Here it is based on particle filter locates simultaneously and builds the particle degenerate problem there will be in drawing method, in this case, must be resampled, retain the larger particle of weights, remove the little particle of weights.

The particle collection is after being obtained new weights, being resampled, and robot, according to optimal landmark and new weights, is estimated the pose in next moment by the equation of motion of robot.Until complete all road signs and each optimal estimation of robot pose constantly, whole robot localization method just finishes

Test known, adopt MAPSO to optimize the method for positioning mobile robot of particle filter, the place that positioning error is larger is arranged, the overall situation although local, positioning error is than the particle filtering method of basic particle filtering method and particle swarm optimization algorithm optimization, and error is less.

Above these embodiment are interpreted as only for the present invention is described, is not used in and limits the scope of the invention.After the content of having read record of the present invention, the technician can make various changes or modifications the present invention, and these equivalences change and modification falls into the scope of the claims in the present invention equally.

Claims (1)

1. the method for positioning mobile robot based on improving particle filter is characterized in that comprising the following steps:

101, initialization mobile robot's condition of work, comprise motion path, move mode, the reference position moved and the target location of setting the mobile robot, the positional information of road sign, obtain number of revolutions n in photoelectric coded disk △ t time on wheels of mobile robot and the resolution p of photoelectric coded disk, try to achieve the displacement d of mobile robot within the △ t time according to formula d=2 π rn/p, mobile robot's the left and right displacement of taking turns is set to respectively d ₁and d ₂, according to formula $\left\{\begin{array}{c}\mathrm{\&Delta;D}=\frac{d_1+d_2}{2}\\ \mathrm{\&Delta;\&theta;}=\frac{d_1-d_2}{l}\end{array}\right.$ Try to achieve displacement increment △ D and the rotating angle increment △ θ of robot, wherein r is radius of wheel, and l is the two-wheeled wheelspan, according to formula

try to achieve the moving radius R of mobile robot within the △ t time, after obtaining above parameter the prediction mobile robot at t+1 pose constantly:

$X(t+1)=\left\{\begin{array}{c}{x}_t+\frac{\mathrm{\&Delta;}{D}_t}{\mathrm{\&Delta;}{\mathrm{\&theta;}}_t}\cos {\mathrm{\&theta;}}_t\\ y_t+\frac{{\mathrm{\&Delta;D}}_t}{{\mathrm{\&Delta;\&theta;}}_t}\sin {\mathrm{\&theta;}}_t\\ {\mathrm{\&theta;}}_t+{\mathrm{\&theta;}}_t\end{array}\right\}+N\left(t\right)---\left(I\right)$

X in formula _t, y _t, θ _tbe respectively robot t horizontal ordinate, ordinate and attitude angle constantly under the planimetric coordinates system; attitude angle is the angle of robot and horizontal ordinate; N (t) is because the process noise of wheel distortion, ground friction formation; obeying average is zero Gaussian distribution; the value of t is [1n], wherein N (1)+N (2)+... + N (n)=0;

102, according to the estimation equation of road sign coordinate

the road markings coordinate estimated, wherein the t of robot pose constantly is X _t=(x _t, y _t, θ _t), x _t, y _t, θ _tbe respectively robot t horizontal ordinate, ordinate and attitude angle constantly under the planimetric coordinates system, the coordinate of road sign M is (x _m, y _m), sensor is positioned at the front end of robot, and center sensor is not when the robot geometric center overlaps, and the center sensor coordinate is (x _s, y _s), center sensor and robot geometric center distance are L, according to formula $\left[\begin{array}{c}{x}_s\\ y_s\end{array}\right]=\left\{\begin{array}{c}{x}_t+L{\cos \mathrm{\&theta;}}_t\\ y_t+L\sin {\mathrm{\&theta;}}_t\end{array}\right\}$ Try to achieve the observed reading of road sign coordinate

ρ _tfor the distance between road sign coordinate and center sensor coordinate,

scan angle while for sensor, observing road sign, the line that scan angle is road sign coordinate and robot geometric center and the angle of horizontal ordinate positive dirction; The estimate equation of observed reading is:

103, a certain moment pose of robot is considered as to several particles and is combined to form the particle collection, release next particle collection constantly according to the mobile robot's who obtains in step 101 the equation of motion (I), with the MAPSO optimized algorithm, the particle collection is optimized, and the renewal weights W that sets the particle collection establishes, the value after optimizing is as the final estimated value to the robot pose;

104, according to the final estimated value of the robot pose obtained in the robot road sign coordinate estimate equation drawn in step 102 and step 103, adopt Kalman filtering algorithm to carry out the iteration renewal to the road sign coordinate of robot, draw the optimal value of road sign coordinate;

Right value update formula W according to the particle collection _t+1=W _t* exp (0.5* ρ _t ²), ρ _tfor the distance between road sign coordinate and center sensor coordinate, when weights are less than the renewal weights W set in step 103 and establish, return to step 101, until the final estimated value of the optimal value of the road sign coordinate of robot and pose determines, robot finishes location.

Images