Active SLAM system, method and equipment based on laser inertial navigation fusion

Abstract

This invention relates to an active SLAM system, method, and device based on laser-inertial navigation fusion, belonging to the field of active SLAM technology. It addresses the problem of low mapping quality and exploration efficiency in structured scenes when using laser-inertial navigation fusion-based active SLAM technology. The system comprises: a laser-inertial navigation SLAM system including: point cloud preprocessing, which preprocesses the point cloud and sends the preprocessed data to a laser-inertial navigation odometry (LINOMA); the LINOMA matches the extracted point cloud with the pose provided by the IMU as an initial value, iteratively optimizes to obtain a coarse pose estimate, and sends it to the backend map building and optimization; the backend map building and optimization matches the coarse pose to complete point cloud map construction and pose optimization; loop closure detection is used for map relocalization to optimize pose and ensure global map consistency. The autonomous exploration decision-making system can plan paths with expandable boundary coverage. This invention is suitable for scene exploration in areas with weak communication signals and where real-time direct control of mobile robots is not possible.

Description

Technical Field

[0001] This application relates to the field of active SLAM technology, and in particular to an active SLAM system based on laser-inertial navigation fusion. Background Technology

[0002] Active Simultaneous Localization and Mapping (SLAM) is one of the important development directions in the robotics industry. It is mainly used to explore unknown environments such as underground mines and Mars in scenarios where communication signals are weak and mobile robots cannot be directly operated in real time, so as to provide a priori environment for the execution of subsequent tasks.

[0003] Currently, active SLAM technology based on laser-inertial navigation fusion has achieved relatively mature results. However, when facing structured scenes, both mapping quality and exploration efficiency suffer a significant decline. This is because the inter-frame features of the point cloud extracted in structured scenes are too consistent, making it impossible to effectively achieve accurate localization and build a point cloud map. Consequently, the point cloud map built by the exploration robot is severely distorted due to localization errors, making it unable to complete the exploration task. Furthermore, the path planning algorithm based on exploration is not efficient enough and is prone to getting trapped in local optima, which can also prevent the robot from completing the exploration task. Summary of the Invention

[0004] The purpose of this invention is to address the problem that existing active SLAM technologies based on laser-inertial navigation fusion suffer from low mapping quality and exploration efficiency when facing structured scenes, and to provide an active SLAM system, method, and device based on laser-inertial navigation fusion.

[0005] The present invention is achieved through the following technical solution. In one aspect, the present invention provides an active SLAM system based on laser-inertial navigation fusion, the system comprising a real-time data acquisition system, a laser-inertial navigation SLAM system, an autonomous exploration and decision-making system, and a low-level control system.

[0006] The real-time data acquisition system synchronizes the data collected by different sensors in time and converts the parameters in different coordinate systems, and sends the processed data to the laser inertial navigation SLAM system.

[0007] The laser inertial navigation SLAM system includes point cloud preprocessing, laser inertial navigation odometry, backend map construction and optimization, and loop closure detection.

[0008] The point cloud preprocessing preprocesses the point cloud and sends the preprocessed data to the laser inertial navigation odometer.

[0009] The laser inertial navigation odometry matches the extracted point cloud with the current robot pose as the initial value, and obtains a coarse pose estimate through continuous iterative optimization and sends it to the backend map building and optimization.

[0010] The backend map construction and optimization are based on coarse pose matching to complete point cloud map construction and pose optimization;

[0011] The loop closure detection is used for map relocalization to optimize pose and ensure global map consistency;

[0012] The autonomous exploration decision system receives the positioning information and point cloud map output by the laser inertial navigation SLAM system. It then randomly generates sampling viewpoints based on surface coverage, calculates the distance between viewpoints through global path planning, and obtains the optimal exploration order. In the local exploration map, it defines two points where the global path planning intersects as boundary points. In the local exploration area, it implements local path planning through DWA and travels along the two boundary points to enter the next exploration area.

[0013] The underlying control system moves smoothly based on the trajectory generated by the optimal velocity space sampled by DWA. As the environment moves, it is updated in real time to ensure the collection and updating of system environment data, thereby achieving the goal of autonomous exploration and mapping.

[0014] Furthermore, the laser inertial navigation odometry is used for feature point extraction, specifically including:

[0015] For the initial point cloud, feature extraction is performed using feature points, which are divided into two types: planar points and corner points;

[0016] Points on the same scanning line are sorted according to curvature. The same laser scanning line is divided into several segments, each segment being a sub-region. The points with the largest curvature in each sub-region are marked as corner points, and the remaining points are planar points.

[0017] Furthermore, the laser inertial navigation odometry is used to generate key points for diagonal points using a K-Means clustering algorithm, specifically including:

[0018] Step 1: Divide the coordinate system of the lidar into several sectors with the origin as the origin, and then divide the points into the corresponding sectors in sequence by calculating the horizontal angle of the point cloud in each frame of data.

[0019] Step 2: Using the initial point of each sector as the centroid of the first cluster of that sector, determine whether the loss function and Manhattan distance between the subsequent point cloud of that sector and the centroid of the previous cluster exceed the threshold.

[0020] Step 3: If the threshold is not exceeded, add the point to the cluster; if the threshold is exceeded, add the point to a new cluster in the sector; discard the outlier.

[0021] Step 4: For subsequent sectors, repeat steps 2 and 3 to filter out those with fewer point clouds in the cluster;

[0022] Step 5: When the loss function and Manhattan distance between clusters are less than the threshold, cluster merging is performed; finally, the key points are obtained by averaging all point clouds in each cluster.

[0023] Furthermore, the laser inertial navigation odometry is used for descriptor generation and matching for key points, specifically including:

[0024] The cluster centroid is divided into several sectors centered on the current centroid, with each sector corresponding to a dimension. The distance from the nearest centroid to the current centroid is then selected as the value for each dimension to generate a descriptor. The direction vector from the current centroid to the nearest centroid is taken as the main direction and is determined as the first sector. Values ​​are assigned to each sector in a counter-clockwise direction. If a sector does not have a centroid, it is set to 0.

[0025] Search for a certain number of nearest key points, generate a certain number of descriptors for each point, and then take the value of the corresponding nearest key point as the final descriptor for each dimension.

[0026] Establish a vector table to avoid repeatedly calculating distance and direction for corresponding points;

[0027] Finally, the confidence level is incremented by one if the difference between the non-zero elements in the descriptors is below a certain threshold, and if the confidence level is above the matching threshold, the match is considered successful.

[0028] Furthermore, the loop closure detection is used to perform coarse matching using Euclidean distance, followed by fine matching using the Scan-Context descriptor to complete the loop closure, specifically including:

[0029] First, point cloud segmentation is performed to generate descriptors. Using the lidar coordinate system as a reference, a frame of 3D lidar point cloud is uniformly divided into different bin segments in the radial and rotational directions. In the radial direction, the point cloud is divided into several uniformly distributed rings, and in the rotational direction, the point cloud is divided into several uniformly distributed sectors. The overlapping area of ​​the two is the bin segment.

[0030] Assign a number to each bin segment as a parameter to form the descriptor matrix. This parameter is determined by the z-value of the highest point in the point cloud of each bin segment. If there are no points in the bin segment, assign a value of 0.

[0031] The segmented point cloud is unfolded to form a descriptor matrix, which is the Scan-Context descriptor;

[0032] Historical frames are filtered using a large threshold based on Euclidean distance, and a one-dimensional vector is generated for each descriptor.

[0033] The one-dimensional vector is stored in a Kd-Tree and subjected to nearest neighbor search with the one-dimensional vectors in historical frames. After finding several historical frames that meet the threshold and completing the initial search, a precise search is performed based on the following formula:

[0034]

[0035] Where Iq and Ic represent the current frame descriptor matrix and the historical frame descriptor matrix, respectively. and Let N represent the column vector of the j-th column in the current frame and the column vector of the j-th column in a historical frame, respectively. s The number of sectors into which the point cloud is divided, and d(Iq, Ic) is the best similarity score between the current frame descriptor matrix and the historical frame descriptor matrix;

[0036] Perform N operations on the historical frame descriptor matrix s Translate once, then perform N translations. s To ensure rotation invariance, the same operation is performed on n historical frames. The minimum d(Iq,Ic) value is selected as the best similarity score between the historical frame and the current frame. If the d(Iq,Ic) value is less than the threshold, the loop closure is satisfied. ICP iterative optimization is performed on the historical frame and the current frame. After finding the optimal pose, the pose near the loop closure is optimized. If the d(Iq,Ic) value is greater than the threshold, the loop closure is not satisfied, and no loop closure optimization is performed.

[0037] Furthermore, the local path planning in the autonomous exploration decision-making system specifically includes:

[0038] The cost function for each segment point is defined as follows:

[0039]

[0040] Among them, l j Let p be the length of the j-th path segment, and p be the penalty for each stop.

[0041] For each pair of viewpoints, an improved A* algorithm is used to calculate the shortest path between viewpoints. This improved A* algorithm includes an adaptive weight cost function proposed for the A* algorithm.

[0042] f(n) = g(n) + α(x)h(n)

[0043] in, l represents the Euclidean distance between the current point and the starting point, L represents the Euclidean distance between the ending point and the starting point, g(n) represents the distance between the current node and the starting node, and h(n) represents the Euclidean distance between the current node and the target node.

[0044] Construct a distance matrix based on the shortest path, solve the problem of finding the shortest path through each viewpoint, and determine the optimal access order for the viewpoint set.

[0045] All viewpoints are set as segment points. The path cost is calculated iteratively after each viewpoint is set as a continuous point. The optimal cost function is obtained, and an improved DWA algorithm is used to generate the optimal velocity space to obtain the optimal trajectory between viewpoints, confirming the optimal smooth path. The improved DWA algorithm specifically includes:

[0046] Improved DWA evaluation function:

[0047] G(v,w)=σ[αheading(v,w)+βdist(v,w)+γvel(v,w)+δenddist(v,w)+etaallgap(v,w)]

[0048] Where enddist(v, w) is the distance evaluation sub-function for the target, allgap(v, w) is the distance global path interval evaluation sub-function, heading(v, w) represents the orientation angle evaluation sub-function, dist(v, w) represents the distance evaluation sub-function between the obstacle and the robot body, vel(v, w) represents the current linear velocity evaluation sub-function, α, β, γ, δ, and μ represent the weights of heading(v, w), dist(v, w), vel(v, w), enddist(v, w), and allgap(v, w) in the evaluation function, and σ represents the normalization factor.

[0049] Furthermore, the global path planning in the autonomous exploration decision-making system specifically includes:

[0050] The improved A* algorithm is used to generate the path cost between the unexplored and currently explored local sub-maps, and to generate the distance matrix between them;

[0051] To solve the TSP problem in this distance matrix, the points on the global path that intersect with the currently explored local submap are defined as boundary point 1 and boundary point 2; local path planning is applied between boundary point 1 and boundary point 2 to achieve two-stage planning.

[0052] Once the current local sub-map exploration is complete, the local path is simplified again to the shortest path from the current viewpoint to boundary point 1 and boundary point 2, so that we can return to global path planning. Based on this, the robot moves to the next exploration point to perform the exploration task.

[0053] Secondly, the present invention provides an active SLAM method for laser-inertial navigation fusion, the method comprising performing active SLAM using an active SLAM system for laser-inertial navigation fusion as described above.

[0054] Thirdly, the present invention provides a computer device including a memory and a processor, wherein the memory stores a computer program, and when the processor runs the computer program stored in the memory, it performs the steps of an active SLAM method for laser inertial navigation fusion as described above.

[0055] Fourthly, the present invention provides a computer-readable storage medium storing a plurality of computer instructions, the plurality of computer instructions being used to cause a computer to execute an active SLAM method for laser-inertial navigation fusion as described above.

[0056] The beneficial effects of this invention are:

[0057] This invention first addresses the problem that the inter-frame features of point clouds extracted in structured scenes are too consistent, hindering accurate localization and point cloud map construction. This application proposes improving the corner feature matching in the front-end matching process by replacing it with descriptor matching, thereby enhancing the efficiency of inter-frame matching in front-end odometry. Simultaneously, addressing the issue of increasing pose accumulation error as the robot's travel distance grows, and the inability of Euclidean distance-based loop closure detection to accurately relocalize and optimize pose and map, this invention proposes a loop closure detection method that integrates Euclidean distance and Scan-Context to reduce the impact of accumulated errors on poor global map consistency. Figure 5 and Figure 6 As shown.

[0058] This invention addresses the inefficiencies and susceptibility to local optima in exploration-based path planning algorithms by proposing an autonomous exploration method based on two-stage planning. Specifically, for local path planning, the traditional sliding window method tends to select a locally optimal trajectory rather than the globally optimal one when obstacles are present, leading to deviations from the global path and increased time to reach the target point. To address this, a cost function for both the target distance and the global path planning distance is added to the sliding window method to optimize the trajectory and global path constraints. Furthermore, to address the time-consuming search issue of the traditional A* (A* Star) algorithm in global path planning, adaptive weights are added to the heuristic function, and a jump-point strategy is incorporated into the A* algorithm to accelerate the search. Figure 7 and Figure 8 As shown.

[0059] The laser inertial navigation SLAM system module of this invention addresses the problem that in structured scenes, the inter-frame features of the point cloud extracted are too consistent, making it impossible to effectively achieve accurate localization and point cloud map construction. This leads to severe distortion of the point cloud map constructed by the exploration robot due to localization errors. The implementation of active SLAM methods requires the mobile robot to possess both localization and mapping capabilities and the ability to generate exploration paths; neither is dispensable. The prerequisite for generating exploration paths is having a map of the current environment and the robot's current pose information. The autonomous exploration decision-making system of this invention provides real-time updates to the map of unknown scene environments and, given the currently known map information, the ability to plan paths with expandable boundary coverage.

[0060] This invention is applicable to exploration of unknown environments such as underground mines and Mars, where communication signals are weak and mobile robots cannot be directly controlled in real time, in order to provide a priori environment for the execution of subsequent tasks. Attached Figure Description

[0061] To more clearly illustrate the technical solution of this application, the drawings used in the embodiments will be briefly introduced below. Obviously, for those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0062] Figure 1 This is a flowchart of the loop closure detection process based on the fusion of Euclidean distance and Scan-Context in this invention.

[0063] Figure 2 This is a point cloud partitioning diagram of the present invention;

[0064] Figure 3 This is the descriptor matrix generation diagram of the present invention;

[0065] Figure 4 This is a schematic diagram of local path planning;

[0066] Figure 5 This is a partial mapping effect of the present invention;

[0067] Figure 6 This is the overall mapping effect of the present invention;

[0068] Figure 7 This represents the overall exploratory results of the present invention;

[0069] Figure 8 This is a diagram illustrating the exploration trajectory of the present invention.

[0070] Figure 9 A schematic diagram illustrating the effects of an active SLAM experiment in a small warehouse scenario;

[0071] Figure 10 This is a schematic diagram illustrating the effects of an active SLAM experiment in a large corridor scene. Detailed Implementation

[0072] Embodiments of the present invention are described in detail below, examples of which are shown in the accompanying drawings, wherein the same or similar reference numerals denote the same or similar elements or elements having the same or similar functions throughout. The embodiments described below with reference to the accompanying drawings are exemplary and intended to explain the present invention, and should not be construed as limiting the present invention.

[0073] Implementation Method 1: An active SLAM system based on laser-inertial navigation fusion, the system comprising a real-time data acquisition system, a laser-inertial navigation SLAM system, an autonomous exploration and decision-making system, and a low-level control system;

[0074] The real-time data acquisition system is responsible for data tuning, synchronizing data collected by different sensors in time, converting parameters in different coordinate systems, and smoothly sending the processed data to the laser inertial navigation SLAM system. This part must have real-time stability to ensure the continuous operation of subsequent algorithms.

[0075] The laser inertial navigation SLAM system consists of four parts: point cloud preprocessing, laser inertial navigation odometry, backend map building and optimization, and loop closure detection.

[0076] First, the sensor data is sent to the point cloud preprocessing unit. After downsampling, ground separation, and feature extraction, the point cloud data is sent to the laser inertial navigation odometry unit.

[0077] This section uses the pose provided by the IMU as an initial value to match the extracted point cloud, and obtains a coarse pose estimate through continuous iteration and optimization before sending it to the backend.

[0078] Then, the backend completes the point cloud map construction and pose optimization based on the ScanToMap matching of the coarse pose.

[0079] Loop closure detection is used for map relocalization to optimize pose and ensure global map consistency.

[0080] The autonomous exploration decision-making system receives positioning information and point cloud maps output by the laser inertial navigation SLAM system. It then randomly generates sampling viewpoints based on surface coverage, calculates the distance between viewpoints through global path planning, and determines the optimal exploration order by solving the traveling salesman problem. In the local exploration map, it defines two points where the global path planning intersects as boundary points. Local exploration is carried out in the local exploration area through DWA. When the local exploration ends, it moves along the two boundary points to enter the next exploration area, thus ensuring the consistency of the global path planning.

[0081] The underlying control system moves smoothly based on the trajectory generated by the optimal velocity space sampled by DWA. As the environment moves, it is updated in real time to ensure the collection and updating of system environment data, thereby achieving the goal of autonomous exploration and mapping, and realizing an active SLAM system.

[0082] In this embodiment, the laser inertial navigation SLAM system module solves the problem that the point cloud frame features extracted in structured scenes are too consistent, making it impossible to effectively achieve accurate positioning and build point cloud maps, resulting in severe distortion of the point cloud map built by the exploration robot due to positioning errors.

[0083] The implementation of active SLAM requires the mobile robot to possess both localization and mapping capabilities and the ability to generate exploration paths; neither is dispensable. The prerequisite for generating exploration paths is having a map of the current environment and the robot's current pose information. An autonomous exploration decision-making system involves real-time updates of the map of the unknown scene and, given the currently known map information, the ability to plan paths with expandable boundaries.

[0084] Implementation Method Two: This implementation method further defines the active SLAM system based on laser-inertial navigation fusion described in Implementation Method One. In this implementation method, the function of the laser-inertial navigation odometry is further defined, specifically including:

[0085] The laser inertial navigation odometry is used for feature point extraction, specifically including:

[0086] The initial point cloud uses feature points for feature extraction. Feature points are divided into two types: planar points and corner points. Planar points are points on a three-dimensional plane with small curvature. Corner points are points at the edges of three-dimensional space with large curvature. Curvature is calculated as shown in formula (1):

[0087]

[0088] Where k represents one period of the scan point, L represents the radar coordinate system, c represents the curvature of the current point i, and S is the set of consecutive points on the same laser scan line. i and j represent a certain number within the set. Let i be the coordinates of the current point i. This is the set of 5 points to the left and 5 points to the right of the current point. Points on the same scan line are sorted according to curvature c.

[0089] Before selecting feature points, some unreliable points must first be eliminated. These unreliable points are divided into two types: occlusion points and parallel beam points. After eliminating these unreliable points, feature point extraction can begin. To ensure a uniform distribution of feature points, the same laser scanning line is divided into several segments, for example, 6 segments, each segment being a sub-region. The points with the largest curvature in each sub-region (e.g., 20 points) are marked as corner points, and the remaining points are planar points.

[0090] Implementation Method 3 further defines the active SLAM system based on laser-inertial navigation fusion described in Implementation Method 2. In this implementation method, the function of the laser-inertial navigation odometry is further defined, specifically including:

[0091] Because corner points have advantages such as faster calculation speed and obvious features compared to surface points, the K-Means clustering algorithm is used to generate key points for corner points. However, the K-Means clustering algorithm is easily affected by initial values ​​and outliers, and the clustering result may not be the global optimum but the local optimum. Sample points can only be classified into a single class. Considering that when the horizontal angle of the target point cloud is within an approximate range, it is likely to come from the same object and should be clustered into the same cluster. Therefore, the coordinate system of the lidar is used as the center to divide the horizontal plane into N sectors, and then the sectors are used as units to perform clustering in order. This minimizes the loss function corresponding to the clustering result. The loss function can be defined as the sum of squared errors of each sample from the center point of its cluster, as shown in formula (2).

[0092]

[0093] Where x i Representing the i-th sample, c i It is the cluster to which xi belongs, μ ci M represents the center point of the cluster, and M is the total number of samples.

[0094] The specific steps are as follows:

[0095] 1) Divide the coordinate system of the lidar into N sectors, and then divide the points into the corresponding sectors in sequence by calculating the horizontal angle of the point cloud in each frame of data.

[0096] 2) Using the initial point of each sector as the centroid of the first cluster of that sector, determine whether the loss function and Manhattan distance between the subsequent point cloud of that sector and the centroid of the previous cluster exceed the threshold.

[0097] 3) If the threshold is not exceeded, add the point to the cluster; if the threshold is exceeded, add a new cluster to the sector; discard abnormal points.

[0098] 4) Repeat steps 2 and 3 for subsequent sectors, and finally filter out the clusters with fewer point clouds.

[0099] 5) When the loss function and Manhattan distance between clusters are less than the threshold, cluster merging is performed. Finally, the centroid of the point cloud in each cluster is calculated by averaging all points in the cluster, which is the key point.

[0100] Implementation Method Four: This implementation method further defines the active SLAM system based on laser-inertial navigation fusion described in Implementation Method Three. In this implementation method, the function of the laser-inertial navigation odometry is further defined, specifically including:

[0101] First, the cluster centroids are divided into N sectors centered on the current centroid M0, where each sector corresponds to a dimension. Then, the distance to the nearest centroid M0 in each sector is selected as the value of each dimension, thereby generating a descriptor.

[0102] To ensure the invariance of the search principal direction orientation, the direction vector from the current point M0 to the nearest centroid M1 is taken as the principal direction and is defined as the first sector. Values ​​are then assigned to each sector in a counter-clockwise direction. If a sector does not have a centroid, the value is set to 0.

[0103] From the current point to other points M i The direction of (i≠1) is represented as and main direction The angle between them determines k i Which sector does it belong to? The formula for calculating this angle is as follows:

[0104]

[0105] Where D i The definition is as follows:

[0106]

[0107] To mitigate interference from abnormal key points, a certain number of nearest key points are searched, and a certain number of descriptors are generated for each point. Then, for each dimension, the value of the nearest key point is taken as the final descriptor. A vector table is established to avoid recalculating distance and direction for corresponding points. Finally, the difference between the non-zero elements of the descriptors is calculated; if it is lower than a certain threshold, the confidence level is incremented by one; if the confidence level is higher than the matching threshold, a match is considered successful.

[0108] After successful matching, the inter-frame motion of the lidar is solved using Gauss-Newton or Levenberg-Marquardt Algorithm (LM) nonlinear optimization. The initial optimization uses ground point features [t]. z ,θ roll ,θ pitch Then, optimization is performed using corner descriptors and face matching between non-ground points [t]. x ,t y ,θ yaw This allows the acquisition of the robot's relative pose change to generate a front-end odometry.

[0109] Implementation method five is a further definition of the laser-inertial navigation fusion active SLAM system described in implementation method one. In this implementation method, the loop closure detection is further defined, specifically including:

[0110] The loop closure detection is used to perform coarse matching using Euclidean distance, followed by fine matching using Scan-Context descriptors to complete the loop closure. Specifically, it includes:

[0111] First, point cloud segmentation is performed to generate descriptors. Using the lidar coordinate system as a reference, a frame of 3D lidar point cloud is uniformly divided into different bin segments in the radial and rotational directions. In the radial direction, the point cloud is divided into several uniformly distributed rings, and in the rotational direction, the point cloud is divided into several uniformly distributed sectors. The overlapping area of ​​the two is the bin segment.

