Laser radar SLAM multi-loop detection and iterative optimization method and electronic equipment

By designing multi-loop routes in the drone/vehicle system and applying ICP iterative nearest point and GTSAM graph optimization methods, the problem of large multi-loop detection error in lidar SLAM technology is solved, and a three-dimensional point cloud with high precision and timeliness is realized Output, suitable for emergency and fast situational awareness scenarios.

CN119001680APending Publication Date: 2024-11-22AEROSPACE PROSPECT (BEIJING) TECH CO LTD
View PDF 0 Cites 4 Cited by

Patent Information

Application Number
CN202310575207.0
Authority / Receiving Office
CN · China
Patent Type
Applications(China)
Current Assignee / Owner
Filing Date
2023-05-22
Publication Date
2024-11-22

AI Technical Summary

Technical Problem

The existing lidar SLAM technology has problems such as large multi-loop detection error, large calculation volume, and not suitable for rapid applications in emergency and rapid situational awareness scenarios.

Method used

Design a multi-loop route for the drone/vehicle system, and through ICP iterating the nearest point and GTSAM graph optimization methods, detect points one by one to match the current frame and keyframe, iteratively optimize all keyframe poses in the loop, and output high-precision three-dimensional point clouds.

Benefits of technology

It realizes three-dimensional point cloud output with higher accuracy and timeliness in emergency and fast situational awareness scenarios, meeting the needs of high precision and rapid application.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN119001680A_ABST
    Figure CN119001680A_ABST
Patent Text Reader

Abstract

The invention relates to the technical field of laser radar data processing, in particular to a laser radar SLAM multi-loop detection and iterative optimization method and electronic equipment. The method comprises the following steps: S1, designing a multi-loop route of an unmanned aerial vehicle / vehicle system, and extracting and storing loop detection point coordinates; s2, the unmanned aerial vehicle / vehicle system travels according to the route designed in the S1, extracts feature points of a current frame, calculates a global pose, selects a key frame and stores point cloud and the pose of the key frame; s3, the unmanned aerial vehicle / vehicle system enters a specified radius of the coordinates of the detection point extracted in the S1 to carry out loopback detection, and a current frame and a key frame of the detection point are matched and selected; s4, solving a relative pose change relationship between the current frame of the detection point selected in the S3 and the key frame point cloud set, and globally optimizing all key frame poses and point clouds thereof in the loop; and S5, repeating the steps S3-S4 for iterative loopback detection and global optimization of the detection points one by one, and iteratively outputting the three-dimensional point cloud applying optimization precision. According to the method, an unmanned aerial vehicle / vehicle system multi-loop route and loop detection points are designed, an ICP iteration nearest point and GTSAM graph optimization method is applied, key frames in the route are selected, the detection points are matched with the current frame and the key frames one by one, the poses of all the key frames in the loop are iteratively optimized, and a three-dimensional point cloud applying optimization precision is iteratively output. Therefore, emergency and rapid situation awareness with higher precision and time efficiency can be realized.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] The present invention relates to the technical field of laser radar data processing, and in particular to a laser radar SLAM multi-loop detection and iterative optimization method and electronic equipment. Background Art

[0002] Laser SLAM (Simultaneous Localization and Mapping) is a widely used laser radar data positioning and mapping technology. Drones and other flying platforms and unmanned vehicles and other walking platforms can carry laser radars. SLAM technology is used to build a three-dimensional point cloud model of the ground situation. It is widely used in scenes such as geographic surveying and mapping, industry inspections, and emergency rescue. The SLAM algorithm uses the laser radar point cloud matching between frames to obtain the transformed pose between frames, and the global pose is obtained by accumulating the pose between frames. Then, each frame of the point cloud is rotated and translated according to the global pose to obtain a three-dimensional point cloud in the global coordinate system. The error generated by the inter-frame matching is generally globally optimized by loop detection. In the current laser radar SLAM technology application, a loop detection is mostly established between the starting point and the end point, resulting in a large error in the matching of the three-dimensional point cloud in the middle of the route, and there are too many global optimization nodes and too much calculation, which is not suitable for emergency and rapid application scenarios.

