A sliding window optimization method for visual-inertial point cloud fusion SLAM system

Abstract

A sliding window optimization method for a visual inertial point cloud fusion SLAM system belongs to the field of SLAM technology. The present invention relates to three sensors: a camera, an IMU, and a LiDAR, and designs a local optimization method for a multi-sensor information fusion SLAM system, which solves the problem of low positioning accuracy caused by the uncertainty of the depth of image feature points. The present invention constructs a measurement constraint relationship of three types of sensor information tightly coupled in a sliding window, updates the observation constraints when the window slides, ensures the timeliness and accuracy of the information in the sliding window, and helps to improve the positioning accuracy; the depth information of the image feature points is estimated by the LiDAR point cloud, the uniqueness of each feature point is maintained in the sliding window, and the influence of the "ghost" feature point on the visual repositioning is eliminated; the method does not change the size of the sliding window, can ensure fast local optimization, and ensures the fast positioning response capability 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 plays a vital role in the field of mobile robots, enabling mobile robots to achieve autonomous positioning and environmental perception. By carrying a variety of sensor devices, such as visual cameras, IMU (inertial measurement units), LiDAR (3D laser radar), etc., the carrier can obtain multi-type and multi-dimensional measurement information, which makes up for the limitations of single sensor measurement information; through the fusion positioning of multi-source information, the accuracy of SLAM system observation of environmental information and the accuracy of SLAM system state estimation can be improved. According to the classification of information fusion schemes, it can be divided into two categories: tight coupling and loose coupling. The loose coupling scheme only processes the data of each sensor, and the subsystems of each sensor are independent of each other, resulting in more data information loss and low result accuracy; while the tight coupling scheme requires accurate modeling of each type of sensor, and the information of each sensor is coupled and processed uniformly, with high data utilization and high result accuracy. According to the information fusion method, it can be divided into filtering-based method and optimization-based method. The filtering-based method assumes the positioning problem as a maximum a posteriori probability estimation problem through the relationship between the state model and the observation model; the optimization-based method is implemented according to the graph optimization theory, constructs the objective function through the factor graph model, iteratively solves the state quantity information, and performs batch updates; for SLAM systems that fuse three or more types of sensor information, due to the large amount of information and the complexity of various types of information, graph optimization methods are usually used to achieve close fusion of information.

[0003] Mobile robot SLAM needs to have fast positioning response capability. State optimization in a local range is a feasible solution. The sliding window method is a classic local area screening optimization method. By selecting a certain number of key frames to form a sliding window, the constraint relationship between key frames is established in the sliding window area, and batch optimization is performed through the factor graph model, so as to achieve global update of the state of each key frame in the sliding window. It not only makes full use of the observation constraints between key frames in the local range, but also takes into account the rapidity of the SLAM system. For the sliding window optimization method, typical ones include "Keyframe-based Visual-inertial Odometry using Nonlinear Optimization" by Leutenegger Stefan et al. of Imperial College London, and "Towards High-Performance Solid-State-LiDAR-Inertial" by Kailai Li et al. of Karlsruhe Institute of Technology. These methods successfully fuse the information of the two types of sensors, and use the Schur complement theory to update the key frames in the sliding window. The accuracy of the sliding window optimization is determined by the quality of the key frames and the error constraint relationship. However, the existing sliding window optimization method is not enough to be applied to the SLAM system that fuses the information of the three sensors of camera, IMU, and LiDAR. It is necessary to redesign the observation constraints between the key frames in the sliding window and update the marginal constraint information after the sliding window. This is also the problem that the present invention needs to solve, in order to realize the sliding window optimization method of the visual inertial point cloud fusion SLAM system with strong robustness and high accuracy. Summary of the invention

[0004] The present invention aims to solve the problem of low precision of the sliding window optimization method of the back end of the existing visual inertial SLAM system, and now provides a sliding window optimization method for a visual inertial point cloud fusion SLAM system.

[0005] The present invention provides a sliding window optimization method for a visual-inertial point cloud fusion SLAM system, comprising the following steps:

[0006] Step 1: When the back-end module of the visual-inertial point cloud fusion SLAM system subscribes to the latest frame of the robot released by its front-end module, it determines whether the latest frame is a key frame. If so, it executes step 2. Otherwise, it retains the IMU measurement information corresponding to the latest frame and then repeats step 1.

[0007] Step 2: Determine whether the sliding window is full. If so, marginalize the oldest key frame in the sliding window, update the sliding window LiDAR point cloud map, and then execute step 3. Otherwise, execute step 4.

[0008] Step 3: Reconstruct the constraint relationship of the sliding window according to the Schur complement theory, and then execute step 4;

[0009] Step 4: Add the latest key frame to the sliding window, perform motion distortion correction on the latest frame LiDAR point cloud according to the IMU measurement data, and then perform ground segmentation on the latest frame LiDAR point cloud in the LiDAR coordinate system, remove the ground points in the latest frame LiDAR point cloud, and then execute step 5;

[0010] Step 5: According to the IMU measurement data, the latest frame LiDAR point cloud without ground points is transformed into the world coordinate system, the LiDAR point cloud map in the sliding window is updated, the LiDAR point cloud in the sliding window is projected onto the image normalization plane of the latest frame, and then step 6 is executed;

[0011] Step 6: Estimate the depth of the visual feature point based on the LiDAR point cloud in the neighborhood of the visual feature point, and determine whether the visual feature point is co-visualized with other key frames in the sliding window. Otherwise, execute step 7. If it is, calculate the difference between the depth estimate of the visual feature point and the original depth value. If it is greater than the deviation threshold, delete the visual feature point. If it is not greater than the deviation threshold, take the average of the two depth values ​​as the depth value of the visual feature point, and then execute step 7.