[0112] Assign a number to each bin segment as a parameter to form the descriptor matrix. This parameter is determined by the z-value of the highest point in the point cloud of each bin segment. If there are no points in the bin segment, assign a value of 0.

[0113] It should be noted that the data collected by the lidar sensor is a point cloud, which stores the values ​​of the (x, y, z) three-axis coordinate system and other data. The z value is the value on the z-axis. Since this lidar is placed horizontally, it is equivalent to the height value of the point cloud.

[0114] The segmented point cloud is unfolded to form a descriptor matrix, which is the Scan-Context descriptor;

[0115] Historical frames are filtered using a large threshold based on Euclidean distance, and a one-dimensional vector is generated for each descriptor.

[0116] The one-dimensional vector is stored in a Kd-Tree and subjected to nearest neighbor search with the one-dimensional vectors in historical frames. After finding several historical frames that meet the threshold and completing the initial search, a precise search is performed based on the following formula:

[0117]

[0118] Where Iq and Ic represent the current frame descriptor matrix and the historical frame descriptor matrix, respectively. and Let N represent the column vector of the j-th column in the current frame and the column vector of the j-th column in a historical frame, respectively. s The number of sectors into which the point cloud is divided, and d(Iq, Ic) is the best similarity score between the current frame descriptor matrix and the historical frame descriptor matrix;

[0119] Perform N operations on the historical frame descriptor matrix s Translate once, then perform N translations.s To ensure rotation invariance, the same operation is performed on n historical frames. The minimum d = d(Iq, Ic) value is selected as the best similarity score between the historical frame and the current frame. If the d value is less than the threshold, the loop closure is satisfied. ICP iterative optimization is performed on the historical frame and the current frame. After finding the optimal pose, the pose near the loop closure is optimized. If the d value is greater than the threshold, the loop closure is not satisfied, and no loop closure optimization is performed.

[0120] In this implementation, loop closure detection based on Scan-Context descriptors differs from loop closure detection based on Euclidean distance. Euclidean distance-based loop closure detection relies heavily on the Euclidean distance between the current frame and historical frames, depending on the pose output after SLAM backend optimization. In contrast, Scan-Context-based loop closure detection registers points based on the similarity between the current frame's point cloud and historical frame's point cloud, independent of localization results, thus offering higher efficiency. However, this method performs a loop matching process for each newly added keyframe, significantly increasing the algorithm's runtime. Therefore, a loop closure detection algorithm fusing Euclidean distance and Scan-Context is introduced. First, coarse matching is performed using Euclidean distance to reduce the number of Scan-Context matches. Then, fine matching using the Scan-Context descriptor completes the loop closure, improving efficiency. The algorithm flowchart is shown below. Figure 1 As shown.

[0121] Implementation method six is ​​a further definition of the laser-inertial navigation fusion active SLAM system described in implementation method one. In this implementation method, the local path planning in the autonomous exploration decision-making system is further defined, specifically including:

[0122] The local path planning in the autonomous exploration decision-making system specifically includes:

[0123] Local path planning involves generating an optimal exploration path that satisfies dynamic constraints from an optimal set of viewpoints. However, relying solely on the extracted set of viewpoints cannot guarantee the smoothness and continuity of the path. Figure 4 As shown. Figure 4 The green box represents a local sub-map maintained by global path planning, while the brown and blank circles represent extracted viewpoints. The brown circles represent continuous viewpoints, and the blank circles represent segmented viewpoints. Due to the existence of segment points, the robot must stop at each segment point and change direction to reach the next viewpoint while maintaining the optimal path. Therefore, to minimize the path cost, the cost function for each segment point is defined as follows:

[0124]

[0125] Among them, l jLet be the length of the j-th path segment, and p represent the penalty for each stop.

[0126] For each pair of viewpoints, the improved A* algorithm is used to calculate the shortest path between viewpoints;

[0127] The node selection result of the overall evaluation function is greatly influenced by the heuristic function. When the current point is far from the target point, h(n) is large. Although it can guide the robot to move towards the target point, it cannot determine the shortest path. When the current point is close to the target point, h(n) is small. Although it guarantees the shortest path, the lack of distance constraints to the target point reduces search efficiency and increases the search time for the target point. Therefore, an adaptive weight cost function is proposed for the A* algorithm:

[0128] f(n) = g(n) + α(x)h(n)

[0129] in, l represents the Euclidean distance between the current point and the starting point, L represents the Euclidean distance between the ending point and the starting point, g(n) is the actual cost function representing the distance between the current node and the starting node, and h(n) is the heuristic function representing the Euclidean distance between the current node and the target node.

[0130] The weights are introduced using a standard normal function, where l represents the Euclidean distance between the current point and the starting point, and L represents the Euclidean distance between the ending point and the starting point. In this case, when the current point is far from the target point, h(n) will not be too large, which can both drive the robot to move towards the target point and determine the shortest path. When the current point is close to the target point, h(n) is small but not too small, which can both ensure the shortest path and the distance constraint of the target point, thus ensuring search efficiency and algorithm running time.

[0131] Construct a distance matrix based on the shortest path, solve the problem of finding the shortest path through each viewpoint, and determine the optimal access order for the viewpoint set.

[0132] All viewpoints are set as segment points. The path cost is calculated iteratively after each viewpoint is set as a continuous point. The optimal cost function is obtained. The improved DWA algorithm is used to generate the optimal velocity space to obtain the optimal trajectory between viewpoints and confirm the optimal smooth path.

[0133] Traditional DWA algorithms only require sequentially reaching the path points planned by the global path planning to complete the local path planning task and reach the target point. However, when obstacles exist, the DWA algorithm will take the safest approach, choosing a trajectory farthest from the obstacle, which deviates from the globally planned trajectory and increases the time to reach the target point. Therefore, an improvement to the traditional DWA algorithm is made by adding a distance-to-target evaluation function and a distance-to-global-path-interval evaluation function to the traditional evaluation function. The improved DWA algorithm specifically includes:

[0134] Improved DWA evaluation function:

[0135] G(v,w)=σ[αheading(v,w)+βdist(v,w)+γvel(v,w)

[0136] +δenddist(v,w)+ηallgap(v,w)]

[0137] Where enddist(v, w) is the distance evaluation sub-function for the target, allgap(v, w) is the distance global path interval evaluation sub-function, heading(v, w) represents the orientation angle evaluation sub-function, dist(v, w) represents the distance evaluation sub-function between the obstacle and the robot body, vel(v, w) represents the current linear velocity evaluation sub-function, α, β, γ, δ, and μ represent the weights of heading(v, w), dist(v, w), vel(v, w), enddist(v, w), and allgap(v, w) in the evaluation function, and σ represents the normalization factor.

[0138] Implementation method seven is a further definition of the laser-inertial navigation fusion active SLAM system described in implementation method six. In this implementation method, the global path planning in the autonomous exploration decision-making system is further defined, specifically including:

[0139] The global path planning in the autonomous exploration decision-making system specifically includes:

[0140] The improved A* algorithm is used to generate the path cost between the unexplored and currently explored local sub-maps, and to generate the distance matrix between them;

[0141] To solve the TSP problem in this distance matrix, the points on the global path that intersect with the currently explored local submap are defined as boundary point 1 and boundary point 2; local path planning is applied between boundary point 1 and boundary point 2 to achieve two-stage planning.

[0142] Once the current local sub-map exploration is complete, the local path is simplified again to the shortest path from the current viewpoint to boundary point 1 and boundary point 2, so that we can return to global path planning. Based on this, the robot moves to the next exploration point to perform the exploration task.

[0143] In this implementation, the global path planning divides the map into local sub-maps, which are categorized into three states: "unexplored," "still being explored," and "exploration complete." These states are distinguished by the "surfaces" stored within each local sub-map. A local sub-map containing uncovered surfaces is in the "still being explored" state; containing only covered surfaces is in the "exploration complete" state; and an empty local sub-map is in the "unexplored" state. The goal of global path planning is to generate an optimal path from the current LiDAR point cloud map, traversing the center points of all local sub-maps and the current viewpoint. This point cloud map and the generated path are updated in real-time as the robot moves.

