Backend optimization method based on multi-factor graph for accurate robot pose acquisition

Abstract

The present invention relates to a back-end optimization method based on a multi-factor graph for obtaining precise posture of a robot, and an improved multi-factor graph model is constructed. First, the IMU data is pre-integrated to obtain the observation model error of the IMU sensor and the IMU factor is constructed. At the same time, the observation model error of the laser radar sensor is calculated using the posture change relationship output by the front-end laser odometer, and the laser odometer factor is constructed. When the loop is successfully detected, a closed-loop factor is constructed, and then the three constraint factors are connected with the associated state variables by edges and added to the factor graph basic model. The method improves the accuracy and efficiency of loop detection, calculates the posture transformation between the two frames of point clouds that form the loop after the loop is successfully detected, and constructs a closed-loop factor to be added to the multi-factor graph model for optimization, thereby improving the accuracy of posture estimation in the back-end optimization.

Description

Technical Field

[0001] The present invention relates to the field of laser SLAM technology, and in particular to an optimization method based on a multi-factor graph for obtaining precise posture of a robot. Background Art

[0002] After the traditional SLAM method obtains the robot's laser odometer and position, the positioning and map results obtained will still have large errors. Especially when the robot has a long movement distance, even if the laser odometer calculation method has a high accuracy, it will be affected by the cumulative error, resulting in a decrease in positioning and mapping accuracy. Therefore, the SLAM method needs to further optimize the results at the back end to obtain more accurate position and position positioning results for the robot.

[0003] In SLAM, back-end optimization can not only filter and correct sensor data, reduce noise and errors, and improve the stability and robustness of the system, but also fuse and correct map information at different times and locations, thereby optimizing the coherence and consistency of the map. At present, the back-end optimization algorithms mainly include filter-based methods and nonlinear (graph optimization)-based methods. Although the filter algorithm has a fast calculation speed, the estimation accuracy is relatively low and is only suitable for small-scale, linear or approximately linear systems. The most representative nonlinear optimization algorithm is the factor graph-based optimization algorithm.

[0004] However, the traditional factor graph-based backend optimization method still has the following problems:

[0005] (1) When a single sensor is used for positioning and mapping, it will face the disadvantage of feature degradation due to scene changes. To solve this problem, multiple sensors can be integrated, such as using IMU, GPS and other data for back-end optimization to achieve more accurate positioning and mapping. The loose coupling solution of LiDAR and IMU has high computational efficiency, but the cumulative error of pose estimation is large.

[0006] (2) Loop detection is an important part of backend optimization. Existing work on loop detection usually uses local or global geometric features of point clouds for encoding, generates local or global descriptors of point clouds, and matches and recognizes point cloud descriptors. Local descriptors are easily affected by noise, and re-identification requires a lot of geometric calculations. Global descriptors have better robustness because they can effectively capture the overall morphological information of objects. However, improper feature encoding methods will seriously affect the computational efficiency of position recognition, and thus affect the performance of backend optimization. Summary of the invention

[0007] In view of the above problems existing in the prior art, the technical problem to be solved by the present invention is: how to obtain the precise position and posture of the robot.

[0008] In order to solve the above technical problems, the present invention adopts the following technical solution: a back-end optimization method based on a multi-factor graph for obtaining precise posture of a robot, comprising the following steps:

[0009] S100: Construct an improved multi-factor graphical model F(X):

[0010]

[0011] Where f (i,j) (X i ,X j ) represents the relationship factor between the i-th frame pose node and the j-th frame pose node, and the solid line box C i , dotted box F i , the black nodes are divided into IMU factors f IMU , laser odometry factor f Lidar and the loop closure factor f Loop , f IMU =f IMU (X i ,X i+1 ), f Lidar =f Lidar (X i ,X i+1 ), f Loop =f Loop (X c ,X i+1 ), E represents all the edges associated with the nodes in F(X).

[0012] The objective function of backend optimization is converted to solving the least squares problem, which is expressed as:

[0013]

[0014] In the formula and They represent the errors of the IMU factor, laser odometer factor and closed-loop factor observation model respectively, and are the corresponding measurement noise covariances respectively. Among them, the error term can be uniformly expressed as:

[0015]

[0016] Where T i With T j are the positions of the robot in the world coordinate system for the i-th and j-th key frames, △T i,j represents the relative position transformation relationship between two variable nodes calculated by three factors, △T i,j include and

[0017] S200: Output the above content to IMU and f Lidar Incrementally add to F(X). When the mobile robot visits the same place during movement, that is, after the loop detection is successful, add f to F(X). Loop , then the relevant variable nodes perform pose optimization according to the update of the factor nodes, and based on the optimized state set X * , the projection map M established by the front-end odometer k The corresponding point cloud in the image is reprojected to the world coordinate system to update the precise position and posture of the robot.

[0018] Preferably, the step S100 obtains f IMU The process is as follows:

[0019] First, assume that the original angular velocity and acceleration output by the IMU at time t are and It is shown that from the IMU measurement model:

[0020]

[0021]

[0022] In the formula Represents the rotation matrix from the map coordinate system W to the sensor coordinate system B, g is the gravitational acceleration in W, ω t and a t They represent the actual angular velocity and acceleration of the lidar at time t respectively;

[0023] and They represent the IMU zero bias of the original angular velocity and the IMU zero bias of the acceleration respectively. and Collectively referred to as IMU zero bias b t , and Respectively represent the white noise of the original angular velocity and the white noise of acceleration, and Collectively referred to as n t ;

[0024] At time t+△t, the robot's velocity v t+Δt , the robot's position p t+Δt and the rotation transformation R t+Δt They are calculated by the following formulas:

[0025]

[0026]

[0027]

[0028] Among them, R t , p t , v t They represent the rotation transformation, position and speed of the lidar at time t respectively, and △t represents a short time.

[0029] The second step is to calculate the relative motion change between the robot i and j, the speed change during this period △v ij 、Position transformation △p ij and the rotation transformation △R ij They are calculated by the following formulas:

[0030]

[0031]

[0032]

[0033] Among them, v i , p i , R i They represent the velocity, position and rotation transformation of the laser radar at time i, respectively, and v j , p j , R j They represent the velocity, position and rotation transformation of the laser radar at time j, △t ij Represents the time difference between time i and time j.

[0034] Then, the open source GTSAM optimization library is used to optimize the IMU’s velocity change △v ij 、Position transformation △p ij and the rotation transformation △R ij Before the next pre-integration result is input, the GTSAM optimizer will output the updated position transformation, velocity transformation, rotation transformation and zero bias matrix b i ′.

[0035] Using the updated zero bias b i ′, recalculate the pre-integral by the following formula to obtain the speed of the optimized IMU at time β Location and rotation transformation

[0036]

[0037]

[0038]

[0039] Among them, b i ' a and b i ' ω They represent the IMU zero bias of the updated original angular velocity and the IMU zero bias of the updated acceleration respectively. and Represents the white noise of the original angular velocity at the i-th moment and the white noise of the acceleration at the i-th moment.

[0040] The relative posture change between two determined moments is obtained by three-stage pre-integration calculation, and then The calculation formula is as follows:

[0041]

[0042]

[0043]

[0044]

[0045] They respectively represent the speed change, position change, and rotation change between the updated moments i and j.

[0046] f IMU Represents the observation model error of the IMU sensor between frame i and frame i+1 The pre-integrated Substituting into formula (41), we can get the observation model error of the IMU sensor between frame i and frame j: In the same way, we can get

[0047] Preferably, the step S100 obtains f Lidar The process is as follows:

[0048] The robot laser odometer outputs the pose transformation relationship △T between two adjacent key frames i and i+1 i,i+1 , △T i,i+1 Used to calculate the error term in the laser odometer factor, expressed as:

[0049]

[0050] f Lidar Represents the observation model error of the lidar sensor between frame i and frame i+1 △T i,i+1 Substituting into formula (41) we can get

[0051] Preferably, the step S100 obtains f Loop The process is as follows:

[0052] The traditional iterative closest point feature matching method is used to obtain the accurate pose transformation △T between the two point sets. Let the current point cloud pose be T i+1 , the pose of the loop frame is T c , then the relative posture change between the two is obtained, that is, the closed-loop factor is expressed as:

[0053]

