CN103487047B - A kind of method for positioning mobile robot based on improving particle filter - Google Patents

Abstract

The invention provides a kind of method for positioning mobile robot based on improving particle filter, comprise step: the equation of motion and the roadmap calculation equation of setting up robot; Optimize particle collection with many agent particle swarm optimization algorithm, the optimal value of gained is the estimation to pose; Utilize Kalman filtering algorithm to estimate environment road sign; Weight is upgraded and normalization, resampling. Localization method accurate positioning of the present invention and be easy to realize, in mobile robot's simulation process, mobile robot's pose estimate and the estimation of environment road sign more accurate.

Description

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

Technical field

The present invention relates to mobile robot and locate simultaneously and build diagram technology field, a kind of mobile apparatus that improves is specifically providedThe method of people's position error.

Background technology

Intelligent, independence that mobile robot has, can replace the task of people in various the unknowns or hazardous environment, and thisIn a little unknown or dangerous environment, be often full of various probabilistic factors, therefore mobile robot must possess autonomous leadingBoat ability and work capacity, the key that mobile robot explores the unknown, hazardous environment is to set up local environment map, all foundationLocal environment map carry out data fusion, finally build overall environmental map, and mobile robot's location technology is mobileRobot sets up the basis of local environment map, also can be described as the basis to other aspect researchs of mobile robot. Only has movementThe accurate location of robot, the local environment map that mobile robot sets up is just more accurate, thus mobile robot's is certainly leadingBoat 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 ", the in the situation that of given road sign and Mobile Robotics Navigation track, asks for the error of robot location and road sign. ConventionalMethod have ekf-slam (robot based on Kalman filtering locates simultaneously and builds figure) and pf-slam (based on particle filterRobot locate simultaneously and build figure). Locate simultaneously and build in drawing method, the method meter based on Kalman filtering at existing two kindsCalculation amount is large, and has the deficiency that error must Gaussian distributed, therefore, both at home and abroad to localization for Mobile Robot with build grinding of figureStudy carefully 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, evenAfter resampling, there will be the monistic phenomenon of particle. So study a kind of can be with less particle and solution " particlePoor " localization method of problem is significant.

Summary of the invention

For above deficiency of the prior art, the object of the present invention is to provide one make mobile robot can be more efficiently, more accurateSet up the method for improving localization for Mobile Robot error of environmental map, this method is mainly for wheeled robot. ForAchieve the above object, technical scheme of the present invention is:

Based on a method for positioning mobile robot that improves particle filter, it comprises the following steps:

101, the condition of work that initializes mobile robot, comprises motion path, move mode, the movement of setting mobile robotOriginal position and the positional information of target location, road sign, obtain in the photoelectric coded disk △ t time on wheels of mobile robotThe resolution ratio p of number of revolutions n and photoelectric coded disk, tries to achieve mobile robot's moving within the △ t time according to formula d=2 π rn/pMoving apart from d, 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{\Δ}D=\frac{d_1+d_2}{2}\\ \mathrm{\Δ}\mathrm{\θ}=\frac{d_1-d_2}{l}\end{array}\right.$ Try to achieveThe displacement increment △ D of robot and rotating angle increment △ θ, wherein r is radius of wheel, l is two-wheeled wheelspan, according to formulaTry to achieve the moving radius R of mobile robot within the △ t time. Predict that mobile robot is in the t+1 moment after above parameter obtainingPose:

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

X in formula_t，y_t，θ_tAbscissa, ordinate and the attitude angle in t moment that is respectively robot under plane coordinates system, appearanceState angle is the angle of robot and abscissa, and N (t) is because the process noise of wheel distortion, ground friction formation is obeyed allValue is zero Gaussian distribution, and the value of t is [1n], wherein N (1)+N (2)+... + N (n)=0;

