Mobile robot positioning method based on laser radar inertia

Abstract

The invention discloses a mobile robot positioning method based on laser radar inertia. Firstly, the pose of a mobile robot is estimated through a laser radar inertia odometer; then detecting that the mobile robot returns to the reached position by using a loopback detection method based on Euclidean distance and descriptor fusion; a factor graph is constructed by utilizing the poses at multiple moments in the movement track of the mobile robot and the constraint relationship among the poses, the pose correction of the mobile robot is realized through factor graph optimization, the accumulated positioning error of the speedometer is eliminated, and finally the high-precision positioning of the mobile robot is realized. According to the method, the accuracy of loopback detection of the mobile robot is improved, and meanwhile, the problem that in the long-term positioning process of the mobile robot, the positioning precision is reduced due to long-time accumulative errors of the speedometer is solved.

Description

Technical Field

[0001] The present invention relates to the technical field of mobile robot positioning, and in particular to a mobile robot positioning method based on lidar inertia. Background Art

[0002] With the progress of technology and the continuous growth of social needs, mobile robots have been widely used in various industrial fields such as industry, agriculture, medical care, and services. The prerequisite for a mobile robot to perform complex operations is that the mobile robot can perceive its own position and map information. Only by perceiving its own position and environmental information can it reach the operation destination and further complete more complex operations. Therefore, the positioning technology of mobile robots is one of the key technologies to improve the intelligence of mobile robots. Improving the positioning accuracy and robustness of mobile robots is of great significance for enhancing the intelligence of mobile robots.

[0003] At the present stage, the mobile robot positioning method based on lidar inertia is one of the research hotspots. In recent years, some excellent open-source methods have emerged, such as positioning methods like LOAM, FAST-LIO2, and LIO-SAM. Methods like LOAM and FAST-LIO2 use the IMU to remove the distortion of the lidar point cloud, and then extract the features of each frame of the lidar point cloud. High-precision positioning of the mobile robot can be completed through the matching of the feature point cloud and the map point cloud. However, such methods cannot detect that the mobile robot returns to a position it has reached before. At the same time, the odometer will generate an increasingly large cumulative positioning error over time. In addition, processing each frame of data will reduce a certain degree of real-time performance. On the other hand, methods like LIO-SAM use loop detection to detect that the mobile robot returns to a position it has reached before, and then reduce the cumulative positioning error of the odometer through the inter-frame pose relationship and loop relationship of the mobile robot. However, the loop detection adopted in such methods is generally loop detection based on Euclidean distance or loop detection based on descriptors. This loop detection will fail due to too large a cumulative positioning error of the odometer or introduce incorrect loop relationships due to similar environmental structures, further leading to an increase in the positioning error of the mobile robot. The above methods provide means worthy of reference for the mobile robot positioning method based on lidar inertia, but in the aspect of mobile robot positioning based on lidar inertia, there are still the following problems:

[0004] 1. In the actual positioning environment, the algorithm usually needs to be deployed on the embedded platform of the mobile robot. The embedded platform with limited computing power cannot support the processing of too much lidar point cloud data, and ultimately leads to the inability of the mobile robot to perform real-time positioning.

[0005] 2. In the case of the mobile robot running for a long time in a complex environment, the odometer will generate a large cumulative positioning error.

[0006] 3. The loop detection will fail due to excessive cumulative positioning errors of the odometer and will also detect false loops due to similar environmental structures.

[0007] Based on the above discussion, it has high practical application significance to invent a mobile robot positioning method that meets real-time performance and high precision in the field of mobile robots. Summary of the Invention

[0008] The object of the present invention is to overcome the shortcomings and deficiencies of the prior art and propose a mobile robot positioning method based on lidar-inertial, which can effectively solve the problem of cumulative errors of the existing mobile robot positioning method based on lidar-inertial under long-term operation. At the same time, it further solves the problem of loop detection method failure or false loops, and finally enables the mobile robot based on lidar-inertial to achieve real-time high-precision and robust positioning in large-scale complex scenarios.

[0009] To achieve the above object, the technical solution provided by the present invention is: a mobile robot positioning method based on lidar-inertial. This method first estimates the pose of the mobile robot through a lidar-inertial odometer, then detects that the mobile robot has returned to a position it has reached before through a loop detection algorithm, and finally constructs a factor graph using the poses at multiple moments and the constraint relationships between the poses in the movement trajectory of the mobile robot, and optimizes the movement trajectory of the mobile robot through the factor graph to eliminate the cumulative positioning errors of the odometer, and finally achieve high-precision positioning of the mobile robot. Among them, the loop detection algorithm improves the accuracy of loop detection through the fusion of Euclidean distance and descriptors. In the algorithm, the environmental structure is represented by descriptors, so as to detect that the mobile robot has returned to a position it has reached before through descriptor matching. Then, a Euclidean distance threshold is used to avoid detecting false loops with a long distance due to similar environmental structures by descriptors. At the same time, descriptors without distance limit and an adaptive Euclidean distance threshold are used to avoid the problem of loop detection failure caused by the cumulative positioning error of the odometer being greater than the Euclidean distance threshold. After detecting a loop, the loop detection algorithm outputs the loop relationship between the current key frame and the loop key frame, where the loop key frame is the time frame when the mobile robot was at the loop location before.

[0010] Furthermore, the mobile robot positioning method based on lidar-inertial includes the following steps:

[0011] 1) The lidar-inertial odometer preliminarily estimates the pose of the mobile robot:

[0012] First, receive multiple frames of lidar point cloud data and multiple frames of IMU data. Then, estimate the pose of the mobile robot in the current frame through the integration of multiple frames of IMU data and the matching of adjacent two frames of lidar point cloud data. The frame rate of the odometer outputting the pose of the mobile robot is the same as the frame rate of the lidar data acquisition. Introduce the key frame mechanism. If the pose of the mobile robot in the current frame changes significantly compared to the previous frame, construct the current frame as a key frame. Finally, avoid the pose optimization calculation of ordinary frames with small pose changes through the key frame mechanism, thereby improving the calculation efficiency and maintaining real-time performance.

[0013] 2) After obtaining the pose of the mobile robot in the current key frame through the odometer, detect that the mobile robot has returned to a position it has reached before through the loop closure detection algorithm.

[0014] 3) After a period of movement, the mobile robot will generate a motion trajectory, which consists of multiple frames of mobile robot pose data. In factor graph optimization, construct a factor graph with multiple frames of mobile robot pose data as variables, and then construct the lidar inertial odometer factor and the loop closure factor respectively through the inter-frame pose transformation relationship output by the odometer and the loop closure relationship output by the loop closure detection algorithm. Furthermore, complete the optimization of the factor graph variables through the above factors, and finally complete the correction of all poses in the mobile robot motion trajectory, making the estimated motion trajectory of the mobile robot closer to the true motion trajectory of the mobile robot, thereby completing the high-precision positioning of the mobile robot.

[0015] Furthermore, step 1) includes the following steps:

[0016] 1.1) Data synchronization: First, read multiple frames of lidar point cloud data and multiple frames of IMU data, and then complete the synchronization of multiple frames of lidar point cloud data and multiple frames of IMU data. Based on the high-frequency multiple frames of IMU data, generate IMU data frames aligned with the time stamps of each frame of lidar point cloud data through interpolation.

[0017] 1.2) Initial pose estimation of the mobile robot: Calculate the integration of multiple frames of high-frequency IMU data, establish a motion model of the mobile robot through the integration result, and finally obtain the initial pose of the current mobile robot. The IMU coordinate system is denoted as I, the lidar coordinate system is denoted as L, and the global coordinate system of the mobile robot is denoted as G. The specific mathematical form of the state x of the mobile robot is as follows:

[0018]

[0019] In the formula, T represents the transpose of a matrix or vector. and respectively represent the rotation and translation of the IMU in the global coordinate system G. and Combined together, they are represented as pose. is the velocity of the IMU in the global coordinate system G, b g and b a are the biases of the IMU gyroscope and accelerometer respectively. G g is the gravity vector in the global coordinate system G; the specific mathematical form of the motion model of the mobile robot from the i-th frame to the (i + 1)-th frame is as follows:

[0020]

[0021] In the formula, x i and x i+1 are the states of the mobile robot at the i-th frame and the (i + 1)-th frame respectively. represents the rotation transformation between the IMU coordinate system and the global coordinate system G at the i-th frame. G g i is the gravitational acceleration in the global coordinate system G at the i-th frame, Δt is the time interval between the i-th frame and the (i + 1)-th frame, w i and a i are the angular velocity and acceleration measured by the IMU at the i-th frame respectively, b gi and b ai are the biases of the IMU angular velocity and acceleration at the i-th frame respectively, n gi and n ai are the measurement Gaussian noises of the IMU angular velocity and acceleration at the i-th frame respectively. and are the noises of the gyroscope and accelerometer biases at the i-th frame respectively.

[0022] 1.3) Construct the point-plane distance residual equation: Match the current frame lidar point cloud after removing motion distortion with the global point cloud map, where the global point cloud map is composed of multiple frames of lidar point clouds stitched together; during the point cloud matching process, first traverse the undistorted lidar point cloud, then find the nearest point q of each point p in the current frame point cloud in the global point cloud map, and at the same time fit the nearest 5 points into a plane to obtain the normal vector u of the plane, and then construct the point-plane distance residual equation, and the specific mathematical form is as follows:

[0023] res = ( G T L p - q)u = 0

[0024] In the formula, res is the distance between point p and its nearest point q. G T L is the transformation from the lidar coordinate system to the global coordinate system. If the current frame state estimation is accurate, due to the successful feature point matching, that is, the two feature points should be in the same position in the global coordinate system, so the residual is 0.

[0025] 1.4) Iterative optimization: Combining the initial state estimation of the mobile robot and the point-plane distance residual equation, the optimal state estimation of the mobile robot can be obtained by minimizing the residual, and high-precision positioning of the mobile robot is achieved using lidar inertial odometry;

[0026] 1.5) Introduce a keyframe mechanism to construct keyframes. First, read the poses of the current frame odometry and the previous frame, calculate the pose change from the current frame to the previous frame. If the rotation or translation from the current frame to the previous frame exceeds the threshold threshold, the current frame is constructed as a keyframe. The specific mathematical form is as follows:

[0027]

[0028] When the pose change between the state of the (i + 1)-th frame and the state of the i-th frame is greater than the threshold threshold, the current (i + 1)-th frame is constructed as a keyframe, and the keyframe is used for subsequent loop closure detection and factor graph optimization.

[0029] Furthermore, step 2) includes the following steps:

[0030] 2.1) Construct a Scan Context descriptor: First, use the keyframe mechanism to determine whether the current frame is a keyframe. If the current frame is a keyframe, use the point cloud of the current keyframe to construct a Scan Context descriptor. The descriptor divides the point cloud into 20 rings, each ring is denoted as Ring, and each Ring has 60 sectors, each sector is denoted as sector. Therefore, the descriptor is represented as a 20 * 60 two-dimensional matrix in total. The smallest data unit in the matrix is the maximum height value of the point cloud in each sector. The Scan Context descriptor can describe the structure of the current environment;

