CN114964212B - Multi-machine collaborative fusion positioning and mapping method oriented to unknown space exploration - Google Patents

Abstract

The invention discloses a multi-machine collaborative fusion positioning and mapping method for unknown space exploration, which is used for realizing the close coupling state estimation of an inertial odometer, a laser-inertial odometer and a vision-inertial odometer based on a lightweight multi-sensor fusion mileage calculation method. Degradation identification and fusion decision algorithm based on the observability analysis judges whether the odometer is degraded or not, fusion of odometer information can be completed based on the degradation state of the odometer, the pose estimation result of the robot in the characteristic sparse scene is improved, multi-source information fusion of the robot is realized, and reliable guarantee is provided for autonomous navigation, real-time obstacle avoidance and map construction of the robot in a complex unknown space. The map information sharing and the optimized distribution of computing resources of all robots are realized based on a multi-machine collaborative map building algorithm, the construction of a global consistent map and the distribution of collaborative map building tasks are completed based on an information sharing network, and the efficiency of the robots in constructing an unknown space map is improved.

Description

Multi-machine collaborative fusion positioning and mapping method for unknown space exploration

Technical Field

The present invention relates to the field of robotics technology, and in particular to a multi-machine collaborative fusion positioning and mapping method for unknown space exploration.

Background Art

Complex unknown spaces such as underground caves and primeval forests generally have the characteristics of shielding satellite positioning signals, limited communication, complex terrain and landforms, and the inability to arrange landmarks in advance. Therefore, in the absence of prior information about the environment, it is of great significance for robots that explore unknown spaces to use their own sensors to model the surrounding environment information during movement and estimate their own posture at the same time. In recent years, researchers have continuously improved real-time positioning and mapping technologies, but most of the application scenarios are for indoor or outdoor open environments, and there are not many studies on the application of unknown space exploration.

A single sensor has deficiencies in perception and robustness, and cannot cope with the various challenges brought by complex environments. Therefore, multi-sensor fusion is the current mainstream solution. However, robots for exploring unknown spaces have limited computing resources on their onboard computers due to their own load and power consumption limitations, and unknown spaces with limited communications do not have the conditions to use cloud computing resources. Therefore, how to make multi-sensor fusion algorithms run in real time on the robot platform becomes extremely challenging. The existing solutions mainly reduce computing costs by achieving loose coupling between sensors. This method lacks comprehensive consideration of the characteristics of the sensor itself, which is not conducive to improving the positioning accuracy of the robot.

When robots explore unknown spaces, they will encounter scenes with a large number of repetitive geometric structures or single textures with sparse features. Due to the lack of constraints in certain directions, such scenes usually cause the state estimation of some sensors to degrade and have a serious impact on the stability of the system. Existing methods mainly judge whether the sensor has degraded by comparing the predicted value with the observed value. This method does not analyze the sensor's own observation equation and has certain limitations.

Due to the limitations of the sensor installation location and the obstruction of obstacles in the surrounding environment, robots will have large perception blind spots when exploring unknown spaces. In addition, when a single robot performs a long mapping task, there will be problems such as long mapping time, high computing power requirements, and difficulty in meeting real-time requirements. Existing research on multi-robot collaborative mapping is still in its infancy, and is mainly focused on indoor mapping, which cannot meet the needs of multi-machine collaborative mapping for unknown space exploration.

Summary of the invention

The purpose of this invention is to propose a multi-machine collaborative fusion positioning and mapping method for unknown space exploration, so as to achieve high-precision positioning and mapping of robots in unknown and complex environments.

To achieve this purpose, the present invention adopts the following technical solution: A multi-machine collaborative fusion positioning and mapping method for unknown space exploration includes the following steps:

The lightweight multi-sensor fusion mileage calculation method designs an inertial odometer, a laser-inertial odometer and a visual-inertial odometer to achieve the fusion of multi-sensor information; the inertial odometer, the laser-inertial odometer and the visual-inertial odometer are used to estimate the robot's position information respectively;

Design a degradation identification and fusion decision strategy algorithm based on observability analysis, and judge whether the laser-inertial odometry and the visual-inertial odometry are degraded based on observability analysis; if degradation occurs, the pose estimation information obtained by the laser-inertial odometry and the pose estimation information of the visual-inertial odometry are subjected to degradation processing, and the degraded pose estimation information and the pose estimation information of the inertial odometry are fused according to the fusion decision to obtain the total pose estimation output; if degradation does not occur, the pose estimation information obtained by the inertial odometry, the laser-inertial odometry and the visual-inertial odometry are fused according to the fusion decision to obtain the total pose estimation output;

In the multi-machine collaborative mapping algorithm, each robot completes the construction of a global consistent map and the allocation of collaborative mapping tasks through the information sharing network.

As an optional embodiment, the method for fusing multi-sensor information in the odometer system is:

The inertial odometer forms a dead reckoning system through an inertial measurement unit and an airspeed meter, and the dead reckoning system is used to provide a high-frequency initial position estimation for the robot; and eliminates the position drift of the inertial odometer in the vertical direction through the height constraint generated by the barometer and the laser ranging module;

The laser-inertial odometry receives the initial pose estimation of the inertial odometry and the observation of the solid-state laser radar, and provides a corrected pose for the inertial odometry by constructing an iterative error state Kalman filter estimator;

The visual-inertial odometry receives the initial pose estimation of the inertial odometry, the image data and the point cloud data collected by the visual sensor, and constructs a local sliding window optimization estimator using formula (1):

表示集合

的最优估计；

表示k时刻滑窗内所有关键帧的集合；

分别表示第i时刻与第j时刻之间激光-惯性里程计、视觉-惯性里程计、惯性里程计的相对约束；

表示滑窗中第一个状态量的先验约束；∑_LIO、∑_VIO、∑_IMU、∑₀分别表示各约束的协方差矩阵。in,

