CN111680747B - Method and apparatus for closed loop detection of occupancy grid subgraphs - Google Patents
Method and apparatus for closed loop detection of occupancy grid subgraphs Download PDFInfo
- Publication number
- CN111680747B CN111680747B CN202010513248.3A CN202010513248A CN111680747B CN 111680747 B CN111680747 B CN 111680747B CN 202010513248 A CN202010513248 A CN 202010513248A CN 111680747 B CN111680747 B CN 111680747B
- Authority
- CN
- China
- Prior art keywords
- pose
- grid
- node
- resolution
- subgraph
- 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
Classifications
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F18/00—Pattern recognition
- G06F18/20—Analysing
- G06F18/22—Matching criteria, e.g. proximity measures
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
-
- 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
- G01S17/89—Lidar systems specially adapted for specific applications for mapping or imaging
-
- 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
- G01S17/93—Lidar systems specially adapted for specific applications for anti-collision purposes
- G01S17/931—Lidar systems specially adapted for specific applications for anti-collision purposes of land vehicles
-
- 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
Abstract
The embodiment of the application discloses a closed loop detection method and device for occupying a grid subgraph, and relates to the technical field of automatic driving. One embodiment of the method comprises the following steps: acquiring currently scanned laser point cloud data and an established occupied grid subgraph; converting the established occupancy grid subgraph into at least one resolution occupancy grid subgraph; constructing a tree structure according to at least one resolution occupying grid subgraph, wherein each layer of nodes in the tree structure represents a pose search space under one resolution, the resolution increases downwards from a root node, and each node represents a pose; and starting from the root node of the tree structure, calculating the matching degree of the pose represented by each node and the laser point cloud data layer by layer, and enabling the node with high matching degree to enter the next layer of nodes preferentially to continue calculating the matching degree until the leaf nodes are matched to obtain the optimal pose. The implementation mode can effectively reduce the search time in closed loop detection, and further reduce the accumulated error.
Description
Technical Field
Embodiments of the present disclosure relate to the field of autopilot technology, and in particular, to a method and apparatus for closed loop detection of occupancy grid subgraphs.
Background
In a conventional laser SLAM (simultaneous localization and mapping) system, the pose estimation of a current frame depends on the pose estimation of a previous frame, and errors are continuously accumulated in the whole positioning process. These errors are not apparent during small scale movements, but as the distance of movement increases, the accumulated errors quickly reach a larger magnitude. Therefore, the positioning track is restrained by closing the loop, and the accumulated error is effectively eliminated.
Disclosure of Invention
In the laser SLAM system, each frame of laser radar scanning data is transmitted to be matched with a recently established sub-graph (Submap), the matching method is to adopt the least square optimization problem of Gaussian Newton solution, so that the laser radar scanning data of the frame is inserted into the optimal position on the current sub-graph, and the process is called scanning matching. With the continuous insertion of new laser radar scan data, the subgraph is updated, and when the subgraph reaches a certain amount of laser radar scan data, no new laser radar scan data is inserted, the subgraph is already built. Then the next sub-graph is created. Large scale scene map creation is achieved by creating a large number of subgraphs. The pose estimation of the subgraph obtained through scanning matching is accurate and reliable in a short time, but accumulated errors exist for a long time, and the poses of all subgraphs are required to be optimized through closed loop detection (also called loop detection), so that the accumulated errors are reduced. Closed loop detection is performed at intervals, and all created sub-images and current frame lidar scan data are used as scan matches for closed loop detection. Generally, the scanning matching strategy of closed loop detection is to set the neighborhood of the neighboring sub-image as a search space according to the laser radar scanning data of the current frame. And searching the optimal matching pose in the search space, so that the score of the laser radar scanning data of the current frame is highest under the pose (namely, the laser radar scanning data is most similar to the sub-image environment), and then, a closed loop is found, and the matching result can be used as a closed loop detection constraint condition of the pose optimization problem. However, this scan matching strategy for closed loop detection is a brute force solution, i.e., a matching calculation score is performed for each frame in the current laser scan and its vicinity search space to find the maximum value thereof as a matching result. Such matching search is inefficient, time consuming and computationally intensive.
Disclosure of Invention
Embodiments of the present disclosure propose methods and apparatus for closed loop detection of occupancy grid subgraphs.
In a first aspect, embodiments of the present disclosure provide a method for closed loop detection of occupancy of a grid subgraph, comprising: acquiring currently scanned laser point cloud data and an established occupied grid subgraph; converting the established occupancy grid subgraph into at least one resolution occupancy grid subgraph; constructing a tree structure according to at least one resolution occupying grid subgraph, wherein each layer of nodes in the tree structure represents a pose search space under one resolution, the resolution increases downwards from a root node, and each node represents a pose; and starting from the root node of the tree structure, calculating the matching degree of the pose represented by each node and the laser point cloud data layer by layer, and enabling the node with high matching degree to enter the next layer of nodes preferentially to continue calculating the matching degree until the leaf nodes are matched to obtain the optimal pose.
In some embodiments, the method further comprises: and splicing the laser point cloud data and the established occupied grid subgraph based on the optimal pose to obtain the optimized pose.
In some embodiments, the method further comprises: acquiring at least one candidate pose adjacent to the optimized pose in a pose search space; and respectively carrying out consistency check on at least one candidate pose and the laser point cloud data.
In some embodiments, the method further comprises: and adding graph optimization to the established occupied grid subgraph according to the pose which is successfully checked.
In some embodiments, the method further comprises: acquiring GPS information of a current vehicle; and clipping the established occupied grid subgraph according to GPS information to reduce the pose search space.
In some embodiments, calculating, layer by layer, a degree of matching of pose of each node characterization with laser point cloud data includes: for each low resolution grid, taking as the occupancy grid probability of the low resolution grid the maximum value of the occupancy grid probabilities of at least one high resolution grid that constitutes the low resolution grid; and calculating the matching degree of the pose represented by each node and the laser point cloud data layer by adopting a branch-and-bound method and a depth optimization search method based on the occupied grid probability of each resolution occupied grid subgraph.
In a second aspect, embodiments of the present disclosure provide an apparatus for closed loop detection of occupancy grid subgraphs, comprising: an acquisition unit configured to acquire currently scanned laser point cloud data and an established occupancy grid subgraph; a conversion unit configured to convert the established occupancy grid subgraph into an occupancy grid subgraph of at least one resolution; a building unit configured to build a tree structure from the occupancy grid subgraphs of at least one resolution, wherein each level of nodes in the tree structure represents a pose search space at one resolution, the resolution increasing downward from the root node, each node representing a pose; the searching unit is configured to calculate the matching degree of the pose represented by each node and the laser point cloud data layer by layer from the root node of the tree structure, and the node with high matching degree preferentially enters the next layer of nodes to continue calculating the matching degree until the leaf node is matched to obtain the optimal pose.
In some embodiments, the apparatus further comprises a pose optimization unit configured to: and splicing the laser point cloud data and the established occupied grid subgraph based on the optimal pose to obtain the optimized pose.
In some embodiments, the apparatus further comprises a verification unit configured to: acquiring at least one candidate pose adjacent to the optimized pose in a pose search space; and respectively carrying out consistency check on at least one candidate pose and the laser point cloud data.
In some embodiments, the apparatus further comprises a graph optimization unit configured to: and adding graph optimization to the established occupied grid subgraph according to the pose which is successfully checked.
In some embodiments, the apparatus further comprises a clipping unit configured to: acquiring GPS information of a current vehicle; and clipping the established occupied grid subgraph according to GPS information to reduce the pose search space.
In some embodiments, the search unit is further configured to: for each low resolution grid, taking as the occupancy grid probability of the low resolution grid the maximum value of the occupancy grid probabilities of at least one high resolution grid that constitutes the low resolution grid; and calculating the matching degree of the pose represented by each node and the laser point cloud data layer by adopting a branch-and-bound method and a depth optimization search method based on the occupied grid probability of each resolution occupied grid subgraph.
In a third aspect, embodiments of the present disclosure provide an electronic device for closed loop detection of occupancy of a grid subgraph, comprising: one or more processors; a storage device having one or more programs stored thereon, which when executed by one or more processors, cause the one or more processors to implement the method as in any of the first aspects.
In a fourth aspect, embodiments of the present disclosure provide a computer readable medium having a computer program stored thereon, wherein the program when executed by a processor implements a method as in any of the first aspects.
The embodiment of the disclosure provides a method and a device for closed loop detection of an occupied grid subgraph, which are used for setting up a tree structure according to search spaces under different resolutions by setting up the occupied grid subgraph with multiple resolutions. Instead of violent detection, the tree can be effectively cut by adopting a branch-and-bound search algorithm and a depth-first search algorithm along the nodes of the tree, so that the searching speed in closed loop detection is effectively increased, the accumulated error is further reduced, and the establishment of a large-scale scene map is realized.
Drawings
Other features, objects and advantages of the present disclosure will become more apparent upon reading of the detailed description of non-limiting embodiments, made with reference to the following drawings:
FIG. 1 is an exemplary system architecture diagram in which an embodiment of the present disclosure may be applied;
FIG. 2 is a flow chart of one embodiment of a method for closed loop detection of occupancy grid subgraphs in accordance with the present disclosure;
fig. 3a, 3b are schematic diagrams of one application scenario of a method for closed loop detection of occupancy grid subgraphs according to the present disclosure;
FIG. 4 is a flow chart of yet another embodiment of a method for closed loop detection of occupancy grid subgraphs in accordance with the present disclosure;
FIG. 5 is a schematic structural view of one embodiment of an apparatus for closed loop detection of occupancy grid subgraphs in accordance with the present disclosure;
fig. 6 is a schematic diagram of a computer system suitable for use in implementing embodiments of the present disclosure.
Detailed Description
The present disclosure 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 application and are not limiting of the application. It should be noted that, for convenience of description, only the portions related to the present application are shown in the drawings.
It should be noted that, without conflict, the embodiments of the present disclosure and features of the embodiments may be combined with each other. The present disclosure will be described in detail below with reference to the accompanying drawings in conjunction with embodiments.
Fig. 1 shows an exemplary system architecture 100 to which an embodiment of the method for closed loop detection of occupancy grid subgraph or the apparatus for closed loop detection of occupancy grid subgraph of the present application may be applied.
As shown in fig. 1, the system architecture 100 may include an unmanned vehicle 101.
The unmanned vehicle 101 may have installed therein a driving control device 1011, a network 1012, and a laser radar 1013. The network 1012 is a medium to provide a communication link between the drive control device 1011 and the lidar 1013. Network 1012 may include various connection types, such as wired, wireless communication links, or fiber optic cables, among others.
The driving control device (also referred to as a vehicle-mounted brain) 1011 is responsible for intelligent control of the unmanned vehicle 101. The driving control device 1011 may be a separately provided controller such as a programmable logic controller (Programmable Logic Controller, PLC), a single chip microcomputer, an industrial controller, or the like; the device can also be equipment consisting of other electronic devices with input/output ports and operation control functions; but also a computer device installed with a vehicle driving control type application.
In practice, at least one sensor such as a camera, a gravity sensor, a wheel speed sensor, or the like may be mounted in the unmanned vehicle 101. In some cases, a GNSS (Global Navigation Satellite System ) device and a SINS (Strap-down Inertial Navigation System, strapdown inertial navigation system) may also be installed in the drone 101, and so on.
It should be noted that, the method for closed-loop detection of occupying the grid subgraph provided by the embodiment of the present application is generally performed by the driving control device 1011, and accordingly, the means for closed-loop detection of occupying the grid subgraph is generally provided in the driving control device 1011.
It should be understood that the number of driving control devices, networks and lidars in fig. 1 is merely illustrative. There may be any number of drive control devices, networks, and lidars as desired for implementation.
With continued reference to FIG. 2, a flow 200 of one embodiment of a method for closed loop detection of occupancy grid subgraphs in accordance with the present application is shown. The method for closed loop detection of occupancy grid subgraphs comprises the following steps:
step 201, acquiring currently scanned laser point cloud data and an established occupancy grid subgraph.
In this embodiment, during the running of the unmanned vehicle, various vehicle-mounted sensors (e.g., lidar, camera, millimeter wave radar, etc.) mounted on the unmanned vehicle may collect external environmental information in real time, and the lidar generates laser point cloud data and transmits the laser point cloud data to an electronic device (e.g., driving control device shown in fig. 1) on which a method for closed-loop detection occupying a grid subgraph operates. The electronic device can analyze and process the received various sensor data to perceive the surrounding environment of the vehicle to construct a subgraph. The method has the advantages that a large number of sub-images are created to realize large-scale scene map establishment, the pose estimation of the sub-images obtained through scanning matching is accurate and reliable in a short time, but accumulated errors exist for a long time, the poses of all the sub-images need to be optimized through closed loop detection, and the accumulated errors are reduced.
The occupancy grid subgraph (occupied grid submap) is a grid map in which the values in the grid are typically probability values, which characterize the probability that the grid is occupied by an obstacle. Each grid is a cube, the side length of the grid being related to the resolution, the higher the resolution the smaller the side length.
Step 202, converting the established occupancy grid subgraph into at least one resolution occupancy grid subgraph.
In this embodiment, the resolution of the original established occupancy grid subgraph is highest, e.g., 5cm in side length. The established occupancy grid subgraph is converted into an occupancy grid subgraph of at least one resolution. It is the actual generation of multiple low resolution occupancy grid subgraphs. For example, converting a grid that originally occupied a grid sub-image with a side length of 5cm to a 10cm grid reduces resolution. Each resolution corresponds to a search window (including translation distance, rotation angle), the lower the resolution, the greater the range of movement per search. For example, when searching for a grid with a side length of 5cm, each translation is 5cm or rotation is 1 degree, and when searching for a grid with a side length of 10cm, each translation is 10cm or rotation is 2 degrees. As shown in fig. 3a, only an example of a bird's eye-view grid is given for ease of understanding. 301 represents an occupied grid sub-graph of the original resolution. 302 is the occupied grid subgraph after the original grid side length is doubled (resolution reduced by one). 303 is the occupied grid subgraph after the original grid edge is doubled (resolution reduced by a factor of two). 304 is the occupied grid subgraph after the original grid side length has been expanded three times (resolution reduced three times).
Step 203, constructing a tree structure according to the occupied grid subgraph of at least one resolution.
In the present embodiment, in SLAM, the so-called pose actually refers to the pose of the camera in the world coordinate system. Pose includes two aspects: position and pose, i.e. three-dimensional coordinates and orientation. Each level of nodes in the tree structure characterizes a pose search space at a resolution that increases downward from the root node, and each node characterizes a pose, i.e., a solution to the discrete search space of coordinates (x, y, z) and angles. Fig. 3b shows a tree structure of a partial occupancy grid subgraph. Each leaf node represents a pose at the original resolution. The layer of leaf nodes corresponds to 301 shown in fig. 3 a. The root node corresponds to 304 shown in fig. 3 a.
The established occupancy grid subgraph includes the occupancy probability for each grid at the original resolution. After conversion to the low resolution occupancy grid subgraph, the occupancy probability of each grid in the low resolution occupancy grid subgraph is the maximum of the multiple sub-resolution grid probability values. For example, the grid 302 is composed of 4 grids 301 of original resolution, and the probabilities of occupying the grids are 0.4,0.6,0.8,0.2, respectively. The probability of occupying the grid 302 is 0.8. The probability of occupying the grid at various resolutions can be calculated from the original resolution.
Optionally, acquiring GPS information of the current vehicle; and clipping the established occupied grid subgraph according to GPS information to reduce the pose search space. Only the subgraph within the predetermined range of GPS information from among the established occupied grid subgraphs is retained.
Step 204, starting from the root node of the tree structure, calculating the matching degree of the pose represented by each node and the laser point cloud data layer by layer, and enabling the node with high matching degree to enter the next layer of nodes preferentially to continue calculating the matching degree until the leaf nodes are matched to obtain the optimal pose.
In this embodiment, after the tree structure is built, the matching degree of the pose represented by each node and the laser point cloud data is calculated layer by layer from the root node. The matching degree is obtained by projecting laser point cloud data of current scanning into a grid map and adding the grid probability values of all hit points. And starting searching downwards from the root node, and if the matching degree of a certain node in the same layer is high, preferentially entering the searching of the next layer node from the node. As shown in fig. 3b, the 303 layers have 4 nodes, the laser point cloud data of the current scanning is projected into the established occupied grid subgraph according to the pose corresponding to the A, B, C, D nodes, and the probability values of the hit grids are added to obtain the matching degree of the node corresponding to the pose, namely the score. It is assumed that node a has a higher degree of matching than node B, node C, and node D. The search of the next layer (layer 302) starts from the child node of node a. And respectively projecting the currently scanned laser point cloud data into the established occupied grid subgraph according to the pose corresponding to the nodes A1, A2, A3 and A4, and adding probability values of hit grids to obtain the matching degree of the node corresponding to the pose, namely the score. Assume that the matching degree of the A1 node is higher than that of other child nodes of the a node. The search of the next layer (layer 301) starts from the child node of the A1 node. And finding out a leaf node with the highest matching degree from the leaf nodes of the layer 301, wherein the corresponding pose of the leaf node is the optimal pose.
In order to increase the search speed, tree structures may be pruned. For example, as shown in FIG. 3a, 304 may be divided into four blocks. Coarse-grained pose, e.g., 4 poses with different xy values, may be tried so that the projections are located in the four regions, respectively, after the laser point cloud data is adjusted with the 4 poses. If the matching degree of a certain area is highest, further searching the pose with fine granularity in the pose range corresponding to the area until the leaf node of the pose corresponding to the area is searched to obtain the optimal score, searching the pose of the other three areas, and if the matching degree of the other three areas is smaller than the optimal score, discarding the search of the other three areas. Otherwise, searching nodes with the above optimal score along subtrees corresponding to the areas with the above optimal score until the leaf node with the highest matching degree in all the nodes is found. Thereby improving the search speed.
The best pose can be found by adopting a branch-and-bound method and depth-first search, and the method is concretely as follows:
(a) Initializing best_score=score_threshold (typically set from empirical values or set directly to a very small value; initializing a stack C, wherein the stack C stores initial nodes of the tree-shaped solution space of the current layer and unprocessed nodes of the previous layer, the storage sequence of the initial nodes in the stack C is ordered according to the score (matching degree) value, and the largest score value is arranged at the top of the stack. The selection of the subsequent node is determined using a depth-first search.
(b) If stack C is not empty, popping up the current top of stack node X, calculating node X score value score (X) (sum of probabilities of hit grid after projection), if score (X) < best_score, returning to step (b), otherwise, making the following determination:
(i) If the current node X is a leaf node, the current score value and node are stored, i.e., best_score=score (X), match=x, and then go back to step (b).
(ii) If the current node X is not a leaf node, splitting the current node X to obtain a child node, pushing the child node to stack, putting the stacking rule in the same step (a) (the child node is placed at the top of the stack according to the sorting of the scores), and returning to the step (b).
(c) If stack C is empty, the algorithm is exited, returning best_score (score for best pose) and match (best pose).
With further reference to fig. 4, a flow 400 of yet another embodiment of a method for closed loop detection of occupancy grid subgraphs is shown. The flow 400 of the method for closed loop detection of occupancy grid subgraphs comprises the steps of:
step 401, acquiring currently scanned laser point cloud data and an established occupancy grid subgraph.
Step 402, converting the established occupancy grid subgraph into at least one resolution occupancy grid subgraph.
Step 403, constructing a tree structure according to the occupancy grid subgraph of at least one resolution.
Step 404, starting from the root node of the tree structure, calculating the matching degree of the pose represented by each node and the laser point cloud data layer by layer, and enabling the node with high matching degree to enter the next layer of nodes preferentially to continue calculating the matching degree until the best pose is obtained by matching at the leaf nodes.
Steps 401-404 are substantially identical to steps 201-204 and are therefore not described in detail.
And step 405, splicing the laser point cloud data and the established occupied grid subgraph based on the optimal pose to obtain the optimized pose.
In this embodiment, the present application may perform the stitching of the laser point cloud data and the established occupied grid subgraph by using an existing stitching algorithm, such as ICP (Iterative Closest Point, closest point search method), and search the best pose as a measurement point, and search the corresponding set of nearby point points, so as to implement curve fitting, thereby converting the discrete best pose into an optimized continuous pose.
Step 406, at least one candidate pose adjacent to the optimized pose in the pose search space is obtained, and consistency verification is carried out on the at least one candidate pose and the laser point cloud data respectively.
In this embodiment, the closed loop detection result generally has consistency, that is, there are a plurality of closed loop detection results around the current candidate, which can be matched, and the closed loop detection result is checked by using the characteristic. For example, the optimized pose is slightly rotated by a certain angle to serve as a candidate pose, then projected to an established occupied grid subgraph, the matching degree is calculated, if the difference between the matching degree and the matching degree of the optimal pose is smaller than a preset value, the candidate pose is considered to be qualified, and if the number of qualified candidate poses exceeds the preset value, the optimized pose passes consistency verification and can be used for graph optimization. Otherwise, the best pose needs to be searched again. The laser point cloud data can be rescanned after the vehicle is moved, and closed loop detection is performed again. And a plurality of frames of point clouds matched to the vicinity of a certain position of the submap can be added for splicing, and consistency verification can be performed.
And step 407, adding graph optimization to the established occupied grid subgraph according to the pose of successful verification.
In this embodiment, when closed-loop information (pose with successful verification) is found, the SLAM may use an algorithm such as Bundle Adjuestment (BA) to correct (i.e. add map optimization) the original pose topological map, so as to effectively correct the map after closed-loop, and realize more reliable environment mapping.
With further reference to fig. 5, as an implementation of the method illustrated in the above figures, the present disclosure provides an embodiment of an apparatus for closed loop detection of occupancy grid subgraphs, which corresponds to the method embodiment illustrated in fig. 2, and which is particularly applicable in various electronic devices.
As shown in fig. 5, the apparatus 500 for closed loop detection of an occupied grid subgraph of the present embodiment includes: an acquisition unit 501, a conversion unit 502, a construction unit 503, and a search unit 504. Wherein the acquiring unit 501 is configured to acquire currently scanned laser point cloud data and an established occupied grid subgraph; a conversion unit 502 configured to convert the established occupancy grid subgraph into an occupancy grid subgraph of at least one resolution; a building unit 503 configured to build a tree structure according to the occupancy grid subgraph of at least one resolution, wherein each level of nodes in the tree structure represents a pose search space at one resolution, the resolution increases downwards from the root node, each node represents a pose; the searching unit 504 is configured to calculate, from the root node of the tree structure, the matching degree of the pose represented by each node and the laser point cloud data layer by layer, and the node with high matching degree preferentially enters the next layer of nodes to continue calculating the matching degree until the leaf node is matched to obtain the optimal pose.
In this embodiment, specific processes of the acquisition unit 501, the conversion unit 502, the construction unit 503, and the search unit 504 of the apparatus 500 for closed loop detection of an occupied grid subgraph may refer to steps 201, 202, 203, and 204 in the corresponding embodiment of fig. 2.
In some optional implementations of the present embodiment, the apparatus 500 further comprises a pose optimization unit (not shown in the drawings) configured to: and splicing the laser point cloud data and the established occupied grid subgraph based on the optimal pose to obtain the optimized pose.
In some optional implementations of the present embodiment, the apparatus 500 further includes a verification unit (not shown in the drawings) configured to: acquiring at least one candidate pose adjacent to the optimized pose in a pose search space; and respectively carrying out consistency check on at least one candidate pose and the laser point cloud data.
In some optional implementations of the present embodiment, the apparatus 500 further includes a graph optimization unit (not shown in the drawings) configured to: and adding graph optimization to the established occupied grid subgraph according to the pose which is successfully checked.
In some optional implementations of the present embodiment, the apparatus 500 further includes a clipping unit (not shown in the drawings) configured to: acquiring GPS information of a current vehicle; and clipping the established occupied grid subgraph according to GPS information to reduce the pose search space.
In some optional implementations of the present embodiment, the search unit 504 is further configured to: for each low resolution grid, taking as the occupancy grid probability of the low resolution grid the maximum value of the occupancy grid probabilities of at least one high resolution grid that constitutes the low resolution grid; and calculating the matching degree of the pose represented by each node and the laser point cloud data layer by adopting a branch-and-bound method and a depth optimization search method based on the occupied grid probability of each resolution occupied grid subgraph.
Referring now to fig. 6, a schematic structural diagram of an electronic device (e.g., the drive control device of fig. 1) 600 suitable for use in implementing embodiments of the present disclosure is shown. The driving control apparatus shown in fig. 6 is only one example, and should not bring any limitation to the function and the range of use of the embodiment of the present disclosure.
As shown in fig. 6, the electronic device 600 may include a processing means (e.g., a central processing unit, a graphics processor, etc.) 601, which may perform various appropriate actions and processes according to a program stored in a Read Only Memory (ROM) 602 or a program loaded from a storage means 608 into a Random Access Memory (RAM) 603. In the RAM 603, various programs and data required for the operation of the electronic apparatus 600 are also stored. The processing device 601, the ROM 602, and the RAM 603 are connected to each other through a bus 604. An input/output (I/O) interface 605 is also connected to bus 604.
In general, the following devices may be connected to the I/O interface 605: input devices 606 including, for example, a touch screen, touchpad, keyboard, mouse, camera, microphone, accelerometer, gyroscope, and the like; an output device 607 including, for example, a Liquid Crystal Display (LCD), a speaker, a vibrator, and the like; storage 608 including, for example, magnetic tape, hard disk, etc.; and a communication device 609. The communication means 609 may allow the electronic device 600 to communicate with other devices wirelessly or by wire to exchange data. While fig. 6 shows an electronic device 600 having various means, it is to be understood that not all of the illustrated means are required to be implemented or provided. More or fewer devices may be implemented or provided instead. Each block shown in fig. 6 may represent one device or a plurality of devices as needed.
In particular, according to embodiments of the present disclosure, the processes described above with reference to flowcharts may be implemented as computer software programs. The computer program contains 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 communication means 609, or from storage means 608, or from ROM 602. The above-described functions defined in the methods of the embodiments of the present disclosure are performed when the computer program is executed by the processing means 601. It should be noted that, the computer readable medium according to the embodiments of the present disclosure 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 an embodiment of the present disclosure, 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. Whereas in embodiments of the present disclosure, the computer-readable signal medium may comprise a data signal propagated in baseband or as part of a carrier wave, with 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: electrical wires, fiber optic cables, RF (radio frequency), and the like, or any suitable combination of the foregoing.
The computer readable medium may be contained in the electronic device; or may exist alone without being incorporated into the electronic device. The computer readable medium carries one or more programs which, when executed by the electronic device, cause the electronic device to: one embodiment of the method comprises the following steps: acquiring currently scanned laser point cloud data and an established occupied grid subgraph; converting the established occupancy grid subgraph into at least one resolution occupancy grid subgraph; constructing a tree structure according to at least one resolution occupying grid subgraph, wherein each layer of nodes in the tree structure represents a pose search space under one resolution, the resolution increases downwards from a root node, and each node represents a pose; and starting from the root node of the tree structure, calculating the matching degree of the pose represented by each node and the laser point cloud data layer by layer, and enabling the node with high matching degree to enter the next layer of nodes preferentially to continue calculating the matching degree until the leaf nodes are matched to obtain the optimal pose.
Computer program code for carrying out operations of embodiments of the present disclosure may be written in one or more programming languages, including an object oriented programming language such as Java, smalltalk, C ++ and conventional procedural programming languages, such as the "C" programming language or similar programming languages. The program code may execute entirely on the user's computer, partly on the user's computer, as a stand-alone software package, partly on the user's computer and partly on a remote computer or entirely on the remote computer or server. In the case of a remote computer, the remote computer may be connected to the user's computer through any kind of network, including a Local Area Network (LAN) or a Wide Area Network (WAN), or may be connected to an external computer (for example, through the Internet using an Internet service provider).
The flowcharts and block diagrams in the figures illustrate the architecture, functionality, and operation of possible implementations of systems, methods according to various embodiments of the present disclosure. 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 and/or flowchart illustration, and combinations of blocks in the block diagrams and/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 units involved in the embodiments described in the present disclosure may be implemented by means of software, or may be implemented by means of hardware. The described units may also be provided in a processor, for example, described as: a processor includes an acquisition unit, a conversion unit, a construction unit, and a search unit. The names of these units do not constitute a limitation on the unit itself in some cases, for example, the acquisition unit may also be described as "acquiring currently scanned laser point cloud data and an established unit occupying a grid subgraph".
The foregoing description is only of the preferred embodiments of the present disclosure and description of the principles of the technology being employed. It will be appreciated by those skilled in the art that the scope of the application referred to in this disclosure is not limited to the specific combination of features described above, but encompasses other embodiments in which any combination of features described above or their equivalents is contemplated without departing from the inventive concepts described. Such as those described above, are mutually substituted with the technical features having similar functions disclosed in the present disclosure (but not limited thereto).
Claims (14)
1. A method for closed loop detection of occupancy grid subgraphs, comprising:
acquiring currently scanned laser point cloud data and an established occupied grid subgraph;
converting the established occupancy grid subgraph into at least one resolution occupancy grid subgraph;
constructing a tree structure according to the occupancy grid subgraph with at least one resolution, wherein each layer of nodes in the tree structure represents a pose search space under one resolution, the resolution increases downwards from a root node, and each node represents a pose;
and starting from the root node of the tree structure, calculating the matching degree of the pose represented by each node and the laser point cloud data layer by layer, and enabling the node with high matching degree to enter the node of the next layer preferentially to continue calculating the matching degree until the best pose is obtained by matching at the leaf node.
2. The method of claim 1, wherein the method further comprises:
and splicing the laser point cloud data and the established occupied grid subgraph based on the optimal pose to obtain the optimized pose.
3. The method of claim 2, wherein the method further comprises:
acquiring at least one candidate pose adjacent to the optimized pose in a pose search space;
and respectively carrying out consistency check on the at least one candidate pose and the laser point cloud data.
4. A method according to claim 3, wherein the method further comprises:
and adding graph optimization to the established occupied grid subgraph according to the pose which is successfully checked.
5. The method of claim 1, wherein the method further comprises:
acquiring GPS information of a current vehicle;
and clipping the established occupied grid subgraph according to the GPS information so as to reduce the pose searching space.
6. The method of one of claims 1-5, wherein the calculating, layer by layer, a degree of matching of pose of each node representation to the laser point cloud data comprises:
for each low resolution grid, taking as the occupancy grid probability of the low resolution grid the maximum value of the occupancy grid probabilities of at least one high resolution grid that constitutes the low resolution grid;
and calculating the matching degree of the pose represented by each node and the laser point cloud data layer by adopting a branch-and-bound method and a depth optimization search method based on the occupied grid probability of each resolution occupied grid subgraph.
7. An apparatus for closed loop detection of occupancy grid subgraphs, comprising:
an acquisition unit configured to acquire currently scanned laser point cloud data and an established occupancy grid subgraph;
a conversion unit configured to convert the established occupancy grid subgraph into at least one resolution occupancy grid subgraph;
a building unit configured to build a tree structure according to the at least one resolution occupancy grid subgraph, wherein each layer of nodes in the tree structure represents a pose search space at one resolution, the resolution increases downwards from a root node, and each node represents one pose;
the searching unit is configured to calculate the matching degree of the pose represented by each node and the laser point cloud data layer by layer from the root node of the tree structure, and the node with high matching degree preferentially enters the next layer of nodes to continue calculating the matching degree until the leaf nodes are matched to obtain the optimal pose.
8. The apparatus of claim 7, wherein the apparatus further comprises a pose optimization unit configured to:
and splicing the laser point cloud data and the established occupied grid subgraph based on the optimal pose to obtain the optimized pose.
9. The apparatus of claim 8, wherein the apparatus further comprises a verification unit configured to:
acquiring at least one candidate pose adjacent to the optimized pose in a pose search space;
and respectively carrying out consistency check on the at least one candidate pose and the laser point cloud data.
10. The apparatus of claim 9, wherein the apparatus further comprises a graph optimization unit configured to:
and adding graph optimization to the established occupied grid subgraph according to the pose which is successfully checked.
11. The apparatus of claim 7, wherein the apparatus further comprises a clipping unit configured to:
acquiring GPS information of a current vehicle;
and clipping the established occupied grid subgraph according to the GPS information so as to reduce the pose searching space.
12. The apparatus according to one of claims 7-11, wherein the search unit is further configured to:
for each low resolution grid, taking as the occupancy grid probability of the low resolution grid the maximum value of the occupancy grid probabilities of at least one high resolution grid that constitutes the low resolution grid;
and calculating the matching degree of the pose represented by each node and the laser point cloud data layer by adopting a branch-and-bound method and a depth optimization search method based on the occupied grid probability of each resolution occupied grid subgraph.
13. An electronic device for closed loop detection of occupancy of a grid subgraph, comprising:
one or more processors;
a storage device having one or more programs stored thereon,
when executed by the one or more processors, causes the one or more processors to implement the method of any of claims 1-6.
14. A computer readable medium having stored thereon a computer program, wherein the program when executed by a processor implements the method of any of claims 1-6.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010513248.3A CN111680747B (en) | 2020-06-08 | 2020-06-08 | Method and apparatus for closed loop detection of occupancy grid subgraphs |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010513248.3A CN111680747B (en) | 2020-06-08 | 2020-06-08 | Method and apparatus for closed loop detection of occupancy grid subgraphs |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN111680747A CN111680747A (en) | 2020-09-18 |
| CN111680747B true CN111680747B (en) | 2023-09-01 |
Family
ID=72454026
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202010513248.3A Active CN111680747B (en) | 2020-06-08 | 2020-06-08 | Method and apparatus for closed loop detection of occupancy grid subgraphs |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN111680747B (en) |
Families Citing this family (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112700497B (en) * | 2021-01-04 | 2023-06-09 | 歌尔股份有限公司 | Loop detection method, device, equipment and computer readable storage medium |
| CN112883290B (en) * | 2021-01-12 | 2023-06-27 | 中山大学 | Automatic graph cutting method based on branch delimitation method |
| CN113421306A (en) * | 2021-08-20 | 2021-09-21 | 北京智行者科技有限公司 | Positioning initialization method, system and mobile tool |
| CN113763551B (en) * | 2021-09-08 | 2023-10-27 | 北京易航远智科技有限公司 | Rapid repositioning method for large-scale map building scene based on point cloud |
| CN114593737A (en) * | 2022-03-11 | 2022-06-07 | 美智纵横科技有限责任公司 | Control method, control device, robot and storage medium |
| CN115685223B (en) * | 2022-12-15 | 2023-03-21 | 深圳市智绘科技有限公司 | Position identification method and device, electronic equipment and readable storage medium |
Citations (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN102855663A (en) * | 2012-05-04 | 2013-01-02 | 北京建筑工程学院 | Method for building CSG (Constructive Solid Geometry) model according to laser radar grid point cloud |
| CN106921978A (en) * | 2015-12-25 | 2017-07-04 | 中国移动通信集团北京有限公司 | A kind of position distribution determines method and device |
| CN106991143A (en) * | 2017-03-22 | 2017-07-28 | 苏州中科图新网络科技有限公司 | Multi-layer image file, generation method and device, read method and device |
| CN107038749A (en) * | 2016-02-03 | 2017-08-11 | 北京八亿时空信息工程有限公司 | Three-dimensional Multi-resolution modeling method and model building device |
| CN207373179U (en) * | 2017-10-26 | 2018-05-18 | 常熟理工学院 | A kind of robot control system for being used for SLAM and navigation |
| CN110244716A (en) * | 2019-05-28 | 2019-09-17 | 苏州大学 | A method of the robot based on wave front algorithm is explored |
| CN110515382A (en) * | 2019-08-28 | 2019-11-29 | 锐捷网络股份有限公司 | A kind of smart machine and its localization method |
| CN110530368A (en) * | 2019-08-22 | 2019-12-03 | 浙江大华技术股份有限公司 | A kind of robot localization method and apparatus |
| CN110907947A (en) * | 2019-12-04 | 2020-03-24 | 同济人工智能研究院(苏州)有限公司 | Real-time loop detection method in SLAM problem of mobile robot |
| CN110930452A (en) * | 2019-10-23 | 2020-03-27 | 同济大学 | Object pose estimation method based on self-supervision learning and template matching |
-
2020
- 2020-06-08 CN CN202010513248.3A patent/CN111680747B/en active Active
Patent Citations (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN102855663A (en) * | 2012-05-04 | 2013-01-02 | 北京建筑工程学院 | Method for building CSG (Constructive Solid Geometry) model according to laser radar grid point cloud |
| CN106921978A (en) * | 2015-12-25 | 2017-07-04 | 中国移动通信集团北京有限公司 | A kind of position distribution determines method and device |
| CN107038749A (en) * | 2016-02-03 | 2017-08-11 | 北京八亿时空信息工程有限公司 | Three-dimensional Multi-resolution modeling method and model building device |
| CN106991143A (en) * | 2017-03-22 | 2017-07-28 | 苏州中科图新网络科技有限公司 | Multi-layer image file, generation method and device, read method and device |
| CN207373179U (en) * | 2017-10-26 | 2018-05-18 | 常熟理工学院 | A kind of robot control system for being used for SLAM and navigation |
| CN110244716A (en) * | 2019-05-28 | 2019-09-17 | 苏州大学 | A method of the robot based on wave front algorithm is explored |
| CN110530368A (en) * | 2019-08-22 | 2019-12-03 | 浙江大华技术股份有限公司 | A kind of robot localization method and apparatus |
| CN110515382A (en) * | 2019-08-28 | 2019-11-29 | 锐捷网络股份有限公司 | A kind of smart machine and its localization method |
| CN110930452A (en) * | 2019-10-23 | 2020-03-27 | 同济大学 | Object pose estimation method based on self-supervision learning and template matching |
| CN110907947A (en) * | 2019-12-04 | 2020-03-24 | 同济人工智能研究院(苏州)有限公司 | Real-time loop detection method in SLAM problem of mobile robot |
Non-Patent Citations (1)
| Title |
|---|
| 用四叉树算法建立自主系统环境模型;杨晓宇等;《计算机仿真》;20060628(第06期);第105-108页 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN111680747A (en) | 2020-09-18 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN111680747B (en) | Method and apparatus for closed loop detection of occupancy grid subgraphs | |
| CN109214248B (en) | Method and device for identifying laser point cloud data of unmanned vehicle | |
| CN111536964B (en) | Robot positioning method and device, and storage medium | |
| CN107677279B (en) | Method and system for positioning and establishing image | |
| US10380890B2 (en) | Autonomous vehicle localization based on walsh kernel projection technique | |
| US10782410B2 (en) | Method and apparatus for constructing reflectance map | |
| CN109407073B (en) | Reflection value map construction method and device | |
| CN111857160A (en) | Unmanned vehicle path planning method and device | |
| CN108280866B (en) | Road point cloud data processing method and system | |
| CN111461981B (en) | Error estimation method and device for point cloud stitching algorithm | |
| CN111638526B (en) | Method for robot to automatically build graph in strange environment | |
| CN111968229A (en) | High-precision map making method and device | |
| CN110954113A (en) | Vehicle pose correction method and device | |
| KR102212825B1 (en) | Method and system for updating map for pose estimation based on images | |
| KR102118357B1 (en) | System for structuring observation data and platform for mobile mapping or autonomous vehicle | |
| US20230042968A1 (en) | High-definition map creation method and device, and electronic device | |
| CN113325389A (en) | Unmanned vehicle laser radar positioning method, system and storage medium | |
| US20160019248A1 (en) | Methods for processing within-distance queries | |
| CN111461980B (en) | Performance estimation method and device of point cloud stitching algorithm | |
| CN116520302A (en) | Positioning method applied to automatic driving system and method for constructing three-dimensional map | |
| CN114964204A (en) | Map construction method, map using method, map constructing device, map using equipment and storage medium | |
| CN115061499A (en) | Unmanned aerial vehicle control method and unmanned aerial vehicle control device | |
| KR20230029981A (en) | Systems and methods for pose determination | |
| US20200058158A1 (en) | System and method for object location detection from imagery | |
| JP2021135473A (en) | Search support system, search support method |
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 |