CN109358316A - Line laser global localization method based on structural unit coding and multiple hypotheis tracking - Google Patents
Line laser global localization method based on structural unit coding and multiple hypotheis tracking Download PDFInfo
- Publication number
- CN109358316A CN109358316A CN201811306776.0A CN201811306776A CN109358316A CN 109358316 A CN109358316 A CN 109358316A CN 201811306776 A CN201811306776 A CN 201811306776A CN 109358316 A CN109358316 A CN 109358316A
- Authority
- CN
- China
- Prior art keywords
- structural unit
- pose
- global
- sues
- feature
- 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.)
- Granted
Links
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S5/00—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations
- G01S5/16—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations using electromagnetic waves other than radio waves
Landscapes
- Physics & Mathematics (AREA)
- Electromagnetism (AREA)
- Engineering & Computer Science (AREA)
- General Physics & Mathematics (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
- Image Analysis (AREA)
Abstract
Line laser global localization method based on structural unit coding and multiple hypotheis tracking.The task of Global localization is provided for using line laser for mobile robot, present invention proposition one is novel to be based on SUES (structural unit encoding mechanism, structural unit encoding scheme) and extension MHT (multiple hypotheis tracking, multiple hypothesis tracking) global localization method.Oriented endpoint feature is encoded by SUES first；Extension MHT is recycled gradually to restore global pose.Compared to existing method, the SUES that the present invention is mentioned is without off-line training, independent of dictionary quality；Meanwhile the extension MHT method mentioned has the advantages that convergence faster and does not depend on feature quantity.The experimental results showed that the present invention has higher success rate and efficiency compared to adaptive Monte Carlo localization.
Description
Technical field
The invention belongs to the technical fields of Environment identification and localization for Mobile Robot, are based on structure list more particularly to one kind
The line laser global localization method of primitive encoding and multiple hypotheis tracking.
Background technique
SLAM (while positioning and building figure, simultaneous localization and mapping) is as intelligent machine
The indispensable key technology in device people field constructs ambient enviroment map by airborne sensor and provides positioning to robot simultaneously
Information [1].By current SLAM technology, environmental map can be obtained；On the basis of obtaining priori map, by vision or
Person's laser sensor provides robust positioning for mobile robot to complete complex task, is that current mobile robot is indispensable
Function [2].However orientation problem not only includes posture tracking, needs to solve in many cases under no initial pose
Orientation problem, for example kidnap, here it is so-called Global localization problems.Compared to posture tracking, Global localization problem is more
It is challenging, since it is desired that solving the problems, such as Environment identification, and there may be multiple similar scenes in entire environment.
During Global localization, solves Environment identification and expression is a vital step.In visual field,
[3] such as Galvez-L ó pez propose a kind of novel bag of words Environment identification method, detect for winding.Later, in text
It offers in [4], Mur-Artal et al. proposes the ORB-SLAM that bag of words are used for winding detection.Document [5] is proposed using complete
Scape gist (outline) descriptor solves the orientation problem in city.Different from visual sensor, line laser can provide more stable
Environmental information and the bigger observation visual field, thus by Environment identification is largely used to.Tipaldi et al. [6] proposes the first
Laser feature-FLIRT (transformation of fast laser area-of-interest, Fast Laser Interest Region Transform),
It is successfully applied to Environment identification.Document [7] solves the environment under large scene using the structural testing of FLIRT feature and reduction
Identification problem.Deray et al. [8] proposes a kind of novel winding detection method, this method combine improved BoW mechanism and
HMM (hidden Markov model, Hidden Markov Model).There is still a need for off-line trainings to generate dictionary for the above method.
Global localization can be divided into based on filtering and being based on two methods of feature, all be widely used in mobile robot neck
Domain.In document [9], Mart í n et al. proposes a kind of based on KLD (KL divergence, Kullback-Leibler divergence)
Global localization method；Fox [10] proposes a kind of AMCL (adaptive Monte Carlo localization, adaptive Monte Carlo
Localization) method, this method improve the efficiency of particle filter by sampling using KLD.Different from traditional side MCL
Method, Valencia et al. propose NDT-MCL (normal distribution transform-Monte Carlo localization method, Normal in document [11]
Distribution Transform Monte Carlo Localisation), in less particle and faster convergence rate
Lower acquisition better performance.The above-mentioned method based on filtering, does not need to recognize current environment, is reached by particle convergence
The effect of Global localization, but particle convergence leads to lower efficiency, while being easy to cause faulty convergence in similar area.Tully
Et al. [12] combine Map Expression technology is mixed with MHT, propose a kind of unified robot global and position filter frame.
He et al. [13] proposes a kind of movement using LRM (line segment relationship match, Line-segment Relation Matching)
Robot global location algorithm.In document [14], Liu et al. people proposes a kind of feasible room-level location solution, leads to
Cross the realization of structuring core sparse coding model.Park et al. proposes one kind based on support vector machines (SVM) in document [15]
The global localization method of position identification and particle filter, the research approach improve the efficiency of Global localization by off-line training.
It is also mature far away to the research of global location although the above method makes great progress.For based on filter
The method of wave device, it usually needs a large amount of particles realize Global localization, this cause higher time overhead and carrying cost and
Low constringency performance.In addition, very likely converging to mistake in the environment with many similar local scenes based on the method for filtering
Result accidentally.For the method based on feature, the selection of characteristic type and descriptor is extremely important, it usually needs off-line training produces
New word allusion quotation is for describing environment.It is especially challenging in dynamic environment, it possibly is present on dynamic object, leads because being characterized
Cause data correlation difficult.Document is summarized it is known that existing method is perhaps in Environment identification or in the success rate of Global localization
It is short of with existing in efficiency, it is difficult to practical application.
Summary of the invention
Present invention aim to address existing Global localization technology above shortcomings, provide a kind of based on structural unit
The line laser global localization method of coding and multiple hypotheis tracking.
The invention proposes a kind of novel line laser Global localization sides based on structural unit coding and multiple hypotheis tracking
Method.The feature of this method maximum includes not needing off-line training to generate dictionary, thus the bad shadow of quality for avoiding offline dictionary
Ring Global localization effect.Secondly, distinguishing traditional multiple hypotheis tracking the invention proposes a kind of extension multiple hypotheis tracking algorithm and calculating
Method, using independent observation as prior information, and by continuous candidate pose and odometer increment as likelihood information, compared to biography
System MHT method, has the advantages that convergence rate faster and independent of feature quantity.Specifically, passing through line segment feature first
It extracts and obtains oriented endpoint feature, the geometrical relationship between oriented endpoint feature is then described by structural unit；Later, it utilizes
The SUES proposed carries out the feature vector that coding obtains laser data to structural unit, obtains candidate pass with priori map match
Key frame and posture information；Gradually restore global pose finally by the extension MHT proposed.Total algorithm process such as 1 institute of attached drawing
Show.By public testing collection and from record data set the experimental results showed that, compared to AMCL (adaptive Monte Carlo localization,
Adaptive Monte Carlo localization), the present invention has higher success rate and efficiency.
It is provided by the invention to include: based on structural unit coding and the line laser global localization method of multiple hypotheis tracking
1st, the geometrical relationship between oriented endpoint feature is encoded using SUES
1.1st construction generates structural unit
After carrying out line segment feature extraction to laser data, (tool can be described in entire environment by oriented endpoint feature
Body can be shown in attached drawing 2).Define P={ p1,...,pNIt is oriented endpoint feature point set, wherein N indicates characteristic point quantity.I-th
Oriented endpoint feature is with pi=[xi yi θi]TIt indicates, wherein (xi,yi) it is defined as feature locations, θiIt is defined as characteristic direction.Together
The direction of the oriented endpoint feature of one line segment two sides differs 180 °.Define ρjIt is characterized point pl=[xl yl θl]TWith characteristic point pn
=[xn yn θn]TBetween Euclidean distance, wherein j be index number,
When meeting ρjWhen≤ξ, definition structure unit sj={ ρj,θj1,θj2Indicate characteristic point plAnd pnBetween geometrical relationship
(specific descriptions can be shown in attached drawing 3), wherein ξ is given threshold value, θj1And θj2It is characterized point plAnd pnBetween directional information, calculate
It is as follows:
Wherein θ 'l,θ′nFor directional information, be located at section [0,360 °).Define S={ s1,…,sMIt is the knot that construction obtains
Structure unit set, wherein M is defined as structural unit quantity, is obtained according to above-mentioned make.Such structural unit construction side
Formula can also distinguish similar structure while guaranteeing rotational invariance, as shown in Fig. 4, four class formation unit Europe having the same
Formula distance, but remain to distinguish these four types of similar structures by angle information.
1.2nd encodes structural unit by SUES
The present invention proposes a kind of novel encoding mechanism SUES (structural unit encoding mechanism, structural unit
Encoding scheme), for being encoded to structural unit obtained above.SUES does not need the offline instruction of priori data
Practice and generate dictionary, while the performance of SUES is also not dependent on dictionary quality.Define δaAnd δdRespectively angular resolution and distance point
Entire angular region and metric space are divided into small grid with above-mentioned resolution ratio by resolution.On this basis, structural unit can be with
It is described by the corresponding particular grid of angle and distance, however such hard coded mechanism is easy the angle near cut-off rule
Degree and distance are divided into different grids.As shown in Fig. 5, the present invention proposes a kind of soft encoding mechanism, and angle, θ can cause point
1 δ of secantaWith 2 δaResponse, equally distance ρ can also cause same effect.Define wiFor by 2 angle cut-off rules and
1 is combined the structure words to be formed apart from cut-off rule, and wherein i is index value, can generate L=b in total2D structure words,
InUsing the soft voting mechanism based on cut-off rule, by the structural unit s of generationj={ ρj,
θj1,θj2Encoded, the segmentation line computation generated first according to soft voting mechanism is as follows:
Wherein φi(i=1 ..., 6) it is specific cut-off rule index value.Then according to determining apart from size apart from cut-off rule
Determine the weight of each cut-off rule, weight calculation corresponding to cut-off rule is as follows:
Wherein ωφi(i=1 ..., 6) it is weight corresponding to cut-off rule.Finally by different groups between cut-off rule
It closes, each structural unit sj={ ρj,θj1,θj2It can produce 8 structure words, the corresponding index value of each structure words and power
Weight is as shown in table 1 below, wherein aijIndicate counter structure word wiIn structural unit sjWeight, i be structure words in dictionary
Index value.
In kth moment laser data, there can be multiple structural units, generate multiple structure words, therefore define aijkFor
Structure words w in kth moment laser dataiIn structural unit sjIn weight, aikThen indicate structure words wiSwash at the kth moment
The weight of light data, aikIt calculates as follows:
1. structural unit s of tablejCorresponding 8 structure words
All structural units are subjected to coding renormalization in this way, it will be able to obtain corresponding to kth moment laser data
Feature vector calculates as follows:
Therefore, defined feature vector vk=[tf1k,tf2k,...,tfLk]T(L is structure words quantity) is for when describing kth
Carve the structural information of laser data.It can use following formula later and seek feature vector v1With feature vector v2Between similarity, meter
It calculates as follows:
2nd, gradually restore global pose by extending MHT
2.1st establishes more hypothesis probabilistic models
By SUES to present laser data carry out cellular construction feature coding, then by similarity calculation can from from
Candidate key-frames are obtained in line pose map and obtain candidate global pose.It is different from traditional MHT method, the present invention is by independent sight
The global posture information of the candidate obtained is surveyed as priori item, by continuous candidate pose and odometer increment information as likelihood
?.Extension MHT (multiple hypotheis tracking, the multiple hypothesis tracking) method proposed is aobvious with following two
The advantages of work: (1) convergence rate is faster；(2) acquisition of candidate pose is independently of feature quantity.About k moment robot pose
The probability Distribution Model of sequence is expressed as follows:
p(X1:k|u1:k-1,z1:k,m), (10)
Wherein u1:k-1It is defined as odometer information sequence；z1:kIndicate observation sequence, while X1:kCandidate machine is respectively indicated with m
Device people's pose sequence and priori map.In order to reduce calculation amount, entire distribution is calculated using following recurrence:
p(X1:k|u1:k-1,z1:k, m) and=p (Xk|uk-1,zk,m,X1:k-1)·p(X1:k-1|u1:k-2,z1:k-1,m), (11)
Wherein p (X1:k-1|u1:k-2,z1:k-1, m) and indicate that the pose sequence probability at k-1 moment is distributed, p (Xk|uk-1,zk,m,
X1:k-1) indicate k moment robot pose probability distribution.Specific derivation is as follows:
Using Bayes rule to p (Xk|uk-1,zk,m,X1:k-1) be unfolded, it obtains as follows:
Wherein p (uk-1|Xk-1,Xk) and p (Xk|zk, m) and respectively indicate likelihood item and priori item, p (uk-1|zk,m,X1:k-1) be
Normalize item.Specific derivation is as follows:
It can obtain probability-distribution function and be proportional to likelihood function: p (Xk|uk-1,zk,m,X1:k-1)∝p(uk-1|Xk-1,Xk)。
2.2nd carries out possibility predication using odometer model
Can obtain the incrementation parameter between two continuous poses using odometer model, including two angles and one away from
From.As shown in Fig. 6, δ ' is definedrot1、δ′tranWith δ 'rot2For continuous candidate global pose Xk-1,XkIncrementation parameter；Definition
δrot1、δtranAnd δrot2For odometer information uk-1Incrementation parameter.Between the incrementation parameter obtained using odometer model mutually
It is independent, therefore p (uk-1|Xk-1,Xk)∝p1·p2·p3, wherein
Wherein, αi(i=1 ..., 4) is the noise parameter in odometer model, and prob () is normal distyribution function.Machine
The probability Distribution Model of people's candidate's pose sequence is calculated by independent observation and odometer model.Practical operation when
It waits, after update probability distribution each time, the pose sequence that removal probability is less than specific threshold will be detected, until remaining unique
One pose sequence corresponds to Global localization result.Entire extension MHT frame is as shown in Fig. 7.
The advantages of the present invention
The invention proposes a kind of line laser global localization methods based on structural unit coding and multiple hypotheis tracking.It is mentioned
Structural unit encoding mechanism SUES out does not need off-line training and generates dictionary, and the bad influence of quality for avoiding offline dictionary is complete
Office's locating effect.Secondly, compared to traditional MHT method, there is convergence rate more the invention proposes a kind of extension MHT method
The advantages of fastly and independent of feature quantity.Contrast and experiment shows that, compared to AMCL, the present invention has higher success rate
And efficiency.
Detailed description of the invention:
Fig. 1 is total algorithm process；
The oriented endpoint feature of Fig. 2 expresses environment schematic.It is extracted by line segment feature and obtains oriented endpoint feature, environment energy
It is enough described using sparse feature, oriented endpoint feature is indicated in figure with arrow and square；
Fig. 3 is structural unit organigram.The structural unit that construction generates includes two angles and a range information；
Fig. 4 distinguishes similar structural unit schematic diagram.(a), (b), (c) and (d) denote like structural unit；
Fig. 5 angular region and metric space divide schematic diagram.Wherein (a) indicates angular region in δaDivided under resolution ratio
It cuts, (b) indicates metric space in δdIt is split under resolution ratio；
Fig. 6 odometer model schematic.Wherein (a) and (b) respectively indicate continuous independent observation and odometer information exists
Incremental model between moment k-1 and moment k；
Fig. 7 is extension MHT block schematic illustration；
Fig. 8 experimental system and environment；Wherein (a) and (b) respectively indicates experimental system and experimental situation；
Global localization schematic diagram of the global localization method that Fig. 9 present invention is mentioned in data set Intel-sorted-01.
Black triangle is candidate robot pose in figure, reasonable connection relationship of the line between pose between triangle；Subgraph
(a)-(d) illustrates entire Global localization process；
Figure 10 AMCL Global localization schematic diagram.Subgraph (a)-(d) illustrates entire Global localization process；
Figure 11 is about data set Intel-sorted-01 Global localization efficiency comparative's schematic diagram；
Figure 12 is about record data set Global localization efficiency comparative's schematic diagram certainly.
Specific embodiment:
Embodiment 1:
1st, the geometrical relationship between oriented endpoint feature is encoded using SUES
1.1st construction generates structural unit
In Global localization specific implementation process, by taking indoor verandas environment as an example (such as shown in attached drawing 8 (b)).First to sharp
Light data carries out line segment feature extraction, and entire environment can be described by oriented endpoint feature and (can specifically see attached drawing 2).It is fixed
Adopted P={ p1,...,pNIt is characterized point set, wherein N indicates characteristic point quantity.I-th of oriented endpoint feature is with pi=[xi yi
θi]TIt indicates, wherein (xi,yi) it is defined as feature locations, θiIt is defined as characteristic direction.ρ1It is characterized point p1=[0 ° of 1m 2m]TWith
Characteristic point p2=[270 ° of 2m 1m]TBetween Euclidean distance,ξ is set to 5m, is met
ρ1≤ ξ condition, obtains structural unit s1={ ρ1,θ11,θ12, directional information calculates as follows in structural unit:
θ′1- 0 ° of=atan2 (1-2,2-1),
θ′2- 270 ° of=atan2 (2-1,1-2),
θ11=max (θ '1,θ′2)=315 °,
θ12=min (θ '1,θ′2)=225 °,
It wherein needs angle information θ '1,θ′2Normalize to section [0,360 °), obtain
Mode with such a configuration obtains the corresponding structural unit set S={ s of laser data1,…,sM}。
1.2nd encodes structural unit by SUES
By angular resolution δaWith distance resolution δdBe respectively set to 20 ° and 0.5m, then by entire angular region and away from
Small grid (as shown in Fig. 5) is divided into above-mentioned resolution ratio from space.Define wiFor by 2 angle cut-off rules and 1 distance
Cut-off rule combines the structure words to be formed, and wherein i is index value；Then cut-off rule quantity is calculated, L=b can be generated in total2D=3564 structure words.Using the soft voting machine based on cut-off rule
System, by the structural unit of generationIt is encoded, the cut-off rule generated first according to soft voting mechanism
It calculates as follows:
Then according to the weight for determining each cut-off rule apart from size apart from cut-off rule, weight meter corresponding to cut-off rule
It calculates as follows:
Finally by the various combination between cut-off rule, structural unit8 knots can be generated
Structure word, the corresponding index value of each structure words and weight are as shown in table 2 below.
The corresponding index value of each structure words of table 2. and weight
In kth moment laser data, there can be multiple structural units, generate multiple structure words, therefore define aijkFor
Structure words w in kth moment laser dataiIn structural unit sjIn weight, aikThen indicate structure words wiSwash at the kth moment
The weight of light data, aikIt calculates as follows:
aik=Σjaijk. (7)
All structural units are subjected to coding renormalization in this way, it will be able to obtain corresponding to kth moment laser data
Feature vector calculates as follows:
Therefore according to formula (8), feature vector v is obtainedk=[tf1k,tf2k,...,tfLk]T(L is structure words quantity) is used
In the structural information of description kth moment laser data.It can use following formula later and seek v1And v2Similarity:
2nd, gradually restore global pose by extending MHT
2.1st establishes more hypothesis probabilistic models
By SUES to present laser data carry out cellular construction feature coding, then by similarity calculation can from from
Candidate key-frames are obtained in line pose map and obtain candidate global pose.It is different from traditional MHT method, the expansion that the present invention is proposed
The global posture information of the candidate that exhibition MHT method obtains independent observation, will continuous candidate pose and odometer as priori item
Increment information is as likelihood item.Have the advantages that following two is significant: (1) convergence rate is faster；(2) acquisition of candidate pose is only
Stand on feature quantity.Probability Distribution Model about k moment robot pose sequence is expressed as follows:
p(X1:k|u1:k-1,z1:k,m), (10)
Wherein u1:k-1It is defined as odometer information sequence；z1:kIndicate observation sequence, while X1:kCandidate machine is respectively indicated with m
Device people's pose sequence and priori map.In order to reduce calculation amount, entire distribution is calculated using following recurrence:
p(X1:k|u1:k-1,z1:k, m) and=p (Xk|uk-1,zk,m,X1:k-1)·p(X1:k-1|u1:k-2,z1:k-1,m), (11)
Wherein p (X1:k-1|u1:k-2,z1:k-1, m) and indicate that the pose sequence probability of last moment is distributed, p (Xk|uk-1,zk,m,
X1:k-1) indicate current robot pose probability distribution.
Using Bayes rule to p (Xk|uk-1,zk,m,X1:k-1) be unfolded, it obtains as follows:
Wherein p (uk-1|Xk-1,Xk) and p (Xk|zk, m) and respectively indicate likelihood item and priori item, p (uk-1|zk,m,X1:k-1) be
Normalize item；It can obtain probability-distribution function and be proportional to likelihood function p (Xk|uk-1,zk,m,X1:k-1)∝p(uk-1|Xk-1,Xk)。
2.2nd carries out possibility predication using odometer model
Can obtain the incrementation parameter between two continuous poses using odometer model, including two angles and one away from
From.As shown in Fig. 6, δ ' is definedrot1, δ 'tranWith δ 'rot2For continuous candidate global pose Xk-1,XkIncrementation parameter；Definition
δrot1, δtranAnd δrot2For odometer information uk-1Incrementation parameter.Between the incrementation parameter obtained using odometer model mutually
It is independent, therefore p (uk-1|Xk-1,Xk)∝p1·p2·p3, wherein
Wherein, αi(i=1 ..., 4) is the noise parameter in odometer model, and prob () is normal distyribution function.Machine
The probability Distribution Model of people's candidate's pose sequence is calculated by independent observation and odometer model.Practical operation when
It waits, after update probability distribution each time, the pose sequence that removal probability is less than specific threshold will be detected, until remaining unique
One pose sequence corresponds to Global localization result.Entire extension MHT frame is as shown in Fig. 7.
3rd, experiment effect description
To verify the above-mentioned validity based on structural unit coding and the global localization method for extending multiple hypotheis tracking, this hair
It is bright to be recorded using two publicly available data sets (Intel-sorted-01 and seattle) and same corridor (hallway)
The data set of system has carried out experimental verification.Experimental system is by two wheel mobile robots (Pioneer3-DX) and 2D laser sensor
(ODM30MR2000-B23) it forms, as shown in attached drawing 8 (a).Comparative experiments includes MHT+SUES, AMCl+SUES and original AMCL
Three kinds of methods, every kind of method are all assessed using success rate and efficiency.Mentioned global localization method under various circumstances be
Parameter of uniting is as shown in table 3.The method proposed is programmed with C/C++, the table 4 that the software and hardware specification of computer used is such as listed
It is shown.
3. system parameter of table
4. software and hardware specification of table
3.1st, public testing collection Global localization result
The sensing data of data set Intel-sorted-01 is obtained from Intel Jones Farms Campus, Oregon.
It is a typical office structure, and size is 40m × 40m, comprising there are many similar scenes.It tests original from 8030
162 key frames are chosen in laser scanning frame as offline pose map carries out Global localization test.
Shown in the process of the global localization method proposed such as attached drawing 9 (a)-(d).In attached drawing 9 (a), pass through for the first time
Laser scanning obtains 11 initial candidate pose sequences；On this basis, pass through second of candidate pose of observation trimming three
Sequence, such as attached drawing 9 (b)；In attached drawing 9 (c), we have only trimmed one group of candidate's pose sequence；It is observed by the 4th time, removal
Six candidate pose sequences, have obtained correct pose sequence.Different from the method proposed, attached drawing 10 (a)-(d) is indicated
AMCL carries out the process of Global localization out.In attached drawing 10 (a), when Global localization starts, it is random on whole configuration space and
Equably obtain a large amount of pose particle (22200 particles).Then, with the movement of mobile robot, these pose particles
Correct pose is gradually converged to, as shown in attached drawing (b)-(d).
Pass through the comparison to Global localization result, it can be seen that the method proposed does not need in entire configuration space
It arranges a large amount of pose particle, while realizing successful Global localization under shorter moving distance.Due to not shown in scene
When writing variation (such as gallery and similar corridor), the convergence rate that AMCL carries out Global localization is generally lower, therefore the receipts of this method
It is fast to hold back speed ratio AMCL.
In order to assess success rate, we select starting point of the interval 10s as Global localization from data set.Use difference
Starting point carry out Global localization the results are shown in Table 5.The result shows that the performance of MHT+SUES is best in three kinds of methods,
And the performance of AMCL+SUES is better than AMCL.
5. Global localization success rate result (Intel-sorted-01) (number of success/testing time) of table
During Global localization, the efficiency of Global localization is indicated using moving distance.We successfully calculate
The average moving distance of AMCL, AMCL+SUES and MHT+SUES, as a result as shown in Fig. 11.It can be seen that from figure attached drawing 11
The average moving distance of MHT+SUES and AMCL+SUES is much smaller than AMCL, and the MHT+SUES ratio AMCL+SUES table proposed
It is now more preferable.Compare Global localization about success rate and efficiency the experimental results showed that, the global localization method ratio AMCL tool mentioned
There is better performance.
Second common data sets of Global localization experiment are Seattle, and experiment is selected from 241 original laser scanning frames
102 are selected as key frame.Since public testing concentrates the data for including very little, AMCL and AMCL+SUES are difficult to obtain satisfied knot
Fruit.However, the success rate of MHT+SUES is up to 98.25% (112/114).In addition, we also calculate average moving distance
Reflect the efficiency of MHT+SUES, result 2.8m.
3.2nd, from record data set positioning result
Our in the same corridor environment (attached drawings 8 (b)) recorded five laser data collection and test for Global localization,
It is made of four static data collection and a dynamic data set.Four static data collection are obtained from static environment, including two
Data set clockwise and two data sets counterclockwise.One of data set is used to generate the pose map about key frame, and
Other three data sets are tested for Global localization.When mobile robot is moved along identical path, while there are two meetings
People walks around mobile robot and causes dynamic environment, to record generation dynamic data set.
Equally the different starting points for selecting interval 10s as Global localization are concentrated from three static datas.It is risen using difference
The Global localization of initial point is as the result is shown in table 6, the results showed that and MHT+SUES has best performance in three kinds of methods, and
Due to using SUES proposed by the invention, AMCL+SUES to obtain than AMCL better performance.It is successful for Global localization
Example, we calculate the average moving distance of AMCL, AMCL+SUES and MHT+SUES, as a result as shown in Fig. 12.12 table of attached drawing
It is bright, it is concentrated in three different data, MHT+SUES has best performance, and AMCL+SUES in terms of average moving distance
Performance also superior to AMCL.
6. Global localization success rate result (hallway) (number of success/testing time) of table
In addition, also having carried out the SUES+MHT and AMCL proposed to verify the present invention to the robustness of ambient noise
Comparative experiments in a dynamic environment.Contrast and experiment about success rate and efficiency is as shown in table 7, the results showed that, environment
Noise does not influence the Global localization success rate of SUES+MHT and AMCL.However, ambient noise reduces the efficiency of AMCL, and
SUES+MHT still maintains high efficiency.
Success rate and efficiency comparative's experimental result (number of success/testing time) under 7. dynamic environment of table
The above-mentioned contrast and experiment about the success rate in corridor environment and efficiency shows that the present invention has more than AMCL
Good performance.Specifically, the method proposed improves the efficiency of Global localization under the premise of compared with high success rate.
Bibliography
1.H.Durrant-Whyte,T.Bailey,“Simultaneous localization and mapping:
Part I,”IEEE Robotics andAutomation Magazine,vol.13,no.2,pp.99-110,2006.
2.C.Cadena,L.Carlone,H.Carrillo,Y.Latif,D.Scaramuzza,J.Neira,I.Reid,
J.J.Leonard,“Past,present,and future of simultaneous localization and
mapping:Toward the robust-perception age,”IEEE Transactions on Robotics,
vol.32,no.6,pp.1309-1332,2016.
3.D.Galvez-López,J.D.Tardos,“Bags of binary words for fast place
recognition in image sequences,”IEEE Transactions on Robotics,vol.28,no.5,
pp.1188-1197,2012
4.R.Mur-Artal,J.M.M.Montiel,J.D.Tard′os,“ORB-SLAM:A versatile and
accurate monocular SLAM system,”IEEE Transactions on Robotics,vol.31,no.5,
pp.1147-1163,2015.
5.A.Murillo,G.Singh,J.J.Guerrero,“Localization in urban
environments using a panoramic gist descriptor,”IEEE Transactions on
Robotics,vol.29,no.1,pp.146-160,2013.
6.G.D.Tipaldi,K.O.Arras,“FLIRT-Interest regions for 2D range data,”in
IEEE International Conference on Robotics andAutomation,2010,pp.3616-3622.
7.G.D.Tipaldi,L.Spinello,W.Burgard,“Geometrical flirt phrases for
large scale place recognition in 2D range data,”in IEEE International
Conference on Robotics and Automation,2013,pp.2693-2698.
8.J.Deray,J.Solá,J.Andrade-Cetto,“Word ordering and document
adjacency for large loop closure detection in 2D laser maps,”IEEE Robotics
and Automation Letters,vol.2,no.3,pp.1532-1539,2017.
9.F.Martín,L.Moreno,D.Blanco,et al.“Kullback-Leibler divergence-based
global localization for mobile robots,”Robotics and Autonomous Systems,
vol.62,no.2,pp.120-130,2014.
10.D.Fox,“Adapting the sample size in particle filters through KLD-
sampling,”International Journal of Robotics Research,vol.22,no.12,pp.985-
1003,2003.
11.R.Valencia,J.Saarinen,H.Andreasson,et al.“Localization in highly
dynamic environments using dual-timescale NDT-MCL,”in IEEE International
Conference on Robotics andAutomation,2014,pp.3956-3962.
12.S.Tully,G.Kantor,H.Choset,“A unified Bayesian framework for global
localization and SLAM in hybrid metric/topological maps,”International
Journal ofRobotics Research,vol.31,no.3,pp.271-288,2012.
13.T.He,S.Hirose,“A global localization approach based on line-
segment relation matching technique,”Robotics andAutonomous Systems,vol.60,
no.1,pp.95-112,2012.
14.H.Liu,F.Sun,B.Fang,X Zhang,“Robotic room-level localization using
multiple sets of sonar measurements,”IEEE Transactions on Instrumentation and
Measurement,vol.66,no.1,pp.2-13,2016.
15.S.Park,K.S.Roh,“Coarse-to-fine localization for a mobile robot
based on place learning with a 2-D range scan,”IEEE Transactions on Robotics,
vol.32,no.3,pp.1-17,2016.
Claims (1)
1. a kind of line laser global localization method based on structural unit coding and multiple hypotheis tracking, it is characterised in that this method packet
It includes:
1st, the geometrical relationship between oriented endpoint feature is encoded using SUES
1.1st construction generates structural unit
Line segments extraction is carried out to laser data, defines P={ p1,...,pNIt is oriented endpoint feature point set, wherein N indicates feature
Point quantity；I-th of oriented endpoint feature is with pi=[xi yi θi]TIt indicates, wherein (xi,yi) it is defined as feature locations, θiIt is defined as
Characteristic direction；Define ρjIt is characterized point pl=[xl yl θl]TWith characteristic point pn=[xn yn θn]TBetween Euclidean distance, wherein j
For index number,
When meeting ρjWhen≤ξ, definition structure unit sj={ ρj,θj1,θj2Indicate characteristic point plAnd pnBetween geometrical relationship,
Middle ξ is given threshold value, θj1And θj2It is characterized point plAnd pnBetween directional information, calculate it is as follows:
Wherein θ 'l,θ′nFor directional information, be located at section [0,360 °)；Define S={ s1,…,sMIt is the structure list that construction obtains
Member set, wherein M is defined as structural unit quantity, is obtained according to above-mentioned make；
1.2nd encodes structural unit by SUES
Define δaAnd δdRespectively angular resolution and distance resolution, by entire angular region and metric space with above-mentioned resolution
Rate is divided into small grid；Define wiTo combine the structure words formed apart from cut-off rule by 2 angle cut-off rules and 1, wherein i
For index value, L=b can be generated in total2D structure words, whereinUsing based on segmentation
The soft voting mechanism of line, by the structural unit s of generationj={ ρj,θj1,θj2Encoded, it is generated first according to soft voting mechanism
Cut-off rule, wherein φi, i=1 ..., 6 are defined as specific cut-off rule index value；Then according to apart from cut-off rule apart from size
Determine the weight of each cut-off rule；Finally by the combination between cut-off rule, each structural unit can generate 8 structure lists
All structural units are carried out coding renormalization in this way by word, it will be able to obtain feature vector corresponding to laser data；
2nd, gradually restore global pose by extending MHT
2.1st establishes more hypothesis probabilistic models
Cellular construction feature coding is carried out to present laser data by SUES, it then can be from offline position by similarity calculation
Candidate key-frames are obtained in appearance map and obtain candidate global pose；It is different from traditional MHT method, the present invention obtains independent observation
The global posture information of the candidate obtained is as priori item, by continuous candidate's pose and odometer increment information as likelihood item；It closes
It is expressed as follows in the probability Distribution Model of k moment robot pose sequence:
p(X1:k|u1:k-1,z1:k,m), (10)
Wherein u1:k-1It is defined as odometer information sequence；z1:kIndicate observation sequence, while X1:kCandidate robot is respectively indicated with m
Pose sequence and priori map；In order to reduce calculation amount, entire distribution is calculated using following recurrence:
p(X1:k|u1:k-1,z1:k, m) and=p (Xk|uk-1,zk,m,X1:k-1)·p(X1:k-1|u1:k-2,z1:k-1,m), (11)
Wherein p (X1:k-1|u1:k-2,z1:k-1, m) and indicate that the pose sequence probability at k-1 moment is distributed, p (Xk|uk-1,zk,m,X1:k-1)
Indicate the probability distribution of k moment robot pose；
Using Bayes rule to p (Xk|uk-1,zk,m,X1:k-1) be unfolded, it obtains as follows:
Wherein p (uk-1|Xk-1,Xk) and p (Xk|zk, m) and respectively indicate likelihood item and priori item, p (uk-1|zk,m,X1:k-1) it is normalizing
Change item；It can obtain probability-distribution function and be proportional to likelihood function p (Xk|uk-1,zk,m,X1:k-1)∝p(uk-1|Xk-1,Xk)；
2.2nd carries out possibility predication using odometer model
The incrementation parameter between two continuous poses, including two angles and a distance can be obtained using odometer model；
Define δ 'rot1、δ′tranWith δ 'rot2For continuous candidate global pose Xk-1,XkIncrementation parameter；Define δrot1、δtranAnd δrot2For
Odometer information uk-1Incrementation parameter；It is mutually indepedent between the incrementation parameter obtained using odometer model, therefore p (uk-1|
Xk-1,Xk)∝p1·p2·p3, wherein
Wherein, αi(i=1 ..., 4) is the noise parameter in odometer model, and prob () is normal distyribution function；Robot waits
The probability Distribution Model of bit selecting appearance sequence is calculated by independent observation and odometer model；When practical operation, often
After update probability distribution, the pose sequence that removal probability is less than specific threshold will be detected, until remaining only one
Pose sequence corresponds to Global localization result.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201811306776.0A CN109358316B (en) | 2018-11-05 | 2018-11-05 | Line laser global positioning method based on structural unit coding and multi-hypothesis tracking |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201811306776.0A CN109358316B (en) | 2018-11-05 | 2018-11-05 | Line laser global positioning method based on structural unit coding and multi-hypothesis tracking |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN109358316A true CN109358316A (en) | 2019-02-19 |
| CN109358316B CN109358316B (en) | 2022-04-22 |
Family
ID=65344184
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201811306776.0A Active CN109358316B (en) | 2018-11-05 | 2018-11-05 | Line laser global positioning method based on structural unit coding and multi-hypothesis tracking |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN109358316B (en) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114413882A (en) * | 2022-03-29 | 2022-04-29 | 之江实验室 | Global initial positioning method and device based on multi-hypothesis tracking |
Citations (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20030228032A1 (en) * | 2002-06-07 | 2003-12-11 | Yong Rui | System and method for mode-based multi-hypothesis tracking using parametric contours |
| US20130234882A1 (en) * | 2012-03-09 | 2013-09-12 | Raytheon Company | Multiple hypothesis tracking using maximum weight independent set |
| CN104019816A (en) * | 2014-05-30 | 2014-09-03 | 西北工业大学 | Flight track extraction method based on probability hypothesis density filter associated with global time and space |
| US20160054430A1 (en) * | 2014-08-25 | 2016-02-25 | Princeton Satellite Systems, Inc. | Multi-sensor target tracking using multiple hypothesis testing |
| CN106406338A (en) * | 2016-04-14 | 2017-02-15 | 中山大学 | Omnidirectional mobile robot autonomous navigation apparatus and method based on laser range finder |
| CN106840179A (en) * | 2017-03-07 | 2017-06-13 | 中国科学院合肥物质科学研究院 | A kind of intelligent vehicle localization method based on multi-sensor information fusion |
| US20170178360A1 (en) * | 2014-03-28 | 2017-06-22 | Philips Lighting Holding B.V. | Locating a portable device based on coded light |
| CN108153310A (en) * | 2017-12-22 | 2018-06-12 | 南开大学 | A kind of Mobile Robot Real-time Motion planing method based on human behavior simulation |
| WO2018177555A1 (en) * | 2017-03-31 | 2018-10-04 | U-Blox Ag | Global navigation satellite system (gnss) signal tracking |
-
2018
- 2018-11-05 CN CN201811306776.0A patent/CN109358316B/en active Active
Patent Citations (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20030228032A1 (en) * | 2002-06-07 | 2003-12-11 | Yong Rui | System and method for mode-based multi-hypothesis tracking using parametric contours |
| US20130234882A1 (en) * | 2012-03-09 | 2013-09-12 | Raytheon Company | Multiple hypothesis tracking using maximum weight independent set |
| US20170178360A1 (en) * | 2014-03-28 | 2017-06-22 | Philips Lighting Holding B.V. | Locating a portable device based on coded light |
| CN104019816A (en) * | 2014-05-30 | 2014-09-03 | 西北工业大学 | Flight track extraction method based on probability hypothesis density filter associated with global time and space |
| US20160054430A1 (en) * | 2014-08-25 | 2016-02-25 | Princeton Satellite Systems, Inc. | Multi-sensor target tracking using multiple hypothesis testing |
| CN106406338A (en) * | 2016-04-14 | 2017-02-15 | 中山大学 | Omnidirectional mobile robot autonomous navigation apparatus and method based on laser range finder |
| CN106840179A (en) * | 2017-03-07 | 2017-06-13 | 中国科学院合肥物质科学研究院 | A kind of intelligent vehicle localization method based on multi-sensor information fusion |
| WO2018177555A1 (en) * | 2017-03-31 | 2018-10-04 | U-Blox Ag | Global navigation satellite system (gnss) signal tracking |
| CN108153310A (en) * | 2017-12-22 | 2018-06-12 | 南开大学 | A kind of Mobile Robot Real-time Motion planing method based on human behavior simulation |
Non-Patent Citations (5)
| Title |
|---|
| J. WEN, X. ZHANG, H. GAO, J. YUAN AND Y. FANG: "《A Novel 2D Laser Scan Matching Algorithm for Mobile Robots Based on Hybrid Features》", 《2018 IEEE INTERNATIONAL CONFERENCE ON REAL-TIME COMPUTING AND ROBOTICS (RCAR)》 * |
| R. DANCHICK AND G.E. NEWNAM: "《Reformulating Reid"s MHT method with generalised Murty K-best ranked linear assignment algorithm》", 《IEE PROCEEDINGS - RADAR, SONAR AND NAVIGATION》 * |
| T. HE, S. HIROSE: "《A global localization approach based on line-segment relation matching technique》", 《ROBOTICS AND AUTONOMOUS SYSTEMS》 * |
| 李晓银: "《传感识别系统中的无线定位技术研究》", 《中国博士学位论文全文数据库 工程科技Ⅱ辑》 * |
| 饶世钧; 姜宁; 张云雯: "《基于结构多假设目标跟踪算法优化与仿真》", 《计算机仿真》 * |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114413882A (en) * | 2022-03-29 | 2022-04-29 | 之江实验室 | Global initial positioning method and device based on multi-hypothesis tracking |
| CN114413882B (en) * | 2022-03-29 | 2022-08-05 | 之江实验室 | Global initial positioning method and device based on multi-hypothesis tracking |
Also Published As
| Publication number | Publication date |
|---|---|
| CN109358316B (en) | 2022-04-22 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN111652934B (en) | Positioning method, map construction method, device, equipment and storage medium | |
| Himstedt et al. | Large scale place recognition in 2D LIDAR scans using geometrical landmark relations | |
| Yang et al. | Toward the repeatability and robustness of the local reference frame for 3D shape matching: An evaluation | |
| Cohen et al. | Inference of human postures by classification of 3D human body shape | |
| Cadena et al. | Robust place recognition with stereo cameras | |
| Song et al. | Body surface context: A new robust feature for action recognition from depth videos | |
| CN113537208A (en) | Visual positioning method and system based on semantic ORB-SLAM technology | |
| CN108986168A (en) | A kind of robot winding detection method and device combining bag of words tree-model based on depth measure study | |
| Li et al. | Shrec’16 track: 3D sketch-based 3D shape retrieval | |
| CN110942110A (en) | Feature extraction method and device of three-dimensional model | |
| Waheed et al. | Improving visual place recognition performance by maximising complementarity | |
| CN111626417B (en) | Closed loop detection method based on unsupervised deep learning | |
| CN109358316A (en) | Line laser global localization method based on structural unit coding and multiple hypotheis tracking | |
| Yoshiki et al. | Scalable change detection from 3d point cloud maps: Invariant map coordinate for joint viewpoint-change localization | |
| Tsintotas et al. | Visual place recognition for simultaneous localization and mapping | |
| Essmaeel et al. | A new 3D descriptor for human classification: Application for human detection in a multi-kinect system | |
| Lee et al. | Robust 3d shape classification method using simulated multi view sonar images and convolutional nueral network | |
| Yi et al. | Moving object detection under dynamic background in 3D range data | |
| Jacobson et al. | Deja vu: Scalable place recognition using mutually supportive feature frequencies | |
| Nospes et al. | Recognition and 6D pose estimation of large-scale objects using 3D semi-global descriptors | |
| Tomoya et al. | Change detection under global viewpoint uncertainty | |
| Wang et al. | Map matching navigation method based on scene information fusion | |
| Chen et al. | A point cloud feature regularization method by fusing judge criterion of field force | |
| CN111724438A (en) | Data processing method and device | |
| Lu et al. | Slicing-tracking-detection: Simultaneous multi-cylinder detection from large-scale and complex point clouds |
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 | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |