Plug-and-Play Factor Graph Fusion Method for Multi-Sensor SLAM System Based on Incremental Smoothing

Abstract

An incremental smoothing-based plug-and-play factor graph fusion method for multi-sensor SLAM systems relates to the field of SLAM technology. The present invention is to solve the fusion problem of the tight coupling joint optimization of information of SLAM systems using multi-class sensors of different sources, and involves the use of three sensors: cameras, IMUs, and LiDARs. The present invention filters out unreliable sensor data in a targeted manner by judging the reliability of camera and LiDAR sensor data, screens out sensor information with reliable data, and improves the sensor fault tolerance of the SLAM system; selectively constructs constraint factors, constructs a variable-structure, plug-and-play tight coupling factor graph, and performs state estimation only using the fusion of reliable data, ensuring the positioning accuracy; uses an incremental smoothing method for optimization and solution, and adds the newly added observation information of the SLAM system by updating the Bayesian tree, ensuring the computational efficiency of global optimization and improving the response speed of the SLAM system.

Description

Technical Field

[0001] The invention belongs to the technical field of SLAM. Background Art

[0002] SLAM (Simultaneous Localization and Mapping) technology enables mobile robots to achieve autonomous navigation, positioning and planning. Through one or more sensor devices carried by the mobile robot itself, it can perceive environmental information, estimate its own posture, track the position of the mobile robot itself, locate its position relative to the surrounding environment, and build an environmental map at the same time. The ability of a single type of sensor to obtain information is limited, resulting in limited positioning accuracy and poor robustness of the mobile robot to achieve SLAM function. The method of using information fusion of multiple types of sensors to achieve the fusion of perception data of different dimensions and types can improve the positioning accuracy of mobile robot SLAM, and have strong fault tolerance and robustness for sensors. According to the different types of sensors, sensors used in SLAM systems can be divided into two categories: one is a sensor that measures the robot's own state, such as IMU (inertial measurement unit); the other is a sensor for mobile robots to perceive information about the surrounding environment, such as cameras, LiDAR (3D laser radar), etc. According to the different information fusion methods, it can be divided into filtering-based methods and optimization-based methods. The filtering-based method is implemented according to Bayesian theory. Through the connection between the state model and the observation model, the positioning problem is assumed to be a maximum a posteriori probability estimation problem for solution; the optimization-based method is implemented according to graph optimization theory. The objective function is constructed through the factor graph model, and finally all state quantity information is solved iteratively.

[0003] Currently, for SLAM methods that fuse information from two types of sensors, either filtering methods or graph optimization methods can be used. For SLAM methods that fuse information from three or more types of sensors, graph optimization methods are generally used. Cameras, IMUs, and LiDARs are three types of sensors commonly selected for multi-sensor fusion SLAM systems. IMUs are used to measure the state of the mobile robot itself. Cameras can obtain continuous and texture-rich environmental information. LiDARs can obtain discrete and accurate point cloud information. The three sensors have strong information complementarity. For SLAM systems based on these three types of sensors, typical ones include "LVI-SAM: Tightly-coupled Lidar-Visual-Inertial Odometry via Smoothing and Mapping" written by Tixiao Shan et al. from the Massachusetts Institute of Technology in the United States, and "LVIO-SAM: A Multi-sensor Fusion Odometry via Smoothing and Mapping" written by Xinliang Zhong et al. from Zhejiang University. For tightly-coupled multi-sensor information fusion schemes, precise modeling of each type of sensor is required, and after coupling the information of each sensor, unified processing is carried out. The utilization rate of data information is high, and the result accuracy is relatively high. However, the fault tolerance ability is weak. If a certain sensor fails, it is easy to cause data distortion after the tight coupling of sensor information. For loose-coupled information fusion schemes, the data of each sensor needs to be processed separately, and the subsystems of each sensor are independent of each other, with strong fault tolerance ability. However, more data information is lost and the accuracy is relatively low. Therefore, existing multi-sensor fusion SLAM systems need to further improve their information fusion methods to improve the information utilization rate and the fault tolerance ability of the system at the same time. Summary of the Invention

[0004] The present invention is to solve the problems of the tightly-coupled information fusion method and fast optimization solution of multi-sensor SLAM systems, and improve the fault tolerance ability of SLAM systems. The present invention involves three types of sensors: monocular cameras, IMUs, and LiDARs. Now, a plug-and-play factor graph fusion method for multi-sensor SLAM systems based on incremental smoothing is provided.

[0005] The plug-and-play factor graph fusion method for multi-sensor SLAM systems based on incremental smoothing according to the present invention includes the following steps:

[0006] Step 1: The backend thread of the visual-inertial-lidar SLAM system queries the key frame queue and determines whether the key frame queue is empty. If it is, step 1 is repeatedly executed until the backend thread is terminated. Otherwise, step 2 is executed;

[0007] Step 2: Take out a key frame from the head of the key frame queue, construct an IMU pre-integration constraint factor according to the IMU measurement information of the key frame, and then execute Step 3;

[0008] Step 3: According to the IMU measurement data, correct the motion distortion of the LiDAR point cloud of the key frame, then filter out the outlier points and noise points in the point cloud, and then obtain a sparse point cloud through voxel filtering. Judge whether the LiDAR point cloud of the key frame is available according to the density and uniformity of the sparse point cloud. If it is available, construct a LiDAR odometry constraint factor, and then execute Step 4, otherwise directly execute Step 4;