102, according to the estimation equation of road sign coordinateRoad markings coordinate estimates, whereinThe pose in the t of robot moment is X_t＝(x_t,y_t,θ_t)，x_t，y_t，θ_tT moment that is respectively robot under plane coordinates systemAbscissa, ordinate and attitude angle, the coordinate of road sign M is (x_m,y_m), sensor is positioned at the front end of robot, center sensorNot when robot geometric center overlaps, center sensor coordinate is (x_s,y_s), center sensor and robot geometric center distanceFor L, according to formula $\left[\begin{array}{c}{x}_s\\ y_s\end{array}\right]=\left\{\begin{array}{c}{x}_t+Lcos{\mathrm{\θ}}_t\\ y_t+{\mathrm{Lsin\θ}}_t\end{array}\right\}$ Try to achieve the observation of road sign coordinateρ_tFor road sign coordinate withDistance between center sensor coordinate,Scan angle while observing road sign for sensor, scan angle is that road sign coordinate and robot are severalWhat line at center and angle of abscissa positive direction; The estimate equation of observation is:

103, a certain robot moment pose is considered as to several particles and is combined to form particle collection, according to moving of obtaining in step 101The equation of motion (I) of mobile robot is released the particle collection in next moment, particle collection is optimized with MAPSO optimized algorithm,And set the renewal weights W of particle collection_If, the value after optimizing is as the final estimated value to robot pose;

104, according to the robot pose obtaining in the robot road sign coordinate estimate equation drawing in step 102 and step 103Final estimated value, adopts Kalman filtering algorithm to carry out iteration renewal to the road sign coordinate of robot, draws the optimum of road sign coordinateValue;

According to the right value update formula W of particle collection_t+1＝W_t*exp(-0.5*ρ_t ²)，ρ_tFor road sign coordinate and center sensor coordinateBetween distance, when weights are less than the renewal weights W setting in step 103_IfTime, return to step 101, until the road of robotThe mark optimal value of coordinate and the final estimated value of pose are determined, robot location end.

Further, in step 103, the optimization method of particle collection is comprised to 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 particleFitness function value, represents the degree of optimization to particle collection, in the time of tried to achieve fitness function value global optimum, to particleThe optimization of collection finishes, wherein Z_tFor observation, Z_tpFor the observation calculating according to the pose of the road sign having observed and predictionPredicted value, R is observation noise covariance;

B, initialization agent, in position and the speed of solution space, ask for the neighbours agent of each agent, initialize the individual utmost pointValue and global optimum's adaptive value, definition initial inertia factor w₀, stop inertial factor w₁, definition maximum iteration time N, whenFront iterations n, defines initial self study factor c_1i, stop self study factor c_1f, the initial factor c of social learning_2i, stopThe factor c of social learning_2f, number of particles is L_size*L_size, wherein L_sizeFor positive integer. ;

The particle collection sample in each moment of C, optimization particle filter, the particle collection sample of particle filter was the particle collection in a upper momentSample evidence moveable robot movement prediction equation obtains. In MAPSO, each agent neighbours optimum with it carry out competingStrive, 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)