[0003] In view of the technical and application limitations of existing laser radar SLAM, the present invention discloses a laser radar SLAM multi-loop detection and iterative optimization technology, which designs multi-loop routes and loop detection points of UAV / vehicle systems, applies ICP iterative closest point and GTSAM graph optimization methods, selects key frames in the route, matches the current frame with the key frame one by one, iteratively optimizes the poses of all key frames in the loop, and iteratively outputs a three-dimensional point cloud with optimized accuracy, so as to achieve emergency and rapid situational awareness with higher accuracy and timeliness. Summary of the invention

[0004] In view of the limitations of the existing technology, the present invention discloses a laser radar SLAM multi-loop detection and iterative optimization method and electronic equipment, which are used to solve the application bottleneck of the existing laser radar SLAM in emergency and rapid situation awareness scenarios.

[0005] The present invention is achieved through the following technical solutions:

[0006] In a first aspect, the present invention discloses a laser radar SLAM multi-loop detection and iterative optimization method, the method comprising the following steps:

[0007] S1 designs multi-loop routes for drone / car systems and extracts and stores the coordinates of loop detection points;

[0008] S2 The drone / car system drives along the route designed in S1, extracts the feature points of the current frame and calculates the global pose, selects the key frame and stores its point cloud and pose;

[0009] S3 The drone / car system enters the specified radius of the detection point coordinates extracted in S1 to perform loop detection, and matches the current frame and key frame of the selected detection point;

[0010] S4 solves the relative position change relationship between the current frame of the detection point selected in S3 and the key frame point cloud set, and globally optimizes all key frame poses and point clouds in the loop;

[0011] S5 repeats S3-S4 to iterate loop detection and global optimization for each detection point, and iteratively outputs a three-dimensional point cloud with optimized accuracy.

[0012] Furthermore, in the S1, the drone / vehicle system includes a drone / vehicle platform, a laser radar, a video camera, a control ground station and a situation ground station. The drone can be equipped with an array laser radar to orthogonally scan the monitoring area, and the unmanned vehicle can be equipped with a mechanical laser radar to scan the monitoring area 360°; the control ground station controls the drone / vehicle and the laser radar and video camera to scan the monitoring area according to the set route and posture. The situation ground station receives and processes laser point clouds and video images in real time, automatically generates three-dimensional point clouds and real-life models, and is equipped with intelligent recognition and analysis function modules based on texture, temperature, geometric features and spatial variables of dynamic videos and three-dimensional models.

[0013] Furthermore, in S1, the method for designing a multi-loop route for the drone / vehicle system is to first diagonally penetrate the monitoring area from a short-range waypoint to a diagonal long-range waypoint, and then return to the short-range waypoint with a bow-shaped route. The return route intersects with the through route multiple times to construct multiple loops and detection points.

[0014] Furthermore, in S2, the method for extracting feature points of the current frame and calculating the global pose is to process the point cloud data of the current frame using the SLAM algorithm, extract corner point features and surface point features in the point cloud, perform inter-frame odometer pose estimation based on the feature points, and obtain the optimal pose estimate of the inter-frame changes and the global pose of the current frame.

[0015] Furthermore, in S2, the method for selecting the key frame is to calculate the distance or time difference between the current frame and the previous key frame, and select the current frame that is greater than a set threshold as a new key frame.

[0016] Furthermore, in S3, the loop detection method is to establish a kd-tree for the coordinates of all current frames and key frames within the set radius of the detection point to perform a nearest neighbor search, match and select the current frame and key frame whose distance to the detection point is less than a threshold and whose time meets the requirements, and construct a detection loop.

[0017] Furthermore, in S4, the ICP (Iterative Closest Point) iterative closest point method is used to solve the relative pose change relationship between the detection point matching the current frame and the key frame point cloud set, and then the GTSAM (Georgia Tech Smoothing and Mapping) graph optimization method is used to obtain the best estimated coordinates and point clouds of all key frames of the loop after global optimization.

[0018] In a second aspect, the present invention discloses an electronic device, comprising a processor and a memory storing execution instructions. When the processor executes the execution instructions stored in the memory, the processor executes the laser radar SLAM multi-loop detection and iterative optimization method described in the first aspect.

