Active SLAM (Simultaneous Localization and Mapping) system, method and equipment for laser inertial navigation fusion

Abstract

The invention discloses an active SLAM (Simultaneous Localization and Mapping) system, method and equipment based on laser inertial navigation fusion, belongs to the technical field of active SLAM, and solves the problem that the mapping quality and exploration efficiency are low when the active SLAM technology based on laser inertial navigation fusion faces a structured scene. The system comprises a laser inertial navigation SLAM system and a laser inertial navigation system, wherein the laser inertial navigation SLAM system comprises a point cloud preprocessing unit for preprocessing a point cloud and sending preprocessed data to a laser inertial navigation odometer; matching the extracted point cloud by the laser inertial navigation odometer by taking the pose provided by the IMU as an initial value, obtaining coarse pose estimation through iterative optimization, and sending the coarse pose estimation to back-end map construction and optimization; the back-end map construction and optimization are matched according to the coarse pose, and point cloud map construction and pose optimization are completed; loopback detection is used for map relocation to optimize the pose and ensure the global consistency of the map. The autonomous exploration decision-making system can plan a path capable of increasing the boundary coverage range. The method is suitable for scene exploration which is weak in communication signal and cannot directly control the mobile robot in real time.

Description

Technical Field

[0001] The present application relates to the field of active SLAM technology, and in particular to an active SLAM system fused with laser inertial navigation. Background Art

[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 where communication signals are weak and mobile robots cannot be directly operated in real time, so as to provide a priori environment for performing subsequent tasks.

[0003] At present, active SLAM technology based on laser inertial navigation fusion has achieved relatively mature results, but when faced with structured scenes, both the quality of mapping and the efficiency of exploration will decline severely. This is because the features between point cloud frames extracted in structured scenes are too consistent, and it is impossible to effectively achieve accurate positioning and build point cloud maps. As a result, the point cloud map built by the exploration robot is seriously distorted due to positioning errors, making it impossible to complete the exploration task. In addition, the exploration-based path planning algorithm is not efficient enough and is prone to falling into local optimality, which also makes the robot unable to complete the exploration task. Summary of the invention

[0004] The purpose of the present invention is to solve the problem that the existing active SLAM technology based on laser inertial navigation fusion has relatively 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 implemented through the following technical solutions. On the one hand, the present invention provides an active SLAM system fused with laser inertial navigation, the system comprising a real-time data acquisition system, a laser inertial navigation SLAM system, an autonomous exploration decision-making system and an underlying control system;

[0006] The real-time data acquisition system synchronizes the data collected by different sensors 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 odometer, back-end map construction and optimization, and loop detection;

[0008] The point cloud preprocessing is to preprocess the point cloud and send the preprocessed data to the laser inertial navigation odometer;

[0009] The laser inertial navigation odometer matches the extracted point cloud with the current robot posture as the initial value, obtains a rough posture estimate through continuous iterative optimization and sends it to the back-end map construction and optimization;

[0010] The back-end map construction and optimization are matched according to the rough pose to complete the point cloud map construction and pose optimization;

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

[0012] The autonomous exploration decision system receives the positioning information and point cloud map output by the laser inertial navigation SLAM system, and randomly generates sampling viewpoints based on the surface coverage, calculates the distance between the viewpoints through global path planning, obtains the best exploration order, defines two points intersecting with the global path planning as boundary points in the local exploration map, implements local path planning through DWA in the local exploration area, and drives along the two boundary points into the next exploration area;

[0013] The underlying control system moves smoothly according to the trajectory generated by the optimal speed space sampled by DWA. As the moving environment is in a real-time update state, the collection and update of the system environment data is ensured, thereby achieving the purpose of autonomous exploration and mapping.

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

[0015] For the initial point cloud, feature points are used for feature extraction. The feature points are divided into two types: plane points and corner points.

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

[0017] Furthermore, the laser inertial navigation odometer is used to generate key points for diagonal points based on the idea of ​​K-Means clustering algorithm, which specifically includes:

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

[0019] Step 2: Taking the initial point of each sector as the centroid of the first cluster of the sector, determine whether the loss function and Manhattan distance between the subsequent point cloud of the 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 a new cluster to the sector; discard the abnormal point;

[0021] Step 4: Repeat steps 2 and 3 for subsequent sectors, and filter out clusters with fewer point clouds;

[0022] Step 5: When the loss function and Manhattan distance between clusters are less than the threshold, clusters are merged; finally, all point clouds in each cluster are averaged to obtain key points.

[0023] Furthermore, the laser inertial navigation odometer is used to generate and match descriptors for key points, specifically including:

[0024] The cluster centroid is divided into several sectors with the current centroid as the center, where each sector corresponds to a dimension, and then the distance value of the centroid closest to the current centroid in each sector is selected as the value of each dimension to generate a descriptor; wherein the direction vector from the current centroid to the nearest centroid is taken as the main direction and is determined as the first sector, and each sector is valued counterclockwise in this way. If there is no centroid in the sector, 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 repeated calculation of distance and direction of corresponding points;

[0027] Finally, by calculating the difference of non-zero elements between descriptors, if it is lower than a certain threshold, the credibility is increased by one, and when the credibility is higher than the matching threshold, the match is considered successful.

[0028] Furthermore, the loop detection is used to perform a coarse match through the Euclidean distance, and then perform a fine match of the Scan-Context descriptor to complete the loop, which specifically includes:

[0029] First, the point cloud is divided to generate descriptors. Taking the laser radar coordinate system as the reference system, a frame of 3D laser radar point cloud is evenly divided into different bin segments in the radial and rotational directions. From the radial direction, the point cloud is divided into several evenly distributed rings, and from the rotational direction, the point cloud is divided into several evenly distributed sectors. The intersection and overlap 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 each bin segment point cloud. If there is no point in the bin segment, the value is 0.

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

[0032] Perform a large threshold screening on the historical frames based on the Euclidean distance and generate a one-dimensional vector for each descriptor;

[0033] The one-dimensional vector is stored in the Kd-Tree and the nearest neighbor search is performed with the one-dimensional vector in the historical frame. After a preliminary search is completed by finding several historical frames that meet the threshold, an accurate search is performed based on the following formula:

[0034]

[0035] Among them, Iq, Ic represent the current frame descriptor matrix and the historical frame descriptor matrix respectively. and Represent the column vector of the jth column of the current frame and the column vector of the jth column of the historical frame, N s is the number of sectors into which the point cloud is divided, d(Iq, Ic) is the best similarity score between the current frame descriptor matrix and the historical frame descriptor matrix;

[0036] Perform N on the historical frame descriptor matrix s Translations, then N s The algorithm performs the same operation on n historical frames, and selects the minimum d(Iq, Ic) value 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 is satisfied. The ICP iterative optimization is performed on the historical frame and the current frame. After the optimal posture is obtained, the posture near the loop is optimized. If the d(Iq, Ic) value is greater than the threshold, the loop is not satisfied and no loop optimization is performed.

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

[0038] The cost function of the segmentation point is defined as:

[0039]

[0040] Among them, l j is the length of the jth path, and p represents the penalty for each stop;

[0041] The improved A* algorithm is used to calculate the shortest path between each pair of viewpoints. The 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 end 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 the shortest path passing through each viewpoint, and determine the best access order of the viewpoint set;

[0045] All viewpoints are set as segmentation points, and each viewpoint is iteratively set as a continuous point and then the path cost is calculated to find the optimal cost function. The improved DWA algorithm is used to generate the optimal speed space to obtain the optimal trajectory between viewpoints and confirm 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] Among them, enddist(v, w) is a sub-function for evaluating the distance to the target, allgap(v, w) is a sub-function for evaluating the distance to the global path, heading(v, w) is a sub-function for evaluating the direction angle, dist(v, w) is a sub-function for evaluating the distance between the obstacle and the robot body, vel(v, w) is a sub-function for evaluating the linear velocity of the current mobile robot, α, β, γ, δ, 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, respectively, and σ represents the normalization factor.

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

[0050] Generate the path cost between the unexplored and currently explored local submaps by the improved A* algorithm, and generate the distance matrix between them;

[0051] Solve the TSP problem in the distance matrix, define the points on the global path that intersect the local submap currently being explored as boundary points 1 and 2; apply local path planning between boundary points 1 and 2 to achieve two-stage planning;

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

[0053] In a second aspect, the present invention provides a laser inertial navigation fusion active SLAM method, the method comprising performing active SLAM using a laser inertial navigation fusion active SLAM system as described above.

[0054] In a third aspect, the present invention provides a computer device, comprising a memory and a processor, wherein the memory stores a computer program, and when the processor runs the computer program stored in the memory, the steps of the active SLAM method of laser inertial navigation fusion as described above are executed.

[0055] In a fourth aspect, the present invention provides a computer-readable storage medium, wherein a plurality of computer instructions are stored in the computer-readable storage medium, and the plurality of computer instructions are used to enable a computer to execute an active SLAM method of laser inertial navigation fusion as described above.

[0056] Beneficial effects of the present invention:

[0057] The present invention firstly addresses the problem that the features between point cloud frames extracted in structured scenes are too consistent, making it impossible to effectively achieve accurate positioning and construct point cloud maps. This application proposes to improve the corner feature matching in the front-end matching to descriptor matching on the basis of extracting point cloud features to enhance the efficiency of the front-end odometer frame matching. At the same time, in order to address the problem that the cumulative error of the posture increases as the robot moves, and the problem that the loop detection based on the Euclidean distance cannot accurately reposition to optimize the posture and map, a loop detection method that integrates the Euclidean distance and Scan-Context is proposed to reduce the impact of the poor global consistency of the map caused by the cumulative error, such as Figure 5 and Figure 6 shown.

[0058] The present invention aims to solve the problem that the exploration-based path planning algorithm is not efficient enough and is prone to fall into the local optimum, and proposes an autonomous exploration method based on two-stage planning. In the local path planning, the traditional sliding window method will select the local optimum instead of the global optimum trajectory when selecting the trajectory due to the existence of obstacles, and then deviate from the global path to increase the time to reach the target point. Therefore, the sliding window method is added with the target distance and the global path planning distance cost function to optimize the trajectory and global path constraints. In order to solve the problem that the traditional A* (A Star) algorithm in global path planning consumes a lot of time to search, an adaptive weight is added to the heuristic function and a jump point strategy is added to the A* algorithm to speed up the search, such as Figure 7 and Figure 8 shown.

[0059] The laser inertial navigation SLAM system module of the present invention solves the problem that the point cloud frame features extracted in the structured scene are too consistent, and it is impossible to effectively achieve accurate positioning and construct the point cloud map, which causes the point cloud map constructed by the exploration robot to be seriously distorted due to positioning errors. The implementation of the active SLAM method requires that the mobile robot has both the ability to locate and build maps and the ability to generate exploration paths, both of which are indispensable. The premise of generating the exploration path is to have a map of the current environment and the posture information of the current robot. The autonomous exploration decision-making system of the present invention is the real-time update of the unknown scene environment map and the ability to plan a path that can increase the boundary coverage range under the current known map information.

[0060] The present invention is suitable for exploring unknown environments in scenarios where communication signals are weak and mobile robots cannot be directly controlled in real time, such as underground mines and Mars, so as to provide a priori environments for executing subsequent tasks. BRIEF DESCRIPTION OF THE DRAWINGS

[0061] In order to more clearly illustrate the technical solution of the present application, the drawings required for use in the embodiments are briefly introduced below. Obviously, for ordinary technicians in this field, other drawings can be obtained based on these drawings without any creative work.

[0062] Figure 1 The flowchart of loop closure detection of Euclidean distance and Scan-Context fusion of the present invention;

[0063] Figure 2 It is a point cloud division diagram of the present invention;

[0064] Figure 3 Generating a graph for the descriptor matrix of the present invention;

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

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

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

[0068] Figure 7 It is the overall exploration result of the present invention;

[0069] Figure 8 This is the exploration trajectory effect diagram of the present invention;

[0070] Fig. 9 This is a schematic diagram of the active SLAM experiment effect in a small warehouse scene;

[0071] Fig.10 Schematic diagram of the active SLAM experimental effect in a large corridor scene. DETAILED DESCRIPTION

[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 throughout represent the same or similar elements or elements having the same or similar functions. The embodiments described below with reference to the accompanying drawings are exemplary and are intended to be used to explain the present invention, and should not be construed as limiting the present invention.

[0073] Embodiment 1: An active SLAM system fused with laser inertial navigation, the system comprising a real-time data acquisition system, a laser inertial navigation SLAM system, an autonomous exploration decision-making system and an underlying control system;

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

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

[0076] First, the sensor data is sent to the point cloud for preprocessing, and then sent to the laser inertial navigation odometer after downsampling, ground separation, feature extraction, etc.

[0077] This part matches the extracted point cloud with the pose provided by IMU as the initial value, and obtains the rough pose estimate through continuous iterative optimization and sends it to the backend.

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

[0079] Among them, loop closure detection is used for map relocalization to optimize the pose and ensure the global consistency of the map.

[0080] The autonomous exploration decision system receives the positioning information and point cloud map output by the laser inertial navigation SLAM system, and randomly generates sampling viewpoints through surface coverage, calculates the distance between viewpoints through global path planning, and obtains the best exploration order by solving the traveling salesman problem. In the local exploration map, two points intersecting with the global path planning are defined as boundary points, and local exploration is realized in the local exploration area through DWA. When the local exploration is completed, it drives along the two boundary points into the next exploration area to ensure the consistency of the global path planning.

[0081] The underlying control system moves smoothly according to the trajectory generated by the optimal speed space sampled by DWA. As the moving environment is updated in real time, the collection and update of system environmental data is ensured, thereby achieving the purpose 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 features between point cloud frames extracted in structured scenes are too consistent, making it impossible to effectively achieve accurate positioning and construct point cloud maps, resulting in serious distortion of the point cloud maps constructed by the exploration robot due to positioning errors.

[0083] The implementation of the active SLAM method requires that the mobile robot has both the ability to locate and build maps and the ability to generate exploration paths. The premise of generating an exploration path is to have a map of the current environment and the current posture information of the robot. The autonomous exploration decision system is the real-time update of the map of the unknown scene environment and the ability to plan a path that can increase the boundary coverage range under the current known map information.

[0084] Embodiment 2: This embodiment further limits the active SLAM system of laser inertial navigation fusion described in embodiment 1. In this embodiment, the function of the laser inertial navigation odometer is further limited, specifically including:

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

[0086] The initial point cloud uses feature points for feature extraction. There are two types of feature points: plane points and corner points. Plane points are points on the plane of three-dimensional space with small curvature. Corner points are points at the edge of three-dimensional space with large curvature. The curvature calculation is shown in formula (1):

[0087]

[0088] Where k represents a cycle of scanning points, L represents the radar coordinate system, c represents the curvature of the current point i, and S is the set of continuous points on the same laser scanning line. i, j represent a number in the set, is the coordinate of the current point i, 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 the curvature c.

[0089] Before selecting feature points, we must first exclude some unreliable points. These unreliable points are divided into two types: occlusion points and parallel beam points. After excluding these unreliable points, we can start extracting feature points. In order to make the feature points evenly distributed, the same laser scan line is divided into several segments, such as 6 segments, each of which is a sub-area. The several points with the largest curvature in each sub-area (such as 20 points) are marked as corner points, and the remaining points are plane points.

[0090] Implementation method 3: This implementation method further limits the active SLAM system of laser inertial navigation fusion described in implementation method 2. In this implementation method, the function of the laser inertial navigation odometer is further limited, specifically including:

[0091] Compared with surface points, corner points have advantages such as fast calculation speed and obvious features. Therefore, the key points are generated based on the K-Means clustering algorithm. Since the K-Means clustering algorithm is easily affected by initial values ​​and outliers, the clustering result may not be the global optimal but the local optimal, and the sample points can only be divided into a single class. Considering that when the horizontal angle of the target point cloud is in an approximate range, it is likely to come from the same object and should be clustered into the same cluster. Therefore, the horizontal plane is divided into N sectors with the laser radar coordinate system as the center, and then clustering is performed in sequence in units of sectors. The loss function corresponding to the clustering result is minimized. Among them, the loss function can be defined as the sum of squares of the errors of each sample from the center point of the cluster to which it belongs, as shown in formula (2):

[0092]

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

[0094] The specific steps are:

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

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

[0097] 3) If it does not exceed the threshold, add the point to the cluster; if it exceeds the threshold, add a new cluster to the sector; abnormal points are discarded.

[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, clusters are merged. Finally, the centroid of all point clouds in each cluster is averaged and obtained as the key point.

[0100] Embodiment 4: This embodiment further limits the active SLAM system of laser inertial navigation fusion described in embodiment 3. In this embodiment, the function of the laser inertial navigation odometer is further limited, including:

[0101] First, the cluster centroid points are divided into N sectors with the current centroid point M0 as the center, where each sector corresponds to a dimension. Then, the distance value of the centroid point closest to M0 in each sector is selected as the value of each dimension to generate a descriptor.

[0102] In order to ensure the invariance of the main direction posture, the direction vector from the current point M0 to the nearest centroid point M1 is used as the main direction and is determined as the first sector. The values ​​are taken for each sector in a counterclockwise direction. If there is no centroid point in the sector, it is set to 0.

[0103] From the current point to other points M i The direction (i≠1) is expressed as and main direction The angle between them determines k i Which sector it belongs to. The angle is calculated as follows:

[0104]

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

[0106]

[0107] At the same time, in order to solve the interference of 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 corresponding nearest key point is taken as the final descriptor. A vector table is established to avoid repeated calculation of distance and direction of corresponding points. Finally, by calculating the difference of non-zero elements between descriptors, if it is lower than a certain threshold, the credibility is increased by one. When the credibility is higher than the matching threshold, the match is considered successful.

[0108] After the matching is successful, the inter-frame motion of the LiDAR is solved by using Gauss-Newton or Levenberg-Marquardt Algorithm (LM) nonlinear optimization, where the ground point features are first optimized [t z ,θ roll ,θ pitch ], and then use the corner descriptors and surface point matching in non-ground points to optimize [t x ,t y ,θ yaw ], thereby obtaining the relative position change of the robot to generate the front-end odometer.

[0109] Implementation mode 5: This implementation mode is a further limitation on the active SLAM system of laser inertial navigation fusion described in Implementation mode 1. In this implementation mode, the loop detection is further limited, specifically including:

[0110] The loop detection is used to perform a rough match through the Euclidean distance, and then perform a fine match of the Scan-Context descriptor to complete the loop, specifically including:

[0111] First, the point cloud is divided to generate descriptors. Taking the laser radar coordinate system as the reference system, a frame of 3D laser radar point cloud is evenly divided into different bin segments in the radial and rotational directions. From the radial direction, the point cloud is divided into several evenly distributed rings, and from the rotational direction, the point cloud is divided into several evenly distributed sectors. The intersection and overlap 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 each bin segment point cloud. If there is no point in the bin segment, the value is 0.

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

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

[0115] Perform a large threshold screening on the historical frames based on the Euclidean distance and generate a one-dimensional vector for each descriptor;

[0116] The one-dimensional vector is stored in the Kd-Tree and the nearest neighbor search is performed with the one-dimensional vector in the historical frame. After a preliminary search is completed by finding several historical frames that meet the threshold, an accurate search is performed based on the following formula:

[0117]

[0118] Among them, Iq, Ic represent the current frame descriptor matrix and the historical frame descriptor matrix respectively. and Represent the column vector of the jth column of the current frame and the column vector of the jth column of the historical frame, N s is the number of sectors into which the point cloud is divided, d(Iq, Ic) is the best similarity score between the current frame descriptor matrix and the historical frame descriptor matrix;

[0119] Perform N on the historical frame descriptor matrix s Translations, then Ns The algorithm performs the same operation on n historical frames, and selects the minimum d=d(Iq,Ic) value as the best similarity score between the historical frame and the current frame. If the d value is less than the threshold, the loop is satisfied. The ICP iterative optimization is performed on the historical frame and the current frame. After the optimal posture is obtained, the posture near the loop is optimized. If the d value is greater than the threshold, the loop is not satisfied and no loop optimization is performed.

[0120] In this implementation, the loop detection based on Scan-Context descriptor is different from the loop detection based on Euclidean distance. The loop detection based on Euclidean distance performs loop closure according to the Euclidean distance between the current frame and the historical frame, and is too dependent on the pose output after the SLAM backend optimization. The loop detection based on Scan-Context descriptor performs alignment based on the similarity between the current frame point cloud and the historical frame point cloud, and does not rely on the positioning result. Therefore, it has higher efficiency. However, this method will perform a cyclic matching for each newly added key frame to find out whether there is a loop, which greatly increases the algorithm running time. Therefore, a loop detection algorithm that combines Euclidean distance and Scan-Context is introduced. First, a coarse match is performed using Euclidean distance to reduce the number of matches using Scan-Context, and then a fine match of the Scan-Context descriptor is performed to complete the loop, thereby improving the efficiency of the loop. The algorithm flow chart is shown below. Figure 1 shown.

[0121] Implementation method 6: This implementation method is a further limitation on the active SLAM system of laser inertial navigation fusion described in Implementation method 1. In this implementation method, the local path planning in the autonomous exploration decision-making system is further limited, specifically including:

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

[0123] Local path planning is to generate the optimal exploration path that satisfies the dynamic constraints for the optimal viewpoint set. However, the smoothness and continuity of the path cannot be guaranteed by relying solely on the extracted viewpoint set. Figure 4 shown. Figure 4 The green box in the middle is the local submap maintained by the global path planning, and the brown and blank circles are the extracted viewpoints, of which the brown circles are continuous viewpoints and the blank circles are segmented viewpoints. Due to the existence of segmentation points, the robot has to stop at the segmentation points and change direction to reach the next viewpoint under the premise of ensuring the optimal path. Therefore, in order to ensure the minimum path cost, the cost function of the segmentation point is defined as:

[0124]

[0125] Among them, l jis the length of the j-th path, and p represents 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 result of selecting nodes by the total evaluation function is greatly affected by the heuristic function. When the current point is far from the target point, h(n) is large. Although the robot can move toward the target point, it cannot be determined to be the shortest path. When the current point is close to the target point, h(n) is small. Although the shortest path is guaranteed, the lack of distance constraints from the target point reduces the search efficiency and prolongs the time to search 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 end point and the starting point, g(n) actual cost function represents the distance between the current node and the starting node, and h(n) heuristic function represents the Euclidean distance between the current node and the target node;

[0130] This weight introduces the standard normal function, l represents the Euclidean distance between the current point and the starting point, and L represents the Euclidean distance between the end point and the starting point. When the current point is far away from the target point, h(n) will not be too large, which can both drive the robot to move toward the target point and determine that it is the shortest path. When the current point is close to the target point, h(n) is small but not too small, which can ensure the shortest path and the distance constraint of the target point, ensure search efficiency and ensure algorithm running time.

[0131] Construct a distance matrix based on the shortest path, solve the problem of the shortest path passing through each viewpoint, and determine the best access order of the viewpoint set;

[0132] All viewpoints are set as segmentation points, and each viewpoint is iteratively set as a continuous point to calculate the path cost, find the optimal cost function, use the improved DWA algorithm to generate the optimal speed space to obtain the optimal trajectory between viewpoints, and confirm the optimal smooth path;

[0133] Because the traditional DWA algorithm only needs to reach the path points planned by the global path planning in sequence to complete the local path planning task and reach the target point, but when there are obstacles, the DWA algorithm will take the safest way, that is, to choose a trajectory away from the obstacles, which will deviate from the trajectory planned by the global path planning and increase the time to reach the target point. Therefore, based on the traditional DWA, an improvement is made, and a distance to target evaluation sub-function and a distance to global path interval evaluation sub-function are added to the traditional evaluation sub-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] Among them, enddist(v, w) is a sub-function for evaluating the distance to the target, allgap(v, w) is a sub-function for evaluating the distance to the global path, heading(v, w) is a sub-function for evaluating the direction angle, dist(v, w) is a sub-function for evaluating the distance between the obstacle and the robot body, vel(v, w) is a sub-function for evaluating the linear velocity of the current mobile robot, α, β, γ, δ, 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, respectively, and σ represents the normalization factor.

[0138] Embodiment 7: This embodiment further limits the active SLAM system of laser inertial navigation fusion described in Embodiment 6. In this embodiment, the global path planning in the autonomous exploration decision-making system is further limited, specifically including:

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

[0140] Generate the path cost between the unexplored and currently explored local submaps by the improved A* algorithm, and generate the distance matrix between them;

[0141] Solve the TSP problem in the distance matrix, define the points on the global path that intersect the local submap currently being explored as boundary points 1 and 2; apply local path planning between boundary points 1 and 2 to achieve two-stage planning;

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

[0143] In this embodiment, the map is divided into local sub-maps in the global path planning, and the local sub-maps are divided into three states: "unexplored", "still exploring" and "exploration completed". The three states are distinguished by the "surface" stored in the local sub-map. If the local sub-map stores uncovered surfaces, it means that the local sub-map is in the "still exploring" state. If the local sub-map only stores covered surfaces, it means that the local sub-map is in the "exploration completed" state. If the local sub-map is empty, it means that the local sub-map is in the "unexplored" state. The goal of global path planning is to generate an optimal path that runs through the center points of each local sub-map and the current viewpoint in the map generated by the current lidar point cloud. The point cloud map and the generated path are updated in real time as the robot moves.

[0144] Embodiment 8: This embodiment is an embodiment of an active SLAM system fused with laser inertial navigation as described above, and specifically includes:

[0145] The real-time data acquisition part is responsible for data adjustment, time synchronization of data collected by different sensors, conversion of parameters in different coordinate systems, and smooth sending of processed data to the laser inertial navigation SLAM system. This part must have real-time stability to ensure the sustainable operation of subsequent algorithms.

[0146] The laser inertial navigation SLAM system is divided into four parts: point cloud preprocessing, laser inertial navigation odometer, back-end map construction and optimization, and loop detection. The system first sends the sensor data to the point cloud preprocessing, and then sends it to the laser inertial navigation odometer after downsampling, ground separation, feature extraction, etc. The extracted point cloud is matched with the current robot's posture provided by IMU as the initial value, and the rough posture estimation is obtained through continuous iterative optimization and sent to the back-end. Then the back-end completes the point cloud map construction and posture optimization based on the rough posture matching based on ScanToMap. Among them, loop detection is used for map relocation to optimize the posture and ensure the global consistency of the map.

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

