CN108256060A - Closed loop detection method, device, terminal and storage medium - Google Patents
Closed loop detection method, device, terminal and storage medium Download PDFInfo
- Publication number
- CN108256060A CN108256060A CN201810039214.8A CN201810039214A CN108256060A CN 108256060 A CN108256060 A CN 108256060A CN 201810039214 A CN201810039214 A CN 201810039214A CN 108256060 A CN108256060 A CN 108256060A
- Authority
- CN
- China
- Prior art keywords
- map
- sub
- score
- matching
- candidate
- 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
- 238000001514 detection method Methods 0.000 title claims abstract description 93
- 238000012216 screening Methods 0.000 claims abstract description 46
- 238000000034 method Methods 0.000 claims abstract description 45
- 238000005056 compaction Methods 0.000 claims description 42
- 238000013519 translation Methods 0.000 claims description 38
- 238000004590 computer program Methods 0.000 claims description 3
- 230000008569 process Effects 0.000 abstract description 8
- 238000010276 construction Methods 0.000 abstract description 3
- 230000015654 memory Effects 0.000 description 9
- 238000005457 optimization Methods 0.000 description 9
- 238000004364 calculation method Methods 0.000 description 6
- 238000012545 processing Methods 0.000 description 6
- 238000004422 calculation algorithm Methods 0.000 description 5
- 230000006835 compression Effects 0.000 description 5
- 238000007906 compression Methods 0.000 description 5
- 238000010586 diagram Methods 0.000 description 5
- 230000006870 function Effects 0.000 description 5
- 230000004888 barrier function Effects 0.000 description 4
- 230000015572 biosynthetic process Effects 0.000 description 2
- 230000008859 change Effects 0.000 description 2
- 238000012512 characterization method Methods 0.000 description 2
- 230000007812 deficiency Effects 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 241001269238 Data Species 0.000 description 1
- 238000009825 accumulation Methods 0.000 description 1
- 230000009471 action Effects 0.000 description 1
- 238000006243 chemical reaction Methods 0.000 description 1
- 238000004891 communication Methods 0.000 description 1
- 239000012141 concentrate Substances 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 238000000605 extraction Methods 0.000 description 1
- 206010016256 fatigue Diseases 0.000 description 1
- 238000007689 inspection Methods 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 230000009467 reduction Effects 0.000 description 1
- 238000012360 testing method Methods 0.000 description 1
- 230000001052 transient effect Effects 0.000 description 1
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
- G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
- G01S17/02—Systems using the reflection of electromagnetic waves other than radio waves
- G01S17/06—Systems determining position data of a target
-
- 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
- G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
- G01S17/88—Lidar systems specially adapted for specific applications
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F16/00—Information retrieval; Database structures therefor; File system structures therefor
- G06F16/20—Information retrieval; Database structures therefor; File system structures therefor of structured data, e.g. relational data
- G06F16/24—Querying
- G06F16/245—Query processing
- G06F16/2458—Special types of queries, e.g. statistical queries, fuzzy queries or distributed queries
- G06F16/2462—Approximate or statistical queries
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F16/00—Information retrieval; Database structures therefor; File system structures therefor
- G06F16/50—Information retrieval; Database structures therefor; File system structures therefor of still image data
- G06F16/58—Retrieval characterised by using metadata, e.g. metadata not derived from the content or metadata generated manually
- G06F16/583—Retrieval characterised by using metadata, e.g. metadata not derived from the content or metadata generated manually using metadata automatically derived from the content
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- Theoretical Computer Science (AREA)
- General Physics & Mathematics (AREA)
- Electromagnetism (AREA)
- Data Mining & Analysis (AREA)
- Remote Sensing (AREA)
- Probability & Statistics with Applications (AREA)
- Radar, Positioning & Navigation (AREA)
- Databases & Information Systems (AREA)
- General Engineering & Computer Science (AREA)
- Computer Networks & Wireless Communication (AREA)
- Library & Information Science (AREA)
- Fuzzy Systems (AREA)
- Mathematical Physics (AREA)
- Software Systems (AREA)
- Computational Linguistics (AREA)
- Traffic Control Systems (AREA)
Abstract
The invention discloses a closed loop detection method, a closed loop detection device, a closed loop detection terminal and a storage medium. The method comprises the following steps: forming a current sub-map according to the captured laser data of the set continuous frames; determining a similarity matching score of the front sub-map and the selected at least one candidate sub-map; determining a target matching score satisfying the first matching screening condition from the similarity matching scores; determining a candidate sub-map corresponding to the target matching score as a closed-loop area of the current sub-map if it is determined that the target matching score exists. By utilizing the method, the closed-loop detection in the large-range map construction is realized from the angle of similarity matching between the sub-map and the sub-map, compared with the prior art, the method can effectively control the number of the matching candidate sets, improve the closed-loop detection speed, ensure the real-time performance of the closed-loop detection in the large-range map construction process, and simultaneously, the technical scheme also effectively reduces the false detection rate of the closed-loop detection and improves the accuracy of the closed-loop detection.
Description
Technical field
The present invention relates to technical field of data processing more particularly to a kind of closed loop detection method, device, terminal and storage to be situated between
Matter.
Background technology
When carrying out map structuring by robot, the position for being currently located robot is generally required, before robot
The position of traversal carries out similitude matching, to determine whether robot present position is place to access, it is above-mentioned this
Kind location matches mode is regarded as the closed loop detection of robot.
In the map structuring based on robot, can generally laser radar be used to obtain environmental information as sensor, because
This closed loop test problems can regard the problem of laser data scan matching result similitude as.In existing closed loop detection method,
The scan matching algorithm (frame-frame) of one frame laser data of generally use and a frame laser data, but the deficiency of this kind of method is：
Easily because of the local similarity of laser data height, the matching result of mistake is generated, causes the closed loop detection of mistake；In addition, institute
It when the body of a map or chart of structure is larger, needs matched laser data amount that can increase considerably, thus influences the real-time of closed loop detection
Property.
Based on the above problem, technical staff proposes the improved method that closed loop detects, and e.g., extraction machine people traverses environment
In small range feature form characteristic area, by comparing the similitude in different characteristic region carry out closed loop detection, for another example,
By the use of the laser data every setting frame as a key frame, closed loop inspection is carried out by comparing the similitude of different key frames
It surveys.The core concept of above-mentioned improved method, which is equivalent to, chooses key frame for closed loop detection, can reach reduction matching candidate
The quantity of collection, improves the purpose of detection speed, but above-mentioned into method, there is also certain deficiencies：The selection of key frame can randomness
Lost part data, cause two key frames of physical presence closed loop relationship that can not be detected the presence of because similar portion is less and close
Ring.
Invention content
A kind of closed loop detection method provided in an embodiment of the present invention, device, terminal and storage medium not only improve closed loop
Detection speed also effectively increases the accuracy rate of closed loop detection.
In a first aspect, an embodiment of the present invention provides a kind of closed loop detection method, including：
Laser data according to setting successive frame is captured forms current sub- map；
It determines the current sub- map and selectes the similitude matching score of at least one sub- map of candidate；
The object matching score for meeting the first matching screening conditions is determined from each similitude matching score；
If it is determined that there are the object matching score, then it will be true corresponding to the sub- map of candidate of the object matching score
It is set to the closed loop region of the current sub- map.
Second aspect, the embodiment of the present invention additionally provide a kind of loop detector, including：
Sub- map structuring module, for forming current sub- map according to the laser data for capturing setting successive frame；
Similar score determining module, for determining the phase of the current sub- map and selected at least one sub- map of candidate
Like property matching score；
Target score determining module, for determining to meet the first matching screening conditions from each similitude matching score
Object matching score；
Closed loop area determination module, for when determining, there are during the object matching score, the target will be corresponded to
The sub- map of candidate with score is determined as the closed loop region of the current sub- map.
The third aspect, the embodiment of the present invention also provide a kind of terminal, including：Laser radar and system odometer, are also wrapped
It includes：
One or more processors；
Storage device, for storing one or more programs；
One or more of programs are performed by one or more of processors so that one or more of processors
Realize the closed loop detection method that first aspect of the embodiment of the present invention provides.
Fourth aspect, the embodiment of the present invention provide a kind of storage medium, are stored thereon with computer program again, the program
The closed loop detection method that first aspect of the embodiment of the present invention provides is realized when being executed by processor.
An embodiment of the present invention provides a kind of closed loop detection method, device, terminal and storage medium, the technical solution, energy
Current sub- map is enough formed by the laser data of laser radar capture setting successive frame in terminal, then will current sub- map with
The sub- map of candidate in the sub- atlas of history carries out similitude matching, and matching process is determines current sub- map and each candidate son
The similitude matching score of map, and determine that satisfaction screens the object matching of matching condition from above-mentioned each similitude matching score
Score, finally the corresponding sub- map of candidate of determining object matching score and the similarity of current sub- map meet wanting for closed loop detection
It asks, it is thus achieved that the closed loop detection in map structuring.The technical program carries out similarity mode from sub- map and sub- map
Angle realizes closed loop detection during a wide range of map structuring, compared with prior art, can effectively control matching candidate collection
Quantity improves closed loop detection speed, ensure that the real-time that closed loop detects in a wide range of map building process, meanwhile, this skill
Art scheme also effectively reduces the false drop rate of closed loop detection, improves the accuracy of closed loop detection.
Description of the drawings
Fig. 1 is a kind of flow chart for closed loop detection method that the embodiment of the present invention one provides；
Fig. 2 a are a kind of flow charts of closed loop detection method provided by Embodiment 2 of the present invention；
Fig. 2 b are that the realization side that the first matching score determines is carried out in closed loop detection method provided by Embodiment 2 of the present invention
Method flow chart；
Fig. 2 c give the exemplary plot of the two-dimensional look-up table of a sub- map of candidate；
Fig. 3 a are a kind of flow charts for closed loop detection method that the embodiment of the present invention three provides；
Fig. 3 b are that the implementation method stream that candidate information determines is carried out in the closed loop detection method that the embodiment of the present invention three provides
Cheng Tu；
Fig. 3 c give the implementation example figure based on two-dimensional look-up table generation two dimensional compaction table；
Fig. 3 d give the display figure that first laser data are presented in the form of grating map；
Fig. 3 e give the display figure that second laser data are presented in the form of grating map；
Fig. 3 f give the display figure that the first sub- map comprising first laser data is presented in the form of grating map；
Fig. 3 g give the display figure that the second sub- map comprising first laser data is presented in the form of grating map；
Fig. 4 is a kind of structure diagram for loop detector that the embodiment of the present invention four provides；
Fig. 5 is a kind of hardware architecture diagram for terminal that the embodiment of the present invention five provides.
Specific embodiment
The present invention is described in further detail with reference to the accompanying drawings and examples.It is understood that this place is retouched
The specific embodiment stated is used only for explaining the present invention rather than limitation of the invention.It also should be noted that in order to just
Part related to the present invention rather than entire infrastructure are illustrated only in description, attached drawing.
It also should be noted that illustrate only for ease of description, in attached drawing part related to the present invention rather than
Full content.It should be mentioned that some exemplary embodiments are described before exemplary embodiment is discussed in greater detail
Into the processing or method described as flow chart.Although operations (or step) are described as the processing of sequence by flow chart,
It is that many of which operation can be implemented concurrently, concomitantly or simultaneously.In addition, the sequence of operations can be by again
It arranges.The processing can be terminated when its operations are completed, it is also possible to have the additional step being not included in attached drawing.
The processing can correspond to method, function, regulation, subroutine, subprogram etc..
It should be noted that herein, relational terms such as first and second and the like are used merely to a reality
Body is either operated to be distinguished with another entity or operation, without necessarily requiring or implying between these entities or operation
There are any actual relationship or orders.For example, " first " in the first matching score and the second matching score and "
Two " for distinguishing two different matching scores in score calculating operation, in the first projection score and the second projection score " the
One " with " second " for distinguishing two different projection scores in score calculating operation, for another example, first setting translation and/or rotation
Turn rule and second setting translation and/or " first " and " second " in pivot rule for distinguish it is two different translation with/
Or pivot rule etc..
Embodiment one
Fig. 1 is a kind of flow chart for closed loop detection method that the embodiment of the present invention one provides, and the present embodiment is applicable to
Terminal present position and historical position are carried out to the situation of closed loop detection, this method can be by of the invention real in map structuring
The loop detector of example offer is applied to perform, which can be realized, and be typically integrated in terminal by hardware and/or software
In.
It should be noted that the terminal, which specifically can refer to robot and intelligent Service equipment etc., can voluntarily obtain surrounding ring
Border information carries out the electric terminal equipment of map structuring.It is understood that when carrying out map structuring based on above-mentioned terminal, it can be with
Closed loop detection method based on the embodiment of the present invention quickly and accurately determines the closed loop region of terminal present position.
As described in Figure 1, a kind of closed loop detection method that the embodiment of the present invention one provides, includes the following steps：
S101, current sub- map is formed according to the laser data for capturing setting successive frame.
In the present embodiment, for carrying out the terminal of map structuring, it is often provided with laser radar thereon, it is described to swash
Specifically laser data can be consequently formed, generally by emitting detecting laser beam barrier to the range information of terminal in optical radar
Ground, one frame of laser radar capture laser data in the time span of unit frame for unit, laser radar can be controlled to rotate, by
This obtains the range information that transmitting laser beam returns under different rotary angle, and the final frame that obtains includes more parts of distance information values
Laser data.Illustratively, the mathematical form of a frame laser data is represented by：Z={ z1`,z2`,z3`,...,zi`...,zm
` }, wherein, Z represents a frame laser data, zi` represents i-th part of distance information value that laser data obtains in unit frame, m tables
Show the total quantity of range information included in the frame laser data.
It should be noted that during map structuring, the frame laser data that laser radar captures can be transformed into
In constructed map, to determine the location information of barrier in map by laser data, it is possible thereby to think laser radar
The laser data captured in real time can be used for the structure of map.This step can form one by setting the laser data of successive frame
The small map of part, to detect required current sub- map as the present embodiment closed loop.
Usually, the current sub- map that setting successive frame laser data is formed can be presented by the form of grating map,
The range information that can also be included according only to the laser data of setting successive frame represents that this step is mainly used to be formed comprising eventually
Hold present position local map, by by figure with scheme it is matched in a manner of carry out closed loop detection.
Illustratively, it when the present embodiment is presented current sub- map by grating map, needs that successive frame will be set
Laser data coordinate information of the laser terminal point in global coordinate system is converted to by range information, then according to laser
The coordinate information of distal point is identified in grating map, it is possible thereby to the current sub- map of grid is formed, wherein, swash
Light distal point can be regarded as laser radar and capture the corresponding barrier position point of every part of distance information value in laser data,
In grating map, laser terminal point is identified in a particular form, represents position of the barrier in map.
S102, it determines current sub- map and selectes the similitude matching score of at least one sub- map of candidate.
In the present embodiment, after determining current sub- map, whether the position to determine terminal current accessed is to have accessed
Historical position, need current sub- map carrying out similitude matching with the sub- map of history formed before.The candidate son ground
What figure specifically can be regarded as selecting from the sub- atlas of history is used to carry out the matched sub- map of similitude with current sub- map.
It is understood that when carrying out a wide range of map structuring, it may be existing more before current sub- map is formed
The sub- map of history of quantity if current sub- map and the sub- map of all history are all carried out similitude matching, needs to consume
Take more time, influence the real-time of closed loop detection, this step can be only at least one with selecting by current sub- map as a result,
Candidate sub- map carries out similitude matching, and this step can determine that the map progress sub- to any candidate of current sub- map is similar
Property matching when similitude matching score, the similitude matching score can characterize current sub- map and the sub- map of corresponding candidate
Similarity matching situation.
In the present embodiment, specifically based on the laser data for forming current sub- map and each candidate son ground can be formed
The laser data of figure determines the similarity score of current sub- map and each sub- map of candidate by the computation rule of setting.Example
Property, the computation rule of the present embodiment setting can be based on the laser data for forming current sub- map in each sub- map of candidate
It is projected in the two-dimentional local map that corresponding laser data is formed, by projecting corresponding score value, determines current son ground
The similitude matching score of figure and each sub- map of candidate.
S103, the object matching score for meeting the first matching screening conditions is determined from each similitude matching score.
In this embodiment, since the similarity score of current sub- map and candidate sub- map is higher, then it is believed that current son
Map is more similar to the sub- map of the candidate, that is, and the possibility that the two belongs to same position in map is higher, therefore, this reality
The the first matching screening conditions for applying example setting are particularly used in and filter out the higher similitude matching score of score, and will filter out
Result be denoted as object matching score.The present embodiment for filter out with the current sub- highest sub- map of map similarity, preferably set
It is fixed to be only used as object matching score there are one similitude matching score.
S104, if it is determined that there are object matching score, then will be determined corresponding to the sub- map of candidate of object matching score
Closed loop region for current sub- map.
The present embodiment determined based on above-mentioned S103 there are during object matching score, can be by the corresponding time of object matching score
Select sub- map be determined as with the current sub- map of the sub- highest history of map similarity, determined when based on the present embodiment above method
After object matching score, it is believed that there is the closed loop region of current sub- map in the map currently built, which is exactly
The corresponding sub- map of candidate of the object matching score, when there are during closed loop region, it is believed that the position of terminal current accessed is
The position accessed, it is no longer necessary to which map structuring is repeated to the position.
Meet the first screening conditions it is understood that being also possible to be not present in the similitude matching score of the present embodiment
Object matching score, at this time it is believed that the position of terminal current accessed accessed not yet, i.e., on the history ground with formation
There is no the closed loop regions of current sub- map in figure.
It should be noted that closed loop detection method provided in this embodiment can be equivalent to a cycle execution method, as long as
Terminal is kept in motion, so that it may according to the operation of above-mentioned S101 formed current sub- map in real time and based on it is subsequent operate into
Row closed loop detects.
The closed loop detection method that the embodiment of the present invention one provides can be connected by laser radar capture setting in terminal first
The laser data of continuous frame forms current sub- map, then carries out current sub- map and the sub- map of candidate in the sub- atlas of history
Similitude matches, and matching process be the similitude matching score for determining current sub- map and each sub- map of candidate, and from above-mentioned
The object matching score for meeting screening matching condition is determined in each similitude matching score, finally determining object matching score corresponds to
The sub- map of candidate and the similarity of current sub- map meet the requirement of closed loop detection, it is thus achieved that the closed loop in map structuring
Detection.Using the above method, the angle that similarity mode can be carried out from sub- map and sub- map realizes a wide range of map structuring
When closed loop detection, compared with prior art, can effectively control the quantity of matching candidate collection, improve closed loop detection speed,
It ensure that the real-time that closed loop detects in a wide range of map building process, meanwhile, this method also effectively reduces closed loop detection
False drop rate improves the accuracy of closed loop detection.
Embodiment two
Fig. 2 a are a kind of flow charts of closed loop detection method provided by Embodiment 2 of the present invention, and the present embodiment two is above-mentioned each
It is optimized on the basis of embodiment.In the present embodiment, it will determine the current sub- map and select at least one candidate
The similitude matching score of sub- map, is further detailed as：Obtain the two dimension lookup for selecting at least one sub- map of candidate
Table, wherein, each two-dimensional look-up table is in advance based on setting the Gaussian Profile of successive frame laser data in the sub- map of corresponding candidate
Probability density is built；Determine current first matching score of the sub- map in each two-dimensional look-up table；By each described
One matching score is determined as the similitude matching score of the current sub- map and the sub- map of corresponding candidate.
Further, the present embodiment will also determine to meet the first matching screening conditions from each similitude matching score
Object matching score, advanced optimize for：Screening is greater than or equal to the first given threshold from each first matching score
The first intermediate match score, and the maximum value in the first intermediate match score is determined as object matching score.
In addition, the present embodiment also further will form current sub- map according to the laser data for capturing setting successive frame,
Specifically it is optimized for：The determining frame number that laser data is continuously captured based on laser radar in terminal；If the frame number reaches setting
It is worth, then the laser terminal point in captured each frame laser data forms current sub- map.
On the basis of above-mentioned optimization, the present embodiment, which also optimizes, to be increased：Using the current sub- map as new historical
Map is added in the sub- atlas of the history, and builds and store the two-dimensional look-up table of the sub- map of the new historical, wherein, institute
State Gaussian Profile probability density of the element value of two-dimensional look-up table to set successive frame laser data in the sub- map of the new historical
Value.
As shown in Figure 2 a, a kind of closed loop detection method provided by Embodiment 2 of the present invention, specifically comprises the following steps：
S201, it determines continuously to capture the frame number of laser data based on laser radar in terminal.
Specifically, in terminal motion process, can laser data be captured by laser radar thereon in real time, to carry out
Figure matches the operation for determining closed loop region with figure in the present embodiment, this step is tired out first after the sub- map of a nearest history is formed
Meter laser radar continuously captures the frame number of laser data.It is understood that the frame number of bulk registration can be in the present embodiment
Rezero operation is carried out after determining current sub- map, for determining for next current sub- map.
If S202, frame number reach setting value, the laser terminal point in captured each frame laser data, which is formed, works as
Preceding sub- map.
In the present embodiment, when accumulative frame number reaches setting value, it is believed that the current laser data energy continuously captured
A current sub- map is enough formed, thus this step can be according to the world coordinates of laser terminal point in captured each frame laser data
Current sub- map is presented in information in the form of grating map；Alternatively, distance corresponding to laser terminal point directly in each frame laser data
The current sub- map of the set expression of the value of information.
S203, acquisition select the two-dimensional look-up table of at least one sub- map of candidate.
In the present embodiment, the sub- map of candidate be equivalent to chosen from the sub- atlas of history with current sub- map into
The sub- map of row similarity mode, it should be noted that the present embodiment is that the sub- map of any history in the sub- atlas of history is pre-
Corresponding two-dimensional look-up table is first constructed, therefore this step can directly acquire the two dimension of selected at least one sub- map of candidate
Look-up table.Wherein, the two-dimensional look-up table of the sub- map of history is effectively equivalent to the another of to the history sub- map in the present embodiment
Grid forms of characterization is planted, current sub- map and sub- map (this reality of history corresponding to two-dimensional look-up table can be incited somebody to action by two-dimensional look-up table
It is mainly candidate sub- map to apply example) carry out similitude matching.
In the present embodiment, the corresponding two-dimensional look-up table of each sub- map of history can actually be regarded as and be based in the sub- atlas of history
Each laser terminal point structure in the history laser data of the sub- map of corresponding history.Specifically, for the sub- map of any history,
It can determine the history laser data of its corresponding setting successive frame, the present embodiment can be the sub- map initial setting of history one
The sufficiently large empty bivariate table of ranks number, and the upper left corner of the sky bivariate table can be regarded as origin, and set in the sky bivariate table
One cell represents a unit coordinate；It is then possible to by residing for laser radar when capturing any frame history laser data
Position as origin, hereby based on the corresponding distance information value of laser terminal point of each frame history laser data, pass through coordinate
Relationship conversion represents each laser terminal point in the sky bivariate table, at this point, the member of the occupied cell of each laser terminal point
Plain value is expressed as 1, and the element value of remaining element lattice still forms intermediate bivariate table for 0；Later, using Gaussian distribution model as each
The noise model of laser terminal point, and it is general for the element value progress Gaussian Profile in intermediate bivariate table based on the Gaussian distribution model
The calculating of rate density, and each Gaussian Profile probability density value obtained will be calculated as the corresponding new element value of each unit lattice, from
And build the two-dimensional look-up table of the sub- map of history of forming.
It should be noted that since the ranks number of the empty bivariate table of initial setting is sufficiently large, the present embodiment can based on
After Gaussian Profile probability density value improves the sky bivariate table, only retain element value there is no 0 row and columns to form final two
Tie up look-up table.Finally formed two-dimensional look-up table is equivalent to another characterization of the sub- map to history, to the structure of two-dimensional look-up table
It builds to be equivalent to and the sub- map of history is included into successive frame laser data in the form of likelihood ratio region shows, this kind of form can be more
The view effect of the good sub- map of presentation history.
S204, current first matching score of the sub- map in each two-dimensional look-up table is determined.
In the present embodiment, the laser terminal of the laser data of successive frame can be set according to corresponding to current sub- map
Point determines current first matching score of the sub- map in each two-dimensional look-up table.
Specifically, Fig. 2 b are to carry out the first matching score in closed loop detection method provided by Embodiment 2 of the present invention to determine
Implementation method flow chart.As shown in Figure 2 b, determine the current sub- map in each two-dimensional look-up table first
With score, following operation is specifically included：
S2041, translation and/or pivot rule are set based on first, in current sub- map in each two-dimensional look-up table
Laser terminal point is translated and/or is rotated.
Specifically, it is to obtain current first matching score of the sub- map in each two-dimensional look-up table, this implementation employs pair
The mode that each laser terminal point included by current sub- map is translated and/or rotated in each two-dimensional look-up table realizes, wherein,
The first setting translation and/or pivot rule, which are equivalent to, to be defined laser terminal point in current sub- map and is looked into each two dimension
Look for specific translation and/or the rotational order in table.
It should be noted that translation, rotation in the present embodiment and translation are put english, turn to be equivalent to will be in current sub- map
Laser terminal spot projection to three kinds of projection operation's modes in each two-dimensional look-up table, and these three projection operations be required into
It goes, the translation in the present embodiment is equivalent to laser terminal point flat with the translation variable progress transverse translation currently set or longitudinal direction
It moves, wherein, set translation variable can start from (0,0), then gradually increase translation variable with one step, and can be with
The form completion translation traversal that transverse translation carries out longitudinal translation again is first carried out, longitudinal translation can also be first carried out and carry out transverse direction again
The form of translation completes translation traversal or the form of the two mixing progress completes translation；Rotation in the present embodiment is equivalent to
Set angle increment rotation is carried out centered on origin to laser terminal point, the present embodiment has preferably set the section of angle rotation
Range can be the interval range between 0 ° to 180 °.
It is understood that this step can be preferably in a parallel fashion by the laser terminal point of current sub- map by upper
It states projection operation's mode to be projected in respectively in the corresponding two-dimensional look-up table of each sub- map of candidate, thus can be greatly reduced and match
The calculating time divided.
S2042, the current sub- map of acquisition is translated every time in each two-dimensional look-up table and/or postrotational first projects
Point.
In the present embodiment, each laser terminal point in current sub- map is once translated in each two-dimensional look-up table
And/or rotation, it is equivalent to a kind of transformational relation project to each laser terminal point in each two-dimensional look-up table, carries out one
After secondary projection, each laser terminal point will correspond to an element value in each two-dimensional look-up table, and all laser terminal points are worked as
Preceding corresponding element value carries out read group total, then the sum of element value can be determined as to current sub- map and searched in each two dimension
It is once translated in table and/or postrotational first projects score.
Illustratively, Fig. 2 c give the exemplary plot of the two-dimensional look-up table of a sub- map of candidate, as shown in Figure 2 c, search
Two-dimensional look-up table (be merely illustrative description) of the table 21 for one of candidate sub- map, it is assumed that current sub- map is by 2 laser
Distal point forms (being merely illustrative description), and according to the corresponding laser data for setting successive frame of current sub- map, determines
Stating 2 laser terminal point relative laser radars, (laser radar during capture any frame laser data sets the position of the laser radar
Be set to opposite origin) coordinate information (relative to the coordinate value of above-mentioned opposite origin) be respectively (1,2) and (2,3)；Assuming that pair
Above-mentioned laser terminal point (1,2) and (2,3) have carried out primary translation variable in look-up table 21 and have been projected for the translation of (1,0), then
It can determine that above-mentioned laser terminal point (1,2) and (2,3) have been respectively fallen in (2,2) and (3,3) of look-up table 21 after translation
In the two cells, and as shown in Figure 2 c, the element value of the two cells of (2,2) and (3,3) is respectively in look-up table 21
1.6 and 0.6, that is, corresponding element value is respectively 1.6 and 0.6, and can calculate to above-mentioned two laser terminal point at this time
The sum of two element values are 2.2, thus, it is believed that current sub- map carries out the first projection after this time translation in the look-up table 21
It is scored at 2.2.
S2043, score is projected based on each translation and/or postrotational first, determines that current sub- map is looked into each two dimension
The highest first looked in table projects score.
In the present embodiment, it can be determined every time to each laser terminal point in current sub- map each two based on aforesaid operations
Corresponding first projection score, this step can be corresponded to from each two-dimensional look-up table after being translated and/or rotated in dimension look-up table
Multiple first projections scores in determine that corresponding highest first projects score.
S2044, each highest first is projected to score it is determined as first of current sub- map in respective two-dimensional look-up table
With score.
The highest determined from each two-dimensional look-up table first can be projected score and be denoted as current sub- map in phase by this step
Answer the first matching score in two-dimensional look-up table.
S205, it each first matching score is determined as current sub- map is matched with the similitude of the sub- map of corresponding candidate
Point.
In the present embodiment, current sub- map first matching in each two-dimensional look-up table is determined based on above-mentioned S204
After point, it may be determined that the corresponding two-dimensional look-up table of each first matching score, while can also determine that each two-dimensional look-up table is corresponding
Candidate sub- map, thus can determine the corresponding sub- map of candidate of each first matching score, and using each first matching score as phase
Should similitude matching score of candidate sub- map when match with current sub- map similitude, it is achieved thereby that current sub- map and respectively
The similitude matching of candidate sub- map.
S206, screening is greater than or equal to the first intermediate match score of the first given threshold from each first matching score,
And the maximum value in the first intermediate match score is determined as object matching score.
In the present embodiment, it is equivalent to and the first matching screening conditions is set greater than or equal to the first given threshold, sheet
First given threshold described in embodiment can be manually set, the operation based on this step, it is possible to determine that above-mentioned determining each
It whether there is the first intermediate match score more than or equal to the first given threshold in one matching score, if it is present needing
It further determines that out the maximum value in the first intermediate match score, and the maximum value is determined as to the target for meeting closed loop requirement
With score.
It is understood that if above-mentioned determining each first matching score is respectively less than the first given threshold, it is believed that
There is no object matching score, at this time it is believed that there is no the closed loop areas of current sub- map in the established sub- atlas of history
Domain, that is, it is believed that the currently accessed position of terminal did not accessed before, thus can will current sub- map structuring to entirely
In local figure.
S207, if it is determined that there are object matching score, then will be determined corresponding to the sub- map of candidate of object matching score
Closed loop region for current sub- map.
In the present embodiment, after determining object matching score based on S206, it may be determined that object matching score corresponding two
Look-up table is tieed up, and thereby determines that the corresponding sub- map of candidate, most the corresponding sub- map of candidate of the object matching score is true at last
It is set to the closed loop region of current sub- map, at this time, it is believed that the currently accessed position of terminal had been accessed before, thus
It can be to establishing the currently restriction relation of sub- map and the sub- map of the candidate, in order to realize that the map in global map structure is excellent
Change.
S208, current sub- map is added to as the sub- map of new historical in the sub- atlas of the history, and build and deposit
Store up the two-dimensional look-up table of the sub- map of new historical.
It is understood that the operation based on above-mentioned S201 to S207, it can be by figure with scheming matched mode come to packet
The current sub- map of the present position containing terminal carries out closed loop detection, and when terminal is constantly in operating status, terminal is worked as
The operation that preceding position carries out closed loop detection will be carried out constantly, this step can be in the closed loop for realizing current sub- map as a result,
After detection, current sub- map is added to as a sub- map of new historical in the sub- atlas of history, can subsequently continue to return
S201 forms the current sub- map of next pending closed loop detection.
In addition, current sub- map is added to as the sub- map of new historical after the sub- atlas of history, it is also necessary to be new historical
Sub- map carries out the structure of two-dimensional look-up table, for the follow-up new closed loop detection for forming current sub- map.Wherein, the two dimension
The element value of look-up table is the Gaussian Profile probability density value that successive frame laser data is set in the sub- map of the new historical.Two dimension
The specific construction step of look-up table is described after above-mentioned S203, and which is not described herein again.
It should be noted that a kind of closed loop detection method provided in this embodiment, core concept are through sub- map
Closed loop detection is carried out with the similarity mode of sub- map.When carrying out closed loop detection using this method, pass through the son ground of structure first
Figure can remove the redundancy between successive frame laser data well, so that carrying out required when figure is matched with figure similitude
Calculation amount be significantly less than calculation amount of existing progress frame when being matched with frame or frame with the similitude of figure, subtract so as to reach
The matched calculation amount of few similitude improves the purpose of similitude matching efficiency.Further, since contain successive frame in sub- map
Laser data, so that the information content included in sub- map is more than the informative that single frames laser data is included, so that
Comparable corner quantity increases when similitude matches, and then can overcome well and be led because the height of map part is similar
The problem of causing false judgment.
A kind of closed loop detection method provided by Embodiment 2 of the present invention embodies the determining operation of current sub- map, together
When embody the determining operation of similitude matching score and embody the determining operation of object matching score, additionally
Increase the update operation of atlas.Using this method, effectively reduce and reduce the matched calculation amount of similitude, improve similar
Property matching efficiency, ensure that the real-time that closed loop in a wide range of map building process detects, at the same also preferably overcome because
The height of map part is similar and the problem of lead to false judgment, improve the accuracy of closed loop detection.
Embodiment three
Fig. 3 a are a kind of flow charts for closed loop detection method that the embodiment of the present invention three provides, and the embodiment of the present invention is with above-mentioned
It is optimized based on embodiment, in the present embodiment, specific optimization increases：According to setting screening rule from the sub- map of history
It concentrates and screens at least one sub- map of candidate.
As shown in Figure 3a, a kind of closed loop detection method that the embodiment of the present invention three provides, specifically includes following operation：
S301, current sub- map is formed according to the laser data for capturing setting successive frame.
S302, at least one sub- map of candidate is screened from the sub- atlas of history according to setting screening rule.
The present embodiment is general to consider current sub- map and the sub- map of history in order to carry out closed loop detection to current sub- map
The sub- map of each history is concentrated to carry out similitude matching, however, when the sub- map quantity of history is more, by current sub- map with owning
The sub- map of history all carry out similitude matching need to expend more match time, influence closed loop detection real-time.
Operation based on this step can be filtered out from the sub- atlas of history and more likely be closed with current sub- map formation
The sub- map of candidate of ring region so as to only consider that current sub- map and the similitude of candidate sub- map match, is thus reduced
The matched calculation amount of similitude improves the real-time of closed loop detection.
The setting thinking of setting screening rule in this step can be that determined from the sub- atlas of history first can be with
The sub- map to be matched of rough matching is carried out with current sub- map, then according to rough matching result again from multiple son ground to be matched
It is filtered out in figure and carries out the matched sub- map of candidate of similitude.
Specifically, Fig. 3 b are that the realization that candidate information determines is carried out in the closed loop detection method that the embodiment of the present invention three provides
Method flow diagram.As shown in Figure 3b, it is described that at least one candidate son is screened from the sub- atlas of history according to setting screening rule
Map specifically includes following operation：
S3021, the positioning screening strategy according to setting, from history sub- atlas screening correspond to current sub- map to
Few two sub- maps to be matched.
The estimated location that this step considers to be currently located from terminal carries out the screening of sub- map to be matched.
Further, the positioning screening strategy according to setting, screening is corresponding to institute from the history sub- atlas
At least two sub- maps to be matched of current sub- map are stated, including：Determine that the terminal exists based on the system odometer in terminal
Current estimated location in constructed global map；Obtain in the sub- atlas of the history each sub- map of history it is described globally
Historical position in figure；Based on each historical position and the current estimated location, determine each sub- map of the history to institute
State the range information of current sub- map；The sub- map of history that the range information is less than to setpoint distance threshold value is determined as described work as
The sub- map to be matched of preceding sub- map.
In the present embodiment, the system odometer is equivalent to a hardware device in terminal, can be current to terminal
Position carry out rough estimate, it is possible thereby to which the present embodiment can obtain the current estimated location of terminal, while can also obtain
Historical position of each sub- map of history in global map in the sub- atlas of history, may finally determine apart from terminal be less than set
The sub- map to be matched of set a distance threshold value.
It should be noted that since system odometer is there are error in actual environment, with the accumulation robot of error
Estimated location is more and more unreliable, can not effectively reduce the range of matching candidate collection, thus can by selected figure optimization method,
Such as standard drawing optimization algorithm (General Graph Optimization, g2o), meter is optimized to the estimated location of terminal
It calculates, to reduce the error distance of terminal current estimated location and actual position, so as to ensure that the closed loop in a wide range of map detects
Similitude matching speed will not be reduced due to data are accumulated.
S3032, the corresponding two dimensional compaction table of each sub- map to be matched is obtained.
The present embodiment also accordingly constructs while history each in for the sub- map of history sub- map structuring two-dimensional look-up table
Two dimensional compaction table, to realize the rough matching of current sub- map and sub- map to be matched by two dimensional compaction table.
Wherein, the two dimensional compaction table passes through the maximum value pond to two-dimensional look-up table corresponding to each sub- map to be matched
Change and calculate generation.Specifically, for the two-dimensional look-up table of the sub- map of each history, the mode that maximum pond may be used is searched
The compression optimization of table generates corresponding two dimensional compaction table, it is to be understood that the two dimensional compaction table that is generated is practical to be equivalent to pair
Two-dimensional look-up table reduce the operation of resolution ratio, considers the two dimensional compaction table by sub- map to be matched and current son ground first
Figure carries out rough matching, can effectively promote the whole similitude matching efficiency of closed loop detection.
The multiple that the present embodiment follows 2 powers carrys out the two-dimensional look-up table of the sub- map of compression histories, it is assumed that d represents times of compression
Number, H and L represent the cell number of two-dimensional look-up table and two dimensional compaction table, then the cell format of two-dimensional look-up table is pressed with two dimension
There are following relationships under different compression multiples for the cell number of contracting table, that is, L=H ÷ 2d；Preferably, the present embodiment is subtracts
Loss of significance caused by few two dimensional compaction table determines the element of each unit lattice in two dimensional compaction table using maximum value pond method
Value chooses element maximum value, using element maximum value as two dimensional compaction table to be formed that is, in the multiple cells to be merged
Element value in cell.
The present embodiment preferably sets the value of d as 2, and illustratively, Fig. 3 c are given based on two-dimensional look-up table generation two
The implementation example figure of compaction table is tieed up, as shown in Figure 3c, after look-up table 31 represents that two-dimensional look-up table, compaction table 32 represent that compression merges
Two dimensional compaction table.It can be found that first element value in compaction table 32 is the element in four cells in look-up table 31
Maximum value.
S3023, current second matching score of the sub- map in each two dimensional compaction table is determined, and from each second matching score
In determine it is at least one meet second screening matching condition candidate matches score.
This step is equivalent to the thick matching that the two dimensional compaction table based on sub- map to be matched is carried out with current sub- map, this step
Suddenly the laser terminal point of the laser data of successive frame can be equally set according to corresponding to current sub- map, determines that current sub- map exists
The second matching score in each two dimensional compaction table.
Specifically, determining current second matching score of the sub- map in each two dimensional compaction table, and from
At least one candidate matches score for meeting the second screening matching condition is determined in each second matching score, including：It is based on
Second setting translation and/or pivot rule, to the laser terminal point in the current sub- map in each two dimensional compaction table
It is translated and/or is rotated；Obtain current sub- map translation and/or postrotational every time in each two dimensional compaction table
Second projection score；Based on each translation and/or postrotational second projection score, determine the current sub- map each described
Highest second in two dimensional compaction table projects score；Each highest second is projected into score it is determined as the current sub- map and exists
The second matching score in respective two-dimensional compaction table；When being greater than or equal to there are at least one in each second matching score
During the second intermediate match score of two given thresholds, each second intermediate match score is determined as candidate matches score；It is no
Then, the highest matching score in each second matching score is determined as candidate matches score.
In the present embodiment, the specific implementation operation of current second matching score of the sub- map in each two dimensional compaction table is determined
It is identical with mode used by above-mentioned determining current first matching score of the sub- map in each two-dimensional look-up table, above-mentioned determining side
Formula is described in detail after the step of S204, and which is not described herein again.
It should be noted that this step can determine candidate matches score by two ways, that is, can be in the second matching
When there is the second intermediate match score more than or equal to the second given threshold in score, directly by the determine second centre
Regard candidate matches score as with score；It is also possible to when the second matching score is respectively less than the second given threshold, by second
Regard candidate matches score as with the highest matching score in score.It can be found that this step can at least determine a candidate
Matching score.
S3024, the corresponding sub- map to be matched of each candidate matches score is determined as candidate sub- map.
This step can determine the corresponding two dimensional compaction table of each candidate matches score, while can determine these two dimensional compactions
The corresponding sub- map to be matched of table, it is possible thereby to which the sub- map to be matched corresponding to each candidate matches score filtered out is determined
For candidate sub- map.
S303, it determines current sub- map and selectes the similitude matching score of at least one sub- map of candidate.
S304, the object matching score for meeting the first matching screening conditions is determined from each similitude matching score.
S305, if it is determined that there are the object matching score, then will be corresponding to candidate's of the object matching score
Map is determined as the closed loop region of the current sub- map.
S306, current sub- map is added to as the sub- map of new historical in the sub- atlas of history, and build and store and is new
The two-dimensional look-up table of the sub- map of history.
It should be noted that closed loop detection method provided in an embodiment of the present invention, matches with traditional frame with frame similitude
Algorithm compare, not only increase closed loop detection calculating speed, also effectively increase closed loop detection accuracy rate.It is exemplary
, Fig. 3 d give the display figure that first laser data are presented in the form of grating map；Fig. 3 e give second laser data with
The display figure that grating map form is presented.Traditional frame is employed herein with the matched algorithm of frame similitude in Fig. 3 d and Fig. 3 e
Two frame laser datas carry out similitude matching, it is found that closed based on traditional frame and the matched algorithm of frame similitude
When ring detects, it may be determined that the laser data that the first circle 33 gone out in Fig. 3 d includes in the second circle 34 in Fig. 3 e with wrapping
The laser data included has higher similitude, thus just determines that the region that the first laser data in Fig. 3 d represent is equivalent to figure
The closed loop region in region represented by second laser data in 3e.
Meanwhile Fig. 3 f give the display that the first sub- map comprising first laser data is presented in the form of grating map
Figure；Fig. 3 g give the display figure that the second sub- map comprising first laser data is presented in the form of grating map.It uses herein
Closed loop detection method provided in this embodiment carries out similitude matchings, Ke Yifa to two sub- maps in Fig. 3 f and Fig. 3 g
It is existing, when the closed loop detection method based on the present embodiment carries out closed loop detection, laser number that the third circle 35 in Fig. 3 f includes
According to the laser data that the first circle 33 being equivalent in Fig. 3 d includes, laser data phase that the 4th circle 36 in Fig. 3 g includes
When the laser data that the second circle 34 in Fig. 3 e includes, at this point, although the laser data that includes of third circle 35 with
The laser data that the 4th circle 36 in Fig. 3 g includes still has higher similarity score, but other parts in the first sub- map
It is relatively low with the similarity score of other parts in the second sub- map, so that the entirety of the first sub- map and the second sub- map
Similitude matching score during matching cannot meet the first screening conditions of the present embodiment setting, thus without by the first sub- map
It is determined as the closed loop region of the second sub- map.By above-mentioned comparison, it can be found that can after closed loop provided in this embodiment detection
More accurately carry out closed loop detection.
A kind of closed loop detection method that the embodiment of the present invention three provides specifically increases the screening operation of candidate sub- map,
And it gives the specific of candidate sub- map and determines operation.Using this method, most possible and current sub- map structure can be filtered out
Into the sub- map of candidate of closed loop relationship, so as to largely reduce calculation amount when figure is matched with figure, ensure that on a large scale
Closed loop detection in figure will not reduce similitude matching speed due to data are accumulated, and then effectively increase the real-time of closed loop detection
Property, at the same also preferably overcome because the height of map part is similar and the problem of lead to false judgment, improve closed loop and examine
The accuracy of survey.
Example IV
Fig. 4 is a kind of structure diagram for loop detector that the embodiment of the present invention four provides, and the present embodiment is applicable
In the situation that terminal present position and historical position are carried out to closed loop detection in map structuring, which can be by hardware
And/or software is realized, and is typically integrated in terminal.As shown in figure 4, the device includes：It is sub- map structuring module 41, similar
Divide determining module 42, target score determining module 43 and closed loop area determination module 44.
Wherein, sub- map structuring module 41, for forming current son ground according to the laser data for capturing setting successive frame
Figure；
Similar score determining module 42, for determining the current sub- map and selecting at least one sub- map of candidate
Similitude matching score；
Target score determining module 43, for determining to meet the first matching screening item from each similitude matching score
The object matching score of part；
Closed loop area determination module 44, for when determining, there are during the object matching score, the target will be corresponded to
The sub- map of candidate of matching score is determined as the closed loop region of the current sub- map.
In the present embodiment, the device first by sub- map structuring module 41 according to capture setting successive frame laser
Data form current sub- map；Then the current sub- map is determined by similar score determining module 42 and selectes at least one
The similitude matching score of a sub- map of candidate；Later by target score determining module 43 from each similitude matching score
In determine meet first matching screening conditions object matching score；Exist eventually by closed loop area determination module 44 when determining
During the object matching score, the current sub- map will be determined as corresponding to the sub- map of candidate of the object matching score
Closed loop region.
A kind of loop detector that the embodiment of the present invention four provides carries out similarity mode from sub- map and sub- map
Angle realizes closed loop detection during a wide range of map structuring, compared with prior art, can effectively control matching candidate collection
Quantity improves closed loop detection speed, ensure that the real-time that closed loop detects in a wide range of map building process, meanwhile, the dress
The false drop rate for also effectively reducing closed loop detection is put, improves the accuracy of closed loop detection.
Further, similar score determining module 42, including：
Look-up table acquiring unit, for obtaining the two-dimensional look-up table for selecting at least one sub- map of candidate, wherein, each institute
State the Gaussian Profile probability density structure that two-dimensional look-up table is in advance based on setting successive frame laser data in the sub- map of corresponding candidate；
First score determination unit, for determining first matching of the current sub- map in each two-dimensional look-up table
Score；
Similar score acquiring unit is waited for each first matching score to be determined as the current sub- map with corresponding
Select the similitude matching score of sub- map.
On the basis of above-mentioned optimization, the first score determination unit is specifically used for：
Based on the first setting translation and/or pivot rule, in the current sub- map in each two-dimensional look-up table
Laser terminal point translated and/or rotated；The current sub- map is obtained to translate every time in each two-dimensional look-up table
And/or postrotational first projection score；Based on each translation and/or postrotational first projection score, determine described current
Highest first of the sub- map in each two-dimensional look-up table projects score；Each highest first is projected into score and is determined as institute
State current first matching score of the sub- map in respective two-dimensional look-up table.
Further, target score determining module 43, is specifically used for：
Screening is greater than or equal to the first intermediate match score of the first given threshold from each first matching score, and
Maximum value in first intermediate match score is determined as object matching score.
Further, the present embodiment also optimizes and includes：Candidate information determining module 45,
Wherein, candidate information determining module 45, for being screened at least from the sub- atlas of history according to setting screening rule
One sub- map of candidate.
On the basis of above-mentioned optimization, candidate information determining module 45, including：
Screening unit is positioned, for the positioning screening strategy according to setting, screens and corresponds to from the sub- atlas of the history
In at least two sub- maps to be matched of the current sub- map；
Compaction table acquiring unit, for obtaining the corresponding two dimensional compaction table of each sub- map to be matched, wherein, described two
It ties up compaction table and generation is calculated by the maximum value pondization to two-dimensional look-up table corresponding to each sub- map to be matched；
Candidate score determination unit, for determining second matching of the current sub- map in each two dimensional compaction table
Score, and at least one candidate matches score for meeting the second screening matching condition is determined from each second matching score；
Candidate map determination unit, for the corresponding sub- map to be matched of each candidate matches score to be determined as candidate
Sub- map.
Further, the positioning screening unit, is specifically used for：
Current estimated location of the terminal in constructed global map is determined based on the system odometer in terminal；It obtains
Take historical position of each sub- map of history in the global map in the sub- atlas of the history；Based on each historical position
And the current estimated location, determine each sub- map of the history to the range information of the current sub- map；By the distance
Information is determined as the sub- map to be matched of the current sub- map less than the sub- map of history of setpoint distance threshold value.
Meanwhile candidate score determination unit, it is specifically used for：
Based on the second setting translation and/or pivot rule, in the current sub- map in each two dimensional compaction table
Laser terminal point translated and/or rotated；The current sub- map is obtained to translate every time in each two dimensional compaction table
And/or postrotational second projection score；Based on each translation and/or postrotational second projection score, determine described current
Highest second of the sub- map in each two dimensional compaction table projects score；Each highest second is projected into score and is determined as institute
State current second matching score of the sub- map in respective two-dimensional compaction table；When in each second matching score exist at least one
It is a be greater than or equal to the second given threshold the second intermediate match score when, each second intermediate match score is determined as waiting
Select matching score；Otherwise, the highest matching score in each second matching score is determined as candidate matches score.
Further, sub- map structuring module 41, is specifically used for：
The determining frame number that laser data is continuously captured based on laser radar in terminal；If the frame number reaches setting value,
Then the laser terminal point in captured each frame laser data forms current sub- map.
On the basis of above-mentioned optimization, which further includes：
Atlas update module 46, for the current sub- map to be added to history as the sub- map of new historical
In atlas, and the two-dimensional look-up table of the sub- map of the new historical is built and stores, wherein, the element value of the two-dimensional look-up table
To set the Gaussian Profile probability density value of successive frame laser data in the sub- map of the new historical.
The said goods can perform the method that any embodiment of the present invention is provided, and have the corresponding function module of execution method
And advantageous effect.
Embodiment five
Fig. 5 is a kind of hardware architecture diagram for terminal that the embodiment of the present invention five provides.As shown in figure 5, the terminal packet
It includes：Laser radar 51 and system odometer 52, further include：One or more processors 53 are with a processor 53 in Fig. 5
Example；Storage device 54；The terminal can also include：Input unit 55 and output device 56.
Laser radar 51, system odometer 52, processor 53, storage device 54,55 and of input unit in the terminal
Output device 56 can be connected by bus or other modes, in Fig. 5 for being connected by bus.
Storage device 54 is used as a kind of non-transient computer readable storage medium storing program for executing, available for storage software program, computer
Executable program and module, if the corresponding program instruction/module of closed loop detection method in the embodiment of the present invention is (for example, attached
Sub- map structuring module 41 shown in Fig. 4, similar score determining module 42, target score determining module 43 and closed loop region are true
Cover half block 44, further includes：Candidate information determining module 45 and atlas update module 46).Processor 53 is stored by running
Software program, instruction and module in storage device 54, so as to perform the various function application of terminal and data processing,
Realize the closed loop detection method of above method embodiment.
Storage device 54 can include storing program area and storage data field, wherein, storing program area can store operation system
System, the required application program of at least one function；Storage data field can be stored uses created data etc. according to terminal.
In addition, storage device 54 can include high-speed random access memory, can also include non-transitory memory, for example, at least one
A disk memory, flush memory device or other non-transitory solid-state memories.In some embodiments, storage device 54
It is optional including relative to the remotely located memory of processor 53, these remote memories can be set by network connection to terminal
It is standby.The example of above-mentioned network includes but not limited to internet, intranet, LAN, mobile radio communication and combinations thereof.
Also, when one or more program included by above-mentioned terminal is performed by one or more of processors 53
When, program proceeds as follows：Laser data according to setting successive frame is captured forms current sub- map；It determines described current
Sub- map and the similitude matching score for selecting at least one sub- map of candidate；It is determined from each similitude matching score
Meet the object matching score of the first matching screening conditions；If it is determined that there are the object matching score, then it will correspond to institute
The sub- map of candidate for stating object matching score is determined as the closed loop region of the current sub- map.
In addition, input unit 55 can be used for receiving the number inputted or character information and the user of generation and terminal sets
It puts and the input of key signals that function control is related.Output device 56 may include that display screen etc. shows equipment.
In addition, the embodiment of the present invention also provides a kind of computer readable storage medium, computer program is stored thereon with, it should
Realize that the embodiment of the present invention one or embodiment two provide closed loop detection method when program is executed by processor, this method includes：Root
Current sub- map is formed according to the laser data for capturing setting successive frame；Determine the current sub- map with it is selected at least one
The similitude matching score of candidate sub- map；It determines to meet the first matching screening conditions from each similitude matching score
Object matching score；If it is determined that there are the object matching score, then it will be corresponding to candidate's of the object matching score
Map is determined as the closed loop region of the current sub- map.
By the description above with respect to embodiment, it is apparent to those skilled in the art that, the present invention
It can be realized by software and required common hardware, naturally it is also possible to which by hardware realization, but the former is more in many cases
Good embodiment.Based on such understanding, what technical scheme of the present invention substantially in other words contributed to the prior art
Part can be embodied in the form of software product, which can be stored in computer readable storage medium
In, floppy disk, read-only memory (Read-Only Memory, ROM), random access memory (Random such as computer
Access Memory, RAM), flash memory (FLASH), hard disk or CD etc., including some instructions with so that a computer is set
Standby (can be personal computer, server or the network equipment etc.) performs the method described in each embodiment of the present invention.
Note that it above are only presently preferred embodiments of the present invention and institute's application technology principle.It will be appreciated by those skilled in the art that
The present invention is not limited to specific embodiment described here, can carry out for a person skilled in the art various apparent variations,
It readjusts and substitutes without departing from protection scope of the present invention.Therefore, although being carried out by above example to the present invention
It is described in further detail, but the present invention is not limited only to above example, without departing from the inventive concept, also
It can include other more equivalent embodiments, and the scope of the present invention is determined by scope of the appended claims.
Claims (13)
1. a kind of closed loop detection method, which is characterized in that including：
Laser data according to setting successive frame is captured forms current sub- map；
It determines the current sub- map and selectes the similitude matching score of at least one sub- map of candidate；
The object matching score for meeting the first matching screening conditions is determined from each similitude matching score；
If it is determined that there are the object matching score, then will be determined as corresponding to the sub- map of candidate of the object matching score
The closed loop region of the current sub- map.
2. according to the method described in claim 1, it is characterized in that, described determine the current sub- map and select at least one
The similitude matching score of a sub- map of candidate, including：
The two-dimensional look-up table for selecting at least one sub- map of candidate is obtained, wherein, each two-dimensional look-up table is in advance based on phase
Answer the Gaussian Profile probability density structure of setting successive frame laser data in candidate sub- map；
Determine current first matching score of the sub- map in each two-dimensional look-up table；
Each first matching score is determined as to the similitude matching score of the current sub- map and the sub- map of corresponding candidate.
3. according to the method described in claim 2, it is characterized in that, described determine that the current sub- map is looked into each two dimension
The first matching score in table is looked for, including：
Based on the first setting translation and/or pivot rule, to swashing in the current sub- map in each two-dimensional look-up table
Light distal point is translated and/or is rotated；
Obtain the current sub- map translation and/or postrotational first projection score every time in each two-dimensional look-up table；
Based on each translation and/or postrotational first projection score, determine that the current sub- map is searched in each two dimension
Highest first in table projects score；
Each highest first is projected into score and is determined as first matching of the current sub- map in respective two-dimensional look-up table
Score.
4. according to the method described in claim 2, it is characterized in that, described determine to meet from each similitude matching score
The object matching score of first matching screening conditions, including：
From each first matching score screening more than or equal to the first given threshold the first intermediate match score, and by the
Maximum value in one intermediate match score is determined as object matching score.
5. it according to the method described in claim 2, it is characterized in that, further includes：
At least one sub- map of candidate is screened from the sub- atlas of history according to setting screening rule.
6. according to the method described in claim 5, it is characterized in that, it is described according to setting screening rule from the sub- atlas of history
At least one sub- map of candidate is screened, including：
According to the positioning screening strategy of setting, screened from the sub- atlas of the history and correspond to the current sub- map at least
Two sub- maps to be matched；
The corresponding two dimensional compaction table of each sub- map to be matched is obtained, wherein, the two dimensional compaction table to each by described treating
The maximum value pondization for matching two-dimensional look-up table corresponding to sub- map calculates generation；
It determines current second matching score of the sub- map in each two dimensional compaction table, and is matched from each described second
At least one candidate matches score for meeting the second screening matching condition is determined in point；
The corresponding sub- map to be matched of each candidate matches score is determined as candidate sub- map.
7. according to the method described in claim 6, it is characterized in that, the positioning screening strategy according to setting, goes through from described
Screening corresponds at least two sub- maps to be matched of the current sub- map in the sub- atlas of history, including：
Current estimated location of the terminal in constructed global map is determined based on the system odometer in terminal；
Obtain historical position of each sub- map of history in the global map in the sub- atlas of the history；
Based on each historical position and the current estimated location, determine each sub- map of the history to the current sub- map
Range information；
The range information is determined as to the son to be matched of the current sub- map less than the sub- map of history of setpoint distance threshold value
Map.
8. according to the method described in claim 6, it is characterized in that, described determine the current sub- map in each two dimension pressure
The second matching score in contracting table, and at least one the second screening of satisfaction matching condition is determined from each second matching score
Candidate matches score, including：
Based on the second setting translation and/or pivot rule, to swashing in the current sub- map in each two dimensional compaction table
Light distal point is translated and/or is rotated；
Obtain the current sub- map translation and/or postrotational second projection score every time in each two dimensional compaction table；
Based on each translation and/or postrotational second projection score, determine the current sub- map in each two dimensional compaction
Highest second in table projects score；
Each highest second is projected into score and is determined as second matching of the current sub- map in respective two-dimensional compaction table
Score；
It to be obtained when there are at least one the second intermediate match for being greater than or equal to the second given threshold in each second matching score
Each second intermediate match score is determined as candidate matches score by timesharing；It otherwise, will be in each second matching score
Highest matching score is determined as candidate matches score.
It is 9. according to the method described in claim 1, it is characterized in that, described according to the laser data shape for capturing setting successive frame
Into current sub- map, including：
The determining frame number that laser data is continuously captured based on laser radar in terminal；
If the frame number reaches setting value, the laser terminal point in captured each frame laser data forms current son ground
Figure.
10. according to claim 1-9 any one of them methods, which is characterized in that further include：
The current sub- map is added to as the sub- map of new historical in the sub- atlas of the history, and described in structure and storage
The two-dimensional look-up table of the sub- map of new historical, wherein, the element value of the two-dimensional look-up table is sets in the sub- map of the new historical
The Gaussian Profile probability density value of successive frame laser data.
11. a kind of loop detector, which is characterized in that including：
Sub- map structuring module, for forming current sub- map according to the laser data for capturing setting successive frame；
Similar score determining module, for determining the current sub- map and selecting the similitude of at least one sub- map of candidate
Matching score；
Target score determining module, for determining to meet the mesh of the first matching screening conditions from each similitude matching score
Mark matching score；
Closed loop area determination module, for when determining, there are during the object matching score, to obtain corresponding to the object matching
The sub- map of candidate divided is determined as the closed loop region of the current sub- map.
12. a kind of terminal, including：Laser radar and system odometer, which is characterized in that including：
One or more processors；
Storage device, for storing one or more programs；
One or more of programs are performed by one or more of processors so that one or more of processors are realized
Closed loop detection method as described in claim 1-10 is any.
13. a kind of computer readable storage medium, is stored thereon with computer program, which is characterized in that the program is by processor
The closed loop detection method as described in claim 1-10 is any is realized during execution.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201810039214.8A CN108256060B (en) | 2018-01-16 | 2018-01-16 | Closed loop detection method, device, terminal and storage medium |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201810039214.8A CN108256060B (en) | 2018-01-16 | 2018-01-16 | Closed loop detection method, device, terminal and storage medium |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN108256060A true CN108256060A (en) | 2018-07-06 |
| CN108256060B CN108256060B (en) | 2021-02-09 |
Family
ID=62741305
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201810039214.8A Active CN108256060B (en) | 2018-01-16 | 2018-01-16 | Closed loop detection method, device, terminal and storage medium |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN108256060B (en) |
Cited By (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109614459A (en) * | 2019-03-06 | 2019-04-12 | 上海思岚科技有限公司 | Map structuring winding detection method and equipment applied to two-dimensional laser |
| CN110376605A (en) * | 2018-09-18 | 2019-10-25 | 北京京东尚科信息技术有限公司 | Map constructing method, air navigation aid and device |
| CN110858076A (en) * | 2018-08-22 | 2020-03-03 | 杭州海康机器人技术有限公司 | Equipment positioning and grid map construction method and mobile robot |
| CN111538918A (en) * | 2020-04-23 | 2020-08-14 | 上海高仙自动化科技发展有限公司 | Recommendation method and device, electronic equipment and storage medium |
| US20200319342A1 (en) * | 2019-04-02 | 2020-10-08 | Quanta Computer Inc. | Positioning system of mobile device |
| CN111795687A (en) * | 2020-06-29 | 2020-10-20 | 深圳市优必选科技股份有限公司 | Robot map updating method and device, readable storage medium and robot |
| CN112102400A (en) * | 2020-09-15 | 2020-12-18 | 上海云绅智能科技有限公司 | Distance-based closed loop detection method and device, electronic equipment and storage medium |
| CN112668653A (en) * | 2020-12-30 | 2021-04-16 | 北京百度网讯科技有限公司 | Loop detection method, device, equipment and medium based on laser radar map |
| WO2024007807A1 (en) * | 2022-07-06 | 2024-01-11 | 杭州萤石软件有限公司 | Error correction method and apparatus, and mobile device |
Citations (11)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103900555A (en) * | 2012-12-30 | 2014-07-02 | 联想(北京)有限公司 | Mobile electronic device and location method of mobile electronic device |
| CN104374395A (en) * | 2014-03-31 | 2015-02-25 | 南京邮电大学 | Graph-based vision SLAM (simultaneous localization and mapping) method |
| CN105678842A (en) * | 2016-01-11 | 2016-06-15 | 湖南拓视觉信息技术有限公司 | Manufacturing method and device for three-dimensional map of indoor environment |
| CN105760811A (en) * | 2016-01-05 | 2016-07-13 | 福州华鹰重工机械有限公司 | Global map closed loop matching method and device |
| CN105758408A (en) * | 2016-01-05 | 2016-07-13 | 福州华鹰重工机械有限公司 | Method and device for building local maps |
| CN105856230A (en) * | 2016-05-06 | 2016-08-17 | 简燕梅 | ORB key frame closed-loop detection SLAM method capable of improving consistency of position and pose of robot |
| CN106092104A (en) * | 2016-08-26 | 2016-11-09 | 深圳微服机器人科技有限公司 | The method for relocating of a kind of Indoor Robot and device |
| CN106153031A (en) * | 2015-04-13 | 2016-11-23 | 骑记(厦门)科技有限公司 | Movement locus method for expressing and device |
| CN106323273A (en) * | 2016-08-26 | 2017-01-11 | 深圳微服机器人科技有限公司 | Robot relocation method and device |
| CN106937531A (en) * | 2014-06-14 | 2017-07-07 | 奇跃公司 | Method and system for producing virtual and augmented reality |
| CN107561549A (en) * | 2017-08-17 | 2018-01-09 | 广州视源电子科技股份有限公司 | Method and device for relocating terminal position, terminal and storage medium |
-
2018
- 2018-01-16 CN CN201810039214.8A patent/CN108256060B/en active Active
Patent Citations (11)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103900555A (en) * | 2012-12-30 | 2014-07-02 | 联想(北京)有限公司 | Mobile electronic device and location method of mobile electronic device |
| CN104374395A (en) * | 2014-03-31 | 2015-02-25 | 南京邮电大学 | Graph-based vision SLAM (simultaneous localization and mapping) method |
| CN106937531A (en) * | 2014-06-14 | 2017-07-07 | 奇跃公司 | Method and system for producing virtual and augmented reality |
| CN106153031A (en) * | 2015-04-13 | 2016-11-23 | 骑记(厦门)科技有限公司 | Movement locus method for expressing and device |
| CN105760811A (en) * | 2016-01-05 | 2016-07-13 | 福州华鹰重工机械有限公司 | Global map closed loop matching method and device |
| CN105758408A (en) * | 2016-01-05 | 2016-07-13 | 福州华鹰重工机械有限公司 | Method and device for building local maps |
| CN105678842A (en) * | 2016-01-11 | 2016-06-15 | 湖南拓视觉信息技术有限公司 | Manufacturing method and device for three-dimensional map of indoor environment |
| CN105856230A (en) * | 2016-05-06 | 2016-08-17 | 简燕梅 | ORB key frame closed-loop detection SLAM method capable of improving consistency of position and pose of robot |
| CN106092104A (en) * | 2016-08-26 | 2016-11-09 | 深圳微服机器人科技有限公司 | The method for relocating of a kind of Indoor Robot and device |
| CN106323273A (en) * | 2016-08-26 | 2017-01-11 | 深圳微服机器人科技有限公司 | Robot relocation method and device |
| CN107561549A (en) * | 2017-08-17 | 2018-01-09 | 广州视源电子科技股份有限公司 | Method and device for relocating terminal position, terminal and storage medium |
Cited By (12)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110858076A (en) * | 2018-08-22 | 2020-03-03 | 杭州海康机器人技术有限公司 | Equipment positioning and grid map construction method and mobile robot |
| CN110858076B (en) * | 2018-08-22 | 2023-06-02 | 杭州海康机器人股份有限公司 | Equipment positioning and grid map construction method and mobile robot |
| CN110376605A (en) * | 2018-09-18 | 2019-10-25 | 北京京东尚科信息技术有限公司 | Map constructing method, air navigation aid and device |
| CN110376605B (en) * | 2018-09-18 | 2022-02-01 | 北京京东尚科信息技术有限公司 | Map construction method, navigation method and device |
| CN109614459A (en) * | 2019-03-06 | 2019-04-12 | 上海思岚科技有限公司 | Map structuring winding detection method and equipment applied to two-dimensional laser |
| US20200319342A1 (en) * | 2019-04-02 | 2020-10-08 | Quanta Computer Inc. | Positioning system of mobile device |
| US11557021B2 (en) * | 2019-04-02 | 2023-01-17 | Quanta Computer Inc. | Positioning system of mobile device |
| CN111538918A (en) * | 2020-04-23 | 2020-08-14 | 上海高仙自动化科技发展有限公司 | Recommendation method and device, electronic equipment and storage medium |
| CN111795687A (en) * | 2020-06-29 | 2020-10-20 | 深圳市优必选科技股份有限公司 | Robot map updating method and device, readable storage medium and robot |
| CN112102400A (en) * | 2020-09-15 | 2020-12-18 | 上海云绅智能科技有限公司 | Distance-based closed loop detection method and device, electronic equipment and storage medium |
| CN112668653A (en) * | 2020-12-30 | 2021-04-16 | 北京百度网讯科技有限公司 | Loop detection method, device, equipment and medium based on laser radar map |
| WO2024007807A1 (en) * | 2022-07-06 | 2024-01-11 | 杭州萤石软件有限公司 | Error correction method and apparatus, and mobile device |
Also Published As
| Publication number | Publication date |
|---|---|
| CN108256060B (en) | 2021-02-09 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN108256060A (en) | Closed loop detection method, device, terminal and storage medium | |
| Deng et al. | An improved ant colony optimization algorithm based on hybrid strategies for scheduling problem | |
| CN107561549B (en) | Method and device for relocating terminal position, terminal and storage medium | |
| JP2018523875A (en) | Lane recognition modeling method, apparatus, storage medium and device, and lane recognition method, apparatus, storage medium and apparatus | |
| WO2021036825A1 (en) | Object tracking method and apparatus, storage medium, and electronic device | |
| CN115017805B (en) | Method and system for planning optimal path of nuclear retired field based on bidirectional A-x algorithm | |
| Li et al. | FC-SLAM: Federated learning enhanced distributed visual-LiDAR SLAM in cloud robotic system | |
| CN109034073A (en) | Predict method, system, equipment and the medium of building demolition waste yield | |
| Zhao et al. | Boundary regularized building footprint extraction from satellite images using deep neural network | |
| CN114548606A (en) | Construction method of cyclone strength prediction model and cyclone strength prediction method | |
| CN107392398A (en) | A kind of agricultural management method, mist calculating platform and system | |
| Stathoulopoulos et al. | 3DEG: Data-Driven Descriptor Extraction for Global re-localization in subterranean environments | |
| Gao et al. | Applying optimized YOLOv8 for heritage conservation: enhanced object detection in Jiangnan traditional private gardens | |
| CN116625395A (en) | Multi-target path planning method based on two-dimensional grid map | |
| Cao et al. | Label-efficient deep learning-based semantic segmentation of building point clouds at LOD3 level | |
| Dobbs et al. | Smart-tree: neural medial axis approximation of point clouds for 3d tree skeletonization | |
| Salah et al. | Summarizing large scale 3D mesh for urban navigation | |
| ME et al. | Face detection by hybrid genetic and ant colony optimization algorithm | |
| CN110135340A (en) | 3D hand gestures estimation method based on cloud | |
| Zhang et al. | TriHelper: Zero-Shot Object Navigation with Dynamic Assistance | |
| Luo et al. | Combining fractal hourglass network and skeleton joints pairwise affinity for multi-person pose estimation | |
| CN109117722A (en) | A kind of remote sensing image river extracting method based on Distributed Architecture | |
| CN116863509B (en) | Method for detecting human-shaped outline and recognizing gesture by using improved polar mask | |
| Liu et al. | Semantic Segmentation of Remote Sensing Images Based on Cross-Layer Multi-Scale Feature Fusion | |
| Xu | Automated and Real-time Scan-to-BIM through Deep Learning-based Object Detection |
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 |