CN108256060B - Closed loop detection method, device, terminal and storage medium - Google Patents

Closed loop detection method, device, terminal and storage medium Download PDF

Info

Publication number
CN108256060B
CN108256060B CN201810039214.8A CN201810039214A CN108256060B CN 108256060 B CN108256060 B CN 108256060B CN 201810039214 A CN201810039214 A CN 201810039214A CN 108256060 B CN108256060 B CN 108256060B
Authority
CN
China
Prior art keywords
map
sub
determining
matching
score
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.)
Active
Application number
CN201810039214.8A
Other languages
Chinese (zh)
Other versions
CN108256060A (en
Inventor
阳方平
文国成
曾碧
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Guangzhou Shiyuan Electronics Thecnology Co Ltd
Original Assignee
Guangzhou Shiyuan Electronics Thecnology Co Ltd
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Guangzhou Shiyuan Electronics Thecnology Co Ltd filed Critical Guangzhou Shiyuan Electronics Thecnology Co Ltd
Priority to CN201810039214.8A priority Critical patent/CN108256060B/en
Publication of CN108256060A publication Critical patent/CN108256060A/en
Application granted granted Critical
Publication of CN108256060B publication Critical patent/CN108256060B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

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

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

Closed loop detection method, device, terminal and storage medium
Technical Field
The present invention relates to the field of data processing technologies, and in particular, to a closed loop detection method, an apparatus, a terminal, and a storage medium.
Background
When a robot is used for map construction, similarity matching is often performed between the current position of the robot and a position traversed by the robot before the robot, so as to determine whether the current position of the robot is a visited place.
In robot-based map construction, a laser radar is generally adopted as a sensor to acquire environmental information, so that the closed-loop detection problem can be regarded as the problem of similarity of laser data scanning matching results. In the existing closed-loop detection method, a scanning matching algorithm (frame-to-frame) of one frame of laser data and one frame of laser data is generally adopted, but the method has the defects that: the method is easy to generate wrong matching results due to high local similarity of laser data, so that wrong closed-loop detection is caused; in addition, when the range of the constructed map is large, the amount of laser data to be matched is greatly increased, thereby affecting the real-time performance of closed-loop detection.
Based on the above problems, the skilled person proposes an improved method of closed-loop detection, for example, extracting a small-range feature in an environment traversed by a robot to form a feature region, performing closed-loop detection by comparing similarities of different feature regions, or, for example, performing closed-loop detection by comparing similarities of different key frames by using laser data of every set frame as a key frame. The core idea of the improved method is equivalent to selecting a key frame for closed-loop detection, and the purposes of reducing the number of matched candidate sets and improving the detection speed can be achieved, but the improved method also has certain defects: the selection of the key frames can randomly lose part of data, so that two key frames actually having a closed-loop relationship cannot detect the existence of a closed loop because the similar parts are fewer.
Disclosure of Invention
The closed-loop detection method, the closed-loop detection device, the terminal and the storage medium provided by the embodiment of the invention not only improve the closed-loop detection speed, but also effectively improve the closed-loop detection accuracy.
In a first aspect, an embodiment of the present invention provides a closed loop detection method, including:
forming a current sub-map according to the captured laser data of the set continuous frames;
determining a similarity matching score of the current sub-map and the selected at least one candidate sub-map;
determining a target matching score satisfying a first matching screening condition from each of 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.
In a second aspect, an embodiment of the present invention further provides a closed loop detection apparatus, including:
the sub-map building module is used for forming a current sub-map according to the captured laser data of the set continuous frames;
a similarity score determination module for determining a similarity matching score of the current sub-map and the selected at least one candidate sub-map;
a target score determining module for determining a target matching score satisfying a first matching screening condition from each of the similarity matching scores;
a closed-loop region determination module for determining a candidate sub-map corresponding to the target matching score as a closed-loop region of the current sub-map when it is determined that the target matching score exists.
In a third aspect, an embodiment of the present invention also provides a terminal, including: laser radar and system odometer still include:
one or more processors;
storage means for storing one or more programs;
the one or more programs are executed by the one or more processors, so that the one or more processors implement the closed-loop detection method provided by the first aspect of the embodiments of the present invention.
In a fourth aspect, an embodiment of the present invention further provides a storage medium, on which a computer program is stored, where the computer program, when executed by a processor, implements the closed loop detection method provided in the first aspect of the embodiment of the present invention.
The embodiment of the invention provides a closed-loop detection method, a device, a terminal and a storage medium, the technical scheme can form a current sub-map by capturing laser data of set continuous frames by a laser radar on the terminal, then the current sub-map is subjected to similarity matching with candidate sub-maps in a historical sub-map set, the matching process comprises the steps of determining similarity matching scores of the current sub-map and each candidate sub-map, determining target matching scores meeting screening matching conditions from the similarity matching scores, and finally determining that the similarity between the candidate sub-map corresponding to the target matching scores and the current sub-map meets the requirement of closed-loop detection, so that the closed-loop detection in map construction is realized. According to the technical scheme, 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 number of the matched candidate sets can be effectively controlled, the closed-loop detection speed is increased, the real-time performance of the closed-loop detection in the large-range map construction process is guaranteed, meanwhile, the false detection rate of the closed-loop detection is effectively reduced, and the accuracy of the closed-loop detection is improved.
Drawings
Fig. 1 is a flowchart of a closed loop detection method according to an embodiment of the present invention;
fig. 2a is a flowchart of a closed loop detection method according to a second embodiment of the present invention;
fig. 2b is a flowchart of an implementation method for determining a first matching score in the closed-loop detection method according to the second embodiment of the present invention;
FIG. 2c shows an exemplary diagram of a two-dimensional look-up table of candidate sub-maps;
fig. 3a is a flowchart of a closed loop detection method according to a third embodiment of the present invention;
fig. 3b is a flowchart of an implementation method for determining candidate information in the closed-loop detection method according to the third embodiment of the present invention;
FIG. 3c presents an exemplary implementation of generating a two-dimensional compression table based on a two-dimensional lookup table;
FIG. 3d depicts a display of the first laser data in the form of a grid map;
FIG. 3e depicts a display of second laser data in the form of a grid map;
FIG. 3f presents a display diagram of a first sub-map containing first laser data in the form of a grid map;
FIG. 3g depicts a display diagram of a second sub-map containing first laser data in the form of a grid map;
fig. 4 is a schematic structural diagram of a closed-loop detection apparatus according to a fourth embodiment of the present invention;
fig. 5 is a schematic diagram of a hardware structure of a terminal according to a fifth embodiment of the present invention.
Detailed Description
The present invention will be described in further detail with reference to the accompanying drawings and examples. It is to be understood that the specific embodiments described herein are merely illustrative of the invention and are not limiting of the invention. It should be further noted that, for the convenience of description, only some of the structures related to the present invention are shown in the drawings, not all of the structures.
It should be further noted that, for the convenience of description, only some but not all of the relevant aspects of the present invention are shown in the drawings. Before discussing exemplary embodiments in more detail, it should be noted that some exemplary embodiments are described as processes or methods depicted as flowcharts. Although a flowchart may describe the operations (or steps) as a sequential process, many of the operations can be performed in parallel, concurrently or simultaneously. In addition, the order of the operations may be re-arranged. The process may be terminated when its operations are completed, but may have additional steps not included in the figure. The processes may correspond to methods, functions, procedures, subroutines, and the like.
It is noted that, herein, relational terms such as first and second, and the like may be used solely to distinguish one entity or action from another entity or action without necessarily requiring or implying any actual such relationship or order between such entities or actions. For example, a "first" and a "second" of the first and second match scores is used to distinguish two different match scores in the score-calculation operation, a "first" and a "second" of the first and second projection scores is used to distinguish two different projection scores in the score-calculation operation, as another example, a "first" and a "second" of the first and second set translation and/or rotation rules is used to distinguish two different translation and/or rotation rules, and so on.
Example one
Fig. 1 is a flowchart of a closed-loop detection method according to an embodiment of the present invention, where this embodiment is applicable to a situation where a current location and a historical location of a terminal are subjected to closed-loop detection in a map building process, and this method may be executed by a closed-loop detection apparatus according to an embodiment of the present invention, and this apparatus may be implemented by hardware and/or software and is generally integrated in the terminal.
The terminal may specifically refer to an electronic terminal device such as a robot or an intelligent service device, which can obtain the surrounding environment information by itself to construct a map. It can be understood that when the map is constructed based on the terminal, the closed-loop area of the current position of the terminal can be accurately and quickly determined based on the closed-loop detection method provided by the embodiment of the invention.
As shown in fig. 1, a closed-loop detection method provided in an embodiment of the present invention includes the following steps:
and S101, forming a current sub-map according to the laser data of the captured set continuous frames.
In this embodiment, for a terminal performing map construction, a laser radar is often arranged on the terminal, and the laser radar can specifically detect distance information from an obstacle to the terminal by emitting a laser beam, so as to form laser data. Illustratively, the mathematical form of a frame of laser data may be represented as: z ═ Z1`,z2`,z3`,...,zi`...,zm"where Z represents a frame of laser data, Zi"represents the i-th distance information value obtained by the laser data in the unit frame, and m represents the total number of distance information contained in the laser data of the frame.
It should be noted that, in the map building process, a frame of laser data captured by the laser radar may be converted into the built map, so as to determine the position information of the obstacle in the map through the laser data, and thus it may be considered that the laser data captured by the laser radar in real time may be used for building the map. In this step, a local small map can be formed by setting laser data of consecutive frames, so as to serve as a current sub-map required by closed-loop detection in this embodiment.
Generally, the current sub-map formed by setting the laser data of the continuous frames can be presented in the form of a grid map, or can be presented only according to the distance information included in the laser data of the set continuous frames.
For example, when the current sub-map is presented through a grid map, laser data of a set continuous frame needs to be converted into coordinate information of a laser end point in a global coordinate system through distance information, and then the laser end point is identified in the grid map according to the coordinate information of the laser end point, so that the current sub-map in a grid form can be formed, wherein the laser end point can be understood as a position point where an obstacle is located corresponding to each distance information value in the laser data captured by a laser radar, and in the grid map, the laser end point is identified in a specific form and represents the position of the obstacle in the map.
And S102, determining the similarity matching score of the current sub-map and the selected at least one candidate sub-map.
In this embodiment, after determining the current sub-map, in order to determine whether the current visited position of the terminal is the visited historical position, similarity matching between the current sub-map and the previously formed historical sub-map is required. The candidate sub-map may specifically be understood as a sub-map selected from the historical sub-map set for similarity matching with the current sub-map.
It can be understood that, when a large-scale map is constructed, a large number of history sub-maps may exist before a current sub-map is formed, and if similarity matching is performed on the current sub-map and all the history sub-maps, it takes much time to affect the real-time performance of closed-loop detection.
In this embodiment, the similarity score between the current sub-map and each candidate sub-map may be determined by a set calculation rule based on the laser data forming the current sub-map and the laser data forming each candidate sub-map. For example, the calculation rule set in this embodiment may be that laser data constituting the current sub-map is projected in a two-dimensional local map formed by each candidate sub-map based on the corresponding laser data, and the similarity matching score between the current sub-map and each candidate sub-map is determined by the score value corresponding to the projection.
And S103, determining a target matching score meeting the first matching screening condition from the similarity matching scores.
In this embodiment, since the higher the similarity score between the current sub-map and the candidate sub-map is, the more similar the current sub-map and the candidate sub-map is, that is, the higher the possibility that the current sub-map and the candidate sub-map belong to the same position in the map is, the first matching and screening condition set in the present embodiment may be specifically used to screen out a similarity matching score with a higher score, and mark the screened result as the target matching score. In this embodiment, in order to filter out the sub-map with the highest similarity with the current sub-map, it is preferable to set only one similarity matching score as the target matching score.
And S104, if the target matching score is determined to exist, determining the candidate sub-map corresponding to the target matching score as the closed-loop area of the current sub-map.
In this embodiment, when it is determined that the target matching score exists based on the above S103, the candidate sub-map corresponding to the target matching score may be determined as the historical sub-map with the highest similarity to the current sub-map, and after the target matching score is determined based on the method in this embodiment, it may be considered that a closed-loop area of the current sub-map exists in the currently constructed map, where the closed-loop area is the candidate sub-map corresponding to the target matching score, and when the closed-loop area exists, it may be considered that the location currently visited by the terminal is the visited location, and it is no longer necessary to repeatedly construct a map for the location.
It is understood that, in the similarity matching score of the present embodiment, there may be no target matching score satisfying the first filtering condition, and at this time, it may be considered that the current visited location of the terminal has not been visited yet, that is, there is no closed-loop area of the current sub-map in the formed history sub-map.
It should be noted that the closed-loop detection method provided in this embodiment may be equivalent to a loop execution method, and as long as the terminal is in a motion state, a real-time current sub-map may be formed according to the operation of S101 and closed-loop detection may be performed based on subsequent operations.
The closed-loop detection method provided by the embodiment of the invention can be used for firstly capturing laser data of set continuous frames through the laser radar on the terminal to form the current sub-map, then carrying out similarity matching on the current sub-map and candidate sub-maps in a historical sub-map set, wherein the matching process comprises the steps of determining similarity matching scores of the current sub-map and each candidate sub-map, determining target matching scores meeting screening matching conditions from the similarity matching scores, and finally determining that the similarity between the candidate sub-map corresponding to the target matching scores and the current sub-map meets the requirement of closed-loop detection, so that the closed-loop detection in map construction is realized. By the method, closed-loop detection during large-range map construction can be 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 effectively reduce the false detection rate of the closed-loop detection and improve the accuracy of the closed-loop detection.
Example two
Fig. 2a is a flowchart of a closed-loop detection method according to a second embodiment of the present invention, and the second embodiment is optimized based on the above embodiments. In this embodiment, the determining the similarity matching score between the current sub-map and the selected at least one candidate sub-map is further embodied as: obtaining a two-dimensional lookup table of at least one selected candidate sub-map, wherein each two-dimensional lookup table is constructed in advance based on Gaussian distribution probability density of continuous frame laser data set in the corresponding candidate sub-map; determining a first matching score of the current sub-map in each two-dimensional lookup table; and determining each first matching score as the similarity matching score of the current sub-map and the corresponding candidate sub-map.
Further, the embodiment further optimizes the determination of the target matching score satisfying the first matching and screening condition from each of the similarity matching scores as follows: and screening first intermediate matching scores which are greater than or equal to a first set threshold value from the first matching scores, and determining the maximum value of the first intermediate matching scores as the target matching score.
In addition, the present embodiment further forms the current sub-map according to the laser data of the captured set continuous frames, and specifically optimizes as follows: determining the frame number of continuously captured laser data based on a laser radar on a terminal; and if the number of the frames reaches a set value, forming a current sub-map according to laser tail end points in the captured laser data of each frame.
On the basis of the optimization, the embodiment further optimizes and adds: and adding the current sub-map serving as a new historical sub-map into the historical sub-map set, and constructing and storing a two-dimensional lookup table of the new historical sub-map, wherein element values of the two-dimensional lookup table are Gaussian distribution probability density values of continuous frame laser data set in the new historical sub-map.
As shown in fig. 2a, a closed loop detection method provided in the second embodiment of the present invention specifically includes the following steps:
s201, determining the frame number of laser data continuously captured by the laser radar on the terminal.
Specifically, in the terminal movement process, the laser data can be captured in real time through the laser radar on the terminal, and in order to perform the operation of determining the closed loop area through map-to-map matching in the embodiment, the step firstly accumulates the frame number of the laser data continuously captured by the laser radar after the latest historical sub-map is formed. It is understood that, in the present embodiment, the number of frames recorded cumulatively may be subjected to a zeroing operation after the current sub-map is determined, so as to be used for determining the next current sub-map.
S202, if the number of frames reaches a set value, forming a current sub map according to laser end points in the captured laser data of each frame.
In this embodiment, when the accumulated number of frames reaches a set value, it may be considered that the laser data continuously captured at present can form a current sub-map, so this step may present the current sub-map in a grid map form according to the global coordinate information of the laser end point in each captured frame of laser data; or, directly representing the current sub map by the set of distance information values corresponding to the laser end points in each frame of laser data.
S203, obtaining a two-dimensional lookup table of the selected at least one candidate sub-map.
In this embodiment, the candidate sub-map is equivalent to a sub-map selected from the history sub-map set and subjected to similarity matching with the current sub-map, and it should be noted that, in this embodiment, a corresponding two-dimensional lookup table is pre-constructed for any history sub-map in the history sub-map set, so that the two-dimensional lookup table of the selected at least one candidate sub-map may be directly obtained in this step. In this embodiment, the two-dimensional lookup table of the history sub-map is actually equivalent to another grid representation form of the history sub-map, and the current sub-map and the history sub-map (mainly, the candidate sub-map in this embodiment) corresponding to the two-dimensional lookup table may be subjected to similarity matching through the two-dimensional lookup table.
In this embodiment, the two-dimensional lookup table corresponding to each history sub-map in the history sub-map set may actually be regarded as each laser end point construction in the history laser data based on the corresponding history sub-map. Specifically, for any history sub-map, the history laser data corresponding to the history sub-map and setting continuous frames can be determined, in this embodiment, a space two-dimensional table with a sufficiently large number of rows and columns can be initially set for the history sub-map, the upper left corner of the space two-dimensional table can be regarded as the origin, and a cell in the space two-dimensional table is set to represent a unit coordinate; then, the position of the laser radar when capturing any frame of historical laser data can be used as an origin, so that each laser end point is represented in the empty two-dimensional table through coordinate relation conversion based on the distance information value corresponding to the laser end point of each frame of historical laser data, at this time, the element value of the cell occupied by each laser end point is represented as 1, and the element values of the rest cells are still 0 to form a middle two-dimensional table; and then, adopting a Gaussian distribution model as a noise model of each laser terminal point, calculating Gaussian distribution probability density for element values in the middle two-dimensional table based on the Gaussian distribution model, and taking each Gaussian distribution probability density value obtained by calculation as a new element value corresponding to each cell, thereby constructing a two-dimensional lookup table for forming the history sub-map.
It should be noted that, because the number of rows and columns of the initially set empty two-dimensional table is large enough, in this embodiment, after the empty two-dimensional table is completed based on the gaussian probability density value, only the rows and columns where the element values do not have 0 are retained to form the final two-dimensional lookup table. The finally formed two-dimensional lookup table is equivalent to another representation of the history sub-map, and the construction of the two-dimensional lookup table is equivalent to displaying the continuous frame laser data contained in the history sub-map in the form of a likelihood ratio area, so that the view effect of the history sub-map can be better presented.
And S204, determining a first matching score of the current sub-map in each two-dimensional lookup table.
In this embodiment, the first matching score of the current sub-map in each two-dimensional lookup table may be determined according to the laser end point of the laser data of the set consecutive frames corresponding to the current sub-map.
Specifically, fig. 2b is a flowchart of an implementation method for determining a first matching score in the closed-loop detection method according to the second embodiment of the present invention. As shown in fig. 2b, the determining the first matching score of the current sub-map in each of the two-dimensional lookup tables specifically includes the following operations:
s2041, based on the first set translation and/or rotation rule, translating and/or rotating the laser end point in the current sub-map in each two-dimensional lookup table.
Specifically, in order to obtain the first matching score of the current sub-map in each two-dimensional lookup table, the implementation is implemented by performing translation and/or rotation on each laser end point included in the current sub-map in each two-dimensional lookup table, where the first set translation and/or rotation rule is equivalent to specifying a specific translation and/or rotation order of the laser end point in each two-dimensional lookup table in the current sub-map.
It should be noted that the translation, the rotation, and the translation plus the rotation in this embodiment are equivalent to three projection operation modes of projecting the laser end point in the current sub-map to each two-dimensional lookup table, and all three projection operations need to be performed, the translation in this embodiment is equivalent to performing a horizontal translation or a vertical translation on the laser end point with a currently set translation variable, where the set translation variable may start from (0,0), then gradually increase the translation variable by a unit step size, and may complete the translation traversal in a form of performing the horizontal translation first and then performing the vertical translation, or may complete the translation traversal in a form of performing the vertical translation first and then performing the horizontal translation, or a form of performing both the horizontal translation and then performing the horizontal translation; the rotation in this embodiment corresponds to incremental rotation of the laser end point by a set angle around the origin, and the range of the angular rotation is preferably set in this embodiment, and may be a range between 0 ° and 180 °.
It can be understood that, in this step, the laser end point of the current sub-map may be projected in the two-dimensional lookup table corresponding to each candidate sub-map in a parallel manner, so that the calculation time of the matching score may be greatly reduced.
S2042, obtaining a first projection score of the current sub-map after each translation and/or rotation in each two-dimensional lookup table.
In this embodiment, performing a translation and/or rotation on each laser end point in each two-dimensional lookup table in the current sub-map is equivalent to projecting each laser end point into each two-dimensional lookup table in a conversion relationship, after performing a projection once, each laser end point corresponds to an element value in each two-dimensional lookup table, and summing up the element values currently corresponding to all laser end points, so that the sum of the element values can be determined as a first projection score of the current sub-map after performing a translation and/or rotation in each two-dimensional lookup table.
Exemplarily, fig. 2c shows an exemplary diagram of a two-dimensional lookup table of one candidate sub-map, as shown in fig. 2c, the lookup table 21 is a two-dimensional lookup table (only exemplarily described) of one candidate sub-map, it is assumed that the current sub-map is composed of 2 laser end points (only exemplarily described), and coordinate information (coordinate values relative to the above-mentioned relative origin) of the above-mentioned 2 laser end points relative to a lidar (the lidar when capturing any frame of laser data, and setting the position of the lidar as the relative origin) is determined to be (1,2) and (2,3) respectively according to the laser data of the set continuous frames corresponding to the current sub-map; assuming that a translation projection with a translation variable of (1,0) is performed on the laser end points (1,2) and (2,3) in the lookup table 21, it can be determined that the laser end points (1,2) and (2,3) respectively fall into two cells (2,2) and (3,3) of the lookup table 21 after being translated, and as shown in fig. 2c, the element values of the two cells (2,2) and (3,3) in the lookup table 21 are 1.6 and 0.6 respectively, that is, the element values corresponding to the two laser end points at this time are 1.6 and 0.6 respectively, and the sum of the two element values is 2.2, so that the first projection of the current sub-map after the translation in the lookup table 21 can be considered as 2.2.
And S2043, determining the highest first projection score of the current sub-map in each two-dimensional lookup table based on the first projection score after each translation and/or rotation.
In this embodiment, based on the above operation, the first projection score corresponding to each laser end point in the current sub-map after being translated and/or rotated in each two-dimensional lookup table may be determined each time, and in this step, the highest first projection score may be determined from a plurality of first projection scores corresponding to each two-dimensional lookup table.
S2044, determining each highest first projection score as a first matching score of the current sub-map in the corresponding two-dimensional lookup table.
In this step, the highest first projection score determined from each two-dimensional lookup table may be recorded as the first matching score of the current sub-map in the corresponding two-dimensional lookup table.
And S205, determining each first matching score as a similarity matching score of the current sub-map and the corresponding candidate sub-map.
In this embodiment, after determining the first matching score of the current sub-map in each two-dimensional lookup table based on the foregoing S204, the two-dimensional lookup table corresponding to each first matching score may be determined, and meanwhile, the candidate sub-map corresponding to each two-dimensional lookup table may also be determined, so that the candidate sub-map corresponding to each first matching score may be determined, and each first matching score is used as the similarity matching score when the corresponding candidate sub-map is similarity matched with the current sub-map, thereby implementing similarity matching between the current sub-map and each candidate sub-map.
S206, screening first intermediate matching scores which are larger than or equal to a first set threshold value from the first matching scores, and determining the maximum value in the first intermediate matching scores as a target matching score.
In this embodiment, the first matching screening condition is set to be greater than or equal to the first set threshold, and in this embodiment, the first set threshold may be set manually, and based on the operation in this step, it may be determined whether or not there is a first intermediate matching score greater than or equal to the first set threshold in each of the determined first matching scores, and if there is a first intermediate matching score, it is necessary to further determine a maximum value in the first intermediate matching scores, and determine the maximum value as the target matching score meeting the requirement of the closed loop.
It is understood that, if each of the first matching scores determined above is smaller than the first set threshold, it may be considered that there is no target matching score, and at this time, it may be considered that there is no closed-loop area of the current sub-map in the formed history sub-map set, that is, it may be considered that the location currently visited by the terminal has not been visited before, and thus the current sub-map may be built into the global map.
And S207, if the target matching score is determined to exist, determining the candidate sub-map corresponding to the target matching score as the closed-loop area of the current sub-map.
In this embodiment, after the target matching score is determined based on S206, the two-dimensional lookup table corresponding to the target matching score may be determined, and the corresponding candidate sub-map is determined accordingly, and finally the candidate sub-map corresponding to the target matching score is determined as the closed-loop area of the current sub-map, at this time, it may be considered that the position currently visited by the terminal has been visited before, and thus, the constraint relationship between the current sub-map and the candidate sub-map may be established, so as to implement map optimization in the global map construction.
And S208, adding the current sub-map serving as a new history sub-map into the history sub-map set, and constructing and storing a two-dimensional lookup table of the new history sub-map.
It can be understood that, based on the operations in S201 to S207, the current sub-map including the current location of the terminal may be subjected to closed-loop detection in a manner of map-to-map matching, and when the terminal is in a running state all the time, the operation of performing closed-loop detection on the current location of the terminal is performed continuously, so that in this step, after the closed-loop detection of the current sub-map is implemented, the current sub-map is added to the history sub-map set as a new history sub-map, and then, the current sub-map may continue to return to S201 to form a next current sub-map to be subjected to closed-loop detection.
In addition, after the current sub-map is added to the history sub-map set as a new history sub-map, a two-dimensional lookup table needs to be constructed for the new history sub-map, so as to be used for closed-loop detection of a subsequent newly formed current sub-map. And setting the Gaussian distribution probability density value of the continuous frame laser data in the new history sub-map by using the element value of the two-dimensional lookup table. The specific steps of constructing the two-dimensional lookup table have been described after S203, and are not described herein again.
It should be noted that the core idea of the closed-loop detection method provided in this embodiment is to perform closed-loop detection through similarity matching between the sub-map and the sub-map. When the method is adopted for closed-loop detection, firstly, redundant information among continuous frames of laser data can be well removed through the constructed sub-map, so that the calculation amount required for matching the similarity of the map and the image is obviously smaller than that of the conventional method for matching the similarity of the frame and the frame or the frame and the image, and the purposes of reducing the calculation amount of similarity matching and improving the similarity matching efficiency are achieved. In addition, because the sub-map comprises the laser data of the continuous frames, the information content contained in the sub-map is far more than that contained in the laser data of a single frame, so that the number of the comparable corners during similarity matching is increased, and the problem of wrong judgment caused by local high similarity of the map can be well overcome.
The closed-loop detection method provided by the embodiment of the invention embodies the determination operation of the current sub-map, the determination operation of the similarity matching score and the determination operation of the target matching score, and additionally adds the update operation of the map set. By using the method, the calculation amount for reducing the similarity matching is effectively reduced, the similarity matching efficiency is improved, the real-time performance of closed-loop detection in the large-range map construction process is ensured, meanwhile, the problem of misjudgment caused by local high similarity of the map is better solved, and the accuracy of closed-loop detection is improved.
EXAMPLE III
Fig. 3a is a flowchart of a closed-loop detection method according to a third embodiment of the present invention, where the third embodiment of the present invention is optimized based on the above-mentioned embodiments, and in this embodiment, specific optimization is added by: and screening at least one candidate sub-map from the historical sub-map set according to a set screening rule.
As shown in fig. 3a, a closed-loop detection method provided in the third embodiment of the present invention specifically includes the following operations:
and S301, forming a current sub-map according to the laser data of the captured set continuous frames.
S302, screening at least one candidate sub map from the history sub map set according to a set screening rule.
In the embodiment, in order to perform closed-loop detection on the current sub-map, generally, similarity matching between the current sub-map and each historical sub-map in the historical sub-map set is considered, however, when the number of the historical sub-maps is large, it takes much matching time to perform similarity matching between the current sub-map and all the historical sub-maps, and thus the real-time performance of closed-loop detection is affected.
Based on the operation of the step, the candidate sub-maps which are more likely to form a closed-loop area with the current sub-map can be screened from the historical sub-map set, so that the similarity matching between the current sub-map and the candidate sub-maps can be considered, the calculation amount of the similarity matching is reduced, and the real-time performance of closed-loop detection is improved.
The setting idea of the setting of the screening rule in this step may be to first determine a sub-map to be matched that can be roughly matched with the current sub-map from the historical sub-map set, and then screen a candidate sub-map for similarity matching from the plurality of sub-maps to be matched according to a rough matching result.
Specifically, fig. 3b is a flowchart of an implementation method for determining candidate information in the closed-loop detection method provided by the third embodiment of the present invention. As shown in fig. 3b, the screening at least one candidate sub-map from the history sub-map set according to the set screening rule specifically includes the following operations:
s3021, screening at least two sub-maps to be matched corresponding to the current sub-map from the historical sub-map set according to a set positioning screening strategy.
In the step, the sub-map to be matched is screened from the estimated position where the terminal is currently located.
Further, the screening, according to a set positioning screening policy, at least two sub-maps to be matched corresponding to the current sub-map from the historical sub-map set includes: determining a current estimated position of a terminal in a constructed global map based on a system odometer in the terminal; acquiring the historical positions of the historical sub-maps in the historical sub-map set in the global map; determining distance information from each historical sub-map to the current sub-map based on each historical position and the current estimated position; and determining the historical sub-map of which the distance information is smaller than a set distance threshold value as a sub-map to be matched of the current sub-map.
In this embodiment, the system odometer is equivalent to a hardware device in the terminal, and can roughly estimate the current position of the terminal, so that the current estimated position of the terminal can be obtained in this embodiment, and meanwhile, the historical positions of the historical sub-maps in the global map can also be obtained, and finally, the sub-map to be matched, which is less than the set distance threshold from the terminal, can be determined.
It should be noted that, because the system odometer in the actual environment has an error, and the estimated position of the robot is less and less reliable along with the accumulation of the error, and the range of the matching candidate set cannot be effectively reduced, the estimated position of the terminal can be optimally calculated by a selected Graph Optimization method, such as a General Graph Optimization algorithm (g 2o), so as to reduce the error distance between the current estimated position of the terminal and the real position, thereby ensuring that the closed-loop detection in the large-range map does not reduce the similarity matching speed due to data accumulation.
And S3022, acquiring a two-dimensional compression table corresponding to each sub-map to be matched.
In the embodiment, the two-dimensional lookup table is constructed for each historical sub-map in the historical sub-maps, and the two-dimensional compression table is correspondingly constructed at the same time, so that the rough matching between the current sub-map and the sub-map to be matched is realized through the two-dimensional compression table.
And the two-dimensional compression table is generated by performing pooling calculation on the maximum value of the two-dimensional lookup table corresponding to each sub-map to be matched. Specifically, for the two-dimensional lookup tables of each historical sub-map, a maximum pooling mode can be adopted to perform compression optimization on the lookup tables to generate corresponding two-dimensional compression tables, and it can be understood that the generated two-dimensional compression tables are actually equivalent to the operation of reducing the resolution of the two-dimensional lookup tables, and the rough matching between the two-dimensional compression tables of the sub-map to be matched and the current sub-map is considered at first, so that the overall similarity matching efficiency of closed-loop detection can be effectively improved.
This embodiment compresses the two-dimensional lookup table of the history sub-map following a multiple of 2, assuming that d represents the multiple of compression, and H and L represent the two-dimensional lookup table and the two-dimensional pressureThe unit cell number of the table is reduced, so that the unit format of the two-dimensional lookup table and the unit cell number of the two-dimensional compression table have the following relationship under different compression multiples, namely L is H/2d(ii) a Preferably, in order to reduce the precision loss caused by the two-dimensional compression table, the maximum pooling method is used to determine the element value of each cell in the two-dimensional compression table, that is, the maximum value of the element is selected from the multiple cells to be merged, and the maximum value of the element is taken as the element value in the cell to form the two-dimensional compression table.
In this embodiment, it may be preferable to set the value of d to be 2, for example, fig. 3c shows an implementation example of generating a two-dimensional compression table based on a two-dimensional lookup table, as shown in fig. 3c, the lookup table 31 represents the two-dimensional lookup table, and the compression table 32 represents the two-dimensional compression table after compression and merging. It can be found that the first element value in the compression table 32 is the maximum value of the elements in the four cells in the look-up table 31.
S3023, determining second matching scores of the current sub-map in each two-dimensional compression table, and determining at least one candidate matching score meeting the second screening matching condition from each second matching score.
The step is equivalent to rough matching between the two-dimensional compression table based on the sub-map to be matched and the current sub-map, and the step can also determine the second matching score of the current sub-map in each two-dimensional compression table according to the laser end point of the laser data of the continuous frame correspondingly set by the current sub-map.
Specifically, the determining second matching scores of the current sub-map in each of the two-dimensional compression tables and determining at least one candidate matching score satisfying a second filtering matching condition from each of the second matching scores includes: translating and/or rotating the laser terminal point in the current sub-map in each two-dimensional compression table based on a second set translation and/or rotation rule; acquiring a second projection score of the current sub-map after each translation and/or rotation in each two-dimensional compression table; determining a highest second projection score of the current sub-map in each of the two-dimensional compression tables based on the second projection score after each translation and/or rotation; determining each of the highest second projection scores as a second matching score of the current sub-map in the corresponding two-dimensional compression table; when at least one second intermediate matching score which is larger than or equal to a second set threshold value exists in each second matching score, determining each second intermediate matching score as a candidate matching score; otherwise, determining the highest matching score in the second matching scores as a candidate matching score.
In this embodiment, the specific implementation operation of determining the second matching score of the current sub-map in each two-dimensional compression table is the same as the manner of determining the first matching score of the current sub-map in each two-dimensional lookup table, and the determination manner is described in detail after the step of S204, and is not described here again.
It should be noted that, in this step, the candidate matching score may be determined in two ways, that is, when a second intermediate matching score greater than or equal to a second set threshold exists in the second matching score, the determined second intermediate matching score may be directly regarded as the candidate matching score; meanwhile, when the second matching scores are all smaller than the second set threshold, the highest matching score in the second matching scores may be regarded as the candidate matching score. It is observed that at least one candidate match score may be determined by this step.
And S3024, determining the sub-map to be matched corresponding to each candidate matching score as a candidate sub-map.
In this step, the two-dimensional compression tables corresponding to the candidate matching scores can be determined, and the sub-maps to be matched corresponding to the two-dimensional compression tables can be determined, so that the screened sub-maps to be matched corresponding to the candidate matching scores can be determined as the candidate sub-maps.
S303, determining the similarity matching score of the current sub-map and the selected at least one candidate sub-map.
And S304, determining a target matching score meeting the first matching screening condition from the similarity matching scores.
S305, if the target matching score is determined to exist, determining the candidate sub-map corresponding to the target matching score as a closed-loop area of the current sub-map.
S306, adding the current sub-map serving as a new history sub-map into the history sub-map set, and constructing and storing a two-dimensional lookup table of the new history sub-map.
It should be noted that, compared with the conventional frame-to-frame similarity matching algorithm, the closed-loop detection method provided by the embodiment of the present invention not only increases the calculation speed of closed-loop detection, but also effectively increases the accuracy of closed-loop detection. For example, FIG. 3d shows a display of the first laser data in the form of a grid map; figure 3e shows a display in which the second laser data is presented in the form of a grid map. The similarity matching is performed on the two frames of laser data in fig. 3d and fig. 3e by using the conventional frame-to-frame similarity matching algorithm, and it can be found that, when closed-loop detection is performed based on the conventional frame-to-frame similarity matching algorithm, it can be determined that the laser data included in the first circle 33 in fig. 3d has higher similarity with the laser data included in the second circle 34 in fig. 3e, so that it is determined that the area represented by the first laser data in fig. 3d is equivalent to the closed-loop area represented by the second laser data in fig. 3 e.
Also, FIG. 3f shows a display diagram of a first sub-map containing first laser data in the form of a grid map; fig. 3g presents a display diagram of a second sub-map containing the first laser data in the form of a grid map. Here, by performing similarity matching on the two sub-maps in fig. 3f and 3g by using the closed-loop detection method provided by this embodiment, it can be found that, when performing closed-loop detection by using the closed-loop detection method according to this embodiment, the laser data included in the third circle 35 in fig. 3f is equivalent to the laser data included in the first circle 33 in fig. 3d, and the laser data included in the fourth circle 36 in fig. 3g is equivalent to the laser data included in the second circle 34 in fig. 3e, at this time, although the laser data included in the third circle 35 and the laser data included in the fourth circle 36 in fig. 3g still have a higher similarity score, the similarity score of other parts in the first sub-map and other parts in the second sub-map is lower, so that the similarity matching score when the whole first sub-map and the second sub-map is not able to satisfy the first screening condition set by this embodiment, whereby the first sub-map is not determined as a closed loop area of the second sub-map. Through the comparison, the closed-loop detection provided by the embodiment can be more accurately performed.
The closed-loop detection method provided by the third embodiment of the invention specifically increases the screening operation of the candidate sub-map and provides the specific determination operation of the candidate sub-map. By using the method, the candidate sub-maps which most possibly form a closed-loop relation with the current sub-map can be screened out, so that the calculated amount during the matching of the map and the map is reduced to a great extent, the closed-loop detection in the large-range map is ensured not to reduce the similarity matching speed due to data accumulation, the real-time performance of the closed-loop detection is effectively improved, meanwhile, the problem of misjudgment caused by the local high similarity of the map is better solved, and the accuracy of the closed-loop detection is improved.
Example four
Fig. 4 is a schematic structural diagram of a closed-loop detection apparatus according to a fourth embodiment of the present invention, which is applicable to a situation where a current location and a historical location of a terminal are subjected to closed-loop detection in map construction. As shown in fig. 4, the apparatus includes: a sub-map construction module 41, a similarity score determination module 42, a target score determination module 43, and a closed-loop region determination module 44.
The sub-map building module 41 is configured to form a current sub-map according to the captured laser data of the set continuous frames;
a similarity score determination module 42 for determining a similarity match score between the current sub-map and the selected at least one candidate sub-map;
a target score determining module 43, configured to determine, from each of the similarity matching scores, a target matching score that satisfies a first matching filtering condition;
a closed-loop region determination module 44, configured to determine a candidate sub-map corresponding to the target matching score as a closed-loop region of the current sub-map when it is determined that the target matching score exists.
In this embodiment, the apparatus first forms a current sub-map from the captured laser data of the set continuous frames by the sub-map construction module 41; then determining a similarity matching score of the current sub-map and the selected at least one candidate sub-map by a similarity score determining module 42; then, a target score determining module 43 determines a target matching score satisfying the first matching screening condition from each of the similarity matching scores; finally, when it is determined that the target matching score exists, the candidate sub-map corresponding to the target matching score is determined as the closed-loop area of the current sub-map by the closed-loop area determination module 44.
The closed-loop detection device provided by the fourth embodiment of the invention realizes closed-loop detection in the process of constructing a large-scale map from the perspective of similarity matching between the sub-map and the sub-map, and compared with the prior art, the closed-loop detection device can effectively control the number of matched candidate sets, improves the closed-loop detection speed, ensures the real-time performance of closed-loop detection in the process of constructing the large-scale map, and simultaneously effectively reduces the false detection rate of the closed-loop detection and improves the accuracy of the closed-loop detection.
Further, the similarity score determining module 42 includes:
the device comprises a lookup table acquisition unit, a two-dimensional lookup table acquisition unit and a two-dimensional lookup table acquisition unit, wherein the lookup table acquisition unit is used for acquiring the two-dimensional lookup table of at least one selected candidate sub-map, and each two-dimensional lookup table is constructed in advance based on Gaussian distribution probability density of continuous frame laser data set in the corresponding candidate sub-map;
a first score determining unit, configured to determine a first matching score of the current sub-map in each of the two-dimensional lookup tables;
and the similarity score acquisition unit is used for determining each first matching score as the similarity matching score of the current sub-map and the corresponding candidate sub-map.
On the basis of the above optimization, the first score determining unit is specifically configured to:
translating and/or rotating the laser terminal point in the current sub-map in each two-dimensional lookup table based on a first set translation and/or rotation rule; acquiring a first projection score of the current sub-map after each translation and/or rotation in each two-dimensional lookup table; determining a highest first projection score of the current sub-map in each of the two-dimensional lookup tables based on the first projection score after each translation and/or rotation; determining each of the highest first projection scores as a first matching score of the current sub-map in a respective two-dimensional look-up table.
Further, the target score determining module 43 is specifically configured to:
and screening first intermediate matching scores which are greater than or equal to a first set threshold value from the first matching scores, and determining the maximum value of the first intermediate matching scores as the target matching score.
Further, the optimization of the embodiment further includes: the candidate information determination module 45 is adapted to determine,
wherein, the candidate information determining module 45 is configured to filter at least one candidate sub-map from the historical sub-map set according to a set filtering rule.
On the basis of the above optimization, the candidate information determining module 45 includes:
the positioning screening unit is used for screening at least two sub-maps to be matched corresponding to the current sub-map from the historical sub-map set according to a set positioning screening strategy;
the compression table acquisition unit is used for acquiring a two-dimensional compression table corresponding to each sub-map to be matched, wherein the two-dimensional compression table is generated by performing pooling calculation on the maximum value of a two-dimensional lookup table corresponding to each sub-map to be matched;
a candidate score determining unit, configured to determine second matching scores of the current sub-map in each of the two-dimensional compression tables, and determine at least one candidate matching score satisfying a second filtering matching condition from each of the second matching scores;
and the candidate map determining unit is used for determining the sub-map to be matched corresponding to each candidate matching score as the candidate sub-map.
Further, the positioning and screening unit is specifically configured to:
determining a current estimated position of a terminal in a constructed global map based on a system odometer in the terminal; acquiring the historical positions of the historical sub-maps in the historical sub-map set in the global map; determining distance information from each historical sub-map to the current sub-map based on each historical position and the current estimated position; and determining the historical sub-map of which the distance information is smaller than a set distance threshold value as a sub-map to be matched of the current sub-map.
Meanwhile, the candidate score determining unit is specifically configured to:
translating and/or rotating the laser terminal point in the current sub-map in each two-dimensional compression table based on a second set translation and/or rotation rule; acquiring a second projection score of the current sub-map after each translation and/or rotation in each two-dimensional compression table; determining a highest second projection score of the current sub-map in each of the two-dimensional compression tables based on the second projection score after each translation and/or rotation; determining each of the highest second projection scores as a second matching score of the current sub-map in the corresponding two-dimensional compression table; when at least one second intermediate matching score which is larger than or equal to a second set threshold value exists in each second matching score, determining each second intermediate matching score as a candidate matching score; otherwise, determining the highest matching score in the second matching scores as a candidate matching score.
Further, the sub-map building module 41 is specifically configured to:
determining the frame number of continuously captured laser data based on a laser radar on a terminal; and if the number of the frames reaches a set value, forming a current sub-map according to laser tail end points in the captured laser data of each frame.
On the basis of the optimization, the device further comprises:
and a map set updating module 46, configured to add the current sub-map as a new history sub-map to the history sub-map set, and construct and store a two-dimensional lookup table of the new history sub-map, where element values of the two-dimensional lookup table are gaussian distribution probability density values of continuous frame laser data set in the new history sub-map.
The product can execute the method provided by any embodiment of the invention, and has corresponding functional modules and beneficial effects of the execution method.
EXAMPLE five
Fig. 5 is a schematic diagram of a hardware structure of a terminal according to a fifth embodiment of the present invention. As shown in fig. 5, the terminal includes: the laser radar 51 and the system odometer 52, further include: one or more processors 53, one processor 53 being exemplified in fig. 5; a storage device 54; the terminal may further include: an input device 55 and an output device 56.
The lidar 51, the system odometer 52, the processor 53, the storage device 54, the input device 55 and the output device 56 in the terminal may be connected by a bus or other means, as exemplified by the bus connection in fig. 5.
The storage device 54, which is a non-transitory computer-readable storage medium, may be used to store software programs, computer-executable programs, and modules, such as program instructions/modules corresponding to the closed-loop detection method in the embodiment of the present invention (e.g., the sub-map construction module 41, the similarity score determination module 42, the target score determination module 43, and the closed-loop region determination module 44 shown in fig. 4, and further includes the candidate information determination module 45 and the map set update module 46). The processor 53 executes various functional applications and data processing of the terminal by running software programs, instructions and modules stored in the storage device 54, i.e. implements the closed loop detection method of the above-described method embodiment.
The storage device 54 may include a storage program area and a storage data area, wherein the storage program area may store an operating system, an application program required for at least one function; the storage data area may store data created according to the use of the terminal, and the like. Further, the storage device 54 may include high speed random access memory, and may also include non-transitory memory, such as at least one magnetic disk storage device, flash memory device, or other non-transitory solid state storage device. In some embodiments, the storage means 54 may optionally comprise a memory located remotely from the processor 53, which may be connected to the terminal device via a network. Examples of such networks include, but are not limited to, the internet, intranets, local area networks, mobile communication networks, and combinations thereof.
And, when the one or more programs included in the above-mentioned terminal are executed by the one or more processors 53, the programs perform the following operations: forming a current sub-map according to the captured laser data of the set continuous frames; determining a similarity matching score of the current sub-map and the selected at least one candidate sub-map; determining a target matching score satisfying a first matching screening condition from each of 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.
In addition, the input device 55 may be used to receive input numeric or character information and generate key signal inputs related to user settings and function control of the terminal. The output device 56 may include a display device such as a display screen.
Furthermore, an embodiment of the present invention further provides a computer-readable storage medium, on which a computer program is stored, where the computer program, when executed by a processor, implements the closed-loop detection method provided in the first embodiment or the second embodiment of the present invention, where the method includes: forming a current sub-map according to the captured laser data of the set continuous frames; determining a similarity matching score of the current sub-map and the selected at least one candidate sub-map; determining a target matching score satisfying a first matching screening condition from each of 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.
From the above description of the embodiments, it is obvious for those skilled in the art that the present invention can be implemented by software and necessary general hardware, and certainly, can also be implemented by hardware, but the former is a better embodiment in many cases. Based on such understanding, the technical solutions of the present invention may be embodied in the form of a software product, which can be stored in a computer-readable storage medium, such as a floppy disk, a Read-Only Memory (ROM), a Random Access Memory (RAM), a FLASH Memory (FLASH), a hard disk or an optical disk of a computer, and includes several instructions for enabling a computer device (which may be a personal computer, a server, or a network device) to execute the methods according to the embodiments of the present invention.
It is to be noted that the foregoing is only illustrative of the preferred embodiments of the present invention and the technical principles employed. It will be understood by those skilled in the art that the present invention is not limited to the particular embodiments described herein, but is capable of various obvious changes, rearrangements and substitutions as will now become apparent to those skilled in the art without departing from the scope of the invention. Therefore, although the present invention has been described in greater detail by the above embodiments, the present invention is not limited to the above embodiments, and may include other equivalent embodiments without departing from the spirit of the present invention, and the scope of the present invention is determined by the scope of the appended claims.

Claims (12)

1. A closed loop detection method, comprising:
forming a current sub-map presented in a grid map form according to the captured laser data of the set continuous frames;
determining a similarity matching score of the current sub-map and the selected at least one candidate sub-map;
determining a target matching score satisfying a first matching screening condition from each of 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;
the determining a similarity matching score of the current sub-map and the selected at least one candidate sub-map comprises:
obtaining a two-dimensional lookup table of at least one selected candidate sub-map, wherein each two-dimensional lookup table is constructed in advance based on Gaussian distribution probability density of continuous frame laser data set in the corresponding candidate sub-map;
determining a first matching score of the current sub-map in each two-dimensional lookup table;
and determining each first matching score as the similarity matching score of the current sub-map and the corresponding candidate sub-map.
2. The method of claim 1, wherein determining a first matching score for the current sub-map in each of the two-dimensional look-up tables comprises:
translating and/or rotating the laser terminal point in the current sub-map in each two-dimensional lookup table based on a first set translation and/or rotation rule;
acquiring a first projection score of the current sub-map after each translation and/or rotation in each two-dimensional lookup table;
determining a highest first projection score of the current sub-map in each of the two-dimensional lookup tables based on the first projection score after each translation and/or rotation;
determining each of the highest first projection scores as a first matching score of the current sub-map in a respective two-dimensional look-up table.
3. The method of claim 1, wherein determining a target match score from each of the similarity match scores that satisfies a first match filtering criteria comprises:
and screening first intermediate matching scores which are greater than or equal to a first set threshold value from the first matching scores, and determining the maximum value of the first intermediate matching scores as the target matching score.
4. The method of claim 1, further comprising:
and screening at least one candidate sub-map from the historical sub-map set according to a set screening rule.
5. The method of claim 4, wherein the filtering at least one candidate sub-map from the historical sub-map set according to the set filtering rule comprises:
screening at least two sub-maps to be matched corresponding to the current sub-map from the historical sub-map set according to a set positioning screening strategy;
acquiring a two-dimensional compression table corresponding to each sub-map to be matched, wherein the two-dimensional compression table is generated by performing pooling calculation on the maximum value of a two-dimensional lookup table corresponding to each sub-map to be matched;
determining second matching scores of the current sub-map in each two-dimensional compression table, and determining at least one candidate matching score meeting a second screening matching condition from each second matching score;
and determining the sub-map to be matched corresponding to each candidate matching score as a candidate sub-map.
6. The method according to claim 5, wherein the filtering at least two sub-maps to be matched corresponding to the current sub-map from the historical sub-map set according to a set positioning filtering strategy comprises:
determining a current estimated position of a terminal in a constructed global map based on a system odometer in the terminal;
acquiring the historical positions of the historical sub-maps in the historical sub-map set in the global map;
determining distance information from each historical sub-map to the current sub-map based on each historical position and the current estimated position;
and determining the historical sub-map of which the distance information is smaller than a set distance threshold value as a sub-map to be matched of the current sub-map.
7. The method of claim 5, wherein determining second matching scores for the current sub-map in each of the two-dimensional compression tables and determining at least one candidate matching score satisfying a second filtered matching condition from each of the second matching scores comprises:
translating and/or rotating the laser terminal point in the current sub-map in each two-dimensional compression table based on a second set translation and/or rotation rule;
acquiring a second projection score of the current sub-map after each translation and/or rotation in each two-dimensional compression table;
determining a highest second projection score of the current sub-map in each of the two-dimensional compression tables based on the second projection score after each translation and/or rotation;
determining each of the highest second projection scores as a second matching score of the current sub-map in the corresponding two-dimensional compression table;
when at least one second intermediate matching score which is larger than or equal to a second set threshold value exists in each second matching score, determining each second intermediate matching score as a candidate matching score; otherwise, determining the highest matching score in the second matching scores as a candidate matching score.
8. The method of claim 1, wherein forming a current sub-map presented in the form of a grid map from the captured laser data of successive frames of settings comprises:
determining the frame number of continuously captured laser data based on a laser radar on a terminal;
and if the number of the frames reaches a set value, forming a current sub-map presented in a grid map form according to laser tail end points in the captured laser data of each frame.
9. The method according to any one of claims 1-8, further comprising:
and adding the current sub-map serving as a new historical sub-map into a historical sub-map set, and constructing and storing a two-dimensional lookup table of the new historical sub-map, wherein element values of the two-dimensional lookup table are Gaussian distribution probability density values of continuous frame laser data set in the new historical sub-map.
10. A closed loop detection apparatus, comprising:
the sub-map building module is used for forming a current sub-map presented in a grid map form according to the captured laser data of the set continuous frames;
a similarity score determination module for determining a similarity matching score of the current sub-map and the selected at least one candidate sub-map;
a target score determining module for determining a target matching score satisfying a first matching screening condition from each of the similarity matching scores;
a closed-loop region determination module for determining a candidate sub-map corresponding to the target matching score as a closed-loop region of the current sub-map when it is determined that the target matching score exists;
wherein, the similarity score determining module comprises:
the device comprises a lookup table acquisition unit, a two-dimensional lookup table acquisition unit and a two-dimensional lookup table acquisition unit, wherein the lookup table acquisition unit is used for acquiring the two-dimensional lookup table of at least one selected candidate sub-map, and each two-dimensional lookup table is constructed in advance based on Gaussian distribution probability density of continuous frame laser data set in the corresponding candidate sub-map;
a first score determining unit, configured to determine a first matching score of the current sub-map in each of the two-dimensional lookup tables;
and the similarity score acquisition unit is used for determining each first matching score as the similarity matching score of the current sub-map and the corresponding candidate sub-map.
11. A terminal, comprising: lidar and system odometer, characterized by including:
one or more processors;
storage means for storing one or more programs;
the one or more programs are executable by the one or more processors to cause the one or more processors to implement the closed loop detection method of any one of claims 1-9.
12. A computer-readable storage medium, on which a computer program is stored, which program, when being executed by a processor, is adapted to carry out the closed loop detection method according to any one of claims 1 to 9.
CN201810039214.8A 2018-01-16 2018-01-16 Closed loop detection method, device, terminal and storage medium Active CN108256060B (en)

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 CN108256060A (en) 2018-07-06
CN108256060B true 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)