Representing a collection

The best estimate of

represents the set of all key frames in the sliding window at time k;

They represent the relative constraints of the laser-inertial odometry, visual-inertial odometry, and inertial odometry between the i-th moment and the j-th moment respectively;

represents the prior constraint of the first state quantity in the sliding window; ∑ _LIO , ∑ _VIO , ∑ _IMU , ∑ ₀ represent the covariance matrices of each constraint respectively.

As an optional embodiment, a method for judging whether a laser-inertial odometer and a visual-inertial odometer are degraded based on observability analysis is:

According to the laser constraints and visual constraints, the information matrix of laser-inertial odometry and visual-inertial odometry is constructed;

Perform eigenvalue decomposition on the information matrix, and characterize the degree of degradation of the odometer by the size of the eigenvalue:

If the eigenvalue is less than the set threshold, the odometer is judged to be degraded, and the eigenvector corresponding to the eigenvalue is regarded as the degradation direction vector; if the eigenvalue is greater than or equal to the set threshold, the odometer is judged to be non-degraded.

As an optional embodiment, the degradation process includes the following steps:

The eigenvalues can be used to determine the direction vector of the nonlinear optimization solution of the pose estimation problem in the degenerate direction and the direction vector of the non-degenerate direction.

The nonlinear optimization solution of pose estimation is decomposed into the projection of the true solution in the non-degenerate direction and the projection of the predicted value in the degenerate direction. Consider adding constraints in some subspaces of the pose estimation problem, do not perform iterative optimization in the degenerate direction, and only output the solution in the non-degenerate direction.

By discarding the component of the optimal solution in the degenerate direction and only adding the increment of the solution in the non-degenerate direction to the state quantity, the pose estimation information after degradation processing is obtained.

As an optional embodiment, obtaining the total pose estimation output includes the following steps:

Inputting degradation state information of the laser-inertial subsystem and the visual-inertial subsystem into the fusion decision table, and obtaining a decision output of the fusion decision table based on the degradation state information;

By fusing the decision output of the decision table and the observation data of each odometer subsystem, the total pose estimation output of the odometer system is obtained and input into the back-end optimization system;

The backend optimization system constructs the relationship between the state quantities at each moment through the pose graph and models it as a batch recursive smoothing problem, as shown in formula (2):

表示集合

的最优估计；

表示截止到k时刻所有关键帧的集合；

分别表示第i时刻与第j时刻之间里程计和回环的相对约束；∑_odom、∑_loop分别表示各约束的协方差矩阵。in,

Representing a collection

The best estimate of

represents the set of all key frames up to time k;

They represent the relative constraints of the odometer and loop between the i-th moment and the j-th moment respectively; ∑ _odom and ∑ _loop represent the covariance matrices of each constraint respectively.

As an optional embodiment, the construction of a global consistent map and the allocation of collaborative mapping tasks among the robots through the information sharing network include the following steps:

A clustering algorithm based on communication quality and group vision constraints maximizes the coupling performance consisting of drone communication quality and group operation vision set under specification constraints, and obtains a set of optimal solutions to divide the drone group information sharing area. In each information sharing area, the network master control unit is selected based on the principle of optimal computing resources.

By finding the overlapping areas between the robot maps to associate information, the coordinate transformation relationship between the robots is determined, and a global consistent map is constructed based on this. The bag-of-words model based on key frames detects overlapping areas. The bag-of-words model uses the similarity between two images to determine whether the two images are from the same scene. If the two robots have passed through the same scene, it proves that there is a high probability that there is an overlapping area between the maps of the two robots.

After receiving the similar keyframes and local map data sent by the robot, the main control unit first performs feature matching on the similar keyframes and its own keyframes and obtains the initial pose transformation matrix using the epipolar constraint relationship, which is then provided to the normal distribution transformation algorithm as the initial value. The normal distribution transformation algorithm is then used to iteratively calculate the precise pose transformation matrix, and the local map sent by the robot is merged with the global map maintained by the main control unit based on the final pose transformation matrix.

The normal distribution transformation algorithm mainly uses the probability density function of point distribution for registration and determines the best match between point clouds through an optimization algorithm, including the following steps:

Step 1: Gridding: Divide the global point cloud map maintained by the main control unit into three-dimensional cells of uniform size;

Step 2: Calculate the parameters of the multidimensional normal distribution. According to formulas (3) and (4), the mean vector q and covariance matrix Σ of each spatial point in each cell are obtained respectively:

Where n is the total number of points in each cell, and x _k represents a point in the cell;

其中R是3×3的旋转矩阵，t是3×1的平移矩阵。利用位姿变换矩阵T将局部点云地图中的每个点变换到全局地图上，变换后的点用y_i(i＝1，…，m)来表示，m为局部点云地图中所有点的总数；Step 3: Coordinate transformation, assuming that the pose transformation matrix is T, the initial value is provided by the epipolar constraint relationship, and there are:

Where R is a 3×3 rotation matrix, and t is a 3×1 translation matrix. Each point in the local point cloud map is transformed to the global map using the pose transformation matrix T. The transformed point is represented by _yi (i=1,…,m), where m is the total number of points in the local point cloud map.

Step 4: Calculate the probability density. Calculate the probability density of each transformation point according to the normal distribution parameters of the cell where the transformation point _yi is located. The calculation formula (5) is:

Where detΣ represents the determinant of the covariance matrix Σ;

Step 5: Create the objective function. For the probability density of each point, use formula (6) to construct the objective function to evaluate the pose transformation matrix T:

Step 6: Optimize the objective function and use the Newton optimization method to solve the minimum value of the objective function to obtain the optimal posture transformation matrix T;

Based on the fast random search tree algorithm, the boundary points between the mapped area and the unknown area are found. The RRT algorithm includes:

Step 1: Call the Init function to initialize the tree T;

Step 2: Call the RandomSample function to randomly sample a point x _rand in the state space X;

Step 3: Call the Nearest function to find the point x _near closest to x _rand on the current tree T;

Step 4: Call the Steer function, starting from x _near and taking ε as the step length, and iteratively check whether the point between x _near and x _rand is a collision-free free state. If not, stop the iteration. At this time, the farthest passable point from x _near is the new node x _new .

Step 5: Add the new node x _new and the new edge between x _near and x _new to the tree T, completing the expansion of this tree and entering the next iteration;

Step 6: The iteration ends and a new tree T is generated;

The main control unit uses the mean shift clustering algorithm to screen a large number of boundary points obtained by the global and local detectors to obtain the required task target points. The mean shift clustering algorithm includes the following steps:

Step 1: Randomly select a point from the unmarked boundary points as the starting center point x _center ;

Step 2: Find all boundary points in the spherical area with radius r and center on x _center and mark them;

Step 3: Calculate the vector from x _center to each boundary point in the spherical area, add these vectors together to get the drift vector x _shift ;

Step 4: Move the center point according to x _center = x _center + x _shift , that is, x _center moves along the direction of x _shift , and the moving distance is ||x _shift ||;

Step 5: Repeat Step 2 to Step 4 until ||x _shift || is very small, which means the iteration has converged. Record the current center point x _center . All boundary points encountered during the iteration will be marked.

Step 6: If the distance between the current center point x _center and other existing center points is less than the threshold value during convergence, then the center point with more boundary points in the spherical area is retained; otherwise, x _center is used as the new center point;

Step 7: Repeat the previous steps until all points are marked as visited;

The master control unit will use an auction mechanism to allocate mission target points to robots in the shared network.

The present invention also provides a multi-machine collaborative fusion positioning and mapping device for unknown space exploration, including a memory, a processor, and a multi-machine collaborative fusion positioning and mapping method program for unknown space exploration stored in the memory and executable on the processor. When the processor executes the multi-machine collaborative fusion positioning and mapping method program for unknown space exploration, the steps of the multi-machine collaborative fusion positioning and mapping method for unknown space exploration are implemented.

A computer-readable storage medium stores a program of a multi-machine collaborative fusion positioning and mapping method for unknown space exploration. When the program of the multi-machine collaborative fusion positioning and mapping method for unknown space exploration is executed by a processor, the steps of the multi-machine collaborative fusion positioning and mapping method for unknown space exploration are implemented.

Compared with the prior art, the embodiments of the present invention have the following beneficial effects:

The lightweight multi-sensor fusion odometer calculation method designed by the present invention can realize the tightly coupled state estimation of inertial odometer, laser-inertial odometer and visual-inertial odometer. Compared with the existing methods, it can effectively improve the accuracy of robot pose estimation and the robustness in dealing with various challenges in unknown space and realize the real-time operation of multi-sensor fusion algorithm on the onboard computer.

The degradation identification and fusion decision algorithm designed by the present invention based on observability analysis can determine whether the odometer is degraded, and can complete the fusion of odometer information based on the degradation state of the odometer. Compared with the existing methods, it can improve the pose estimation results of the robot in feature-sparse scenes and realize the fusion of multi-source information of the robot, providing reliable guarantee for the robot's autonomous navigation, real-time obstacle avoidance and map construction in complex unknown spaces.

The multi-machine collaborative mapping algorithm designed by the present invention can realize the sharing of map information among robots and the optimal allocation of computing resources, and complete the construction of a global consistent map and the allocation of collaborative mapping tasks based on an information sharing network. Compared with existing methods, this patent designs a multi-machine collaborative mapping algorithm for unknown space exploration, which improves the efficiency of robots in building maps of unknown spaces.

BRIEF DESCRIPTION OF THE DRAWINGS

FIG1 is a schematic diagram of an algorithm framework of one embodiment of the present invention;

FIG2 is a schematic diagram of a sensor information fusion process according to an embodiment of the present invention;

FIG3 is a schematic diagram of an iterative error Kalman filter according to one embodiment of the present invention;

FIG4 is a schematic diagram of an odometer fusion process according to an embodiment of the present invention;

FIG5 is a schematic diagram of degradation judgment based on observability analysis in one embodiment of the present invention;

FIG6 is a schematic diagram of an information fusion framework according to an embodiment of the present invention;

FIG7 is a schematic diagram of an overlapping area detection process according to an embodiment of the present invention;

FIG8 is a schematic diagram of a process of constructing a globally consistent map according to one embodiment of the present invention;

FIG9 is a schematic diagram of a collaborative mapping system framework according to one embodiment of the present invention;

FIG. 10 is a flowchart of an auction mechanism according to one embodiment of the present invention.

DETAILED DESCRIPTION

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 only used to explain the present invention, and cannot be understood as limiting the present invention.

The following describes a multi-machine collaborative fusion positioning and mapping method and system for unknown space exploration according to an embodiment of the present invention in conjunction with Figures 1 to 10.

In view of the complex and unknown spatial environment, it is urgent to improve the robot's autonomous positioning and environmental perception capabilities. As shown in Figure 1, the present invention designs a lightweight multi-sensor fusion mileage calculation method, a degradation identification and fusion decision strategy algorithm based on observability analysis, and a multi-machine collaborative mapping algorithm to improve the robustness and smoothness of the robot's positioning and mapping when exploring an unknown and complex spatial environment.

As an optional embodiment, an embodiment of designing a lightweight multi-sensor fusion mileage calculation method is shown in Figure 2. The lightweight multi-sensor fusion mileage calculation method adopts a redundant design, and designs three subsystems, namely, an inertial odometry, a laser-inertial odometry, and a visual-inertial odometry, to achieve the fusion of multi-sensor information and enhance the robustness of the robot to cope with various challenges in unknown spaces.

