CN119618199B - Active SLAM system, method and equipment based on laser inertial navigation fusion - Google Patents

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

Active SLAM system, method and equipment for laser inertial navigation fusion

Technical Field

The application relates to the technical field of active SLAM, in particular to an active SLAM system for laser inertial navigation fusion.

Background

Active SLAM (Active Simultaneous Local ization AND MAPPING) is one of the important development directions in the robot industry, and is mainly used for exploring unknown environments in scenes where communication signals are weak and mobile robots cannot be directly operated in real time, such as underground mine holes, mars and the like, so as to provide a priori environment for executing subsequent tasks.

At present, an active SLAM technology based on laser inertial navigation fusion has relatively mature results, but serious downslide occurs in view of both construction quality and exploration efficiency when facing a structural scene. The method is characterized in that the characteristics among the point cloud frames extracted from the structured scene are too consistent, so that accurate positioning and construction of a point cloud map cannot be effectively realized, the point cloud map constructed by the exploration robot is severely distorted due to positioning errors, so that an exploration task cannot be completed, and the exploration task cannot be completed by the robot due to the fact that a path planning algorithm based on exploration is not efficient enough and is easy to sink into local optimum.

Disclosure of Invention

The invention aims to solve the problem that the existing active SLAM technology based on laser inertial navigation fusion has lower construction quality and exploration efficiency when facing a structured scene, and provides an active SLAM system, method and equipment for laser inertial navigation fusion.

The invention is realized by the following technical scheme, in one aspect, the invention provides an active SLAM system for laser inertial navigation fusion, which comprises a real-time data acquisition system, a laser inertial navigation SLAM system, an autonomous exploration decision system and a bottom layer control system;

The real-time data acquisition system performs time synchronization on data acquired by different sensors, converts parameters under different coordinate systems and sends the processed data to the laser inertial navigation SLAM system;

the laser inertial navigation SLAM system comprises point cloud preprocessing, a laser inertial navigation odometer, back-end map construction and optimization and loop detection;

The point cloud preprocessing is used for preprocessing the point cloud and sending the preprocessed data to the laser inertial navigation odometer;

the laser inertial navigation odometer is used for matching the extracted point cloud with the current robot pose as an initial value, obtaining coarse pose estimation through continuous iterative optimization and sending the coarse pose estimation to a back-end map construction and optimization;

the back-end map construction and optimization are matched according to the rough pose, so that the point cloud map construction and pose optimization are completed;

The loop detection is used for map repositioning to optimize pose and ensure global consistency of the map;

The autonomous exploration decision system receives positioning information and a point cloud map output by the laser inertial navigation SLAM system, so that sampling viewpoints are randomly generated through surface coverage, the distance between the viewpoints is calculated through global path planning, the optimal exploration sequence is obtained, two points intersected through the global path planning are defined in the local exploration map to serve as boundary points, local path planning is realized in a local exploration area through DWA, and the vehicle travels to the next exploration area along the two boundary points;

The bottom layer control system performs smooth movement according to the track generated by the optimal speed space of DWA sampling, and is in a real-time updating state along with the moving environment, so that the acquisition and updating of system environment data are ensured, and the aim of independently exploring and building a map is fulfilled.

Further, the laser inertial navigation odometer is used for extracting characteristic points, and specifically comprises:

for the initial point cloud, feature points are used for feature extraction, wherein the feature points are divided into two types, namely plane points and corner points;

The points on the same scanning line are sequenced according to the curvature, the same laser scanning line is equally divided into a plurality of sections, each section is a sub-area, a plurality of points with the largest curvature of each section of sub-area are marked as corner points, and the rest points are plane points.

Further, the laser inertial navigation odometer is used for generating key points aiming at the diagonal points by adopting a K-Means clustering algorithm-based idea, and specifically comprises the following steps:

step1, dividing a plurality of sectors by taking a laser radar coordinate system as an origin, and then dividing corresponding sectors in sequence by calculating the horizontal angle of point clouds in each frame of data;

step 2, taking the initial point of each sector as the mass center of the first cluster of the sector, and judging whether the loss function of the subsequent point cloud of the sector and the mass center of the last cluster and the Manhattan distance exceed a threshold value or not;

step 3, if the threshold value is not exceeded, adding the point into the cluster, and if the threshold value is exceeded, adding a new cluster into the sector;

step 4, repeating the steps 2 and 3 for the subsequent sectors, and filtering the clustering with less point cloud numbers;

And 5, carrying out cluster merging when the loss function among clusters and the Manhattan distance are smaller than a threshold value, and finally averaging all point clouds in each cluster to obtain key points.

Further, the laser inertial navigation odometer is used for generating and matching descriptors aiming at key points, and specifically comprises the following steps:

dividing a cluster centroid point into a plurality of sectors by taking a current centroid point as a center, wherein each sector corresponds to one dimension, and then selecting the distance value of the centroid point, closest to the current centroid point, of each sector as the value of each dimension to generate a descriptor, wherein the direction vector from the current centroid point to the closest centroid point is taken as a main direction and is determined as a first sector, each sector is taken as a value according to the anticlockwise direction, and if the centroid point does not exist in the sector, the value is set to 0;

Searching a certain number of nearest key points, generating a certain number of descriptors by each point, and taking the value of the corresponding nearest key point as a final descriptor for each dimension;

establishing a vector table to avoid repeated calculation of the distance and direction of the corresponding point;

and finally, calculating the difference value of the nonzero elements between the descriptors, wherein the difference value is lower than a certain threshold value, the reliability is increased by one, and when the reliability is higher than a matching threshold value, the matching is considered to be successful.

Further, the loop detection is used for performing coarse matching through Euclidean distance and performing fine matching of Scan-Context descriptors to complete loop, and specifically comprises the following steps:

Firstly, carrying out point cloud division to generate descriptors, taking a laser radar coordinate system as a reference system, uniformly dividing a frame of three-dimensional laser radar point cloud into different bin sections in radial and rotation angle directions, wherein the point cloud is divided into a plurality of uniformly distributed circular rings when seen from the radial direction, the point cloud is divided into a plurality of uniformly distributed sectors when seen from the rotation direction, and the intersection overlapping area of the two is the bin section;

designating a number for each bin segment as a parameter for forming a description submatrix, wherein the parameter is determined by a z value of a highest point in a point cloud of each bin segment, and no point in the bin segment is assigned with 0;

expanding the divided point cloud to form a description submatrix, wherein the description submatrix is a Scan-Context descriptor;

performing large threshold screening on the historical frames based on the Euclidean distance, and generating a one-dimensional vector for each descriptor;

Storing the one-dimensional vector into Kd-Tree to perform nearest neighbor search with the one-dimensional vector in the historical frames, finding a plurality of historical frames meeting a threshold value, and performing accurate search based on the following formula after completing preliminary search:

where Iq, ic represent the current frame descriptor matrix and the history frame descriptor matrix, respectively, AndN _s is the number of the divided point cloud into sectors, and d (Iq, ic) is the best similarity score of the current frame descriptor matrix and the history frame descriptor matrix;

Performing N _s times of translation on the history frame descriptor matrix, performing N _s times of matching to ensure rotation invariance, performing the same operation on N history frames, selecting a minimum d (Iq, ic) value as an optimal similarity score of the history frames and the current frame, if the d (Iq, ic) value is smaller than a threshold value, satisfying loop, performing ICP iterative optimization on the history frames and the current frame, solving the optimal pose, and then optimizing the pose nearby the loop, and if the d (Iq, ic) value is larger than the threshold value, not satisfying loop, and not performing loop optimization.