[0009] Step 4: Perform data processing on the image information of the key frame, extract key points, calculate descriptors, use the quadtree method to realize the uniform distribution of image feature points, and judge whether the image information of the key frame is available according to the number and distribution of feature points. If it is available, construct a visual reprojection constraint factor, and then execute Step 5, otherwise directly execute Step 5;

[0010] Step 5: Judge whether the index of the key frame is greater than 10. Otherwise, return to Step 1. If it is, continue to judge whether the visual-inertial LiDAR SLAM system has created a global Bayesian tree. If it has not been created, execute Step 6. If it has been created, execute Step 7;

[0011] Step 6: Construct a global factor graph model, perform variable elimination on the global factor graph variables to generate a Bayesian network, and then convert it into a Bayesian tree structure, and then return to Step 1;

[0012] Step 7: Take out the Bayesian branch tree associated with the key frame from the global Bayesian tree, then reconstruct the Bayesian branch tree into a local factor graph, add the newly constructed factor of the key frame to the local factor graph, and then perform variable elimination on the local factor variables to generate a new local Bayesian tree. Replace the Bayesian branch tree associated with the key frame in the global Bayesian tree with the local Bayesian tree, and then execute Step 8;

[0013] Step 8: Judge whether the key frame is a loop closure frame according to the visual bag-of-words method and geometric appearance. Otherwise, return to Step 1. If it is, construct a loop closure constraint factor, take out the Bayesian branch tree associated with the loop closure frame from the global Bayesian tree, then reconstruct the Bayesian branch tree into a local factor graph, add the loop closure constraint factor to the local factor graph, and then perform variable elimination on the local factor graph variables, and then generate a loop closure Bayesian tree. Add the loop closure Bayesian tree to the global Bayesian tree, and then execute Step 9;

[0014] Step 9: Determine whether it is the first global optimization. If so, construct an optimization objective function, use a non-linear optimization algorithm to iteratively solve it, then update all state variables, and then return to Step 1. Otherwise, propagate the updated solution from the root node to the leaf nodes, check the deviation values of the variables in each clique, re-linearize the variables with deviation values greater than the deviation threshold until the deviation value is less than the threshold, and then return to Step 1.

[0015] Further, in the above Step 2, the construction of the IMU pre-integration constraint residual e B (x i ,x j ) is expressed as:

[0016]

[0017] where r p , r q , r v , are the position residual, attitude residual, velocity residual, accelerometer residual, and gyroscope residual of the robot from time t i to time t j respectively. The times t i and t j are the times of the k-th and (k + 1)-th frames of the key frames respectively.

[0018] Further, in the above Step 3, the construction of the LiDAR odometry constraint residual e L is expressed as:

[0019]

[0020] where T k is the relative transformation relationship between the scanned point cloud of the k-th key frame and the local map, and T k+1 is the relative transformation relationship between the scanned point cloud of the k-th key frame and the local map.

[0021] Further, in the above Step 4, the construction of the visual reprojection constraint residual e C (x i ,x j ) is expressed as:

[0022]

[0023] where is the projected coordinate of the map point P in space on the image normalization plane of the j-th key frame, and is the observation coordinate when the map point P in space is first observed in the image of the j-th key frame.

[0024] The beneficial effects of the invention are as follows:

[0025] The present invention improves the fusion optimization method for the backend of a multi-sensor SLAM system, and proposes a plug-and-play factor graph fusion method for a multi-sensor SLAM system based on incremental smoothing.

[0026] This method sets the data reliability judgment conditions for two types of sensors, namely cameras and LiDARs, filters out unreliable sensor data in a targeted manner, and screens out reliable sensor information, ensuring the reliability of the data source for information fusion, thereby improving the sensor fault tolerance and robustness of the visual inertial laser SLAM system.

[0027] Furthermore, according to the sensor data reliability conditions, the present invention selectively constructs visual reprojection constraint factors and LiDAR odometry constraint factors, constructs a variable-structure, plug-and-play factor graph, and performs state estimation only using the tight coupling of reliable information of the sensors, ensuring the positioning accuracy of the SLAM system.

[0028] Furthermore, the present invention optimizes and solves the factor graph model by means of incremental smoothing, transforms the factor graph into the structure of a Bayesian tree, and adds the newly added observation information of the SLAM system by updating the Bayesian tree. Each time the factor graph is expanded, only the branch tree related to the newly added observation information needs to be modified, and the other parts remain unchanged, thereby improving the efficiency and response speed of the global optimization of the SLAM system. Brief Description of the Drawings

[0029] Figure 1 It is a flowchart of the plug-and-play factor graph fusion method for a multi-sensor SLAM system based on incremental smoothing described in the specific implementation manner.

[0030] Figure 2 (a) is a schematic diagram of the principle of LiDAR point cloud distortion caused by the translational motion of the carrier.

[0031] Figure 2 (b) is a schematic diagram of the principle of LiDAR point cloud distortion caused by the rotational motion of the carrier.

[0032] Figure 3 It is a schematic diagram of the principle of LiDAR point cloud voxel filtering.

[0033] Figure 4 (a) is a schematic diagram of extracting the line features of the LiDAR point cloud.