[0031] 2.2) Preliminary search for candidate loop closure keyframes: Construct the feature description of each ring of the descriptor, and its feature description is denoted as Ring-Key. Ring-Key is the mean value of all sectors within each Ring, that is, the descriptor is reduced to a 20 * 1 vector; Multiple keyframes are constructed during the long-term positioning of the mobile robot, and these keyframes form a set of historical keyframes; To improve the search efficiency, use the Ring-Key after the descriptor is reduced to search for candidate loop closure keyframes in the set of historical keyframes. The specific search process is as follows: Calculate the distance between the Ring-Key vectors corresponding to two keyframes. If the distance is less than the preset threshold, it is determined that the two keyframes have a loop closure relationship, and then all the found candidate keyframes are constructed into a set of candidate loop closure keyframes;

[0032] 2.3) Use an adaptive Euclidean distance threshold to screen candidate loop closure key frames: Based on the initial candidate loop closure key frames, use an adaptive Euclidean distance threshold for screening to avoid false matches caused by similar environmental structures in descriptors. First, calculate the Euclidean distance between the current key frame and each key frame in the candidate loop closure key frame set. Then, to prevent the fixed Euclidean distance threshold from failing due to the long-term cumulative error of the odometer, use an adaptive Euclidean distance threshold to determine whether it is a loop closure key frame. The specific mathematical form is as follows:

[0033] dst(p cur ,p i )=||p cur -p i || 2 <r cur

[0034] r cur =r ori +αt

[0035] In the formula, p cur and p i are the poses of the current key frame and the candidate loop closure key frame at the i-th historical frame respectively. r cur and r ori are the Euclidean distance threshold at the current moment and the initial Euclidean distance threshold respectively. α is the average inter-frame error of the odometer, and t is the number of key frames since the last loop closure;

[0036] 2.4) Obtain the final loop closure key frame through descriptor matching: After screening the candidate loop closure key frames using the adaptive Euclidean distance threshold, calculate the similarity between the descriptor of the current key frame and the descriptors of the candidate loop closure key frames. Finally, select the candidate loop closure key frame with a similarity score higher than the threshold and the highest similarity score as the final loop closure key frame to achieve loop closure detection. The mathematical form for calculating the similarity is as follows:

[0037]

[0038] In the formula, I c and I q are the descriptors of the current key frame and the candidate loop closure key frame respectively. N is the number of column vectors of the candidate loop closure key frame, and are the j-th column vectors of the descriptors of the current key frame and the candidate loop closure key frame respectively. d is the cosine similarity.

[0039] Furthermore, step 3) includes the following steps:

[0040] 3.1) Construct a factor graph: Obtain the pose of the current key frame of the mobile robot through the odometer, and then regard the state of the current key frame as the variable nodes of the factor graph. Construct the factor graph through multiple variable nodes and the factor constraints between the variable nodes;

[0041] 3.2) Construct the lidar inertial odometry factor: First, obtain the pose state of the current key frame of the mobile robot through the odometer, and then construct the lidar inertial odometry factor according to the pose transformation relationship between the current frame and the previous frame. The specific mathematical form is as follows:

[0042] e L (x i ,x i-1 )=x i -x i-1 -u i

[0043] In the formula, x i and x i-1 are the states of the mobile robot in the i-th frame and the (i - 1)-th frame respectively, e L is the error between the estimated pose and the true pose between the i-th frame and the (i - 1)-th frame, and u i is the pose transformation relationship between the current frame and the previous frame; The optimal state x of the current frame mobile robot is obtained by minimizing the error i ;

[0044] 3.3) Construct the loop closure factor: When the current i-th frame, the state of the mobile robot is x i . First, detect the loop closure key frame through the loop closure detection algorithm based on the fusion of Euclidean distance and descriptors. The state of the mobile robot in the loop closure key frame is x j ; Then, complete the registration of the current key frame point cloud and the loop closure key frame point cloud through the ICP algorithm. The ICP algorithm continuously optimizes the pose transformation between the current key frame point cloud and the loop closure key frame point cloud, and finally minimizes the distance between the two point clouds, so as to obtain the accurate pose transformation relationship T between the current key frame and the loop closure key frame ji ; Among them, the loop closure factor is constructed through the inter-frame pose transformation relationship provided by the loop closure detection algorithm. The specific mathematical form is as follows:

[0045] e lc (x i ,x j )=x i -x j -T ji

[0046] In the formula, x j is the state of the mobile robot in the j-th frame loop closure key frame, e lc is the error between the estimated pose and the true pose between the two loop closure frames, and Tji is the pose transformation relationship from the current key frame to the loop closure key frame;

[0047] 3.4) Factor graph optimization: After determining that the variable node is the mobile robot state x and the factors are the lidar inertial odometry factor and the loop closure factor, the factor graph constructs a non-linear least squares problem to solve for the globally optimal robot state. The specific mathematical form is as follows:

[0048]

[0049] In the formula, ∑ L and ∑ lc are the covariance matrices of e L and e lc respectively, and X is the set of all states x from the initial to the current moment of the mobile robot; By minimizing the residuals of the lidar inertial odometry factor and the loop closure factor, the globally optimal state of the mobile robot is obtained, realizing high-precision and robust positioning of the mobile robot.

[0050] Compared with the prior art, the present invention has the following advantages and beneficial effects:

[0051] 1. The present invention introduces a key frame mechanism in the lidar inertial odometry, and only selects the frames with certain changes in the pose of the mobile robot as key frames for calculation, which not only ensures the positioning accuracy but also improves the calculation efficiency, and maintains the real-time nature of positioning.