Further, the local path planning in the autonomous exploration decision system specifically includes:

The cost function defining the segmentation point is:

wherein l _j is the length of the path of the j-th section, and p represents the penalty brought by each parking;

Calculating shortest paths between views using a modified a-algorithm for each pair of views, the modified a-algorithm comprising an adaptive weight cost function proposed for the a-algorithm:

f(n)=g(n)+α(x)h(n)

Wherein, the 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 point, and h (n) represents the Euclidean distance between the current node and the target node;

Constructing a distance matrix according to the shortest path, solving the problem that the path is shortest through each viewpoint, and confirming the optimal access sequence of the viewpoint set;

Setting all viewpoints as segmentation points, iteratively setting each viewpoint as a continuous point, calculating path cost, solving an optimal cost function, generating an optimal speed space by adopting an improved DWA algorithm to obtain an optimal track among viewpoints, and confirming an optimal smooth path, wherein the improved DWA algorithm specifically comprises the following steps:

improved DWA evaluation function:

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

Wherein enddist (v, w) is a distance-from-target evaluation sub-function, allgap (v, w) is a distance-from-global path interval evaluation sub-function, head (v, w) represents a direction angle evaluation sub-function, dist (v, w) represents a distance-from-obstacle-to-robot-body evaluation sub-function, vel (v, w) represents a current mobile robot linear velocity evaluation sub-function, α, β, γ, δ, μ represent weights of head (v, w), dist (v, w), vel (v, w), enddist (v, w), allgap (v, w) in the evaluation function, σ represents a normalization factor, respectively.

Further, the global path planning in the autonomous exploration decision system specifically includes:

Generating path cost between partial sub-maps which are not explored and explored by the current robot through an improved A-algorithm, and generating a distance matrix between the partial sub-maps;

solving the TSP problem in the distance matrix, defining points intersecting with the local sub-map in the current exploration on the global path as boundary points 1 and 2, and applying the local path planning between the boundary points 1 and 2 so as to achieve the double-stage planning;

and after the current local sub-map exploration is completed, the local path is simplified to be the shortest path from the current viewpoint to the boundary point 1 and the boundary point 2 again, so that the robot can return to the global path planning again, and the exploration task is executed based on the robot moving to the next exploration point.

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

In a third aspect, the present invention provides a computer device comprising a memory and a processor, the memory having stored therein a computer program which when executed by the processor performs the steps of an active SLAM method of laser inertial navigation fusion as described above.

In a fourth aspect, the present invention provides a computer readable storage medium having stored therein a plurality of computer instructions for causing a computer to perform an active SLAM method of laser inertial navigation fusion as described above.

The invention has the beneficial effects that:

The method aims at the problems that the characteristics among the point cloud frames extracted from the structured scene are too consistent, and accurate positioning and construction of the point cloud map cannot be effectively realized. The application provides a loop detection method for fusing Euclidean distance and Scan-Context on the basis of extracting point cloud features, which improves corner feature matching in front end matching into descriptor matching to enhance the efficiency of front end odometer inter-frame matching, and simultaneously aims at the problem that the accumulated errors of pose continuously increase along with the increase of the moving distance of a robot, and simultaneously aims at solving the problem that loop detection based on Euclidean distance cannot be precisely repositioned to optimize pose and a map so as to reduce the influence of poor global consistency of the map caused by the accumulated errors, as shown in fig. 5 and 6.

The invention provides an autonomous exploration method based on double-stage planning aiming at the problems that a path planning algorithm based on exploration is not efficient enough and is easy to fall into local optimum. The method comprises the steps of selecting a track with a local optimum instead of a global optimum due to the existence of an obstacle selection track, and then increasing the time for reaching a target point by deviating from a global path by using a traditional sliding window method in local path planning, and increasing a target distance and a global path planning distance cost function to optimize the track and global path constraint. Aiming at the problem that the traditional A-Star algorithm in global path planning is large in searching time, self-adaptive weight is added to a heuristic function, and a point jump strategy is added to the A-Star algorithm to accelerate searching, as shown in fig. 7 and 8.

The laser inertial navigation SLAM system module solves the problem that the point cloud map constructed by the exploration robot is severely distorted due to positioning errors because the characteristics among the point cloud frames extracted in the structural scene are too consistent and the accurate positioning and the construction of the point cloud map cannot be effectively realized. The realization of the active SLAM method requires that the mobile robot has the capability of positioning and mapping and the capability of generating a search path, which are indispensable. The premise of generating the search path is to have a map of the current environment and pose information of the current robot. The autonomous exploration decision system, namely the real-time updating of the unknown scene environment map, can plan a path capable of increasing the boundary coverage under the current known map information.

The method is suitable for exploration of unknown environments in scenes such as underground mine holes and mars, where communication signals are weak and the mobile robots cannot be directly controlled in real time, so that prior environments are provided for executing subsequent tasks.

Drawings

In order to more clearly illustrate the technical solution of the present application, the drawings that are needed in the embodiments will be briefly described below, and it will be obvious to those skilled in the art that other drawings can be obtained from these drawings without inventive effort.

FIG. 1 is a flowchart of loop detection for Euclidean distance and Scan-Context fusion of the present invention;

FIG. 2 is a point cloud partition diagram of the present invention;

FIG. 3 is a descriptor matrix generation diagram of the present invention;

FIG. 4 is a schematic diagram of a partial path plan;

FIG. 5 is a partial diagramming effect of the present invention;

FIG. 6 is an overall mapping effect of the present invention;

FIG. 7 is a graph showing the overall search results of the present invention;

FIG. 8 is a diagram of the effect of the exploration trajectory of the present invention;

FIG. 9 is a schematic diagram of the effect of active SLAM experiments on small scenes in a warehouse;

fig. 10 is a schematic diagram of the experimental effect of the active SLAM of the corridor large scene.

Detailed Description

Embodiments of the present invention are described in detail below, examples of which are illustrated in the accompanying drawings, wherein the same or similar reference numerals refer to the same or similar elements or elements having the same or similar functions throughout. The embodiments described below by referring to the drawings are exemplary and intended to illustrate the present invention and should not be construed as limiting the invention.

The first embodiment is an active SLAM system for laser inertial navigation fusion, which comprises a real-time data acquisition system, a laser inertial navigation SLAM system, an autonomous exploration decision system and a bottom layer control system;

The real-time data acquisition system is responsible for data setting, time synchronization of data acquired by different sensors and conversion of parameters under different coordinate systems are completed, the processed data are stably sent to the laser inertial navigation SLAM system, and the part must have real-time stability so as to ensure sustainable operation of a subsequent algorithm.

The laser inertial navigation SLAM system comprises four parts of point cloud preprocessing, laser inertial navigation odometer, back-end map construction and optimization and loop detection, and is characterized in that the system comprises a laser inertial navigation system and a laser inertial navigation system

Firstly, sensor data is sent to point cloud preprocessing, and is sent to a laser inertial navigation odometer after downsampling, ground separation, feature extraction and the like are carried out on the point cloud,

The part takes the pose provided by the extracted point cloud through the IMU as an initial value to be matched, obtains coarse pose estimation through continuous iterative optimization and sends the coarse pose estimation to the back end,

And then the rear end completes the point cloud map construction and pose optimization based on ScanToMap matching according to the rough pose.

Wherein loop detection is used for map repositioning to optimize pose and ensure global consistency of the map.