[0034] Figure 4 (b) is a schematic diagram of extracting the surface features of the LiDAR point cloud.

[0035] Figure 5 It is an example schematic diagram of using a quadtree to achieve uniform distribution of image feature points.

[0036] Figure 6 A typical plug-and-play factor graph structure model instance constructed for the present invention.

[0037] Figure 7 An example of generating a Bayesian network by variable elimination of a factor graph.

[0038] Figure 8 An example of generating a Bayesian tree from a Bayesian network

[0039] Figure 9 An example of updating a Bayesian tree.

[0040] Figure 10 The carrier motion trajectory map estimated by the SLAM system implemented based on the present invention on the garden dataset.

[0041] Figure 11 The environmental map constructed by the SLAM system implemented based on the present invention on the garden dataset. Detailed implementation manners

[0042] Next, the technical solutions in the embodiments of the present invention will be clearly and completely described in conjunction with the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are only a part of the embodiments of the present invention, rather than all the embodiments. All other embodiments obtained by those of ordinary skill in the art based on the embodiments of the present invention without creative efforts shall fall within the protection scope of the present invention. It should be noted that, without conflict, the embodiments in the present invention and the features in the embodiments may be combined with each other.

[0043] Refer to Figures 1 to 11 For a specific description of this implementation manner, the plug-and-play factor graph fusion method for the multi-sensor SLAM system based on incremental smoothing described in this implementation manner, in combination with Figure 1 , the implementation process of the present invention includes the following steps:

[0044] Step 1: The backend thread of the visual inertial laser SLAM system queries the key frame queue and determines whether the key frame queue is empty. If it is empty, step 1 is repeatedly executed until the backend thread is terminated; if it is not empty, step 2 is executed.

[0045] Step 2: Take out a key frame from the head of the key frame queue, and construct an IMU pre-integration constraint factor according to the IMU measurement information of the key frame.

[0046] The IMU pre-integration constraint factor is used to describe the relative pose change relationship of the IMU measurement information between the time differences of two key frames. Align the i-th and j-th moments of the IMU sampling with the k-th and (k + 1)-th frames of the key frame, then the variables to be optimized of the IMU pre-integration factor are expressed as:

[0047]

[0048] Among them, and are the velocities of the IMU at times t i and t j respectively. and are the accelerometer biases of the IMU at times t i and t j respectively. and are the gyro biases of the IMU at times t i and t j respectively. The times t i and t j correspond to the k-th frame and the (k + 1)-th frame of the key frame respectively.

[0049] The position j , velocity , and attitude of the IMU at time t

[0050]

[0051] are obtained by the following formula: i where Δt is the time interval between times t j and t i , t j , is the rotation matrix from the IMU coordinate system to the world coordinate system, g w is the gravity vector in the world coordinate system, δt is the time interval between two adjacent IMU measurement data, is the relative rotation amount of the carrier at time t in the IMU coordinate system with respect to time t i . and are the accelerometer bias and gyro bias of the IMU at time t respectively. and represent the acceleration and angular velocity output by the IMU at time t respectively. and represent the acceleration noise and angular velocity noise of the IMU measurement information at time t respectively, and it is assumed that they are both white noises obeying Gaussian distribution.

[0052] Transform the above formula to change the reference coordinate system of the position quantity, velocity quantity and rotation quantity from the world coordinate system to the body coordinate system at time t i , and we can get:

[0053]

[0054] In the above formula are the IMU pre-integration variables of position, velocity, and attitude, and the expressions are as follows:

[0055]

[0056] represents the IMU pre-integration variable of the robot's attitude at time t relative to time t i time.

[0057] It can be seen that the state quantity at the j-th frame time is only related to the measurement value of the IMU and will not be affected by the state quantity at the i-th frame time, thus ensuring that the measurement value is independent of the state quantity and avoiding repeated integration. Then there is the IMU pre-integration constraint residual e B (x i , x j ) and the expression is as follows:

[0058]

[0059] Among them, r p , r q , r v , are respectively the position residual, attitude residual, velocity residual, accelerometer residual, and gyroscope residual of the robot from time t i to time t j . The time t i and time t j are respectively the times of the k-th and k + 1-th frames of the key frames, is the quaternion multiplication symbol, [·] xyz represents taking the imaginary part of the quaternion to form a three-dimensional vector, is the rotation amount from the world coordinate system to the IMU coordinate system at time t i , is the rotation amount from the IMU coordinate system at time t i to the IMU coordinate system at time t j .

[0060] Step 3: According to the IMU measurement data, correct the motion distortion of the LiDAR point cloud data of the key frame, then filter out the outlier points and noise points in the point cloud, and then obtain a sparse point cloud through voxel filtering. Determine whether the LiDAR data of the key frame is available according to the density and uniformity of the sparse point cloud.

[0061] Due to the movement of the carrier, relative movement is generated by the lidar within a scanning period, causing the coordinate system at the start time and the end time of the scanning frame to change, resulting in distortion of the obtained point cloud data, such asFigure 2 As shown in the figure, a schematic diagram of the principle of LiDAR point cloud distortion caused by the translational and rotational motions of the carrier is given.