[0052] 2. In the loop closure detection of the present invention, a loop closure detection algorithm based on the fusion of Euclidean distance and descriptors is used to replace the traditional loop closure detection method based on Euclidean distance or based on descriptors, which not only alleviates the influence of the loop closure detection by the cumulative positioning error of the odometry but also reduces the probability of false loop closures.

[0053] 3. The present invention adds factor graph optimization after the lidar inertial odometry, which reduces the cumulative positioning error generated by the lidar inertial odometry during long-term operation and improves the positioning accuracy and robustness of the mobile robot in various scale scenarios. BRIEF DESCRIPTION OF THE DRAWINGS

[0054] Figure 1 is the overall architecture diagram of the method of the present invention.

[0055] Figure 2 is the loop closure detection schematic diagram of the embodiment of the present invention; In the figure, the black line is the positioning trajectory of the mobile robot, each dot represents a different key frame, the retrieval radius r is the Euclidean distance threshold, the false loop closure key frames identified by the loop closure detection based on descriptors are outside the retrieval radius, and the high-precision candidate loop closure key frames identified by the loop closure detection based on the fusion of Euclidean distance and descriptors are inside the retrieval radius.

[0056] Figure 3 Schematic diagram of the backend factor graph for the embodiments of the present invention.

[0057] Figure 4 Comparison graph of the experimental results for the embodiments of the present invention; in the figure, (a) is the positioning trajectory and loop closure detection effect of the traditional lidar inertial positioning method, (b) is the positioning trajectory and loop closure detection effect of the method of the present invention, Trajectory is the algorithm positioning trajectory, Loop Closure is the loop closure pair, (c) is the mapping effect of the traditional lidar inertial odometer, and (d) is the mapping effect of the method of the present invention. Specific embodiments

[0058] The present invention will be further described in detail below in conjunction with embodiments and the accompanying drawings, but the embodiments of the present invention are not limited thereto.

[0059] As Figures 1 to 4 shown, this embodiment discloses a mobile robot positioning method based on lidar inertia. This method first estimates the pose of the mobile robot through a lidar inertial odometer, then detects through a loop closure detection algorithm that the mobile robot has returned to a position it has reached before, and finally constructs a factor graph using the poses at multiple moments in the motion trajectory of the mobile robot and the constraint relationships between the poses, and optimizes the motion trajectory of the mobile robot through the factor graph to eliminate the cumulative positioning error of the odometer, and finally realizes the high-precision positioning of the mobile robot; the specific implementation includes the following steps:

[0060] 1) The lidar inertial odometer initially estimates the pose of the mobile robot. First, receive multiple frames of point cloud data from the lidar and multiple frames of data from the IMU, and then estimate the pose of the mobile robot in the current frame through the integration of multiple frames of data from the IMU and the matching of adjacent two frames of point cloud data from the lidar, where the frame rate at which the odometer outputs the pose of the mobile robot is the same as the frame rate at which the lidar collects data; introduce a key frame mechanism. If the pose of the mobile robot in the current frame changes greatly compared with the pose of the mobile robot in the previous frame, then construct the current frame as a key frame, and finally avoid the pose optimization calculation of ordinary frames with too small pose changes through the key frame mechanism, thereby improving the calculation efficiency and maintaining real-time performance. It includes the following steps:

[0061] 1.1) Data synchronization: The vehicle movement trajectory in the scene will form a closed loop, and the movement trajectory is about 3640 meters long. First, read multiple frames of point cloud data from the lidar and multiple frames of data from the IMU, and then complete the synchronization of multiple frames of point cloud data from the lidar and multiple frames of data from the IMU. Based on the high-frequency multiple frames of data from the IMU, generate IMU data frames aligned with the time stamps of each frame of point cloud data from the lidar through interpolation;

[0062] 1.2) Initial pose estimation of the mobile robot: Calculate the integration of multiple frames of data from the high-frequency IMU, establish the motion model of the mobile robot through the integration results, and finally obtain the initial pose of the current mobile robot; The IMU coordinate system is denoted as I, the lidar coordinate system is denoted as L, and the global coordinate system of the mobile robot is denoted as G. The specific mathematical form of the state x of the mobile robot is as follows:

[0063]

[0064] In the formula, T represents the transpose of a matrix or vector, and respectively represent the rotation and translation of the IMU in the global coordinate system G, and combined together represent the pose, is the velocity of the IMU in the global coordinate system G, b g and b a are the biases of the IMU gyroscope and accelerometer respectively, G g is the gravity vector in the global coordinate system G; The specific mathematical form of the motion model of the mobile robot from the i-th frame to the i + 1-th frame is as follows:

[0065]

[0066] In the formula, x i and x i+1 are the states of the mobile robot at the i-th frame and the i + 1-th frame respectively, represents the rotation transformation between the IMU coordinate system and the global coordinate system G at the i-th frame, G g i is the gravitational acceleration in the global coordinate system G at the i-th frame, Δt is the time interval between the i-th frame and the i + 1-th frame, w i and a i are the angular velocity and acceleration measured by the IMU at the i-th frame respectively, b gi and b ai are the biases of the IMU angular velocity and acceleration at the i-th frame respectively, n gi and n ai are the measurement Gaussian noises of the IMU angular velocity and acceleration at the i-th frame respectively, and are the noises of the gyroscope and accelerometer biases at the i-th frame respectively;

[0067] 1.3) Construct the point-plane distance residual equation: Match the current frame of lidar point cloud after removing motion distortion with the global point cloud map, where the global point cloud map is composed of multiple frames of lidar point clouds stitched together; during the point cloud matching process, first traverse the undistorted lidar point cloud, then find the nearest point q of each point p in the current frame of point cloud in the global point cloud map, and at the same time fit the nearest 5 points into a plane to obtain the normal vector u of the plane, and then construct the point-plane distance residual equation, and the specific mathematical form is as follows:

[0068] res = ( G T L p - q)u = 0

[0069] In the formula, res is the distance between point p and its nearest point q, G T L is the transformation from the lidar coordinate system to the global coordinate system. If the current frame state estimation is accurate, due to the successful feature point matching, that is, the two feature points should be in the same position in the global coordinate system, so the residual is 0;

[0070] 1.4) Iterative optimization: Combine the initial state estimation of the mobile robot and the point-plane distance residual equation, and by minimizing the residual, the optimal state estimation of the mobile robot can be obtained, and the high-precision positioning of the mobile robot can be realized by using the lidar inertial odometer;

[0071] 1.5) Introduce the key frame mechanism to construct key frames. First, read the poses of the current frame odometer and the previous frame, calculate the pose change from the current frame to the previous frame. If the rotation or translation from the current frame to the previous frame exceeds the threshold threshold, the current frame will be constructed as a key frame, and the specific mathematical form is as follows:

[0072]

[0073] When the pose change in the state of the (i + 1)-th frame and the i-th frame is greater than the threshold threshold, the current (i + 1)-th frame is constructed as a key frame, and the key frame is used for subsequent loop detection and factor graph optimization.

[0074] 2) Loop detection identifies that the mobile robot has returned to a previously visited location. After obtaining the pose of the mobile robot at the current key frame through odometry, the loop detection algorithm is used to detect that the mobile robot has returned to a previously visited location. This loop detection algorithm improves the accuracy of loop detection through the fusion of Euclidean distance and descriptors. In the algorithm, the environmental structure is represented by descriptors, so that the loop detection algorithm can detect that the mobile robot has returned to a previously visited location through descriptor matching. The Euclidean distance threshold is used to avoid false loops with a large distance detected due to similar environmental structures by descriptors. At the same time, descriptors without distance limit and an adaptive Euclidean distance threshold are used to avoid the problem of loop failure caused by the cumulative positioning error of odometry being greater than the Euclidean distance threshold. After detecting a loop, the loop detection algorithm outputs the loop relationship between the current key frame and the loop key frame, where the loop key frame is the time frame when the mobile robot was at the loop location. It includes the following steps:

[0075] 2.1) Construct the Scan Context descriptor: First, use the key frame mechanism to determine whether the current frame is a key frame. If the current frame is a key frame, use the point cloud of the current key frame to construct the Scan Context descriptor. The descriptor divides the point cloud into 20 rings, each ring is denoted as Ring, and each Ring has 60 sectors, each sector is denoted as sector. Therefore, the descriptor is divided into a two-dimensional matrix of 20 * 60 for representation. The smallest data unit in the matrix is the maximum height value of the point cloud in each sector. The Scan Context descriptor can describe the structure of the current environment;

[0076] 2.2) Initially search for candidate loop key frames: Construct the feature description of each ring of the descriptor, and its feature description is denoted as Ring-Key. Ring-Key is the mean value of all sectors within each Ring, that is, the descriptor is reduced to a 20 * 1 vector; Multiple key frames are constructed during the long-term positioning of the mobile robot, and these key frames form a set of historical key frames; To improve the search efficiency, use the Ring-Key after the descriptor is reduced to search for candidate loop key frames in the set of historical key frames. The specific search process is as follows: Calculate the distance between the Ring-Key vectors corresponding to two key frames. If the distance is less than the preset threshold, it is considered that the two frames have a loop relationship; Then construct all the candidate key frames found into a set of candidate loop key frames;

[0077] 2.3) Use an adaptive Euclidean distance threshold to screen candidate loop closure key frames: Based on the initial candidate loop closure key frames, use an adaptive Euclidean distance threshold for screening to avoid false matches caused by similar environmental structures for descriptors; first calculate the Euclidean distance between the current key frame and each key frame in the candidate loop closure key frame set, and then, to avoid the failure of a fixed Euclidean distance threshold due to the long-term cumulative error of the odometer, use an adaptive Euclidean distance threshold to determine whether it is a loop closure key frame. The specific mathematical form is as follows:

[0078] dst(p cur ,p i )=||p cur -p i || 2 <r cur

[0079] r cur =r ori +αt

[0080] In the formula, p cur and p i are the poses of the current key frame and the candidate loop closure key frame at the i-th historical frame respectively, r cur and r ori are the Euclidean distance threshold at the current moment and the initial Euclidean distance threshold respectively, α is the average inter-frame error of the odometer, and t is the number of key frames since the last loop closure;

[0081] 2.4) Obtain the final loop closure key frame through descriptor matching: After screening out candidate loop closure key frames using an adaptive Euclidean distance threshold, calculate the similarity between the descriptor of the current key frame and the descriptors of the candidate loop closure key frames. Finally, select the candidate loop closure key frame with a similarity score higher than the threshold and the highest similarity score as the final loop closure key frame to achieve loop closure detection; the mathematical form for calculating the similarity is as follows:

[0082]

[0083] In the formula, I c and I q are the descriptors of the current key frame and the candidate loop closure key frame respectively, N is the number of column vectors of the candidate loop closure key frame, and are the j-th column vectors of the descriptors of the current key frame and the candidate loop closure key frame respectively, and d is the cosine similarity.

[0084] 3) After a period of movement, the mobile robot generates a motion trajectory, which consists of pose data of the mobile robot in multiple frames. In factor graph optimization, a factor graph is constructed with pose data of the mobile robot in multiple frames as variables, and then a lidar inertial odometry factor and a loop closure factor are constructed respectively through the inter-frame pose transformation relationship output by the odometer and the loop closure relationship output by the loop closure detection algorithm. Furthermore, the variables of the factor graph are optimized through the above factors, and finally the correction of all poses in the motion trajectory of the mobile robot is completed, making the estimated motion trajectory of the mobile robot closer to the true motion trajectory of the mobile robot, thereby completing the high-precision positioning of the mobile robot. The steps include:

[0085] 3.1) Construct a factor graph: Obtain the pose of the mobile robot at the current key frame through the odometer, and then regard the state of the current key frame as a variable node of the factor graph. The factor graph is constructed through multiple variable nodes and factor constraints between variable nodes;

[0086] 3.2) Construct a lidar inertial odometry factor: First, obtain the pose state of the mobile robot at the current key frame through the odometer, and then construct a lidar inertial odometry factor according to the pose transformation relationship between the current frame and the previous frame. The specific mathematical form is as follows:

[0087] e L (x i ,x i-1 )=x i -x i-1 -u i

[0088] In the formula, x i and x i-1 are the states of the mobile robot in the i-th frame and the (i - 1)-th frame respectively, e L is the error between the estimated pose and the true pose between the i-th frame and the (i - 1)-th frame, and u i is the pose transformation relationship between the current frame and the previous frame; The optimal state x of the mobile robot at the current frame is obtained by minimizing the error i ;

[0089] 3.3) Construct a loop closure factor: The state of the mobile robot at the current i-th frame is x i , and then the loop closure key frame is detected through a loop closure detection algorithm based on the fusion of Euclidean distance and descriptors. The state of the mobile robot at the loop closure key frame is x j ; Then, the registration of the point cloud of the current key frame and the point cloud of the loop closure key frame is completed through the ICP algorithm. The ICP algorithm continuously optimizes the pose transformation between the point cloud of the current key frame and the point cloud of the loop closure key frame, and finally minimizes the distance between the two point clouds, thereby obtaining an accurate pose transformation relationship T between the current key frame and the loop closure key frame ji; Among them, the loop factor is constructed based on the inter-frame pose transformation relationship provided by the loop detection algorithm, and the specific mathematical form is as follows:

[0090] e lc (x i ,x j )=x i -x j -T ji

[0091] In the formula, x j is the state of the j-th loop key frame of the mobile robot, e lc is the error between the estimated pose and the true pose between the two loop frames, and T ji is the pose transformation relationship from the current key frame to the loop key frame;

[0092] 3.4) Factor graph optimization: After determining that the variable node is the state x of the mobile robot and the factors are the lidar inertial odometry factor and the loop factor, the factor graph constructs a non-linear least squares problem to solve the globally optimal robot state, and the specific mathematical form is as follows:

[0093]

[0094] In the formula, ∑ L and ∑ lc are the covariance matrices of e L and e lc respectively, and X is the set of all states x from the initial to the current moment of the mobile robot; by minimizing the residuals of the lidar inertial odometry factor and the loop factor, the globally optimal state of the mobile robot is obtained, realizing high-precision and robust positioning of the mobile robot.

[0095] The experimental results of the above-mentioned lidar-inertial based mobile robot positioning method in this embodiment are described in detail below:

[0096] The comparison results between the traditional lidar-inertial based mobile robot positioning method and the method of the present invention are as Figure 4As shown in the figure, (a) shows the trajectory of the traditional algorithm in the closed-loop scenario and its loop closure detection effect, and (b) shows the loop closure detection effect of the method of the present invention in the closed-loop scenario. From (a) and (b), it can be seen that due to the large cumulative error generated by the odometer during long-term operation, the trajectory of the traditional algorithm cannot achieve closed-loop. Even if loop closure detection based on Euclidean distance is added, the loop cannot be detected due to the excessive cumulative error, as shown in (a) of the figure. However, in (b) of the figure, the loop can be accurately detected at the loop closure of the scenario. The loop closure detection adds a connecting line Loop Closure at the fault of the trajectory in (a) of the figure, indicating that the loop closure detection success rate of the method of the present invention is higher. (c) and (d) in the figure respectively show the mapping effects of the traditional algorithm and the method of the present invention at the loop closure of the scenario. In (c), due to the lack of loop closure detection and backend optimization in the traditional algorithm, a large cumulative error is generated by the algorithm, and the positioning drifts, resulting in the mapping drifting up and down. However, in (d) of the figure, the trajectory drift can be accurately corrected by using loop closure detection and factor graph optimization, and high-precision mapping is further completed, indicating that the positioning accuracy and robustness of the method of the present invention are higher.

[0097] The above embodiments are preferred embodiments of the present invention, but the embodiments of the present invention are not limited to the above embodiments. Any other changes, modifications, substitutions, combinations, and simplifications made without departing from the spirit and principle of the present invention shall be equivalent replacement methods and shall be included in the protection scope of the present invention.

Claims

1. A mobile robot positioning method based on lidar and inertia, characterized in that, This method first estimates the pose of the mobile robot through lidar inertial odometry, then detects that the mobile robot has returned to a previously reached position through a loop closure detection algorithm, and finally constructs a factor graph using the poses at multiple moments in the motion trajectory of the mobile robot and the constraint relationships between the poses, optimizes the motion trajectory of the mobile robot through the factor graph, eliminates the cumulative positioning error of the odometry, and finally realizes the high-precision positioning of the mobile robot. Among them, this loop closure detection algorithm improves the accuracy of loop closure detection through the fusion of Euclidean distance and descriptors, represents the environmental structure through descriptors in the algorithm, thereby detects that the mobile robot has returned to a previously reached position through descriptor matching, and then uses a Euclidean distance threshold to avoid false loop closures with far distances detected by descriptors due to similar environmental structures. At the same time, descriptors without distance limitations and an adaptive Euclidean distance threshold are used to avoid the problem of loop closure failure caused by the cumulative positioning error of the odometry being greater than the Euclidean distance threshold. After detecting a loop closure, the loop closure detection algorithm outputs the loop closure relationship between the current key frame and the loop closure key frame, where the loop closure key frame is the time frame when the mobile robot was at the loop closure location.

