A kind of adaptive Kano Meng Te localization method incorporating two-dimensional barcode informationTechnical field
The invention belongs to Mobile Robotics Navigation field, especially a kind of Monte Carlo localization side for incorporating two-dimensional barcode informationMethod.
Background technique
Under the premise of known to the environmental map, mobile robot determines it in the environment according to environment sensing and displacementPose problem be known as orientation problem.The Kano Meng Te positions (Monte Carlo localization, MCL) algorithm to move mouldType sampling, and observation model is combined to assess the weights of importance of each particle, obtain the posteriority reliability distribution of system mode.SuccessApplied to mobile robot field, it is suitable for two class problem of local positioning and Global localization.Odometer motion model passes through integrationPhotoelectric encoder information on wheel, and then relative mistake of the robot relative to upper sampling instant pose is obtained, fixedTime interval can carry out pose estimation.But due to the influence for the factors such as drift about or skid, lead to the precision of motion model at any timeBetween increase and decline, so as to cause the Kano Meng Te location algorithm position error increase；In addition, particle can generate after resamplingDegradation effect, particle diversity reduce, and fixed large sample particle will lead to computing resource waste, therefore scholars are studying alwaysHow to solve the problems, such as these two types of.Odometer error is just divided into systematic error in the nineties by Borenstein et al. and nonsystematic missesPoor two parts, and propose a kind of mileage meter calibration method " UMBmark " and eliminate systematic error odometer precision is influenced, machineDevice people can calibrate differential gear model parameter by desired trajectory movement several times.Yap et al. is built using EM algorithm and combining environmentalFigure carrys out while calculating the parameter of odometer motion model and laser observation model, finally realizes online adaptive calibration.Alhashimi et al. improves the observation model of Monte carlo algorithm, and the big of particle sample set is determined by the threshold value of settingIt is small, effectively reduce calculation amount.Huang Lu et al. design artificial landmark simultaneously establishes road sign library to correct the cumulative errors of odometer.But need mass data to cause positioning accuracy low since artificial landmark designs complicated and road sign library and establishes, it is unable to satisfy accuracyAnd it is computationally intensive.
Therefore, a kind of adaptive Kano Meng Te location algorithm incorporating two-dimensional barcode information, incorporates two dimension in sampling processThe absolute location information that code carries, the class for improving odometer add up to error；Laser sensor information establishes observation model based onIt calculates and updates particle weights；And using Kullback-Leibler distance (Kullback-Leibler Distance, KLD) weightSampling, the statistics boundary according to sampling in the APPROXIMATE DISTRIBUTION of state space determine population come online, avoid big calculation amount.
Summary of the invention
Present invention seek to address that the above problem of the prior art.Propose a kind of amendment odometer cumulative errors, adaptiveAdjust the adaptive Kano the Meng Te localization method of particle assembly size, the involvement two-dimensional barcode information for reducing calculation amount.Of the inventionTechnical solution is as follows:
A kind of adaptive Kano Meng Te localization method incorporating two-dimensional barcode information comprising following steps:
S1, the absolute location information provided according to two dimensional code and odometer control amount utMovement mould after establishing amendment errorType；
S2, according to the motion model and the sampling set χ at t-1 moment after amendment errort-1Particle sampler is carried out, estimates machineThe initial pose of device people；
S3 establishes observation model using two-dimensional laser sensor instrument distance；
S4 calculates the weights of importance of each particle according to the likelihood-domain of given map m and observation model and with new weight；
S5, the distribution situation according to particle in state space, Kullback-Leibler distance (Kullback-LeiblerDistance, KLD) resampling adaptively adjusts population needed for next iteration；
S6 determines the position of robot in the environment according to the distribution situation of particle.
Further, the motion model that step S1 is established after correcting error specifically includes:
Time interval (t-1, t] in, give motion information utAre as follows:
WhereinRespectively indicate the pose at t and t-1 moment under odometer coordinate system, utIt is transformed into three stepsSequence: initial rotation δrot1, translation δtransWith second of rotation δrot2.Establish the model of kinematic error:
ε indicate mean value be 0, variance b2Noise variance.Parameter alpha1-α4It is the error parameter for robot, they refer toSurely the cumulative errors moved, therefore physical location xtFrom xt-1By initial rotation angleFollow translation distanceFollowed byAnother rotation angleIt obtains, so that
Then physical location xt=(x ', y ', θ ').
Further, the step S2 particle sampler is estimated the initial pose of robot, is specifically included:
Motion model is sampled initial attitude xt-1, odometer read utX is read with cameracAs input, robot is being transportedWhen arriving two dimensional code without scanning during dynamic, pose isWhen camera is gotTwo-dimensional barcode information, the pose x with the sampling output of odometer at this timetIt is compared, the two error is greater than critical value τ, then samples calculationThe value of method output is the absolute value x of two dimensional code coordinatec, and enable the pose x at current timetFor the pose that two-dimensional barcode information provides, afterIt is continuous to be sampled until scanning next two dimensional code.
Further, the step S3 establishes observation model using two-dimensional laser sensor instrument distance, specifically includes following stepIt is rapid:
Conditional probability distribution p (zt|xt, m) be observation model, a possibility that each single observation be multiplied can be obtained it is generalRate is as follows:
Wherein, xtIt is the pose of robot, ztIt is the observation of t moment, ztkIndicate k-th of distance measurement value of t moment.M is ringCondition figure, it is assumed that independent between each observation beam noise.
Further, the step S4 calculates the important of each particle according to the likelihood-domain of given map m and observation modelProperty weight and with new weight, specifically includes:
It is obtained in x-y space at a distance from nearest barrier with likelihood-domain calculating observation probability using map m as conditionDist:
Since the noise of sensor different beams is independent from each other, to kValue be multiplied, by by oneA be just distributed very much is uniformly distributed the likelihood result q for being mixed to get observation model with one:
Give three parameter zhit、zrandAnd zmaxIt is weighted and averaged mixing, and zhit+zrand+zmax=1.
Further, in the step S5, Kullback-Leibler distance (Kullback-Leibler Distance,KLD) the calculation method of sub- resampling:
KLD sampling all determines sample number to each particle filter iteration with probability 1- δ, so that true Posterior distrbutionp and baseError between the APPROXIMATE DISTRIBUTION of sampling is less than ε, thereby determines that the size of resampling sample set, when population n meets oneWhen definite value, it is ensured that the K-L distance between the true value and estimated value of probability is less than threshold epsilon, at this time the value of n are as follows:
Wherein z1-δIt is the standardized normal distribution of upper quantile 1- δ, h indicates the histogram for being at least filled with a particleDigit is meeting nxBefore counting boundary, KLD sampling will generate always particle.
It advantages of the present invention and has the beneficial effect that:
The present invention provides a kind of adaptive Kano Meng Te localization methods for incorporating two-dimensional barcode information.It is provided using two dimensional codeAbsolute location information amendment odometer model cumulative errors after sampled；Effectively amendment cumulative errors improve positioning accurateDegree；Resampling part using Kullback-Leibler distance (KLD) resampling, according to particle state space distribution situationPopulation needed for adaptive adjustment next iteration, reduces calculation amount.
Detailed description of the invention
Fig. 1 is the adaptive Kano the Meng Te localization method process that the present invention provides that preferred embodiment incorporates two-dimensional barcode informationFigure.
Specific embodiment
Following will be combined with the drawings in the embodiments of the present invention, and technical solution in the embodiment of the present invention carries out clear, detailedCarefully describe.Described embodiment is only a part of the embodiments of the present invention.
The technical solution that the present invention solves above-mentioned technical problem is:
As shown in Figure 1, the present invention provides a kind of adaptive Kano Meng Te localization method for incorporating two-dimensional barcode information, packetInclude following steps:
S1, by the sampling set χ at momentt-1, each particle corresponds to robot in the estimated motion track of this point, when t-1Carve the control amount u appliedtThe world coordinates information x provided with two dimensional codecThe absolute location information provided as input, two dimensional codeOdometer motion model can be corrected；
Time interval (t-1, t] in, give motion information utAre as follows:
WhereinRespectively indicate the pose at t and t-1 moment under odometer coordinate system, utIt is transformed into three stepsSequence: initial rotation δrot1, translation δtransWith second of rotation δrot2.Establish the model of kinematic error:
ε indicate mean value be 0, variance b2Noise variance.Parameter alpha1-α4It is the error parameter for robot, they refer toSurely the cumulative errors moved, therefore physical location xtFrom xt-1By initial rotation angleFollow translation distanceFollowed byAnother rotation angleIt obtains, so that
Then physical location xt=(x ', y ', θ ')
S2 carries out the initial pose of particle sampler estimation robot according to revised motion model；
Motion model is sampled initial attitude xt-1, odometer read utX is read with cameracAs input, robot is being transportedWhen arriving two dimensional code without scanning during dynamic, pose isWhen camera is gotTwo-dimensional barcode information, the pose x with the sampling output of odometer at this timetIt is compared, the two error is greater than critical value τ, then samples calculationThe value of method output is the absolute value x of two dimensional code coordinatec, and enable the pose x at current timetFor the pose that two-dimensional barcode information provides, afterIt is continuous to be sampled until scanning next two dimensional code.
S3 establishes observation model using two-dimensional laser sensor instrument distance；With likelihood-domain calculating observation probability,
Conditional probability distribution p (zt|xt, m) be observation model, a possibility that each single observation be multiplied can be obtained it is generalRate is as follows:
Wherein, xtIt is the pose of robot, ztIt is the observation of t moment, ztkIndicate k-th of distance measurement value of t moment.M is ringCondition figure, it is assumed that independent between each observation beam noise.
S4 calculates the weights of importance of each particle according to given map m and observation model likelihood-domain and with new weight；
It is obtained in x-y space at a distance from nearest barrier with likelihood-domain calculating observation probability using map m as conditionDist:
Since the noise of sensor different beams is independent from each other, to kValue be multiplied, by by oneA be just distributed very much is uniformly distributed the likelihood result q for being mixed to get observation model with one:
Give three parameter zhit、zrandAnd zmaxIt is weighted and averaged mixing, and zhit+zrand+zmax=1.
S5, population needed for adaptively adjusting next iteration in the distribution situation of state space according to particle；
KLD sampling all determines sample number to each particle filter iteration with probability 1- δ, so that true Posterior distrbutionp and baseError between the APPROXIMATE DISTRIBUTION of sampling is less than ε, thereby determines that the size of resampling sample set, when population n meets oneWhen definite value, it is ensured that the K-L distance between the true value and estimated value of probability is less than threshold epsilon, at this time the value of n are as follows:
Wherein z1-δIt is the standardized normal distribution of upper quantile 1- δ, h indicates the histogram for being at least filled with a particleDigit is meeting nxBefore counting boundary, KLD sampling will generate always particle.
S6 determines the position of robot in the environment according to the distribution situation of particle.The present invention can carry out robotIt is accurately positioned.
The above embodiment is interpreted as being merely to illustrate the present invention rather than limit the scope of the invention.?After the content for having read record of the invention, technical staff can be made various changes or modifications the present invention, these equivalent changesChange and modification equally falls into the scope of the claims in the present invention.