[0062] Use the IMU measurement data for motion compensation to correct the distorted LiDAR point cloud data. Transform all the point clouds of the LiDAR scan frame to the coordinate system at the starting time of this scan frame. Let the starting time of the scan frame be t m , and the ending time of the scan frame be t n . The rotational increment ΔR m of the carrier and the displacement increment ΔP n between the time t mn and t mn can be obtained by IMU integration. Use the interpolation method to correct the motion distortion of each point in the scan frame. Let the coordinate of any point in this scan frame be p a , and its corresponding time be t a . Let the coordinate of the point p a after motion distortion correction be expressed as Then the calculation expression is as follows:

[0063]

[0064] In the above formula, ΔT mn is the pose transformation matrix from the time t m to the time t n , and ΔT ma is the pose transformation matrix during the period from the time t m to the time t a .

[0065] Set the threshold of the effective ranging range of the point cloud as (x max , y max , z max ), and use the method of conditional filtering to filter out the point cloud outside the effective ranging range of LiDAR. After the motion distortion correction of the point cloud, for any point If it meets the following conditions, it will be filtered out.

[0066] or or

[0067] Due to the influence of weather and working environment, there are some outlier points and noise points in the LiDAR point cloud, which will interfere with the feature extraction and matching of the point cloud. Further, use the method of statistical filtering to filter out the outlier points and noise points in the point cloud. Let any point in the point cloud calculate the average Euclidean distance d a between this point and m nearest neighbor points in its neighborhood. The expression is as follows:

[0068]

[0069] Assume that the average Euclidean distance of all point clouds follows a Gaussian distribution. Let μ be the mean of the average Euclidean distance and σ be the standard deviation of the average Euclidean distance. Set an effective interval defined by the mean and the standard deviation. If the average distance of a certain point exceeds the interval range, then it is regarded as an outlier and filtered out. The expression of the interval range is as follows:

[0070] μ - σλ ≤ d a ≤ μ + σλ,

[0071] In the above formula, λ is the coefficient of the standard deviation.

[0072] Furthermore, use the voxel filtering method to downsample the point cloud. Let the maximum and minimum values of the point cloud of a scanning frame in three dimensions be x max 、x min 、y max 、y min 、z max 、z min , and let the scale size of the voxel grid be r x 、r y 、r z . Then the total number N of the voxel grid of a scanning frame can be expressed as:

[0073]

[0074] On the premise of ensuring not to lose the geometric features of the point cloud, reduce the number of the point cloud to improve the matching efficiency and accuracy of the point cloud, Figure 3 and a schematic diagram of the voxel filtering principle is given. To ensure the accuracy of the voxel filtering, set the threshold of the number of laser points contained in each voxel grid at least. In the present invention, this threshold is set to 5. If the number of laser points in the voxel grid is less than the threshold, then discard the laser points in the voxel grid and empty the voxel grid; if the number of laser points in the voxel grid is greater than or equal to the threshold, then calculate the centroid of the laser points in the voxel grid to replace all the laser points in the voxel grid. Let the index of a certain voxel grid be i, j, k, then the centroid g ijk of this voxel grid is calculated by the following expression:

[0075]

[0076] In the above formula, N represents the number of all laser points in this voxel grid, represents the coordinates of the laser points in this voxel grid.

[0077] After the voxel filtering is completed, let the number of non-empty voxel grids be N g , if If the LiDAR point cloud information of this key frame is considered too little, the LiDAR data of this key frame is regarded as unreliable information, the point cloud information is discarded, and then step 4 is executed; if then the LiDAR odometer constraint factor of this key frame is further constructed.

[0078] The smoothness of the point cloud data is represented by the curvature value. The \(i\)-th laser point in the LiDAR point cloud data of the \(k\)-th key frame is expressed as \(L\) is the LiDAR sensor coordinate system, and the curvature value \(c\) of this point i is calculated by the following expression:

[0079]

[0080] In the above formula, \(S\) is a set composed of 5 points before and after the \(i\)-th laser point and satisfies being on the same laser line; is the point in the set \(S\), that is Set the class threshold of the point cloud, compare the curvature value with the threshold. Points greater than the threshold are edge points, and points less than the threshold are plane points. All edge points form a line feature set All plane points form a plane feature set Use the motion transformation matrix obtained by the IMU as the initial value, and transform and to the world coordinate system, which are respectively expressed as and

[0081] According to the feature matching relationship between the current key frame and the local map, construct the LiDAR odometer constraint factor. Assume that the current key frame is the \((k + 1)\)-th frame, select the point cloud data of the first 10 key frames of the current key frame, and transform them to the world coordinate system to form the local map \(M\) k , and the local map includes the local map of edge points and the local map of plane points The expression is as follows:

[0082]

[0083] In the world coordinate system, the set of edge points and the set of plane points corresponding to the current key frame are respectively expressed as and Now match the features of the current key frame with the local map \(M\) k , and let the distance from the edge point to the line feature be expressed as \(d\) e , and the distance from the plane point to the plane feature be expressed as \(d\) p , and the calculation expressions are as follows:

[0084]