The autonomous exploration decision system receives positioning information and a point cloud map output by the laser inertial navigation SLAM system, so that sampling viewpoints are randomly generated through surface coverage, the distance between the viewpoints is calculated through global path planning, an optimal exploration sequence is obtained through solving a tourist problem, two points intersected with the global path planning are defined in a local exploration map to serve as boundary points, local exploration is realized in a local exploration area through DWA, and when the local exploration is finished, the next exploration area is driven along the two boundary points, so that the consistency of the global path planning is ensured.

The bottom layer control system performs smooth movement according to the track generated by the DWA sampling optimal speed space, and the system environment data is ensured to be acquired and updated along with the real-time updating state of the mobile environment, so that the purpose of independently exploring and constructing a graph is achieved, and an active SLAM system is realized.

In the embodiment, the laser inertial navigation SLAM system module solves the problem that the point cloud map constructed by the exploration robot is severely distorted due to positioning errors because the characteristics among the point cloud frames extracted in the structural scene are too consistent and the accurate positioning and the construction of the point cloud map cannot be effectively realized.

The realization of the active SLAM method requires that the mobile robot has the capability of positioning and mapping and the capability of generating a search path, which are indispensable. The premise of generating the search path is to have a map of the current environment and pose information of the current robot. The autonomous exploration decision system, namely the real-time updating of the unknown scene environment map, can plan a path capable of increasing the boundary coverage under the current known map information.

In a second embodiment, the present embodiment is further defined by the active SLAM system with laser inertial navigation fusion according to the first embodiment, where the function of the laser inertial navigation odometer is further defined, and specifically includes:

the laser inertial navigation odometer is used for extracting characteristic points and specifically comprises the following steps:

The initial point cloud uses feature points for feature extraction. The feature points are divided into two types, plane points and corner points. The plane point is a point on the three-dimensional space plane with a small curvature. Corner points are points at the edges of a three-dimensional space with a large curvature. The curvature calculation is shown in formula (1):

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 a set of consecutive points on the same laser scan line. i, j represents a number in the set, As the coordinates of the current point i,Is a set of 5 points to the left and 5 points to the right of the current point. The points on the same scan line are ordered according to the curvature c.

Before selecting the feature points, some unreliable points are first excluded, and these unreliable points are classified into two types, a blocked point and a parallel beam point. After these unreliable points are eliminated, feature points can be extracted. In order to make the characteristic points uniformly distributed, the same laser scanning line is equally divided into a plurality of sections, for example 6 sections, and each section is a sub-area. A plurality of points (such as 20 points) with the largest curvature of each segment of the sub-area are marked as corner points, and the rest points are plane points.

In a third embodiment, the present embodiment is further defined by the active SLAM system with laser inertial navigation fusion according to the second embodiment, where the function of the laser inertial navigation odometer is further defined, and specifically includes:

Because the corner points have the advantages of high calculation speed, obvious characteristics and the like compared with the face points, the key points are generated by adopting the thought based on the K-Means clustering algorithm, the clustering result is not globally optimal but locally optimal because the K-Means clustering algorithm is easily influenced by initial values and abnormal points, sample points can be divided into single classes, and the fact that when the horizontal angle of the target point cloud is in an approximate range, the large probability comes from the same object and the clustering is needed to be carried out into the same cluster is considered. Therefore, the laser radar coordinate system is taken as the center to be divided into N sectors on the horizontal plane, and then the sectors are taken as units to be clustered in sequence. And minimizing the loss function corresponding to the clustering result. The loss function may be defined as the sum of squares of errors of the samples from the center point of the cluster to which the samples belong, and the formula is shown in (2):

Where x _i represents the ith sample, c _i is the cluster to which xi belongs, μ _ci represents the center point to which the cluster corresponds, and M is the total number of samples.

The method comprises the following specific steps:

1) And dividing N sectors by taking a laser radar coordinate system as an origin, and then dividing the horizontal angle of the point cloud in each frame of data into corresponding sectors in sequence.

2) And taking the initial point of each sector as the mass center of the first cluster of the sector, and judging whether the loss function between the subsequent point cloud of the sector and the mass center of the last cluster and the Manhattan distance exceed a threshold value.

3) If the threshold is not exceeded, adding the point to the cluster, if the threshold is exceeded, adding a new cluster to the sector, and discarding the outlier.

4) And (3) repeating the steps 2 and 3 in the subsequent sector, and finally filtering the clustering with less point cloud numbers.

5) Cluster merging is performed when the inter-cluster loss function and the manhattan distance are less than a threshold. And finally, averaging all point clouds in each cluster to obtain a centroid, namely the key point.

In a fourth embodiment, the present embodiment is further defined by the active SLAM system with laser inertial navigation fusion according to the third embodiment, where the function of the laser inertial navigation odometer is further defined, and specifically includes:

Firstly, dividing a cluster centroid point into N sectors by taking a current centroid point M0 as a center, wherein each sector corresponds to one dimension, and then selecting a distance value of a centroid point closest to the M0 of each sector as a value of each dimension, thereby generating a descriptor.

In order to ensure invariance of the searching main direction posture, the direction vector from the current point M0 to the nearest centroid point M1 is taken as the main direction and is determined as a first sector, and each sector is sequentially valued according to anticlockwise. If the sector does not have a centroid point, then set to 0.

The direction from the current point to the other points M _i (i+.1) is expressed asAnd main directionThe angle between them determines to which sector k _i belongs. The calculation formula of the angle is as follows:

Wherein D _i is defined as follows:

Meanwhile, in order to solve the interference of abnormal key points, a certain number of nearest key points are searched, a certain number of descriptors are generated by each point, and then the numerical value of the corresponding nearest key point is taken as a final descriptor for each dimension. A vector table is built to avoid repeated calculation of distance and direction for the corresponding points. And finally, calculating the difference value of the nonzero elements between the descriptors, wherein the difference value is lower than a certain threshold value, the reliability is increased by one, and when the reliability is higher than a matching threshold value, the matching is considered to be successful.

After successful matching, the laser radar interframe motion is solved by using Gaussian Newton or Levenberg-Marquardt method (Levenberg-Marquardt Algorithm, LM) nonlinear optimization, wherein the ground point feature optimization is firstly used [ t _z,θ_roll,θ_pitch ], and then the angular point descriptor and the face point matching optimization in non-ground points are used [ t _x,t_y,θ_yaw ], so that the relative pose change quantity of the robot is obtained to generate the front-end odometer.

In a fifth embodiment, the present embodiment is further defined by the active SLAM system with laser inertial navigation fusion according to the first embodiment, where the loop detection is further defined by the method specifically including:

The loop detection is used for performing coarse matching through Euclidean distance and then performing fine matching of Scan-Context descriptors to complete loop, and specifically comprises the following steps:

Firstly, carrying out point cloud division to generate descriptors, taking a laser radar coordinate system as a reference system, uniformly dividing a frame of three-dimensional laser radar point cloud into different bin sections in radial and rotation angle directions, wherein the point cloud is divided into a plurality of uniformly distributed circular rings when seen from the radial direction, the point cloud is divided into a plurality of uniformly distributed sectors when seen from the rotation direction, and the intersection overlapping area of the two is the bin section;

designating a number for each bin segment as a parameter for forming a description submatrix, wherein the parameter is determined by a z value of a highest point in a point cloud of each bin segment, and no point in the bin segment is assigned with 0;

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

Expanding the divided point cloud to form a description submatrix, wherein the description submatrix is a Scan-Context descriptor;

performing large threshold screening on the historical frames based on the Euclidean distance, and generating a one-dimensional vector for each descriptor;

Storing the one-dimensional vector into Kd-Tree to perform nearest neighbor search with the one-dimensional vector in the historical frames, finding a plurality of historical frames meeting a threshold value, and performing accurate search based on the following formula after completing preliminary search:

where Iq, ic represent the current frame descriptor matrix and the history frame descriptor matrix, respectively, AndN _s is the number of the divided point cloud into sectors, and d (Iq, ic) is the best similarity score of the current frame descriptor matrix and the history frame descriptor matrix;

performing N _s times of translation on the history frame descriptor matrix, performing N _s times of matching to ensure rotation invariance, performing the same operation on N history frames, selecting a minimum d=d (Iq, ic) value as an optimal similarity score of the history frames and the current frame, if the d value is smaller than a threshold value, satisfying loop, performing ICP iterative optimization on the history frames and the current frame, solving the optimal pose, optimizing the pose nearby the loop, and if the d value is larger than the threshold value, not satisfying loop, and not performing loop optimization.

In this embodiment, loop detection based on the Scan-Context descriptor is different from loop detection based on the euclidean distance, loop detection based on the euclidean distance performs loop detection according to the euclidean distance between the current frame and the historical frame, so that the pose outputted after SLAM back-end optimization is depended on, loop detection based on the Scan-Context descriptor performs registration according to the similarity of the current frame point cloud and the historical frame point cloud, and does not depend on a positioning result, so that higher efficiency is achieved, but the method performs loop matching on each newly added key frame once, searches whether loop exists or not, greatly improves algorithm operation time, and therefore introduces a loop detection algorithm integrating the euclidean distance and the Scan-Context, performs rough matching through the euclidean distance, performs fine matching of the Scan-Context descriptor to complete loop, and improves loop returning efficiency. The algorithm flow chart is shown in fig. 1.

In a sixth embodiment, the present embodiment is further defined by the active SLAM system with laser inertial navigation fusion according to the first embodiment, where the local path planning in the autonomous exploration decision system is further defined, and specifically includes:

The local path planning in the autonomous exploration decision system specifically comprises the following steps:

The local path planning is to generate an optimal exploration path meeting dynamics constraint aiming at the optimal viewpoint set. But depending on the extracted viewpoint set alone, smoothness and continuity of the path cannot be ensured as shown in fig. 4. In fig. 4, the green box is a local sub-map maintained by global path planning, and the brown circle and the blank circle are extracted viewpoints, wherein the brown circle is a continuous viewpoint, and the blank circle is a segmented viewpoint. Due to the existence of the segmentation point, the robot has to stop at the segmentation point to reach the next viewpoint by switching directions under the premise of ensuring the optimal path. Therefore, in order to ensure that the path cost value is minimum, the cost function of defining the segmentation point is as follows:

where l _j is the length of the jth path, and p represents the penalty incurred for each stop.

Calculating the shortest path between the viewpoints by adopting an improved A-algorithm between each pair of viewpoints;

The result of the total evaluation function selecting node is greatly influenced by the heuristic function, when the current point is far away from the target point, h (n) is large, although the robot can move towards the target point, the shortest path cannot be determined, when the current point is close to the target point, h (n) is small, although the shortest path is ensured, the searching efficiency is reduced due to the lack of distance constraint with the target point, and the time for searching the target point is long, so that the self-adaptive weight cost function provided for the A-type algorithm:

f(n)=g(n)+α(x)h(n)

Wherein, the 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 point, and h (n) represents the Euclidean distance between the current node and the target node;

the weight is introduced into a standard normal function, 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, so that when the current point is far from the target point, h (n) is not too large, the robot can move towards the target point and can determine the shortest path, when the current point is near to the target point, h (n) is smaller but not too small, the shortest path and the distance constraint of the target point can be ensured, and the search efficiency can be ensured, so that the algorithm running time can be ensured.

Constructing a distance matrix according to the shortest path, solving the problem that the path is shortest through each viewpoint, and confirming the optimal access sequence of the viewpoint set;

Setting all the viewpoints as segmentation points, iteratively setting each viewpoint as a continuous point, calculating path cost, solving an optimal cost function, generating an optimal speed space by adopting an improved DWA algorithm to obtain an optimal track among the viewpoints, and confirming an optimal smooth path;

Because the conventional DWA algorithm only needs to sequentially reach the path points planned by the global path planning to complete the local path planning task to reach the target point, when an obstacle exists, the DWA algorithm may take the safest way to select the trajectory away from the obstacle, so that the trajectory away from the global path planning plan may deviate, and the time for reaching the target point increases. Thus, the method is improved based on the traditional DWA, and a distance-to-target evaluation sub-function and a distance-to-global path interval evaluation sub-function are added on the traditional evaluation sub-function, wherein the improved DWA algorithm specifically comprises the following steps:

improved DWA evaluation function:

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

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

Wherein enddist (v, w) is a distance-from-target evaluation sub-function, allgap (v, w) is a distance-from-global path interval evaluation sub-function, head (v, w) represents a direction angle evaluation sub-function, dist (v, w) represents a distance-from-obstacle-to-robot-body evaluation sub-function, vel (v, w) represents a current mobile robot linear velocity evaluation sub-function, α, β, γ, δ, μ represent weights of head (v, w), dist (v, w), vel (v, w), enddist (v, w), allgap (v, w) in the evaluation function, σ represents a normalization factor, respectively.

In a seventh embodiment, the present embodiment is further defined by the active SLAM system with laser inertial navigation fusion according to the sixth embodiment, where global path planning in the autonomous exploration decision system is further defined, and specifically includes:

The global path planning in the autonomous exploration decision system specifically comprises the following steps:

Generating path cost between partial sub-maps which are not explored and explored by the current robot through an improved A-algorithm, and generating a distance matrix between the partial sub-maps;

solving the TSP problem in the distance matrix, defining points intersecting with the local sub-map in the current exploration on the global path as boundary points 1 and 2, and applying the local path planning between the boundary points 1 and 2 so as to achieve the double-stage planning;

and after the current local sub-map exploration is completed, the local path is simplified to be the shortest path from the current viewpoint to the boundary point 1 and the boundary point 2 again, so that the robot can return to the global path planning again, and the exploration task is executed based on the robot moving to the next exploration point.

In the present embodiment, the map is divided into individual local sub-maps in the global path plan, and the local sub-maps are divided into three states of "unexplored", "still-explored" and "explored completed". The three states are distinguished by the 'surface' stored in the local sub-map, the uncovered surface stored in the local sub-map indicates that the local sub-map is in a 'still explored' state, the covered surface stored in the local sub-map indicates that the local sub-map is in an 'explored complete' state, and the local sub-map stored in the local sub-map is empty indicates that the local sub-map is in an 'explored' state. The global path planning aims at generating an optimal path penetrating through the center point of each local sub-map and the current viewpoint in the map generated by the current laser radar point cloud, and the point cloud map and the generated path are updated in real time along with the movement of the robot.

An eighth embodiment is an example of an active SLAM system for laser inertial navigation fusion as described above, specifically including:

the real-time data acquisition part is responsible for data setting, carrying out time synchronization on data acquired by different sensors, completing conversion on parameters under different coordinate systems, and stably transmitting the processed data to the laser inertial navigation SLAM system, wherein the part is required to have real-time stability so as to ensure sustainable operation of a subsequent algorithm.

The laser inertial navigation SLAM system is divided into four parts, namely point cloud preprocessing, laser inertial navigation odometer, rear end map construction and optimization and loop detection, wherein sensor data are firstly sent to the point cloud preprocessing, the sensor data are sent to the laser inertial navigation odometer after downsampling, ground separation, feature extraction and the like are carried out on the point cloud, the extracted point cloud is matched with the pose of a current robot serving as an initial value through an IMU, coarse pose estimation is obtained through continuous iterative optimization and is sent to the rear end, and then the rear end completes the point cloud map construction and pose optimization according to the matching of the coarse pose based on ScanToMap. Wherein loop detection is used for map repositioning to optimize pose and ensure global consistency of the map.