[0144] Implementation method eight is an embodiment of an active SLAM system for laser-inertial navigation fusion as described above, specifically including:

[0145] The real-time data acquisition section is responsible for data tuning, synchronizing data collected by different sensors in time, converting parameters in different coordinate systems, and smoothly sending the processed data to the laser inertial navigation SLAM system. This section must have real-time stability to ensure the continuous operation of subsequent algorithms.

[0146] The laser inertial navigation SLAM system consists of four parts: point cloud preprocessing, laser inertial navigation odometry, backend map construction and optimization, and loop closure detection. The system first sends sensor data to point cloud preprocessing, which performs downsampling, ground separation, and feature extraction on the point cloud before sending it to the laser inertial navigation odometry. This part uses the robot's current pose provided by the IMU as an initial value for matching the extracted point cloud. Through continuous iterative optimization, a coarse pose estimate is obtained and sent to the backend. Then, the backend uses ScanToMap matching based on the coarse pose to complete point cloud map construction and pose optimization. Loop closure detection is used for map relocalization to optimize pose and ensure global map consistency.

[0147] Furthermore, the laser inertial navigation odometry includes a feature point extraction method, specifically including:

[0148] The initial point cloud uses feature points for feature extraction. Feature points are divided into two types: planar points and corner points. Planar points are points on a three-dimensional plane with small curvature. Corner points are points at the edges of three-dimensional space with large curvature. Curvature is calculated as shown in formula (1):

[0149]

[0150] Where k represents one period of the scan point, L represents the radar coordinate system, c represents the curvature of the current point i, and S is the set of consecutive points on the same laser scan line. i and j represent a certain number within the set. Let i be the coordinates of the current point i. This is the set of 5 points to the left and 5 points to the right of the current point. Points on the same scan line are sorted according to curvature c.

[0151] Before selecting feature points, some unreliable points must first be eliminated. These unreliable points are divided into two types: occlusion points and parallel beam points. After eliminating these unreliable points, feature point extraction can begin. To ensure a uniform distribution of feature points, the same laser scanning line is divided into 6 segments, each segment being a sub-region. The 20 points with the largest curvature in each sub-region are marked as corner points, and the remaining points are planar points.

[0152] Furthermore, the laser inertial navigation odometry also includes improved keypoint generation, specifically including:

[0153] Because corner points have advantages such as faster calculation speed and obvious features compared to surface points, the K-Means clustering algorithm is used to generate key points for corner points. However, the K-Means clustering algorithm is easily affected by initial values ​​and outliers, and the clustering result may not be the global optimum but the local optimum. Sample points can only be classified into a single class. Considering that when the horizontal angle of the target point cloud is within an approximate range, it is likely to come from the same object and should be clustered into the same cluster. Therefore, the coordinate system of the lidar is used as the center to divide the horizontal plane into N sectors, and then the sectors are used as units to perform clustering in order. This minimizes the loss function corresponding to the clustering result. The loss function can be defined as the sum of squared errors of each sample from the center point of its cluster, as shown in formula (2).

[0154]

[0155] Where x i Representing the i-th sample, c i It is x i The cluster to which it belongs, μ ci M represents the center point of the cluster, and M is the total number of samples.

[0156] The specific steps are as follows:

[0157] 1) Divide the coordinate system of the lidar into N sectors, and then divide the points into the corresponding sectors in sequence by calculating the horizontal angle of the point cloud in each frame of data.

[0158] 2) Using the initial point of each sector as the centroid of the first cluster of that sector, determine whether the loss function and Manhattan distance between the subsequent point cloud of that sector and the centroid of the previous cluster exceed the threshold.

[0159] 3) If the threshold is not exceeded, add the point to the cluster; if the threshold is exceeded, add the point to the new cluster in the sector; discard outliers. Repeat steps 2 and 3 for subsequent sectors, and finally filter the clusters with fewer point clouds.

[0160] 4) When the loss function and Manhattan distance between clusters are less than the threshold, cluster merging is performed. Finally, the centroid of the point cloud in each cluster is calculated by averaging all points in the cluster, which is the key point.

[0161] Furthermore, the laser inertial odometry also includes improved descriptor generation and matching, specifically including:

[0162] First, the cluster centroids are divided into N sectors centered on the current centroid M0, where each sector corresponds to a dimension. Then, the distance to the nearest centroid M0 in each sector is selected as the value of each dimension, thereby generating a descriptor.

[0163] To ensure the invariance of the search principal direction orientation, the direction vector from the current point M0 to the nearest centroid M1 is taken as the principal direction and is defined as the first sector. Values ​​are then assigned to each sector in a counter-clockwise direction. If a sector does not have a centroid, the value is set to 0.

[0164] From the current point to other points M i The direction of (i≠1) is represented as and main direction The angle between them determines k i Which sector does it belong to? The formula for calculating this angle is as follows:

[0165]

[0166] Where D i The definition is as follows:

[0167]

[0168] To mitigate interference from abnormal key points, a certain number of nearest key points are searched, and a certain number of descriptors are generated for each point. Then, for each dimension, the value of the nearest key point is taken as the final descriptor. A vector table is established to avoid recalculating distance and direction for corresponding points. Finally, the difference between the non-zero elements of the descriptors is calculated; if it is lower than a certain threshold, the confidence level is incremented by one; if the confidence level is higher than the matching threshold, a match is considered successful.

[0169] After successful matching, the inter-frame motion of the lidar is solved using Gauss-Newton or Levenberg-Marquardt Algorithm (LM) nonlinear optimization. The initial optimization uses ground point features [t]. z ,θ roll ,θpitch Then, optimization is performed using corner descriptors and face matching between non-ground points [t]. x ,t y ,θ yaw This allows the acquisition of the robot's relative pose change to generate a front-end odometry.

[0170] Furthermore, loop closure detection specifically includes:

[0171] Loop closure detection based on Scan-Context descriptors differs from loop closure detection based on Euclidean distance. Euclidean distance-based loop closure relies heavily on the pose output after SLAM backend optimization, as it calculates the Euclidean distance between the current and historical frames, making it highly dependent on the localization results. Scan-Context-based loop closure, however, registers points based on the similarity between the current and historical frame points, independent of localization results, thus offering higher efficiency. However, this method performs a loop matching process for each newly added keyframe, significantly increasing runtime. Therefore, a loop closure detection algorithm fusing Euclidean distance and Scan-Context is introduced. First, coarse matching using Euclidean distance reduces the number of Scan-Context matches, followed by fine matching using the Scan-Context descriptor to complete the loop closure, thus improving efficiency. The algorithm flowchart is shown below. Figure 1 As shown, the specific method is as follows:

[0172] First, point cloud partitioning is performed to generate descriptors, such as... Figure 2 As shown, using the lidar coordinate system as a reference, a frame of 3D lidar point cloud is uniformly divided into different bin segments in both radial and rotational directions. This division method better adapts to the sparsity of the point cloud; that is, the point cloud closer to the lidar is denser, and the point cloud farther away from the lidar is sparser. Specifically, from the radial direction, the point cloud is divided into N... r The point cloud is divided into N uniformly distributed rings, as shown in yellow in the diagram, when viewed from the direction of rotation. s The points are uniformly distributed sectors, represented by the green areas in the diagram. The overlapping area between the two is the bin segment, represented by the black areas. Therefore, the segmented point cloud can be represented by the following formula:

[0173]

[0174] Where P represents a point cloud frame, P ij The 'i' represents each segment that has been divided, 'i' represents the radial direction, and 'j' represents the rotation direction.

[0175] The maximum scanning distance of a lidar is defined as L. max If the polar coordinates of the point emitted by the lidar are (r, θ), then the i and j of that point can be represented as:

[0176]

[0177]