[0148] The initial point cloud uses feature points for feature extraction. There are two types of feature points: plane points and corner points. Plane points are points on the plane of three-dimensional space with small curvature. Corner points are points at the edge of three-dimensional space with large curvature. The curvature calculation is shown in formula (1):

[0149]

[0150] Where k represents a cycle of scanning points, L represents the radar coordinate system, c represents the curvature of the current point i, and S is the set of continuous points on the same laser scanning line. i, j represent a number in the set, is the coordinate of the current point i, 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 the curvature c.

[0151] Before selecting feature points, we must first exclude some unreliable points. These unreliable points are divided into two types: occlusion points and parallel beam points. After excluding these unreliable points, we can start extracting feature points. In order to make the feature points evenly distributed, the same laser scan line is divided into 6 segments, each segment is a sub-area. The 20 points with the largest curvature in each sub-area are marked as corner points, and the remaining points are plane points.

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

[0153] Compared with surface points, corner points have advantages such as fast calculation speed and obvious features. Therefore, the key points are generated based on the K-Means clustering algorithm. Since the K-Means clustering algorithm is easily affected by initial values ​​and outliers, the clustering result may not be the global optimal but the local optimal, and the sample points can only be divided into a single class. Considering that when the horizontal angle of the target point cloud is in an approximate range, it is likely to come from the same object and should be clustered into the same cluster. Therefore, the horizontal plane is divided into N sectors with the laser radar coordinate system as the center, and then clustering is performed in sequence in units of sectors. The loss function corresponding to the clustering result is minimized. Among them, the loss function can be defined as the sum of squares of the errors of each sample from the center point of the cluster to which it belongs, as shown in formula (2):

[0154]

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

[0156] The specific steps are:

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

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

[0159] 3) If the threshold is not exceeded, the point is added to the cluster; if the threshold is exceeded, a new cluster is added to the sector; abnormal points are discarded. Repeat steps 2 and 3 for subsequent sectors, and finally filter out the clusters with fewer point clouds.

[0160] 4) When the loss function and Manhattan distance between clusters are less than the threshold, clusters are merged. Finally, the centroid of all point clouds in each cluster is averaged and obtained as the key point.

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

[0162] First, the cluster centroid points are divided into N sectors with the current centroid point M0 as the center, where each sector corresponds to a dimension. Then, the distance value of the centroid point closest to M0 in each sector is selected as the value of each dimension to generate a descriptor.

[0163] In order to ensure the invariance of the main direction posture, the direction vector from the current point M0 to the nearest centroid point M1 is used as the main direction and is determined as the first sector. The values ​​are taken for each sector in a counterclockwise direction. If there is no centroid point in the sector, it is set to 0.

[0164] From the current point to other points M i The direction (i≠1) is expressed as and main direction The angle between them determines k i Which sector it belongs to. The angle is calculated as follows:

[0165]

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

[0167]

[0168] At the same time, in order to solve the interference of 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 corresponding nearest key point is taken as the final descriptor. A vector table is established to avoid repeated calculation of distance and direction of corresponding points. Finally, by calculating the difference of non-zero elements between descriptors, if it is lower than a certain threshold, the credibility is increased by one. When the credibility is higher than the matching threshold, the match is considered successful.

[0169] After the matching is successful, the inter-frame motion of the LiDAR is solved by using Gauss-Newton or Levenberg-Marquardt Algorithm (LM) nonlinear optimization, where the ground point features are first optimized [t z ,θ roll ,θpitch ], and then use the corner descriptors and surface point matching in non-ground points to optimize [t x ,t y ,θ yaw ], thereby obtaining the relative position change of the robot to generate the front-end odometer.

[0170] Furthermore, the loop detection specifically includes:

[0171] The loop detection based on Scan-Context descriptor is different from the loop detection based on Euclidean distance. The loop detection based on Euclidean distance performs loop closure according to the Euclidean distance between the current frame and the historical frame, and is too dependent on the output posture after the SLAM backend optimization. The loop detection based on Scan-Context descriptor performs registration based on the similarity between the current frame point cloud and the historical frame point cloud, and does not rely on the positioning result. Therefore, it has higher efficiency. However, this method will perform a cyclic matching for each newly added key frame to find out whether there is a loop, which greatly increases the algorithm running time. Therefore, the loop detection algorithm that combines Euclidean distance and Scan-Context is introduced. First, a coarse match is performed through Euclidean distance to reduce the number of matches using Scan-Context, and then a fine match of Scan-Context descriptor is performed to complete the loop to improve the efficiency of the loop. The algorithm flow chart is as follows Figure 1 As shown, the specific method is as follows:

[0172] First, the point cloud is divided to generate descriptors, such as Figure 2 As shown in the figure, taking the laser radar coordinate system as the reference system, a frame of 3D laser radar point cloud is evenly divided into different bin segments in the radial and rotation angle directions. This division method can better adapt to the sparsity of the point cloud, that is, the point cloud closer to the laser radar is denser, and the point cloud farther away from the laser radar is sparser. From the radial direction, the point cloud is divided into N r Evenly distributed rings, i.e. the yellow part in the figure, divide the point cloud into N s The evenly distributed sectors are the green parts in the figure. The overlapping area between the two is the bin segment, which is the black part. Therefore, the divided point cloud can be expressed as follows:

[0173]

[0174] Among them, P represents a frame of point cloud, P ij Represents each divided segment, i represents the radial direction, and j represents the rotation direction.

[0175] Define the maximum scanning distance of the laser radar as L max The polar coordinates of the point emitted by the laser radar are (r,θ), then the i and j of the point can be expressed as:

[0176]

[0177]

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

[0179] After dividing the point cloud, a number needs to be assigned 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 each bin segment point cloud. If there is no point in the bin segment, the value is 0, that is:

[0180]

[0181] After assignment, the divided point cloud can be expanded to form a descriptor matrix, such as Figure 3 As shown, the descriptor matrix is ​​N r ×N s The matrix generated by the descriptor matrix is ​​the Scan-Context descriptor.

[0182] from Figure 3 It can be seen that although the circle is rotationally invariant, after being converted into a 2D matrix, if the direction of the laser radar changes slightly, resulting in a change in the scanning start position, the 2D matrix will undergo a large lateral shift, so it is not possible to intuitively match based on the difference between the two descriptors.

[0183] In order to solve the above problems, we first perform a large threshold screening on the historical frames based on the Euclidean distance to reduce the subsequent processing time of the algorithm. Then, based on the fact that although the matrix has undergone lateral displacement, the overall point cloud density of each row will not change, a one-dimensional vector k is generated for each descriptor, namely:

[0184] k=(ψ(r 1 ),ψ(r 2 ),...,ψ(r Nr )) (9)

[0185] in:

[0186]

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

[0188] The k value is stored in the Kd-Tree and the nearest neighbor search is performed with the k value in the historical frame. After the initial search is completed by finding n historical frames that meet the threshold, the precise search is performed based on the following formula:

[0189]

[0190] Among them, Iq, Ic represent the current frame descriptor matrix and the historical frame descriptor matrix respectively. and Represent the column vector of the j-th column of the current frame and the column vector of the j-th column of the historical frame respectively.

[0191] Since the descriptor matrix has undergone a large translation, the historical frame descriptor matrix is ​​N s Translation times, that is, 2π / N translations are performed on the historical frame point cloud s rotations, then N s The algorithm performs a match to ensure rotation invariance, and then performs the same operation 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 is satisfied. The ICP iterative optimization is performed on the historical frame and the current frame. After the optimal posture is obtained, the posture near the loop is optimized to optimize the map. If the d value is greater than the threshold, the loop is not satisfied and no loop optimization is performed.

[0192] The autonomous exploration decision system receives the positioning information and point cloud map output by the laser inertial navigation SLAM system, and randomly generates sampling viewpoints through surface coverage, calculates the distance between viewpoints through global path planning, and obtains the best exploration order by solving the traveling salesman problem. In the local exploration map, two points intersecting with the global path planning are defined as boundary points, and local exploration is realized in the local exploration area through DWA. When the local exploration is completed, it drives along the two boundary points into the next exploration area to ensure the consistency of the global path planning.

[0193] Furthermore, the definition of boundaries includes:

[0194] The global environment that the current robot is going to explore is defined as The barrier-free local environment is defined as The position of the laser radar on the robot is the viewpoint v∈SE(3), v=[p v ,q v ], p v Indicates location information, q v represents the posture information. A surface is defined to represent the generalized boundary between the free environment and the non-free environment, where the non-free environment includes the occupied environment and the unknown environment. The surface perceived by the viewpoint v is represented as Then all perceived surfaces can be expressed as follows:

[0195]

[0196] in, represents the set of all viewpoints, and S represents all covered surfaces and uncovered surfaces When the current viewpoint v is givencurrent and uncovered surfaces After that, we can find the shortest path T* formed by the viewpoint set v1, v2, ... that satisfies the dynamic constraints to explore And as the exploration continued, part of Will be updated to S cov and new uncovered surfaces will be added to In the process, the shortest path is formed periodically to achieve full coverage of the surface (boundary).

[0197] Furthermore, for viewpoint sampling, it specifically includes:

[0198] The premise for forming a periodic shortest path is to find the optimal viewpoint set. First, define the center of the covered surface as p s ∈M, whose normal vector is n s ∈R 3 , the premise that the surface is covered by the viewpoint v is to satisfy the formula and formula:

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

[0200] Where D represents the plane distance constraint.

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

[0202] Where T represents the plane direction constraint.

[0203] As the viewpoint increases, The range of will become smaller and smaller, but the reward provided by the newly added viewpoint will also decrease, where the reward refers to the uncovered surface range scanned by the viewpoint lidar. This is because the closer the distance between adjacent viewpoints is, the higher the scanning overlap rate is. Therefore, the reward of a viewpoint is closely related to the previous viewpoint selection. That is, viewpoint v i The reward needs 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 connected region within the local planning scope.

[0205] The reward for this group of viewpoints is updated according to the reward update formula, and then stored in the priority queue as the priority according to the size of their reward.

[0206] K groups of viewpoint subsets covering the local range are randomly generated through k iterations. In each viewpoint subset, three viewpoints are required, namely the current viewpoint, the boundary viewpoint entering the local area, and the boundary viewpoint leaving the local area. These two boundary viewpoints are to ensure the coherence of the local path and the global path. Other viewpoints are selected from the priority queue. The selection probability is proportional to the reward of the viewpoint. Since there is a problem of observation area coverage between adjacent viewpoints, the reward needs to be updated in real time. When the queue is empty or the reward of the newly added viewpoint is lower than the threshold, the viewpoint sampling ends.

[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] Through the above viewpoint sampling, we can get the viewpoint set of the local exploration map. Therefore, local path planning is to generate the optimal exploration path that satisfies the dynamic constraints for 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 in the middle is the local submap maintained by the global path planning, and the brown and blank circles are the extracted viewpoints, of which the brown circles are continuous viewpoints and the blank circles are segmented viewpoints. Due to the existence of segmentation points, the robot has to stop at the segmentation points and change direction to reach the next viewpoint under the premise of ensuring the optimal path. Therefore, in order to ensure the minimum path cost, the cost function of the segmentation point is defined as:

[0211]

[0212] Among them, l j is the length of the jth path, and p represents the penalty for each stop. Therefore, in order to obtain the minimum cost function, the improved A* algorithm is first used to calculate the shortest path between each pair of viewpoints. The improved method is described in detail below, and then the distance matrix is ​​constructed based on the shortest path. The problem of solving the shortest path through each viewpoint is transformed into the traveling salesman problem (TSP). After solving the problem with the Lin-Kernighan algorithm, the optimal access order of the viewpoint set can be confirmed. Then all viewpoints are set as segmented points, and each viewpoint is iteratively set as a continuous point and the path cost is calculated to obtain the optimal cost function. The DWA algorithm is used to generate the optimal speed space to obtain the optimal trajectory between viewpoints and confirm the optimal smooth path.

[0213] Because the traditional DWA algorithm only needs to reach the path points planned by the global path planning in sequence to complete the local path planning task and reach the target point, but when there are obstacles, the DWA algorithm will take the safest way, that is, choose a trajectory away from obstacles, which will deviate from the trajectory planned by the global path planning and increase the time to reach the target point. Therefore, based on the traditional DWA, an improvement is made by adding a distance to the target evaluation sub-function and a distance to the global path interval evaluation sub-function to the traditional evaluation sub-function.

[0214] Among them, the distance evaluation subfunction is:

[0215]

[0216] In the formula, (x end ,y end ) is the local sub-target point generated by the robot’s path planning between each viewpoint, (x current ,y current ) is the position of the end of the trajectory generated by the robot’s current sampling space, (x begin ,y begin ) is 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 a trajectory closer to the target point to reach the target point faster.

[0217] The subfunction for evaluating the distance from the global path interval is:

[0218]

[0219] Where gap is the end point of the trajectory generated by the current robot sampling velocity space (x i ,y i ) to the interval between the current local target point and the previous local target point Ax+By+C=0, which is expressed as:

[0220]

[0221] This evaluation subfunction adds a gap constraint to 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:

[0225] a) Initialize the initial position, local subnodes, 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 IMU.

[0226] b) Generate sampling trajectories for the sampling speed space according to the differential chassis characteristics.

[0227] c) Based on the improved DWA evaluation function, the sampling trajectories are prioritized and the velocity space (v, w) corresponding to the trajectory with the highest priority is sent to the chassis.

[0228] d) Determine whether the end of the trajectory is the endpoint. If not, repeat steps b and c. If yes, the task is completed and the operation is terminated.

[0229] Furthermore, the global path planning specifically includes:

[0230] In global path planning, the map is divided into local sub-maps, and the local sub-maps are divided into three states: "unexplored", "still exploring" and "exploration completed". The three states are distinguished by the "surface" stored in the local sub-map. If the local sub-map stores uncovered surfaces, it means that the local sub-map is in the "still exploring" state. If the local sub-map only stores covered surfaces, it means that the local sub-map is in the "exploration completed" state. If the local sub-map is empty, it means that the local sub-map is in the "unexplored" state. The goal of global path planning is to generate an optimal path that runs through the center points of each local sub-map and the current viewpoint in the map generated by the current lidar point cloud. The point cloud map and the generated path are updated in real time as the robot moves. The specific process is:

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

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

[0233] c) After the current local sub-map is explored, the local path is simplified again to the shortest path from the current viewpoint to boundary point 1 and boundary point 2 so that the global path planning can be returned again. Based on this, the robot moves to the next exploration point to perform the exploration task.

[0234] Because during the operation of the traditional A* algorithm, the current node will obtain the reachable neighbor nodes in the surrounding eight directions as the next mobile node. However, in scenarios with large map scales, this algorithm will bring unnecessary redundant searches and the computational complexity will increase significantly as the scenario becomes larger. Therefore, the heuristic function of the A* algorithm is improved.

[0235] Since the kinematic constraint problem that the four-wheel differential chassis cannot achieve left and right lateral movement must be met, the Euclidean distance is used to calculate the cost function.

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

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

[0238]

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

[0240] h(n) is expressed as:

[0241]

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

[0243] From the above formula, we can see that the result of selecting nodes by the total evaluation function is greatly affected by the heuristic function. When the current point is far from the target point, h(n) is large. Although the robot can move toward the target point, it cannot be determined to be the shortest path. When the current point is close to the target point, h(n) is small. Although the shortest path is guaranteed, the lack of distance constraints from the target point reduces the search efficiency and the time to search for the target point becomes longer. Therefore, an adaptive weight cost function is proposed for the A* algorithm:

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

[0245] Where:

[0246]

[0247]

[0248] This weight introduces the standard normal function, l represents the Euclidean distance between the current point and the starting point, and L represents the Euclidean distance between the end point and the starting point. When the current point is far away from the target point, h(n) will not be too large, which can both drive the robot to move toward the target point and determine that it is the shortest path. When the current point is close to the target point, h(n) is small but not too small, which can ensure the shortest path and the distance constraint of the target point, ensure search efficiency and ensure algorithm running time.

[0249] The calculation steps of improved A* are:

[0250] a) Initialize the starting point A and the end point B, and use the improved A* algorithm to generate a grid map.

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

[0252] c) Search for jump points other than the nodes and obstacles in Close_set in the 8 neighboring directions of the current node. If no jump point is found after expanding to the obstacle, stop searching in this direction; add the searched jump point 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 end point is reached.

[0254] The underlying control system moves smoothly according to the trajectory generated by the optimal speed space sampled by DWA. As the moving environment is updated in real time, the collection and update of the system environment data is guaranteed, thereby achieving the purpose of autonomous exploration and mapping and realizing an active SLAM system.

[0255] The present invention provides an active SLAM system fused with laser inertial navigation, which has the following technical effects:

[0256] (1) First, the problem that the features between point cloud frames extracted in structured scenes are too consistent to effectively achieve accurate positioning and construct point cloud maps. This application proposes to improve the corner feature matching in the front-end matching to descriptor matching on the basis of extracting point cloud features to enhance the efficiency of the front-end odometer frame matching. At the same time, in order to solve the problem that the cumulative error of the posture increases as the robot moves, and the loop detection based on Euclidean distance cannot accurately reposition to optimize the posture and map, a loop detection method that integrates Euclidean distance and Scan-Context is proposed to reduce the impact of the poor global consistency of the map caused by the cumulative error, such as Figure 5 and Figure 6 shown.

[0257] (2) In order to solve the problem that the exploration-based path planning algorithm is not efficient enough and easily falls into the local optimum, an autonomous exploration method based on two-stage planning is proposed. In the local path planning, the traditional sliding window method will select the local optimal trajectory instead of the global optimal trajectory when selecting the trajectory due to the presence of obstacles, and then deviate from the global path to increase the time to reach the target point. Therefore, the sliding window method is added with the target distance and the global path planning distance cost function to optimize the trajectory and global path constraints. In order to solve the problem that the traditional A* (A Star) algorithm in global path planning consumes a lot of time to search, an adaptive weight is added to the heuristic function and a jump point strategy is added to the A* algorithm to speed up the search, such as Figure 7 and Figure 8 shown.

[0258] (3) Fig. 9 and Fig.10 As shown in the figure, the autonomous exploration effect diagram based on the improved active SLAM algorithm in a small warehouse scene and a large corridor scene in the real scene.

Claims

1. An active SLAM system fused with laser inertial navigation, characterized in that: The system includes a real-time data acquisition system, a laser inertial navigation SLAM system, an autonomous exploration decision-making system and an underlying control system; The real-time data acquisition system synchronizes the data collected by different sensors 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 odometer, back-end map construction and optimization, and loop detection; The point cloud preprocessing is to preprocess the point cloud and send the preprocessed data to the laser inertial navigation odometer; The laser inertial navigation odometer matches the extracted point cloud with the current robot posture as the initial value, obtains a rough posture estimate through continuous iterative optimization and sends it to the back-end map construction and optimization; The back-end map construction and optimization are matched according to the rough pose to complete the point cloud map construction and pose optimization; The loop closure detection is used for map relocalization to optimize the pose and ensure the global consistency of the map; The autonomous exploration decision system receives the positioning information and point cloud map output by the laser inertial navigation SLAM system, and randomly generates sampling viewpoints based on the surface coverage, calculates the distance between the viewpoints through global path planning, obtains the best exploration order, defines two points intersecting with the global path planning as boundary points in the local exploration map, implements local path planning through DWA in the local exploration area, and drives along the two boundary points into the next exploration area; The underlying control system moves smoothly according to the trajectory generated by the optimal speed space sampled by DWA. As the moving environment is in a real-time update state, the collection and update of the system environment data is ensured, thereby achieving the purpose of autonomous exploration and mapping.