[0012] Step 7: Determine whether the visual feature points in the key frame are traversed, if yes, update all the visual feature points in the sliding window, and then execute step 8, otherwise return to step 6;

[0013] Step 8: construct a visual reprojection constraint factor, a LiDAR odometer constraint factor, an IMU pre-integration constraint factor, and a marginalization prior constraint factor respectively, and add the visual reprojection constraint factor, the LiDAR odometer constraint factor, the IMU pre-integration constraint factor, and the marginalization prior constraint factor to the sliding window factor graph optimization model to construct an objective function;

[0014] Step nine: Optimize the objective function using a nonlinear optimization algorithm, update the state of the robot in the sliding window, and complete the sliding window optimization.

[0015] Furthermore, the above step 3 reconstructs the constraint relationship of the sliding window according to the Schur complement theory, including:

[0016] The state variables χ of all key frames in the sliding window are expressed as incremental equations as follows:

[0017]

[0018] in, is the state vector to be marginalized, is the retained state vector, H a for The covariance matrix, H c for The covariance matrix of Indicates H b The transpose of H b for and The covariance matrix between d and b s They are and A constant vector of ;

[0019] According to the Schur complement theory, Gaussian elimination is performed on the incremental equation of the state variable χ and the following is derived:

[0020]

[0021] Then, according to the above formula, the retained state vector is obtained Realize the reconstruction of sliding window constraint relationship.

[0022] Furthermore, the above step 4 performs ground segmentation on the LiDAR point cloud of the latest frame to remove ground points in the LiDAR point cloud of the latest frame, including:

[0023] The present invention uses a ray-based point cloud ground segmentation method. The idea of ​​this method is to perform fan-shaped segmentation of the point cloud based on the laser beam ray. The laser beams of every two adjacent rows in the same column form an inclination angle with the ground, and the ground points are screened out according to the inclination angle threshold; then, the LiDAR point with the minimum height is used as a reference, and the ground points are further screened out according to the height threshold.

[0024] First, the multi-line LiDAR point cloud is sorted, and the laser beams of the LiDAR points in adjacent rows of the same column in the sorted point cloud are used to calculate the ground inclination angle β. Set the inclination angle threshold of the ground segmentation to β ts , the height threshold is h ts , mark the LiDAR points with tilt angle or height difference less than the threshold as ground points, and then separate the ground points from the non-ground points. The judgment expression of ground point marking is as follows:

[0025]

[0026] In the above formula, 0 represents a ground point, 1 represents a non-ground point, and p i is the i-th LiDAR point, β iis the inclination angle between the laser beam of the i-th LiDAR point and the LiDAR point in the previous row and the ground, z min is the minimum height value of all LiDAR points in the current frame LiDAR point cloud, z i is the height value of the i-th LiDAR point.

[0027] Furthermore, the estimating the depth of the visual feature point in the above step 6 includes:

[0028] First, the LiDAR point cloud in the sliding window is projected onto the image normalization plane. A fixed neighborhood is defined for a visual feature point, and the corresponding LiDAR point in the sliding window LiDAR point cloud map is found through the projection points of all LiDAR points in the neighborhood.

[0029] Then, the depth data of all LiDAR points in the neighborhood are inserted into the histogram to calculate the average depth value and variance of all LiDAR points. Check whether the number of LiDAR points in the area with twice the variance of the average depth value accounts for more than a certain threshold, such as 70%. If so, take the average value of the depth of the LiDAR points in this area, and stipulate that this depth value is the depth value of the visual feature point. Otherwise, the visual feature point cannot obtain the depth information and is deleted.

[0030] Furthermore, in the above step 6, it is determined whether the visual feature point is co-viewed with other key frames in the sliding window. If so, the difference between the depth estimation of the feature point in the latest frame and the original depth value of the feature point in the sliding window is calculated. If it is greater than the deviation threshold, the visual feature point is deleted. If it is not greater than the deviation threshold, the average of the two depth values ​​is taken as the depth value of the visual feature point.

[0031] Furthermore, in the above step eight, the objective function expression is as follows:

[0032]

[0033] In the above formula, the state vector of the kth frame is and are the position, velocity and posture of the key frame at the kth frame, b a and b ω They are the accelerometer bias and gyroscope bias of IMU respectively; e C (x i ,x j )、e B (x i ,x j )、e L (M,F k ) and e MThey represent the visual reprojection constraint residual, IMU pre-integration constraint residual, LiDAR odometer constraint residual and marginalized prior constraint residual, respectively. i and x j are the state vectors of the i-th key frame and the j-th key frame respectively.

[0034] Furthermore, the LiDAR odometer constraint residual e L (M,F k ) is expressed as follows:

[0035]

[0036] In the above formula, F k is the feature point set of the latest frame, M is the sliding window LiDAR point cloud map; m represents the edge point set in the kth key frame The total number of edge points in the kth key frame; n represents the set of plane points in the kth key frame The total number of midplane points; Represents the local map M from the i-th edge point to the edge point in the point cloud of the k-th key frame e The line feature distance, Represents the local map M from the i-th plane point to the plane point in the point cloud of the k-th key frame p The surface feature distance.

[0037] Furthermore, the above marginalized prior constrained residual e M The expression is:

[0038]

[0039] In the above formula, is the state increment of all key frames in the sliding window,

[0040]

[0041] In the above formula, H a is the state vector to be marginalized The covariance matrix, H c The state vector is retained The covariance matrix of Indicates H b The transpose of H b for and The covariance matrix between d and b s They are and A constant vector of .

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

[0043] The present invention utilizes three sensors of different sources and types, namely camera, IMU and LiDAR, to perform information tight coupling, and proposes a sliding window optimization method for a visual-inertial point cloud fusion SLAM system.