P_i,j＝P_i,j+V_i,jWherein V_i,jIt is abscissaFor i, the particle rapidity that ordinate is j, P_i,jFor position, W is that particle weight is inertial factor, asks for particle weightW＝(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 because ofSub-computing formula is tried to achieve C₁＝(c_1f-c_1i)*n/N+c_1i, try to achieve according to social learning's factor computing formulaC₂＝(c_2f-c_2i)*n/N+c_2i; In the time that the fitness function value Fitness of particle is less than or equal to the threshold value of setting, optimizes and calculateMethod stops optimizing.

Further, in step 104, the more new formula of weight is W=W*exp (0.5* ρ_t ²)，ρ_tFor road sign coordinate and biographyDistance between sensor centre 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, obtains to such an extent that optimal value replaces with optimizingParticle collection average in standard particle filtering algorithm, estimates to set up associating posterior probability density function with the road sign based on Kalman filtering.The present invention simultaneously also considers particle degeneration and the poor problem of particle, and particle weight is constantly upgraded and resampling. ThisBright simple and be easy to realize, in mobile robot's simulation process, mobile robot's pose is estimated and environment road sign is estimated moreAdd accurately. Adopt MAPSO to optimize the method for positioning mobile robot of particle filter, have the place that position error is larger although local,But the overall situation, position error is than the particle filtering of basic particle filtering method and particle swarm optimization algorithm optimizationMethod, error is less.

Brief description of the drawings

Fig. 1 is a kind of method for positioning mobile robot flow chart based on improving particle filter of the embodiment of the present invention;

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

Detailed description of the invention

The invention will be further elaborated to provide a non-limiting embodiment below in conjunction with accompanying drawing.

(1) mobile robot generally calculates displacement with odometer, and the operation principle of odometer is to utilize to be arranged on carThe radian that photoelectric coded disk on wheel turns within a certain period of time calculates displacement. Suppose that radius of wheel is r, two-wheeled wheelspanFor l, photoelectric coded disk is output as n time/△ t, and resolution ratio is p line/turn, and mobile robot's displacement computing formula isD=2 π rn/p (rad), according to this formula, mobile robot is carved into the t+1 moment during from t, and two-wheeled displacement is because differential drivesThe moving d that is respectively₁And d₂, can obtain displacement increment, rotating angle increment and the moving radius of robot by above parameter. According to movingMobile robot t moment pose and angle can be released the equation of motion of mobile robot t+1 moment pose. In the present invention, move asking forIn the equation of motion of mobile robot pose, added because the formation such as wheel distortion, ground friction process noise, this noiseThe Gaussian distribution that obedience average is zero.

Referring to Fig. 2, asking for road sign according to observation and mobile robot's pose is the basis that robot pose and road sign are estimated.If the pose in the t of robot moment is X_t＝(x_t,y_t,θ_t)，x_t，y_t，θ_tBe respectively robot t moment under plane coordinates systemGeometric center abscissa, ordinate and attitude angle, road sign coordinate is (x_m,y_m), sensor is positioned at the front end of robot, if notWhen robot is considered as to a point, the geometric center of robot does not overlap with center sensor, and center sensor coordinate is (x_s,y_s)，Center sensor and robot geometric center distance are L, observationρ_tFor road sign coordinate and center sensor seatDistance between mark,Scan angle while observing road sign for sensor. The estimate equation of road markings coordinate is:

(2) location of robot is to set up the model of asking for robot pose and environment road sign with the core of building figureP(x_t,x_m|Z_t, u, n), wherein u, n represents respectively dominant vector and data correlation here. Robot based on particle filter is fixedMethod for position can be divided into pose location based on particle filter and two independent processes of road sign estimation of 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}{\Π}}P\left(x_{m_i}\right|X_t,Z_t,u,n)---\left(2\right)$

Wherein, X_tFor pose sequence vector.

In pose position fixing process, adopt MAPSO to being optimized based on particle filter particle collection in particle filtering method.MAPSO is the combination of particle swarm optimization algorithm (PSO) and many agent system (MAS), and MAPSO looks each agentBe one " particle ", so total L of agent mono-_size*L_sizeIndividual, L_size*L_sizeFor the environmental map of definition, L_sizeBe oneIndividual positive integer. The action strategy of MAPSO is that each agent and its neighbours are at war with and cooperation operation, first asks for four neighboursAdaptive value, get its optimum neighbours, each agent neighbours optimum with it are at war with, if be not defeated by optimum neighbours, itsInvariant 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 is 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) fitness function of definition MAPSO, is used for representing robot predicting position (the particle collection in particle filter algorithm)Degree of optimization, fitness function computing formula is as follows:

Fitness＝exp{-0.5*sqrt[(Z_t-Z_tp)^TR^-1(Z_t-Z_tp)/2]}(4)