First, the inertial odometer forms a dead reckoning system through an inertial measurement unit and an airspeed meter to provide the robot with a high-frequency initial pose estimation. At the same time, the height constraint generated by the barometer and laser ranging module can effectively eliminate the vertical pose drift of the inertial odometer. And because the inertial odometer is only an estimate of the robot's own state, even if the visual-inertial odometer and laser-inertial odometer degrade or fail, the accuracy of the robot's pose estimation can be guaranteed in a short time.

Secondly, the laser-inertial odometry receives the initial pose estimate of the inertial odometry and the solid-state lidar observation, as shown in Figure 3. By constructing an iterative error state Kalman filter estimator, it provides a corrected pose for the inertial odometry and eliminates the accumulated error of the inertial odometry.

Finally, the visual-inertial odometry receives the initial pose estimate of the inertial odometry, the visual sensor image data, and the point cloud data, and constructs a local sliding window optimization estimator using the following formula (1):

表示集合

的最优估计；

表示k时刻滑窗内所有关键帧的集合；

分别表示第时刻与第时刻之间激光-惯性里程计、视觉-惯性里程计、惯性里程计的相对约束；

表示滑窗中第一个状态量的先验约束；∑_LIO、∑_VIO、∑_IMU、∑₀分别表示各约束的协方差矩阵。此融合方法通过使用点云数据来辅助视觉进行深度估计来克服由于深度估计不准，影响位姿估计的问题。in,

Representing a collection

The best estimate of

represents the set of all key frames in the sliding window at time k;

They represent the relative constraints of the laser-inertial odometry, visual-inertial odometry, and inertial odometry between the th moment and the th moment respectively;

represents the prior constraint of the first state in the sliding window; ∑ _LIO , ∑ _VIO , ∑ _IMU , ∑ ₀ represent the covariance matrix of each constraint respectively. This fusion method overcomes the problem of inaccurate depth estimation affecting pose estimation by using point cloud data to assist vision in depth estimation.

Furthermore, an embodiment of designing a degradation identification and fusion decision algorithm based on observability analysis is as follows:

The robot's pose estimation can be obtained through the inertial odometry, visual-inertial odometry, and laser-inertial odometry respectively. Next, the pose estimation information of the three odometry meters needs to be fused.

For visual-inertial odometers and laser-inertial odometers, visual sensors require texture information to determine feature points, while laser sensors require geometric structure information to determine feature points. In sparse feature scenes with single texture or repeated geometric structure, such as long straight corridors, long tunnels, single-side walls, open roads, bridges, etc., both visual sensors and laser sensors will not be able to extract enough feature points. The lack of sufficient feature points will cause the state estimators of the visual-inertial odometer and laser-inertial odometer to degrade, thereby affecting the accuracy and stability of pose estimation. In response to the problem that visual-inertial odometers and laser-inertial odometers may degrade, the present invention judges and analyzes whether the odometer has degraded based on observability analysis, and fuses the pose estimation information of multiple odometer subsystems based on the degradation state of the odometer.

As shown in Figure 4, since the inertial odometry is not affected by the sparsely structured scene, the pose estimation of the inertial odometry is directly input into the fusion decision system. However, the laser-inertial odometry and visual-inertial odometry need to first determine whether they are degraded. If they are not degraded, the pose estimation is directly input into the fusion decision system like the inertial odometry. The pose estimation information of the degraded odometry will be input into the fusion decision system after processing.

As shown in FIG5 , the present invention determines whether the visual-inertial odometry and the laser-inertial odometry are degraded in a feature-sparse scene based on observability analysis.

First, according to the laser constraint and visual constraint, the information matrix of the laser-inertial odometry and visual-inertial odometry is constructed, and the observability analysis problem is further converted into the problem of analyzing the observable subspace. Then, the information matrix is decomposed by eigenvalue, and the degradation degree of the odometry is characterized by the size of the eigenvalue. When the eigenvalue is less than the set threshold, the odometry is considered to have degraded, and the eigenvector corresponding to the eigenvalue is regarded as the degradation direction vector.

When it is determined that the laser-inertial odometer and/or visual-inertial odometer has degraded, its pose estimation information needs to be processed. For the odometer system, the pose estimation problem can eventually be transformed into a nonlinear optimization problem. There is no necessary connection between whether the odometer is degraded and whether there is a solution to the pose estimation problem. Therefore, even if the odometer is degraded, the pose estimation problem may have a solution. The eigenvalue can be used to determine the direction vector of the nonlinear optimization solution of the pose estimation problem in the degraded direction and the direction vector of the non-degraded direction. Therefore, the present invention decomposes the nonlinear optimization solution of the pose estimation into the projection of the true solution in the non-degraded direction and the projection of the predicted value in the degraded direction, and considers adding constraints to some subspaces of the pose estimation problem, does not perform iterative optimization in the degraded direction, and only outputs the solution in the non-degraded direction. Finally, by discarding the component of the optimal solution in the degraded direction, only the increment of the non-degraded direction solution is superimposed on the state quantity, thereby preventing the optimal solution from moving in the null space and ensuring the accuracy of the optimal solution. Finally, the processed pose estimation information is input into the fusion decision system. If the odometer is degraded in all directions, the pose estimation information is directly discarded and not input into the fusion decision system.

The fusion decision system receives the pose estimation information of the three odometers. Aiming at the information fusion problem of multiple odometer subsystems of the robot, the present invention designs a decision-driven fusion method based on degradation state. First, the degradation state information of the laser-inertial subsystem and the visual-inertial subsystem is input into the decision table, and then the decision output of the decision table is obtained based on the degradation state of the odometer subsystem. Obviously, when the odometer has not degraded or the degree of degradation is relatively low, its proportion in the decision output is higher. Then, by fusing the decision data of the decision table and the observation data of each odometer subsystem, the total pose estimation output of the lightweight multi-sensor fusion odometer is obtained and input into the back-end optimization system.