[0054] in,

[0055] f Loop Represents the observation model error between the current frame i+1 and the loop frame c in the loop detection Will Substituting into formula (41) we can get

[0056] Preferably, the process of loop detection in S200 is as follows:

[0057] S201: The point cloud obtained by the laser radar to obtain environmental information includes distance information and intensity information. After normalizing and calibrating the intensity information through equation (59), η is obtained. k ′, assuming that each point cloud in the kth frame is represented as [x k ,y k ,z k ,η k ′], projecting the 3D point cloud in the Cartesian coordinate system to the polar coordinate system composed of the XY plane. The polar coordinates of the transformed point cloud are It is expressed as:

[0058]

[0059]

[0060]

[0061]

[0062] S202: Then, with the sensor as the center, the point cloud is divided into N equal parts by annular angle and radial direction. s ×N r sub-regions, each segment is represented as:

[0063]

[0064] where a∈[1,N s],b∈[1,N r ], L max It means that the laser radar scans the maximum sensing range in sequence. Because the intensity value returned by the same object is the same, for each small area S ab The point cloud in the sub-region is replaced by the maximum intensity information in the sub-region. ab :

[0065]

[0066] S203: Extract each sub-region S ab After the intensity information of each sub-region S ab The point cloud in the vertical direction is divided into 8 units and marked as z from bottom to top. k , where k∈(1,2,...,8), points with the same z coordinate are in the same unit. If there is no laser point in a unit, z k is set to zero, otherwise, if there is at least one laser point in a cell, z k Set to 1. In this way, each sub-area S ab will get an 8-bit binary code, and then according to z k The binary value is converted to a decimal value, and the geometric information of each area is d ab It can be expressed as:

[0067]

[0068] Where z(·) represents the ab The function of converting the binary code of into a decimal value. Combining the geometric information in this formula with the intensity information in formula (64) gives the point cloud global descriptor Ω(a,b):

[0069]

[0070] S204: A two-step hierarchical fast detection method is used to calculate geometric similarity and intensity similarity respectively to accelerate matching, and then the validity of the matching is verified based on consistency.

[0071] S204-1: Geometric similarity calculation

[0072] The global descriptor of the current point cloud is represented as Ω q , the global descriptor of the point cloud to be matched is Ω c The geometric code values ​​at the same index of the two are and calculate and The cosine distance of and normalized processing, then get the geometric similarity function:

[0073]

[0074] Among them, N S Indicates the N mentioned above S sectors, d q , d c Respectively represent the current point cloud global descriptor Ω q And the global descriptor Ω of the point cloud to be matched c The corresponding geometric information in .

[0075] Assume that the global descriptor of a point cloud is Here n means that the global descriptor of the point cloud is translated by n units in the horizontal direction. * and the geometric similarity score after the move D(d q ,d c ) are defined as:

[0076]

[0077]

[0078] S204-2: Intensity Similarity Calculation

[0079] set up and are η q and η c Columns with the same index value, take and Calculate the intensity similarity score as follows:

[0080]

[0081] Among them, η q and η c Represent the intensity features between two point cloud descriptors, Represent the intensity similarity scores respectively.

[0082] By comparing η c and η q Translation * Intensity characteristics after the column The final calculation result of the intensity similarity is:

[0083]

[0084] n * Indicates a change in perspective.

[0085] S204-3: Consistency Verification

[0086] The average of the similarities between the point cloud descriptors of the past N frames of the current frame and the point cloud descriptors of the past N frames of the K candidate frames is used as the final similarity, which is used as the verification standard for consistency. The calculation formula is as follows:

[0087]

[0088] where Θ(p q ,p c ) represents the average similarity between the point cloud descriptor of the past N frames of the current frame and the point cloud descriptor of the past N frames of a frame in the K candidate frames, p q Indicates the current frame, p c represents a frame among K candidate frames, d q-i , d c-i , η q-i , η c-i They represent the geometric information and intensity information in the global descriptor of the point cloud of the current frame and the candidate frame respectively.

[0089] When the mobile robot visits in the opposite direction, that is, when the sensor observation angle changes, Ω q-i Change to Ω accordingly q+i . Use the obtained K objects to be matched to build a K nearest neighbor search tree, compare the descriptor of the target point cloud with that in the search tree, and use formula (72) to measure the similarity between them. Finally, set a threshold to select the most similar candidate point and use it as the revisited position, expressed as:

[0090]

[0091] Where C represents the index value of K objects to be matched in the search tree. is the threshold value set. * Represents the final closed-loop position index value, and obtains the pose T of the loop frame from the historical key frame c , and used it for subsequent construction of closed-loop factors.

[0092] Compared with the prior art, the present invention has at least the following advantages:

[0093] The present invention adds the IMU factor to the existing factor graph optimization model. In addition to the posture constraints constructed by the laser odometer at the front end of the SLAM framework, the IMU factor is constructed using the observation data of the inertial sensor to achieve tight coupling between the IMU and the lidar sensor. At the same time, the point cloud keyframe and sliding window strategy are introduced to meet the real-time performance of SLAM.

[0094] The method of the present invention integrates a new loop detection method based on point cloud global descriptor, which improves the accuracy and efficiency of loop detection. After the loop is successfully detected, the pose transformation between the two frames of point cloud forming the loop is calculated, and a closed-loop factor is constructed and added to the multi-factor graph model for optimization, thereby improving the accuracy of pose estimation in the back-end optimization of the SLAM method. BRIEF DESCRIPTION OF THE DRAWINGS

[0095] Figure 1 The figure is a flowchart of a laser odometer estimation method.

[0096] Figure 2 This is the framework diagram of the FCNN-based segmentation network model.

[0097] Figure 3 Framework diagram of the multi-target tracking method.

[0098] Figure 4 It is a framework diagram of the method of the present invention.

[0099] Figure 5 Optimizing models for improved multi-factor graphs.

[0100] Figure 6 This is the framework diagram of the loop detection method.

[0101] Figure 7 Schematic diagram of point cloud segmentation.

[0102] Figure 8 Schematic diagram of geometric information encoding.

[0103] Fig. 9 For the Bayesian network model

[0104] Fig.10 It is a factor graph model.

[0105] Fig.11 Comparison of global trajectories before and after the loop detection function is enabled, where (a) is the 02 sequence with the loop enabled, (b) is the 02 sequence with the loop closed, (c) is the 05 sequence with the loop enabled, and (d) is the 05 sequence with the loop closed. DETAILED DESCRIPTION

[0106] The present invention is described in further detail below.

[0107] A laser odometer estimation method in the present invention is as follows:

[0108] A laser odometer estimation method based on dynamic target tracking includes the following steps:

[0109] S1: Preprocess all laser points in each frame of the lidar point cloud collected by the lidar, and use the FCNN-based point cloud segmentation network to detect potential dynamic targets in the preprocessed point cloud;

[0110] S2: Use the Kalman filter method to estimate the state of the detected potential dynamic target and obtain the optimal estimate of the center of mass state of the potential dynamic target at time t in, represents the plane coordinates of the potential dynamic target at time t, represents the speed of the potential dynamic target at time t;

[0111] S3: Calculate the geometric information and intensity information of potential dynamic targets;

[0112] S4: Fuse the geometric information and intensity information of potential dynamic targets, detect all potential dynamic targets using the association method, and calculate the speed of the potential dynamic targets according to S2. If the speed of the potential dynamic target at time t is 0, then the potential dynamic target is determined to be a static target, otherwise it is a dynamic target.

[0113] S5: Extract feature points of static targets through curvature;

[0114] S6: assign different weights to the feature points by curvature, then construct a pose constraint function based on the weighted function and solve it to obtain the pose estimation of the static target;

[0115] S7: Based on the sliding window, key frames and the pose estimation obtained in S6, the laser points corresponding to the static target are updated with distortion compensation to obtain the latest laser odometer, that is, the pose transformation relationship △T between adjacent key frames k,k+1 And the latest coordinates of the laser point corresponding to the updated static target. The pose transformation of the two adjacent frames of the static target point cloud can be used to obtain the pose transformation of the lidar in two adjacent frames. The pose transformation relationship of the current frame relative to the previous frame and the pose of the lidar in the previous frame can be used to calculate the pose estimate of the lidar in the current frame. The initial pose of the lidar is obtained by IMU pre-integration, and then the pose transformation of the two adjacent frames of the static target point cloud can be used to obtain the pose transformation of the two adjacent frames of the lidar. The pose transformation relationship of the current frame relative to the previous frame and the pose of the lidar in the previous frame can be used to calculate the pose estimate of the lidar in the current frame.