[0085] In the above formula, represents the line feature distance from the i-th edge point in the point cloud of the (k + 1)-th key frame to the edge point local map ; represents the plane feature distance from the i-th plane point in the point cloud of the (k + 1)-th key frame to the plane point local map ; Figure 4 Figure gives a schematic diagram of extracting line features and plane features from the LiDAR point cloud. Combining Figure 4 to explain the above expressions, is the set of edge points of the (k + 1)-th key frame in which the points satisfy is the set of plane points of the (k + 1)-th key frame in which the points satisfy where i is the index number of the point. is the nearest neighbor point in the local map ; is the nearest neighbor point in the local map satisfying and and the two points come from different laser lines, forming a line feature, where j and l are index numbers. is the nearest neighbor point in the local map ; and are the two nearest neighbor points in the local map and satisfy that only two of the three points come from the same laser line, ensuring that the three points form a plane feature, where j, l, and m are index numbers.

[0086] Minimize the sum of the distances d of all edge points to the line feature e and the distances d of all plane points to the plane feature p to obtain the transformation matrix T k+1 between the key frame and the local map, and the expression is as follows:

[0087]

[0088] In the above formula, m represents the total number of edge points in the set of edge points of the (k + 1)-th key frame; n represents the total number of plane points in the set of plane points of the (k + 1)-th key frame. Further, construct the LiDAR odometry residual e L between two adjacent key frames, and the expression is as follows:

[0089]

[0090] Step 4: Perform data processing on the image information of the key frame, extract key points, calculate descriptors, use the quadtree method to achieve uniform distribution of image feature points, and determine whether the image information of the key frame is available according to the number and distribution of feature points.

[0091] Use the image pyramid to extract ORB image feature points from the image information of the key frame. Set the image pyramid to have 8 layers, extract the FAST key points of the image, use the gray centroid method to add the main direction to the key points, and calculate the steered BRIEF descriptor vector of the key points. Assume that the total number of feature points to be extracted for each image is N. Then, for the i-th layer of the image pyramid, the number of feature points N i The expression is:

[0092]

[0093] In the above formula, s is the scaling factor of the image pyramid, which is set to s = 0.8 in the present invention.

[0094] Since the over-concentration of the distribution of image feature points will affect the accuracy of pose estimation, use the quadtree to achieve uniform distribution of image feature points. Set the original image as the root node, and each node is split into 4 sub-nodes each time. Sort all nodes according to the number of feature points they contain, and give priority to splitting the nodes with more feature points. This can make the areas with dense feature points more subdivided. At the same time, for the nodes with fewer feature points, it may no longer split because the number of feature points extracted meets the requirement N in advance. Finally, select the feature point with the highest response value from each node as the only feature point of the node. Figure 5 An example of using the quadtree to achieve uniform distribution of image feature points is given. Set the minimum threshold for the number of feature points extracted for each image. After achieving uniform distribution of image feature points using the quadtree, if the number of feature points is less than the threshold, it is considered that the image information is too sparse and unreliable, and the image information is discarded, and then step 5 is executed; if the number of feature points is not less than the threshold, it is considered that the image information is sufficient, and a visual constraint factor is further constructed. The threshold is set to 25 in the present invention.

[0095] The visual constraint factor is constructed based on the reprojection error. The map points in space are projected onto each key frame that can observe the map point, and the coordinates of the projected points should coincide with the observed coordinates of the map point. The reprojection error between the two types of coordinates is the residual of the visual constraint factor. In the visual inertial laser SLAM system, the state quantity χ to be optimized of the visual factor c is expressed as:

[0096]

[0097] where and are the positions of the camera at the i-th frame and the j-th frame respectively, and are the poses of the camera at the i-th frame and the j-th frame respectively. The i-th frame and the j-th frame are two adjacent key frames, and are the position and rotation amount of the camera relative to the IMU respectively. λ is the inverse depth of the map point.

[0098] Assume that there is a map point P in space, its index number is l, and its projection coordinates on the normalized plane of the j-th frame are The observed coordinates in the j-th frame are Then there is a visual constraint residual e C (x i ,x j ) The expression is as follows:

[0099]

[0100] Among them, can be obtained from the reprojection estimation model, and the expression is:

[0101]

[0102] In the above formula, represents the observed value on the normalized plane of the i-th frame when the map point P is first observed by the i-th frame, is the rotation amount from the IMU coordinate system to the camera coordinate system, is the rotation amount from the world coordinate system to the IMU coordinate system of the j-th frame, is the rotation amount from the IMU coordinate system of the i-th frame to the world coordinate system, is the rotation amount from the camera coordinate system to the IMU coordinate system.

[0103] Steps 2, 3, and 4 complete the construction of the relevant factors of the key frame. Among them, the IMU is a sensor for measuring the state of the carrier itself, and the camera and LiDAR are sensors for measuring the environmental information around the carrier. Due to the limitations of the application environment of the camera and LiDAR sensors, there may be situations where sensor data is unavailable in certain environments. Therefore, in practical applications, it is inevitable to construct IMU pre-integration constraint factors, and for visual reprojection constraint factors and LiDAR odometry factors, they are selectively constructed according to the judgment conditions.

[0104] Step 5: Judge whether the key frame index is greater than 10. Otherwise, return to Step 1. If it is, then continue to judge whether the visual inertial laser SLAM system has created a global Bayesian tree. If it has not been created, execute Step 6. If it has been created, execute Step 7;

[0105] In this step, the purpose of determining whether the index of the key frame is greater than 10 is that there needs to be a sufficient number of factors for constructing the factor graph, and 10 is the threshold of the key frame number set in the present invention.

[0106] Step 6: Construct a global factor graph model, perform variable elimination on the global factor graph variables to generate a Bayesian network, then transform it into a Bayesian tree structure, and then return to Step 1.