[0178] in This means that only the integer part is retained.

[0179] After dividing the point cloud, a number needs to be assigned to each bin as a parameter for constructing the descriptor matrix. This parameter is determined by the z-value of the highest point in the point cloud of each bin. Bins without points are assigned a value of 0.

[0180]

[0181] After assignment, the partitioned point cloud can be unfolded to form a descriptive submatrix, such as... Figure 3 As shown, the descriptor matrix is ​​N. r ×N s The resulting descriptor matrix is ​​the Scan-Context descriptor.

[0182] from Figure 3 It is known that although a circle has rotational invariance, a slight change in the orientation of the lidar, leading to a change in the scanning start position, will cause a significant lateral shift in the 2D matrix after it has been transformed into a 2D matrix. Therefore, matching cannot be performed intuitively based on the difference between the two descriptors.

[0183] To address the aforementioned issues, a large threshold is first applied to historical frames based on Euclidean distance to reduce subsequent processing time. Then, based on the fact that although the matrix undergoes a lateral shift, the overall point cloud density of each row remains unchanged, a one-dimensional vector k is generated for each descriptor:

[0184] k=(ψ(r1),ψ(r2),...,ψ(r Nr (9)

[0185] in:

[0186]

[0187] Where ||r i ||0 represents the total number of non-zero values ​​in the bin section of the corresponding row.

[0188] Store the k value in a Kd-Tree and perform a nearest neighbor search with the k values ​​in historical frames to find n historical frames that meet the threshold. After completing the initial search, perform a precise search based on the following formula:

[0189]

[0190] Where Iq and Ic represent the current frame descriptor matrix and the historical frame descriptor matrix, respectively. and These represent the column vector of the j-th column in the current frame and the column vector of the j-th column in the historical frame, respectively.

[0191] Because the descriptor matrix has undergone a significant shift, the historical frame descriptor matrix is ​​subjected to N... s The second translation, i.e., performing a 2π / N shift on the historical frame point cloud. s Rotate N times, then perform N rotations. s The matching is performed to ensure rotation invariance. Then, the same operation is performed on n historical frames. The minimum d value is selected as the best similarity score between the historical frame and the current frame. If the d value is less than the threshold, the loop closure is satisfied. ICP iterative optimization is performed on the historical frame and the current frame. After finding the optimal pose, the pose near the loop closure is optimized to optimize the map. If the d value is greater than the threshold, the loop closure is not satisfied, and no loop closure optimization is performed.

[0192] The autonomous exploration decision-making system receives positioning information and point cloud maps output by the laser inertial navigation SLAM system. It then randomly generates sampling viewpoints based on surface coverage, calculates the distance between viewpoints through global path planning, and determines the optimal exploration order by solving the traveling salesman problem. In the local exploration map, it defines two points where the global path planning intersects as boundary points. Local exploration is carried out in the local exploration area through DWA. When the local exploration ends, it moves along the two boundary points to enter the next exploration area, thus ensuring the consistency of the global path planning.

[0193] Furthermore, the definition of the boundary specifically includes:

[0194] Define the global environment that the robot is currently exploring as The unobstructed local environment is defined as follows: The pose of the lidar on the robot is viewpoint v∈SE(3), v=[p v ,q v ], p v Represents location information, q v This represents attitude information. A surface is defined to represent the generalized boundary between free and non-free environments, where the non-free environment includes both occupied and unknown environments. The surface perceived by viewpoint v is represented as... Then all perceived surfaces can be represented by the following formula:

[0195]

[0196] in, Let S represent the set of all viewpoints, and let S represent all covered surfaces. and uncovered surfaces Given the current viewpoint v currentand uncovered surfaces Then, the shortest path T* satisfying the dynamic constraints, formed by the viewpoint set v1, v2, ..., can be found for exploration. And as the exploration continues, a portion It will be updated to S cov And new uncovered surfaces will be added. In this process, the shortest path can be formed periodically to achieve complete surface (boundary) coverage.

[0197] Furthermore, regarding viewpoint sampling, specifically including:

[0198] To form a periodic shortest path, the optimal set of viewpoints must be found. First, define the center of the covered surface as p. s ∈M, its normal vector is n s ∈R 3 The surface is covered by the viewpoint v provided that formulas and are satisfied:

[0199] |p s -p v |≤D (13)

[0200] Where D represents the planar distance constraint.

[0201] n s ·(p v -p s ) / |n s |·|(p v -p s )|≥T(14)

[0202] Where T represents the planar orientation constraint.

[0203] Although the viewpoint increases The coverage area will become smaller over time, but the reward provided by each new viewpoint will also decrease. The reward refers to the area of ​​uncovered surface scanned by the lidar at that viewpoint. This is because the closer adjacent viewpoints are, the higher their scan overlap. Therefore, the reward for a viewpoint is closely related to the previous viewpoint selection. That is, viewpoint v... i The rewards need to be updated to Therefore, the specific steps of the viewpoint sampling algorithm are as follows:

[0204] Generate a uniform set of candidate viewpoints from the connectable regions within the local planning scope.

[0205] The rewards for this set of viewpoints are updated according to the reward update formula, and then stored in a priority queue based on the reward size.

[0206] K subsets of viewpoints covering local areas are randomly generated through k iterations. Within each subset, three viewpoints are mandatory: the current viewpoint, the boundary viewpoint for entering the local area, and the boundary viewpoint for exiting the local area. These two boundary viewpoints ensure the continuity between the local and global paths. Other viewpoints are selected from a priority queue, with the selection probability proportional to the reward for that viewpoint. Due to the issue of observation area coverage between neighboring viewpoints, the reward needs to be updated in real time. A viewpoint sampling cycle ends when the queue is empty or the reward for a newly added viewpoint falls below a threshold.

[0207] Calculate the path cost generated by each set of viewpoint sampling, and select the viewpoint set with the lowest cost as the optimal viewpoint set.

[0208] Furthermore, local path planning specifically includes:

[0209] The viewpoint sampling described above yields a set of viewpoints for the local exploration map. Therefore, local path planning involves generating an optimal exploration path that satisfies dynamic constraints based on the optimal viewpoint set. However, relying solely on the extracted viewpoint set cannot guarantee the smoothness and continuity of the path. Figure 4 As shown:

[0210] Figure 4 The green box represents a local sub-map maintained by global path planning, while the brown and blank circles represent extracted viewpoints. The brown circles represent continuous viewpoints, and the blank circles represent segmented viewpoints. Due to the existence of segment points, the robot must stop at each segment point and change direction to reach the next viewpoint while maintaining the optimal path. Therefore, to minimize the path cost, the cost function for each segment point is defined as follows:

[0211]

[0212] Among them, l j Let be the length of the j-th path segment, and p represent the penalty for each stop. Therefore, to obtain the minimum cost function, we first use an improved A* algorithm to calculate the shortest path between each pair of viewpoints. The improved method is described in detail below. Then, we construct a distance matrix based on the shortest path. Solving the problem of finding the shortest path through every viewpoint transforms into the Traveling Salesman Problem (TSP). After solving this problem using the Lin-Kernighan algorithm, the optimal access order of the viewpoint set can be determined. Then, all viewpoints are set as segment points, and the path cost is calculated iteratively after setting each viewpoint as a continuous point. The optimal cost function is obtained, and the optimal velocity space is generated using the DWA algorithm to obtain the optimal trajectory between viewpoints, thus confirming the optimal smooth path.

[0213] Traditional DWA algorithms only require sequentially reaching the path points planned by the global path plan to complete the local path planning task and reach the target point. However, when obstacles exist, the DWA algorithm will take the safest approach, choosing a trajectory farthest from the obstacle. This deviates from the globally planned trajectory, increasing the time to reach the target point. Therefore, an improvement is made to the traditional DWA algorithm by adding a distance-to-target evaluation function and a distance-to-global-path-interval evaluation function to the traditional evaluation function.

[0214] The distance-to-target evaluation sub-function is as follows:

[0215]

[0216] In the formula, (x end ,y end (x) represents the local sub-target points generated by path planning between each viewpoint of the robot. current ,y current (x) represents the position of the end of the trajectory generated by the robot in the current sampling space. begin ,y begin () represents the previous local sub-target point of the current local sub-target point. This evaluation sub-function adds a distance constraint to the target point, which can ensure that the robot chooses the trajectory closer to the target point and reaches the target point faster.

[0217] The global path interval evaluation subfunction is:

[0218]

[0219] In the formula, gap is the endpoint of the trajectory generated in the current robot sampling velocity space (x i ,y i The interval from the current local target point to the line connecting the previous local target point (Ax + By + C = 0) is expressed by the following formula:

[0220]

[0221] The evaluation subfunction adds an interval constraint with the global path planning. The higher the allgap value, the higher the evaluation of the trajectory and the closer it is to the global path.

[0222] Therefore, the improved DWA evaluation function is:

[0223] G(v,w)=σ[αheading(v,w)+βdist(v,w)+γvel(v,w)+δenddist(v,w)+etaallgap(v,w)] (19)

[0224] The calculation steps of the improved DWA algorithm are as follows:

[0225] a) Initialize the initial position, local child nodes, path generated by the A* algorithm, DWA evaluation function, robot linear acceleration, angular acceleration, maximum speed and minimum speed, and obtain the current robot posture through the IMU.

[0226] b) Based on the characteristics of the differential chassis, a sampling trajectory is generated for the sampling speed space.

[0227] c) Prioritize the sampled trajectories based on the improved DWA evaluation function, and send the velocity space (v, w) corresponding to the trajectory with the highest priority to the chassis.

[0228] d) Determine if the end of the trajectory is the endpoint. If not, repeat steps b and c; if yes, the task ends and the operation terminates.

[0229] Furthermore, global path planning specifically includes:

[0230] In global path planning, the map is divided into local sub-maps, which are categorized into three states: "unexplored," "still being explored," and "exploration complete." These states are distinguished by the "surfaces" stored within each sub-map. A sub-map containing uncovered surfaces is in the "still being explored" state; containing only covered surfaces is in the "exploration complete" state; and an empty sub-map is in the "unexplored" state. The goal of global path planning is to generate an optimal path from the current LiDAR point cloud map, traversing the center points of all sub-maps and the current viewpoint. This point cloud map and the generated path are updated in real-time as the robot moves. The specific process is as follows:

[0231] a) Generate path costs between unexplored and currently explored local sub-maps using the improved A* algorithm and generate distance matrices between them.

[0232] b) Solve the TSP problem in the distance matrix, defining the points on the global path that intersect with the currently explored local submap as boundary point 1 and boundary point 2. Apply local path planning between boundary point 1 and boundary point 2 to achieve two-stage planning.

[0233] c) Once the current local sub-map exploration is complete, the local path is simplified again to the shortest path from the current viewpoint to boundary point 1 and boundary point 2, so that we can return to global path planning and move the robot to the next exploration point to perform the exploration task.

[0234] Because the traditional A* algorithm obtains the reachable neighbor nodes in eight directions as the next moving node during operation, this algorithm will bring redundant search burden in the case of large map scale, and the computational complexity will also increase significantly with the increase of the scene. Therefore, the heuristic function of the A* algorithm is improved.

[0235] Because the kinematic constraints of the four-wheel differential chassis being unable to achieve lateral movement need to be met, the cost function is calculated using Euclidean distance.

[0236] f(n)=g(n)+h(n) (20)

[0237] Where g(n) is represented as:

[0238]

[0239] g(f) represents the cumulative distance between the parent node and the starting node, (x current ,y current (x) represents the current node position. father ,y father ) represents the position of the parent node, and g(n) is the actual cost function representing the distance between the current node and the starting node.

[0240] h(n) is represented as:

[0241]

[0242] In this formula (x) goal ,y goal ) represents the target node position, and h(n) is the heuristic function representing the Euclidean distance between the current node and the target node.

[0243] As shown in the above equation, the node selection result of the overall evaluation function is greatly influenced by the heuristic function. When the current point is far from the target point, h(n) is large. Although it can guide the robot to move towards the target point, it cannot determine the shortest path. When the current point is close to the target point, h(n) is small. Although it ensures the shortest path, the lack of distance constraints to the target point reduces search efficiency and increases the search time for the target point. Therefore, an adaptive weight cost function is proposed for the A* algorithm:

[0244] f(n)=g(n)+α(x)h(n) (23)

[0245] In the formula:

[0246]

[0247]

[0248] The weights are introduced using a standard normal function, where l represents the Euclidean distance between the current point and the starting point, and L represents the Euclidean distance between the ending point and the starting point. In this case, when the current point is far from the target point, h(n) will not be too large, which can both drive the robot to move towards the target point and determine the shortest path. When the current point is close to the target point, h(n) is small but not too small, which can both ensure the shortest path and the distance constraint of the target point, thus ensuring search efficiency and algorithm running time.

[0249] The improved calculation steps for A* are as follows:

[0250] a) Initialize the starting position A and the ending position B, and generate a raster map using the improved A* algorithm.

[0251] b) Add the starting point to Open_set and set it to the highest priority.

[0252] c) Search for jump points in the eight neighborhood directions of the current node, excluding nodes in Close_set and obstacles. If no jump point is found after extending to obstacles, stop searching in this direction; add the found jump points to Open_set and store the historical starting point in Close_set.

[0253] d) Compare the jump points in Open_set using the adaptive weight cost function (23) to select the optimal jump point as the new starting point and execute steps 3 and 4 again until the endpoint is reached.

[0254] The underlying control system moves smoothly based on the trajectory generated by the optimal velocity space sampled by DWA. As the environment moves, it is updated in real time to ensure the collection and updating of system environment data, thereby achieving the goal of autonomous exploration and mapping, and realizing an active SLAM system.

[0255] This invention provides an active SLAM system based on laser-inertial navigation fusion, which has the following technical advantages:

[0256] (1) Firstly, addressing the issue that the inter-frame features of point clouds extracted in structured scenes are too consistent, hindering accurate localization and point cloud map construction, this application proposes improving corner feature matching in front-end matching to descriptor matching based on extracted point cloud features to enhance the efficiency of inter-frame matching in front-end odometry. Simultaneously, addressing the problem that pose accumulation error increases with robot movement distance, and the inability of Euclidean distance-based loop closure detection to accurately relocalize and optimize pose and map, this application proposes a loop closure detection method that integrates Euclidean distance and Scan-Context to reduce the impact of accumulated error on poor global map consistency. Figure 5 and Figure 6 As shown.

[0257] (2) To address the inefficiency and tendency to get trapped in local optima in exploration-based path planning algorithms, a two-stage autonomous exploration method is proposed. In local path planning, the traditional sliding window method tends to select a locally optimal trajectory rather than the globally optimal one when obstacles are present, thus deviating from the global path and increasing the time to reach the target point. To address this, a cost function for the target distance and global path planning distance is added to the sliding window method to optimize the trajectory and global path constraints. In global path planning, to address the time-consuming search of the traditional A* (A* Star) algorithm, adaptive weights are added to the heuristic function, and a jump-point strategy is incorporated into the A* algorithm to accelerate the search. Figure 7 and Figure 8 As shown.

[0258] (3) Figure 9 and Figure 10 The image shows the autonomous exploration effect based on the improved active SLAM algorithm in a small warehouse scene and a large corridor scene in a real-world scenario.

Claims