[0019] The beneficial effects of the present invention are:

[0020] The present invention designs a drone / vehicle system to diagonally penetrate the monitoring area from a short-range waypoint to a long-range waypoint, and then return to the short-range waypoint in a bow-shaped route. The return route intersects with the through route multiple times to construct multiple loops and detection points. The key frames in the route are selected, and the ICP iterative nearest point and GTSAM graph optimization methods are applied to match the current frame with the key frame one by one, and the poses of all key frames in the loop are globally optimized, and then the three-dimensional point cloud with optimized accuracy is iteratively output in the route to meet the higher accuracy and timeliness requirements of emergency and rapid situational awareness scenarios. BRIEF DESCRIPTION OF THE DRAWINGS

[0021] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative work.

[0022] Figure 1 is a system architecture diagram of an embodiment of the present invention;

[0023] Figure 2 is a diagram of the architecture of a drone / car system according to an embodiment of the present invention;

[0024] Figure 3 Schematic diagram of a multi-loop route according to an embodiment of the present invention. DETAILED DESCRIPTION

[0025] In order to make the purpose, technical solution and advantages of the embodiments of the present invention clearer, the technical solution in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0026] Example 1

[0027] This embodiment discloses a laser radar SLAM multi-loop detection and iterative optimization method, including the following steps:

[0028] S1 designs multi-loop routes for drone / car systems and extracts and stores the coordinates of loop detection points;

[0029] S2 The drone / car system drives along the route designed in S1, extracts the feature points of the current frame and calculates the global pose, selects the key frame and stores its point cloud and pose;

[0030] S3 The drone / car system enters the specified radius of the detection point coordinates extracted in S1 to perform loop detection, and matches the current frame and key frame of the selected detection point;

[0031] S4 solves the relative posture change relationship between the detection point selected in S3 and the current frame and the key frame point cloud set, and globally optimizes all key frame postures and point clouds in the loop.

[0032] S5 repeats S3-S4 to iterate loop detection and global optimization one by one, and iteratively outputs a three-dimensional point cloud with optimized accuracy.

[0033] The system architecture of this embodiment includes four core modules: multi-loop route design module, key frame processing module, loop detection module and loop global optimization module. Figure 1 .

[0034] Among them, the multi-loop route design module uses a through route that diagonally penetrates the monitoring area from a short-distance waypoint to a long-distance waypoint and then intersects with a return route that returns to the short-distance waypoint in a bow-shaped route to construct multiple loops and detection points, and extracts and stores the coordinates of the loop detection points.

[0035] The key frame processing module uses the SLAM algorithm to process the point cloud data of the current frame, extracts the corner point features and surface point features in the point cloud, performs inter-frame odometer pose estimation based on feature points, obtains the optimal pose estimation of inter-frame changes and the global pose of the current frame, and then calculates the distance or time difference between the current frame and the previous key frame, selects the current frame greater than the set threshold as the new key frame, and stores its point cloud and pose.

[0036] The loop detection module establishes a kd-tree for the nearest neighbor search of all current frames and key frames within the set radius of the loop detection point, matches and selects the current frame and key frame whose distance to the detection point is less than the threshold and whose time meets the requirements, and constructs a detection loop.

[0037] The loop global optimization module adopts the ICP iterative closest point method to solve the relative pose change relationship between the detection point matching the current frame and the key frame point cloud set, and then uses the GTSAM graph optimization method to obtain the best estimated coordinates and point cloud of all key frames of the loop after global optimization, and iterates the loop detection and global optimization one by one, and iteratively outputs the three-dimensional point cloud with optimized accuracy.

[0038] Example 2

[0039] This embodiment discloses a UAV / vehicle system equipped with a laser radar SLAM multi-loop detection and iterative optimization method and electronic equipment, including a UAV / vehicle platform, a laser radar, a video camera, a control ground station and a situation ground station. The system architecture is shown in Figure 2 .

[0040] The control ground station controls the drone / vehicle, laser radar, and video camera to scan the monitoring area according to the set route and posture. The situation ground station receives and processes laser point clouds and video images in real time, automatically generates three-dimensional point clouds and real-scene models, and is equipped with intelligent recognition and analysis function modules based on texture, temperature, geometric features and spatial variables of dynamic videos and three-dimensional models.