[0107] Combine all the constructed factors to build a global factor graph. Figure 6 A typical factor graph structure model example constructed by the present invention is given, as Figure 6 shown. If all three sensors are available, a factor graph with visual-IMU-LiDAR joint constraints is constructed; if the lidar is not available, a visual-IMU joint constraint factor graph is constructed; if the camera is not available, a LiDAR-IMU joint constraint factor graph is constructed; if both the camera and the lidar are not available, only an IMU pre-integration constraint factor graph is constructed. Since each factor is selectively constructed according to the data quality of the sensor, it is a plug-and-play combined factor graph model.

[0108] Two types of nodes are defined in the framework of the factor graph. The variable nodes are used to represent unknown variables, and the function nodes are used to represent local functions. The factor graph is converted into a Bayesian network by eliminating the variables of the function nodes in the factor graph. Figure 7 An example of generating a Bayesian tree from variable elimination of the factor graph is given, and the Figure 7 process of variable elimination of the factor graph is discussed. Figure 7 In the factor graph shown in (a), a total of 6 function factors are constructed: f1(x1), f2(x1,x2), f3(x1,x3), f4(x2,x3), f5(x3,x5), f6(x4,x5). According to the elimination order of variable nodes x2→x4→x1→x3→x5, one variable is eliminated in each step, and all the function nodes associated with this variable are combined into a joint distribution. Figure 7 As shown in (b), the elimination process of the first variable x2 is given. First, the factors f2(x1,x2) and f4(x2,x3) associated with x2 are eliminated, and S = {x1,x3} is defined, representing the variables associated with it except x2; secondly, a factor f joint (x1,x2,x3) of the joint distribution is constructed according to these two factors; then the joint factor is decomposed into two parts. The first part is converted into the conditional probability distribution of the variable eliminated in this step, that is, p(x2|x1,x3), corresponding to Figure 7 the two dashed arrows in (b), and the second part generates a new factor f new (x1,x3) based on S, corresponding to Figure 7The square between x2 and x3 in (b). From Figure 7 (b) to Figure 7 (e), in each step of eliminating variables, a legend containing both a factor graph and a Bayesian network is shown. When all variables are eliminated, the factor graph is transformed into a Bayesian network, as shown in Figure 7 (f).

[0109] Furthermore, the generated Bayesian network is transformed into a Bayesian tree structure. A Bayesian tree is a directed tree, and its nodes correspond to the cliques C k in the Bayesian network. For each node, a conditional probability density p(F k |S k ) is defined, where the separator S k is the intersection C k of the clique C k and its parent clique Π k ; the frontier variables F k are the set of remaining variables in C k except for S k , denoted as C k = F k :S k . In the Bayesian tree, the separator S k of a clique C k is always a subset of its parent clique Π k . For the variables X on the Bayesian tree, the expression of its joint probability density p(X) is as follows: k

[0110]

[0111] Figure 8 gives an example of the transformation from a Bayesian network to a Bayesian tree, and the transformation principle from a Bayesian network to a Bayesian tree is discussed in combination with Figure 8 . Figure 8 (a) is the Bayesian network structure to be transformed, x1, x2, and x3 are state variables, and l1 and l2 are the position quantities of the observed points. Transform the Bayesian network in Figure 8 (a) into the Bayesian tree structure form in Figure 8 (b). As shown in Figure 8 (b), the root clique is C1 = {x2, x3}, which contains the variables x2 and x3. It intersects with the other two cliques, namely clique C2 = {l1, x1, x2} = {l1, x1}:{x2} and clique C3 = {l2, x3} = {l2}:{x3}. Figure 8 ​(c) is in the form of the square root information matrix corresponding to the Bayesian tree. It can be seen that each row of the square root information matrix maps to different graphical trees on the Bayesian tree. The Bayesian tree and the square root information matrix jointly describe the clique structure in the Bayesian network. The Bayesian tree can better express the equivalence relationship between solving sparse linear systems and graphical model inference.

[0112] To maintain the very good property of the sparsity of the information matrix for the benefit of solving the linear system, in the update of the Bayesian tree, only the front-end variables in the affected part of the Bayesian tree will be updated. Therefore, to ensure that the affected part in the Bayesian tree is as small as possible each time, the recently visited variables are forced to be placed at the end of the root clique, that is, the variable addition order strategy of the CCOLAMD algorithm is adopted. Through the sorting optimization of the variable order, a relatively sparse information matrix is maintained.

[0113] Step 7: Update the global Bayesian tree.

[0114] When a new observation is added to the visual inertial laser SLAM system, a factor is added corresponding to the key frame. The Bayesian branch tree associated with the key frame is taken out from the global Bayesian tree, and then the Bayesian branch tree is reconstructed into a local factor graph. The newly constructed factor related to the key frame is added to the local factor graph, and then variable elimination is performed on the local factor graph to generate a new local Bayesian tree. Finally, the Bayesian branch tree associated with the key frame in the global Bayesian tree is replaced with the newly generated local Bayesian tree, and the part in the global Bayesian tree that is not related to the key frame remains unchanged, completing the update of the global Bayesian tree.