Further, the laser inertial navigation odometer comprises a feature point extraction method, and specifically comprises the following steps:

The initial point cloud uses feature points for feature extraction. The feature points are divided into two types, plane points and corner points. The plane point is a point on the three-dimensional space plane with a small curvature. Corner points are points at the edges of a three-dimensional space with a large curvature. The curvature calculation is shown in formula (1):

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 a set of consecutive points on the same laser scan line. i, j represents a number in the set, As the coordinates of the current point i,Is a set of 5 points to the left and 5 points to the right of the current point. The points on the same scan line are ordered according to the curvature c.

Before selecting the feature points, some unreliable points are first excluded, and these unreliable points are classified into two types, a blocked point and a parallel beam point. After these unreliable points are eliminated, feature points can be extracted. In order to make the characteristic points uniformly distributed, the same laser scanning line is equally divided into 6 sections, and each section is a sub-area. The 20 points with the largest curvature of each segment of the subregion are marked as corner points, and the rest points are plane points.

Further, the laser inertial navigation odometer also comprises improved key point generation, and specifically comprises the following steps:

Because the corner points have the advantages of high calculation speed, obvious characteristics and the like compared with the face points, the key points are generated by adopting the thought based on the K-Means clustering algorithm, the clustering result is not globally optimal but locally optimal because the K-Means clustering algorithm is easily influenced by initial values and abnormal points, sample points can be divided into single classes, and the fact that when the horizontal angle of the target point cloud is in an approximate range, the large probability comes from the same object and the clustering is needed to be carried out into the same cluster is considered. Therefore, the laser radar coordinate system is taken as the center to be divided into N sectors on the horizontal plane, and then the sectors are taken as units to be clustered in sequence. And minimizing the loss function corresponding to the clustering result. The loss function may be defined as the sum of squares of errors of the samples from the center point of the cluster to which the samples belong, and the formula is shown in (2):

Where x _i represents the ith sample, c _i is the cluster to which x _i belongs, μ _ci represents the center point to which the cluster corresponds, and M is the total number of samples.

The method comprises the following specific steps:

1) And dividing N sectors by taking a laser radar coordinate system as an origin, and then dividing the horizontal angle of the point cloud in each frame of data into corresponding sectors in sequence.

2) And taking the initial point of each sector as the mass center of the first cluster of the sector, and judging whether the loss function between the subsequent point cloud of the sector and the mass center of the last cluster and the Manhattan distance exceed a threshold value.

3) If the threshold is not exceeded, adding the point to the cluster, if the threshold is exceeded, adding a new cluster to the sector, and discarding the outlier. And (3) repeating the steps 2 and 3 in the subsequent sector, and finally filtering the clustering with less point cloud numbers.

4) Cluster merging is performed when the inter-cluster loss function and the manhattan distance are less than a threshold. And finally, averaging all point clouds in each cluster to obtain a centroid, namely the key point.

Further, the laser inertial navigation odometer further comprises improved descriptor generation and matching, including in particular:

Firstly, dividing a cluster centroid point into N sectors by taking a current centroid point M0 as a center, wherein each sector corresponds to one dimension, and then selecting a distance value of a centroid point closest to the M0 of each sector as a value of each dimension, thereby generating a descriptor.

In order to ensure invariance of the searching main direction posture, the direction vector from the current point M0 to the nearest centroid point M1 is taken as the main direction and is determined as a first sector, and each sector is sequentially valued according to anticlockwise. If the sector does not have a centroid point, then set to 0.

The direction from the current point to the other points M _i (i+.1) is expressed asAnd main directionThe angle between them determines to which sector k _i belongs. The calculation formula of the angle is as follows:

Wherein D _i is defined as follows:

Meanwhile, in order to solve the interference of abnormal key points, a certain number of nearest key points are searched, a certain number of descriptors are generated by each point, and then the numerical value of the corresponding nearest key point is taken as a final descriptor for each dimension. A vector table is built to avoid repeated calculation of distance and direction for the corresponding points. And finally, calculating the difference value of the nonzero elements between the descriptors, wherein the difference value is lower than a certain threshold value, the reliability is increased by one, and when the reliability is higher than a matching threshold value, the matching is considered to be successful.

After successful matching, the laser radar interframe motion is solved by using Gaussian Newton or Levenberg-Marquardt method (Levenberg-MarquardtAlgorithm, LM) nonlinear optimization, wherein the ground point feature optimization is firstly used [ t _z,θ_roll,θ_pitch ], and then the angular point descriptor and the face point matching optimization in non-ground points are used [ t _x,t_y,θ_yaw ], so that the relative pose change quantity of the robot is obtained to generate a front-end odometer.

Further, loop detection specifically includes:

The loop detection based on the Scan-Context descriptor is different from the loop detection based on the Euclidean distance, loop detection based on the Euclidean distance is carried out according to the Euclidean distance between the current frame and the historical frame, the pose outputted after SLAM back end optimization is too depended, loop detection based on the Scan-Context descriptor is registered according to the similarity of the point cloud of the current frame and the point cloud of the historical frame, and the positioning result is not depended, therefore, higher efficiency is achieved, but the method carries out one-time loop matching on each newly added key frame, searches whether loop exists or not, greatly improves algorithm operation time, and therefore a loop detection algorithm fused by Euclidean distance and Scan-Context is introduced, firstly carries out rough matching through the Euclidean distance, reduces the matching times of using Scan-Context, and then carries out fine matching of the Scan-Context descriptor to complete loop, so that loop efficiency is improved. The algorithm flow chart is shown in fig. 1, and the specific method is as follows:

Firstly, performing point cloud division to generate descriptors, as shown in fig. 2, taking a laser radar coordinate system as a reference system, uniformly dividing a frame of three-dimensional laser radar point cloud into different bin sections in radial and rotation angle directions, wherein the division method can be better adapted to sparsity of the point cloud, namely, the point cloud which is closer to the laser radar is about dense, and the point cloud which is farther from the laser radar is about sparse. Wherein the point cloud is divided into N _r evenly distributed rings, i.e. yellow parts in the figure, seen in radial direction, and N _s evenly distributed sectors, i.e. green parts in the figure, seen in rotational direction. The overlapping area of the two intersections is a bin segment, namely a black part. The partitioned point cloud can be expressed by the following formula:

where P represents a frame of point cloud, P _ij represents each small segment divided, i represents radial, and j represents rotational direction.

The furthest scan distance of the lidar is defined as L _max. The polar coordinates of the point from which the lidar emits are (r, θ), then i, j of the point can be expressed as:

Wherein the method comprises the steps of Indicating that only the integer part is reserved.

After the point cloud division is performed, a number needs to be designated for each bin segment as a parameter for forming the description submatrix. The parameter is determined by the z value of the highest point in the point cloud of each bin segment, and no point in the bin segment is assigned 0, namely:

after assignment, the divided point clouds are unfolded to form a description sub-matrix, and as shown in fig. 3, the description sub-matrix is a matrix of N _r×N_s, and the generated description sub-matrix is a Scan-Context descriptor.

As can be seen from fig. 3, although the circle has rotation invariance, if the orientation of the lidar changes slightly after becoming the 2-dimensional matrix, the scanning start position changes, so that the 2-dimensional matrix is shifted laterally greatly. So that no intuitive matching can be performed according to the gap between the two descriptors.