[0041] The unmanned aerial vehicle of this embodiment can be equipped with an array laser radar to orthogonally scan the monitoring area, and the unmanned vehicle can be equipped with a mechanical laser radar to scan the monitoring area 360°, thereby realizing airborne integrated situation scanning and analysis.

[0042] Example 3

[0043] This embodiment discloses a multi-loop route design module based on a method of multiple intersections of a long-distance waypoint through-route and a bow-shaped return route.

[0044] The drone / car system generally adopts a bow-shaped route design to scan the monitoring area, starting from the short-distance waypoint and ending the scanning at the long-distance waypoint before returning. The return route can intersect with the bow-shaped scanning route multiple times to form multiple loops and detection points. After returning, all data can be globally optimized to obtain a high-precision 3D point cloud model. However, in emergency and rapid situational awareness scenarios, the drone / car system is required to iteratively output a high-precision 3D point cloud model of the scanned monitoring area in the scanning route. Conventional route design methods cannot meet higher accuracy and timeliness requirements.

[0045] This embodiment adopts a route design method based on multiple intersections of a long-distance waypoint through-route and a bow-shaped return route. It first diagonally penetrates the monitoring area from a short-distance waypoint to the long-distance waypoint, and then returns to the short-distance waypoint with a bow-shaped route. During the scanning process, the return route and the through-route intersect in turn to construct multiple loops and detection points, and then iteratively performs loop detection and global optimization, and iteratively outputs a high-precision three-dimensional point cloud model of the scanned monitoring area in the application route.

[0046] The multi-loop route design is based on the bow-shaped route to cover the monitoring area, such as Figure 3 As shown in the figure, the short-distance waypoint is waypoint 1 and the long-distance waypoint is waypoint 10, which are arranged in order of distance from the starting point. The short-distance waypoint and the long-distance waypoint are diagonal points. The UAV / vehicle system starts from the starting point, passes through the monitoring area diagonally through the short-distance waypoint 1 to reach the long-distance waypoint 10, and then travels along the bow-shaped route to scan and return to the short-distance waypoint 1 to complete the coverage scan of the monitoring area. During the driving, the return route intersects with the through route four times in sequence, forming 4 detection points and 4 loops.

[0047] Example 4

[0048] This embodiment discloses a key frame processing module based on the SLAM algorithm and the inter-frame distance / time threshold.

[0049] The laser radar current frame point cloud data is processed using the SLAM algorithm. First, the curvature of each point is calculated to extract the feature points. The points with a curvature less than the set corner point threshold are corner points, and the points with a curvature greater than the set surface point threshold are surface points. The curvature formula is:

[0050]

[0051] Where S is the surrounding point set, k is the kth frame, i is the point in the frame, and j is a point in the surrounding point set.

[0052] Then, the inter-frame odometer pose estimation is performed based on the feature points. The corner points of the current frame are converted to the point cloud coordinate system of the previous frame through the IMU pre-integration results. A kd-tree is established for all the corner points of the previous frame. The nearest neighbor search is performed on the converted corner points of the current frame in the kd-tree. A straight line is fitted to the nearest neighbor points. The residual is constructed through the point-to-line distance. The point-to-line direction is used as the residual direction, and an optimization constraint is established. The formula for the point-to-line distance is:

[0053]

[0054] in and are the coordinates of points i, j, and l in the lidar coordinate system {L} respectively.

[0055] The numerator of this formula is the modulus of the cross product of two vectors, that is, the area of ​​the parallelogram formed by the two vectors, and the denominator is the modulus of the two vectors, that is, the length of the base of the parallelogram. The area of ​​a parallelogram divided by the length of one side can be calculated to find the distance from the side to the corresponding vertex, that is, the distance from the corner point to the corner line.