Families Citing this family (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110858076B (en) * 2018-08-22 2023-06-02 杭州海康机器人股份有限公司 Equipment positioning and grid map construction method and mobile robot
CN110376605B (en) * 2018-09-18 2022-02-01 北京京东尚科信息技术有限公司 Map construction method, navigation method and device
CN109614459B (en) * 2019-03-06 2019-05-28 上海思岚科技有限公司 Map structuring winding detection method and equipment applied to two-dimensional laser
TWI687706B (en) * 2019-04-02 2020-03-11 廣達電腦股份有限公司 Positioning system of mobile device
CN111538918B (en) * 2020-04-23 2022-03-29 上海高仙自动化科技发展有限公司 Recommendation method and device, electronic equipment and storage medium
CN111795687B (en) * 2020-06-29 2022-08-05 深圳市优必选科技股份有限公司 Robot map updating method and device, readable storage medium and robot
CN112102400B (en) * 2020-09-15 2022-08-02 上海云绅智能科技有限公司 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
CN115290066A (en) * 2022-07-06 2022-11-04 杭州萤石软件有限公司 Error correction method and device and mobile equipment

Citations (11)

* Cited by examiner, † Cited by third party
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 for relocating, device, terminal and the storage medium of terminal location

Patent Citations (11)

* Cited by examiner, † Cited by third party
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 for relocating, device, terminal and the storage medium of terminal location

Also Published As

Publication number Publication date
CN108256060A (en) 2018-07-06

Similar Documents

Publication Publication Date Title
CN108256060B (en) Closed loop detection method, device, terminal and storage medium
CN108256577B (en) Obstacle clustering method based on multi-line laser radar
WO2020134082A1 (en) Path planning method and apparatus, and mobile device
JP2018523875A (en) Lane recognition modeling method, apparatus, storage medium and device, and lane recognition method, apparatus, storage medium and apparatus
CN111968229A (en) High-precision map making method and device
CN113701760B (en) Robot anti-interference positioning method and device based on sliding window pose diagram optimization
CN106643721B (en) Construction method of environment topological map
CN108170807B (en) Map data processing method, map drawing method, map data processing device, map drawing device, map data processing equipment and storage medium
CN113467851B (en) Dynamic vehicle computing task unloading method and device based on vehicle clustering
CN112802204A (en) Target semantic navigation method and system for three-dimensional space scene prior in unknown environment
WO2020181508A1 (en) Digital surface model construction method, and processing device and system
CN112414403A (en) Robot positioning and attitude determining method, equipment and storage medium
CN113221403A (en) Finite element mesh optimization method, device and medium based on deep reinforcement learning
CN113096181B (en) Method and device for determining equipment pose, storage medium and electronic device
CN113313765B (en) Positioning method, positioning device, electronic equipment and storage medium
CN114491822A (en) Automobile stamping die process optimization method and device, storage medium and server
CN112581533B (en) Positioning method, positioning device, electronic equipment and storage medium
CN116051699B (en) Dynamic capture data processing method, device, equipment and storage medium
WO2011085434A1 (en) Extraction processes
CN116958529A (en) Robot remote control method, system and medium
CN111080080A (en) Method and system for estimating risk of geological disaster of villages and small towns
WO2011085437A1 (en) Extraction processes
CN115239779A (en) Three-dimensional point cloud registration method, device, medium and electronic equipment
CN114115231A (en) Mobile robot space pose point cloud correction method and system suitable for hospital scene
CN114740867A (en) Intelligent obstacle avoidance method and device based on binocular vision, robot and medium

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