In order to solve the above problem, firstly, a large threshold value screening is performed on a history frame based on the Euclidean distance, the subsequent processing time of an algorithm is reduced, and then, a one-dimensional vector k is generated for each descriptor according to the fact that the overall point cloud density of each row is not changed although the matrix is laterally shifted, namely:

k=(ψ(r₁),ψ(r₂),...,ψ(r_Nr)) (9)

wherein:

Where r _i 0 represents the total number of bin segment values in the corresponding row that are non-zero.

Storing k values into Kd-Tree, performing nearest neighbor search with k values in historical frames, finding n historical frames meeting a threshold, and performing accurate search based on the following formula, namely:

where Iq, ic represent the current frame descriptor matrix and the history frame descriptor matrix, respectively, AndRespectively representing the column vector of the j-th column of the current frame and the column vector of the j-th column of the history frame.

Because the descriptor matrix is greatly translated, the historical frame descriptor matrix is translated for N _s times, namely the historical frame point cloud is rotated for 2 pi/N _s times, then is matched for N _s times to ensure rotation invariance, then N historical frames are operated in the same way, a minimum d value is selected as the optimal similarity score of the historical frames and the current frame, if the d value is smaller than a threshold value, loop-back is satisfied, ICP iterative optimization is performed on the historical frames and the current frame, the pose near the loop is optimized to optimize a map after the optimal pose is obtained, if the d value is larger than the threshold value, loop-back is not satisfied, and loop-back optimization is not performed.

The autonomous exploration decision system receives positioning information and a point cloud map output by the laser inertial navigation SLAM system, so that sampling viewpoints are randomly generated through surface coverage, the distance between the viewpoints is calculated through global path planning, an optimal exploration sequence is obtained through solving a tourist problem, two points intersected with the global path planning are defined in a local exploration map to serve as boundary points, local exploration is realized in a local exploration area through DWA, and when the local exploration is finished, the next exploration area is driven along the two boundary points, so that the consistency of the global path planning is ensured.

Further, for the definition of the boundary, specifically including:

defining the global environment to be explored by the current robot as Wherein the local environment through which no obstruction can pass is defined asThe pose of the laser radar on the robot is the viewpoint v epsilon SE (3), v= [ p _v,q_v],p_v ] represents the position information, and q _v represents the pose information. Surfaces are defined to represent generalized boundaries of free and non-free environments, where the non-free environments include occupied environments and unknown environments. The surface perceived by viewpoint v is expressed asAll perceived surfaces can be expressed as:

Wherein, the Representing a set of all viewpoints, S representing all covered surfacesAnd an uncovered surfaceWhen the current viewpoint v _current and the uncovered surface are givenThe shortest path T formed by the viewpoint sets v1, v2,..And as exploration proceeds, a portion ofWill update to S _cov and will have a new uncovered surface added to itIn (3), the shortest path is formed periodically, so that the whole surface (boundary) coverage can be realized.

Further, for view sampling, specifically including:

The precondition for forming periodic shortest paths is to find the optimal set of viewpoints. Firstly, defining the center of the surface as p _s epsilon M and the normal vector as n _s∈R³ in the covered surface, wherein the precondition that the surface is covered by the view point v is that the formula and the formula are required to be satisfied:

|p_s-p_v|≤D (13)

Where D represents a planar distance constraint.

n_s·(p_v-p_s)/|n_s|·|(p_v-p_s)|≥T(14)

Where T represents the planar directional constraint.

As the viewpoint increasesThe smaller the range of the viewpoint, but the less rewards are provided by the newly added viewpoint, where rewards refer to the range of uncovered surfaces scanned by the viewpoint lidar. This is because the closer the adjacent view is, the higher the scan overlap ratio thereof is. So that rewards of one view are closely related to previous view selections. I.e. the rewards of the viewpoint v _i need to be updated toThe specific steps of the viewpoint sampling algorithm are therefore as follows:

a uniform set of candidate viewpoints is generated from the communicable regions within the local planning range.

And updating rewards according to a rewards updating formula for the group of viewpoints, and storing the rewards according to the rewards of the group of viewpoints into a priority queue as priority.

K groups of view subsets covering the local range are randomly generated by k iterations. Three views in each group of view subsets are necessary options, namely a current view, a boundary view entering the local area, and a boundary view exiting the local area, wherein the two boundary views are used for guaranteeing the continuity of the local path and the global path. The other view points are selected from the priority queue, the selection probability is in direct proportion to the view point rewards, and due to the problem that the observation area is covered between the adjacent view points, the rewards need to be updated in real time, and when the queue is empty or the rewards of the newly added view points are lower than a threshold value, the view point sampling is finished once.

The path cost generated by each group of viewpoint samples is calculated, and the viewpoint set with the lowest cost is selected as the optimal viewpoint set.

Further, the local path planning specifically includes:

The viewpoint set of the local exploration map can be obtained through the viewpoint sampling, so that the local path planning is to generate an optimal exploration path meeting the dynamics constraint aiming at the optimal viewpoint set. But depending on the extracted viewpoint set alone, smoothness and continuity of the path cannot be guaranteed as shown in fig. 4:

in fig. 4, the green box is a local sub-map maintained by global path planning, and the brown circle and the blank circle are extracted viewpoints, wherein the brown circle is a continuous viewpoint, and the blank circle is a segmented viewpoint. Due to the existence of the segmentation point, the robot has to stop at the segmentation point to reach the next viewpoint by switching directions under the premise of ensuring the optimal path. Therefore, in order to ensure that the path cost value is minimum, the cost function of defining the segmentation point is as follows:

Where l _j is the length of the jth path, and p represents the penalty incurred for each stop. Therefore, in order to obtain the minimum cost function, a modified a-algorithm is first used to calculate the shortest path between the views for each pair of views, the modified method is described in detail below, and then a distance matrix is constructed according to the shortest path. Solving the problem with the shortest path passing through each viewpoint is converted into a travel business problem (TSP), and the optimal access sequence of the viewpoint set can be confirmed after solving the problem through the Lin-KERNIGHAN algorithm. And setting all the viewpoints as segmentation points, iteratively setting each viewpoint as a continuous point, calculating path cost, solving an optimal cost function, generating an optimal speed space by adopting a DWA algorithm to obtain an optimal track among the viewpoints, and confirming an optimal smooth path.

Because the conventional DWA algorithm only needs to sequentially reach the path points planned by the global path planning to complete the local path planning task to reach the target point, when an obstacle exists, the DWA algorithm may take the safest way to select the trajectory away from the obstacle, so that the trajectory away from the global path planning plan may deviate, and the time for reaching the target point increases. Therefore, the method is improved based on the traditional DWA, and a distance evaluation sub-function from a target and a distance evaluation sub-function from a global path interval are added on the traditional evaluation sub-function.

Wherein, the distance evaluation sub-function of the distance from the target is:

Wherein, (x _end,y_end) is a local sub-target point generated by path planning between each viewpoint of the robot, (x _current,y_current) is a position of a track tail end generated by a current sampling space of the robot, and (x _begin,y_begin) is a last local sub-target point of the current local sub-target point. The evaluation sub-function adds the distance constraint with the target point, so that the robot can be ensured to select a track nearer to the target point to reach the target point faster.

The distance global path interval evaluation sub-function is:

in the formula, gap is the interval from the end point (x _i,y_i) of the track generated in the current robot sampling speed space to the connection line ax+by+c=0 between the current local target point and the last local target point, and the formula is as follows:

the evaluation sub-function adds a spacing constraint with the global path plan, and the higher the allgap value is, the higher the track evaluation is, the closer the track evaluation is to the global path.

The improved DWA evaluation function is therefore:

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

the calculation steps of the improved DWA algorithm are as follows:

a) Initializing an initial position, a local sub-node, a path generated by an A-algorithm, a DWA evaluation function, a robot linear acceleration, an angular acceleration, a maximum speed and a minimum speed, and acquiring the current robot gesture through an IMU.

B) And generating a sampling track for the sampling speed space according to the characteristics of the differential chassis.

C) And (3) evaluating the priority of the sampling track based on the improved DWA evaluation function, and sending the speed space (v, w) corresponding to the track with the highest priority to the chassis.

D) And c, judging whether the tail end of the track is an end point, if not, repeating the steps b and c, and if so, ending the task and ending the operation.

Further, the global path planning specifically includes:

The map is divided into a plurality of local sub-maps in the global path planning, and the local sub-maps are divided into three states of unexplored, still explored and explored. The three states are distinguished by the 'surface' stored in the local sub-map, the uncovered surface stored in the local sub-map indicates that the local sub-map is in a 'still explored' state, the covered surface stored in the local sub-map indicates that the local sub-map is in an 'explored complete' state, and the local sub-map stored in the local sub-map is empty indicates that the local sub-map is in an 'explored' state. The global path planning aims at generating an optimal path penetrating through the center point of each local sub-map and the current viewpoint in the map generated by the current laser radar point cloud, and the point cloud map and the generated path are updated in real time along with the movement of the robot. The specific process is as follows:

a) The method comprises the steps of generating unexplored path cost among local sub-maps explored by a current robot through an improved A-algorithm, and generating a distance matrix among the path cost.

B) And solving the TSP problem in the distance matrix, and defining points on the global path intersecting with the local sub-map in the current exploration as boundary points 1 and 2. The local path planning is applied between boundary point 1 and boundary point 2, so that a two-stage planning is achieved.

C) And after the current local sub-map exploration is completed, the local path is simplified to be the shortest path from the current viewpoint to the boundary point 1 and the boundary point 2 again, so that the robot can return to the global path planning again, and the exploration task is executed based on the robot moving to the next exploration point.

Because the current node can acquire reachable neighbor nodes in eight directions around as the next mobile node in the running process of the traditional A-type algorithm, the algorithm can bring redundant searching of redundant burden under the scene of larger map scale, and the computational complexity can be obviously increased along with the scene, so that the heuristic function of the A-type algorithm is improved.

The problem that the four-wheel differential chassis cannot realize the kinematic constraint of left and right lateral movement is solved, so that the Euclidean distance is adopted to calculate the cost function.

f(n)=g(n)+h(n) (20)

Wherein g (n) is represented as:

g (f) represents the accumulated distance of the parent node from the starting node, (x _current,y_current) represents the current node position, (x _father,y_father) represents the parent node position, and g (n) represents the distance of the current node from the starting node.

H (n) is expressed as:

In the formula, (x _goal,y_goal) represents the position of the target node, and the h (n) heuristic function represents the Euclidean distance between the current node and the target node.

It can be known from the above that the result of selecting the node 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, the shortest path cannot be determined, when the current point is near to the target point, h (n) is small, although the shortest path is ensured, the search efficiency is reduced due to lack of distance constraint with the target point, and the time for searching the target point is long. An adaptive weight cost function is therefore proposed for the a-algorithm:

f(n)=g(n)+α(x)h(n) (23)

wherein:

the weight is introduced into a standard normal function, 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, so that when the current point is far from the target point, h (n) is not too large, the robot can move towards the target point and can determine the shortest path, when the current point is near to the target point, h (n) is smaller but not too small, the shortest path and the distance constraint of the target point can be ensured, and the search efficiency can be ensured, so that the algorithm running time can be ensured.

The calculation steps of improvement a are:

a) Initializing a starting point position A and an end point position B, and generating a grid map by using an improved A-scale algorithm.

B) The start point is added to open_set and set to the highest priority.

C) Searching the jump points except the nodes in the close_set and the obstacles in the 8 neighborhood directions of the current node, stopping searching the directions if the jump points are not found by expanding to the obstacles, adding the searched jump points into the open_set, and storing the history starting point into the close_set.

D) And (3) comparing the jump points in the Open_set through the adaptive weight cost function (23) to select the optimal jump point as a new starting point, and executing the steps 3 and 4 again until the end point position is reached.

The bottom layer control system performs smooth movement according to the track generated by the DWA sampling optimal speed space, and is in a real-time update state along with the mobile environment, so that the acquisition and update of system environment data are ensured, the purpose of independently exploring and constructing a map is achieved, and an active SLAM system is realized.

The invention provides an active SLAM system for laser inertial navigation fusion, which has the following technical effects:

(1) Firstly, aiming at the problem that the characteristics among the point cloud frames extracted from the structured scene are too consistent, and accurate positioning and construction of a point cloud map cannot be effectively realized. The application provides a loop detection method for fusing Euclidean distance and Scan-Context on the basis of extracting point cloud features, which improves corner feature matching in front end matching into descriptor matching to enhance the efficiency of front end odometer inter-frame matching, and simultaneously aims at the problem that the accumulated errors of pose continuously increase along with the increase of the moving distance of a robot, and simultaneously aims at solving the problem that loop detection based on Euclidean distance cannot be precisely repositioned to optimize pose and a map so as to reduce the influence of poor global consistency of the map caused by the accumulated errors, as shown in fig. 5 and 6.

(2) Aiming at the problems that a path planning algorithm based on exploration is not efficient enough and is easy to fall into local optimum, an autonomous exploration method based on double-stage planning is provided. The method comprises the steps of selecting a track with a local optimum instead of a global optimum due to the existence of an obstacle selection track, and then increasing the time for reaching a target point by deviating from a global path by using a traditional sliding window method in local path planning, and increasing a target distance and a global path planning distance cost function to optimize the track and global path constraint. Aiming at the problem that the traditional A-Star algorithm in global path planning is large in searching time, self-adaptive weight is added to a heuristic function, and a point jump strategy is added to the A-Star algorithm to accelerate searching, as shown in fig. 7 and 8.

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

Claims (6)

1. The active SLAM system for laser inertial navigation fusion is characterized by comprising a real-time data acquisition system, a laser inertial navigation SLAM system, an autonomous exploration decision system and a bottom layer control system;

The real-time data acquisition system performs time synchronization on data acquired by different sensors, converts parameters under different coordinate systems and sends the processed data to the laser inertial navigation SLAM system;

the laser inertial navigation SLAM system comprises point cloud preprocessing, a laser inertial navigation odometer, back-end map construction and optimization and loop detection;

The point cloud preprocessing is used for preprocessing the point cloud and sending the preprocessed data to the laser inertial navigation odometer;

the laser inertial navigation odometer is used for matching the extracted point cloud with the current robot pose as an initial value, obtaining coarse pose estimation through continuous iterative optimization and sending the coarse pose estimation to a back-end map construction and optimization;

the back-end map construction and optimization are matched according to the rough pose, so that the point cloud map construction and pose optimization are completed;

The loop detection is used for map repositioning to optimize pose and ensure global consistency of the map;

The autonomous exploration decision system receives positioning information and a point cloud map output by the laser inertial navigation SLAM system, so that sampling viewpoints are randomly generated through surface coverage, the distance between the viewpoints is calculated through global path planning, the optimal exploration sequence is obtained, two points intersected through the global path planning are defined in the local exploration map to serve as boundary points, local path planning is realized in a local exploration area through DWA, and the vehicle travels to the next exploration area along the two boundary points;