2. The method for positioning a mobile robot based on lidar and inertia according to claim 1, wherein, It includes the following steps: 1) The lidar inertial odometry initially estimates the pose of the mobile robot: First, receive multiple frames of point cloud data from the lidar and multiple frames of data from the IMU, and then estimate the pose of the mobile robot in the current frame through the integration of multiple frames of data from the IMU and the matching of adjacent two frames of point cloud data from the lidar. The frame rate at which the odometry outputs the pose of the mobile robot is the same as the frame rate at which the lidar collects data. Introduce a key frame mechanism. If the pose of the mobile robot in the current frame changes significantly compared to the pose of the mobile robot in the previous frame, then construct the current frame as a key frame. Finally, through the key frame mechanism, avoid the pose optimization calculation of ordinary frames with small pose changes, thereby improving the calculation efficiency and maintaining real-time performance. 2) After obtaining the pose of the mobile robot in the current key frame through the odometry, detect that the mobile robot has returned to a previously reached position through the loop closure detection algorithm. 3) After a period of movement, the mobile robot will generate a motion trajectory, which consists of pose data of multiple frames of the mobile robot. In factor graph optimization, construct a factor graph with the pose data of multiple frames of the mobile robot as variables, and then construct a lidar inertial odometry factor and a loop closure factor respectively through the inter-frame pose transformation relationship output by the odometry and the loop closure relationship output by the loop closure detection algorithm. Furthermore, complete the optimization of the factor graph variables through the above factors, and finally complete the correction of all poses in the motion trajectory of the mobile robot, making the estimated motion trajectory of the mobile robot closer to the true motion trajectory of the mobile robot, thereby completing the high-precision positioning of the mobile robot.

3. A method for positioning a mobile robot based on lidar and inertia according to claim 2, characterized in that, The step 1) includes the following steps: 1.1) Data synchronization: First read multiple frames of point cloud data from the lidar and multiple frames of data from the IMU, and then complete the synchronization of multiple frames of point cloud data from the lidar and multiple frames of data from the IMU. Based on the high-frequency multiple frames of data from the IMU, generate IMU data frames aligned with the time stamps of each frame of point cloud data from the lidar through interpolation. 1.2) Initial pose estimation of the mobile robot: Calculate the integration of multiple frames of data from the high-frequency IMU, establish the motion model of the mobile robot through the integration result, and finally obtain the initial pose of the current mobile robot; The IMU coordinate system is denoted as I, the lidar coordinate system is denoted as L, and the global coordinate system of the mobile robot is denoted as G. The specific mathematical form of the state x of the mobile robot is as follows: where T represents the transpose of a matrix or vector, and represent the rotation and translation of the IMU in the global coordinate system G respectively, and combined together are represented as the pose, is the velocity of the IMU in the global coordinate system G, b g and b a are the biases of the IMU gyroscope and accelerometer respectively, G g is the gravity vector in the global coordinate system G; the specific mathematical form of the motion model of the mobile robot from the i-th frame to the (i + 1)-th frame is as follows: where x i and x i+1 are the states of the mobile robot at the i-th frame and the (i + 1)-th frame respectively, represents the rotation transformation between the IMU coordinate system and the global coordinate system G at the i-th frame, G g i is the gravitational acceleration in the global coordinate system G at the i-th frame, Δt is the time interval between the i-th frame and the (i + 1)-th frame, w i and a i are the angular velocity and acceleration measured by the IMU at the i-th frame respectively, b gi and b ai are the biases of the IMU angular velocity and acceleration at the i-th frame respectively, n gi and n ai are the measurement Gaussian noises of the IMU angular velocity and acceleration at the i-th frame respectively, and are the noises of the gyroscope and accelerometer biases at the i-th frame respectively; 1.3) Construct the point-plane distance residual equation: Match the current frame of lidar point cloud after removing motion distortion with the global point cloud map, where the global point cloud map is composed of multiple frames of lidar point clouds stitched together; During the point cloud matching process, first traverse the de-distorted lidar point cloud, then find the nearest point q of each point p in the current frame of point cloud in the global point cloud map. At the same time, fit the nearest 5 points into a plane to obtain the normal vector u of the plane, and then construct the point-plane distance residual equation. The specific mathematical form is as follows: res = ( G T L p - q)u = 0 where res is the distance between point p and its nearest point q G T L is the transformation from the lidar coordinate system to the global coordinate system. If the current frame state estimation is accurate, due to the successful feature point matching, that is, the two feature points should be in the same position in the global coordinate system, so the residual is 0; 1.4) Iterative optimization: Combine the initial state estimation of the mobile robot and the point-plane distance residual equation. By minimizing the residual, the optimal state estimation of the mobile robot can be obtained, and high-precision positioning of the mobile robot can be achieved using lidar inertial odometry. 1.5) Introduce the key frame mechanism to construct key frames. First, read the pose of the current frame odometer and the pose of the previous frame, calculate the pose change from the current frame to the previous frame. If the rotation or translation from the current frame to the previous frame exceeds the threshold threshold, the current frame is constructed as a key frame. The specific mathematical form is as follows: When the pose change in the state of the (i + 1)-th frame and the state of the i-th frame is greater than the threshold threshold, the current (i + 1)-th frame is constructed as a key frame, and the key frame is used for subsequent loop detection and factor graph optimization.

