CN113761091B - Closed loop detection method, device, electronic equipment, system and storage medium - Google Patents
Closed loop detection method, device, electronic equipment, system and storage medium Download PDFInfo
- Publication number
- CN113761091B CN113761091B CN202011360108.3A CN202011360108A CN113761091B CN 113761091 B CN113761091 B CN 113761091B CN 202011360108 A CN202011360108 A CN 202011360108A CN 113761091 B CN113761091 B CN 113761091B
- Authority
- CN
- China
- Prior art keywords
- point cloud
- landmark information
- current position
- cloud frame
- closed
- 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
Links
- 238000001514 detection method Methods 0.000 title claims abstract description 90
- 238000000034 method Methods 0.000 claims abstract description 30
- 239000011159 matrix material Substances 0.000 claims description 18
- 238000006243 chemical reaction Methods 0.000 claims description 14
- 238000004590 computer program Methods 0.000 claims description 13
- 230000008569 process Effects 0.000 description 14
- 238000010586 diagram Methods 0.000 description 9
- 238000004891 communication Methods 0.000 description 6
- 238000002474 experimental method Methods 0.000 description 5
- 230000006870 function Effects 0.000 description 5
- 238000005457 optimization Methods 0.000 description 5
- 230000003287 optical effect Effects 0.000 description 4
- 238000013519 translation Methods 0.000 description 4
- 230000001960 triggered effect Effects 0.000 description 4
- 238000012545 processing Methods 0.000 description 3
- 239000000969 carrier Substances 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 239000000835 fiber Substances 0.000 description 2
- 230000004807 localization Effects 0.000 description 2
- 238000013507 mapping Methods 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 230000000644 propagated effect Effects 0.000 description 2
- 239000004065 semiconductor Substances 0.000 description 2
- 238000013461 design Methods 0.000 description 1
- 230000010354 integration Effects 0.000 description 1
- 239000004973 liquid crystal related substance Substances 0.000 description 1
- 239000013307 optical fiber Substances 0.000 description 1
- 238000000059 patterning Methods 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
Classifications
-
- 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/29—Geographical information databases
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/70—Determining position or orientation of objects or cameras
- G06T7/73—Determining position or orientation of objects or cameras using feature-based methods
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T2207/00—Indexing scheme for image analysis or image enhancement
- G06T2207/10—Image acquisition modality
- G06T2207/10028—Range image; Depth image; 3D point clouds
Landscapes
- Engineering & Computer Science (AREA)
- Theoretical Computer Science (AREA)
- Databases & Information Systems (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Remote Sensing (AREA)
- Data Mining & Analysis (AREA)
- General Engineering & Computer Science (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Management, Administration, Business Operations System, And Electronic Commerce (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
The embodiment of the invention discloses a closed loop detection method, a closed loop detection device, an electronic device, a closed loop detection system and a storage medium, wherein the method comprises the following steps: acquiring a current point cloud frame and landmark information of a current position of a carrier; determining whether the current position is a closed-loop candidate position according to the current point cloud frame and the historical point cloud frame; when the current position is a closed-loop candidate position, determining whether the current position is a closed-loop target position according to landmark information of the current position and landmark information of historical positions of the carrier. The embodiment of the invention can improve the accuracy of closed loop detection.
Description
Technical Field
The present invention relates to positioning technologies, and in particular, to a closed loop detection method, apparatus, electronic device, system, and storage medium.
Background
In the instant localization and mapping (simultaneous localization and mapping, SLAM) system, the closed loop detection technique is a key technique. The closed loop detection, also called loop detection, is to identify whether the current scene of the carrier moving independently is the scene that has arrived, so that the composition of the SLAM system forms a closed loop in the moving process of the carrier. In the process of realizing the invention, the inventor discovers that the current closed loop detection technology has low accuracy.
Disclosure of Invention
The embodiment of the invention provides a closed loop detection method, a closed loop detection device, electronic equipment, a closed loop detection system and a storage medium, which can improve the accuracy of closed loop detection.
In a first aspect, an embodiment of the present invention provides a closed loop detection method, where the method includes:
acquiring a current point cloud frame and landmark information of a current position of a carrier;
determining whether the current position is a closed-loop candidate position according to the current point cloud frame and the historical point cloud frame;
and when the current position is a closed-loop candidate position, determining whether the current position is a closed-loop target position according to landmark information of the current position and landmark information of historical positions of the carrier.
In a second aspect, an embodiment of the present invention provides a closed loop detection apparatus, the apparatus including:
the acquisition module is used for acquiring the current point cloud frame and landmark information of the current position of the carrier;
the first determining module is used for determining whether the current position is a closed-loop candidate position according to the current point cloud frame and the historical point cloud frame;
and the second determining module is used for determining whether the current position is a closed-loop target position according to landmark information of the current position and landmark information of historical positions of the carrier when the current position is a closed-loop candidate position.
In a third aspect, an embodiment of the present invention further provides an electronic device, including a memory, a processor, and a computer program stored in the memory and capable of running on the processor, where the processor implements the closed loop detection method according to any one of the embodiments of the present invention when executing the program.
In a fourth aspect, an embodiment of the present invention further provides a closed loop detection system, including a carrier and a method for performing the closed loop detection method according to any one of the embodiments of the present invention.
In a fifth aspect, embodiments of the present invention further provide a computer readable storage medium having stored thereon a computer program which, when executed by a processor, implements a closed loop detection method according to any of the embodiments of the present invention.
In the embodiment of the invention, after the current point cloud frame is acquired and landmark information of the current position of the carrier is acquired, whether the current position is a closed-loop candidate position or not is determined according to the current point cloud frame and the historical point cloud frame; when the current position is a closed-loop candidate position, determining whether the current position is a closed-loop target position according to landmark information of the current position and landmark information of historical positions of the carrier. In the embodiment of the invention, after the initial closed-loop detection is performed by using the point cloud frame, the landmark information is used for further closed-loop detection, and the two types of information, namely the point cloud frame and the landmark information, are fused for closed-loop detection, so that the accuracy of closed-loop detection is improved.
Drawings
Fig. 1 is a schematic flow chart of a closed loop detection method according to an embodiment of the present invention.
Fig. 2 is another flow chart of a closed loop detection method according to an embodiment of the present invention.
Fig. 3 is a schematic structural diagram of a closed loop detection device according to an embodiment of the present invention.
Fig. 4 is a schematic structural diagram of a closed loop detection system according to an embodiment of the present invention.
Fig. 5 is a schematic structural diagram of an electronic device according to an embodiment of the present invention.
Detailed Description
The invention is described in further detail below with reference to the drawings and examples. It is to be understood that the specific embodiments described herein are merely illustrative of the invention and are not limiting thereof. It should be further noted that, for convenience of description, only some, but not all of the structures related to the present invention are shown in the drawings.
Fig. 1 is a schematic flow chart of a closed loop detection method according to an embodiment of the present invention, where the method may be performed by a closed loop detection device according to an embodiment of the present invention, and the device may be implemented in software and/or hardware. In a specific embodiment, the apparatus may be integrated in an electronic device. The electronic device may be a computer device having a storage unit and a microprocessor mounted therein, such as a tablet computer, a notebook computer, and a desktop computer, and having an arithmetic capability. The following embodiments will be described taking the integration of the device in an electronic apparatus as an example. Referring to fig. 1, the method may specifically include the steps of:
step 101, acquiring a current point cloud frame and landmark information of a current position of a carrier.
Illustratively, the carrier may be an autonomously movable carrier of an unmanned vehicle, an unmanned aerial vehicle, a robot, or the like. The carrier can move under the control of remote control equipment (such as a remote controller, a handle and the like), a laser radar can be installed on the carrier, the laser radar can be used for collecting position information of obstacles (such as buildings, vehicles and the like) and boundaries in the environment in the moving process of the carrier, and the collected information can be represented by a point cloud frame.
In the moving process of the carrier, the laser radar on the carrier can be utilized to collect the point cloud frames in real time, and the collected point cloud frames are uploaded to the electronic equipment, wherein each frame of the point cloud frames can comprise point cloud data, position data, time stamps and the like. The position data in the point cloud frame can be used for identifying the position of the carrier when the point cloud frame is acquired, and the position data can be represented by coordinates; the timestamp in the point cloud frame may be used to identify the time when the point cloud frame was acquired.
In a specific embodiment, a landmark set may be preset according to a travel route planned for the carrier, where the preset landmark set may include a plurality of landmark identifiers, and the landmark identifiers may be names, codes, and the like of the landmarks. Illustratively, the landmark may be a post, sort point, building (e.g., cell, building, etc.) or the like that plans a route in the travel route.
In the process of moving the carrier, when one landmark in the landmark set is preset by the carrier path, landmark information of the current position can be automatically triggered or manually triggered to the electronic device by the carrier. Landmark identification and a time stamp may be included in the landmark information. Wherein the timestamp in the landmark information may be used to identify a time at which the landmark information was sent to the electronic device. For example, when the carrier passes through a designated post, a post worker may trigger a landmark information sending button on the carrier, so that the carrier sends landmark information of the current position to the electronic device; or when the electronic equipment detects that the electronic equipment reaches a designated post, landmark information of the current position is automatically sent to the electronic equipment.
The electronic equipment receives the point cloud frame and landmark information sent by the carrier, and provides the received point cloud frame and landmark information for the SLAM system for positioning and composition. In addition, the electronic device may also acquire inertial navigation data, wheel speed meter data, and the like of the carrier, and provide the acquired inertial navigation data, wheel speed meter data, and the like to the SLAM system to assist the SLAM system in positioning and patterning.
In the process of composition of the SLAM system, the electronic equipment can perform closed loop detection in real time, when the closed loop detection is needed, the electronic equipment can acquire a current point cloud frame and landmark information of the current position of the carrier, wherein position data contained in the current point cloud frame can be the current position, namely, the current point cloud frame is the point cloud frame acquired at the current position, and a time stamp in the current point cloud frame is the same as a time stamp in the landmark information of the current position.
Step 102, determining whether the current position is a closed-loop candidate position according to the current point cloud frame and the historical point cloud frame of the carrier.
Specifically, the history point cloud frame may have multiple frames, and it may be determined whether the current position is a closed-loop candidate position according to an iterative closest point (Iterative Closest Point, ICP) matching algorithm, specifically as follows:
(1) And selecting a candidate point cloud frame from the historical point cloud frames.
For example, according to the position data included in each frame of history point cloud frame and the current position included in the current point cloud frame, the distance between each frame of history point cloud frame and the current point cloud frame is calculated, a point cloud frame, the distance between which is smaller than the preset distance and the time difference between the time stamp and the time stamp included in the current point cloud frame is larger than the second time difference, is selected from the history point cloud frames, and the selected point cloud frame is used as the candidate point cloud frame.
The preset distance and the second time difference value can be set according to actual needs, or experiments, or experience. For example, the preset distance is a radius R, and the second time difference is T2, that is, a history point cloud frame with a time difference greater than T2 and within the radius R around the current position is selected as the candidate point cloud frame.
(2) And calculating ICP matching scores of the current point cloud frame and each frame of candidate point cloud frame to obtain a matching score set.
For example, the current point cloud frame may be represented by P, the candidate point cloud frame of any frame may be represented by Q, then the nearest point of each point in P in Q may be found out, a corresponding point pair is obtained, an average value or an average value evolution of the distances between all the point pairs is calculated, the calculated value is used as the ICP matching score of P and Q, and similarly, the ICP matching score of the current point cloud frame and the candidate point cloud frame of each frame may be calculated, and a matching score set may be obtained.
(3) And when the minimum ICP matching score in the matching score set is smaller than the preset matching score, determining the current position as a closed-loop candidate position.
The preset matching score can be set according to actual needs, or experiments, or experience. In a specific embodiment, for example, the preset match score may be 0.2.
Specifically, if the minimum ICP matching score in the matching score set is smaller than the preset matching score, determining that the current position is a closed-loop candidate position, that is, the current position is relatively close to the position of the candidate point cloud frame corresponding to the minimum ICP matching score, and the current position may be a target position of closed-loop detection; if the minimum ICP matching score in the matching score set is not smaller than the preset matching score, determining that the current position is not a closed-loop candidate position, namely that the current position is far away from the position of the candidate point cloud frame corresponding to the minimum ICP matching score, and the current position is not a target position of closed-loop detection to a large extent.
In addition, whether the current position is the closed-loop candidate position may be determined in other manners, for example, the similarity between the current point cloud frame and each frame of candidate point cloud frame is calculated, a similarity set is obtained, when a similarity value satisfying a preset similarity condition exists in the similarity set, the current position is determined to be the closed-loop candidate position, and a method for determining whether the current position is the closed-loop candidate position is specifically determined.
And step 103, when the current position is a closed-loop candidate position, determining whether the current position is a closed-loop target position according to landmark information of the current position and landmark information of historical positions of the carrier.
For example, it may be determined whether the current position is a closed loop target position as follows:
(1) It is determined whether landmark information matching landmark information of the current location exists among landmark information of the history location.
For example, it may be determined whether landmark information having the same landmark identifier as that contained in the landmark information of the current position and having a time difference greater than a first time difference value from a time stamp contained in the landmark information of the current position exists in the landmark information of the historical position, and if landmark information having the same landmark identifier as that contained in the landmark information of the current position and having a time difference greater than a preset time difference value from a time stamp contained in the landmark information of the current position exists, landmark information matching with the landmark information of the current position exists in the landmark information of the historical position; otherwise, determining that landmark information matched with landmark information of the current position does not exist in landmark information of the historical position. The first time difference may be set according to actual needs, or experiment, or experience.
(2) If landmark information matched with the landmark information of the current position exists, the current position is determined to be a closed-loop target position.
For example, if the current position is the closed-loop target position, the closed-loop detection is successful, and if the current position is not the closed-loop target position, the closed-loop detection fails.
In a specific embodiment, when the closed loop detection fails, the detection task may be ended, or the step 101 may be returned to, and the detection is continued. When the closed loop detection is successful, the composition of the SLAM system can be optimized according to the closed loop detection result, and the specific optimization method can be as follows:
(1) And determining candidate point cloud frames corresponding to the minimum ICP matching score to obtain the key point cloud frames.
The key point cloud frame is the frame with the smallest ICP matching score with the current point cloud frame in the candidate point cloud frames.
(2) And calculating an ICP conversion matrix between the current point cloud frame and the key point cloud frame.
For example, the ICP conversion matrix may include a rotation matrix and a translation matrix, that is, a conversion matrix that performs operations such as rotation and translation on the current point cloud frame, so that the current point cloud frame and the key point cloud frame overlap to the greatest extent may be found.
(3) The composition of the SLAM system is optimized according to the ICP conversion matrix.
For example, the relevant point cloud frames in the composition of the SLAM system can be subjected to pose adjustment and optimization according to the ICP conversion matrix, so that the composition of the SLAM system forms a closed loop.
In the embodiment of the invention, after the current point cloud frame is acquired and landmark information of the current position of the carrier is acquired, whether the current position is a closed-loop candidate position or not is determined according to the current point cloud frame and the historical point cloud frame; when the current position is a closed-loop candidate position, determining whether the current position is a closed-loop target position according to landmark information of the current position and landmark information of historical positions of the carrier. In the embodiment of the invention, after the initial closed-loop detection is performed by using the point cloud frame, the landmark information is used for further closed-loop detection, and the two types of information, namely the point cloud frame and the landmark information, are fused for closed-loop detection, so that the accuracy of closed-loop detection is improved.
The above embodiments describe a method for performing closed-loop detection by combining two types of information, namely, a point cloud frame and landmark information, and in some embodiments, only the type of information, namely, landmark information, can be used for performing closed-loop detection; or if the current position is not the closed-loop candidate position, continuing to adopt landmark information to carry out closed-loop detection; or when the two types of information, namely the point cloud frame and the landmark information are adopted for closed-loop detection, the sequence of the closed-loop detection by adopting the point cloud frame and the closed-loop detection by adopting the landmark information can be adjusted according to actual needs, and the method is not particularly limited.
The closed loop detection method provided by the embodiment of the invention is described below by taking a carrier as an unmanned logistics vehicle as an example, and as shown in fig. 2, the method comprises the following steps:
step 201, acquiring a current point cloud frame and landmark information of a current position of an unmanned logistics vehicle.
For example, in the running process of the unmanned logistics vehicle, the laser radar on the unmanned logistics vehicle can be utilized to collect the point cloud frame in real time, the collected point cloud frame is uploaded to the electronic equipment, and each frame of the point cloud frame can comprise point cloud data, position data, a time stamp and the like. The position data in the point cloud frame can be used for identifying the position of the unmanned logistics vehicle when the point cloud frame is acquired, and the position data can be represented by coordinates; the time stamp in the point cloud frame may be used to identify the time when the point cloud frame was acquired, and may be represented, for example, in the following format: year-month-day-hour-minute-second.
In the running process of the unmanned logistics vehicle, when one landmark in the landmark set is preset in the way of the unmanned logistics vehicle, the unmanned logistics vehicle can be automatically triggered or manually triggered to send landmark information of the current position to the electronic equipment. Landmark identification and a time stamp may be included in the landmark information. Wherein the timestamp in the landmark information may be used to identify a time at which the landmark information was sent to the electronic device. For example, when the unmanned logistics vehicle passes through a designated post, a post worker can trigger a landmark information sending button on the unmanned logistics vehicle, so that the unmanned logistics vehicle sends landmark information of the current position to the electronic equipment; or when the electronic equipment detects that the electronic equipment reaches a designated post, landmark information of the current position is automatically sent to the electronic equipment.
The electronic equipment receives the point cloud frame and landmark information sent by the unmanned logistics vehicle, and provides the received point cloud frame and landmark information for the SLAM system for positioning and composition. In addition, the electronic device can also acquire inertial navigation data, wheel speed meter data and the like of the unmanned logistics vehicle, and provide the acquired inertial navigation data, the wheel speed meter data and the like for the SLAM system so as to assist the SLAM system in positioning and composition.
In the process of the SLAM system composition, the electronic equipment can perform closed loop detection in real time. When closed loop detection is required, the electronic device can acquire a current point cloud frame and landmark information of the current position of the unmanned logistics vehicle, wherein position data contained in the current point cloud frame can be the current position, namely, the current point cloud frame is a point cloud frame acquired at the current position, and a timestamp in the current point cloud frame is the same as a timestamp in landmark information of the current position.
Step 202, selecting a candidate point cloud frame from the history point cloud frames.
For example, according to the position data included in each frame of history point cloud frame and the current position included in the current point cloud frame, the distance between each frame of history point cloud frame and the current point cloud frame is calculated, a point cloud frame, the distance between which is smaller than the preset distance and the time difference between the time stamp and the time stamp included in the current point cloud frame is larger than the second time difference, is selected from the history point cloud frames, and the selected point cloud frame is used as the candidate point cloud frame. For example, a history point cloud frame within 30 m of the radius around the current position and with a time difference of more than 1 hour is selected as the candidate point cloud frame.
And 203, calculating ICP matching scores of the current point cloud frame and each frame of candidate point cloud frame to obtain a matching score set.
For example, P represents the current point cloud frame, Q represents any candidate point cloud frame, then the nearest point of each point in P in Q may be found out, a corresponding point pair is obtained, an average value or an average value evolution of distances between all the point pairs is calculated, the calculated value is used as an ICP matching score of P and Q, and similarly, an ICP matching score of the current point cloud frame and each candidate point cloud frame may be calculated, and a matching score set may be obtained. For example, if the candidate point cloud frame has 100 frames, there will be 100 ICP match scores in the match score set.
Step 204, determining whether the minimum ICP match score in the match score set is smaller than the preset match score, if so, executing step 205, otherwise, executing step 211.
The preset matching score can be set according to actual needs, or experiments, or experience. For example, the preset match score may be 0.2.
Specifically, if the minimum ICP matching score in the matching score set is smaller than the preset matching score, determining that the current position is a closed-loop candidate position, that is, the current position is relatively close to the position of the candidate point cloud frame corresponding to the minimum ICP matching score, and the current position may be a target position of closed-loop detection; if the minimum ICP matching score in the matching score set is not smaller than the preset matching score, determining that the current position is not a closed-loop candidate position, namely that the current position is far away from the position of the candidate point cloud frame corresponding to the minimum ICP matching score, and the current position is not a target position of closed-loop detection to a large extent.
Step 205, determining the current location as a closed loop candidate location.
Step 206, judging whether landmark information matched with landmark information of the current position exists in landmark information of the historical position of the unmanned logistics vehicle, if so, executing step 207, otherwise, executing step 211.
For example, it may be determined whether landmark information having the same landmark identifier as that contained in the landmark information of the current position and having a time difference greater than a first time difference value from a time stamp contained in the landmark information of the current position exists in the landmark information of the historical position, and if landmark information having the same landmark identifier as that contained in the landmark information of the current position and having a time difference greater than a preset time difference value from a time stamp contained in the landmark information of the current position exists, landmark information matching with the landmark information of the current position exists in the landmark information of the historical position; otherwise, determining that landmark information matched with landmark information of the current position does not exist in landmark information of the historical position. The first time difference may be set according to actual needs, or experiment, or experience.
For example, if the landmark information of the current location is (landmark identifier a, timestamp 10:00), the landmark information of the history location is (landmark identifier a, timestamp 7:30), (landmark identifier B, timestamp 8:00), (landmark identifier C, timestamp 9:00), (landmark identifier D, timestamp 9:30), and the first time difference is one hour, it may be determined that there is landmark information matching the landmark information of the current location in the landmark information of the history location.
Step 207, determining the current position as a closed loop target position.
For example, if the current position is the closed-loop target position, the closed-loop detection is successful, and if the current position is not the closed-loop target position, the closed-loop detection fails. In a specific embodiment, when the closed loop detection fails, the detection task can be ended, or the previous step can be returned to, and the detection is continued.
And step 208, determining a candidate point cloud frame corresponding to the minimum ICP matching score to obtain a key point cloud frame.
Step 209, calculating an ICP conversion matrix between the current point cloud frame and the key point cloud frame.
For example, the ICP conversion matrix may include a rotation matrix and a translation matrix, that is, a conversion matrix that performs operations such as rotation and translation on the current point cloud frame, so that the current point cloud frame and the key point cloud frame overlap to the greatest extent may be found.
Step 210, optimizing the composition of the SLAM system according to the ICP conversion matrix.
For example, the relevant point cloud frames in the composition of the SLAM system can be subjected to pose adjustment and optimization according to the ICP conversion matrix, so that the composition of the SLAM system forms a closed loop.
Step 211, end.
In the embodiment of the invention, after the current point cloud frame is acquired and landmark information of the current position of the carrier is acquired, whether the current position is a closed-loop candidate position or not is determined according to the current point cloud frame and the historical point cloud frame; when the current position is a closed-loop candidate position, determining whether the current position is a closed-loop target position according to landmark information of the current position and landmark information of historical positions of the carrier. In the embodiment of the invention, after the initial closed-loop detection is performed by using the point cloud frame, the landmark information is used for further closed-loop detection, and the two types of information, namely the point cloud frame and the landmark information, are fused for closed-loop detection, so that the accuracy of closed-loop detection is improved.
Fig. 3 is a block diagram of a closed loop detection apparatus according to an embodiment of the present invention, where the apparatus is adapted to perform the closed loop detection method according to the embodiment of the present invention. As shown in fig. 3, the apparatus may specifically include:
the acquisition module 301 is configured to acquire a current point cloud frame, and acquire landmark information of a current position of a carrier;
a first determining module 302, configured to determine whether the current location is a closed-loop candidate location according to the current point cloud frame and the historical point cloud frame;
and a second determining module 303, configured to determine, when the current position is a closed-loop candidate position, whether the current position is a closed-loop target position according to landmark information of the current position and landmark information of a historical position of the carrier.
In one embodiment, the second determining module 303 determines whether the current location is a closed loop target location according to landmark information of the current location and landmark information of a historical location of the carrier, including:
determining whether landmark information matched with landmark information of the current position exists in landmark information of the historical position;
and if the landmark information matched with the landmark information of the current position exists, determining the current position as a closed-loop target position.
In one embodiment, landmark identification and a timestamp are included in landmark information of each historical location, and the second determining module 303 determines whether landmark information matched with landmark information of the current location exists in landmark information of the historical location, including:
determining whether landmark information with the same landmark identifier as that contained in the landmark information of the current position and a time difference between a time stamp and the time stamp contained in the landmark information of the current position is larger than a first time difference value exists in the landmark information of the historical position;
if the landmark identification is the same as the landmark identification contained in the landmark information of the current position and the time difference between the time stamp and the time stamp contained in the landmark information of the current position is larger than the preset time difference value, determining that landmark information matched with the landmark information of the current position exists in the landmark information of the historical position.
In an embodiment, the landmark identifier included in the landmark information of the current position and the landmark identifier included in the landmark information of the historical position belong to a preset landmark set.
In one embodiment, the first determining module 302 determines whether the current location is a closed loop candidate location according to the current point cloud frame and the historical point cloud frame of the carrier, including:
selecting a candidate point cloud frame from the history point cloud frames;
calculating ICP matching scores of the current point cloud frame and the candidate point cloud frame of each frame to obtain a matching score set;
and when the minimum ICP matching score in the matching score set is smaller than a preset matching score, determining the current position as a closed-loop candidate position.
In an embodiment, each frame of the historical point cloud frames includes point cloud data, location data, and a timestamp, and the first determining module 302 selects a candidate point cloud frame from the historical point cloud frames, including:
calculating the distance between the historical point cloud frame and the current point cloud frame according to the position data included in the historical point cloud frame and the current position of each frame;
selecting a frame with a distance smaller than a preset distance from the current point cloud frame and a time difference between a time stamp and a time stamp contained in the current point cloud frame being larger than a second time difference value from the historical point cloud frame, and taking the selected frame as the candidate point cloud frame.
In one embodiment, the apparatus further comprises an optimization module;
the optimization module is used for determining candidate point cloud frames corresponding to the minimum ICP matching score to obtain key point cloud frames; calculating an ICP conversion matrix between the current point cloud frame and the key point cloud frame; and optimizing the composition of the SLAM system according to the ICP conversion matrix.
It will be apparent to those skilled in the art that, for convenience and brevity of description, only the above-described division of the functional modules is illustrated, and in practical application, the above-described functional allocation may be performed by different functional modules according to needs, i.e. the internal structure of the apparatus is divided into different functional modules to perform all or part of the functions described above. The specific working process of the functional module described above may refer to the corresponding process in the foregoing method embodiment, and will not be described herein.
According to the device provided by the embodiment of the invention, after the current point cloud frame is acquired and landmark information of the current position of the carrier is acquired, whether the current position is a closed-loop candidate position or not is determined according to the current point cloud frame and the historical point cloud frame; when the current position is a closed-loop candidate position, determining whether the current position is a closed-loop target position according to landmark information of the current position and landmark information of historical positions of the carrier. In other words, after the initial closed-loop detection is performed by using the point cloud frame, the device of the embodiment of the invention performs further closed-loop detection by using the landmark information, combines the two types of information, namely the point cloud frame and the landmark information, and improves the accuracy of the closed-loop detection.
The embodiment of the invention also provides electronic equipment, which comprises a memory, a processor and a computer program stored in the memory and capable of running on the processor, wherein the closed loop detection method provided by any embodiment is realized when the processor executes the program.
The embodiment of the invention also provides a computer readable medium, on which a computer program is stored, the program, when executed by a processor, implementing the closed loop detection method provided by any of the above embodiments.
Fig. 4 illustrates an exemplary system architecture 600 in which the closed loop detection method or closed loop detection apparatus of embodiments of the present invention may be applied.
As shown in fig. 4, the system architecture 600 may include electronic devices 601, 602, 603, a network 604, and a carrier 605. The network 604 is used as a medium to provide communications links between the electronic devices 601, 602, 603 and the carrier 605. The network 604 may include various connection types, such as wired, wireless communication links, or fiber optic cables, among others.
A user may interact with the carrier 605 via the network 604 using the electronic devices 601, 602, 603 to receive or send messages, etc. The electronic devices 601, 602, 603 may have installed thereon various client applications such as enterprise application clients, e-commerce application clients, web browser applications, search class applications, instant messaging tools, mailbox clients, and the like.
The electronic devices 601, 602, 603 may be a variety of electronic devices having a display screen and supporting a variety of clients, including but not limited to smartphones, tablets, laptop and desktop computers, and the like.
The carrier 605 may be a carrier that provides various services, such as providing logistical services. The carrier may process the received request and feed back the request data to the electronic device.
It should be noted that, the closed loop detection method provided by the embodiment of the present invention is generally executed by an electronic device, and accordingly, the closed loop detection device is generally disposed in the electronic device.
It should be understood that the number of electronic devices, networks, and carriers in fig. 4 are merely illustrative. Any number of electronic devices, networks, and carriers may be present as desired for implementation.
Referring now to FIG. 5, there is illustrated a schematic diagram of a computer system 700 suitable for use in implementing an electronic device of an embodiment of the present invention. The electronic device shown in fig. 5 is only an example and should not be construed as limiting the functionality and scope of use of the embodiments of the present invention.
As shown in fig. 5, the computer system 700 includes a Central Processing Unit (CPU) 701, which can perform various appropriate actions and processes according to a program stored in a Read Only Memory (ROM) 702 or a program loaded from a storage section 708 into a Random Access Memory (RAM) 703. In the RAM 703, various programs and data required for the operation of the system 700 are also stored. The CPU 701, ROM 702, and RAM 703 are connected to each other through a bus 704. An input/output (I/O) interface 705 is also connected to bus 704.
The following components are connected to the I/O interface 705: an input section 706 including a keyboard, a mouse, and the like; an output portion 707 including a Cathode Ray Tube (CRT), a Liquid Crystal Display (LCD), and the like, a speaker, and the like; a storage section 708 including a hard disk or the like; and a communication section 709 including a network interface card such as a LAN card, a modem, or the like. The communication section 709 performs communication processing via a network such as the internet. The drive 710 is also connected to the I/O interface 705 as needed. A removable medium 711 such as a magnetic disk, an optical disk, a magneto-optical disk, a semiconductor memory, or the like is mounted on the drive 710 as necessary, so that a computer program read therefrom is mounted into the storage section 708 as necessary.
In particular, according to embodiments of the present disclosure, the processes described above with reference to flowcharts may be implemented as computer software programs. For example, embodiments of the present disclosure include a computer program product comprising a computer program embodied on a computer readable medium, the computer program comprising program code for performing the method shown in the flow chart. In such an embodiment, the computer program may be downloaded and installed from a network via the communication portion 709, and/or installed from the removable medium 711. The above-described functions defined in the system of the present invention are performed when the computer program is executed by a Central Processing Unit (CPU) 701.
The computer readable medium shown in the present invention may be a computer readable signal medium or a computer readable storage medium, or any combination of the two. The computer readable storage medium can be, for example, but not limited to, an electronic, magnetic, optical, electromagnetic, infrared, or semiconductor system, apparatus, or device, or a combination of any of the foregoing. More specific examples of the computer-readable storage medium may include, but are not limited to: an electrical connection having one or more wires, a portable computer diskette, a hard disk, a Random Access Memory (RAM), a read-only memory (ROM), an erasable programmable read-only memory (EPROM or flash memory), an optical fiber, a portable compact disc read-only memory (CD-ROM), an optical storage device, a magnetic storage device, or any suitable combination of the foregoing. In the context of this document, a computer readable storage medium may be any tangible medium that can contain, or store a program for use by or in connection with an instruction execution system, apparatus, or device. In the present invention, however, the computer-readable signal medium may include a data signal propagated in baseband or as part of a carrier wave, with the computer-readable program code embodied therein. Such a propagated data signal may take any of a variety of forms, including, but not limited to, electro-magnetic, optical, or any suitable combination of the foregoing. A computer readable signal medium may also be any computer readable medium that is not a computer readable storage medium and that can communicate, propagate, or transport a program for use by or in connection with an instruction execution system, apparatus, or device. Program code embodied on a computer readable medium may be transmitted using any appropriate medium, including but not limited to: wireless, wire, fiber optic cable, RF, etc., or any suitable combination of the foregoing.
The flowcharts and block diagrams in the figures illustrate the architecture, functionality, and operation of possible implementations of systems, methods and computer program products according to various embodiments of the present invention. In this regard, each block in the flowchart or block diagrams may represent a module, segment, or portion of code, which comprises one or more executable instructions for implementing the specified logical function(s). It should also be noted that, in some alternative implementations, the functions noted in the block may occur out of the order noted in the figures. For example, two blocks shown in succession may, in fact, be executed substantially concurrently, or the blocks may sometimes be executed in the reverse order, depending upon the functionality involved. It will also be noted that each block of the block diagrams or flowchart illustration, and combinations of blocks in the block diagrams or flowchart illustration, can be implemented by special purpose hardware-based systems which perform the specified functions or acts, or combinations of special purpose hardware and computer instructions.
The modules and/or units involved in the embodiments of the present invention may be implemented in software, or may be implemented in hardware. The described modules and/or units may also be provided in a processor, e.g., may be described as: a processor includes an acquisition module, a first determination module, and a second determination module. The names of these modules do not constitute a limitation on the module itself in some cases.
As another aspect, the present invention also provides a computer-readable medium that may be contained in the apparatus described in the above embodiments; or may be present alone without being fitted into the device. The computer readable medium carries one or more programs which, when executed by a device, cause the device to include: acquiring a current point cloud frame and landmark information of a current position of a carrier; determining whether the current position is a closed-loop candidate position according to the current point cloud frame and the historical point cloud frame; and when the current position is a closed-loop candidate position, determining whether the current position is a closed-loop target position according to landmark information of the current position and landmark information of historical positions of the carrier.
According to the technical scheme of the embodiment of the invention, after the current point cloud frame of the carrier is obtained and landmark information of the current position of the carrier is obtained, whether the current position is a closed-loop candidate position or not is determined according to the current point cloud frame and the historical point cloud frame of the carrier; when the current position is a closed-loop candidate position, determining whether the current position is a closed-loop target position according to landmark information of the current position and landmark information of historical positions of the carrier. In the embodiment of the invention, after the initial closed-loop detection is performed by using the point cloud frame, the landmark information is used for further closed-loop detection, and the two types of information, namely the point cloud frame and the landmark information, are fused for closed-loop detection, so that the accuracy of closed-loop detection is improved.
The above embodiments do not limit the scope of the present invention. It will be apparent to those skilled in the art that various modifications, combinations, sub-combinations and alternatives can occur depending upon design requirements and other factors. Any modifications, equivalent substitutions and improvements made within the spirit and principles of the present invention should be included in the scope of the present invention.
Claims (9)
1. A closed loop detection method, comprising:
acquiring a current point cloud frame and landmark information of a current position of a carrier;
determining whether the current position is a closed-loop candidate position according to the current point cloud frame and the historical point cloud frame;
when the current position is a closed-loop candidate position, determining whether the current position is a closed-loop target position according to landmark information of the current position and landmark information of historical positions of the carrier comprises the following steps: determining whether landmark information with the same landmark identifier as that contained in the landmark information of the current position and a time difference between a time stamp and the time stamp contained in the landmark information of the current position is larger than a first time difference value exists in the landmark information of the historical position; if landmark information with the same landmark identifier as that contained in the landmark information of the current position and a time difference between a time stamp and the time stamp contained in the landmark information of the current position being larger than a preset time difference value exists, determining that landmark information matched with the landmark information of the current position exists in the landmark information of the historical position; if landmark information matched with the landmark information of the current position exists, determining the current position as a closed-loop target position; landmark information for each of the historical locations includes a landmark identification and a timestamp.
2. The closed loop detection method according to claim 1, wherein the landmark identifier contained in the landmark information of the current position and the landmark identifier contained in the landmark information of the history position each belong to a preset landmark set.
3. The closed loop detection method according to claim 1 or 2, wherein the determining whether the current position is a closed loop candidate position according to the current point cloud frame and the history point cloud frame comprises:
selecting a candidate point cloud frame from the history point cloud frames;
calculating ICP matching scores of the current point cloud frame and the candidate point cloud frame of each frame to obtain a matching score set;
and when the minimum ICP matching score in the matching score set is smaller than a preset matching score, determining the current position as a closed-loop candidate position.
4. The method of claim 3, wherein each frame of the historical point cloud frames includes point cloud data, location data, and a timestamp, and the selecting candidate point cloud frames from the historical point cloud frames includes:
calculating the distance between the historical point cloud frame and the current point cloud frame according to the position data included in the historical point cloud frame and the current position of each frame;
and selecting a point cloud frame with a distance smaller than a preset distance from the current point cloud frame and a time difference between a time stamp and a time stamp contained in the current point cloud frame being larger than a second time difference value from the historical point cloud frame, and taking the selected point cloud frame as the candidate point cloud frame.
5. The closed loop detection method of claim 3, wherein when the current position is a closed loop target position, the method further comprises:
determining candidate point cloud frames corresponding to the minimum ICP matching score to obtain key point cloud frames;
calculating an ICP conversion matrix between the current point cloud frame and the key point cloud frame;
and optimizing the composition of the SLAM system according to the ICP conversion matrix.
6. A closed loop detection device, comprising:
the acquisition module is used for acquiring the current point cloud frame and landmark information of the current position of the carrier;
the first determining module is used for determining whether the current position is a closed-loop candidate position according to the current point cloud frame and the historical point cloud frame;
a second determining module, configured to determine, when the current position is a closed-loop candidate position, whether the current position is a closed-loop target position according to landmark information of the current position and landmark information of a historical position of the carrier, where the second determining module includes: determining whether landmark information with the same landmark identifier as that contained in the landmark information of the current position and a time difference between a time stamp and the time stamp contained in the landmark information of the current position is larger than a first time difference value exists in the landmark information of the historical position; if landmark information with the same landmark identifier as that contained in the landmark information of the current position and a time difference between a time stamp and the time stamp contained in the landmark information of the current position being larger than a preset time difference value exists, determining that landmark information matched with the landmark information of the current position exists in the landmark information of the historical position; if landmark information matched with the landmark information of the current position exists, determining the current position as a closed-loop target position; landmark information for each of the historical locations includes a landmark identification and a timestamp.
7. An electronic device comprising a memory, a processor and a computer program stored on the memory and executable on the processor, wherein the processor implements the closed loop detection method of any of claims 1 to 5 when the program is executed by the processor.
8. A closed loop detection system comprising a carrier and an electronic device for performing the closed loop detection method according to any of claims 1 to 5.
9. A computer readable storage medium having stored thereon a computer program, characterized in that the program, when executed by a processor, implements the closed loop detection method according to any of claims 1 to 5.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202011360108.3A CN113761091B (en) | 2020-11-27 | 2020-11-27 | Closed loop detection method, device, electronic equipment, system and storage medium |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202011360108.3A CN113761091B (en) | 2020-11-27 | 2020-11-27 | Closed loop detection method, device, electronic equipment, system and storage medium |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN113761091A CN113761091A (en) | 2021-12-07 |
| CN113761091B true CN113761091B (en) | 2024-04-05 |
Family
ID=78786099
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202011360108.3A Active CN113761091B (en) | 2020-11-27 | 2020-11-27 | Closed loop detection method, device, electronic equipment, system and storage medium |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN113761091B (en) |
Families Citing this family (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN115290066A (en) * | 2022-07-06 | 2022-11-04 | 杭州萤石软件有限公司 | Error correction method and device and mobile equipment |
Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109974721A (en) * | 2019-01-08 | 2019-07-05 | 武汉中海庭数据技术有限公司 | A kind of vision winding detection method and device based on high-precision map |
| CN110125928A (en) * | 2019-03-27 | 2019-08-16 | 浙江工业大学 | A kind of binocular inertial navigation SLAM system carrying out characteristic matching based on before and after frames |
| CN110689622A (en) * | 2019-07-05 | 2020-01-14 | 电子科技大学 | Synchronous positioning and composition algorithm based on point cloud segmentation matching closed-loop correction |
| CN111476305A (en) * | 2020-04-10 | 2020-07-31 | 深圳前海达闼云端智能科技有限公司 | S L AM closed loop detection method, device, medium and equipment |
| CN111795704A (en) * | 2020-06-30 | 2020-10-20 | 杭州海康机器人技术有限公司 | Method and device for constructing visual point cloud map |
| CN111862162A (en) * | 2020-07-31 | 2020-10-30 | 湖北亿咖通科技有限公司 | Loop detection method and system, readable storage medium and electronic device |
Family Cites Families (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| EP3014296A4 (en) * | 2013-06-27 | 2017-06-21 | Intel Corporation | Systems and methods for detection of magnetic and motion-based landmarks |
| CN107179086B (en) * | 2017-05-24 | 2020-04-24 | 北京数字绿土科技有限公司 | Drawing method, device and system based on laser radar |
| CN109064506B (en) * | 2018-07-04 | 2020-03-13 | 百度在线网络技术（北京）有限公司 | High-precision map generation method and device and storage medium |
| US10782137B2 (en) * | 2019-01-28 | 2020-09-22 | Qfeeltech (Beijing) Co., Ltd. | Methods, apparatus, and systems for localization and mapping |
-
2020
- 2020-11-27 CN CN202011360108.3A patent/CN113761091B/en active Active
Patent Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109974721A (en) * | 2019-01-08 | 2019-07-05 | 武汉中海庭数据技术有限公司 | A kind of vision winding detection method and device based on high-precision map |
| CN110125928A (en) * | 2019-03-27 | 2019-08-16 | 浙江工业大学 | A kind of binocular inertial navigation SLAM system carrying out characteristic matching based on before and after frames |
| CN110689622A (en) * | 2019-07-05 | 2020-01-14 | 电子科技大学 | Synchronous positioning and composition algorithm based on point cloud segmentation matching closed-loop correction |
| CN111476305A (en) * | 2020-04-10 | 2020-07-31 | 深圳前海达闼云端智能科技有限公司 | S L AM closed loop detection method, device, medium and equipment |
| CN111795704A (en) * | 2020-06-30 | 2020-10-20 | 杭州海康机器人技术有限公司 | Method and device for constructing visual point cloud map |
| CN111862162A (en) * | 2020-07-31 | 2020-10-30 | 湖北亿咖通科技有限公司 | Loop detection method and system, readable storage medium and electronic device |
Non-Patent Citations (4)
| Title |
|---|
| Delay-CLD:一种基于三维激光雷达的闭环检测算法;汪小龙;滕滕;;荆楚理工学院学报(03);6-12 * |
| 一种基于地磁信号的激光同时定位与建图的闭环检测方法;陈贝章;李慧云;;集成技术(05);60-70 * |
| 一种适用于激光SLAM大尺度地图的闭环检测方法;文国成;曾碧;陈云华;;计算机应用研究(06);130-133+138 * |
| 融合语义激光与地标信息的SLAM技术研究;杨爽;曾碧;何炜婷;;计算机工程与应用(18);268-277 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN113761091A (en) | 2021-12-07 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN111209978B (en) | Three-dimensional visual repositioning method and device, computing equipment and storage medium | |
| CN110989633B (en) | Robot control method and device, computer equipment and storage medium | |
| AU2017345227B2 (en) | Method and system for generating navigation data transporting object | |
| CN108876857B (en) | Method, system, device and storage medium for positioning unmanned vehicle | |
| CN111859597B (en) | Evaluation method and system of automatic driving algorithm | |
| CN113135178A (en) | Parking route sharing method, device, equipment and storage medium | |
| CN110554688B (en) | Method and device for generating topological map | |
| CN111988524A (en) | Unmanned aerial vehicle and camera collaborative obstacle avoidance method, server and storage medium | |
| CN113761091B (en) | Closed loop detection method, device, electronic equipment, system and storage medium | |
| CN110888433B (en) | Control method and device for automatic alignment charging pile | |
| CN112622923B (en) | Method and device for controlling a vehicle | |
| CN114012740B (en) | Target place leading method and device based on robot and robot | |
| CN109189082B (en) | Method, device and control system for controlling mobile robot | |
| CN109933058B (en) | Method, apparatus, server and storage medium for determining movement error of removable device | |
| US11873009B2 (en) | Method, apparatus and control system for controlling mobile robot | |
| CN111399489B (en) | Method and device for generating information | |
| CN113932796A (en) | High-precision map lane line generation method and device and electronic equipment | |
| US20210041257A1 (en) | Method, Apparatus, Computer Device and Readable Storage Medium for Processing Position Information | |
| CN111966767A (en) | Track thermodynamic diagram generation method and device, electronic equipment and storage medium | |
| CN117406725A (en) | Path planning method, device, equipment and storage medium | |
| CN112880675B (en) | Pose smoothing method and device for visual positioning, terminal and mobile robot | |
| CN110375752B (en) | Method and device for generating navigation points | |
| CN112414391B (en) | Repositioning method and device for robot | |
| CN113778073A (en) | Robot running method, device and system applied to indoor scene | |
| CN113970754A (en) | Positioning method and device of autonomous travelable equipment |
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 |