The backend optimization system constructs the relationship between the state quantities at each moment through the pose graph and models it as a batch recursive smoothing problem, as shown in formula (2):

表示集合

的最优估计；

表示截止到k时刻所有关键帧的集合；

分别表示第i时刻与第j时刻之间里程计和回环的相对约束；∑_odom、∑_loop分别表示各约束的协方差矩阵。最后，通过求解公式(2)得到具有全局一致性的轨迹，进而得到稠密的三维点云地图。in,

Representing a collection

The best estimate of

represents the set of all key frames up to time k;

They represent the relative constraints of the odometer and loop between the i-th moment and the j-th moment respectively; ∑ _odom and ∑ _loop represent the covariance matrices of each constraint respectively. Finally, by solving formula (2), a trajectory with global consistency is obtained, and then a dense 3D point cloud map is obtained.

As shown in Figure 6, each odometer subsystem fuses multi-sensor data in a tightly coupled manner for state estimation, and then makes fusion decisions on the pose estimation information of each odometer in a loosely coupled manner. This fusion method that combines tight coupling and loose coupling improves positioning accuracy while also ensuring system stability.

Furthermore, an embodiment of designing a multi-machine collaborative mapping algorithm is as follows:

First, in response to the problem of complex unknown spatial networks and limited communications, the present invention adopts a clustering algorithm based on communication quality and group vision constraints. It maximizes the coupling performance composed of the drone communication quality and the group operation vision set under specification constraints, obtains a set of better solutions, and realizes the efficient division of the drone group information sharing area. In each information sharing area, the network master control unit is selected based on the principle of optimal computing resources.

Secondly, in order to solve the problem that the probability of robots meeting in unknown spaces is low and road signs cannot be arranged in advance, the present invention constructs a globally consistent map based on the overlapping areas of the maps. By finding the overlapping areas between the robot maps and performing information association, the coordinate transformation relationship between the robots is determined, and a globally consistent map is constructed based on this. Overlapping area detection is completed using a bag-of-words model based on key frames. The bag-of-words model measures the similarity between two images, so that it can be determined whether the two images are from the same scene. Therefore, by comparing the key frames between the two robots using the bag-of-words model, it can be determined whether the two robots have passed through the same scene. If the two robots have passed through the same scene, it proves that there is a high probability of overlapping areas between their maps.

At the same time, as shown in Figure 7, in order to avoid the real-time performance of system communication being affected by the continuous transmission of three-dimensional map data, the main control unit will first send the global descriptor corresponding to its key frame to each robot in the network by broadcasting on the communication network. After receiving the global descriptor sent by the main control unit, the robot in the network will use the bag-of-words model to determine whether there is an overlapping map area with the main control unit. The robot will only transmit data when there is an overlapping area with the main control unit, and send the corresponding similar key frame and local map data to the main control unit. This mechanism can effectively avoid the transmission of a large amount of redundant data and improve transmission efficiency.

As shown in Figure 8, after the main control unit receives the similar key frames and local map data sent by the robot, it first performs feature matching on the similar key frames and its own key frames and uses the epipolar constraint relationship to obtain the initial pose transformation matrix, which is provided to the normal distribution transformation algorithm as the initial value. The normal distribution transformation algorithm is then used to iteratively calculate the precise pose transformation matrix, and the local map sent by the robot is merged with the global map maintained by the main control unit according to the final pose transformation matrix.

The Normal Distribution Transformation (NDT) algorithm mainly uses the probability density function of point distribution for registration and determines the best match between point clouds through an optimization algorithm. The specific steps are as follows:

Step 1: Gridding, divide the global point cloud map maintained by the main control unit into three-dimensional cells of uniform size.

Step 2: Calculate the parameters of the multidimensional normal distribution. According to formulas (3) and (4), the mean vector q and covariance matrix ∑ of each spatial point in each cell are obtained respectively:

Where n is the total number of points in each cell and _xk represents a point in the cell.

其中R是3×3的旋转矩阵，t是3×1的平移矩阵。利用位姿变换矩阵T将局部点云地图中的每个点变换到全局地图上，变换后的点用y_i(i＝1，…，m)来表示，m为局部点云地图中所有点的总数。Step 3: Coordinate transformation, assuming that the pose transformation matrix is T, the initial value is provided by the epipolar constraint relationship, and there are:

Where R is a 3×3 rotation matrix, and t is a 3×1 translation matrix. Each point in the local point cloud map is transformed to the global map using the pose transformation matrix T. The transformed point is represented by _yi (i=1,…,m), where m is the total number of points in the local point cloud map.

Step 4: Calculate the probability density. Calculate the probability density of each transformation point according to the normal distribution parameters of the cell where the transformation point _yi is located:

Where detΣ represents the determinant of the covariance matrix Σ;

Step 5: Create the objective function. For the probability density of each point, use formula (6) to construct the objective function to evaluate the pose transformation matrix T:

Step 6: Optimize the objective function and use the Newton optimization method to solve the minimum value of the objective function to obtain the optimal posture transformation matrix T.

Global optimization is performed based on the pose graph optimization algorithm to obtain a higher-precision global consistent map and the pose constraint relationship between the robot.

Finally, in order to solve the problem of how multiple robots can collaboratively build maps when exploring unknown spaces, we use the Rapidly-exploring Random Trees (RRT) algorithm to find the boundary points between the mapped areas and the unknown areas. The RRT algorithm includes:

Step 1: Call the Init function to initialize the tree T;

Step 2: Call the RandomSample function to randomly sample a point x _rand in the state space X;

Step 3: Call the Nearest function to find the point x _near closest to x _rand on the current tree T;

Step 4: Call the Steer function, starting from x _near and taking ε as the step length, and iteratively check whether the point between x _near and x _rand is a collision-free free state. If not, stop the iteration. At this time, the farthest passable point from x _near is the new node x _new .

Step 5: Add the new node x _new and the new edge between x _near and x _new to the tree T, completing the expansion of this tree and entering the next iteration;

Step 6: The iteration ends and a new tree T is generated.

As shown in FIG9 , the present invention takes into account both the efficiency and integrity of the boundary point search process by setting a global boundary detector and a local boundary detector. Each robot in the network will maintain a local detector, and randomly detect the surrounding environment through the RRT algorithm. When a boundary point is detected, the robot will delete the original tree and restart the tree expansion at the current position, thereby speeding up the detection of boundary points. The global boundary detector is maintained by the main control unit. Unlike the local detector, the root node of the tree in the global detector will not be updated. Only one tree will be generated during the task. The global detector can detect boundary points ignored by the local detector and provide more valuable boundary points when the RRT algorithm of the local detector falls into a growth "trap". The main control unit uses the mean shift clustering algorithm to screen the large number of boundary points obtained by the global and local detectors to obtain the required task target points.

The specific steps of the mean shift clustering algorithm are as follows:

Step 1: Randomly select a point among the unmarked boundary points as the starting center point x _center .

Step 2: Find all boundary points in the spherical area with radius r and center on x _center and mark these boundary points.

Step 3: Calculate the vector from the x _center to each boundary point in the spherical area, add these vectors together to get the drift vector x _shift .

Step 4: Move the center point according to x _center = x _center + x _shift , that is, x _center moves along the direction of x _shift , and the moving distance is ||x _shift ||.

Step 5: Repeat Step 2 to Step 4 until ||x _shift || is very small, which means the iteration has converged. Record the current center point x _center . All boundary points encountered during the iteration will be marked.

Step 6: If the distance between the current center point x _center and other existing center points is less than the threshold value during convergence, then the center point with more boundary points in the spherical area is retained. Otherwise, x _center is used as the new center point.

Step 7: Repeat the previous steps until all points are marked as visited.

After mean shift clustering, the originally large number of boundary points will be clustered to some center points. The center points are retained and the remaining boundary points are deleted. At this time, these center points are the robot's task target points.

After obtaining the mission target point, the main control unit will use the auction mechanism shown in Figure 10 to allocate the mission target point to the robots in the shared network. The auction algorithm simulates the market auction process, with the main control unit acting as the auctioneer and the idle robots in the information sharing network acting as bidders. The auctioneer selects the one with the highest return as the winner among all bidders based on the principle of maximizing system benefits. Among them, this patent uses exploration paths, field of view overlap, and "islands" as cost criteria, and allocates task nodes by measuring the length of the path for each robot to reach the task node, the level of repeated coverage, and the number of "islands" that will be formed, thereby achieving the optimal allocation of multi-robot collaborative mapping tasks.

The present invention also discloses a multi-machine collaborative fusion positioning and mapping device for unknown space exploration, including a memory, a processor, and a multi-machine collaborative fusion positioning and mapping method program for unknown space exploration stored in the memory and executable on the processor. When the processor executes the multi-machine collaborative fusion positioning and mapping method program for unknown space exploration, the steps of the multi-machine collaborative fusion positioning and mapping method for unknown space exploration are implemented.

The method comprises at least one processor and a memory. The processor is an integrated circuit chip with the ability to process signals. In the implementation process, each step of the above method can be completed by an integrated logic circuit of hardware in the processor or an instruction in the form of software. The above processor can be a general-purpose processor, a digital signal processor (DSP), an application-specific integrated circuit (ASIC), a field programmable gate array (FPGA) or other programmable logic devices, discrete gates or transistor logic devices, discrete hardware components. The methods, steps and logic block diagrams disclosed in the embodiments of the present invention can be implemented or executed. The general-purpose processor can be a microprocessor or the processor can also be any conventional processor, etc. The software module can be located in a mature storage medium in the field such as a random access memory, a flash memory, a read-only memory, a programmable read-only memory or an electrically erasable programmable memory, a register, etc. The storage medium is located in the memory, and the processor reads the information in the memory and completes the steps of the above method in combination with its hardware.

It can be understood that the memory in the embodiment of the present invention can be a volatile memory or a non-volatile memory, or can include both volatile and non-volatile memories. Among them, the non-volatile memory can be a read-only memory (ROM), a programmable read-only memory (PROM), an erasable programmable read-only memory (EPROM), an electrically erasable programmable read-only memory (EEPROM), or a flash memory. The volatile memory can be a random access memory (RAM), which is used as an external cache. By way of example but not limitation, many forms of RAM are available, such as static random access memory (SRAM), dynamic random access memory (DRAM), synchronous dynamic random access memory (SDRAM), double data rate synchronous dynamic random access memory (DDRSDRAM), enhanced synchronous dynamic random access memory (ESDRAM), synchronous link dynamic random access memory (SLDRAM) and direct RAM bus random access memory (DRRAM). The memory of the system and method described in the embodiments of the present invention is intended to include, but is not limited to, these and any other suitable types of memory.

The present invention also discloses a computer-readable storage medium, on which is stored a program of a multi-machine collaborative fusion positioning and mapping method for unknown space exploration. When the program of the multi-machine collaborative fusion positioning and mapping method for unknown space exploration is executed by a processor, the steps of the above-mentioned multi-machine collaborative fusion positioning and mapping method for unknown space exploration are implemented.

Those skilled in the art will appreciate that embodiments of the present invention may be provided as methods, systems, or computer program products. Therefore, the present invention may take the form of a complete hardware embodiment, a complete software embodiment, or an embodiment combining software and hardware. Moreover, the present invention may take the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

The present invention is described with reference to the flowchart and/or block diagram of the method, device (system), and computer program product according to the embodiment of the present invention. It should be understood that each process and/or box in the flowchart and/or block diagram, as well as the combination of the process and/or box in the flowchart and/or block diagram can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, a special-purpose computer, an embedded processor or other programmable data processing device to produce a machine, so that the instructions executed by the processor of the computer or other programmable data processing device produce a device for implementing the functions specified in one or more processes in the flowchart and/or one or more boxes in the block diagram.

These computer program instructions may also be stored in a computer-readable memory that can direct a computer or other programmable data processing device to work in a specific manner, so that the instructions stored in the computer-readable memory produce a manufactured product including an instruction device that implements the functions specified in one or more processes in the flowchart and/or one or more boxes in the block diagram.

These computer program instructions may also be loaded onto a computer or other programmable data processing device so that a series of operational steps are executed on the computer or other programmable device to produce a computer-implemented process, whereby the instructions executed on the computer or other programmable device provide steps for implementing the functions specified in one or more processes in the flowchart and/or one or more boxes in the block diagram.

Other structures and operations of the multi-machine collaborative fusion positioning and mapping method for unknown space exploration according to an embodiment of the present invention are known to ordinary technicians in the field and will not be described in detail here.

In the description of this specification, the description with reference to the terms "embodiment", "example", etc. means that the specific features, structures, materials or characteristics described in conjunction with the embodiment or example are included in at least one embodiment or example of the present invention. In this specification, the schematic representation of the above terms does not necessarily refer to the same embodiment or example. Moreover, the specific features, structures, materials or characteristics described can be combined in any one or more embodiments or examples in a suitable manner.

Although the embodiments of the present invention have been shown and described, it will be appreciated by those skilled in the art that various changes, modifications, substitutions and variations may be made to the embodiments without departing from the principles and spirit of the present invention, and that the scope of the present invention is defined by the claims and their equivalents.

Claims (7)

1. A multi-machine collaborative fusion positioning and mapping method for unknown space exploration, characterized in that it includes the following steps: The lightweight multi-sensor fusion mileage calculation method designs an inertial odometer, a laser-inertial odometer and a visual-inertial odometer to achieve the fusion of multi-sensor information; the inertial odometer, the laser-inertial odometer and the visual-inertial odometer are used to estimate the position and posture information of the robot respectively; Design a degradation identification and fusion decision strategy algorithm based on observability analysis, and judge whether the laser-inertial odometry and the visual-inertial odometry are degraded based on observability analysis; if degradation occurs, the pose estimation information obtained by the laser-inertial odometry and the pose estimation information of the visual-inertial odometry are subjected to degradation processing, and the degraded pose estimation information and the pose estimation information of the inertial odometry are fused according to the fusion decision to obtain the total pose estimation output; if degradation does not occur, the pose estimation information obtained by the inertial odometry, the laser-inertial odometry and the visual-inertial odometry are fused according to the fusion decision to obtain the total pose estimation output; Multi-machine collaborative mapping algorithm, each robot completes the construction of a global consistent map and the allocation of collaborative mapping tasks through an information sharing network; The fusion method of the multi-sensor information is: The inertial odometer forms a dead reckoning system through an inertial measurement unit and an airspeed meter, and the dead reckoning system is used to provide a high-frequency initial position estimation for the robot; and eliminates the position drift of the inertial odometer in the vertical direction through the height constraint generated by the barometer and the laser ranging module; The laser-inertial odometry receives an initial pose estimate of the inertial odometry and an observation value of a solid-state laser radar, and provides a corrected pose for the inertial odometry by constructing an iterative error state Kalman filter estimator; The visual-inertial odometry receives the initial pose estimation of the inertial odometry, the image data and point cloud data collected by the visual sensor, and constructs a local sliding window optimization estimator using formula (1):

(1); in,

Representing a collection

The best estimate of

express

The set of all key frames in the time sliding window;

Respectively represent

Moment and

Relative constraints between laser-inertial odometry, visual-inertial odometry, and inertial odometry at each moment;

Represents the prior constraint on the first state quantity in the sliding window;

Represent the covariance matrix of each constraint. 2. The multi-machine collaborative fusion positioning and mapping method for unknown space exploration according to claim 1 is characterized in that: the method for judging whether the laser-inertial odometer and the visual-inertial odometer are degraded based on observability analysis is: According to the laser constraints and visual constraints, the information matrix of laser-inertial odometry and visual-inertial odometry is constructed; Perform eigenvalue decomposition on the information matrix, and characterize the degree of degradation of the odometer by the size of the eigenvalue: If the eigenvalue is less than the set threshold, the odometer is judged to be degraded, and the eigenvector corresponding to the eigenvalue is regarded as the degradation direction vector; if the eigenvalue is greater than or equal to the set threshold, the odometer is judged to be non-degraded. 3. The multi-machine collaborative fusion positioning and mapping method for unknown space exploration according to claim 1 is characterized in that the degradation processing includes the following steps: Determine the direction vector of the nonlinear optimization solution of the pose estimation problem in the degenerate direction and the direction vector of the non-degenerate direction through the eigenvalue; The nonlinear optimization solution of pose estimation is decomposed into the projection of the true solution in the non-degenerate direction and the projection of the predicted value in the degenerate direction. Consider adding constraints in some subspaces of the pose estimation problem, do not perform iterative optimization in the degenerate direction, and only output the solution in the non-degenerate direction. By discarding the component of the optimal solution in the degenerate direction and adding the increment of the solution in the non-degenerate direction to the state quantity, the pose estimation information after degradation processing is obtained. 4. The multi-machine collaborative fusion positioning and mapping method for unknown space exploration according to claim 3 is characterized in that obtaining the total pose estimation output comprises the following steps: Inputting degradation state information of the laser-inertial subsystem and the visual-inertial subsystem into the fusion decision table, and obtaining a decision output of the fusion decision table based on the degradation state information; By fusing the decision output of the decision table and the observation data of each odometer, the total pose estimation output is obtained and input into the back-end optimization system; The backend optimization system constructs the relationship between the state quantities at each moment through the pose graph and models it as a batch recursive smoothing problem, as shown in formula (2):

