WO2022022256A1 - 回环检测方法及系统、可读存储介质、电子设备 - Google Patents

回环检测方法及系统、可读存储介质、电子设备 Download PDF

Info

Publication number
WO2022022256A1
WO2022022256A1 PCT/CN2021/105322 CN2021105322W WO2022022256A1 WO 2022022256 A1 WO2022022256 A1 WO 2022022256A1 CN 2021105322 W CN2021105322 W CN 2021105322W WO 2022022256 A1 WO2022022256 A1 WO 2022022256A1
Authority
WO
WIPO (PCT)
Prior art keywords
node
target
point cloud
loop
target node
Prior art date
Application number
PCT/CN2021/105322
Other languages
English (en)
French (fr)
Inventor
鲁荣荣
杨再甫
李正宁
林宝尉
王彦哲
Original Assignee
湖北亿咖通科技有限公司
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by 湖北亿咖通科技有限公司 filed Critical 湖北亿咖通科技有限公司
Publication of WO2022022256A1 publication Critical patent/WO2022022256A1/zh

Links

Images

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/20—Analysis of motion
    • G06T7/246—Analysis of motion using feature-based methods, e.g. the tracking of corners or segments
    • G06T7/248—Analysis of motion using feature-based methods, e.g. the tracking of corners or segments involving reference images or patches
    • 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
    • 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
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/01—Satellite radio beacon positioning systems transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/13—Receivers
    • G01S19/14—Receivers specially adapted for specific applications
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F16/00—Information retrieval; Database structures therefor; File system structures therefor
    • G06F16/20—Information retrieval; Database structures therefor; File system structures therefor of structured data, e.g. relational data
    • G06F16/22—Indexing; Data structures therefor; Storage structures
    • G06F16/2228—Indexing structures
    • G06F16/2255—Hash tables
    • 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/30—Determination of transform parameters for the alignment of images, i.e. image registration
    • G06T7/33—Determination of transform parameters for the alignment of images, i.e. image registration using feature-based methods
    • G06T7/337—Determination of transform parameters for the alignment of images, i.e. image registration using feature-based methods involving reference images or patches
    • 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
    • G06T7/74—Determining position or orientation of objects or cameras using feature-based methods involving reference images or patches
    • 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
    • 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/30—Subject of image; Context of image processing
    • G06T2207/30241—Trajectory