[0116] Specifically, the steps of preprocessing all laser points in the point cloud collected by the laser radar in S1 are as follows:

[0117] When the linear mechanical laser radar rotates to obtain three-dimensional information of the environment, the frequency is 10HZ, that is, it only takes 0.1s to scan a circle. This means that when the robot is moving, due to its own rotational or translational motion, all points in a frame of laser radar point cloud may not be collected at the same position. When the carrier moves forward, the coordinate system of the point cloud collected in the same frame is different. For the generated motion distortion, data compensation must be used to eliminate the distortion of the laser point cloud, that is, reprojecting the point cloud of a certain frame to the coordinate system at the same time.

[0118] Suppose the laser point cloud collected by the kth scan is represented by P k Indicates that point cloud distortion compensation is to convert P k All laser points in are reprojected to t k In the radar coordinate system at the end of the moment. Using the transformation relationship between the laser radar coordinate system and the IMU coordinate system The laser point cloud can be mapped to the IMU coordinate system:

[0119]

[0120] Where p I With p L They represent the laser points in the IMU coordinate system and the radar coordinate system respectively.

[0121] Since the point cloud collected by the LiDAR only contains geometric information and intensity information, but not the collection order, the point cloud needs to be serialized before interpolation. The LiDAR rotates at a constant angular velocity, and the point cloud can be numbered according to the angle difference based on the timing of scanning a circle of point clouds, θ e Indicates the angle of the end scan line relative to the start line, θ n is the angle of the current scan line relative to the starting line, and the moment of the current point is expressed as:

[0122]

[0123] Among them, t s represents the start scanning time of the laser radar, and f represents the rotation frequency of the laser radar.

[0124] After serializing the unordered point cloud, find the point cloud with t based on the time sequence. k The two most recent frames of IMU data are pre-integrated and then the calculation results are projected into the world coordinate system W to obtain t k IMU attitude under W at all times Similarly, we can get t k+1 IMU attitude at all times Based on this, we can get t k+1 Time relative to t kPosition at the moment:

[0125]

[0126] Last passed Perform transformation calculation to obtain n The motion compensation transformation matrix of each laser point at the moment

[0127]

[0128] Compensate for motion distortion and obtain compensated laser points

[0129]

[0130] Specifically, the process of detecting potential dynamic targets by the FCNN-based point cloud segmentation network in S1 is as follows:

[0131] S1.1: Perform dimensionality reduction on the preprocessed point cloud, then divide the 3D space of the preprocessed point cloud into regions according to the bird's-eye view projection and establish its 2D index, and then extract the feature information of the point columns in each grid in the projection image. These feature information are used to generate the input of the FCNN network, that is, the two-dimensional feature map;

[0132] Specifically, 8 feature information of the point column in each grid in the projection map is extracted, including the number of laser points in the cell, average intensity, average height, intensity of the highest point, maximum height, angle deviation and distance between the cell center and the origin, and binary representation of whether the cell is occupied. These feature information are used to generate the input of the FCNN network, that is, the two-dimensional feature map. The bird's-eye view projection point cloud data is cut using a two-dimensional grid, and each laser point falls in a grid, thus becoming an ordered two-dimensional matrix. There may be several or zero laser points in a grid.

[0133] The FCNN network includes a feature encoder, a feature decoder, and an attribute predictor;

[0134] S1.2: The feature encoder extracts semantic features from the two-dimensional feature map through the convolution of the convolution layer, continuously downsamples the spatial resolution of the two-dimensional feature map, and finally outputs a feature vector. The convolution layer includes a trained VGG network and a ResNet network.

[0135] S1.3: The feature vector is input into the feature decoder and upsampled using a deconvolution layer to obtain a prediction map of the same size as the two-dimensional feature map;

[0136] S1.4: The attribute predictor calculates four attribute values ​​for each grid in the prediction map, wherein the four attribute values ​​are a center offset value, a center offset direction value, an attribute probability value, and an object height value;

[0137] S1.5: Construct a deviation pointer for each grid according to the center offset value and center offset direction of each grid, and the cells pointed to by each of the multiple pointers are used as candidate classification objects.

[0138] For candidate classification objects, if the attribute probability value of the classification object is not less than the probability threshold, it is considered to be a reliable clustering object.

[0139] The reliable clustering objects screened out are potential dynamic targets.

[0140] Specifically, the calculation process of calculating the geometric information and intensity information of the potential dynamic target in S3 is as follows:

[0141] S3.1: Calculate the geometric information of the potential dynamic target, that is, calculate the center of mass position of the potential dynamic target, and use the laser point coordinates corresponding to the potential dynamic target to calculate the center of mass position of the potential dynamic target As shown in formula (6):

[0142]

[0143] Where n represents the number of laser points corresponding to potential dynamic targets, x i ,y i ,z i represents the coordinates of the i-th laser point corresponding to the dynamic target, Represents the coordinates of the center of mass of the dynamic target;

[0144] S3.2: Convert the raw laser point data output by the laser radar into decimal representation and perform normalization to obtain the intensity information of each laser point.

[0145] S3.3: The intensity channel of the LiDAR is noisy, which is due to the influence of the target surface characteristics, such as roughness, surface reflectivity, and acquisition geometric distance. Therefore, in order to reduce the interference of these factors, the intensity information of the laser point is calibrated through the mapping function:

[0146]

[0147] Where d is the distance information of the original laser point, η k represents the intensity information of the kth original laser point, η k ′ represents the intensity information of the kth laser point after calibration.

[0148] Specifically, the process of detecting all potential dynamic targets using the association method in S4 is as follows:

[0149] S4.1: Calculate the surface reflection coefficient ρ′ of the laser point corresponding to the potential dynamic target:

[0150]

[0151] Wherein, M is a constant, which is related to the laser radar transmission power, optical transmission system, atmospheric transmission coefficient, etc., and α is the laser incident angle;

[0152] S4.2: Determine whether the potential dynamic targets in different frames are the same target. If formula (9) is satisfied, then the potential dynamic targets in different frames are considered to be the same target. Otherwise, they are not:

[0153]

[0154] Where d thr and ρ thr Respectively represent the set distance threshold and intensity threshold, represents the centroid position of the potential dynamic target a at time t and time t-1, represents the average intensity of a at time t and time t-1. The reflectivity ρ of the surface of the potential dynamic target can be obtained by taking the average of the reflectivity ρ′ of each laser point corresponding to the potential dynamic target.

[0155] The potential dynamic targets in the image at time t are associated with the potential dynamic targets in the image at time t-1 by formula (9), and the potential dynamic targets blocked by obstacles are associated, so that all potential dynamic targets are detected.

[0156] After the association method is completed, the target of the current frame can be associated with the target of the previous frame, such as Figure 3 As shown in FIG. 1 , the multi-target tracking method combines the association result and the Kalman filter to predict the state of the target to obtain the tracking result, that is, the tracking result includes the motion trajectory and position, speed and other states of the target, and then divides the target into static target and dynamic target according to whether the target speed is 0.

[0157] Specifically, the process of extracting feature points of a static target by curvature in S5 is as follows:

[0158] S5.1: The laser radar scans a vertical plane with M parallel readings, each of which has N points. Denote all laser points corresponding to all static targets in the kth laser radar scan as P. k ′, each laser point is represented as where m∈[1,M],n∈[1,N],x k ,y k ,z k is the three-dimensional coordinate of the kth laser point;

[0159] Will The continuous point set of the harness in the horizontal direction is Indicates that, for The plane formed by adjacent points, Curvature The calculation formula is:

[0160]

[0161] Where N s express The number of laser points in express The jth laser point in the mth row.

[0162] Then according to the size of c value All laser points in the image are classified, and those with c values ​​greater than the set threshold are regarded as corner points or edge points, and those with c values ​​less than or equal to the set threshold are regarded as plane feature points.

[0163] S5.2: By setting the threshold σ Marked as the edge points to be selected and the plane feature points to be selected. In order to obtain evenly distributed feature points, the points collected by each scan line are It is evenly divided into four sub-regions, and at most two edge feature points and four plane feature points are extracted from each sub-region. The set of all edge feature points in the four sub-regions is recorded as ε k , the set of all plane feature points in the four sub-areas is recorded as s k .

[0164] Specifically, the process of solving the static target pose estimation in S6 is as follows:

[0165] S6.1: Denote all laser points corresponding to all static targets scanned by the kth laser radar as P k ′,P k A laser point in ′ is represented by p k ={x k ,y k ,z k ,1} T , let the position of the laser radar in the kth frame be T k , then the 6-DOF pose transformation between two consecutive frames k-1 and k is:

[0166]

[0167] in, yes The function log(·) is a mapping from the homogeneous transformation matrix to the Lie algebra, from the kth frame to the short time δ t The pose transformation inside is expressed as:

[0168]

[0169]

[0170] In the formula Indicates that all laser points corresponding to the static target in the current k-th frame are reprojected to the position at the beginning of the current frame.

[0171] Assume ε k The edge feature point p ε The local smoothness is c ε ,s k Midplane feature point p s The local smoothness is c s , their weights are defined as shown in formula (14):

[0172]

[0173] Among them, W(p ε )、W(p s ) represent p ε and p s The weight of Represents ε k The jth edge feature point in the i-th row The curvature.

[0174] S6.2: Calculate each edge feature point p ε The covariance matrix of the neighboring points in the global edge feature set, the global edge feature set is all the ε contained in a frame of image k If the covariance matrix contains the largest eigenvalue, it means that p ε The adjacent points in the global edge feature are distributed on a straight line, and the eigenvector corresponding to the maximum eigenvalue As the direction of the line, the geometric center of the adjacent point As the position of the straight line, the edge feature point p ε The distance to the global edge feature is:

[0175]

[0176] where the symbol · is the dot product, p n is a unit vector;

[0177]

[0178] S6.3: Calculate each plane feature point p S , the covariance matrix with the neighboring points in the global plane feature set, the global plane feature set is all s contained in a frame of image k If the covariance matrix contains a minimum eigenvalue, it means that these adjacent points are distributed on a plane, and the eigenvector corresponding to this minimum eigenvalue Considered as the direction of the surface normal, the geometric center of the nearby point It can be regarded as the position of the plane. At this time, the plane feature point p S The distance to the global plane feature set is expressed as:

[0179]

[0180] S6.4: The optimization function is defined as:

[0181]

[0182] S6.5: Solve the nonlinear equation of formula (12) using the Gauss-Newton method to obtain the optimal attitude estimate, and apply the model with left perturbation to estimate the Jacobian matrix J p :

[0183]

[0184] Where [Tp] × That is [T k p k ] × , which means converting the 4D point expression {x, y, z, 1} into a 3D point expression {x, y, z} and calculating its skew-symmetric matrix. δ and ξ represent short time, δ t and the 6-DOF pose transformation between two consecutive frames T k p k Represents a 4*4 matrix and a 4*1 vector to obtain a 4*1 vector, the Jacobian matrix J of the edge residual ε Calculated by the following formula:

[0185]

[0186] Jacobian matrix J of the plane residual s :

[0187]

[0188] The iterative increment △T is represented by the Jacobian matrix k , using △T k Update the laser odometer estimation result and iterate the pose optimization until △T kConvergence, the current LiDAR pose estimate T can be calculated k .

[0189] Specifically, the process of obtaining the latest laser odometer output in S7 is as follows:

[0190] S7.1: Select key frames by setting a threshold for posture change. The translation and rotation thresholds are set to 1m and 10° respectively. That is, when the posture change between the kth moment and the k+1th moment exceeds the threshold for posture change, the current frame will be selected as the key frame F k+1 And add a sliding window, the F k+1 With p k To store the projections of the laser point cloud of a fixed number of recent frames, a stack is first maintained as a sliding window. Then, the i closest key frames are extracted in time sequence and these key frames are represented as a sub-key frame set {F k-i ,...,F k}, through the transformation matrix {T' k-i ,...,T' k}, project the sub-keyframe set onto W to obtain the projection map M k .

[0191] M k Projection image from edge feature point cloud And plane feature point cloud projection Each key frame and the corresponding projection map M k They are related to each other, and the relationship definition is as follows:

[0192]

[0193] in, Respectively represent the edge feature points of the kth key frame and the kith key frame in the sub-key frame set, They represent the plane feature points of the kth key frame and the kith key frame in the sub-key frame set respectively.

[0194] S7.2: The latest k+1 key frames F k+1 Transform to W and compare with the projection map M k Associate, and obtain the incremental pose transformation △T of the k+1th frame k,k+1 :

[0195]

[0196] in, Indicates T k The inverse matrix, T k+1 Represents the pose estimate of the k+1 frame lidar.

[0197] Recalculating the distortion using the 6-DOF pose transformation yields:

[0198]

[0199]

[0200] In the formula is △T k,k+1 The Lie algebra representation of the updated optimal positioning estimate of the lidar can be calculated by formula (19), △T k,k+1 Represents the pose transformation relationship between two adjacent key frames. Indicates the latest coordinates of the laser point corresponding to the updated static target;

[0201] All laser points corresponding to the static targets scanned by the k+1th laser radar are represented as P′ k+1 , P′ k+1 A laser point is denoted as p k+1 , is the 6-DOF pose transformation between two consecutive frames k and k+1, according to the △T of the k+1th frame k,k+1 Calculated, Indicates the latest coordinates of the laser point corresponding to the updated static target.

[0202] Output and the pose transformation relationship between two adjacent key frames △T k,k+1 .

[0203] The factor graph backend optimization method is a common graph optimization method that uses variables and factors in the factor graph to represent the relationship between the robot's posture and environmental features. It is essentially a Bayesian network such as Fig. 9 As shown, the Bayesian network is a directed acyclic graph. Assume that t i The state of the robot at this moment is represented by X K ={x i}, i∈(0...K) represents the position of the object detected by the sensor, that is, the landmark point is L N = {l j}, where j∈(1...N), the received laser sensor observation is Z K ={z i}, the robot's control input value is expressed as U K = {u i}, where i∈(0...K). The thin and thick lines in the figure represent the prediction equation g and the measurement equation h, respectively. The solid-line box contains the landmark l1 and the position x1, which determine the observation data z1 under the action of the measurement equation h. The position x1 determines the next position x2 based on the prediction equation g and the control input u2, and progresses step by step through this basic process.

[0204] The process of estimating the state quantity from the observation quantity can be converted into a posterior probability problem, which is expressed as:

[0205] P(X K |Z K ) (26)

[0206] According to Bayes' theorem, we have:

[0207]

[0208] Where P(Z K |X K ) and P(X K ) is usually called likelihood and prior probability. According to the Markov hypothesis, the observations corresponding to each state are independent of each other, and the current state is only related to the previous moment, then:

[0209]

[0210] At each moment, considering the control input value of the robot as the parameter value of the state quantity, the motion model on the right side of equation (27) can be further rewritten as:

[0211]

[0212] Where P(x0) is the prior value of the initial state.

[0213] Then solving the optimal state estimation problem becomes solving the joint probability density function of each variable and the measured value:

[0214]

[0215] Assuming that the prediction model is a Gaussian measurement model, the prediction model can be expressed as:

[0216] x i =g(x i-1 ,u i )+ω i (31)

[0217]

[0218] where ω i Indicates the process of motion i The prediction noise of the input at the moment, and ωi ~N(0,Λ i ), Λ i represents the noise covariance matrix.

[0219] Similarly, assuming that the observation model is also a Gaussian measurement model, the observation model can be expressed as:

[0220] z i =h(x i ,l j )+v i (33)

[0221]

[0222] where v i is the measurement noise, and v i ~N(0,Γ i ), Γ i Represents the corresponding covariance matrix.

[0223] Then, combining equations (32) and (34), we can get the joint probability density function P(X K ,L K ,U K ,Z K ) takes the negative logarithm, the maximum a posteriori probability problem can be converted into a least squares problem, expressed as:

[0224]

[0225] The factor graph model {X, F, E} is simplified on the Bayesian network model, such as Fig.10 As shown, the probability connection between variable nodes is represented by factor node f i ∈F means that the state variable node x i ∈X factor f i (x i-1 ,x i ) corresponds to the probability P(x i |x i-1 ,u i ), the factor correspondence probability P(z i |x i ,l j ). ij ∈E is the edge connecting the factor node and the variable node, indicating the constraint relationship between the nodes. Then the joint probability density function P(X K ,L K ,U K ,Z K ) can be converted into a product of multiple factors:

[0226]

[0227] Where X i is related to the factor node f i Adjacent x i Set, then Fig.10 The factor graph model optimization equation is:

[0228] F(X,L)=f0(x0)f1(x0,x1)f2(x1,x2)f3(x2,x3)f4(l1,x1)f5(l2,x2) (37)

[0229]

[0230] In the formula, f0(x0) corresponds to the prior factor p(x0), and then the nonlinear least squares problem is obtained in the form of negative logarithm. According to the nonlinear minimum problem solving method LM, the variables to be solved can be optimized to obtain the global optimal posture.

[0231] A backend optimization method based on a multi-factor graph for obtaining precise posture of a robot comprises the following steps:

[0232] S100: Construct an improved multi-factor graphical model F(X):

[0233]

[0234] Where f (i,j) (X i ,X j ) represents the relationship factor between the i-th frame pose node and the j-th frame pose node, and the solid line box C i , dotted box F i , the black nodes are divided into IMU factors f IMU , laser odometry factor f Lidar and the loop closure factor f Loop , f IMU =f IMU (X i ,X i+1 ), f Lidar =f Lidar (X i ,X i+1 ), f Loop =f Loop (X c ,X i+1 ), E represents all the edges associated with the nodes in F(X).

[0235] A multi-factor graph model is constructed based on IMU factors, laser odometer factors, and closed-loop factors for back-end optimization. First, the IMU data is pre-integrated to obtain the observation model error of the IMU sensor and construct the IMU factor f IMUAt the same time, the position change relationship output by the front-end laser odometer is used to calculate the observation model error of the laser radar sensor and construct the laser odometer factor f Lidar , and construct the closed loop factor f when the loop is successfully detected Loop , and then these three constraint factors are associated with the state variable X i Connect them with edges and add them to the basic factor graph model. Fig.10 The factor graph optimization model of can be further expressed as a multi-factor graph optimization model, such as Figure 5 shown.

[0236] The objective function of backend optimization is converted to solving the least squares problem, which is expressed as:

[0237]

[0238] In the formula and They represent the errors of the IMU factor, laser odometer factor and closed-loop factor observation model respectively, and are the corresponding measurement noise covariances respectively. Among them, the error term can be uniformly expressed as:

[0239]

[0240] Where T i With T j are the positions of the robot in the world coordinate system for the i-th and j-th key frames, △T i,j represents the relative position transformation relationship between two variable nodes calculated by three factors, △T i,j include and

[0241] S200: Output the above content to IMU and f Lidar Incrementally add to F(X). When the mobile robot visits the same place during movement, that is, after the loop detection is successful, add f to F(X). Loop , then the relevant variable nodes perform pose optimization according to the update of the factor nodes, and based on the optimized state set X * , the projection map M established by the front-end odometer k The corresponding point cloud in the image is reprojected to the world coordinate system to update the precise position and posture of the robot.

[0242] Specifically, in S100, f is obtained IMU The process is as follows:

[0243] In order to estimate the position and orientation of the robot more accurately, the IMU factor calculation is divided into three steps. The first step is to perform the initial pre-integration calculation. The second step is to optimize the initial pre-integration result using the open source factor graph optimization framework GTSAM and correct the errors in the IMU data. The third step is to recalculate the integral using the updated IMU zero bias and output the IMU factor related items. The specific calculation process is as follows:

[0244] First, assume that the original angular velocity and acceleration output by the IMU at time t are and It is shown that from the IMU measurement model:

[0245]

[0246]

[0247] In the formula represents the rotation matrix from the map coordinate system W to the sensor coordinate system B, g is the gravitational acceleration in W, and it can be seen from the formula that and Affected by IMU zero bias b t and white noise n t The influence of t and a t They represent the actual angular velocity and acceleration of the lidar at time t respectively;

[0248] and They represent the IMU zero bias of the original angular velocity and the IMU zero bias of the acceleration respectively. and Collectively referred to as IMU zero bias b t , and Respectively represent the white noise of the original angular velocity and the white noise of acceleration, and Collectively referred to as n t ;

[0249] Then the first stage of calculation is performed to estimate the robot's motion by integrating the multi-frame angular velocity and acceleration of the IMU. At time t+△t, the robot's velocity v t+Δt , the robot's position p t+Δt and the rotation transformation R t+Δt They are calculated by the following formulas:

[0250]

[0251]

[0252]

[0253] Among them, R t , p t , v t They represent the rotation change, position and speed of the laser radar at time t respectively, and △t represents a short time.

[0254] The second step is to calculate the relative motion change between the robot i and j, the speed change during this period △v ij 、Position transformation △p ij and the rotation transformation △R ij They are calculated by the following formulas:

[0255]

[0256]

[0257]

[0258] Among them, v i , p i , R i They represent the velocity, position and rotation transformation of the laser radar at time i, respectively, and v j , p j , R j They represent the velocity, position and rotation transformation of the laser radar at time j, △t ij Represents the time difference between time i and time j.

[0259] Then, the open source GTSAM optimization library is used to optimize the IMU’s velocity change △v ij 、Position transformation △p ij and the rotation transformation △R ij Three state variables. In the GTSAM optimization library, the prior factors are the speed and position of the IMU at time i, the zero bias matrix of the IMU is used as the factor node, and the optimized variable nodes are the speed, position and transformation matrix at time j. Before the next pre-integration result is input, the GTSAM optimizer will output the updated position transformation, speed transformation and rotation transformation as well as the zero bias matrix b i ′.

[0260] Using the updated zero bias b i ′, recalculate the pre-integral by the following formula to obtain the speed of the optimized IMU at time β Location and rotation transformation

[0261]

[0262]

[0263]

[0264] Among them, b i ' a and b i ' ω They represent the IMU zero bias of the updated original angular velocity and the IMU zero bias of the updated acceleration respectively. and Represents the white noise of the original angular velocity at the i-th moment and the white noise of the acceleration at the i-th moment.

[0265] The relative posture change between two determined moments is obtained by three-stage pre-integration calculation, and then The calculation formula is as follows:

[0266]

[0267]

[0268]

[0269]

[0270] They respectively represent the speed change, position change, and rotation change between the updated moments i and j.

[0271] f IMU Represents the observation model error of the IMU sensor between frame i and frame i+1 The pre-integrated Substituting into formula (41), we can get the observation model error of the IMU sensor between frame i and frame j: In the same way, we can get

[0272] Specifically, in S100, f is obtained Lidar The process is as follows:

[0273] The robot laser odometer outputs the pose transformation relationship △T between two adjacent key frames i and i+1 i,i+1 , △T i,i+1 Used to calculate the error term in the laser odometer factor, expressed as:

[0274]

[0275] f Lidar Represents the observation model error of the lidar sensor between frame i and frame i+1 △T i,i+1 Substituting into formula (41) we can get

[0276] Specifically, in S100, f is obtained Loop The process is as follows:

[0277] When the closed loop is detected successfully, the closed loop factor is constructed by forming the relative posture change relationship between the two closed key frames. The traditional iterative closest point feature matching method is used to obtain the accurate posture transformation △T between the two point sets. Let the current point cloud posture be T i+1 , the pose of the loop frame is T c , then the relative posture change between the two is obtained, that is, the closed-loop factor is expressed as:

[0278]

[0279] Among them, f Loop Represents the observation model error between the current frame i+1 and the loop frame c in the loop detection Will Substituting into formula (41) we can get

[0280] Specifically, the process of loop detection in S200 is as follows:

[0281] Loop detection mainly uses different judgment strategies to identify the robot's position and detect whether the robot has formed a closed loop with its historical posture or route during operation; if a closed loop is formed, a closed-loop factor is constructed and added to the factor graph model to optimize the robot's posture, thereby effectively correcting the trajectory with large drift.

[0282] S201: The point cloud obtained by the laser radar to obtain environmental information includes distance information and intensity information. After normalizing and calibrating the intensity information through equation (59), η is obtained. k ′, assuming that each point cloud in the kth frame is represented as [x k ,y k ,z k ,η k ′], projecting the 3D point cloud in the Cartesian coordinate system to the polar coordinate system composed of the XY plane. The polar coordinates of the transformed point cloud are It is expressed as:

[0283]

[0284]

[0285]

[0286]

[0287] S202: Then, with the sensor as the center, the point cloud is divided into N equal parts by annular angle and radial direction. s ×N r sub-regions, such as Figure 7 This is a schematic diagram of point cloud segmentation. A frame of point cloud is segmented into N s sectors and N r Rings. Each segment is represented by:

[0288]

[0289] where a∈[1,N s ],b∈[1,N r ], L max It means that the laser radar scans the maximum sensing range in sequence. Because the intensity value returned by the same object is the same, for each small area S ab The point cloud in the sub-region is replaced by the maximum intensity information in the sub-region. ab :

[0290]

[0291] S203: Extract each sub-region S ab After obtaining the intensity information, it is necessary to continue encoding the point set in the area. The encoding diagram is shown in Figure 8 As shown, each sub-area S ab The point cloud in the vertical direction is divided into 8 units and marked as z from bottom to top. k , where k∈(1,2,...,8), points with the same z coordinate are in the same unit. If there is no laser point in a unit, z k is set to zero, otherwise, if there is at least one laser point in a cell, z k Set to 1. In this way, each sub-area S ab will get an 8-bit binary code, and then according to z k The binary value is converted to a decimal value, and the geometric information of each area is d ab It can be expressed as:

[0292]

[0293] Where z(·) represents the ab The function of converting the binary code of into a decimal value. Combining the geometric information in this formula with the intensity information in formula (64) gives the point cloud global descriptor Ω(a,b):

[0294]

[0295] Location recognition is the process of finding the historical location closest to the current location by calculating the Euclidean distance between the current location and the historical location. That is, the process of matching the descriptor of the current location with the descriptor of the historical location. However, as the number of historical locations increases, the computational cost of searching for matches will also increase.

[0296] S204: A two-step hierarchical fast detection method is used to calculate geometric similarity and intensity similarity respectively to accelerate matching, and then the validity of the matching is verified based on consistency.

[0297] S204-1: Geometric similarity calculation

[0298] For a pair of global descriptors to be matched, the geometric similarity scores of the two global descriptors are first calculated. The global descriptor of the current point cloud is represented as Ω q , the global descriptor of the point cloud to be matched is Ω c The geometric code values ​​at the same index of the two are and calculate and The cosine distance of and normalized processing, then get the geometric similarity function:

[0299]

[0300] Among them, N S Indicates the N mentioned above S sectors, d q , d c Respectively represent the current point cloud global descriptor Ω q And the global descriptor Ω of the point cloud to be matched c The corresponding geometric information in .

[0301] However, the representation of the calculated global scan descriptor of the point cloud is affected by the position of the sensor, that is, the descriptor calculated at the same index will be different if the scanning position of the lidar is different. This is mainly reflected in the features encoded in the circumferential order. When the point cloud is rotated, the constructed point cloud descriptor may be horizontally displaced in the index.

[0302] In order to solve the problem of rotation invariance of point cloud descriptors, we can first calculate all possible displacement distances between the global descriptors of each pair of point clouds in the horizontal direction to obtain a set of distance values. Finally, the minimum value is selected from this set of distance values ​​as the distance between the point cloud descriptor pair, and then the geometric similarity is solved. The calculation process is as follows:

[0303] Assume that the global descriptor of a point cloud is Here n means that the global descriptor of the point cloud is translated by n units in the horizontal direction. The column displacement number n* and the geometric similarity score after the shift D(dq ,d c ) are defined as:

[0304]

[0305]

[0306] S204-2: Intensity Similarity Calculation

[0307] The purpose of intensity similarity matching is to reduce the error and noise in geometric similarity matching, so as to obtain more accurate matching results. Its core is to calculate the intensity feature η between two point cloud descriptors column by column. q and η c This is achieved by the similarity of and are η q and η c Columns with the same index value, take and Calculate the intensity similarity score as follows:

[0308]

[0309] Among them, η q and η c Represent the intensity features between two point cloud descriptors, Represent the intensity similarity scores respectively.

[0310] By comparing η c and η q Translation * Intensity characteristics after the column The final calculation result of intensity similarity is:

[0311]

[0312] n * Indicates a change in perspective.

[0313] Select the K historical descriptors that are most similar to the target point cloud, and input the corresponding historical descriptors of the K candidate frames into the consistency verification part for verification.

[0314] S204-3: Consistency Verification

[0315] In the SLAM system, the point cloud similarity of adjacent frames of the loop frame is high, and the sensor feedback is continuous in time. In order to reduce the mismatch, the average of the similarities of the point cloud descriptors of the past N frames of the current frame and the point cloud descriptors of the past N frames of K candidate frames is used as the final similarity, which is used as the verification standard for consistency. The calculation formula is as follows:

[0316]

[0317] where Θ(p q ,p c ) represents the average similarity between the point cloud descriptor of the past N frames of the current frame and the point cloud descriptor of the past N frames of a frame among the K candidate frames, p q Indicates the current frame, p c represents a frame among K candidate frames, d q-i , d c-i , η q-i , η c-i They represent the geometric information and intensity information in the global descriptor of the point cloud of the current frame and the candidate frame respectively.

[0318] When the mobile robot visits in the opposite direction, that is, when the sensor observation angle changes, Ω q-i Change to Ω accordingly q+i . Use the obtained K objects to be matched to build a K nearest neighbor search tree, compare the descriptor of the target point cloud with that in the search tree, and use formula (72) to measure the similarity between them. Finally, set a threshold to select the most similar candidate point and use it as the revisited position, expressed as:

[0319]

[0320] Where C represents the index value of K objects to be matched in the search tree. is the threshold value set. * Represents the final closed-loop position index value, and obtains the pose T of the loop frame from the historical key frame c , and used it for subsequent construction of closed-loop factors.

[0321] Experimental verification:

[0322] The classic LOAM algorithm based on the laser radar SLAM framework is adopted as the baseline model framework, and the back-end optimization algorithm based on the multi-factor graph proposed in the present invention is added to the LOAM framework. For the proposed multi-factor graph back-end optimization algorithm, the following groups of comparative experiments are carried out: the first group is a comparative experiment of the back-end optimization algorithm proposed in the present invention before and after the loop detection method is turned on, the second group is a comparative experiment of the loop detection method adopted in the present invention and other loop detection algorithms based on point cloud descriptors, and the third group is a comparative experiment of the back-end optimization algorithms in different laser SLAM frameworks.

[0323] 1. Experimental Dataset

[0324] The experiment uses the RawData dataset in the KITTI dataset, which has a total of 22 sets of data, of which the first 11

[0325] Sequences (0-10) have ground truth trajectories, and the remaining 11 sequences (11-21) do not have ground truth trajectories. The detailed information of each sequence can be found in Table 1.

[0326] Table 1 Sequence information table of KITTI dataset

[0327]

[0328]

[0329] 2. Evaluation Criteria

[0330] To verify the effectiveness of the loop detection method, we essentially examine the classification effect of a binary classification problem. The examination indicators are precision and recall rate. These two indicators are calculated through the four basic indicators of true positive (TP), true negative (TN), false positive (FP), and false negative (FN).

[0331] The accuracy of closed-loop detection K p It is expressed as the proportion of correctly predicted closed loops among all predicted closed loops, and the calculation formula is:

[0332]

[0333] The recall rate K of the closed-loop detection experiment r It is expressed as the proportion of correctly predicted closed loops to all closed loop environments, and the calculation formula is:

[0334]

[0335] The relevant indicators for evaluating the performance of the backend optimization algorithm include the absolute pose error (ATE) and relative pose error (RPE) of the algorithm output trajectory. The absolute pose error (ATE) is obtained by calculating the error between the trajectory output by the SLAM algorithm and the actual trajectory, which can evaluate the accuracy of the algorithm and the consistency of the trajectory. Assume that the output pose of the algorithm at time i is p i , at this time the ground truth value of the pose is q i , and the transformation matrix from the output pose to the true pose is T, then the absolute pose difference at time i is E i for:

[0336]

[0337] Usually use E i The root mean square error (RMSE) is used as the ATE indicator to evaluate the global positioning consistency. The RMSE of the global trajectory can be calculated based on the absolute pose error at each moment:

[0338]

[0339] Where m represents the number of poses in the trajectory, trans(E i ) represents the translation part of the absolute error.

[0340] The relative pose error (RPE) describes the error of the odometer by calculating the pose difference between two frames with equal time intervals. It can help us understand the pose changes between adjacent frames, and can also reflect the accuracy and robustness of the local pose estimation of the positioning algorithm in the global trajectory. Similarly, the root mean square error (RMSE) is usually used as the RPE indicator. First, the relative pose error at each moment is calculated. Assuming that the equal time interval is △, the pose difference F at time i is i for:

[0341]

[0342] The RMSE of the relative posture difference at each moment is:

[0343]

[0344] Where n represents the number of poses in the trajectory, trans(F i ) represents the translation part of the relative error.

[0345] 3. Experimental Setup

[0346] As can be seen from Table 1, among the first 11 sequences in the data set, only the 00 sequence, the 02 sequence, and the 05-08 sequence have loops, so the closed-loop detection experiment is carried out on these sequences of the KITTI data set. On the one hand, the performance of the closed-loop detection method is evaluated by comparing the RPE index of the global trajectory of the back-end optimization algorithm proposed in the present invention when the closed-loop detection function is turned on and off. On the other hand, the loop detection method in this paper continues to be compared with the cutting-edge closed-loop detection methods in other SLAM algorithms, including the DBoW3 method in visual SLAM, the loop detection method based on the point cloud global descriptor SC, and the descriptor-based Lidar Iris loop detection method. At the same time, the trajectories output by the back-end optimization algorithm proposed in the present invention, LOAM and LeGO-LOAM under the 07 sequence and 08 sequence of the KITTI data set are compared with the ground truth, and the ATE and RPE related values ​​of the output trajectory of each SLAM system are calculated, so as to compare the accuracy of the pose estimation of the entire algorithm.

[0347] 4. Experimental results analysis

[0348] Fig.11 The global trajectory comparison of the method of the present invention with the loop detection function turned on and off is shown in Table 2.

[0349] Table 2 RPE results before and after the closed-loop test

[0350] Table 2RPE results before and after opening closed-loop detection

[0351]

[0352] Fig.11 The circled area represents the closed-loop scene of the sequence. The left side is the global positioning result in the 02 and 05 sequences when the loop detection is turned on by the back-end optimization algorithm proposed in the present invention, and the right side is the global positioning result when the loop detection is turned off. It can be seen intuitively that when the loop detection function is turned off, the error will accumulate over time, resulting in obvious deviations in the pose estimation, and the motion trajectory output by the algorithm is not completely closed at the loop. On the contrary, after the closed-loop detection is turned on, the method in this paper can better achieve closed-loop matching in the complex environment of the 02 sequence with reverse access to the closed-loop scene, and the accumulated drift is eliminated due to the successful construction of the closed-loop factor added to the multi-factor graph optimization model, thereby improving the accuracy of global pose estimation.

[0353] It can be seen from the relative pose error (RPE) results in Table 2 that in the two modes of opening the closed loop and closing the closed loop, the RPE results of the loop detection method adopted by the present invention on the 05 sequence are significantly different, and the global positioning error is significantly larger after the closed loop function is closed; and in the 02 sequence with a more complex loop scene, the difference in RPE of the pose estimation of the two modes is more significant. The pose error after opening the closed loop function is reduced by 71% compared with when it is closed, which proves the importance of the loop detection method adopted by the present invention in the SLAM back-end optimization algorithm.

[0354] The loop detection method is further compared with other cutting-edge loop detection methods in SLAM algorithms, including the DBoW3 method in visual SLAM, the loop detection method based on the point cloud global descriptor SC, and the descriptor-based LidarIris loop detection method. The results are shown in Table 3.

[0355] Table 3 Comparison of loop closure detection methods under KITTI dataset

[0356]

[0357] As can be seen from Table 3, for the 02 and 08 sequences with reverse access loop closure scenes, the recall rate of the vision-based method is significantly reduced, because the vision-based method cannot achieve reverse loop closure matching, resulting in fewer detected loops, while the proposed method and Lidar Iris method consider the impact of multi-angle access to historical locations when constructing global descriptors, and therefore show better recall rates. However, in the 02 sequence with more non-structural features, the road is narrow and there are trees on both sides of the road. The accuracy of loop detection is affected by the geometric shape, and the global descriptors constructed by geometric information and intensity information are less different, resulting in the performance loss of SC and the proposed loop detection method, and the accuracy is reduced. In the 00 and 05 sequences, the proposed method has higher accuracy and recall than other methods, which fully demonstrates the stability of the method's scene recognition ability.

[0358] Table 4 Experimental results of each algorithm under 07 sequence

[0359]

[0360] Table 4 shows the global positioning results and errors in the horizontal and rotational directions of LOAM, LeGO-LOAM and the backend optimization algorithm proposed in this invention for the 07 sequence of the KITTI dataset. From the data in the table, it can be seen that the performance difference of the three algorithms under the 07 sequence is not very large, but it can be seen that the error of the algorithm in the horizontal direction is significantly lower than that of the other two algorithms. For the odometer with a distance of 694 meters, the root mean square error and standard deviation of the absolute trajectory error and relative trajectory error of the method in this invention are better, among which the maximum value of the absolute trajectory error is controlled within 0.451 meters, and the relative posture error is controlled within 0.081 meters, indicating that the accuracy and robustness of the posture estimation of this invention are better than the other two methods.

[0361] Table 5 Experimental results of each algorithm under 08 sequence

[0362]

[0363] Table 5 shows the global positioning results and the errors in the horizontal and rotational directions of LOAM, LeGO-LOAM and the back-end optimization algorithm proposed in the present invention for the KITTI dataset 08 sequence.

[0364] From the data in the table, we can see that after the back-end optimization, the three algorithms have effectively corrected the drift accumulated in the front end, and for the driving distance of up to 3222 meters, the performance difference of the three algorithms under the 08 sequence is quite obvious. According to the positioning results after the back-end optimization, the accuracy of the algorithm in this paper is better than the other two algorithms. Combined with the pose error data in Table 5, it can be seen that compared with the LOAM back-end optimization without loop detection, the LeGO-LOAM back-end optimization link directly combines the frame to map (scan to map) matching method, and the back-end optimization of this paper integrates the IMU factor to correct the long-term accumulated drift. The effect is the most significant, followed by the LeGO-LOAM method.

[0365] In summary, the performance and performance of LOAM, LeGO-LOAM and the multi-factor graph backend optimization algorithm proposed in this invention are verified in different scenarios through two different sequences of KITTI datasets. It can be concluded that the advantages of this algorithm are not obvious in short-distance road scenarios; in long-distance dynamic and complex environments, this algorithm has a good improvement in accuracy and stability.

[0366] Finally, it should be noted that the above embodiments are only used to illustrate the technical solution of the present invention rather than to limit it. Although the present invention has been described in detail with reference to the preferred embodiments, those skilled in the art should understand that the technical solution of the present invention can be modified or replaced by equivalents without departing from the purpose and scope of the technical solution of the present invention, which should be included in the scope of the claims of the present invention.

Claims

1. A back-end optimization method based on a multi-factor graph for obtaining precise robot posture, characterized in that: The steps include: S100: Construct an improved multi-factor graphical model F(X): Where f (i,j) (X i ,X j ) represents the relationship factor between the i-th frame pose node and the j-th frame pose node, and the solid box C i , dotted box F i , the black nodes are divided into IMU factors f IMU , laser odometry factor f Lidar and the closed-loop factor f Loop , f IMU =f IMU (X i ,X i+1 ), f Lidar =f Lidar (X i ,X i+1 ), f Loop =f Loop (X c ,X i+1 ), E represents all edges associated with the node in F(X); The objective function of backend optimization is converted to solving the least squares problem, which is expressed as: In the formula and They represent the errors of the IMU factor, laser odometer factor and closed-loop factor observation model respectively, and are the corresponding measurement noise covariances, where the error terms can be uniformly expressed as: Where T i With T j are the positions of the robot in the world coordinate system for the i-th and j-th key frames, △T i,j represents the relative position transformation relationship between two variable nodes calculated by three factors, △T i,j include and S200: Output the above content to IMU and f Lidar Incrementally add to F(X). When the mobile robot visits the same place during movement, that is, after the loop detection is successful, add f to F(X). Loop , then the relevant variable nodes perform pose optimization according to the update of the factor nodes, and based on the optimized state set X * , the projection map M established by the front-end odometer k The corresponding point cloud in the image is reprojected to the world coordinate system to update the precise position and posture of the robot.

2. A back-end optimization method based on a multi-factor graph for obtaining precise posture of a robot according to claim 1, characterized in that: In S100, f is obtained IMU The process is as follows: First, assume that the original angular velocity and acceleration output by the IMU at time t are and It is shown that from the IMU measurement model: In the formula Represents the rotation matrix from the map coordinate system W to the sensor coordinate system B, g is the gravitational acceleration in W, ω t and a t They represent the actual angular velocity and acceleration of the lidar at time t respectively; and They represent the IMU zero bias of the original angular velocity and the IMU zero bias of the acceleration respectively. and Collectively referred to as IMU zero bias b t , and Respectively represent the white noise of the original angular velocity and the white noise of acceleration, and Collectively referred to as n t ; At time t+△t, the robot's velocity v t+Δt , the robot's position p t+Δt and the rotation transformation R t+Δt They are calculated by the following formulas: Among them, R t , p t , v t They represent the rotation transformation, position and speed of the laser radar at time t, respectively, and △t represents a short time; The second step is to calculate the relative motion change between the robot i and j, the speed change during this period △v ij 、Position transformation △p ij and the rotation transformation △R ij They are calculated by the following formulas: Among them, v i , p i , R i They represent the velocity, position and rotation transformation of the laser radar at time i, respectively, and v j , p j , R j They represent the velocity, position and rotation transformation of the laser radar at time j, △t ij Represents the time difference between time i and time j; Then, the open source GTSAM optimization library is used to optimize the IMU’s velocity change △v ij 、Position transformation △p ij and the rotation transformation △R ij Before the next pre-integration result is input, the GTSAM optimizer will output the updated position transformation, velocity transformation, rotation transformation and zero bias matrix b′ i ; Using the updated zero bias b′ i , recalculate the pre-integral by the following formula to obtain the speed of the optimized IMU at time β Location and rotation transformation in, and They represent the IMU zero bias of the updated original angular velocity and the IMU zero bias of the updated acceleration respectively. and Represents the white noise of the original angular velocity at the i-th moment and the white noise of the acceleration at the i-th moment; The relative posture change between two determined moments is obtained by three-stage pre-integration calculation, and then The calculation formula is as follows: Respectively represent the speed change, position change, and rotation change between the updated moments i and j; f IMU Represents the observation model error of the IMU sensor between frame i and frame i+1 The pre-integrated Substituting into formula (41), we can get the observation model error of the IMU sensor between frame i and frame j: In the same way, we can get 3. A back-end optimization method based on a multi-factor graph for obtaining precise posture of a robot as claimed in claim 2, characterized in that: In S100, f is obtained Lidar The process is as follows: The robot laser odometer outputs the pose transformation relationship △T between two adjacent key frames i and i+1 i,i+1 , △T i,i+1 Used to calculate the error term in the laser odometer factor, expressed as: f Lidar Represents the observation model error of the lidar sensor between frame i and frame i+1 △T i,i+1 Substituting into formula (41) we can get 4. A back-end optimization method based on a multi-factor graph for obtaining precise posture of a robot as claimed in claim 3, characterized in that: In S100, f is obtained Loop The process is as follows: The traditional iterative closest point feature matching method is used to obtain the accurate pose transformation △T between the two point sets. Let the current point cloud pose be T i+1 , the pose of the loop frame is T c , then the relative posture change between the two is obtained, that is, the closed-loop factor is expressed as: in, f Loop Represents the observation model error between the current frame i+1 and the loop frame c in the loop detection Will Substituting into formula (41) we can get 5. A back-end optimization method based on a multi-factor graph for obtaining precise posture of a robot as claimed in claim 4, characterized in that: The process of loop detection in S200 is as follows: S201: The point cloud obtained by the laser radar to obtain environmental information includes distance information and intensity information. The intensity information is normalized and calibrated by equation (59) to obtain η′ k , assuming that each point cloud in the kth frame is represented as [x k ,y k ,z k ,η′ k ], projecting the 3D point cloud in the Cartesian coordinate system to the polar coordinate system composed of the XY plane. The polar coordinates of the transformed point cloud It is expressed as: S202: Then, with the sensor as the center, the point cloud is divided into N equal parts by annular angle and radial direction. s ×N r sub-regions, each segment is represented as: where a∈[1,N s ],b∈[1,N r ], L max Indicates that the laser radar scans the maximum sensing range in sequence; because the intensity value returned by the same object is the same, for each small area S ab In the point cloud, the maximum intensity information in the sub-region is used to replace the spatial intensity value η ab : S203: Extract each sub-region S ab After the intensity information of each sub-region S ab The point cloud in the vertical direction is divided into 8 units and marked as z from bottom to top. k , where k∈(1,2,...,8), points with the same z coordinate are in the same unit; if there is no laser point in a unit, z k is set to zero, otherwise, if there is at least one laser point in a cell, z k Set to 1, each sub-area S ab will get an 8-bit binary code, and then according to z k The binary value is converted to a decimal value, and the geometric information of each area is d ab It can be expressed as: Where z(·) represents the ab The function of converting the binary code of into a decimal value, combining the geometric information in this formula with the intensity information in formula (64), we get the point cloud global descriptor Ω(a,b): S204: using a two-step hierarchical fast detection method to calculate geometric similarity and intensity similarity respectively to accelerate matching, and then verifying the validity of the matching based on consistency; S204-1: Geometric similarity calculation The global descriptor of the current point cloud is represented as Ω q , the global descriptor of the point cloud to be matched is Ω c The geometric code values ​​at the same index of the two are and calculate and The cosine distance of and normalized processing, then get the geometric similarity function: Among them, N S Indicates the N mentioned above S sectors, d q , d c Respectively represent the current point cloud global descriptor Ω q And the global descriptor Ω of the point cloud to be matched c The corresponding geometric information in Assume that the global descriptor of a point cloud is Here n means that the global descriptor of the point cloud is translated by n units in the horizontal direction, and the column displacement number n* and the geometric similarity score after the shift D(d q ,d c ) are defined as: S204-2: Intensity Similarity Calculation set up and are η q and η c Columns with the same index value, take and Calculate the intensity similarity score as follows: Among them, η q and η c Represent the intensity features between two point cloud descriptors, They represent the intensity similarity scores respectively; By comparing η c and η q Translation * Intensity characteristics after the column The final calculation result of intensity similarity is: n * Indicates a change in perspective; S204-3: Consistency Verification The average of the similarities between the point cloud descriptors of the past N frames of the current frame and the point cloud descriptors of the past N frames of the K candidate frames is used as the final similarity, which is used as the verification standard for consistency. The calculation formula is as follows: where Θ(p q ,p c ) represents the average similarity between the point cloud descriptor of the past N frames of the current frame and the point cloud descriptor of the past N frames of a frame among the K candidate frames, p q Indicates the current frame, p c represents a frame among K candidate frames, d q-i , d c-i , η q-i , η c-i Represent the geometric information and intensity information in the global descriptor of the point cloud of the current frame and the candidate frame respectively; When the mobile robot visits in the opposite direction, that is, when the sensor observation angle changes, Ω q-i Change to Ω accordingly q+i , use the obtained K objects to be matched to build a K nearest neighbor search tree, compare the descriptor of the target point cloud with that in the search tree, and use formula (72) to measure the similarity between them; finally, select the most similar candidate point by setting a threshold and use it as the revisited position, which is expressed as: Where C represents the index value of K objects to be matched in the search tree. is the threshold value set; c * Represents the final closed-loop position index value, and obtains the pose T of the loop frame from the historical key frame c , and used it for subsequent construction of closed-loop factors.

Images