Wherein Z_tpFor the observation predicted value calculating according to the pose of the road sign having observed and prediction, R sees with sensorSurvey coverage error, vehicle wheel rotational speed, the relevant observation noise covariance of observation error.

2) initialize position and the speed of agent in solution space, ask for the neighbours agent of each agent, initialize the individual utmost pointValue and global optimum's adaptive value, definition initial inertia factor w₀, stop inertial factor w₁, definition maximum iteration time N, whenFront iterations n, defines initial self study factor c_1i, stop self study factor c_1f, the initial factor c of social learning_2i, stopThe factor c of social learning_2f, number of particles is L_size*L_size。

3) the particle collection sample in each moment of optimization particle filter. The particle collection sample of particle filter was the particle collection in a upper momentSample evidence moveable robot movement prediction equation obtains. In MAPSO, each agent neighbours optimum with it carry out competingStrive, 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 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 algorithmDegree, constantly approaches to the actual value of location, in the time that the fitness function value of particle meets the threshold values of setting, has shown particle collectionDivide near actual value, particle swarm optimization algorithm stops optimizing.

(3) utilize Kalman filtering road markings to estimate

Robot based on particle filter locates with the research of building figure simultaneously can be divided into two parts: the pose of robot estimate andThe road sign of estimating based on mobile robot's pose is estimated two processes. The pose of robot is estimated as previously mentioned, and the road of robotMark estimates to adopt Kalman filtering algorithm. In the pose estimation procedure of robot, each particle is by the local road generating separatelyMark estimation, therefore, m particle and n road sign are by corresponding m × n stand-alone card Thalmann filter. The estimation of road markings is exactly baseIn the renewal iterative process of Kalman filtering.

When mobile robot observes the road sign within the scope of sensor scan for the first time, by formula (1), obtain some road signsInitial estimate, suppose that the road sign in environmental map is constant, that is to say that road markings predicted value is constant, be initialEstimated value. In robot each moment afterwards, introduce estimation pose and up-to-date sight that MAPSO algorithm optimization particle filter obtainsMeasured value, estimates, the value obtaining is called measured value again by formula (1) road markings, with Kalman filtering algorithm to thisTwo values are upgraded iteration, and the optimal value finally obtaining is the final estimated value of road markings, the renewal that each road sign is estimatedProcess 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_tRepresent v_tFor average in road markings estimation procedureBe zero white Gaussian noise, its covariance is Q. If m particle estimated some road signs, generate a samplem＝(m₁,m₂,.......m_m), initial variance P₁=Var (m)+Q. Introduce the pose (X after optimization when for the first time_t) ' and up-to-dateObservation Z_tAfter, function f (X_t,Z_t-1)+v_tIn X_tBy (X_t) ' institute replaces, and has the Gauss that different obedience averages is zeroWhite noise, covariance is now the R mentioning above, road sign estimated value now, uses X_1mRepresent, m particle is to thisOne road sign reappraises and generates new sample M, variance P now₂＝Var(M)+R。

2) calculate kalman gain K_gEstimate with road sign and covariance renewal

K_gObtain by initial variance and measurement variance, initial kalman gain isThe estimated value that road markings is new is X_2m, can be by kalman gain and predicted value X_mAnd X_1mObtain new estimated valueX_2m＝X_m+K_g(X_1m-X_m), the covariance P that this is stylish₃＝(1-K_g)P₁. So constantly carry out kalman gain,The renewal iteration that covariance, road sign are estimated, the continuous approaching to reality value of road sign estimation meeting obtaining, completes the road sign of Kalman filteringEstimation procedure.

(5) weight normalization and resampling. In robot, locate and build in the research of figure, the particle filter of optimizing based on MAPSOAfter iteration of wave-particle subset 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}{\Σ}}W\text{---}\left(11\right)$

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

(5) weight normalization and resampling. In robot, locate and build in the research of figure, the particle filter of optimizing based on MAPSOAfter iteration of wave-particle subset 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_{\text{i}}={\text{W}}_i/\underset{n=1}{\overset{100}{\Σ}}{W}_i---\left(11\right)$