[0044] The present invention constructs a measurement constraint relationship of three types of tightly coupled sensor information in a sliding window, updates the observation constraints when the window slides, ensures the timeliness and accuracy of the information in the sliding window, and helps improve positioning accuracy. The method constructs a visual reprojection constraint factor, an IMU pre-integration constraint factor, a LiDAR odometer constraint factor, and a marginalization constraint factor generated by updating the sliding window in the sliding window, fuses multi-sensor information through a factor graph, and improves the accuracy of the mobile robot's posture estimation by increasing the constraint factors; at the same time, when the window slides, the present invention designs an update scheme for the constraint relationship in the sliding window to ensure the timeliness and accuracy of the constraint relationship in the sliding window, which helps improve positioning accuracy.

[0045] Furthermore, the present invention uses LiDAR point cloud to estimate the depth information of image feature points, and only retains reliable LiDAR point cloud data by performing ground segmentation and statistical histogram grouping on the LiDAR point cloud to estimate the depth of image feature points; by fusing the common view feature points in the sliding window, the uniqueness of the feature points in the sliding window is guaranteed, the influence of the "ghost" feature points on visual relocation is eliminated, and the observation accuracy is guaranteed. Furthermore, the present invention does not change the size of the sliding window, and the number of key frames to be optimized that can be accommodated in the sliding window is fixed, which can ensure fast local optimization and the fast positioning response capability of the SLAM system. BRIEF DESCRIPTION OF THE DRAWINGS

[0046] Figure 1 A flowchart of a sliding window optimization method for a visual-inertial point cloud fusion SLAM system described in a specific implementation manner;

[0047] Figure 2 Schematic diagram of marginalizing the oldest keyframe in the sliding window of the visual-inertial point cloud fusion SLAM system;

[0048] Figure 3 This is a schematic diagram of the LiDAR point cloud ordering processing principle;

[0049] Figure 4 Schematic diagram of the tilt angles between the laser beams of adjacent row indices and the ground when the column indices are the same;

[0050] Figure 5 This is a schematic diagram of the relationship between the measured distance and true depth of the LiDAR point;

[0051] Figure 6is a schematic diagram of the neighborhood box of image feature points containing different LiDAR lines;

[0052] Figure 7 is an instance of the statistical histogram of the LiDAR points in the neighborhood box of an image feature point;

[0053] Figure 8 Schematic diagram of the sliding window factor graph optimization model for visual-inertial point cloud fusion;

[0054] Fig. 9 It is a carrier motion trajectory diagram estimated by the SLAM system implemented based on the present invention on the Handle data set;

[0055] Fig.10 The environmental map is constructed on the Handle data set by the SLAM system implemented based on the present invention. DETAILED DESCRIPTION

[0056] The following will be combined with the accompanying drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, rather than all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work belong to the scope of protection of the present invention. It should be noted that the embodiments of the present invention and the features in the embodiments can be combined with each other without conflict.

[0057] Reference Figures 1 to 10 Specifically describing this embodiment, a sliding window optimization method for a visual-inertial point cloud fusion SLAM system described in this embodiment includes the following steps:

[0058] Step 1: Set the sliding window size to accommodate N number of key frames to be optimized.

[0059] Step 2: Use the back-end module of the SLAM system with visual-inertial point cloud fusion to subscribe to the posture status information of the mobile robot published by the front-end module. When the back-end module collects the latest frame, it determines whether the latest frame is a key frame based on the parallax between the image information of the latest frame and the previous key frame. If it is, execute step 3; otherwise, update the IMU measurement information and repeat step 2.

[0060] For the content of step 2, it is assumed that the sampling of the three sensors, camera, IMU, and LiDAR, has been hardware synchronized, and the three sensors obtain image information, inertial information, and LiDAR point cloud information respectively. Among them, IMU is used to measure the state of the carrier itself, and camera and LiDAR are used to observe the surrounding environment information of the carrier. Since the measurement data of IMU and LiDAR are relatively accurate, and the image is prone to ghosting, this method selects key frames based on the quality of the image. If the parallax between the current frame and the previous key frame is greater than the threshold, the current frame is selected as the key frame, and the IMU and LiDAR data synchronously collected by the current frame are used as the inertial information and LiDAR point cloud information of the key frame.

[0061] Step 3: Determine whether the sliding window is full. If the window is full, marginalize the oldest keyframe in the sliding window, directly delete the IMU constraint relationship of the oldest keyframe, delete the LiDAR point cloud data of the oldest keyframe from the sliding window LiDAR point cloud map, and store the visual constraint relationship of the oldest keyframe in other keyframes in the sliding window. According to the Schur complement theory, reconstruct the constraint relationship of the sliding window, then add the latest keyframe to the sliding window, and then execute step 4.

[0062] If the window is not full, the latest key frame is directly added to the sliding window, and then step 4 is executed.

[0063] For the content of step 3, the update function of the key frame in the sliding window is implemented, including the sliding out of the oldest key frame in the sliding window and the sliding in of the latest key frame. Depending on whether the sliding window is full, it is decided whether to marginalize the oldest key frame in the sliding window, such as Figure 2 As shown in FIG. 1 , a schematic diagram of marginalizing the oldest key frame in a sliding window is given. The purpose of marginalizing the oldest key frame in a sliding window is to delete the oldest key frame in the sliding window area while retaining the key frame constraint relationship. The process of deleting a key frame from a sliding window while retaining the key frame constraint relationship is called marginalizing the key frame from the sliding window, which is usually implemented using the Schur complement method.

[0064] The state variables χ of all key frames in the sliding window construct the least squares expression in the sliding window and are simplified to In the form of, H is the system covariance matrix, δ χ is the increment of the state quantity, and b is a constant vector. The state variable χ is divided into two categories, namely, the state quantity to be marginalized and the amount of state that needs to be retained Split the system covariance matrix H into four block matrices H a , H c , H b , H a for The covariance matrix of H is the marginalized variable; c for The covariance matrix of , that is, the covariance matrix of the retained state variables; Indicates H b The transpose of H b for and The covariance matrix between .

[0065] The state variable χ is expressed as an incremental equation as follows:

[0066]

[0067] Among them, b d and b s They are and A constant vector of .

[0068] According to the Schul complement theory, without calculating the state quantity to be marginalized In the case of Performing Gaussian elimination, we get:

[0069]

[0070] in, Indicates H a In H b According to the above formula, we can deduce for:

[0071]

[0072] The state quantity in the above formula is only The constraint information of the marginalized keyframes is retained, and the state quantity of the marginalized keyframes is eliminated without losing accuracy. During the marginalization process, although the state variables are constantly updated, the linearization point is fixed by optimizing the consistency strategy, that is, when calculating the marginalized Jacobian matrix, the value of the derivative variable is fixed, instead of using the state variables updated after each iteration to calculate the Jacobian, so as to ensure the unobservable dimension of the system and avoid changes in the null space of the covariance matrix.

[0073] Step 4: According to the IMU measurement data, motion distortion correction is performed on the LiDAR point cloud of the latest frame, and ground segmentation is performed on the LiDAR point cloud of the latest frame to remove ground points.

[0074] For step 4 of performing ground segmentation on the latest frame of the LiDAR point cloud to remove ground points, the present invention uses a ray-based point cloud ground segmentation method. The idea of ​​this method is to perform fan-shaped segmentation on the point cloud based on the laser beam ray, and the laser beams of every two adjacent rows in the same column form an inclination angle with the ground, and the ground points are screened out according to the inclination angle threshold; then, the LiDAR point with the minimum height is used as a reference, and the ground points are further screened out according to the height threshold.

[0075] The point cloud of multi-line LiDAR is processed in an orderly manner, such as Figure 3 As shown in the figure, a schematic diagram of the point cloud ordering processing principle is given. In the polar coordinate system of LiDAR, the coordinates of the i-th LiDAR point in the point cloud are set to p i (α i ,θ i ,R i ), α, θ and R are the pitch angle, azimuth angle and measured depth value of the LiDAR point respectively. The row index and column index expressions of the LiDAR point are:

[0076]

[0077] In the above formula, r_angle is the vertical angle resolution of LiDAR, and hr_angle is the horizontal angle resolution. Figure 3 As shown, the row index number starts with the horizontal laser beam as the index, the row index number is set to 0, the pitch angle is greater than 0 is the positive index number, represented by a positive number, the pitch angle is less than 0 is the reverse index number, represented by a negative number; the starting index of the column index number is on the coordinate axis and is sorted in a counterclockwise direction.

[0078] The LiDAR point cloud is fanned out according to the column index. For point clouds with negative row index numbers, the inclination angle β of the ground is calculated using the laser beams of the LiDAR points in adjacent rows of the same column in the ordered point cloud according to the order of row index numbers from small to large, as follows: Figure 4 As shown in the figure, a schematic diagram of the tilt angle between the laser beams of adjacent row indices and the ground is given when the column indices are the same. Figure 4 , the calculation expression of the inclination angle is given:

[0079]

[0080] In the above formula, r i and r i +1 indicates the line index number of the laser beam, and is the pitch angle of the laser beam, and is the adjacent row r i and r i +1 for the measured distance of the LiDAR point.

[0081] Set the tilt angle threshold for ground segmentation to β ts , the height threshold is h ts , mark the LiDAR points with tilt angle or height difference less than the threshold as ground points, and then separate the ground points from the non-ground points. The judgment expression of ground point marking is as follows:

[0082]

[0083] In the above formula, 0 represents a ground point, 1 represents a non-ground point, and z min is the minimum height value of all LiDAR points in the current frame LiDAR point cloud, z i is the height value of the i-th LiDAR point. It should be noted that the two laser beams form an inclination angle with the ground. If the inclination angle is less than the inclination angle threshold, the LiDAR points corresponding to the two laser beams are marked as ground points.

[0084] Step 5: According to the IMU measurement information, the latest frame LiDAR point cloud without the ground points is transformed into the world coordinate system, the LiDAR point cloud map in the sliding window is updated, the LiDAR point cloud in the sliding window is projected onto the image normalization plane of the latest frame, and then step 6 is executed;

[0085] Step 6: Estimate the depth value of each feature point in the image information of the latest keyframe based on the LiDAR point cloud information. Determine whether each feature point can be observed by other keyframes in the sliding window one by one. Otherwise, execute step 7. If yes, calculate the difference between the depth estimate of the feature point in the latest keyframe and the original depth value of the feature point in the sliding window. If it is greater than the deviation threshold, delete the visual feature point. If it is not greater than the deviation threshold, take the average of the two depth values ​​as the depth value of the feature point, and then execute step 7.

[0086] For the content of step 6, the depth information of the image feature points is estimated based on the LiDAR point cloud information. The LiDAR point depth calculation expression is:

[0087] Z=Rcosα,

[0088] In the above formula, R is the measured distance of the LiDAR point, and α is the pitch angle of the LiDAR ray. Figure 5 As shown in the figure, a schematic diagram of the relationship between the measured distance and the true depth of the LiDAR point is given.

[0089] Take a neighborhood box of the feature point, which is required to contain the point cloud information of different LiDAR lines of LiDAR, such as Figure 6 As shown in Figure 1, a schematic diagram of the neighborhood box of image feature points containing different LiDAR lines is given. Assume that the total number of LiDAR points in the neighborhood box is N all, calculate the mean μ and variance σ of the depth values ​​of all LiDAR points in the neighborhood box, then group the depth values ​​of all LiDAR points in the neighborhood box into statistical histograms, and count the number of LiDAR points N within the range of twice the mean μ and variance σ μ , according to the expression Filter out the neighborhood boxes whose depth values ​​of LiDAR points approximately follow Gaussian distribution, such as Figure 7 As shown, an example of a statistical histogram of depth values ​​of LiDAR points within the neighborhood box of image feature points is given.

[0090] Then take this N μ The depth values ​​of the LiDAR points are calculated, and the mean depth value is recalculated as the depth value of the feature point. If the above expression is not satisfied, the feature point is deleted. Through the screening process in this step, it can be ensured that the LiDAR points in the neighborhood box of the feature point basically fall on the same plane, eliminating the possibility that the LiDAR points in the neighborhood box fall on the fault plane.

[0091] Further, it is determined whether the feature point in the latest key frame can be observed by other key frames in the sliding window, that is, it is determined whether the latest key frame and other key frames in the sliding window have a common view of the feature point. Otherwise, step 7 is executed. If it is, the difference between the depth estimate of the feature point in the latest frame and the original depth value of the feature point in the sliding window is calculated. If it is greater than the deviation threshold, the visual feature point is deleted. If it is not greater than the deviation threshold, the average of the two depth values ​​is taken as the depth value of the visual feature point, and then step 7 is executed. Through the common view feature point processing in this step, the uniqueness of the feature points in the sliding window can be guaranteed, the influence of the "ghost" map points on the visual reprojection is eliminated, and the accuracy of the visual constraint relationship is improved.

[0092] Step 7: Determine whether the visual feature points of the latest key frame are traversed, otherwise return to step 6; if yes, execute step 8.

[0093] Step 8: Construct the visual reprojection constraint factor based on the visual reprojection perspective relationship; construct the IMU pre-integration constraint factor based on the IMU pre-integration theory; construct the LiDAR odometer constraint factor based on the matching relationship between the LiDAR point cloud and the sliding window point cloud; construct the prior constraint factor based on the marginalization processing result of the updated sliding window. Add the visual constraint factor, IMU pre-integration constraint factor, LiDAR odometer constraint factor and marginalization prior constraint factor to the sliding window factor graph model, and then construct the optimization objective function.

[0094] like Figure 8As shown, a schematic diagram of the sliding window factor graph optimization model constructed with four constraint factors is given. According to the sliding window factor graph optimization model, the optimization objective function is established according to the tightly coupled bundle adjustment method. By minimizing the sum of the visual reprojection constraint residual, the IMU pre-integration constraint residual, the LiDAR odometer constraint residual and the marginalized prior constraint residual, the maximum a posteriori probability estimate of the state is obtained, which is in the following form:

[0095] The state variables of all key frames in the sliding window are expressed as N is the total number of all key frames in the sliding window, and the objective function expression is as follows:

[0096]

[0097] In the above formula, the state vector of the kth frame is and are the position, velocity and posture of the key frame at the kth frame, b a and b ω They are the accelerometer bias and gyroscope bias of the IMU respectively. and They are the position and rotation of the camera relative to the IMU, and are the position and rotation of LiDAR relative to IMU; e C (x i ,x j )、e B (x i ,x j )、e L (M,F k ) and e M They represent the visual reprojection constraint residual, IMU pre-integration constraint residual, LiDAR odometer constraint residual and marginalized prior constraint residual, respectively. i and x j are the state vectors of the i-th key frame and the j-th key frame respectively.

[0098] The visual reprojection constraint factor is constructed based on the reprojection error. When a map point in space is projected onto each keyframe that can observe the map point, the coordinates of the projected point should coincide with the observed coordinates of the map point. The reprojection error of the two types of coordinates is the residual of the visual reprojection constraint factor. In the SLAM system with visual-inertial point cloud fusion, the state quantity χ to be optimized for the visual reprojection constraint factor is c It is expressed as:

[0099]

[0100] in, and are the positions of the camera in the i-th frame and the j-th frame respectively, and are the postures of the camera in 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 They are the position and rotation of the camera relative to the IMU.

[0101] Assume that there is a map point P in the space, its index number is l, and its projection coordinates on the normalized plane of the image in the jth frame are The observation coordinates in the jth frame are Then there is the visual reprojection constraint residual e C (x i ,x j ) is expressed as follows:

[0102]

[0103] In the above formula, It can be obtained from the estimated model of reprojection, and the expression is:

[0104]

[0105] In the above formula, It represents the observation value of the map point P on the normalized plane of the i-th frame when it is first observed in the i-th frame. is the rotation from the IMU coordinate system to the camera coordinate system, is the rotation from the world coordinate system to the j-th frame IMU coordinate system, is the rotation amount from the IMU coordinate system to the world coordinate system in the i-th frame, is the rotation from the camera coordinate system to the IMU coordinate system, and λ is the depth of the visual feature point, which is obtained by step 6.

[0106] The IMU pre-integration constraint factor is used to describe the relative position change relationship of the IMU measurement information between the time difference of two key frames. Align the i and j moments of the IMU sampling with the kth and k+1th frames of the key frame, and the variable to be optimized of the IMU pre-integration factor is expressed as:

[0107]

[0108] in, and t i Time and t j The speed of IMU at the moment, b ai and b aj t i Time and t j The accelerometer bias of the IMU at the moment, and t i Time and t j The gyroscope bias of the IMU at time t i Time and t j The moments are the sampling moments of the key frames i-th frame and j-th frame respectively.

[0109] The j The position of the IMU at this moment speed and posture Obtained by the following formula:

[0110]

[0111] In the above formula, Δt is the time interval between the i-th frame and the j-th frame, t i ≤t≤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, The position of the carrier at the time t in the IMU coordinate system is taken as the reference, and the carrier is relative to t i The amount of rotation at that moment.

[0112] and They are the accelerometer bias and gyroscope bias of the IMU at time t respectively.

[0113] and Respectively represent the acceleration and angular velocity output by the IMU at time t, and Respectively represent the acceleration noise and angular velocity noise of the IMU measurement information at time t, and assume that they are white noises that obey Gaussian distribution. Transform the above formula, transform the reference coordinate system of the position, velocity and rotation from the world coordinate system to the body coordinate system of the i-th frame, and we can get:

[0114]

[0115] In the above formula, That is, the IMU pre-integrated variables of position, velocity, and attitude, and the expressions are as follows:

[0116]

[0117] In the above formula, represents time t relative to t i IMU pre-integrated variables of the robot posture at time t.

[0118] It can be seen that the state quantity at the jth frame is only related to the measurement value of the IMU and will not be affected by the state quantity at the i-th frame, 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 ) is expressed as follows:

[0119]

[0120] 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 is the sampling time of the i-th frame and the j-th frame of the key frame, is the quaternion multiplication symbol, [·] xyz It means taking the imaginary part of the quaternion to form a three-dimensional vector. From the world coordinate system to t i The rotation of the IMU coordinate system at the moment (quaternion representation), From t i The IMU coordinate system at time t j The rotation of the IMU coordinate system at this moment (in quaternion notation).

[0121] The LiDAR odometry constraint factor is constructed based on the distance difference between the plane point of the latest frame LiDAR point cloud and the plane feature of the sliding window LiDAR point cloud map, and the distance difference between the edge point of the latest frame LiDAR point cloud and the line feature of the sliding window LiDAR point cloud map. The LiDAR point cloud in space can be classified into edge points and plane points according to smoothness. Line features can be constructed from edge points, and surface features can be constructed from plane points. The line feature matching error and surface feature matching error between the latest frame and the sliding window LiDAR point cloud map are used as the LiDAR odometry constraint residual.

[0122] Let the index number of the latest frame be k, and the feature point set of the latest frame be F k , including the edge point set and the plane point set The sliding window LiDAR point cloud map is M, including the edge point feature local map M e and local map M of plane point features p , the distance from the edge point of the latest frame to the edge point feature of the point cloud map is expressed as d e, the distance from the plane point of the latest frame to the surface feature of the point cloud map is expressed as d p , the expression is as follows:

[0123]

[0124] In the above formula, Represents the local map M from the i-th edge point to the edge point in the point cloud of the k-th key frame e The line feature distance, Represents the local map M from the i-th plane point to the plane point in the point cloud of the k-th key frame p The surface feature distance. is the edge point set of the kth key frame The point in is the plane point set of the kth key frame The point in and yes At the edge point local map M e The two nearest neighbor points in and j and l are index numbers; and yes At the plane point local map M p The three nearest neighbor points in and j, l, m are index numbers.

[0125] The sum of all edge point-to-edge feature distances and the sum of all plane point-to-plane feature distances constitute the LiDAR odometry constraint residual of the latest frame, expressed as follows:

[0126]

[0127] In the above formula, m represents the edge point set in the kth key frame The total number of edge points in the kth key frame; n represents the set of plane points in the kth key frame The total number of midplane points.

[0128] The prior constraint factor is the constraint relationship that is retained after removing the marginalized keyframes when updating the sliding window. The least squares expression is constructed using the factor graph optimization model, and the state vector χ is set as a node, and the marginalized state vector χ d The pose information in the map points and the constraint relationship are added to the retained state vector χ in the form of prior information. s In the optimization relationship of , the loss of constraint information corresponding to the key frames that are marginalized after updating the sliding window is avoided. After marginalization, the prior matrix H pand the corresponding prior residual b p , which is consistent with the marginalization expression in step 3, which prior constrains the residual e M The expression is:

[0129]

[0130] In the above formula, is the state increment of all key frames in the sliding window,

[0131]

[0132] In the above formula, H a is the state vector to be marginalized The covariance matrix, H c The state vector is retained The covariance matrix of Indicates H b The transpose of H b for and The covariance matrix between d and b s They are and Constant vector.

[0133] Step 9: Use nonlinear optimization algorithms to optimize the objective function. Typical methods include Gauss-Newton method and Levenberg-Marquardt method. Taking Levenberg-Marquardt method as an example, by setting a confidence region for the state increment, the approximate failure problem caused by large changes in the state increment is successfully avoided. This approach not only effectively prevents the coefficient matrix of the linear equation system from having non-singular and ill-conditioned problems, but also ensures a more stable and accurate estimate of the state increment. The objective function is constructed in the form of a state increment equation:

[0134] (H h +μI)Δχ=g,

[0135] In the above formula, H h is the Hessian matrix, μ is the Lagrange multiplier, and the accuracy of the nonlinear quadratic approximation model is reflected by adjusting the size of the parameter μ, and then the optimal solution is obtained by iterative optimization.

[0136] Update the poses of all key frames in the sliding window, complete a sliding window optimization, and determine whether the optimization thread of the SLAM backend is terminated. If not, return to step 2; if it is terminated, end.

[0137] This implementation involves three sensors: camera, IMU, LiDAR, and designs a local optimization method for a multi-sensor information fusion SLAM system to solve the problem of low positioning accuracy caused by the uncertainty of the depth of image feature points. This implementation constructs a measurement constraint relationship of three types of sensor information tightly coupled in the sliding window, updates the observation constraints when the window slides, ensures the timeliness and accuracy of the information in the sliding window, and helps improve the positioning accuracy; by performing ground segmentation on the latest frame LiDAR point cloud and removing the ground point cloud, it helps to improve the accuracy of the map and reduce errors; by using the LiDAR point cloud to assist in estimating the depth information of the image feature points, the uniqueness of each feature point is maintained in the sliding window, and the influence of the "ghost" feature point on the visual repositioning is eliminated; this method does not change the size of the sliding window, can ensure fast local optimization, and ensure the fast positioning response capability of the SLAM system.

[0138] Implementation and verification of the present invention:

[0139] The present invention proposes a sliding window optimization method for a visual-inertial point cloud fusion SLAM system, which is implemented by programming in C++ language. The beneficial effects of the present invention are verified by evaluating the motion trajectory of the mobile robot estimated by the SLAM system and the environmental map constructed. A SLAM system based on visual-inertial point cloud fusion is designed, and the sliding window optimization method proposed in the present invention is implemented. The Handle data set is used as a verification data set. The data set connects the three sensors of camera, IMU and LiDAR together rigidly, and there is no relative motion between the sensors. The data is collected by walking in an urban environment with a handheld sensor assembly.

[0140] The Handle dataset is collected in an open urban environment, including buildings, trees, stationary or moving vehicles, pedestrians or cyclists. This is a typical and complex street environment. At the same time, the dataset is collected by handheld devices, and the data collectors are constantly walking, which inevitably causes the sensor equipment to sway, which also increases the difficulty of the Handle dataset and is a challenge for the accurate trajectory estimation and environment mapping of the SLAM system. Fig. 9 As shown in the figure, the carrier motion trajectory diagram estimated by the SLAM system based on the present invention on the Handle data set is given. The entire trajectory is smooth and continuous. Although the collector's walking route has many turns and the route is not straight, the estimated trajectory is continuous and effective. Since this method does not have loop detection, there is no overlap at the starting point and end point of the trajectory, which will produce a small drift phenomenon. Fig.10 As shown in FIG. 1 , the environment map constructed by the SLAM system implemented by the present invention on the Handle data set is given. Fig.10 and Fig. 9Correspondingly, the environment map is represented by edge points and plane points. The map is relatively clear and regular, without ghosting and obvious drift, which verifies the effectiveness of the sliding window optimization method of the visual-inertial point cloud fusion SLAM system proposed in the present invention.

[0141] In summary, the present invention uses three sensors of different sources and different types, namely, camera, IMU and LiDAR, to couple information tightly, and proposes a sliding window optimization method for visual inertial point cloud fusion SLAM system. By fusing the constraint information of each sensor, the fastening belt of the mobile robot posture estimation is improved, and the update rules of the constraint relationship in the sliding window are designed to ensure the timeliness and accuracy of the constraint relationship in the sliding window; by performing ground segmentation and statistical histogram grouping on the LiDAR point cloud, reliable point cloud data is screened out for depth estimation of image feature points, and then by fusing the common view feature points, the uniqueness of the feature points in the sliding window is guaranteed, and the influence of the "ghost" feature points on visual relocation is eliminated. The present invention does not change the size of the sliding window, and on the premise of ensuring the fast response capability of the sliding window optimization of the SLAM system, it takes into account the improvement of the observation accuracy of the sliding window and the accuracy of posture estimation.

[0142] Although the present invention is 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. It should therefore be understood that many modifications may be made to the exemplary embodiments and that other arrangements may be devised without departing from the spirit and scope of the present invention as defined by the appended claims. It should be understood that the various dependent claims and features described herein may be combined in a manner different from that described in the original claims. It should also be understood that the features described in conjunction with a single embodiment may be used in other described embodiments.

Claims

1. A sliding window optimization method for a visual-inertial point cloud fusion SLAM system, characterized in that: The following steps are involved: Step 1: When the back-end module of the visual-inertial point cloud fusion SLAM system subscribes to the latest frame of the robot released by its front-end module, it determines whether the latest frame is a key frame. If so, it executes step 2. Otherwise, it retains the IMU measurement information corresponding to the latest frame and then repeats step 1. Step 2: Determine whether the sliding window is full. If so, marginalize the oldest key frame in the sliding window, update the sliding window LiDAR point cloud map, and then execute step 3. Otherwise, execute step 4. Step 3: Reconstruct the constraint relationship of the sliding window according to the Schur complement theory, and then execute step 4; Step 4: Add the latest key frame to the sliding window, perform motion distortion correction on the latest frame LiDAR point cloud according to the IMU measurement data, and then perform ground segmentation on the latest frame LiDAR point cloud in the LiDAR coordinate system, remove the ground points in the latest frame LiDAR point cloud, and then execute step 5; Step 5: According to the IMU measurement data, the latest frame LiDAR point cloud without ground points is transformed into the world coordinate system, the LiDAR point cloud map in the sliding window is updated, the LiDAR point cloud in the sliding window is projected onto the image normalization plane of the latest frame, and then step 6 is executed; Step 6: Estimate the depth of the visual feature point based on the LiDAR point cloud in the neighborhood of the visual feature point, and determine whether the visual feature point is co-visualized with other key frames in the sliding window. Otherwise, execute step 7. If it is, calculate the difference between the depth estimate of the visual feature point and the original depth value. If it is greater than the deviation threshold, delete the visual feature point. If it is not greater than the deviation threshold, take the average of the two depth values ​​as the depth value of the visual feature point, and then execute step 7. Step 7: Determine whether the visual feature points in the key frame are traversed, if yes, update all the visual feature points in the sliding window, and then execute step 8, otherwise return to step 6; Step 8: construct a visual reprojection constraint factor, a LiDAR odometer constraint factor, an IMU pre-integration constraint factor, and a marginalization prior constraint factor respectively, and add the visual reprojection constraint factor, the LiDAR odometer constraint factor, the IMU pre-integration constraint factor, and the marginalization prior constraint factor to the sliding window factor graph optimization model to construct an objective function; Step nine: Optimize the objective function using a nonlinear optimization algorithm, update the state of the robot in the sliding window, and complete the sliding window optimization.

2. The sliding window optimization method for a visual-inertial point cloud fusion SLAM system according to claim 1, characterized in that: Step 3, reconstructing the constraint relationship of the sliding window according to the Schur complement theory, includes: The state variables χ of all key frames in the sliding window are expressed as incremental equations as follows: Among them, δχ d is the state vector to be marginalized, δχ s is the retained state vector, H a is δχ d The covariance matrix, H c is δχ s The covariance matrix of Indicates H b The transpose of H b is δχ d With δχ s The covariance matrix between d and b s are δχ d and δχ s A constant vector of ; According to the Schur complement theory, Gaussian elimination is performed on the incremental equation of the state variable χ and the following is derived: Then, according to the above formula, the retained state vector δχ is obtained s , realizing the reconstruction of sliding window constraint relationship.

3. The sliding window optimization method for a visual-inertial point cloud fusion SLAM system according to claim 1, characterized in that: Step 4 performs ground segmentation on the latest frame of the LiDAR point cloud to remove ground points in the latest frame of the LiDAR point cloud, including: Use a ray-based point cloud ground segmentation method; the idea of ​​this method is to perform fan-shaped segmentation of the point cloud based on the laser beam ray. The laser beams of every two adjacent rows in the same column form an inclination angle with the ground, and the ground points are screened out according to the inclination angle threshold; then, the LiDAR point with the minimum height is used as the reference, and the ground points are further screened out according to the height threshold; First, the multi-line LiDAR point cloud is segmented in an ordered manner, and the inclination angle β of the ground is calculated using the laser beams of the LiDAR points in adjacent rows of the same column in the ordered point cloud; the inclination angle threshold of the ground segmentation is set to β ts , the height threshold is h ts , mark the LiDAR points with tilt angle or height difference less than the threshold as ground points, and then separate the ground points from the non-ground points; the judgment expression of the ground point mark is as follows: In the above formula, 0 represents a ground point, 1 represents a non-ground point, and p i is the i-th LiDAR point, β i is the inclination angle between the laser beam at the i-th LiDAR point and the laser beam in the previous row and the ground, z min is the minimum height value of all LiDAR points in the current frame LiDAR point cloud, z i is the height value of the i-th LiDAR point.

4. The sliding window optimization method for a visual-inertial point cloud fusion SLAM system according to claim 1, characterized in that: Step 6, estimating the depth of the visual feature point, includes: First, the LiDAR point cloud in the sliding window is projected onto the image normalization plane, a fixed neighborhood is defined for a visual feature point, and the corresponding LiDAR point in the sliding window LiDAR point cloud map is found through the projection points of all LiDAR points in the neighborhood; Then, the depth data of all LiDAR points in the neighborhood are inserted into the histogram to calculate the average depth value and variance of all LiDAR points. Check whether the ratio of the number of LiDAR points in the area with the average depth value twice the variance to the total number of LiDAR points is greater than the preset threshold. If so, the depth of the LiDAR points in this area is averaged and this depth value is defined as the depth value of the visual feature point. Otherwise, the depth information of the visual feature point cannot be obtained and the visual feature point is deleted.

5. The sliding window optimization method for a visual-inertial point cloud fusion SLAM system according to claim 1, characterized in that: In step 6, it is determined whether the visual feature point is in common view with other key frames in the sliding window. If so, the difference between the depth estimation of the feature point in the latest frame and the original depth value of the feature point in the sliding window is calculated. If it is greater than the deviation threshold, the visual feature point is deleted. If it is not greater than the deviation threshold, the average of the two depth values ​​is taken as the depth value of the visual feature point.

6. The sliding window optimization method for a visual-inertial point cloud fusion SLAM system according to claim 1, characterized in that: In step nine, the objective function expression is as follows: In the above formula, the state vector of the kth frame is and are the position, velocity and posture of the key frame at the kth frame, b a and b ω They are the accelerometer bias and gyroscope bias of IMU respectively; e C (x i ,x j ), e B (x i ,x j ), e L (M,F k ) and e M They represent the visual reprojection constraint residual, IMU pre-integration constraint residual, LiDAR odometer constraint residual and marginalized prior constraint residual, respectively. i and x j are the state vectors of the i-th key frame and the j-th key frame respectively.

7. The sliding window optimization method for a visual-inertial point cloud fusion SLAM system according to claim 6, characterized in that: The LiDAR odometry constraint residual e L (M,F k ) is expressed as follows: In the above formula, F k is the feature point set of the latest frame, M is the sliding window LiDAR point cloud map; m represents the edge point set in the kth key frame The total number of edge points in the kth key frame; n represents the set of plane points in the kth key frame The total number of midplane points; Represents the local map M from the i-th edge point to the edge point in the point cloud of the k-th key frame e The line feature distance, Represents the local map M from the i-th plane point to the plane point in the point cloud of the k-th key frame p The surface feature distance.

8. The sliding window optimization method for a visual-inertial point cloud fusion SLAM system according to claim 6, characterized in that: The marginalized prior constrains the residual e M The expression is: e M =b p -H p δχ 1:N , In the above formula, δχ 1:N is the state increment of all key frames in the sliding window, In the above formula, H a is the state vector δχ to be marginalized d The covariance matrix, H c is the retained state vector δχ s The covariance matrix of Indicates H b The transpose of H b is δχ d With δχ s The covariance matrix between d and b s are δχ d and δχ s A constant vector of .

Images