CN108645413A - The dynamic correcting method of positioning and map building while a kind of mobile robot - Google Patents
The dynamic correcting method of positioning and map building while a kind of mobile robot Download PDFInfo
- Publication number
- CN108645413A CN108645413A CN201810574387.XA CN201810574387A CN108645413A CN 108645413 A CN108645413 A CN 108645413A CN 201810574387 A CN201810574387 A CN 201810574387A CN 108645413 A CN108645413 A CN 108645413A
- Authority
- CN
- China
- Prior art keywords
- pose
- mobile robot
- observation
- particle
- positioning
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Withdrawn
Links
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
Abstract
Positioning and map building correcting method, include the following steps while the present invention discloses a kind of mobile robot：Step1：Historical data acquisition verification, analysis and arrangement history message are recorded in caused bias factor when positioning is with map structuring (SLAM) immediately；Step2：Adjustment revision parameter sets fitness function, structure search more new model, double sampling；Knowledge revision is first verified, by closing on fitness function of the inconsistent definition of observation about Q and R, on the basis of this fitness function, a discreet value of Q and R is searched out by improved fractional order particle cluster algorithm, according to discreet value, completes the amendment of Qt and Rt；Step3：New route amendment updates the update that step completes particle collection by prediction, data correlation, pose update, weight calculation, road sign；Resampling calculates number of effective particles, resampling is normalized.The present invention solves the problems, such as that invalid data bias factor caused by sensor and extraneous destabilizing factor causes path deviation.
Description
Technical field
The present invention relates to positioning and maps while a kind of localization for Mobile Robot technical field, specially mobile robot
The dynamic correcting method of establishment.
Background technology
Mobile robot realizes independent navigation by positioning and map building field (SLAM) technology.
In a practical situation, the elasticity of road surface, temperature, tire condition and crawler belt, the speed of driving influence used
Property navigation system error and sensitivity sonar sensor probe transmission data when the typing of invalid data information can occur.
The interference of unpredictable barrier can be occurred by also exploring circumstances not known, for example is encountered barrier and can not be moved, wheel slip, incident
Meet " kidnapping ", road sign is influenced by external force, when for localization for Mobile Robot and map building under circumstances not known, in wrong nothing
Accuracy in the case of effect data influence or external disturbance is poor.
For control and observation error invalid data screen with environment and the mechanism of dynamic change is particularly important.
So lacking a kind of dynamic corrections in path planning of mobile robot at present and forming the algorithm of new route.
Invention content
The purpose of the present invention is to provide positioning while a kind of mobile robot and map building correcting methods, to solve
The problem of positioning is caused to deviate with map influence due to the error component of invalid data caused by sensor and extraneous barrier.
To achieve the above object, the present invention provides the following technical solutions：It is fixed while a kind of mobile robot of the present invention
The dynamic correcting method of position and map building, includes the following steps：
Step1：Data acquisition verification,
Analysis and arrangement history message is recorded in caused bias factor when positioning is with map structuring immediately；
Step2：Adjustment revision parameter,
First, mobile robot is initialized, acquiescence initial position is zero-bit,
Then, according to the posture information at mobile robot t-1 moment, the i.e. posterior probability density function of t-1, by having
Priori the state at current time is predicted, t moment priori density function is obtained, according to priori probability density function
Acquisition particle collection is generated, N number of particle is acquired, it is 1/N to carry out initialization process to the weights of particle；
Fitness function is reset, the bias factor at t-1 moment and t moment observation, i.e. setting control error association side are passed through
Poor matrix QtWith observation error covariance matrix Rt,
Finally structure search more new model,
The discreet value that Q and R is searched out by improved fractional order particle cluster algorithm completes Q according to discreet valuetAnd Rt's
It corrects；
Step3：Simultaneous localization and mapping,
According to QtAnd RtData correlation is added in factor, pose realizes pose update, weight calculation, road sign update step are completed
The update of particle collection；
Double sampling calculates number of effective particles, resampling is normalized.
Further, in step step1, by dedicated laser radar and inertial navigation sensors gathered data, then
Calculation system is carried out to it, laser radar data converts distance and deflection, inertial navigation data conduct as observation data
Data are controlled, speed and steering angle, the bias factor of analysis and arrangement historical data are converted to.
Further, in step step2, adjustment revision parametric procedure is
1) it is to initialize position of mobile robot and map at the t-1 moment with previous position, is labeled as the first pose；
2) t moment pose is predicted under polar coordinates, is labeled as the second pose；
3) the error co-variance matrix Q of the first pose and the second pose is calculatedtWith error co-variance matrix RtFitness letter
Number；
4) by the data correlation of the first pose and the second pose；
5) discreet value that the t+1 moment is calculated according to search more new model, is labeled as third pose；
6) effective particle is generated with such, the probability distribution for showing observation is pushed away according to particle collection, carries out historical data
Adjustment, adjusting parameter.
Further, structure search more new model is the Q obtained using polar coordinate systemtAnd RtAdjust particle X[ex]It is as follows：
Wherein,Be arranged to initialize origin [0,0,0], based under cartesian coordinate system movement and observation model,
Movement and observation model under polar coordinates is as follows：
Further, the fitness function of Q and R is
Further, the third pose search process is
Wherein, α is exponent number, is adjusted according to the trace information of search dynamic,
Compared with prior art, the beneficial effects of the invention are as follows：
Setting dynamic revision fitness function, fractional order population search model, to improve in FastSLAM frames
Mobile robot path planning, dynamically screens and corrects the interference that invalid data is brought, improve the precision of layout of roads with
And stability, reliability.
Description of the drawings
Fig. 1 is the FastSLAM algorithm comparison figures based on different prioris of the present invention；
Fig. 2 is the present invention based on polar observation model figure；
Fig. 3 is the algorithm detail flowchart of the present invention；
Fig. 4 is two kinds of algorithms of the present invention based on the experimental result picture under wrong Q；
Fig. 5 is two kinds of algorithms of the present invention based on the experimental result picture under wrong R；
Fig. 6 is the root-mean-square error figure on robot pose under the wrong Q of the present invention；
Fig. 7 is the root-mean-square error figure in road sign position under the wrong Q of the present invention；
Fig. 8 is the root-mean-square error figure on robot pose under the wrong R of the present invention；
Fig. 9 is the root-mean-square error figure in road sign position under the wrong R of the present invention；
Figure 10 is two kinds of algorithms of the present invention based on the experimental result picture under wrong Q；
Figure 11 is two kinds of algorithms of the present invention based on the experimental result picture under wrong R；
Figure 12 is two kinds of algorithms of the present invention based on the experimental result picture under interim control mistake；
Figure 13 is the root-mean-square error figure on robot pose under the wrong Q of the present invention；
Figure 14 is the root-mean-square error figure in road sign position under the wrong Q of the present invention；
Figure 15 is the root-mean-square error figure in road sign position under the wrong R of the present invention；
Figure 16 is the root-mean-square error figure in road sign position under the wrong R of the present invention；
Figure 17 SLAM lab diagrams under " Car Park Dataset " data set for the Q based on mistake of the invention；
Figure 18 SLAM lab diagrams under " Car Park Dataset " data set for the R based on mistake of the invention.
Specific implementation mode
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, complete
Site preparation describes, it is clear that described embodiments are only a part of the embodiments of the present invention, instead of all the embodiments.It is based on
Embodiment in the present invention, it is obtained by those of ordinary skill in the art without making creative efforts every other
Embodiment shall fall within the protection scope of the present invention.
In order to preferably illustrate, wherein the priori of mistake is the invalid data or interference data of sensor typing, real
The derivation for applying priori and disclosed first example in example is to more preferably illustrating that the operability of those skilled in the art is said
It is bright.
Embodiment 1
As shown in Figs. 1-3, positioning and the dynamic of map building are entangled while present embodiments providing a kind of mobile robot
Correction method includes the following steps：
Step1：Data acquisition verification,
Analysis and arrangement history message is recorded in caused bias factor when positioning is with map structuring immediately；
Step2：Adjustment revision parameter,
First, mobile robot is initialized, acquiescence initial position is zero-bit,
Then, according to the posture information at mobile robot t-1 moment, the i.e. posterior probability density function of t-1, by having
Priori the state at current time is predicted, t moment priori density function is obtained, according to priori probability density function
Acquisition particle collection is generated, N number of particle is acquired, it is 1/N to carry out initialization process to the weights of particle；
Fitness function is reset, the bias factor at t-1 moment and t moment observation, i.e. setting control error association side are passed through
Poor matrix QtWith observation error covariance matrix Rt,
Finally structure search more new model,
The discreet value that Q and R is searched out by improved fractional order particle cluster algorithm completes Q according to discreet valuetAnd Rt's
It corrects；
Step3：Simultaneous localization and mapping,
According to QtAnd RtData correlation is added in factor, pose realizes pose update, weight calculation, road sign update step are completed
The update of particle collection；
Double sampling calculates number of effective particles, resampling is normalized.
In step step1, by dedicated laser radar and inertial navigation sensors gathered data, then to it into line number
It being handled according to change, laser radar data converts distance and deflection as observation data, and inertial navigation data is used as control data,
Be converted to speed and steering angle, the bias factor of analysis and arrangement historical data.
In step step2, adjustment revision parametric procedure is
1) it is to initialize position of mobile robot and map at the t-1 moment with previous position, is labeled as the first pose；
2) t moment pose is predicted under polar coordinates, is labeled as the second pose；
3) the error co-variance matrix Q of the first pose and the second pose is calculatedtWith error co-variance matrix RtFitness letter
Number；
4) by the data correlation of the first pose and the second pose；
5) discreet value that the t+1 moment is calculated according to search more new model, is labeled as third pose；
6) effective particle is generated with such, the probability distribution for showing observation is pushed away according to particle collection, carries out historical data
Adjustment, adjusting parameter.
The structure search more new model is the Q obtained using polar coordinate systemtAnd RtAdjust particle X[ex]It is as follows：
Wherein,Be arranged to initialize origin [0,0,0], based under cartesian coordinate system movement and observation model,
Movement and observation model under polar coordinates is as follows：
It closes on and observes z twicet-1And ztFitness function that inconsistency defines is realized；
It is assumed that mobile robot is in origin at the t-1 moment, pose and road sign position are all made of polar coordinates, in t-1
It carves, the position and steering angle under robot polar coordinates are 0；
zt-1For indicating cartographic information, map estimationIt presents with zt-1For the normal distribution of mean value：
Due to there is control and observation error, ztAnd zt-1It is usually it is difficult to consistent, based on the inconsistent fixed of front and back observation
Justice fitness function, it is the product of the estimation of robot pose and the observability estimate based on pose：
WithWhat is represented is that can make the maximum control error covariance of fitness function value, observation error covariance
With the pose of t moment robot.Indicate that robot estimates in the pose of t moment, robot pose estimationIt obeys
Following normal distribution：
Function f is the motion model under polar coordinate system, FuIt is the Jacobian matrix of function f, the observability estimate based on pose
Obey following normal distribution：
Function s is the observation model under polar coordinate system, based on Rao-Blackwellise decompose thought,It can be write as following form：
Function s first order Taylors are as follows：
WithIt is the t moment robot pose and observation information according to movement under polar coordinates and observation model prediction.It is the location estimation at j-th of road sign t-1 moment, Sx and Sm are observation functions about robot pose and road sign position
Jacobian matrix, by the approximate representation of observation function, observability estimate based on pose obeys mean value and isCovariance is Zt
Normal distribution：
The derivation respectively of estimation by mobile robot pose and the observability estimate based on pose, fitness function ytShape
Formula is as follows：
So the fitness function of the Q and R is
The third pose search process is
Wherein, α is exponent number, is adjusted according to the trace information of search dynamic,
Embodiment 2
The present embodiment provides the deductions of positioning and the dynamic correcting method of map building while a kind of mobile robot
Journey, each particle record current position, speed and the optimal location accessed, represent a kind of solution party of algorithm
Case, in iteration renewal process, each particle is towards moving in global optimum and the direction for oneself once reaching optimal location：
Xi(k)=Xi(k)+Vi(k+1)
Xi(k) indicate that position of i-th of particle in kth time iteration, particle are based on Vi(k+1) speed is from position Xi(k)
It flies to Xi(k+1) position, Vi(k+1) speed is made of inertia, cognition and social three parts, and inertia portion simulates small bird
Last running orbit, ω is inertia weight, represents influence of the pervious track to current new track, c1And c2It is cognition
The accelerator coefficient of part and social part, r1And r2The numerical value being randomly generated, pib(k) and pgb(k) small bird and bird are respectively represented
The optimal location that group had arrived at.
Slow, local convergence problem and Q based on traditional PS O algorithm the convergence speedtAnd RtThe multimode of the fitness function of adjustment
Characteristic, proposition are based on the population searching algorithm of Gr ü nwald-Letnikov form fractions ranks and fractal cloth to search for
OptimalWithSo that fitness function maximizes, the memory that the update of particle combines fractional calculus to have is special
Property, trace information has been incorporated, convergence speed of the algorithm is improved, fractal cloth is used to replace being uniformly distributed so that particle is certain
Can escape local minimum point under Probability Condition, improve the ability of searching optimum of particle so that search forWithMore connect
The actual value of nearly Q and R；
Gr ü nwald-Letnikov are a kind of widely used fractional calculus form of Definition, and discrete form is as follows：
Wherein α DtIndicate α rank calculus, it is fractional order integration that α, which is negative, and canonical is fractional order differential, and T is sampling interval, r
It is to block number, above-mentioned formula inertia weight ω is set as 1, following form can be written as：
Vi(k+1)-Vi(k)=φ1+φ2
Wherein φ1=c1r1(pib(k)-Xi(k)), φ2=c2r2(pgb(k)-Xi(k)), publicity left-hand component Vt+1,i-Vt,i
It is first-order difference, in PSO algorithms, particle flight is based on discrete time, time interval T=1, the difference of particle rapidity
Form is as follows：
αDtF (t)=φ1+φ2
According to the recurrence formula of formula and gamma function, 4 approximate expressions are as follows before difference formula：
α will dynamically be adjusted with the process of search.Exponent number α is adjusted according to the trace information dynamic of search.Initial value is
0.5, section is [0.4,0.8], and every 50 successive step is primary.
dgbestIt is the summation of optimal particle and each particle distance.dmaxIt is maximum distance between any two particle,
dminIt is distance between any two particle
The distribution of fractal cloth indicates that probability is close by scale factor, performance index, shift parameters and degree of bias parameter
Degree function can be defined with the continuous Fourier transform of characteristic function；Improved fractional order particle swarm optimization algorithm use divides shape
The method that distribution replaces generating random number in formula, generates the random number based on fractal cloth, and generating random number flow is as follows：
Firstly generate two independent stochastic variable V and W, V be oneThe equally distributed variable in section, W are
Then the exponential distribution variable that one mean value is 1 generates according to V and W and obeys S (α, β, 1,0) stochastic variable X：
Wherein Sα,βAnd Bα,βIt is defined as follows：
The stochastic variable Y for obeying S (α, β, σ, μ) is finally generated according to stochastic variable X：
It is proposed Lay dimension distribution of the algorithm using α=1.5, this distribution between Gaussian Profile α=2.0 and Cauchy be distributed α=
1.0；Larger value can be obtained under certain Probability Condition based on the random function of the distribution, so that particle has an opportunity
It escapes minimum point, expands its search range, algorithm description uses the fractional order grain based on Gr ü nwald-Letnikov forms
Subgroup searching algorithm search is optimalWithMake the maximum process of fitness function, detailed process as follows：
Input：
T moment mobile robot predicts pose(it is assumed that t-1 moment robots are in origin), the t-1 moment controls and sees
Survey covariance matrix Qt-1、Rt-1, the observation input z and z at t and t-1 momentt-1；
Step：
// initialization population
// whether terminated according to observation road sign quantity, search iteration number and convergent determination search
While (IsStop ()==false)
// fitness value calculated according to formula, update the optimal value and global optimum of each particle
CalculateEvaluation(particles)；
Fori=1toLength (particles)
UpdateVelocity(particles(i))；// according to formula (3-31) update particle rapidity
UpdatePosition(particles(i))；// according to formula (3-27) update particle position
endfor
endwhile
Output：
So that control and observation error covariance and t moment robot pose that fitness function is as big as possibleWith
Embodiment 3
The present embodiment provides positioning while a kind of mobile robot and the dynamic correcting method of map building, including it is following
Step：
Establish the movement for Qt and Rt adjustment particles and observation model；
The priori of proposition corrects FastSLAM algorithms and increases Q before particle updatetAnd RtThe process of adjustment, often
The method of a particle update and resampling still uses the algorithm of FastSLAM2.0；Due to QtAnd RtThe process of adjustment is similar to
The renewal process of FastSLAM2.0 particles will be used to adjust Q during adjustmenttAnd RtMobile robot pose and environmental information
(based on different coordinates and origin position) is used as a special particle X[ex]Treat, QtAnd RtAdjustment process is as follows：
1) it initializes：If the road sign quantity of this or last time observation is very few, control and observation error can not be determined
Closing on the corresponding influence observed twice in inconsistency.When the road sign quantity that this or last time are observed is less than 3, Q is not executedt
And RtAdjustment；Otherwise the position of mobile robot last moment is set as origin, steering angle is set as 0, and cartographic information is by upper one
Secondary observation zt-1It indicates：
2) pose is predicted：The pose of t moment robot is predicted according to the motion model under polar coordinates,
3) data correlation：The process is associated the road sign observed twice is closed on, and adjustment process uses dynamic syndicated
Nearest neighbor algorithm (dynamicjointnearestneighbor, DJNN) realizes data correlation, and the algorithm is in nearest neighbor algorithm base
It is improved on plinth, the interference situation between association matching result is eliminated by the correlation between observation, is passed through simultaneously
Pretreatment filters out the pseudo-characteristic in observational characteristic, not only increases the accuracy of data correlation, also improves the effect of data correlation
Rate；
4) optimized parameter is searched for by the fractional order population searching algorithm based on Gr ü nwald-Letnikov forms：It will
The pose of predictionLast moment Qt-1And Rt-1And observation ztAnd zt-1As input parameter, by being based on Gr ü nwald-
The fractional order population searching algorithm of Letnikov forms is searched for optimalWithSo that fitness function value is as far as possible
Greatly：
5) observation information of t moment is predicted in observation prediction according to robot pose and cartographic information：
6)QtAnd RtAdjustment：According to the robot pose of prediction and pass through particle group huntingIt is compared, works as the two
Between difference it is larger when, adjust QtAnd Rt
It is the observation of j-th of road sign,It is the observation of j-th of road sign prediction, MoIt is to close on to weigh twice
The road sign quantity observed again,
It usesIndicate kinematic error, Dxt~N (0,1) Normal Distribution, in order to avoid continually adjusting, only
Q is just adjusted in the case that certain probability event occurstIf | Dxt| it is more than 1.4 or less than 0.2, QtAdjustment is as follows：
Otherwise QtIt does not adjust：
Qt=Qt-1
ωqIt is the parameter for the road sign Number dynamics adjustment that a basis observes, settingIt represents j-th
The observation error of road sign, Mn are indicatedAbsolute value is more than 1.4 quantity, and Ln is indicatedAbsolute value is less than 0.2 number
Amount,
If MnpOr LnpMore than the threshold values N of settingmax, RtAdjustment is as follows：
Wherein, ωrIt is a regulation coefficient, works as M0When reduction, ωqAnd ωrIt reduces；Conversely, ωqAnd ωrIncrease；
5th step：Emulation experiment is carried out to model；
For adjusting QtAnd RtSpecial particle X[ex]Mobile robot pose and road sign position are indicated using polar coordinate system
It sets：
It is arranged to initialize origin [0,0,0], the movement under reference above-mentioned formula cartesian coordinate system and observation mould
Type, the movement and observation model under polar coordinates are as follows：
The robot location of predictionJ-th of road sign predicted positionWithThree points constitute triangle as shown in Figure 2
Shape：
WithIt is endpoint respectivelyWithAngle, the length on three sides is respectivelyWithPass through observation
Model calculates the predicted position of j-th of road sign：
It is calculated by triangle formulaWithDistanceThat is the pre- ranging of j-th of road sign of mobile robot t moment pair
From：
Endpoint is calculated by triangle formulaAngle
If γoIn [0, π] and [- 2 π ,-π] section,In endpointIt arrivesUnder line, thenIn section
If γoIn [π, 2 π] and the section [- π, 0],In endpointIt arrivesOn line, thenIn section
Consider the steering of mobile robot, the observation angle of j-th of road sign of t moment pair predictionFor：
6th step：Experimental result and analysis：For the performance of testing algorithm, the comparison of design and simulation environmental experiment
FastSLAM2.0 and be based on modified the improvements FastSLAM algorithm flows of priori, simulated environment based on Matlab2013 put down
The movement velocity of platform, mobile robot is 3 meter per seconds, steering locking angleRadian.Velocity error ε v are 0.3 meter per seconds, turn to and turn
Angle error isRadian, robot are configured with π radians, the laser radar that farthest observed range is 30 meters, the range error of observation
It it is 0.2 meter, angular error isRadian：
Embodiment 4
Experiment under sparse road sign environment；
One size of design is the environment that 250m × 200m include 35 road signs, the initial position of mobile robot for (0,
0), priori Q is wrong, is amplified 6 times, R is correct.Based on this setting, be respectively adopted FastSLAM2.0 algorithms and
The road sign in the path and environment of mobile robot is estimated based on priori modified improvement FastSLAM algorithm flows
Meter, the number of particles that two kinds of algorithms use is 20, the simulation experiment result as shown in figure 4,
Wherein, the actual position of+number expression road sign,
Zero indicates to propose algorithm road sign estimated location,
Indicates FastSLAM2.0 algorithm road sign estimated locations,
Solid line indicates the movement locus of mobile robot,
Dotted line represents the estimation for proposing algorithm and FastSLAM2.0 algorithms for mobile robot path；
Since Q is amplified in simulated environment, so the position of mobile robot estimation will be closer to the information of observation,
This to trust the accuracy for reducing SLAM and estimating to the excessive of observation information, in this experiment simulation environment, robot can
The road sign observed is in most cases at 3 or so, although not being very fully, after iteration adjustment several times
QtAnd RtValue also can carry out 50 emulation experiment comparisons, warp close to actual value, in order to assess the estimation for priori
The adjustment for spending early period, for QtMost amendment can reach 85% or more accuracy, propose that algorithm is first by early period
The amendment for testing knowledge improves the precision of SLAM estimations, as can be seen from the figure proposes algorithm ratio FastSLAM2.0 algorithms in machine
It is significantly improved in device people pose and road sign position estimated accuracy；
Based on simulated environment identical with Fig. 4, setting priori R is wrong, is amplified 6 times, Q is correct, is made
With FastSLAM2.0 algorithms and based on the modified path and environment for improving FastSLAM algorithms to mobile robot of priori
In road sign estimated that, since priori R is amplified, the estimation of robot pose is closer to the knot predicted by motion model
Fruit, this excessive trust to control information reduce the accuracy of estimation, and propose that algorithm passes through and introduce priori amendment
Link improves the precision of SLAM estimations；The simulation experiment result of Fig. 5 can be seen that based on the modified improvement of priori
FastSLAM algorithms are better than FastSLAM2.0 algorithms；
Since control and observation noise randomly generate in simulated environment, the result of each emulation experiment all differs
Sample, in order to obtain more accurate comparison, with root-mean-square error (RMSE) for standard, execute 50 emulation experiments；The display of table 1 is imitative
True experiment as a result, wherein RMSE_P indicate location estimation root-mean-square error, RMSE_L indicate road sign estimation root-mean-square error,
Algorithm is proposed in order to obtain more accurate SLAM estimations, introduces QtAnd RtAdjustment, increase the execution time of algorithm；
Emulation experiment of the table 1 under sparse road sign environment
In 50 emulation experiments based on wrong priori Q, mobile robot has moved about 220 seconds every time, and Fig. 6 is aobvious
Shown that two kinds of algorithms are based on time root-mean-square error curve graph, included fourth officer subgraph in figure, be respectively X-axis, Y-axis, deflection and
The case where error of position changes over time, mobile robot observe the road that initial time observes again at the eleventh hour
Mark, starts to reduce so as to cause pose evaluated error, is as can be seen from the figure based on the modified improvement FastSLAM of priori
Algorithm ratio FastSLAM2.0 algorithms are in all periods all closer to the true pose of mobile robot；Fig. 7 shows moving machine
30 road signs that device people observes final moment location estimation root-mean-square error, as can be seen from the figure propose algorithm for
The estimation of road sign position is better than FastSLAM2.0 algorithms：
Fig. 8 shows that mobile robot pose of two kinds of algorithms Jing Guo 50 emulation experiments at wrong priori R is square
Root error curve diagram, Fig. 9 show the root-mean-square error of 30 road sign positions estimation, as can be seen from the figure propose algorithm ratio
The root-mean-square error of FastSLAM2.0 algorithms estimation wants small, and estimation is more accurate.
Embodiment 5
Experiment under intensive road sign environment：
It includes 135 roads target area that its simulated environment, which is 250m × 200m, and setting priori R is mistake
, be amplified 6 times, Q is correct, be respectively adopted in such circumstances FastSLAM2.0 algorithms and based on priori it is modified
It improves FastSLAM algorithms and Figure 10, which shows experimental result, to be estimated to the road sign in the path and environment of mobile robot；
Since Q is amplified 6 times, the estimation of mobile robot pose and road sign closer to observation information, it is this to observation information
Excessively trusting reduces the accuracy of estimation；In intensive road sign simulated environment, the road sign that robot can observe is above
10, the adjustment that sufficient information is used for priori is provided, so the Q after being adjusted in innovatory algorithmtAnd RtIt is in close proximity to
Actual value；In order to assess the estimation for priori, 50 emulation experiment comparisons are carried out；Wherein 43 times experiments change by 32 times
For R after generationtEvaluated error within 10%, for QtEvaluated error within 20%, propose algorithm by repairing early period
The more accurate estimation for priori is just being obtained, the precision of robot location's estimation is improved, it can from figure
Go out to propose that algorithm ratio FastSLAM2.0 algorithms are significantly improved on robot pose and road sign position estimated accuracy；
Figure 11 shows that, based on the simulation experiment result under wrong priori R, R is amplified 6 times, and the R being amplified causes pair
The excessive of control information trusts the accuracy for reducing estimation, and especially in the environment of intensive road sign, Figure 11 demonstrates proposition
Algorithm can improve the estimated accuracy of SLAM by the amendment of priori；
Under intensive road sign environment, simulation causes to control the interim great increased feelings of error due to various inside and outsides
Condition, in emulation experiment, mobile robot actual motion speed when 4 to 8 seconds is 0.1 meter per second, and the speed that sensor obtains
Degree is still 3 meter per seconds, and the speed of service is restored to 3 meter per seconds after 8 seconds, and this kind of situation is also frequently encountered in true environment, than
If robot hinders advance by certain objects, although the speed of trailing wheel does not change, actual speed is slack-off, in SLAM
When iteration updates mobile-robot system state, the road sign that can be observed is still relatively more sufficient, so can adjust quickly
QtAnd RtValue；Figure 12 shows experimental result, it can be seen that based on the modified improvement FastSLAM algorithms of priori on enough roads
In the case of mark observation, the estimation of SLAM is not influenced substantially by external disturbance, and FastSLAM2.0 algorithms are due to interim
Control mistake causes estimated accuracy to be substantially reduced；
Two kinds of algorithms are obtained under intensive road sign environment more accurately to compare, and execute 50 emulation experiments；The display experiment of table 2
As a result, wherein RMSE_P indicates the root-mean-square error of location estimation, RMSE_L indicates the root-mean-square error of road sign estimation, from table
It can be seen that being estimated based on the modified improvement FastSLAM algorithms of priori either robot location's estimation or road sign position
Meter is better than FastSLAM2.0 algorithms；
Emulation experiment of the table 3. 2 under intensive road sign environment
In 50 emulation experiments based on wrong priori Q, robot moves 220 seconds left sides in simulated environment every time
It is right.Figure 13 shows two kinds of algorithm X-axis, Y-axis, deflection and position root-mean-square error time history plot.From figure
It can be seen that proposing that algorithm is better than FastSLAM2.0 algorithms in the estimation of mobile robot pose, Figure 14 shows road sign position
Root-mean-square error curve graph, it is the same with the pose of robot, propose algorithm road sign position evaluated error again smaller than
FastSLAM2.0 algorithms；
Refering to fig. 15：Show two kinds of algorithms 50 emulation experiments of process of wrong priori R root-mean-square error,
Since road sign quantity is more in simulated environment, so influence biggers of the mistake priori R for algorithm accuracy, from Figure 15 and
In 16 it can be seen that at wrong priori R, propose that algorithm has higher for the estimation of robot pose and road sign position
Precision；It can be verified by emulation experiment above and propose that algorithm can obtain more based on wrong priori
Accurate estimation, while being blocked for such as barrier is encountered, wheel slip leads to the control data and reality that robot measures
The problem of having a long way to go can also accomplish to correct as far as possible.
Embodiment 6
Feasibility in true environment and validity use a standard data set of SLAM research fields
" UniversityCarPark " is tested；The dataset acquisition from the intelligent vehicle of Sydney University ACFR,
Parking lot environment is about 45 × 30m2, placed some artificial landmarks, intelligent vehicle low speed row in parking lot
It sails, has recorded inertial navigation sensor, laser radar and GPS in the measured value at each moment, observed by inertial navigation system
Measured value substitute into formula can predict mobile robot subsequent time pose, by Laser Radar Observation to data repair
Positive mobile robot pose and map estimation；
The two kinds of algorithms of display of Figure 17 and 18 are based on different priori situations under " UniversityCarPark " data set
Under experimental result：Priori Q, Q of the Figure 17 based on mistake are amplified 3 times, and Figure 18 is priori R, the R quilt based on mistake
3 times of amplification, it can be seen from the figure that the path ratio FastSLAM2.0 algorithms of innovatory algorithm estimation are closer to GPS, estimation
Road sign position is also closer to the true position of road sign：
Table 3 is shown to be less than based on the modified road sign error for improving the estimation of FastSLAM algorithms of priori
FastSLAM2.0 algorithms, due to increasing adjustment link, run time is also greater than FastSLAM2.0 algorithms, due to the data set
In the road sign quantity that observes and few, propose that algorithm has invoked 57 times in total, it is 0.0020 second that single step, which executes the time, is less than
In 0.025 second sampling period, it disclosure satisfy that requirement of real-time：
The road sign position error of 3 two kinds of algorithms of table
In summary：It is provided by the invention a kind of based on the modified improvement FastSLAM algorithms of priori, for mistake
Priori influences the problem of SLAM estimated accuracies, proposes that algorithm will move and the covariance of observation noise is set as dynamic,
The modified process of covariance is dissolved into FastSLAM frames；It closes on observation inconsistency twice and is defined as fitness function
It can more reflect true error situation, be searched using the fractional order population searching algorithm based on Gr ü nwald-Letnikov forms
The discreet value of rope priori, search process are incorporated historical track information and improve convergence rate, carried using Alpha fractal cloth
High ability of searching optimum, emulation and the experiment of the fields SLAM standard data set " UniversityCarPark " show in mistake
Accidentally in the case of priori, propose that algorithm can improve localization for Mobile Robot and the precision of map building.
The foregoing is only a preferred embodiment of the present invention, but scope of protection of the present invention is not limited thereto,
Any one skilled in the art in the technical scope disclosed by the present invention, according to the technique and scheme of the present invention and its
Inventive concept is subject to equivalent substitution or change, should be covered by the protection scope of the present invention.
Claims (6)
1. the dynamic correcting method of positioning and map building while a kind of mobile robot, which is characterized in that including following step
Suddenly：
Step1：Data acquisition verification,
Analysis and arrangement history message is recorded in caused bias factor when positioning is with map structuring immediately；
Step2：Adjustment revision parameter,
First, mobile robot is initialized, acquiescence initial position is zero-bit,
Then, according to the posture information at mobile robot t-1 moment, the i.e. posterior probability density function of t-1, pass through existing elder generation
It tests knowledge to predict the state at current time, obtains t moment priori density function, generated according to priori probability density function
Particle collection is acquired, N number of particle is acquired, it is 1/N to carry out initialization process to the weights of particle；
Fitness function is reset, the bias factor at t-1 moment and t moment observation, i.e. setting control error covariance square are passed through
Battle array QtWith observation error covariance matrix Rt,
Finally structure search more new model,
The discreet value that Q and R is searched out by improved fractional order particle cluster algorithm completes Q according to discreet valuetAnd RtRepair
Just；
Step3：Simultaneous localization and mapping,
Data correlation is added according to Qt and Rt factors, pose realizes pose update, weight calculation, road sign update step complete particle
The update of collection；
Double sampling calculates number of effective particles, resampling is normalized.
2. the dynamic correcting method of positioning and map building while a kind of mobile robot as described in claim 1, special
Sign is：In step step1, by dedicated laser radar and inertial navigation sensors gathered data, then to it into line number
It being handled according to change, laser radar data converts distance and deflection as observation data, and inertial navigation data is used as control data,
Be converted to speed and steering angle, the bias factor of analysis and arrangement historical data.
3. the dynamic correcting method of positioning and map building while a kind of mobile robot as described in claim 1, special
Sign is：In step step2, adjustment revision parametric procedure is
1) it is to initialize position of mobile robot and map at the t-1 moment with previous position, is labeled as the first pose；
2) t moment pose is predicted under polar coordinates, is labeled as the second pose；
3) the error co-variance matrix Q of the first pose and the second pose is calculatedtWith error co-variance matrix RtFitness function；
4) by the data correlation of the first pose and the second pose；
5) discreet value that the t+1 moment is calculated according to search more new model, is labeled as third pose；
6) effective particle is generated with such, the probability distribution for showing observation is pushed away according to particle collection, carries out the adjustment of historical data,
Adjusting parameter.
4. the dynamic correcting method of positioning and map building while a kind of mobile robot as claimed in claim 3, special
Sign is：The structure search more new model is the Q obtained using polar coordinate systemtAnd RtAdjust particle X[ex]It is as follows：
Wherein,Be arranged to initialize origin [0,0,0], based under cartesian coordinate system movement and observation model, pole sit
Movement and observation model under mark is as follows：
5. the dynamic correcting method of positioning and map building while a kind of mobile robot as claimed in claim 3, special
Sign is that the fitness function of Q and R are
6. the dynamic correcting method of positioning and map building while a kind of mobile robot as claimed in claim 3, special
Sign is that the third pose search process is
Wherein, α is exponent number, is adjusted according to the trace information of search dynamic,
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201810574387.XA CN108645413A (en) | 2018-06-06 | 2018-06-06 | The dynamic correcting method of positioning and map building while a kind of mobile robot |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201810574387.XA CN108645413A (en) | 2018-06-06 | 2018-06-06 | The dynamic correcting method of positioning and map building while a kind of mobile robot |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN108645413A true CN108645413A (en) | 2018-10-12 |
Family
ID=63751893
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201810574387.XA Withdrawn CN108645413A (en) | 2018-06-06 | 2018-06-06 | The dynamic correcting method of positioning and map building while a kind of mobile robot |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN108645413A (en) |
Cited By (14)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110031880A (en) * | 2019-04-16 | 2019-07-19 | 杭州易绘科技有限公司 | High-precision augmented reality method and apparatus based on Geographic mapping |
| CN110260867A (en) * | 2019-07-29 | 2019-09-20 | 浙江大华技术股份有限公司 | Method, equipment and the device that pose is determining in a kind of robot navigation, corrects |
| CN110320520A (en) * | 2019-06-26 | 2019-10-11 | 哈尔滨工程大学 | A kind of robust rear end figure optimization method depth measurement synchronizing information positioning and build figure |
| CN111238470A (en) * | 2020-01-09 | 2020-06-05 | 哈尔滨工程大学 | Intelligent glasses road planning method, medium and equipment under artificial intelligence big data |
| CN111421548A (en) * | 2020-04-21 | 2020-07-17 | 武汉理工大学 | Mobile robot positioning method and system |
| CN111829509A (en) * | 2020-07-20 | 2020-10-27 | 深圳名仕堂贸易有限公司 | New energy automobile positioning method and positioning device |
| CN112014854A (en) * | 2020-08-31 | 2020-12-01 | 华通科技有限公司 | Positioning method based on particle swarm positioning algorithm |
| CN112084937A (en) * | 2020-09-08 | 2020-12-15 | 清华大学 | Dynamic vehicle detection method |
| CN112684728A (en) * | 2019-10-17 | 2021-04-20 | 广东原点智能技术有限公司 | Robot stacking control method based on laser SLAM |
| WO2021077769A1 (en) * | 2019-10-25 | 2021-04-29 | 江苏海事职业技术学院 | Streaming computing-based map creation method and system therefor |
| CN112732854A (en) * | 2021-01-11 | 2021-04-30 | 哈尔滨工程大学 | Particle filtering BSLAM method |
| CN113126613A (en) * | 2019-12-30 | 2021-07-16 | 南京德朔实业有限公司 | Intelligent mowing system and autonomous mapping method thereof |
| CN114543807A (en) * | 2022-01-14 | 2022-05-27 | 安徽海博智能科技有限责任公司 | High-precision evaluation method for SLAM algorithm in extreme scene |
| CN116822773A (en) * | 2023-08-30 | 2023-09-29 | 山东福富新材料科技有限公司 | Freight path prediction method and system based on big data |
-
2018
- 2018-06-06 CN CN201810574387.XA patent/CN108645413A/en not_active Withdrawn
Cited By (20)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110031880A (en) * | 2019-04-16 | 2019-07-19 | 杭州易绘科技有限公司 | High-precision augmented reality method and apparatus based on Geographic mapping |
| CN110320520A (en) * | 2019-06-26 | 2019-10-11 | 哈尔滨工程大学 | A kind of robust rear end figure optimization method depth measurement synchronizing information positioning and build figure |
| CN110260867A (en) * | 2019-07-29 | 2019-09-20 | 浙江大华技术股份有限公司 | Method, equipment and the device that pose is determining in a kind of robot navigation, corrects |
| CN112684728A (en) * | 2019-10-17 | 2021-04-20 | 广东原点智能技术有限公司 | Robot stacking control method based on laser SLAM |
| CN112684728B (en) * | 2019-10-17 | 2021-09-28 | 广东原点智能技术有限公司 | Robot stacking control method based on laser SLAM |
| WO2021077769A1 (en) * | 2019-10-25 | 2021-04-29 | 江苏海事职业技术学院 | Streaming computing-based map creation method and system therefor |
| CN113126613A (en) * | 2019-12-30 | 2021-07-16 | 南京德朔实业有限公司 | Intelligent mowing system and autonomous mapping method thereof |
| CN111238470A (en) * | 2020-01-09 | 2020-06-05 | 哈尔滨工程大学 | Intelligent glasses road planning method, medium and equipment under artificial intelligence big data |
| CN111238470B (en) * | 2020-01-09 | 2023-05-02 | 中耘(广东)信息科技有限公司 | Intelligent glasses road planning method, medium and equipment under artificial intelligent big data |
| CN111421548A (en) * | 2020-04-21 | 2020-07-17 | 武汉理工大学 | Mobile robot positioning method and system |
| CN111421548B (en) * | 2020-04-21 | 2021-10-19 | 武汉理工大学 | Mobile robot positioning method and system |
| CN111829509A (en) * | 2020-07-20 | 2020-10-27 | 深圳名仕堂贸易有限公司 | New energy automobile positioning method and positioning device |
| CN111829509B (en) * | 2020-07-20 | 2021-09-03 | 泉州森泸玩具有限公司 | New energy automobile positioning method and positioning device |
| CN112014854A (en) * | 2020-08-31 | 2020-12-01 | 华通科技有限公司 | Positioning method based on particle swarm positioning algorithm |
| CN112084937A (en) * | 2020-09-08 | 2020-12-15 | 清华大学 | Dynamic vehicle detection method |
| CN112732854A (en) * | 2021-01-11 | 2021-04-30 | 哈尔滨工程大学 | Particle filtering BSLAM method |
| CN114543807A (en) * | 2022-01-14 | 2022-05-27 | 安徽海博智能科技有限责任公司 | High-precision evaluation method for SLAM algorithm in extreme scene |
| CN114543807B (en) * | 2022-01-14 | 2023-10-20 | 安徽海博智能科技有限责任公司 | High-precision evaluation method of SLAM algorithm in extreme scene |
| CN116822773A (en) * | 2023-08-30 | 2023-09-29 | 山东福富新材料科技有限公司 | Freight path prediction method and system based on big data |
| CN116822773B (en) * | 2023-08-30 | 2023-11-14 | 山东福富新材料科技有限公司 | Freight path prediction method and system based on big data |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN108645413A (en) | The dynamic correcting method of positioning and map building while a kind of mobile robot | |
| CN109828592B (en) | A kind of method and apparatus of detection of obstacles | |
| CN106873585B (en) | A kind of navigation method for searching, robot and system | |
| Chen et al. | A hybrid prediction method for bridging GPS outages in high-precision POS application | |
| Li et al. | GPS/INS/Odometer integrated system using fuzzy neural network for land vehicle navigation applications | |
| CN110362089A (en) | A method of the unmanned boat independent navigation based on deeply study and genetic algorithm | |
| CN106772524B (en) | A kind of agricultural robot integrated navigation information fusion method based on order filtering | |
| CN103743402B (en) | A kind of underwater intelligent self adaptation Approach of Terrain Matching of topographic information based amount | |
| CN107450593A (en) | A kind of unmanned plane autonomous navigation method and system | |
| Bird et al. | Closing the loop in dynamic soaring | |
| CN104236548A (en) | Indoor autonomous navigation method for micro unmanned aerial vehicle | |
| CN111538241B (en) | Intelligent control method for horizontal track of stratospheric airship | |
| CN109445456A (en) | A kind of multiple no-manned plane cluster air navigation aid | |
| CN105447574B (en) | A kind of auxiliary blocks particle filter method, device and method for tracking target and device | |
| Zhang et al. | A hybrid intelligent algorithm DGP-MLP for GNSS/INS integration during GNSS outages | |
| CN110081875B (en) | Unmanned aerial vehicle autonomous navigation system and method imitating pigeon intelligence | |
| CN110779519B (en) | Underwater vehicle single beacon positioning method with global convergence | |
| CN109740209A (en) | Hypersonic aircraft on-line parameter identification method and the mechanical model for using it | |
| CN108492324A (en) | Aircraft method for tracing based on fully-connected network and Kalman filter | |
| CN107066806A (en) | Data Association and device | |
| CN106679672A (en) | AGV (Automatic Guided Vehicle) location algorithm based on DBN (Dynamic Bayesian Network) and Kalman filtering algorithm | |
| CN110906933A (en) | AUV (autonomous Underwater vehicle) auxiliary navigation method based on deep neural network | |
| CN110530424A (en) | A kind of aerial target Method of Sensor Management based on air threat priority | |
| CN109855623A (en) | Geomagnetic model online approximating method based on Legendre multinomial and BP neural network | |
| CN108871365A (en) | Method for estimating state and system under a kind of constraint of course |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| WW01 | Invention patent application withdrawn after publication | ||
| WW01 | Invention patent application withdrawn after publication |
Application publication date: 20181012 |