[0056] Similarly, the surface points of the current frame are projected to the point cloud coordinate system of the previous frame through the IMU pre-integration results, and a kd-tree is established for all the surface points of the previous frame. The nearest neighbor search is performed on the surface points after the current frame conversion in the kd-tree, and the nearest neighbor points are fitted on the plane. The residual is constructed through the point-to-plane distance, and the point-to-plane direction is used as the residual direction to establish the optimization constraint. The formula for the point-to-plane distance is:

[0057]

[0058] in is the coordinate of point m in the laser radar coordinate system {L}.

[0059] The numerator of this formula consists of two parts. The modulus of the cross product of the two vectors below is the area of ​​the parallelogram formed by the two vectors, and then multiplied by the upper vector to get the volume of the cube formed by the three vectors; the denominator is the area of ​​the base triangle of the cube. The volume of the cube divided by the area of ​​the base triangle can calculate the distance from the base to the corresponding vertex, that is, the distance from the edge point to the corner face.

[0060] The residual and direction of the feature points can be iteratively solved through the Gauss-Newton gradient descent method to obtain the optimal pose estimation for inter-frame changes, which includes 6 state quantities, namely translation x, y, z and attitude angles roll, pitch, and yaw. The superposition of the pose of each frame is the global pose of the corresponding frame.

[0061] Since the output frequency of the laser radar point cloud can reach 10Hz, if it is stored frame by frame, it will cause excessive memory usage and excessive global optimization calculation. Therefore, this embodiment adopts a key frame storage method based on spatial distance and time, calculates the distance or time difference between the current frame and the previous key frame, selects the current frame greater than the set threshold as a new key frame, and stores its point cloud and posture, where the formula for the distance between the current frame and the previous key frame is:

[0062]

[0063] Where Δd is the distance between the current frame and the previous key frame, (x, y, z) is the global position coordinate of the current frame, and (x′, y′, z′) is the global position coordinate of the previous key frame.

[0064] Example 5

[0065] This embodiment discloses a loop detection module based on the kd-tree nearest neighbor search method.

[0066] In order to reduce the number of calculations for loop detection, this embodiment only performs loop detection when the drone / vehicle system enters the set radius of the loop detection point of the designed route.

[0067] The loop detection method is to establish a kd-tree for the coordinates of all current frames and key frames within the set radius of the detection point to perform nearest neighbor search, match and select the current frame and key frame whose distance to the detection point is less than the threshold and whose time meets the requirements, and construct a detection loop.

[0068] The distance and time between the current frame and the key frame are calculated as follows:

[0069]

[0070] Where (x, y, z) is the position coordinate of the current frame, (x′, y′, z′) is the position coordinate of the key frame, and D min is the distance threshold, t is the current frame time, t′ is the key frame time, T min is the time threshold.

[0071] Example 6

[0072] This embodiment discloses a loop global optimization module based on ICP iterative closest point and GTSAM graph optimization method.

[0073] This embodiment adopts the ICP iterative closest point method to solve the relative position change relationship between the detection point matching the current frame and the key frame point cloud set. The core idea is to obtain the rotation matrix R and the translation vector t so that the following formula is minimized:

[0074]

[0075] where x i is the point in the point cloud of the current frame, p i is the key frame x i The nearest neighbor of .

[0076] The relative pose transformation of the current frame and the key frame point cloud is superimposed on the key frame pose, which is the pose of the current frame in the map coordinate system calculated by loop closure detection. There will be a certain deviation between this pose and the pose of the current frame in the map coordinate system superimposed by the odometer. The deviation is used to adjust the poses of all key frames in the loop to achieve global pose optimization based on loop closure detection.

[0077] The specific optimization is performed using the GTSAM open source graph optimization library. GTSAM is a C++ library based on factor graphs. The inter-frame constraints obtained by loop detection are added to the factor graph. The confidence of the loop is the ICP score. Then, isam is updated to obtain the best estimated coordinates of the key frame after global optimization.

[0078] The globally optimized three-dimensional point cloud model of all key frames of the loop is generated according to the best estimated coordinates after global optimization.

[0079] Then, the current frame and the key frame are matched one by one in the route of the drone / vehicle system, the key frame pose is globally optimized one by one through loop closure, and a three-dimensional point cloud with optimized accuracy is iteratively output.

[0080] Example 7