W_iBe the weight of particle after the i time iteration, ρ_tFor between the t moment road sign coordinate and center sensor coordinate mentioned aboveDistance. The number that particle is chosen is 100.

In the continuous renewal of particle weight, the particle weight often having can become very little, the machine that Here it is based on particle filterPeople locates simultaneously and builds the particle degenerate problem there will be in drawing method, in this case, must carry out resampling, right of retentionBe worth larger particle, remove the little particle of weights. Use N_effRepresent the degree of degeneration of particle. W establishes corresponding minimum number of particlesN_min

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

Work as N_effBe less than the minimum number of particles N allowing that in particle swarm optimization algorithm, particle is set after degenerating_minTime, particle collectionCarry out resampling. Particle collection is obtaining new weights, is carrying out after resampling, and robot is according to optimal landmark and new weights, logicalCrossing the equation of motion of robot estimates the pose in next moment. Until complete to all road signs and each in moment robotThe optimal estimation of pose, whole robot localization method just finishes.

In the continuous renewal of particle weight, the particle weight often having can become very little, the machine that Here it is based on particle filterPeople locates simultaneously and builds the particle degenerate problem there will be in drawing method, in this case, must carry out resampling, right of retentionBe worth larger particle, remove the little particle of weights.

Particle collection is obtaining new weights, is carrying out after resampling, and robot, according to optimal landmark and new weights, passes through robotThe equation of motion pose in next moment is estimated. Until complete to all road signs and each moment robot poseExcellent estimation, whole robot localization method just finishes

Test knownly, adopt MAPSO to optimize the method for positioning mobile robot of particle filter, although part has position error largerPlace, but the overall situation, the particle that position error is optimized than basic particle filtering method and particle swarm optimization algorithmFiltering localization method, error is less.

These embodiment are interpreted as being only not used in and limiting the scope of the invention for the present invention is described above. ReadingAfter the content of record of the present invention, technical staff can make various changes or modifications the present invention, and these equivalences change and modifyFall into equally the scope of the claims in the present invention.

Claims (1)

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

101, initialize mobile robot's condition of work, comprise and set mobile robot's motion path, move mode, the original position moving and target location, the positional information of road sign, obtain number of revolutions n in the photoelectric coded disk Δ t time on wheels of mobile robot and the resolution ratio 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 formulaThe displacement increment Δ D and the rotating angle increment Δ θ that try to achieve robot, wherein r is radius of wheel, l is two-wheeled wheelspan, according to formulaTry to achieve the moving radius R of mobile robot within the Δ t time, after the above parameter of acquisition, predict the pose of mobile robot in the t+1 moment:

X in formula_t，y_t，θ_tAbscissa, ordinate and the attitude angle in t moment that is respectively robot under plane coordinates system, attitude angle is the angle of robot and abscissa, N (t) is because the process noise of wheel distortion, ground friction formation, the Gaussian distribution that obedience average is zero, the value of t is [1n], wherein N (1)+N (2)+... + N (n)=0;

102, according to the estimation equation of road sign coordinateRoad markings coordinate estimates, wherein the pose in the t of robot moment is X_t＝(x_t,y_t,θ_t), 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 robot geometric center overlaps, and center sensor coordinate is (x_s,y_s), center sensor and robot geometric center distance are L, according to formulaTry to achieve the observation of road sign coordinateρ_tFor the distance between road sign coordinate and center sensor coordinate,Scan angle while observing road sign for sensor, scan angle is the line of road sign coordinate and robot geometric center and the angle of abscissa positive direction; The estimate equation of observation is:

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

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

According to the right value update formula W of particle collection_t+1＝W_t*exp(-0.5*ρ_t ²), when weights are less than the renewal weights W setting in step 103_IfTime, return to step 101, until the optimal value of the road sign coordinate of robot and the final estimated value of pose are definite, robot finishes location.