Definitions

  • the present invention requires the priority of the Chinese patent application with the application number of 202010761223.5 and the title of the invention "loopback detection method and system, readable storage medium, electronic device" submitted to the China Patent Office on July 31, 2020, the entire contents of which are approved by Reference is incorporated herein.
  • the present invention relates to the technical field of automatic driving, and in particular, to a loopback detection method and system, a readable storage medium, and an electronic device.
  • High-precision maps can provide accurate three-dimensional environmental information, which is an indispensable element for L4-level unmanned driving.
  • LiDAR as the main sensor to automatically construct high-precision maps using Simultaneous Localization and Mapping (SLAM) technology is the current mainstream trend.
  • SLAM Simultaneous Localization and Mapping
  • a complete SLAM system can be divided into two parts: the front-end and the back-end.
  • the odometer in the front-end estimates the position and attitude (pose) of the unmanned vehicle in real time through inter-frame matching; Over time, the error gradually accumulates, which will cause the trajectory to drift or even diverge.
  • the back-end mainly optimizes the entire trajectory through loopback detection to ensure the accuracy of the trajectory.
  • the so-called loopback detection means that the unmanned vehicle needs to determine whether the current position has been here before. If it has been, it is also necessary to estimate the relationship between the current pose and the historical pose of the vehicle.
  • the pose can be corrected with the help of pose graph optimization, thereby improving the accuracy and robustness of the entire SLAM system.
  • the mainstream method of loop closure detection is to use the bag-of-words model to perform feature retrieval, and to judge whether the current position constitutes a loop closure by comparing the similarity of the scenes.
  • the bag-of-words model for loop closure detection can achieve higher accuracy, but when the unmanned vehicle travels a long distance and has a large range, the bag-of-words model will saturate with the feature. , resulting in a gradual decline in the retrieval accuracy.
  • the use of bag-of-words model for loopback retrieval requires additional feature extraction operations. For the SLAM system, this will undoubtedly increase the computational cost of the entire SLAM system and affect the operating efficiency of the SLAM system.
  • the present invention provides a lidar-based loop closure detection method and system, a readable storage medium, and an electronic device to overcome the above problems or at least partially solve the above problems.
  • a loop closure detection method comprising:
  • Receive node data of the target node includes: the timestamp of the target node, the GPS position of the target node, the pose information of the target node, and the three-dimensional point cloud of the target node;
  • Hash coding the GPS position to obtain the key value of the target node
  • the time interval with the timestamp of the target node is greater than the set time threshold, and the historical nodes whose geocentric distance is less than the set geocentric distance threshold are selected as candidate loop nodes;
  • a historical node whose trajectory distance from the target node is greater than the set trajectory distance threshold is selected as the target loop node;
  • Obtaining the three-dimensional point cloud of the target loop node is recorded as the target point cloud, and the three-dimensional point cloud of the target node is recorded as the source point cloud;
  • a corresponding relationship is established between the target point cloud and the source point cloud through feature matching, and a rigid body transformation matrix between the target point cloud and the source point cloud is obtained;
  • the target loopback node and the target node form a loopback
  • the trajectory between the target loop node and the target node in the loop is optimized, and the optimized trajectory is used to update the pose state of each node on the loop.
  • the method further includes:
  • the key value is stored in the historical GPS position encoding library.
  • performing hash coding on the GPS position to obtain the key value of the target node including:
  • hash coding is performed to the GPS position of the target node
  • the target node When the target node is not the first node, judge whether the distance between the target node and the last node stored in the GPS location coding library is greater than the set threshold; The location is hash encoded; if not, the target node is ignored.
  • establishing a corresponding relationship between the target point cloud and the source point cloud through feature matching to obtain a rigid body transformation matrix between the target point cloud and the source point cloud, including:
  • the node data of the target node after the node data of the target node is connected, it further includes:
  • a loop closure detection system comprising:
  • a data receiving module configured to receive node data of the target node;
  • the node data includes: the timestamp of the target node, the GPS position of the target node, the pose information of the target node, and the three-dimensional point cloud of the target node;
  • a coding module configured to perform hash coding on the GPS position to obtain the key value of the target node
  • a collection generation module configured to search for at least one historical node with the same key value as the key value in the historical GPS location coding library, and generate a collection of historical nodes
  • the first node screening module is configured to screen out the historical nodes whose time interval with the time stamp of the target node is greater than the set time threshold and whose geocentric distance is less than the set geocentric distance threshold from the set of historical nodes as candidates loopback point;
  • the second node screening module is configured to screen out, in the candidate loop nodes, a historical node whose trajectory distance from the target node is greater than the set trajectory distance threshold as the target loop node;
  • a point cloud acquisition module configured to acquire the three-dimensional point cloud of the target loop node and record it as the target point cloud, and record the three-dimensional point cloud of the target node as the source point cloud;
  • a corresponding relationship establishment module configured to establish a corresponding relationship between the target point cloud and the source point cloud through feature matching, and obtain a rigid body transformation matrix between the target point cloud and the source point cloud;
  • a loopback building module configured to use the rigid body transformation matrix as a loopback constraint to form a loopback with the target loopback node and the target node;
  • the node updating module is configured to optimize the trajectory between the target loop node and the target node in the loop, and use the optimized trajectory to update the pose state of each node on the loop.
  • a computer-readable storage medium wherein the storage medium stores at least one instruction, at least one piece of program, code set or instruction set, the at least one instruction, at least one piece of program,
  • the code set or instruction set is loaded by the processor and executes the loop closure detection method described in any one of the above.
  • an electronic device includes a processor and a memory, the memory stores at least one instruction, at least a piece of program, a code set or an instruction set, the at least one The instructions, the at least one piece of program, the code set or the instruction set are loaded and executed by the processor to implement the loop closure detection method described in any one of the above.
  • a chip for running instructions includes a memory and a processor, the memory stores codes and data, the memory is coupled with the processor, and the processor Running the code in the memory causes the chip to execute to implement the loop closure detection method described in any of the above.
  • a computer program product includes a computer program, the computer program is stored in a computer-readable storage medium, and at least one processor can be stored from the computer-readable storage medium The medium reads the computer program, and when the at least one processor executes the computer program, any one of the loop closure detection methods described above can be implemented.
  • the present invention provides a loopback detection method and system, a readable storage medium, and an electronic device.
  • the vehicle can be separately utilized. While receiving the global position (GPS) information, the near real-time loopback retrieval efficiency is realized; in the process of estimating the loopback error, on the premise of ensuring that the 3D point cloud registration meets the requirements of the overlap rate, the odometer is adjusted according to the appropriate distance, etc. Interval sampling can greatly reduce the temporary storage space of the 3D point cloud and reduce the calculation cost; the use of time plus trajectory length constraints can quickly eliminate false loops formed by long-term parking, and improve the accuracy of trajectory optimization.
  • FIG. 1 is a schematic flowchart of a loop closure detection method according to an embodiment of the present invention
  • FIG. 2 is a schematic diagram of a storage structure of a GPS location coding library according to an embodiment of the present invention
  • FIG. 3 is a schematic diagram of a loopback of an odometer node according to an embodiment of the present invention.
  • FIG. 4 is a schematic diagram of an actual trajectory of a vehicle according to an embodiment of the present invention.
  • FIG. 5 is a schematic diagram before optimization of a vehicle trajectory collected by an odometer according to an embodiment of the present invention
  • FIG. 6 is a schematic diagram of an optimized loop closure detection trajectory according to an embodiment of the present invention.
  • FIG. 7 is a schematic structural diagram of a loopback detection system according to an embodiment of the present invention.
  • the embodiment of the present invention provides a flow of a loop closure detection method, which can be applied to a laser SLAM system.
  • the loopback detection method provided by the embodiment of the present invention may include steps S101 to S109.
  • Step S101 receiving node data of the target node; the node data includes: the timestamp of the target node, the GPS position of the target node, the pose information of the target node, and the three-dimensional point cloud of the target node.
  • the target node refers to the data node output by the odometer that collects data at any time.
  • the node data of each target node contains four fields, namely: the timestamp of the target node, that is, when the odometer collects the data of the target node.
  • the time stamp of the vehicle the GPS position of the vehicle (which may include latitude, longitude and altitude); the position and attitude information of the vehicle to which the odometer belongs, that is, the position and attitude information of the vehicle relative to the initial moment, and the position and attitude information may include the position and attitude of the vehicle change.
  • the three-dimensional point cloud therein can also be written into a hard disk file in binary form.
  • Step S102 hash coding the GPS position to obtain the key value of the target node.
  • the key value can also be stored in the historical GPS position encoding library.
  • the target node can be the first node or other nodes after the first node.
  • hash code the GPS position of the target node when the target node is not the first node, determine whether the distance between the target node and the last node stored in the GPS position coding library is not greater than the set threshold; if so, hash the GPS location of the target node; if not, ignore the target node.
  • the hash coding is to turn the collected raw node data into a key value. Refer to Table 1 to Table 3 for the coding method.
  • the latitude and longitude coordinates (116.3895°, 39.9232°) are taken as an example to introduce how to encode the GPS position into a corresponding character string.
  • the value range of the latitude of the earth is [-90°, 90°].
  • the corresponding value is taken when the latitude falls in the corresponding interval (divided interval 0 or divided interval 1 in Table 1).
  • Table 1 The code corresponding to the latitude value 39.9232 after 15 divisions is displayed: 101110001100011; the value range of the earth's longitude is [-180°, 180°].
  • the longitude value 116.3895 is divided 15 times, and the obtained code is 110100101100010 .
  • each group of five digits is divided into 11100 11101 00100 01111 00000 01101 as follows, and each group is regarded as a binary string and converted into the corresponding decimal number, which are 28, 29, 4, 15, 0, 13.
  • the corresponding strings are obtained as: wx4g0e.
  • a string composed of letters and numbers is a key value.
  • Base32 represents the base32 code corresponding to decimal.
  • Base32 encoding is a scheme for encoding arbitrary byte data using 32 printable characters (letters A-Z and numbers 2-7).
  • Table 3 shows the comparison table of different encoding lengths and distance accuracy of geohash encoding in spatial search, where width and height represent grid width and height, respectively.
  • 1 code length corresponds to 5 bits, the longer the code length, the more digits, the more times the number of divisions, the more accurate the division, and the more accurate the positioning.
  • the coding length is set to 7, which can achieve near real-time retrieval efficiency.
  • the node After encoding the GPS location, the node is stored in the GPS location encoding library according to its corresponding key value.
  • the GPS location encoding library is managed in a single-key multi-value manner, as shown in Figure 2, key_1, key_2...key_m respectively represent The 1st, 2nd...mth key value key in the coding library, for each key value key, multiple node data can be stored, for example, key_1 can simultaneously store the corresponding data of node n i , node n j and node n k
  • the data may specifically include a timestamp (Timestamp), a GPS location (GPS location), a pose (Pose), and a point cloud file location (Point cloud file location).
  • the 3D point cloud corresponding to the target node is written to the hard disk file in binary form (the hard disk file is stored in the hard disk that collects the point cloud data at each moment in real time), and its file path is recorded.
  • the GPS location encoding library uses a single-key multi-value data structure to store, and nodes with adjacent geographic locations get the same key value after GPS location encoding. What each node in the GPS location coding library stores is no longer the node data itself, but the path of the node data, which can reduce the storage pressure of the running memory, so that the laser SLAM system is not affected by the running memory.
  • the target node is not the first node, such as the node data n k corresponding to the kth node, judge whether the distance between this node and the last node stored in the GPS location coding library is greater than the given threshold s d , if If it is greater than that, encode its GPS position, save the corresponding three-dimensional point cloud, and continue to perform step S103; otherwise, ignore the node message and wait for a new node message.
  • n i and node n j Take node n i and node n j as examples, where n j is the node at the jth time, n i is the node at the i time, and the i time is the previous time at the j time, that is, the node n i is the node of the node n j .
  • the distance between them is calculated as follows:
  • T i , T j are the six-degree-of-freedom pose matrices corresponding to node n i and node n j respectively, and T i (1:3,4) means to take the first row to the fourth column in the fourth column of the matrix T i
  • T j (1:3,4) represents a vector consisting of elements from the first row to the third row in the fourth column of the matrix T j ;
  • SE(3) represents a special Euclidean group.
  • the threshold s d is the sampling interval of the node, which can control the frequency of loop closure detection and reduce the storage space of the 3D point cloud.
  • Step S103 searching for at least one historical node with the same key value as the key value of the target node in the historical GPS position coding library, and generating a historical node set.
  • step S101 For the target node received through step S101 is node n k , retrieve all historical nodes with the same key value from the GPS position coding library according to its key value (because the GPS position of the node message is the same or the position is similar, after the hash algorithm The key value obtained later is the same) set H.
  • Step S104 the historical nodes whose time interval with the time stamp of the target node is greater than the set time threshold and whose geocentric distance is less than the set geocentric distance threshold are selected from the historical node set as candidate loop nodes.
  • lon_i, lat_i, height_i are the longitude, latitude and height corresponding to node n i respectively
  • lon_j, lat_j, height_j are the longitude, latitude and height corresponding to node n j respectively
  • R mh , R nh are the main curvature of the meridian circle of the earth, respectively Radius and principal radius of curvature of the unitary circle.
  • the time interval between nodes is the absolute value of the difference between the two timestamps
  • the threshold ⁇ t is to prevent the historical nodes retrieved by n k from being nodes that have passed through in a short period of time
  • the threshold ⁇ d is to ensure the 3D point cloud of the target node.
  • the 3D point cloud corresponding to the historical node has an appropriate overlapping area, so as to ensure that the subsequent loop closure pose estimation can be successful.
  • step S105 a historical node whose trajectory distance from the target node is greater than the set trajectory distance threshold is selected from the candidate loopback nodes as the target loopback node.
  • the candidate loopback points retrieved through step S104 satisfy both the time interval constraint and the distance constraint, thereby forming a pseudo loopback. To this end, it is necessary to filter the candidate loopback points again.
  • the method is as follows: Calculate the trajectory distance between the historical node and the target node. If the trajectory distance is greater than the given threshold ⁇ s , record the historical node as the target loopback node, Enter the geometry verification stage; otherwise, go back to step S101 and wait for a new node message. Taking the historical node n h as an example, the calculation method of the trajectory distance to the target node n k is as follows:
  • step S106 the acquired three-dimensional point cloud of the target loop node is recorded as the target point cloud, and the three-dimensional point cloud of the target node is recorded as the source point cloud.
  • the corresponding 3D point cloud is read according to the stored 3D point cloud path, which is recorded as the target point cloud C 2 , and the 3D point cloud of the target node is recorded as the source point cloud C 1 .
  • Step S107 establish a correspondence between the target point cloud and the source point cloud through feature matching, and obtain a rigid body transformation matrix between the target point cloud and the source point cloud.
  • it can include:
  • S2 perform local features on the target point cloud and the source point cloud based on several key points extracted from the target point cloud and the source point cloud respectively, and match each target point of the target point cloud and each source point of the source point cloud based on the local features.
  • the corresponding relationship between the two points is obtained, and multiple matching pairs are obtained; wherein, the corresponding relationship refers to the corresponding relationship between the points with the same name established in the same scene scanned from different perspectives;
  • the key point extraction method can be Harris3D, ISS3D or other key point extraction methods.
  • Local feature description can use SHOT, FPFH and other description methods.
  • the point correspondence between C 1 and C 2 is established by feature matching.
  • the correspondence here refers to the correspondence between points with the same name established in the same scene scanned from different perspectives.
  • the point with the same name means that points collected from different perspectives are in the physical world. represents the same point.
  • the main purpose is to establish the correspondence between points with the same name in the same scene from different perspectives, which may cause mismatches.
  • the geometric consistency algorithm is used. Eliminating the mismatch in the point correspondence between C1 and C2 can improve the accuracy between the point correspondences between C1 and C2 .
  • the pose transformation of C 1 and C 2 is estimated by using the matching pair after eliminating false matches, and finally, the iterative nearest neighbor algorithm (Iterative Closest Point, ICP) or G-ICP algorithm is used for precise registration, and C 1 is obtained. and the rigid body transformation matrix T loop between C 2 and C 2 , and input it to step S108 as a loop closure constraint; otherwise, go back to step S101 to wait for a new node message.
  • ICP Intelligent Closest Point
  • Step S108 using the rigid body transformation matrix as the loopback constraint, the target loopback node and the target node form a loopback.
  • Step S109 optimize the trajectory between the target loop node and the target node in the loop, and use the optimized trajectory to update the pose state of each node on the loop.
  • the target loop node n h and the target node n k form a loop, and the trajectory between the node n h and the node n k is optimized by means of pose graph optimization.
  • the target node n k detects the target loop node n h through steps S101 to S107 to form a loop, and the loop pose relationship T loop between the two is estimated through local feature matching.
  • the following describes how to use the pose graph to optimize and correct the poses of all nodes between node n h and node n k .
  • ⁇ j are Lie algebras corresponding to ⁇ T ij , T i and T j respectively; is a 6 ⁇ 6 information matrix, usually set as an identity matrix.
  • the Gauss-Newton algorithm or the LM algorithm can be used to optimize the above cost function, because these two algorithms need to use the Jacobian matrix of the variable to be optimized.
  • the specific derivation is as follows:
  • the above derivation is the Jacobian matrix of the derivation of the variable to be optimized by the cost function constructed by the error between any two points in the loop.
  • the small loop has been optimized by the pose graph.
  • the commonly used strategy is to fix those nodes that have been optimized in the small loop. Only optimize nodes that have not yet been optimized.
  • fixing the small loop nodes may cause the optimization algorithm to fail to converge. Therefore, the strategy adopted in the embodiment of the present invention is not to fix those nodes that have been optimized, but to add a self-loop to them.
  • Edge constraints that is to rewrite equation (4) as:
  • the open-source Ceres library and the g2o graph optimization library can be used for loopback pose graph optimization.
  • step S101 Use the optimized trajectory to update the pose states of all nodes in the loop. Go to step S101, wait for a new node message; if there is no new message for a long time, save the final track and exit the loopback detection program.
  • the global position (GPS) information received by the vehicle can be fully utilized, and a near real-time loopback retrieval efficiency can be realized by hash coding the GPS position;
  • the temporary storage space of the 3D point cloud can be greatly reduced and the calculation cost can be greatly reduced by sampling the odometer according to the appropriate distance and equal interval; the constraint of time plus trajectory length can be quickly eliminated.
  • pose graph optimization self-loop edge constraints are used to replace fixed node constraints, which can improve the convergence of long-distance loop closure optimization. Especially in long-distance operation, it can still output accurate and reliable trajectory accuracy, thereby ensuring the automatic construction of high-precision maps.
  • FIG. 4 is a schematic diagram of the actual trajectory of the vehicle
  • FIG. 5 is a schematic diagram of the vehicle trajectory collected by the odometer before optimization
  • FIG. 6 is a schematic diagram of the vehicle trajectory after optimization by the loopback detection scheme according to the embodiment of the present invention.
  • an embodiment of the present invention also provides a loop closure detection system, as shown in FIG. 7 , the system may include:
  • the data receiving module 710 is configured to receive node data of the target node; the node data includes: the timestamp of the target node, the GPS position of the target node, the pose information of the target node, and the three-dimensional point cloud of the target node;
  • the encoding module 720 is configured to perform hash encoding on the GPS position to obtain the key value of the target node;
  • a set generating module 730 configured to search for at least one historical node with the same key value as the key value in the historical GPS location coding library, and generate a set of historical nodes;
  • the first node screening module 740 is configured to screen out the historical nodes whose time interval with the time stamp of the target node is greater than the set time threshold and whose geocentric distance is less than the set geocentric distance threshold from the set of historical nodes as candidate loopback nodes ;
  • the second node screening module 750 is configured to screen out, among the candidate loop nodes, the historical nodes whose trajectory distance from the target node is greater than the set trajectory distance threshold as the target loop node;
  • the point cloud acquisition module 760 is configured to acquire the 3D point cloud of the target loop node and record it as the target point cloud, and record the 3D point cloud of the target node as the source point cloud;
  • a corresponding relationship establishing module 770 configured to establish a corresponding relationship between the target point cloud and the source point cloud through feature matching, and obtain a rigid body transformation matrix between the target point cloud and the source point cloud;
  • the loopback building module 780 is configured to use the rigid body transformation matrix as a loopback constraint to form a loopback with the target loopback node and the target node;
  • the node updating module 790 is configured to optimize the trajectory between the target loop node and the target node in the loop, and use the optimized trajectory to update the pose state of each node on the loop.
  • the encoding module 720 is further configured to, after hash encoding the GPS location to obtain the key value of the target node, store the key value in the historical GPS location encoding library.
  • the encoding module 720 may be further configured to, when the target node is the first node, perform hash encoding on the GPS position of the target node;
  • the target node When the target node is not the first node, judge whether the distance between the target node and the last node stored in the GPS location coding library is greater than the set threshold; if so, hash the GPS position of the target node; if not , the target node is ignored.
  • the correspondence establishing module 770 may be further configured to extract several key points from the target point cloud and the source point cloud respectively;
  • the local feature description of the target point cloud and the source point cloud is performed, and the relationship between each target point of the target point cloud and each source point of the source point cloud is established based on the local feature description.
  • the corresponding relationship between the two points is obtained, and multiple matching pairs are obtained; wherein, the corresponding relationship refers to the corresponding relationship between the points with the same name established in the same scene scanned from different perspectives;
  • the data receiving module 710 may also be configured to write the three-dimensional point cloud into a hard disk file in binary form.
  • a computer-readable storage medium in which the storage medium stores at least one instruction, at least one piece of program, code set or instruction set, at least one instruction, at least one piece of program, code set Or the instruction set is loaded by the processor and executes the lidar-based loop closure detection method described in any of the foregoing embodiments.
  • an electronic device including a processor and a memory, and the memory stores at least one instruction, at least one program, code set or instruction set, at least one instruction, at least one program, The code set or instruction set is loaded and executed by the processor to implement the lidar-based loop closure detection method described in any of the above embodiments.
  • a chip for running instructions includes a memory and a processor, the memory stores codes and data, the memory is coupled to the processor, and the memory is coupled to the processor.
  • the processor runs the code in the memory to cause the chip to execute to implement the lidar-based loop closure detection method described in any of the above embodiments.
  • a computer program product includes a computer program, the computer program is stored in a computer-readable storage medium, and at least one processor can be obtained from the computer.
  • the computer program is read by reading the storage medium, and when the at least one processor executes the computer program, the lidar-based loop closure detection method described in any of the foregoing embodiments can be implemented.
  • the above method is implemented in the form of software and sold or used as an independent product, it can be stored in a computer-readable storage medium.
  • the technical solution of the present invention or all or part of the technical solution can be embodied in the form of a software product, and the computer software product is stored in a storage medium, which includes several instructions to make a computer
  • a computing device such as a personal computer, a server, or a network device, etc.
  • the aforementioned storage medium includes: U disk, removable hard disk, read only memory (ROM), random access memory (RAM), magnetic disk or optical disk and other media that can store program codes.
  • all or part of the steps of implementing the foregoing method embodiments may be accomplished by program instructions related to hardware (such as a personal computer, a server, or a computing device such as a network device), and the program instructions may be stored in a computer-readable storage
  • the program instructions when executed by the processor of the computing device, the computing device executes all or part of the steps of the methods described in the embodiments of the present invention.

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • General Physics & Mathematics (AREA)
  • Remote Sensing (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Databases & Information Systems (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Data Mining & Analysis (AREA)
  • General Engineering & Computer Science (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Electromagnetism (AREA)
  • Multimedia (AREA)
  • Software Systems (AREA)
  • Position Fixing By Use Of Radio Waves (AREA)
  • Navigation (AREA)

Abstract

一种回环检测方法及系统、可读存储介质、电子设备，该方法通过接收节点数据，并基于节点数据中的GPS位置进行哈希编码得到键值，在历史GPS位置编码库中查找具有相同键值的包括至少一个历史节点的历史节点集合，进而从历史节点集合中进行筛选目标回环节点，并基于目标节点和目标回环节点构建回环。该方法采用时间加轨迹长度的约束可快速剔除由于长时间停车形成的伪回环，改善轨迹优化的精度。

Description

回环检测方法及系统、可读存储介质、电子设备
本发明要求于2020年07月31日提交中国专利局、申请号为202010761223.5、发明名称为“回环检测方法及系统、可读存储介质、电子设备”的中国专利申请的优先权，其全部内容通过引用结合在本发明中。
技术领域
本发明涉及自动驾驶技术领域，尤其涉及一种回环检测方法及系统、可读存储介质、电子设备。
背景技术
高精度地图可提供精准三维环境信息，是实现L4级无人驾驶不可或缺的要素。以激光雷达作为主要传感器，利用即时定位与建图(Simultaneous Localization and Mapping,SLAM)技术自动构建高精度地图是当前主流的趋势。
一个完整的SLAM系统可分为前端和后端两个部分，其中，前端的里程计通过帧间匹配实时估计无人车的位置和姿态(位姿)；由于帧间匹配存在误差，随着时间推移，误差逐渐累积，会造成轨迹漂移甚至发散，后端主要通过回环检测对整个轨迹进行优化，从而保证轨迹的精度。所谓回环检测是指无人车需要判断当前位置之前是否来过，如果来过，还需要估计车辆当前位姿与历史位姿之间关联。
利用回环间的位姿约束，借助位姿图优化便可以修正位姿，从而提升整个SLAM系统的精度和鲁棒性。
目前回环检测的主流方法是借助词袋模型进行特征检索，通过对比场景的相似性，判断当前位置是否构成回环。当无人车在小范围内运动时，利用词袋模型进行回环检测可以取得较高的准确率，但是当无人车行驶距离较远、范围较大时，词袋模型会随着特征的饱和，导致检索的准确率逐渐下降。此外，利用词袋模型进行回环检索需要额外进行特征提取操作，对于SLAM系统而言，这无疑会增加整个SLAM系统的计算代价，影响SLAM系统的运行 效率。
发明内容
本发明提供了一种基于激光雷达的回环检测方法及系统、可读存储介质、电子设备以克服上述问题或者至少部分地解决上述问题。
根据本发明的一个方面，提供了一种回环检测方法，包括：
接收目标节点的节点数据；所述节点数据包括：目标节点的时间戳、目标节点的GPS位置、目标节点的位姿信息以及目标节点的三维点云；
对所述GPS位置进行哈希编码得到所述目标节点的键值；
在历史GPS位置编码库中查找与所述目标节点的键值具有相同键值的至少一个历史节点，生成历史节点集合；
在所述历史节点集合中筛选出与所述目标节点的时间戳的时间间隔大于设定时间阈值、且地心距离小于设定地心距离阈值的历史节点作为候选回环节点；
在所述候选回环节点中筛选出与所述目标节点的轨迹距离大于设定轨迹距离阈值的历史节点作为目标回环节点；
获取所述目标回环节点的三维点云记为目标点云，将所述目标节点的三维点云记为源点云；
通过特征匹配在所述目标点云和源点云之间建立对应关系，得到所述目标点云和源点云之间的刚体变换矩阵；
以所述刚体变换矩阵作为回环约束，将所述目标回环节点和所述目标节点构成回环；
对所述回环中所述目标回环节点和目标节点之间的轨迹进行优化，利用优化后的轨迹更新所述回环上各节点的位姿状态。
可选地，所述对所述GPS位置进行哈希编码得到所述目标节点的键值之后，还包括：
将所述键值存入所述历史GPS位置编码库。
可选地，所述对所述GPS位置进行哈希编码得到所述目标节点的键值，包括：
当所述目标节点为第一个节点时，对所述目标节点的GPS位置进行哈 希编码；
当所述目标节点不是第一个节点时，判断所述目标节点与上一个存入所述GPS位置编码库中的节点间的距离是否大于设定阈值；若是，则对所述目标节点的GPS位置进行哈希编码；若否，则忽略所述目标节点。
可选地，所述通过特征匹配在所述目标点云和源点云之间建立对应关系，得到所述目标点云和源点云之间的刚体变换矩阵，包括：
分别从所述目标点云和源点云中提取若干关键点；
分别基于所述目标点云和源点云中提取的若干关键点对所述目标点云和源点云进行局部特征描述，并基于所述局部特征描述建立所述目标点云的各目标点和源点云的各源点之间对应关系，得到多个匹配对；其中，所述对应关系指不同视角扫描同一个场景中建立的同名点对应关系；
利用几何一致性算法在多个所述匹配对中筛选出误匹配对并进行删除，得到剩余匹配对；
判断所述剩余匹配对中的匹配对不小于设定数量时，利用所述剩余匹配对包含的匹配对估计所述目标点云和源点云之间的位姿变换以进行精准匹配后得到所述目标点云和源点云之间的刚体变换矩阵。
可选地，所述接目标节点的节点数据之后，还包括：
将所述三维点云以二进制的形式写入硬盘文件中。
根据本发明的另一个方面，还提供了一种回环检测系统，包括：
数据接收模块，配置为接收目标节点的节点数据；所述节点数据包括：目标节点的时间戳、目标节点的GPS位置、目标节点的位姿信息以及目标节点的三维点云；
编码模块，配置为对所述GPS位置进行哈希编码得到所述目标节点的键值；
集合生成模块，配置为在历史GPS位置编码库中查找与所述键值具有相同键值的至少一个历史节点，生成历史节点集合；
第一节点筛选模块，配置为在所述历史节点集合中筛选出与所述目标节点的时间戳的时间间隔大于设定时间阈值、且地心距离小于设定地心距离阈值的历史节点作为候选回环节点；
第二节点筛选模块，配置为在所述候选回环节点中筛选出与所述目标 节点的轨迹距离大于设定轨迹距离阈值的历史节点作为目标回环节点；
点云获取模块，配置为获取所述目标回环节点的三维点云记为目标点云，将所述目标节点的三维点云记为源点云；
对应关系建立模块，配置为通过特征匹配在所述目标点云和源点云之间建立对应关系，得到所述目标点云和源点云之间的刚体变换矩阵；
回环构建模块，配置为以所述刚体变换矩阵作为回环约束，将所述目标回环节点和所述目标节点构成回环；
节点更新模块，配置为对所述回环中所述目标回环节点和目标节点之间的轨迹进行优化，利用优化后的轨迹更新所述回环上各节点的位姿状态。
根据本发明的又一个方面，还提供了一种计算机可读存储介质，所述存储介质中存储有至少一条指令、至少一段程序、代码集或指令集，所述至少一条指令、至少一段程序、代码集或指令集由处理器加载并执行上述任意一项所述的回环检测方法。
根据本发明的又一个方面，还提供了一种电子设备，所述电子设备包括处理器和存储器，所述存储器中存储有至少一条指令、至少一段程序、代码集或指令集，所述至少一条指令、所述至少一段程序、所述代码集或指令集由所述处理器加载并执行以实现上述任意一项所述的回环检测方法。
根据本发明的又一个方面，还提供了一种运行指令的芯片，所述芯片包括存储器、处理器，所述存储器中存储代码和数据，所述存储器与所述处理器耦合，所述处理器运行所述存储器中的代码使得所述芯片用于执行以实现上述任意一项所述的回环检测方法。
根据本发明的又一个方面，还提供了一种计算机程序产品，所述程序产品包括计算机程序，所述计算机程序存储在计算机可读存储介质中，至少一个处理器可以从所述计算机可读存储介质读取所述计算机程序，所述至少一个处理器执行所述计算机程序时可实现上述任意一项所述的回环检测方法。
本发明提供了一种回环检测方法及系统、可读存储介质、电子设备，在本发明提供的方法中，基于本发明提供的回环检测方法，通过对GPS 位置进行哈希编码，可以分利用车辆接收的全球位置(GPS)信息的同时，实现近乎实时的回环检索效率；估计回环误差的过程中，在保证三维点云配准满足重叠率要求的前提下，通过对里程计按照适当的距离等间隔采样，可大幅缩减三维点云的临时存储空间并降低计算代价；采用时间加轨迹长度的约束可快速剔除由于长时间停车形成的伪回环，改善轨迹优化的精度。
附图说明
图1是根据本发明实施例的回环检测方法的流程示意图；
图2是根据本发明实施例的GPS位置编码库存储结构示意图；
图3是根据本发明实施例的里程计节点回环示意图；
图4是根据本发明实施例的车辆实际轨迹示意图；
图5是根据本发明实施例的里程计采集到的车辆轨迹优化前的示意图；
图6是根据本发明实施例的回环检测轨迹优化后的示意图；
图7是根据本发明实施例的回环检测系统结构示意图。
具体实施方式
下面将参照附图更详细地描述本发明的示例性实施例。虽然附图中显示了本发明的示例性实施例，然而应当理解，可以以各种形式实现本发明而不应被这里阐述的实施例所限制。相反，提供这些实施例是为了能够更透彻地理解本发明，并且能够将本发明的范围完整的传达给本领域的技术人员。
本发明实施例提供了一种回环检测方法的流程，可应用于激光SLAM系统。参见图1可知，本发明实施例提供的回环检测方法可以包括步骤S101～S109。
步骤S101，接收目标节点的节点数据；节点数据包括：目标节点的时间戳、目标节点的GPS位置、目标节点的位姿信息以及目标节点的三维点云。
目标节点，是指里程计输出的对应任一时刻采集数据的数据节点，每个目标节点的节点数据共包含四个字段，分别是：目标节点的时间戳，即里程计采集目标节点的数据时的时间戳；车辆的GPS位置(可以包括纬度值，经度值和高度值)；里程计所属车辆的位姿信息，即车辆相对于初始时刻的位姿信息，位姿信息可以包括车辆变化的位姿，包括位置和姿态；激光雷达在 同一时刻采集的三维点云数据。实际应用中，可选地，对于所接收到的节点数据，还可以将其中的三维点云以二进制的形式写入硬盘文件中。
步骤S102，对GPS位置进行哈希编码得到目标节点的键值。并且，还可以将键值存入历史GPS位置编码库。
实际应用中，目标节点可以是第一个节点，也可以是第一个节点之后的其他节点。当目标节点为第一个节点时，对目标节点的GPS位置进行哈希编码；当目标节点不是第一个节点时，判断目标节点与上一个存入GPS位置编码库中的节点间的距离是否大于设定阈值；若是，则对目标节点的GPS位置进行哈希编码；若否，则忽略目标节点。
也就是说，对接收第一个节点的节点数据n 1，首先对其GPS位置进行哈希编码，哈希编码是将采集的原始节点数据变成键值，编码方式参考表1～表3。
表1维度二分示意表
Figure PCTCN2021105322-appb-000001
Figure PCTCN2021105322-appb-000002
表2数值与字母映射表
Decimal 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15
Base 32 0 1 2 3 4 5 6 7 8 9 b c d e f g
Decimal 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31
Base 32 h j k m n p q r s t u v w x y z
表3编码长度与距离精度对照表
哈希长度 宽度 高度
1 5,009.4km 4.992.6km
2 1,252.3km 624.1km
3 156.5km 156km
4 39.1km 19.5km
5 4.9km 4.9km
6 1.2km 609.4m
7 152.9m 152.4m
8 38.2m 19m
9 4.8m 4.8m
10 1.2m 59.5cm
11 14.9cm 14.9cm
12 3.7cm 1.9cm
如表1～表3所示，以经纬度坐标(116.3895°，39.9232°)为例介绍如何将GPS位置编码为相应的字符串。地球纬度的取值范围为[-90°,90°]，通过对该区间不断二分，纬度落在相应的区间则取对应的值(表1中的划分区间0或划分区间1)，表1显示了纬度值39.9232经过15次二分后对应的编码情况：101110001100011；地球经度的取值范围为[-180°，180°]，同理，对经度值116.3895进行15次二分，得到的编码是110100101100010。然后将二者交替合并到一起，奇数位放置经度值，偶数位放置纬度值，得到30位的编码：111001110100100011110000001101。
然后每五位一组，划分如下11100 11101 00100 01111 00000 01101，将每个组看成二进制串，并转化为相应的十进制数，分别是28、29、4、15、0、 13。根据表2的数值字符映射表，得到它们对应的字符串为：wx4g0e。这样由字母和数字组成的字符串即键值，基于上述操作，可以将地球上任意位置的经纬度坐标转换为相应的字符串，且字符串的前缀匹配越多，表示两个位置越接近，字符串长度越长，表示位置越精确。其中，表2中Decimal表示十进制对应的数值，Base32表示十进制对应的base32编码。Base32编码是使用32个可打印字符(字母A-Z和数字2-7)对任意字节数据进行编码的方案。
表3显示了空间搜索中geohash编码的不同编码长度与距离精度对照表，其中，宽度和高度分别表示网格宽和高。1个编码长度对应5位，编码长度越长，位数越多，二分的次数越多，划分越精确，定位越准。在本发明中，将编码长度设置为7，可以实现近乎实时的检索效率。
对GPS位置进行编码之后，将该节点根据其对应的键值存入GPS位置编码库中，GPS位置编码库采用单键多值的方式管理，如图2所示，key_1、key_2…key_m分别表示编码库中第1个、第2个…第m个键值key，对于每个键值key可以存储多个节点数据，如key_1可同时对应存储节点n i、节点n j以及节点n k对应的数据，具体可以包括时间戳(Timestamp)、GPS位置(GPS location)、位姿(Pose)、点云文件路径(Point cloud file location)。
此外，将目标节点应的三维点云以二进制的形式写到硬盘文件(硬盘文件存储在实时采集每个时刻的点云数据的硬盘)中，并记录其文件路径。
GPS位置编码库采用单键多值的数据结构存储，具有邻近地理位置的节点经过GPS位置编码后得到相同的键值。GPS位置编码库中每个节点存储的不再是节点数据本身，而是节点数据的路径，从而可以减轻运行内存的存储压力，使得激光SLAM系统不受运行内存的影响。
假设目标节点为非第一个节点，如第k个节点对应的节点数据n k,判断该节点与上一个存入GPS位置编码库中的节点间的距离是否大于给定的阈值s d，如果大于，则对其GPS位置进行编码，并保存相应的三维点云，并继续执行步骤S103；否则，忽略该节点消息，等待新的节点消息。
以节点n i和节点n j为例，其中，n j为第j时刻节点，n i为第i时刻的节点，其中i时刻为j时刻的前一时刻，即节点n i为节点n j的上一个节点，它们间的距离计算方式如下：
d(n i,n j)＝||T i(1:3,4)-T j(1:3,4)|| 2,T i,T j∈SE(3)      (1)
其中，T i,T j分别是节点n i和节点n j对应的六自由度位姿的矩阵，T i(1:3,4)表示取矩阵T i的第四列中第一行至第三行元素组成的向量；T j(1:3,4)表示取矩阵T j的第四列中第一行至第三行元素组成的向量；SE(3)表示特殊欧式群。阈值s d是节点的采样间隔，可以控制回环检测的频率，并减少三维点云的存储空间。
步骤S103，在历史GPS位置编码库中查找与目标节点的键值具有相同键值的至少一个历史节点，生成历史节点集合。
对于通过步骤S101接收的目标节点为节点n k,根据其键值从GPS位置编码库中检索所有与之具有相同键值的历史节点(由于节点消息的GPS位置相同或者位置相近，经过哈希算法后得到的键值相同)集合H。
步骤S104，在历史节点集合中筛选出与目标节点的时间戳的时间间隔大于设定时间阈值、且地心距离小于设定地心距离阈值的历史节点作为候选回环节点。
从历史节点集合H中挑选出与目标节点的时间戳的时间间隔大于给定阈值δ t且距离最近的历史节点，由于SLAM系统存在的轨迹漂移，利用(1)式估计目标节点与历史节点间的距离不再可靠，所以需要计算二者在地心坐标系下的距离，计算方式如下：
Figure PCTCN2021105322-appb-000003
dx＝(lon_j-lon_i)R nhcos(lat_i),         (2)
dy＝(lat_j-lat_i)R mh,
dz＝height_j-height_i,
其中lon_i,lat_i,height_i分别是节点n i对应的经度、纬度以及高度，lon_j,lat_j,height_j分别是节点n j对应的经度、纬度以及高度；R mh，R nh分别是地球的子午圈主曲率半径以及卯酉圈主曲率半径。
如果历史节点与目标节点n k的地心距离小于δ d，则确定该历史节点为候选回环节点，并转步骤S105；否则，转步骤S101。其中，节点间的时间间隔就是二者时间戳的差的绝对值，阈值δ t是为了防止n k检索到的历史节点就是短期内经过的节点，阈值δ d是为了保证目标节点的三维点云与历史节点对应的三维点云具有适当的重叠区域，从而保证后续的回环位姿估计能够成功。
步骤S105，在候选回环节点中筛选出与目标节点的轨迹距离大于设定轨 迹距离阈值的历史节点作为目标回环节点。
当车辆在行进的过程中遇到长时间停车时，通过步骤S104检索到的候选回环点既满足时间间隔约束，也满足距离约束，从而形成伪回环。为此，需要对候选回环点进行再次过滤，做法如下：计算该历史节点与目标节点间的轨迹距离，如果轨迹距离大于给定的阈值δ s，则将该历史节点记为目标回环节点，进入几何验证阶段；否则，回到步骤S101，等待新的节点消息。以历史节点n h为例，它到目标节点n k的轨迹距离的计算方式如下：
Figure PCTCN2021105322-appb-000004
步骤S106，获取目标回环节点的三维点云记为目标点云，将目标节点的三维点云记为源点云。
对通过步骤S105的目标回环节点，根据其保存的三维点云路径读取相应的三维点云，记为目标点云C 2，目标节点的三维点云记为源点云C 1。
步骤S107，通过特征匹配在目标点云和源点云之间建立对应关系，得到目标点云和源点云之间的刚体变换矩阵。具体可以包括：
S1，分别从目标点云和源点云中提取若干关键点；
S2，分别基于目标点云和源点云中提取的若干关键点对目标点云和源点云进行局部特征，并基于局部特征匹配目标点云的各目标点和源点云的各源点之间对应关系，得到多个匹配对；其中，对应关系指不同视角扫描同一个场景中建立的同名点对应关系；
S3，利用几何一致性算法在多个匹配对中筛选出误匹配对并进行删除，得到剩余匹配对；
S4，判断剩余匹配对中的匹配对不小于设定数量时，利用剩余匹配对包含的匹配对估计目标点云和源点云之间的位姿变换以进行精准匹配后得到目标点云和源点云之间的刚体变换矩阵。
举例来讲，计算目标点云和源点云之间的刚体变换矩阵时，需要先分别从C 1，C 2中提取若干关键点，并对其进行局部特征描述。关键点提取方法可以采用Harris3D、ISS3D或是其他关键点提取法。局部特征描述可以采用SHOT、FPFH等描述方法。
然后，通过特征匹配方式建立C 1和C 2之间的点对应关系，这里的对应关系指不同视角扫描同一个场景中建立的同名点对应关系，同名点是说不同视 角采集的点在物理世界中代表同一个点。接着利用几何一致性算法进行误匹配剔除。实际应用中，在C 1和C 2之间建立点对应关系时，主要是建立不同视角下同一个场景中同名点间的对应关系，可能会产生误匹配，此时，通过采用几何一致性算法将C 1和C 2之间的点对应关系中的误匹配剔除，可以提升C 1和C 2之间的点对应关系之间的准确性。
如果剔除误匹配后的匹配对的个数不小于N m，则表示通过几何验证。为了估计回环误差，利用剔除误匹配后的匹配对估计C 1和C 2的位姿变换，最后利用迭代最近邻算法(Iterative Closest Point,ICP)或者G-ICP算法进行精配准，得到C 1和C 2之间的刚体变换矩阵T loop，并将其作为回环约束输入到步骤S108；否则，回到步骤S101等待新的节点消息。
步骤S108，以刚体变换矩阵作为回环约束，将目标回环节点和目标节点构成回环。
步骤S109，对回环中目标回环节点和目标节点之间的轨迹进行优化，利用优化后的轨迹更新回环上各节点的位姿状态。
将目标回环节点n h与目标节点n k构成回环，采用位姿图优化的方式对节点n h与节点n k之间的轨迹进行优化。
如图3所示，假设目标节点n k通过步骤S101～S107检测到目标回环节点n h与之构成回环，且通过局部特征匹配估计出二者之间的回环位姿关系T loop。下面介绍如何利用位姿图优化修正节点n h与节点n k之间所有节点的位姿。
假设节点n i与n j是回环路径中的两个节点，则二者间的相对位姿
Figure PCTCN2021105322-appb-000005
构造如下代价函数：
Figure PCTCN2021105322-appb-000006
其中，
Figure PCTCN2021105322-appb-000007
ξ j分别是ΔT ij，T i和T j对应的李代数；
Figure PCTCN2021105322-appb-000008
是6×6信息矩阵，通常设为单位阵。可以采用高斯牛顿算法或者LM算法对上述代价函数进行优化，由于这两种算法需要用到待优化变量的雅各比矩阵。下面具体推导如下：
对误差项e ij施加左乘扰动，有
Figure PCTCN2021105322-appb-000009
利用伴随性质，exp(ξ^)T＝Texp((Ad(T -1)ξ)^)，可将(4)式改写为
Figure PCTCN2021105322-appb-000010
其中，
Figure PCTCN2021105322-appb-000011
Figure PCTCN2021105322-appb-000012
Figure PCTCN2021105322-appb-000013
上述推导的是由回环中任意两点间的误差构建的代价函数对待优化变量求导的雅各比矩阵。在实际应用中，存在大回环套小环的情形，即小环已经经过位姿图优化过了，在优化大环轨迹时，常用的策略是固定住那些在小环中已经优化过的节点，只优化还未被优化的节点。但是当大环中包含的节点数量较多时，固定小环节点可能会使优化算法无法收敛，所以本发明实施例采取的策略是不固定那些已经优化过的节点，而是给它们添加一个自环边约束，即将(4)式改写为：
Figure PCTCN2021105322-appb-000014
其中
Figure PCTCN2021105322-appb-000015
Ω是那些已经优化过的节点集合。自环边误差项对待优化变量求导的雅各比矩阵推导方式与上面基本类似，这里直接给出：
Figure PCTCN2021105322-appb-000016
通过添加自环边约束，可以提升算法对大环优化时的收敛性。工程实现中可以利用网上开源的Ceres库以及g2o图优化库进行回环位姿图优化。
利用优化后的轨迹更新回环内所有节点的位姿状态。转步骤S101，等待新的节点消息；如果长时间没来新的消息，则保存最终的轨迹，退出回环检测程序。
基于本发明实施例提供的回环检测方法，充分利用车辆接收的全球位置(GPS)信息，通过对GPS位置进行哈希编码，可以实现近乎实时的回环检索效率；估计回环误差的过程中，在保证三维点云配准满足重叠率要求的前提下，通过对里程计按照适当的距离等间隔采样，可大幅缩减三维点云的临时存储空间并降低计算代价；采用时间加轨迹长度的约束可快速剔除由于长时间停车形成的伪回环，改善轨迹优化的精度。位姿图优化中利用自环边约束替代固定节点约束，可提升长距离回环优化的收敛性。尤其是在长距离的运行下仍旧能够输出准确可靠的轨迹精度，进而确保高精度地图的自动构建。
图4是车辆实际轨迹示意图，图5是里程计采集到的车辆轨迹优化前的示意图，图6是通过本发明实施例回环检测方案优化后的车辆轨迹示意图。通过对比图5、图6可知，采用本发明实施例提供的方法可以对里程计所获取的车辆轨迹进行修正及优化，使得回环轨迹更加接近真实路线，从而解决了迹漂移发散的问题，为制作高精度地图提供了很好的轨迹初值。
基于同一发明构思，本发明实施例还提供了一种回环检测系统，如图7所示，该系统可包括：
数据接收模块710，配置为接收目标节点的节点数据；节点数据包括：目标节点的时间戳、目标节点的GPS位置、目标节点的位姿信息以及目标节点的三维点云；
编码模块720，配置为对GPS位置进行哈希编码得到目标节点的键值；
集合生成模块730，配置为在历史GPS位置编码库中查找与键值具有相同键值的至少一个历史节点，生成历史节点集合；
第一节点筛选模块740，配置为在历史节点集合中筛选出与目标节点的时间戳的时间间隔大于设定时间阈值、且地心距离小于设定地心距离阈值的历史节点作为候选回环节点；
第二节点筛选模块750，配置为在候选回环节点中筛选出与目标节点的轨迹距离大于设定轨迹距离阈值的历史节点作为目标回环节点；
点云获取模块760，配置为获取目标回环节点的三维点云记为目标点云，将目标节点的三维点云记为源点云；
对应关系建立模块770，配置为通过特征匹配在目标点云和源点云之间建立对应关系，得到目标点云和源点云之间的刚体变换矩阵；
回环构建模块780，配置为以刚体变换矩阵作为回环约束，将目标回环节点和目标节点构成回环；
节点更新模块790，配置为对回环中目标回环节点和目标节点之间的轨迹进行优化，利用优化后的轨迹更新回环上各节点的位姿状态。
在本发明一可选实施例中，编码模块720还配置为，对GPS位置进行哈希编码得到目标节点的键值之后，将键值存入历史GPS位置编码库。
在本发明一可选实施例中，编码模块720还可以配置为，当目标节点为第一个节点时，对目标节点的GPS位置进行哈希编码；
当目标节点不是第一个节点时，判断目标节点与上一个存入GPS位置编码库中的节点间的距离是否大于设定阈值；若是，则对目标节点的GPS位置进行哈希编码；若否，则忽略目标节点。
在本发明一可选实施例中，对应关系建立模块770还可以配置为分别从目标点云和源点云中提取若干关键点；
分别基于目标点云和源点云中提取的若干关键点对目标点云和源点云进行局部特征描述，并基于局部特征描述建立目标点云的各目标点和源点云的各源点之间对应关系，得到多个匹配对；其中，对应关系指不同视角扫描同一个场景中建立的同名点对应关系；
利用几何一致性算法在多个匹配对中筛选出误匹配对并进行删除，得到剩余匹配对；
判断剩余匹配对中的匹配对不小于设定数量时，利用剩余匹配对包含的匹配对估计目标点云和源点云之间的位姿变换以进行精准匹配后得到目标点云和源点云之间的刚体变换矩阵。
在本发明一可选实施例中，数据接收模块710还可以配置为将三维点云以二进制的形式写入硬盘文件中。
在本发明一可选实施例中，还提供了一种计算机可读存储介质，存储介质中存储有至少一条指令、至少一段程序、代码集或指令集，至少一条指令、至少一段程序、代码集或指令集由处理器加载并执行上述任一实施例所述的基于激光雷达的回环检测方法。
在本发明一可选实施例中，还提供了一种电子设备，包括处理器和存储器，存储器中存储有至少一条指令、至少一段程序、代码集或指令集，至少 一条指令、至少一段程序、代码集或指令集由处理器加载并执行以实现上述任一实施例所述的基于激光雷达的回环检测方法。
在本发明一可选实施例中，还提供了一种运行指令的芯片，所述芯片包括存储器、处理器，所述存储器中存储代码和数据，所述存储器与所述处理器耦合，所述处理器运行所述存储器中的代码使得所述芯片用于执行以实现上述任一实施例所述的基于激光雷达的回环检测方法。
在本发明一可选实施例中，还提供了一种计算机程序产品，所述程序产品包括计算机程序，所述计算机程序存储在计算机可读存储介质中，至少一个处理器可以从所述计算机可读存储介质读取所述计算机程序，所述至少一个处理器执行所述计算机程序时可实现上述任一实施例所述的基于激光雷达的回环检测方法。
所属领域的技术人员可以清楚地了解到，上述描述的系统具体工作过程，可以参考前述方法实施例中的对应过程，为简洁起见，在此不另赘述。
本领域普通技术人员可以理解：上述的方法如果以软件的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明的技术方案本质上或者该技术方案的全部或部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，其包括若干指令，用以使得一台计算设备(例如个人计算机，服务器，或者网络设备等)在运行所述指令时执行本发明各实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(ROM)、随机存取存储器(RAM)，磁碟或者光盘等各种可以存储程序代码的介质。
或者，实现前述方法实施例的全部或部分步骤可以通过程序指令相关的硬件(诸如个人计算机，服务器，或者网络设备等的计算设备)来完成，所述程序指令可以存储于一计算机可读取存储介质中，当所述程序指令被计算设备的处理器执行时，所述计算设备执行本发明各实施例所述方法的全部或部分步骤。
最后应说明的是：以上各实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述各实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：在本发明的精神和原则之内，其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分或者全部技术特征进行等同 替换；而这些修改或者替换，并不使相应技术方案脱离本发明的保护范围。

Claims (10)

  1. 一种回环检测方法，其特征在于，包括：
    接收目标节点的节点数据；所述节点数据包括：目标节点的时间戳、目标节点的GPS位置、目标节点的位姿信息以及目标节点的三维点云；
    对所述GPS位置进行哈希编码得到所述目标节点的键值；
    在历史GPS位置编码库中查找与所述目标节点的键值具有相同键值的至少一个历史节点，生成历史节点集合；
    在所述历史节点集合中筛选出与所述目标节点的时间戳的时间间隔大于设定时间阈值、且地心距离小于设定地心距离阈值的历史节点作为候选回环节点；
    在所述候选回环节点中筛选出与所述目标节点的轨迹距离大于设定轨迹距离阈值的历史节点作为目标回环节点；
    获取所述目标回环节点的三维点云记为目标点云，将所述目标节点的三维点云记为源点云；
    通过特征匹配在所述目标点云和源点云之间建立对应关系，得到所述目标点云和源点云之间的刚体变换矩阵；
    以所述刚体变换矩阵作为回环约束，将所述目标回环节点和所述目标节点构成回环；
    对所述回环中所述目标回环节点和目标节点之间的轨迹进行优化，利用优化后的轨迹更新所述回环上各节点的位姿状态。
  2. 根据权利要求1所述的方法，其特征在于，所述对所述GPS位置进行哈希编码得到所述目标节点的键值之后，还包括：
    将所述键值存入所述历史GPS位置编码库。
  3. 根据权利要求2所述的方法，其特征在于，所述对所述GPS位置进行哈希编码得到所述目标节点的键值，包括：
    当所述目标节点为第一个节点时，对所述目标节点的GPS位置进行哈希编码；
    当所述目标节点不是第一个节点时，判断所述目标节点与上一个存入所述GPS位置编码库中的节点间的距离是否大于设定阈值；若是，则对所述目标节点的GPS位置进行哈希编码；若否，则忽略所述目标节点。
  4. 根据权利要求1-3任一项所述的方法，其特征在于，所述通过特征匹 配在所述目标点云和源点云之间建立对应关系，得到所述目标点云和源点云之间的刚体变换矩阵，包括：
    分别从所述目标点云和源点云中提取若干关键点；
    分别基于所述目标点云和源点云中提取的若干关键点对所述目标点云和源点云进行局部特征描述，并基于所述局部特征描述建立所述目标点云的各目标点和源点云的各源点之间对应关系，得到多个匹配对；其中，所述对应关系指不同视角扫描同一个场景中建立的同名点对应关系；
    利用几何一致性算法在多个所述匹配对中筛选出误匹配对并进行删除，得到剩余匹配对；
    判断所述剩余匹配对中的匹配对不小于设定数量时，利用所述剩余匹配对包含的匹配对估计所述目标点云和源点云之间的位姿变换以进行精准匹配后得到所述目标点云和源点云之间的刚体变换矩阵。
  5. 根据权利要求1-3任一项所述的方法，其特征在于，所述接收目标节点的节点数据之后，还包括：
    将所述三维点云以二进制的形式写入硬盘文件中。
  6. 一种回环检测系统，其特征在于，包括：
    数据接收模块，配置为接收目标节点的节点数据；所述节点数据包括：目标节点的时间戳、目标节点的GPS位置目标节点的位姿信息以及目标节点的三维点云；
    编码模块，配置为对所述GPS位置进行哈希编码得到所述目标节点的键值；
    集合生成模块，配置为在历史GPS位置编码库中查找与所述键值具有相同键值的至少一个历史节点，生成历史节点集合；
    第一节点筛选模块，配置为在所述历史节点集合中筛选出与所述目标节点的时间戳的时间间隔大于设定时间阈值、且地心距离小于设定地心距离阈值的历史节点作为候选回环节点；
    第二节点筛选模块，配置为在所述候选回环节点中筛选出与所述目标节点的轨迹距离大于设定轨迹距离阈值的历史节点作为目标回环节点；
    点云获取模块，配置为获取所述目标回环节点的三维点云记为目标点云，将所述目标节点的三维点云记为源点云；
    对应关系建立模块，配置为通过特征匹配在所述目标点云和源点云之间 建立对应关系，得到所述目标点云和源点云之间的刚体变换矩阵；
    回环构建模块，配置为以所述刚体变换矩阵作为回环约束，将所述目标回环节点和所述目标节点构成回环；
    节点更新模块，配置为对所述回环中所述目标回环节点和目标节点之间的轨迹进行优化，利用优化后的轨迹更新所述回环上各节点的位姿状态。
  7. 一种计算机可读存储介质，其特征在于，所述存储介质中存储有至少一条指令、至少一段程序、代码集或指令集，所述至少一条指令、至少一段程序、代码集或指令集由处理器加载并执行如权利要求1-5任意一项所述的回环检测方法。
  8. 一种电子设备，其特征在于，包括处理器和存储器，所述存储器中存储有至少一条指令、至少一段程序、代码集或指令集，所述至少一条指令、所述至少一段程序、所述代码集或指令集由所述处理器加载并执行以实现如权利要求1-5任意一项所述的回环检测方法。
  9. 一种运行指令的芯片，其特征在于，所述芯片包括存储器、处理器，所述存储器中存储代码和数据，所述存储器与所述处理器耦合，所述处理器运行所述存储器中的代码使得所述芯片用于执行上述权利要求1-5中任一项所述的回环检测方法。
  10. 一种计算机程序产品，包括计算机程序，其特征在于，所述计算机程序被处理器执行时实现权利要求1至5中任一项所述的回环检测方法。
PCT/CN2021/105322 2020-07-31 2021-07-08 回环检测方法及系统、可读存储介质、电子设备 WO2022022256A1 (zh)

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
CN202010761223.5 2020-07-31
CN202010761223.5A CN111862162B (zh) 2020-07-31 2020-07-31 回环检测方法及系统、可读存储介质、电子设备

Publications (1)

Publication Number Publication Date
WO2022022256A1 true WO2022022256A1 (zh) 2022-02-03

Family

ID=72954065

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/CN2021/105322 WO2022022256A1 (zh) 2020-07-31 2021-07-08 回环检测方法及系统、可读存储介质、电子设备

Country Status (2)

Country Link
CN (1) CN111862162B (zh)
WO (1) WO2022022256A1 (zh)

Cited By (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114998540A (zh) * 2022-06-12 2022-09-02 哈尔滨工程大学 一种智慧城市传感器探测主动同步定位与建图方法
CN115047487A (zh) * 2022-03-11 2022-09-13 武汉科技大学 基于点云强度与高度信息的回环检测方法及设备
CN115079202A (zh) * 2022-06-16 2022-09-20 智道网联科技(北京)有限公司 激光雷达建图方法、装置及电子设备、存储介质
CN116358532A (zh) * 2023-05-31 2023-06-30 小米汽车科技有限公司 回环检测方法、装置、存储介质以及车辆
CN116481517A (zh) * 2023-06-25 2023-07-25 深圳市普渡科技有限公司 扩展建图方法、装置、计算机设备和存储介质
CN117289298A (zh) * 2023-10-19 2023-12-26 广州行深智能科技有限公司 基于激光雷达的多机协同在线建图方法、系统及终端设备
WO2024007807A1 (zh) * 2022-07-06 2024-01-11 杭州萤石软件有限公司 一种误差校正方法、装置及移动设备
CN117761717A (zh) * 2024-02-21 2024-03-26 天津大学四川创新研究院 一种自动回环三维重建系统及运行方法
CN117824664A (zh) * 2024-03-05 2024-04-05 河海大学 基于多波束测深声呐的自主无人系统主动slam方法
CN118274820A (zh) * 2024-06-03 2024-07-02 新石器慧通（北京）科技有限公司 回环检测方法、装置、电子设备和存储介质

Families Citing this family (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111862162B (zh) * 2020-07-31 2021-06-11 湖北亿咖通科技有限公司 回环检测方法及系统、可读存储介质、电子设备
CN113761091B (zh) * 2020-11-27 2024-04-05 北京京东乾石科技有限公司 闭环检测方法、装置、电子设备、系统和存储介质
CN112614187B (zh) * 2020-12-31 2024-03-26 深圳市优必选科技股份有限公司 回环检测方法、装置、终端设备和可读存储介质
CN112835080B (zh) * 2021-01-21 2024-03-19 成都路行通信息技术有限公司 车辆静止状态的轨迹修复方法、装置及电子设备
CN113642102B (zh) * 2021-07-23 2024-03-15 一汽奔腾轿车有限公司 一种碰撞模型中刚体对的自动化建模方法
CN113783753B (zh) * 2021-09-10 2022-08-16 北京云杉世纪网络科技有限公司 一种环路检测方法、装置、设备及存储介质
CN113899361B (zh) * 2021-12-10 2022-03-01 西安电子科技大学 基于空间划分的slam快速回环检测方法
CN115420275A (zh) * 2022-08-22 2022-12-02 先临三维科技股份有限公司 回环路径的预测方法及装置、非易失性存储介质、处理器

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108267141A (zh) * 2016-12-30 2018-07-10 乐视汽车（北京）有限公司 道路点云数据处理系统
CN109711245A (zh) * 2018-11-05 2019-05-03 广东工业大学 一种基于图像候选区域的闭环检测方法
US20190137604A1 (en) * 2017-11-09 2019-05-09 Vadum, Inc. Target Identification and Clutter Mitigation in High Resolution Radar Systems
CN111862162A (zh) * 2020-07-31 2020-10-30 湖北亿咖通科技有限公司 回环检测方法及系统、可读存储介质、电子设备

Family Cites Families (18)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104851094A (zh) * 2015-05-14 2015-08-19 西安电子科技大学 一种基于rgb-d的slam算法的改进方法
CN105783913A (zh) * 2016-03-08 2016-07-20 中山大学 一种融合车载多传感器的slam装置及其控制方法
CN108133496B (zh) * 2017-12-22 2021-11-26 北京工业大学 一种基于g2o与随机蕨类算法的稠密地图创建方法
US10636114B2 (en) * 2018-08-04 2020-04-28 Beijing Jingdong Shangke Information Technology Co., Ltd. System and method for scan-matching oriented visual slam
CN111060101B (zh) * 2018-10-16 2022-06-28 深圳市优必选科技有限公司 视觉辅助的距离slam方法及装置、机器人
CN109658449B (zh) * 2018-12-03 2020-07-10 华中科技大学 一种基于rgb-d图像的室内场景三维重建方法
CN109709801B (zh) * 2018-12-11 2024-02-02 智灵飞（北京）科技有限公司 一种基于激光雷达的室内无人机定位系统及方法
CN109902619B (zh) * 2019-02-26 2021-08-31 上海大学 图像闭环检测方法及系统
CN110163095B (zh) * 2019-04-16 2022-11-29 中国科学院深圳先进技术研究院 回环检测方法、回环检测装置及终端设备
CN110378997B (zh) * 2019-06-04 2023-01-20 广东工业大学 一种基于orb-slam2的动态场景建图与定位方法
CN110689622B (zh) * 2019-07-05 2021-08-27 电子科技大学 一种基于点云分割匹配闭环校正的同步定位与构图方法
CN110689562A (zh) * 2019-09-26 2020-01-14 深圳市唯特视科技有限公司 一种基于生成对抗网络的轨迹回环检测优化方法
CN110645974B (zh) * 2019-09-26 2020-11-27 西南科技大学 一种融合多传感器的移动机器人室内地图构建方法
CN110781262B (zh) * 2019-10-21 2023-06-02 中国科学院计算技术研究所 基于视觉slam的语义地图的构建方法
CN111060115B (zh) * 2019-11-29 2022-03-22 中国科学院计算技术研究所 一种基于图像边缘特征的视觉slam方法及系统
CN111161412B (zh) * 2019-12-06 2024-02-09 苏州艾吉威机器人有限公司 三维激光建图方法及系统
CN111076733B (zh) * 2019-12-10 2022-06-14 亿嘉和科技股份有限公司 一种基于视觉与激光slam的机器人室内建图方法及系统
CN111429344B (zh) * 2020-02-19 2022-04-26 上海交通大学 基于感知哈希的激光slam闭环检测方法及系统

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108267141A (zh) * 2016-12-30 2018-07-10 乐视汽车（北京）有限公司 道路点云数据处理系统
US20190137604A1 (en) * 2017-11-09 2019-05-09 Vadum, Inc. Target Identification and Clutter Mitigation in High Resolution Radar Systems
CN109711245A (zh) * 2018-11-05 2019-05-03 广东工业大学 一种基于图像候选区域的闭环检测方法
CN111862162A (zh) * 2020-07-31 2020-10-30 湖北亿咖通科技有限公司 回环检测方法及系统、可读存储介质、电子设备

Cited By (14)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115047487A (zh) * 2022-03-11 2022-09-13 武汉科技大学 基于点云强度与高度信息的回环检测方法及设备
CN114998540A (zh) * 2022-06-12 2022-09-02 哈尔滨工程大学 一种智慧城市传感器探测主动同步定位与建图方法
CN115079202A (zh) * 2022-06-16 2022-09-20 智道网联科技(北京)有限公司 激光雷达建图方法、装置及电子设备、存储介质
WO2024007807A1 (zh) * 2022-07-06 2024-01-11 杭州萤石软件有限公司 一种误差校正方法、装置及移动设备
CN116358532A (zh) * 2023-05-31 2023-06-30 小米汽车科技有限公司 回环检测方法、装置、存储介质以及车辆
CN116358532B (zh) * 2023-05-31 2023-09-26 小米汽车科技有限公司 回环检测方法、装置、存储介质以及车辆
CN116481517B (zh) * 2023-06-25 2023-10-13 深圳市普渡科技有限公司 扩展建图方法、装置、计算机设备和存储介质
CN116481517A (zh) * 2023-06-25 2023-07-25 深圳市普渡科技有限公司 扩展建图方法、装置、计算机设备和存储介质
CN117289298A (zh) * 2023-10-19 2023-12-26 广州行深智能科技有限公司 基于激光雷达的多机协同在线建图方法、系统及终端设备
CN117761717A (zh) * 2024-02-21 2024-03-26 天津大学四川创新研究院 一种自动回环三维重建系统及运行方法
CN117761717B (zh) * 2024-02-21 2024-05-07 天津大学四川创新研究院 一种自动回环三维重建系统及运行方法
CN117824664A (zh) * 2024-03-05 2024-04-05 河海大学 基于多波束测深声呐的自主无人系统主动slam方法
CN117824664B (zh) * 2024-03-05 2024-05-28 河海大学 基于多波束测深声呐的自主无人系统主动slam方法
CN118274820A (zh) * 2024-06-03 2024-07-02 新石器慧通（北京）科技有限公司 回环检测方法、装置、电子设备和存储介质

Also Published As

Publication number Publication date
CN111862162A (zh) 2020-10-30
CN111862162B (zh) 2021-06-11

Similar Documents

Publication Publication Date Title
WO2022022256A1 (zh) 回环检测方法及系统、可读存储介质、电子设备
CN113593017B (zh) 露天矿地表三维模型构建方法、装置、设备及存储介质
CN110501017A (zh) 一种基于orb_slam2的移动机器人导航地图生成方法
Botterill et al. Bag‐of‐words‐driven, single‐camera simultaneous localization and mapping
CN102810118B (zh) 一种变权网k近邻搜索方法
CN113065594B (zh) 一种基于北斗数据与遥感影像融合的路网提取方法及装置
CN104200523A (zh) 一种融合附加信息的大场景三维重建方法
CN112084289B (zh) 一种轨迹融合方法及装置
CN105091889A (zh) 一种热点路径的确定方法及设备
CN115077556A (zh) 一种基于多维度地图的无人车野战路径规划方法
CN114707611B (zh) 基于图神经网络特征提取与匹配的移动机器人地图构建方法、存储介质及设备
CN107229682A (zh) 一种基于大型数据库的出租车位置数据处理方法
CN108537263B (zh) 一种基于最大公共子图的栅格地图融合方法
CN117571012A (zh) 一种越野环境无人车辆全局路径规划方法、系统及设备
Howe et al. Deformable part models for automatically georeferencing historical map images
CN117824667A (zh) 一种基于二维码和激光的融合定位方法及介质
CN113624239B (zh) 基于层级可开关稀疏位姿图优化的激光建图方法及装置
CN112100308B (zh) 一种北斗时空轨迹的相似性查询方法
CN117875533A (zh) 矿用安全逃生路径规划方法及系统
CN115170826A (zh) 基于局部搜索的运动小目标快速光流估计方法及存储介质
US20150127302A1 (en) Method and apparatus for optimized routing
CN116737851A (zh) 一种节点式点云地图的存储和更新方法
CN113988184A (zh) 一种矢量道路数据匹配与更新方法、装置和计算机设备
CN108763817B (zh) 一种基于最小二乘法建模的电力地下管网匹配方法
Wang et al. Grid‐Based Whole Trajectory Clustering in Road Networks Environment

Legal Events

Date Code Title Description
121 Ep: the epo has been informed by wipo that ep was designated in this application

Ref document number: 21849113

Country of ref document: EP

Kind code of ref document: A1

NENP Non-entry into the national phase

Ref country code: DE

122 Ep: pct application non-entry in european phase

Ref document number: 21849113

Country of ref document: EP

Kind code of ref document: A1

32PN Ep: public notification in the ep bulletin as address of the adressee cannot be established

Free format text: NOTING OF LOSS OF RIGHTS PURSUANT TO RULE 112(1) EPC (EPO FORM 1205A DATED 290623)

122 Ep: pct application non-entry in european phase

Ref document number: 21849113

Country of ref document: EP

Kind code of ref document: A1