[0081] This embodiment discloses an electronic device, including a processor and a memory storing execution instructions. When the processor executes the execution instructions stored in the memory, the processor executes a laser radar SLAM multi-loop detection and iterative optimization method.

[0082] In summary, the present invention designs a drone / vehicle system to diagonally penetrate the monitoring area from a short-range waypoint to a long-range waypoint, and then return to the short-range waypoint in a bow-shaped route. The return route intersects with the through route multiple times to construct multiple loops and detection points. The key frames in the route are selected, and the ICP iterative nearest point and GTSAM graph optimization methods are applied to match the current frame with the key frame one by one, and globally optimize the poses of all key frames in the loop, and then iteratively output the three-dimensional point cloud with optimized accuracy in the route to meet the higher accuracy and timeliness requirements of emergency and rapid situational awareness scenarios.

[0083] The above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit the same. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that the technical solutions described in the aforementioned embodiments may still be modified, or some of the technical features may be replaced by equivalents. However, these modifications or replacements do not deviate the essence of the corresponding technical solutions from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims

1. A laser radar SLAM multi-loop detection and iterative optimization method, It is characterized in that The method comprises the following steps: S1 designs multi-loop routes for drone / car systems and extracts and stores the coordinates of loop detection points; S2 The drone / car system drives along the route designed in S1, extracts the feature points of the current frame and calculates the global pose, selects the key frame and stores its point cloud and pose; S3 The drone / car system enters the specified radius of the detection point coordinates extracted in S1 to perform loop detection, and matches the current frame and key frame of the selected detection point; S4 solves the relative posture change relationship between the detection point selected in S3 and the current frame and the key frame point cloud set, and globally optimizes all key frame postures and point clouds in the loop. S5 repeats S3-S4 to iterate loop detection and global optimization one by one, and iteratively outputs a three-dimensional point cloud with optimized accuracy.

2. The laser radar SLAM multi-loop detection and iterative optimization method according to claim 1, It is characterized in that In S1, the drone / vehicle system includes a drone / vehicle platform, a laser radar, a video camera, a control ground station and a situation ground station. The drone can be equipped with an array laser radar to orthogonally scan the monitoring area, and the unmanned vehicle can be equipped with a mechanical laser radar to scan the monitoring area 360°; the control ground station controls the drone / vehicle and the laser radar and the video camera to scan the monitoring area according to the set route and posture. The situation ground station receives and processes laser point clouds and video images in real time, automatically generates three-dimensional point clouds and real-life models, and is equipped with intelligent recognition and analysis function modules based on texture, temperature, geometric features and spatial variables of dynamic videos and three-dimensional models.

3. The laser radar SLAM multi-loop detection and iterative optimization method according to claim 1, It is characterized in that In S1, the method for designing a multi-loop route for the drone / vehicle system is to first diagonally penetrate the monitoring area from a short-range waypoint to a diagonally long-range waypoint, and then return to the short-range waypoint in a bow-shaped route. The return route intersects with the through route multiple times to construct multiple loops and detection points.

4. The laser radar SLAM multi-loop detection and iterative optimization method according to claim 1, It is characterized in that In S2, the method for extracting the feature points of the current frame and calculating the global pose is to process the point cloud data of the current frame with the SLAM algorithm, extract the corner point features and surface point features in the point cloud, perform inter-frame odometer pose estimation based on the feature points, and obtain the optimal pose estimation of the inter-frame changes and the global pose of the current frame. The method for extracting corner point features and surface point features in the point cloud is to calculate the curvature of each point. Points with curvatures less than the set corner point threshold are corner points, and points with curvatures greater than the set surface point threshold are surface points. The curvature formula is: Where S is the surrounding point set, k is the kth frame, i is the point in the frame, and j is a point in the surrounding point set. The method of inter-frame odometer pose estimation is to convert the corner points of the current frame to the point cloud coordinate system of the previous frame through the IMU pre-integration result, establish a kd-tree for all the corner points of the previous frame, perform nearest neighbor search in the kd-tree for the converted corner points of the current frame, perform straight line fitting on the nearest neighbor points, construct the residual through the point-to-line distance, take the point-to-line direction as the residual direction, and establish an optimization constraint. The formula for the point-to-line distance is: in and are the coordinates of points i, j, and l in the lidar coordinate system {L} respectively. Similarly, the surface points of the current frame are projected to the point cloud coordinate system of the previous frame through the IMU pre-integration results, and a kd-tree is established for all the surface points of the previous frame. The nearest neighbor search is performed on the surface points after the current frame conversion in the kd-tree, and the nearest neighbor points are fitted on the plane. The residual is constructed through the point-to-plane distance, and the point-to-plane direction is used as the residual direction to establish the optimization constraint. The formula for the point-to-plane distance is: in is the coordinate of point m in the laser radar coordinate system {L}. The residual and direction of the feature points can be iteratively solved through the Gauss-Newton gradient descent method to obtain the optimal pose estimation for inter-frame changes, which includes 6 state quantities, namely translation x, y, z and attitude angles roll, pitch, and yaw. The superposition of the pose of each frame is the global pose of the corresponding frame.