the bottom layer control system performs smooth movement according to the track generated by the optimal speed space of DWA sampling, and is in a real-time updating state along with the mobile environment, so that the acquisition and updating of system environment data are ensured, and the aim of independently exploring and building a graph is fulfilled;

further, the laser inertial navigation odometer is used for extracting characteristic points, and specifically comprises:

for the initial point cloud, feature points are used for feature extraction, wherein the feature points are divided into two types, namely plane points and corner points;

Ordering the points on the same scanning line according to the curvature, equally dividing the same laser scanning line into a plurality of sections, wherein each section is a sub-area, a plurality of points with the largest curvature of each section of sub-area are marked as corner points, and the rest points are plane points;

further, the laser inertial navigation odometer is used for generating key points aiming at the diagonal points by adopting a K-Means clustering algorithm-based idea, and specifically comprises the following steps:

step1, dividing a plurality of sectors by taking a laser radar coordinate system as an origin, and then dividing corresponding sectors in sequence by calculating the horizontal angle of point clouds in each frame of data;

step 2, taking the initial point of each sector as the mass center of the first cluster of the sector, and judging whether the loss function of the subsequent point cloud of the sector and the mass center of the last cluster and the Manhattan distance exceed a threshold value or not;

step 3, if the threshold value is not exceeded, adding the point into the cluster, and if the threshold value is exceeded, adding a new cluster into the sector;

step 4, repeating the steps 2 and 3 for the subsequent sectors, and filtering the clustering with less point cloud numbers;

Step 5, carrying out cluster merging when the loss function among clusters and the Manhattan distance are smaller than a threshold value, and finally averaging all point clouds in each cluster to obtain key points;

further, the laser inertial navigation odometer is used for generating and matching descriptors aiming at key points, and specifically comprises the following steps:

dividing a cluster centroid point into a plurality of sectors by taking a current centroid point as a center, wherein each sector corresponds to one dimension, and then selecting the distance value of the centroid point, closest to the current centroid point, of each sector as the value of each dimension to generate a descriptor, wherein the direction vector from the current centroid point to the closest centroid point is taken as a main direction and is determined as a first sector, each sector is taken as a value according to the anticlockwise direction, and if the centroid point does not exist in the sector, the value is set to 0;

Searching a certain number of nearest key points, generating a certain number of descriptors by each point, and taking the value of the corresponding nearest key point as a final descriptor for each dimension;

establishing a vector table to avoid repeated calculation of the distance and direction of the corresponding point;

Finally, calculating the difference value of non-zero elements between descriptors, wherein the difference value is lower than a certain threshold value, the reliability is increased by one, and when the reliability is higher than a matching threshold value, the matching is considered to be successful;

Further, the loop detection is used for performing coarse matching through Euclidean distance and then performing fine matching of Scan-Context descriptors to complete loop, and specifically includes:

Firstly, carrying out point cloud division to generate descriptors, taking a laser radar coordinate system as a reference system, uniformly dividing a frame of three-dimensional laser radar point cloud into different bin sections in radial and rotation angle directions, wherein the point cloud is divided into a plurality of uniformly distributed circular rings when seen from the radial direction, the point cloud is divided into a plurality of uniformly distributed sectors when seen from the rotation direction, and the intersection overlapping area of the two is the bin section;

designating a number for each bin segment as a parameter for forming a description submatrix, wherein the parameter is determined by a z value of a highest point in a point cloud of each bin segment, and no point in the bin segment is assigned with 0;

expanding the divided point cloud to form a description submatrix, wherein the description submatrix is a Scan-Context descriptor;

performing large threshold screening on the historical frames based on the Euclidean distance, and generating a one-dimensional vector for each descriptor;

Storing the one-dimensional vector into Kd-Tree to perform nearest neighbor search with the one-dimensional vector in the historical frames, finding a plurality of historical frames meeting a threshold value, and performing accurate search based on the following formula after completing preliminary search:

Wherein, the Representing a current frame descriptor matrix and a history frame descriptor matrix respectively,AndRespectively representing the column vector of the j-th column of the current frame and the column vector of the j-th column of the history frame,To divide the point cloud into the number of sectors,Scoring best similarity of the current frame descriptor matrix and the historical frame descriptor matrix;

matrix of historical frame descriptors Secondary translation is then carried outMinor matching to ensure rotational invariance, pairThe same operation is carried out on each historical frame, and the minimum is selectedThe value is scored as the best similarity of the historical frame to the current frame ifIf the value is smaller than the threshold value, the loop is satisfied, ICP iterative optimization is carried out on the history frame and the current frame, the pose near the loop is optimized after the optimal pose is obtained, and ifAnd if the value is larger than the threshold value, loop-back is not satisfied, and loop-back optimization is not performed.

2. The active SLAM system of claim 1, wherein the local path planning in the autonomous exploration decision system comprises:

The cost function defining the segmentation point is:

Wherein, the Is the firstThe length of the segment path is chosen to be the same,Indicating punishment brought by each parking;

Calculating shortest paths between views using a modified a-algorithm for each pair of views, the modified a-algorithm comprising an adaptive weight cost function proposed for the a-algorithm:

Wherein, the ,,Representing the euclidean distance of the current point from the starting point,Indicating the euclidean distance of the end point from the start point,Representing the distance of the current node from the starting node,Representing the Euclidean distance between the current node and the target node;

Constructing a distance matrix according to the shortest path, solving the problem that the path is shortest through each viewpoint, and confirming the optimal access sequence of the viewpoint set;

Setting all viewpoints as segmentation points, iteratively setting each viewpoint as a continuous point, calculating path cost, solving an optimal cost function, generating an optimal speed space by adopting an improved DWA algorithm to obtain an optimal track among viewpoints, and confirming an optimal smooth path, wherein the improved DWA algorithm specifically comprises the following steps:

improved DWA evaluation function:

Wherein, the To evaluate the subfunction for distance from the target,For the distance global path interval evaluation sub-function, head (v, w) represents the direction angle evaluation sub-function, dist (v, w) represents the distance evaluation sub-function of the obstacle and the robot body, vel (v, w) represents the current mobile robot linear velocity evaluation sub-function, and α, β, γ, δ, μ represent head (v, w), dist (v, w), vel (v, w), respectively,、In the weight in the evaluation function, σ represents the normalization factor.

3. The active SLAM system of claim 2, wherein the global path planning in the autonomous exploration decision system comprises:

Generating path cost between partial sub-maps which are not explored and explored by the current robot through an improved A-algorithm, and generating a distance matrix between the partial sub-maps;

solving the TSP problem in the distance matrix, defining points intersecting with the local sub-map in the current exploration on the global path as boundary points 1 and 2, and applying the local path planning between the boundary points 1 and 2 so as to achieve the double-stage planning;

and after the current local sub-map exploration is completed, the local path is simplified to be the shortest path from the current viewpoint to the boundary point 1 and the boundary point 2 again, so that the robot can return to the global path planning again, and the exploration task is executed based on the robot moving to the next exploration point.

4. An active SLAM method of laser inertial navigation fusion, the method comprising performing active SLAM using the system of any of claims 1-3.

5. A computer device comprising a memory and a processor, the memory having stored therein a computer program, characterized in that the processor, when running the computer program stored in the memory, performs the steps of the method as claimed in claim 4.

6. A computer-readable storage medium, wherein a plurality of computer instructions are stored in the computer-readable storage medium, the plurality of computer instructions being for causing a computer to perform the method recited in claim 4.