2. The active SLAM system of laser inertial navigation fusion according to claim 1 is characterized in that: The laser inertial navigation odometer is used for feature point extraction, specifically including: For the initial point cloud, feature points are used for feature extraction. The feature points are divided into two types: plane points and corner points. The points on the same scan line are sorted according to the curvature, and the same laser scan line is divided into several segments, each segment is a sub-area, and the points with the largest curvature in each sub-area are marked as corner points, and the remaining points are plane points.

3. The active SLAM system of laser inertial navigation fusion according to claim 2 is characterized in that: The laser inertial navigation odometer is used to generate key points for diagonal points based on the K-Means clustering algorithm, which specifically includes: Step 1: Divide the laser radar coordinate system into several sectors with the origin as the point, and then divide the point cloud into corresponding sectors in order by calculating the horizontal angle of each frame of data; Step 2: Taking the initial point of each sector as the centroid of the first cluster of the sector, determine whether the loss function and Manhattan distance between the subsequent point cloud of the 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 a new cluster to the sector; discard the abnormal point; Step 4: Repeat steps 2 and 3 for subsequent sectors, and filter out clusters with fewer point clouds; Step 5: When the loss function and Manhattan distance between clusters are less than the threshold, clusters are merged; finally, all point clouds in each cluster are averaged to obtain key points.

4. The active SLAM system of laser inertial navigation fusion according to claim 3 is characterized in that: The laser inertial navigation odometer is used to generate and match descriptors for key points, specifically including: The cluster centroid is divided into several sectors with the current centroid as the center, where each sector corresponds to a dimension, and then the distance value of the centroid closest to the current centroid in each sector is selected as the value of each dimension to generate a descriptor; wherein the direction vector from the current centroid to the nearest centroid is taken as the main direction and is determined as the first sector, and each sector is valued counterclockwise in this way. If there is no centroid in the 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 repeated calculation of distance and direction of corresponding points; Finally, by calculating the difference of non-zero elements between descriptors, if it is lower than a certain threshold, the credibility is increased by one, and when the credibility is higher than the matching threshold, the match is considered successful.

5. The active SLAM system of laser inertial navigation fusion according to claim 1, characterized in that: The loop detection is used to perform a rough match through the Euclidean distance, and then perform a fine match of the Scan-Context descriptor to complete the loop, specifically including: First, the point cloud is divided to generate descriptors. Taking the laser radar coordinate system as the reference system, a frame of 3D laser radar point cloud is evenly divided into different bin segments in the radial and rotational directions. From the radial direction, the point cloud is divided into several evenly distributed rings, and from the rotational direction, the point cloud is divided into several evenly distributed sectors. The intersection and overlap 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 each bin segment point cloud. If there is no point in the bin segment, the value is 0. The divided point cloud is expanded to form a descriptor matrix, which is the Scan-Context descriptor; Perform a large threshold screening on the historical frames based on the Euclidean distance and generate a one-dimensional vector for each descriptor; The one-dimensional vector is stored in the Kd-Tree and the nearest neighbor search is performed with the one-dimensional vector in the historical frame. After a preliminary search is completed by finding several historical frames that meet the threshold, an accurate search is performed based on the following formula: Among them, Iq, Ic represent the current frame descriptor matrix and the historical frame descriptor matrix respectively, and Represent the column vector of the jth column of the current frame and the column vector of the jth column of the historical frame, N s is the number of sectors into which the point cloud is divided, d(Iq, Ic) is the best similarity score between the current frame descriptor matrix and the historical frame descriptor matrix; Perform N on the historical frame descriptor matrix s Translations, then N s The algorithm performs a match to ensure rotation invariance, performs the same operation on n historical frames, and selects the minimum d(Iq, Ic) value 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 is satisfied. The ICP iterative optimization is performed on the historical frame and the current frame. After the optimal posture is obtained, the posture near the loop is optimized. If the d(Iq, Ic) value is greater than the threshold, the loop is not satisfied and no loop optimization is performed.

6. The active SLAM system of laser inertial navigation fusion according to claim 1, characterized in that: The local path planning in the autonomous exploration decision system specifically includes: The cost function of the segmentation point is defined as: Among them, l j is the length of the jth path, and p represents the penalty for each stop; The improved A* algorithm is used to calculate the shortest path between each pair of viewpoints. The improved A* algorithm includes an adaptive weight cost function proposed for the A* algorithm: f(n)=g(n)+α(x)h(n) in, l represents the Euclidean distance between the current point and the starting point, L represents the Euclidean distance between the end 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; Construct a distance matrix based on the shortest path, solve the problem of the shortest path passing through each viewpoint, and determine the best access order of the viewpoint set; All viewpoints are set as segmentation points, and each viewpoint is iteratively set as a continuous point and then the path cost is calculated to find the optimal cost function. The improved DWA algorithm is used to generate the optimal speed space to obtain the optimal trajectory between viewpoints and confirm the optimal smooth path. The improved DWA algorithm specifically includes: Improved DWA evaluation function: G(v,w)=σ[αheading(v,w)+βdist(v,w)+yvel(v,w)+δenddist(v,w)+etaallgap(v,w)] Among them, enddist(v, w) is a sub-function for evaluating the distance to the target, allgap(v, w) is a sub-function for evaluating the distance to the global path, heading(v, w) is a sub-function for evaluating the direction angle, dist(v, w) is a sub-function for evaluating the distance between the obstacle and the robot body, vel(v, w) is a sub-function for evaluating the linear velocity of the current mobile robot, α, β, γ, δ, 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, respectively, and σ represents the normalization factor.

7. The active SLAM system of laser inertial navigation fusion according to claim 6, characterized in that: The global path planning in the autonomous exploration decision-making system specifically includes: Generate the path cost between the unexplored and currently explored local submaps by the improved A* algorithm, and generate the distance matrix between them; Solve the TSP problem in the distance matrix, define the points on the global path that intersect the local submap currently being explored as boundary points 1 and 2; apply local path planning between boundary points 1 and 2 to achieve two-stage planning; When the current local sub-map exploration is completed, the local path is simplified again to the shortest path from the current viewpoint to boundary point 1 and boundary point 2 so that the global path planning can be returned again. Based on this, the robot moves to the next exploration point to perform the exploration task.

8. An active SLAM method fused with laser inertial navigation, characterized in that: The method comprises performing active SLAM using the system described in any one of claims 1-7.

9. A computer device comprising a memory and a processor, wherein a computer program is stored in the memory, wherein: The steps of the method recited in claim 8 are performed when the processor runs the computer program stored in the memory.

10. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores a plurality of computer instructions, and the plurality of computer instructions are used to enable a computer to execute the method described in claim 8.

Images