5. The laser radar SLAM multi-loop detection and iterative optimization method according to claim 1, It is characterized in that In S2, the method for selecting the key frame is to calculate the distance or time difference between the current frame and the previous key frame, and select the current frame that is greater than a set threshold as a new key frame. The formula for the distance between the current frame and the previous key frame is: Where Δd is the distance between the current frame and the previous key frame, (x, y, z) is the global position coordinate of the current frame, and (x′, y′, z′) is the global position coordinate of the previous key frame.

6. The laser radar SLAM multi-loop detection and iterative optimization method according to claim 1, It is characterized in that In S3, the loop detection method is to establish a kd-tree for the coordinates of all current frames and key frames within the set radius of the detection point to perform nearest neighbor search, match and select the current frame and key frame whose distance to the detection point is less than a threshold and whose time meets the requirements, and construct a detection loop. The distance and time between the current frame and the key frame are calculated as follows: Where (x, y, z) is the position coordinate of the current frame, (x′, y′, z′) is the position coordinate of the key frame, and D min is the distance threshold, t is the current frame time, t′ is the key frame time, T min is the time threshold.

7. The laser radar SLAM multi-loop detection and iterative optimization method according to claim 1, It is characterized in that In S4, the method for solving the relative position change relationship between the detection point matching the current frame and the key frame is to use the ICP (Iterative Closest Point) iterative closest point method to calculate the rotation matrix R and the translation vector t so that the following formula is minimized: where x i is the point in the point cloud of the current frame, p i is the key frame x i The nearest neighbor of .

8. The laser radar SLAM multi-loop detection and iterative optimization method according to claim 1, It is characterized in that In S4, the method for globally optimizing the key frame pose and its point cloud is to use the GTSAM (Georgia Tech Smoothing and Mapping) graph optimization method, and superimpose the relative pose transformation of the current frame and the key frame point cloud on the key frame pose, which is the pose of the current frame in the map coordinate system calculated by loop detection. There will be a certain deviation between this pose and the pose of the current frame in the map coordinate system superimposed by the odometer, and the deviation is used to adjust the pose of all key frames in the loop. The specific optimization is carried out using the GTSAM open source graph optimization library, and the inter-frame constraints obtained by the loop detection are added to the factor graph. The confidence of the loop is the ICP score, and the isam is updated to obtain the best estimated coordinates after global optimization of all key frames of the loop. Then, the three-dimensional point cloud model of all key frames of the loop after global optimization is generated according to the best estimated coordinates after global optimization.

9. An electronic device, comprising a processor and a memory storing execution instructions, wherein when the processor executes the execution instructions stored in the memory, the processor executes the laser radar SLAM multi-loop detection and iterative optimization method as described in any one of claims 1 to 8.

Citation Information

Cited By

  • Indoor loopback detection method and system based on angle characteristics

    CN120198419A

  • Semi-distributed cross-modal relative positioning method and system

    CN121095343A

  • Unmanned aerial vehicle inspection loopback detection method based on semantic triangle descriptor

    CN121721656A

  • Laser SLAM (Simultaneous Localization and Mapping) navigation method for point cloud processing and pose optimization two-way closed-loop adjustment

    CN121804453A