1. An active SLAM system integrating laser and inertial navigation, characterized in that, The system includes a real-time data acquisition system, a laser inertial navigation SLAM system, an autonomous exploration and decision-making system, and a low-level control system. The real-time data acquisition system synchronizes the data collected by different sensors in time and converts the parameters in different coordinate systems, and sends the processed data to the laser inertial navigation SLAM system. The laser inertial navigation SLAM system includes point cloud preprocessing, laser inertial navigation odometry, backend map construction and optimization, and loop closure detection. The point cloud preprocessing preprocesses the point cloud and sends the preprocessed data to the laser inertial navigation odometer. The laser inertial navigation odometry matches the extracted point cloud with the current robot pose as the initial value, and obtains a coarse pose estimate through continuous iterative optimization and sends it to the backend map building and optimization. The backend map construction and optimization are based on coarse pose matching to complete point cloud map construction and pose optimization; The loop closure detection is used for map relocalization to optimize pose and ensure global map consistency; The autonomous exploration decision system receives the positioning information and point cloud map output by the laser inertial navigation SLAM system. It then randomly generates sampling viewpoints based on surface coverage, calculates the distance between viewpoints through global path planning, and obtains the optimal exploration order. In the local exploration map, it defines two points where the global path planning intersects as boundary points. In the local exploration area, it implements local path planning through DWA and travels along the two boundary points to enter the next exploration area. The underlying control system moves smoothly based on the trajectory generated by the optimal velocity space sampled by DWA. As the environment moves, it is updated in real time to ensure the collection and updating of system environment data, thereby achieving the purpose of autonomous exploration and mapping. Furthermore, the laser inertial navigation odometry is used for feature point extraction, specifically including: For the initial point cloud, feature extraction is performed using feature points, which are divided into two types: planar points and corner points; Points on the same scanning line are sorted according to curvature. The same laser scanning line is divided into several segments, each segment is a sub-region, and the points with the largest curvature in each sub-region are marked as corner points, while the remaining points are planar points. Furthermore, the laser inertial navigation odometry is used to generate key points for diagonal points using a K-Means clustering algorithm, specifically including: Step 1: Divide the coordinate system of the lidar into several sectors with the origin as the origin, and then divide the points into the corresponding sectors in sequence by calculating the horizontal angle of the point cloud in each frame of data. Step 2: Using the initial point of each sector as the centroid of the first cluster of that sector, determine whether the loss function and Manhattan distance between the subsequent point cloud of that sector and the centroid of the previous cluster exceed the threshold. Step 3: If the threshold is not exceeded, add the point to the cluster; if the threshold is exceeded, add the point to a new cluster in the sector; discard the outlier. Step 4: For subsequent sectors, repeat steps 2 and 3 to filter out those with fewer point clouds in the cluster; Step 5: When the loss function and Manhattan distance between clusters are less than the threshold, cluster merging is performed; finally, the key points are obtained by averaging all point clouds in each cluster. Furthermore, the laser inertial navigation odometry is used for descriptor generation and matching for key points, specifically including: The cluster centroid is divided into several sectors centered on the current centroid, with each sector corresponding to a dimension. The distance from the nearest centroid to the current centroid is selected as the value of each dimension to generate a descriptor. The direction vector from the current centroid to the nearest centroid is taken as the main direction and is determined as the first sector. The values ​​for each sector are then taken in a counterclockwise direction. If there is no centroid in a sector, it is set to 0. Search for a certain number of nearest key points, generate a certain number of descriptors for each point, and then take the value of the corresponding nearest key point as the final descriptor for each dimension. Establish a vector table to avoid repeatedly calculating distance and direction for corresponding points; Finally, by calculating the difference between the non-zero elements between the descriptors, if it is lower than a certain threshold, the confidence level is incremented by one; if the confidence level is higher than the matching threshold, the match is considered successful. Furthermore, the loop closure detection is used to perform coarse matching via Euclidean distance, followed by fine matching using the Scan-Context descriptor to complete the loop closure, specifically including: First, point cloud segmentation is performed to generate descriptors. Using the lidar coordinate system as a reference, a frame of 3D lidar point cloud is uniformly divided into different bin segments in the radial and rotational directions. In the radial direction, the point cloud is divided into several uniformly distributed rings, and in the rotational direction, the point cloud is divided into several uniformly distributed sectors. The overlapping area of ​​the two is the bin segment. Assign a number to each bin segment as a parameter to form the descriptor matrix. This parameter is determined by the z-value of the highest point in the point cloud of each bin segment. If there are no points in the bin segment, assign a value of 0. The segmented point cloud is unfolded to form a descriptor matrix, which is the Scan-Context descriptor; Historical frames are filtered using a large threshold based on Euclidean distance, and a one-dimensional vector is generated for each descriptor. The one-dimensional vector is stored in a Kd-Tree and subjected to nearest neighbor search with the one-dimensional vectors in historical frames. After finding several historical frames that meet the threshold and completing the initial search, a precise search is performed based on the following formula: in, These represent the current frame descriptor matrix and the historical frame descriptor matrix, respectively. and Let these represent the column vector of the j-th column in the current frame and the column vector of the j-th column in the historical frame, respectively. The number of sectors into which the point cloud is divided. The best similarity score is given between the current frame descriptor matrix and the historical frame descriptor matrices; Perform historical frame descriptor matrix First translation, then proceed This matching is used to ensure rotation invariance. Perform the same operation on each historical frame, selecting the smallest value. The value is used as the best similarity score between the historical frame and the current frame. If the value is less than the threshold, a loop closure is satisfied. ICP iterative optimization is performed on historical frames and the current frame. After finding the optimal pose, the pose near the loop closure is optimized. If the value is greater than the threshold, the loop closure condition is not met, and loop closure optimization is not performed.

2. The active SLAM system with laser-inertial navigation fusion according to claim 1, characterized in that, The local path planning in the autonomous exploration decision-making system specifically includes: The cost function for each segment point is defined as follows: in, For the first Length of the path segment This indicates the penalty for each parking incident; For each pair of viewpoints, an improved A* algorithm is used to calculate the shortest path between viewpoints. This improved A* algorithm includes an adaptive weight cost function proposed for the A* algorithm. in, , , This represents the Euclidean distance between the current point and the starting point. This represents the Euclidean distance between the starting and ending points. This represents the distance between the current node and the starting node. This represents the Euclidean distance between the current node and the target node; Construct a distance matrix based on the shortest path, solve the problem of finding the shortest path through each viewpoint, and determine the optimal access order for the viewpoint set. All viewpoints are set as segment points. The path cost is calculated iteratively after each viewpoint is set as a continuous point. The optimal cost function is obtained, and an improved DWA algorithm is used to generate the optimal velocity space to obtain the optimal trajectory between viewpoints, confirming the optimal smooth path. The improved DWA algorithm specifically includes: Improved DWA evaluation function: in, For evaluating the distance to the target, This is a sub-function for evaluating the distance to the global path interval. heading(v,w) This represents the direction angle evaluation sub-function. dist(v,w) This represents the sub-function for evaluating the distance between the obstacle and the robot body. vel(v,w) This represents the current linear velocity evaluation sub-function of the mobile robot, where α, β, γ, δ, and μ represent... heading(v,w) , dist(v,w) , vel(v,w) , , In the evaluation function, the weight σ represents the normalization factor.

3. The active SLAM system based on laser-inertial navigation fusion according to claim 2, characterized in that, The global path planning in the autonomous exploration decision-making system specifically includes: The improved A* algorithm is used to generate the path cost between the unexplored and currently explored local sub-maps, and to generate the distance matrix between them; To solve the TSP problem in this distance matrix, the points on the global path that intersect with the currently explored local submap are defined as boundary point 1 and boundary point 2; local path planning is applied between boundary point 1 and boundary point 2 to achieve two-stage planning. Once the current local sub-map exploration is complete, the local path is simplified again to the shortest path from the current viewpoint to boundary point 1 and boundary point 2, so that we can return to global path planning. Based on this, the robot moves to the next exploration point to perform the exploration task.

4. An active SLAM method for laser-inertial navigation fusion, characterized in that, The method includes performing active SLAM using the system described in any one of claims 1-3.

5. A computer device comprising a memory and a processor, wherein the memory stores a computer program, characterized in that, The steps of the method of claim 4 are performed when the processor runs the computer program stored in the memory.

6. A computer-readable storage medium, characterized in that, The computer-readable storage medium stores a plurality of computer instructions, which are used to cause a computer to perform the method described in claim 4.

Images