[0115] Updating the Bayesian tree only requires modifying the top of the tree structure because the Bayesian tree is generated from the Bayesian network in the reverse elimination order. The variables in each clique collect information by eliminating their sub-cliques, so the information in any clique propagates upward until the root node; in addition, a new factor cannot affect the subsequent branch trees of any variables that are not connected to the factor. Figure 9 An example of updating the Bayesian tree is given, combined with Figure 9 Discuss the process of Bayesian tree update. Figure 9 (a) shows the structure of a Bayesian tree. A new factor f(x1,x5) is added between x1 and x5, and this factor only affects the left branch of the Bayesian tree. In Figure 9 the Bayesian tree in (a), the affected branch tree is framed with a dashed oval. The affected branch tree is taken out, and a factor is generated for the probability density p(x3,x5) and p(x1,x2|x3) of each clique in the branch tree. The branch tree is reconstructed into a local factor graph, and the new factor f(x1,x5) is added, as Figure 9(as shown in (b)). For the new local factor graph, perform variable elimination, Figure 9 (c) shows the new local Bayesian network structure obtained by performing variable elimination in the order of x2→x1→x3→x5. Then transform it into a local Bayesian tree structure. Finally, replace the affected branch tree in the original Bayesian tree with the newly generated local Bayesian tree to complete the update of the Bayesian tree. The unaffected branch trees in the original Bayesian tree remain unchanged, as Figure 9 (d) shows.

[0116] Step 8: Determine whether the key frame is a loop frame according to the bag-of-words method of vision and geometric appearance. Otherwise, return to Step 1. If it is, construct a loop constraint factor, extract the Bayesian branch tree associated with the loop frame from the global Bayesian tree, then reconstruct the Bayesian branch tree into a local factor graph, add the loop constraint factor to the local factor graph, then perform variable elimination on the local factor graph, and then generate a loop Bayesian tree. Add the loop Bayesian tree to the global Bayesian tree, and then execute Step 9;

[0117] Whenever a loop frame is detected, perform a global optimization on the visual inertial laser SLAM system. The process of adding a loop factor and updating the Bayesian tree in Step 9 is the same as the process of adding a new key frame factor and updating the Bayesian tree in Step 7.

[0118] Step 9: Determine whether it is the first global optimization. If it is, construct an optimization objective function, use a nonlinear optimization algorithm to iteratively solve it, then update all state variables, and then return to Step 1; otherwise, propagate the updated solution from the root node to the leaf nodes, check the deviation values of the variables in each clique, re-linearize the variables with deviation values greater than the deviation threshold until the deviation value is less than the threshold and then stop propagation, and then return to Step 1.

[0119] If a loop frame is detected for the first time, that is, the first global optimization, then construct an optimization objective function. Based on the factor graph constructed from all key frame states, establish the objective function for global optimization, and the expression is as follows:

[0120]

[0121] Use a nonlinear optimization method to optimize the objective function. Taking the Levenberg-Marquardt algorithm as an example, by setting a confidence interval as the region of trust for quadratic polynomial approximation, it avoids the approximation failure caused by large changes in state increments and avoids the problems of non-singularity and ill-conditioning of the sparse matrix of the linear equation system. Therefore, it can provide more stable and accurate state increments. Transform the objective function into the form of a state increment equation:

[0122] (J T J + ξdiag(J T J))Δχ = JT b,

[0123] In the above formula, J is the Jacobian matrix, J T J is the Hessian matrix, b is the prediction error, ξ is a non - negative constant added to the diagonal of the matrix, which reflects whether the nonlinear quadratic approximation model is accurate and provides a faster convergence rate.

[0124] If it is not the first detection of the loop closure frame, that is, not the first global optimization, only the solution in the local Bayesian tree needs to be updated, and it is not necessary to recalculate the solutions of all variables. The back substitution on the Bayesian tree is processed from the root node to the leaf node. For each clique, check the deviation of the variable from the linearization point. If it is greater than the threshold, then re - linearization is required. The update of the solution propagates to the leaf node until the deviation value is less than a certain threshold, and the propagation process stops, completing the update of all affected variables in the Bayesian tree.

[0125] Implementation and verification of the present invention:

[0126] A plug - and - play factor graph fusion method for a multi - sensor SLAM system based on incremental smoothing proposed by the present invention is implemented by programming in C++. The beneficial effects of the present invention are verified by evaluating the accuracy of the motion trajectory of the mobile robot estimated by the SLAM system. A SLAM system based on incremental smoothing and plug - and - play factor graph fusion of the present invention is designed and implemented. The garden dataset is used as the verification dataset. This dataset rigidly connects the camera, IMU, and LiDAR together, and there is no relative motion between the three sensors. It is collected by holding a device combining the three sensors in a park environment.

[0127] For the park environment, it is an open, multi - tree, hilly road environment, and there are pedestrians walking in the park. It is an irregular environment, which poses a great challenge to the SLAM system for accurate local positioning and environmental mapping. Figure 10 The carrier motion trajectory map estimated by the SLAM system implemented by the present invention on the garden dataset is given. The entire trajectory is smooth and continuous. According to the application constraints of each sensor, the data reliability of each sensor is judged, and it is decided whether to use the data of this sensor in information fusion. The tight - coupling plug - and - play factor graph fusion of multi - source information can improve the positioning accuracy of the SLAM system. Figure 11 The environmental map constructed by the SLAM system implemented by the present invention on the garden dataset is given. Figure 11 and Figure 10 correspondingly, Figure 11The trees and grass in the park can be seen in it. The environmental map is relatively clear, the objects are clearly distinguishable, there are no virtual images, overlaps, etc., and there is no map drift phenomenon, verifying the effectiveness of the SLAM system based on multi-sensor fusion proposed by the present invention.