(2); in,

Representing a collection

The best estimate of

Indicates that the deadline

The collection of all key frames at the moment;

Respectively represent

Moment and

Relative constraints on odometers and loop closures between moments;

Represent the covariance matrix of each constraint. 5. The multi-machine collaborative fusion positioning and mapping method for unknown space exploration according to claim 1 is characterized in that: the construction of a global consistent map and the allocation of collaborative mapping tasks between the robots through the information sharing network include the following steps: A clustering algorithm based on communication quality and group vision constraints maximizes the coupling performance consisting of robot communication quality and group operation vision set under specification constraints, and obtains a set of optimal solutions to divide the information sharing area of the drone group. In each information sharing area, the network master control unit is selected based on the principle of optimal computing resources. By finding the overlapping areas between the robot maps to associate information, the coordinate transformation relationship between the robots is determined, and a global consistent map is constructed based on this. The bag-of-words model based on key frames detects overlapping areas. The bag-of-words model uses the similarity between two images to determine whether the two images are from the same scene. If the two robots have passed through the same scene, it proves that there is a high probability that there is an overlapping area between the maps of the two robots. After receiving the similar keyframes and local map data sent by the robot, the main control unit first performs feature matching on the similar keyframes and its own keyframes and obtains the initial pose transformation matrix using the epipolar constraint relationship, which is then provided to the normal distribution transformation algorithm as the initial value. The normal distribution transformation algorithm is then used to iteratively calculate the precise pose transformation matrix, and the local map sent by the robot is merged with the global map maintained by the main control unit based on the final pose transformation matrix. The normal distribution transformation algorithm mainly uses the probability density function of point distribution for registration and determines the best match between point clouds through an optimization algorithm, including the following steps: Step 1: Gridding, dividing the global point cloud map maintained by the main control unit into three-dimensional cells of uniform size; Step 2: Calculate the parameters of the multidimensional normal distribution and find the mean vector of each spatial point in each cell according to formulas (3) and (4)

And the covariance matrix Σ:

(3); Σ=

(4); in,

is the total number of points in each cell,

Represents a point in a cell; Step 3: Coordinate transformation, assuming that the pose transformation matrix is

, the initial value is provided by the epipolar constraint relation, and:

;in

yes

The rotation matrix of

yes

Translation matrix; using the pose transformation matrix

Each point in the local point cloud map is transformed to the global map, and the transformed point is used

To express,

is the total number of all points in the local point cloud map; Step 4: Calculate the probability density according to the transformation point

The normal distribution parameters of the cell where the probability density of each transformation point is calculated, where the calculation formula (5) is:

(5); in

Σ represents the determinant of the covariance matrix Σ; Step 5: Create the objective function. For the probability density of each point, use formula (6) to construct the objective function to evaluate the pose transformation matrix

:

(6); Step 6: Optimize the objective function and use the Newton optimization method to solve the minimum value of the objective function to obtain the optimal posture transformation matrix

; Based on the fast random search tree algorithm, the boundary points between the mapped area and the unknown area are found. The RRT algorithm includes: Step 1: Call the Init function to initialize the tree T; Step 2: Call the RandomSample function in the state space

Randomly sample a point

; Step 3: Call the Nearest function to find the closest node in the current tree T.

of

; Step 4: Call the Steer function to

As the starting point, ε is the step size, and iteratively checks

and

Are the points between them free without collision? If not, stop the iteration.

The farthest traversable point is the new node

; Step 5: Add the new node

as well as

and

The new edges between them are added to the tree T, completing the expansion of this tree and entering the next iteration; Step 6: The iteration ends and a new tree T is generated; The main control unit uses the mean shift clustering algorithm to screen a large number of boundary points obtained by the global and local detectors to obtain the required task target points. The mean shift clustering algorithm includes the following steps: Step 1: Randomly select a point from the unmarked boundary points as the starting center point

; Step2: Find out

All boundary points in the spherical area with a radius of r as the center are marked; Step 3: Calculate from

Start with the vectors to each boundary point in the spherical region, add these vectors together to get the drift vector

; Step 4: According to

=

+

Move the center point, that is

Along

The moving distance is

; Step5: Repeat Step2 to Step4 until

Very small, indicating that the iteration has converged, record the current center point

, all boundary points encountered during the iteration will be marked; Step 6: If the current center point converges

If the distance from other existing center points is less than the threshold, then the center point with more boundary points in the spherical area is retained; otherwise,

As a new center point; Step 7: Repeat the previous steps until all points are marked as visited; The master control unit will use an auction mechanism to allocate mission target points to robots in the shared network. 6. A multi-machine collaborative fusion positioning and mapping device for unknown space exploration, characterized in that it includes a memory, a processor, and a multi-machine collaborative fusion positioning and mapping method program for unknown space exploration stored in the memory and executable on the processor, wherein the processor implements the steps of the multi-machine collaborative fusion positioning and mapping method for unknown space exploration as described in any one of claims 1-5 when executing the multi-machine collaborative fusion positioning and mapping method program for unknown space exploration. 7. A computer-readable storage medium, characterized in that a multi-machine collaborative fusion positioning and mapping method program for unknown space exploration is stored on the computer-readable storage medium, and when the multi-machine collaborative fusion positioning and mapping method program for unknown space exploration is executed by a processor, the steps of the multi-machine collaborative fusion positioning and mapping method for unknown space exploration described in any one of claims 1-5 are implemented.

Images