4. A method for positioning a mobile robot based on lidar and inertia according to claim 3, characterized in that, The step 2) includes the following steps: 2.1) Construct the Scan Context descriptor: First, use the key frame mechanism to determine whether the current frame is a key frame. If the current frame is a key frame, use the current key frame point cloud to construct the Scan Context descriptor. The descriptor divides the point cloud into 20 rings, each ring is denoted as Ring, and each Ring has 60 sectors, each sector is denoted as sector. Therefore, the descriptor is represented by a 20 * 60 two-dimensional matrix. The smallest data unit in the matrix is the maximum height value of the point cloud in each sector. The Scan Context descriptor can describe the structure of the current environment. 2.2) Preliminary search for candidate loop key frames: Construct the feature description of each loop of the descriptor, and its feature description is denoted as Ring-Key. Ring-Key is the mean value of all sectors within each Ring, that is, the descriptor is reduced to a 20*1 vector; Multiple key frames are constructed during the long-term positioning of the mobile robot, and these key frames form the set of historical key frames; To improve the search efficiency, use the Ring-Key after descriptor dimensionality reduction to search for candidate loop key frames in the set of historical key frames. The specific search process is as follows: Calculate the distance between the Ring-Key vectors corresponding to two key frames. If the distance is less than the preset threshold, it is determined that the two key frames have a loop relationship, and then all the found candidate key frames are constructed into a set of candidate loop key frames; 2.3) Use an adaptive Euclidean distance threshold to filter candidate loop key frames: Based on the initial candidate loop key frames, use an adaptive Euclidean distance threshold for filtering to avoid false matches caused by similar environmental structures of the descriptors; First, calculate the Euclidean distance between the current key frame and each key frame in the set of candidate loop key frames, and then, to avoid the failure of the fixed Euclidean distance threshold due to the long-term cumulative error of the odometer, use an adaptive Euclidean distance threshold to determine whether it is a loop key frame. The specific mathematical form is as follows: dst(p cur ,p i ) = ||p cur -p i || 2 < r cur r cur ＝r ori +αt where p cur and p i are the poses of the current key frame and the candidate loop closure key frame located at the historical i-th frame respectively, r cur and r ori are the Euclidean distance threshold at the current moment and the initial Euclidean distance threshold respectively, α is the average inter-frame error of the odometer, and t is the number of key frames since the last loop closure; 2.4) Obtain the final loop key frame through descriptor matching: After filtering out the candidate loop key frames through the adaptive Euclidean distance threshold, calculate the similarity between the descriptor of the current key frame and the descriptors of the candidate loop key frames. Finally, select the candidate loop key frame with a similarity score higher than the threshold and the highest similarity score as the final loop key frame to achieve loop detection; The mathematical form for calculating the similarity is as follows: where I c and I q are the descriptors of the current key frame and the candidate loop closure key frame respectively, N is the number of column vectors of the candidate loop closure key frame, and are the j-th column vectors of the descriptors of the current key frame and the candidate loop closure key frame respectively, and d is the cosine similarity.

5. A method for positioning a mobile robot based on lidar and inertia according to claim 4, characterized in that, Step 3) includes the following steps: 3.1) Construct a factor graph: Obtain the pose of the mobile robot at the current key frame through the odometer, and then regard the state of the current key frame as the variable node of the factor graph. The factor graph is constructed through multiple variable nodes and factor constraints between the variable nodes; 3.2) Construct the lidar inertial odometer factor: First, obtain the pose state of the mobile robot at the current key frame through the odometer, and then construct the lidar inertial odometer factor according to the pose transformation relationship between the current frame and the previous frame. The specific mathematical form is as follows: e L (x i ,x i-1 )=x i -x i-1 -u i where x i and x i-1 are the states of the mobile robot in the i-th frame and the (i - 1)-th frame respectively, e L is the error between the estimated pose and the true pose between the i-th frame and the (i - 1)-th frame, u i is the pose transformation relationship between the current frame and the previous frame; the optimal state x i of the mobile robot in the current frame is obtained by minimizing the error; 3.3) Construct the loop factor: The state of the mobile robot at the current $i$-th frame is $x$. i , first, detect the loop key frames through the loop detection algorithm based on the fusion of Euclidean distance and descriptors. The state of the mobile robot in the loop key frames is $x$. j ; Then, complete the registration of the current key frame point cloud and the loop key frame point cloud through the ICP algorithm. The ICP algorithm continuously optimizes the pose transformation between the current key frame point cloud and the loop key frame point cloud, and finally minimizes the distance between the two point clouds, so as to obtain the accurate pose transformation relationship $T$ between the current key frame and the loop key frame. ji ; Among them, the loop factor is constructed through the inter-frame pose transformation relationship provided by the loop detection algorithm, and the specific mathematical form is as follows: e lc (x i ,x j )=x i -x j -T ji where x j is the state of the j-th loop key frame of the mobile robot, and e lc is the error between the estimated pose and the true pose between two loop frames, and T ji is the pose transformation relationship from the current key frame to the loop key frame; 3.4) Factor graph optimization: After determining that the variable node is the state x of the mobile robot, and the factors are the lidar inertial odometer factor and the loop factor, the factor graph constructs a non-linear least squares problem to solve for the globally optimal robot state. The specific mathematical form is as follows: Wherein, ∑ L and ∑ lc are the covariance matrices of e L and e lc respectively, and X is the set of all states x of the mobile robot from the initial to the current moment; by minimizing the residuals of the lidar-inertial odometry factor and the loop closure factor, the globally optimal state of the mobile robot is obtained, realizing high-precision and robust positioning of the mobile robot.

Images