[0128] Although the present invention has been described herein with reference to specific embodiments, it should be understood that these embodiments are merely examples of the principles and applications of the present invention. Therefore, it should be understood that many modifications can be made to the exemplary embodiments, and other arrangements can be designed, as long as they do not deviate from the spirit and scope of the present invention as defined by the appended claims. It should be understood that the different dependent claims and the features described herein can be combined in a manner different from that described in the original claims. It should also be understood that the features described in connection with a single embodiment can be used in other described embodiments.

Claims

1. A plug-and-play factor graph fusion method for multi-sensor SLAM system based on incremental smoothing, characterized in that: The following steps are involved: Step 1: The backend thread of the visual inertial laser SLAM system queries the key frame queue to determine whether the key frame queue is empty. If so, step 1 is repeated until the backend thread is terminated, otherwise step 2 is executed; Step 2: Take a key frame from the head of the key frame queue, construct an IMU pre-integration constraint factor according to the IMU measurement information of the key frame, and then execute step 3; Step 3: According to the IMU measurement data, the LiDAR point cloud of the key frame is corrected for motion distortion, and then the outliers and noise points in the point cloud are filtered out, and then the sparse point cloud is obtained by voxel filtering. The density and uniformity of the sparse point cloud are used to determine whether the LiDAR point cloud of the key frame is available. If yes, the LiDAR odometer constraint factor is constructed, and then step 4 is executed. Otherwise, step 4 is directly executed; Step 4: Process the image information of the key frame, extract key points, calculate descriptors, use the quadtree method to achieve uniform distribution of image feature points, and determine whether the image information of the key frame is available based on the number and distribution of feature points. If yes, construct a visual reprojection constraint factor and then execute step 5. Otherwise, directly execute step 5. Step 5: Determine whether the index of the key frame is greater than 10, otherwise return to step 1, if yes, continue to determine whether the visual inertial laser SLAM system has created a global Bayesian tree, if not, execute step 6, if it has been created, execute step 7; Step 6: construct a global factor graph model, eliminate the variables of the global factor graph, generate a Bayesian network, convert it into a Bayesian tree structure, and then return to step 1; Step 7: Take out the Bayesian branch tree associated with the key frame from the global Bayesian tree, then reconstruct the Bayesian branch tree into a local factor graph, add the newly constructed factors of the key frame to the local factor graph, then eliminate the variables of the local factor graph to generate a new local Bayesian tree, replace the Bayesian branch tree associated with the key frame in the global Bayesian tree with the local Bayesian tree, and then execute step 8; Step 8: Determine whether the key frame is a loop frame according to the visual word bag method and the geometric appearance, otherwise return to step 1, if yes, construct a loop constraint factor, take out the Bayesian branch tree associated with the loop frame from the global Bayesian tree, then reconstruct the Bayesian branch tree into a local factor graph, add the loop constraint factor to the local factor graph, then eliminate the variables of the local factor graph, regenerate a loop Bayesian tree, add the loop Bayesian tree to the global Bayesian tree, and then execute step 9; Step 9: Determine whether it is the first global optimization. If so, construct the optimization objective function, use the nonlinear optimization algorithm to iteratively solve it, then update all state variables, and then return to step 1; otherwise, propagate the updated solution from the root node to the leaf nodes, check the deviation value of the variables in each group, re-linearize the variables that are greater than the deviation threshold, stop propagating when the deviation value is less than the threshold, and then return to step 1.

2. The multi-sensor SLAM system plug-and-play factor graph fusion method based on incremental smoothing according to claim 1, characterized in that: The IMU pre-integration constraint factor constructed in step 2 is represented by the IMU pre-integration constraint residual. Let the IMU pre-integration constraint residual be e B (x i ,x j ), whose expression is defined as follows: Among them, r p 、r q 、r v , t i Time to t j The position residual, attitude residual, velocity residual, accelerometer residual and gyroscope residual of the robot at the moment t i Time and t j The time points are the kth frame and the k+1th frame of the key frame respectively.

3. The multi-sensor SLAM system plug-and-play factor graph fusion method based on incremental smoothing according to claim 1, characterized in that: The LiDAR odometer constraint factor constructed in step 3 is represented by the LiDAR odometer constraint residual. Let the LiDAR odometer residual be e L , whose expression is defined as follows: Among them, T k is the relative transformation relationship between the scanned point cloud of the kth key frame and the local map, T k+1 is the relative transformation relationship between the scan point cloud of the k+1th key frame and the local map.

4. The multi-sensor SLAM system plug-and-play factor graph fusion method based on incremental smoothing according to claim 1, characterized in that: The visual reprojection constraint factor constructed in step 4 is represented by the visual reprojection constraint residual. Let the visual reprojection constraint residual be e C (x i ,x j ), whose expression is defined as follows: in, is the projection coordinate of the map point P in the space on the normalized plane of the image of the jth key frame, It is the observed coordinates of the map point P in the space when it is first observed in the image of the j-th key frame.

Images