SLAM (Simultaneous Localization and Mapping) method and system based on visual inertial guidance and laser cascade registration

Abstract

The invention discloses an SLAM (Simultaneous Localization and Mapping) method and system based on visual inertial guidance and laser cascade registration, and belongs to the technical field of environmental perception, and the method comprises the following steps: acquiring multi-source data, predicting a pose by adopting an IMU (Inertial Measurement Unit) pre-integration method, taking the pose as an initial value, and outputting a robot visual-inertial pose by combining a camera image and utilizing a visual-inertial odometer technology; constructing a visual inertia factor; taking the vision-inertia pose as an initial value of registration, performing multi-stage registration on the input laser radar point cloud frame, outputting a laser estimation pose, and constructing a laser factor; visual loopback detection and laser loopback detection are respectively carried out, an effective closed loop is determined according to the space-time consistency of the two kinds of loopback detection, loopback factors are constructed, a factor graph is constructed, graph optimization is executed, and globally consistent tracks and map points are obtained. According to the method, the real-time performance is maintained, the registration convergence is improved, and the method is suitable for positioning and mapping in a complex degraded scene.

Description

Technical Field

[0001] This invention belongs to the field of environmental perception technology, specifically relating to a SLAM method and system based on visual inertial guidance and laser cascade registration. Background Technology

[0002] With the widespread application of intelligent systems such as mobile robots, autonomous vehicles, drones, and augmented reality devices, the requirements for environmental perception and autonomous positioning accuracy are constantly increasing. Simultaneous Localization and Mapping (SLAM) technology, as a core means of achieving autonomous navigation, can simultaneously build maps and estimate its own pose in unknown environments, and has become a fundamental research direction in the field of intelligent perception and navigation.

[0003] Most existing SLAM systems are based on a single sensor. Visual SLAM relies on image features acquired by a camera for pose estimation, offering advantages such as compact structure and high update frequency. However, in environments with weak texture, varying lighting, or motion blur, feature point extraction and matching are easily affected, leading to unstable estimation or even loss. LiDAR SLAM, on the other hand, relies on the geometric features of lidar point clouds for spatial registration, offering high ranging accuracy and scale consistency. However, in geometrically degraded scenarios (such as long corridors, tunnels, and open planes), the point cloud features lack sufficient constraints, easily resulting in registration drift or accumulation of local errors.

[0004] To overcome the limitations of single-modality SLAM, multi-sensor fusion SLAM methods have been proposed. These methods improve the robustness and positioning accuracy of the system by complementing information from multiple sources, such as vision, inertial measurement units (IMUs), and lidar. However, existing multi-modal fusion SLAM systems still face the following technical challenges in complex dynamic or degrading environments: When visual information is obstructed, blurred, or interfered with by lighting, the feature extraction and matching results are unstable, leading to amplified front-end pose estimation errors. Laser registration algorithms (such as ICP and NDT) are highly dependent on initial pose estimation, and when the initial value error is large, they are prone to getting trapped in local optima. The real-time performance of multimodal fusion is limited by complex registration and optimization calculations, making it difficult to run stably in high-frequency scenarios. Most existing loop closure detection modules are based on single-modal matching, which are prone to false or missed loop closures in changing viewpoints or dynamic environments. Summary of the Invention

[0005] This invention addresses the shortcomings of existing technologies by providing a SLAM method and system based on visual inertial guidance and laser cascade registration, which improves registration convergence while maintaining real-time performance and is suitable for localization and mapping in complex and degraded scenarios.

[0006] This invention provides the following technical solution: Firstly, a SLAM method based on visual inertial guidance and laser cascade registration is provided, including: Acquire and align multi-source data, including: camera images, lidar point cloud data, and IMU data; Based on IMU data, the pose is predicted using the IMU pre-integration method. This predicted pose is used as the initial value. Combined with camera images, the visual-inertial odometry technology is used to output the robot's visual-inertial pose. The visual-inertial factor is constructed based on the pose constraint relationship of the visual-inertial pose. Using the visual-inertial pose as the initial value for registration, multi-stage registration is performed on the input lidar point cloud frame to output the estimated lidar pose, and the lidar factor is constructed based on the pose constraint relationship of the estimated lidar pose. Visual loop closure and laser loop closure are performed by using the feature similarity between historical keyframes and the current frame. The effective loop closure is determined based on the spatiotemporal consistency of the two loop closure results, and the loop closure factor is constructed based on the loop closure constraint relationship of the effective loop closure. Using the robot's pose, velocity, and IMU bias as state variables, and visual inertial factors, laser factors, and loop closure factors as factor nodes, a factor graph is constructed and graph optimization is performed to obtain a globally consistent trajectory and map points.

[0007] Optionally, the step of using the predicted pose as an initial value, combining it with camera images, and outputting the robot's visual-inertial pose using visual-inertial odometry technology specifically involves: Pre-integrate the IMU data between adjacent image frames to obtain the inter-frame predicted pose; The inter-frame predicted pose is used as the initial value for optical flow feature matching. The Lucas-Kanade method based on the pyramid structure is used to extract and match features from adjacent image frames. During the matching process, abnormal feature matching points are removed. The constraints of inter-frame predicted pose and the visual reprojection constraints after removing anomalous feature matching points are jointly constructed into a nonlinear least squares optimization problem, and the visual-inertial pose is obtained by solving it.

[0008] Optionally, the multi-stage registration includes two-stage registration or three-stage registration. The two-stage registration includes local fine registration and probabilistic optimization. The three-stage registration includes global coarse registration, local fine registration, and probabilistic optimization. Before performing multi-stage registration on the input LiDAR point cloud frame, the confidence level corresponding to the robot's visual-inertial pose output by the visual-inertial odometry is obtained. When the confidence level is lower than a set threshold, a three-stage registration method is used to perform multi-stage registration on the input LiDAR point cloud frame. When the confidence level is higher than or equal to the set threshold, a two-stage registration method is used to perform multi-stage registration on the input LiDAR point cloud frame.

[0009] Optionally, a three-stage registration method is used to perform multi-stage registration on the input lidar point cloud frame, and the estimated lidar pose is output, specifically as follows: The Super4PCS algorithm is used to perform global coarse registration between the current frame point cloud and the historical keyframe point cloud to obtain the initial pose transformation. Using the initial pose transformation as the initial value, the ICP algorithm based on point-to-surface constraints is used to perform local fine registration of the current frame point cloud and the historical key frame point cloud to obtain the rigid body transformation matrix. Based on the rigid body transformation matrix, a probability density model is constructed using the NDT algorithm for fine optimization, and the laser estimated pose is output.

[0010] Optionally, when performing multi-stage registration on the input LiDAR point cloud frames, an adaptive trigger registration stage backoff strategy is introduced; specifically, the ICP convergence residual is detected in real time, and when the residual of two consecutive frames exceeds the threshold, it automatically backoffs to Super4PCS global registration.

[0011] Optionally, the step of performing visual loop closure detection and laser loop closure detection based on the feature similarity between historical keyframes and the current frame, and determining the effective loop closure based on the spatiotemporal consistency of the two loop closure detection results, specifically involves: Visual features of historical keyframes and current frames are extracted using a visual bag-of-words model, and laser features of historical keyframes and current frames are extracted using a laser 3D bag-of-words model. The first candidate loop closure frame that satisfies visual loop closure and the second candidate loop closure frame that satisfies laser loop closure are selected based on the similarity of features extracted from historical keyframes and the current frame. The intersection of the first candidate loopback frame and the second candidate loopback frame is selected, and a spatial consistency constraint is applied to a set number of loopback frames in the intersection. If the constraint is satisfied, it is a valid loop closure; otherwise, it is not a loop closure. The formula for the spatial consistency constraint is: ; in, For the set spatial distance threshold, and It is the feature vector corresponding to loopback frames i and j in the intersection of the first candidate loopback frame and the second candidate loopback frame.

[0012] Optionally, the conditions for determining whether visual loop closure and laser loop closure are satisfied are: whether the similarity score between the current frame and each historical keyframe is greater than the similarity score threshold. If the similarity score is greater than the threshold value, then either visual loop closure or laser loop closure is satisfied; otherwise, loop closure is not satisfied. It adaptively adjusts based on the maximum number of shared words and information entropy.

[0013] Secondly, a SLAM system based on visual inertial guidance and laser cascade registration is provided, including: The data acquisition module is used to acquire and align multi-source data, including camera images, lidar point cloud data, and IMU data. The visual-inertial front-end module is used to predict the pose based on IMU data using the IMU pre-integration method. The predicted pose is used as the initial value, combined with camera images, and the visual-inertial odometry technology is used to output the robot's visual-inertial pose. The visual-inertial factor is constructed based on the pose constraint relationship of the visual-inertial pose. The laser cascaded registration module is used to perform multi-stage registration on the input lidar point cloud frame using the visual-inertial pose as the initial value for registration, output the estimated laser pose, and construct the laser factor based on the pose constraint relationship of the estimated laser pose. The dual-modal loop closure detection module is used to perform visual loop closure detection and laser loop closure detection respectively by using the feature similarity between historical keyframes and the current frame. The effective loop closure is determined based on the spatiotemporal consistency of the two loop closure detection results, and the loop closure factor is constructed based on the loop closure constraint relationship. The factor graph optimization module is used to construct a factor graph and perform graph optimization using the robot's pose, velocity, and IMU bias as state variables, and the visual inertial factor, laser factor, and loop closure factor as factor nodes, to obtain a globally consistent trajectory and map points.

[0014] Thirdly, a computer device is provided, including a processor and a memory; wherein, when the processor executes a computer program stored in the memory, it implements the steps of the SLAM method based on visual inertial guidance and laser cascade registration as described in any one of the first aspects.

[0015] Fourthly, a computer-readable storage medium is provided for storing a computer program; when executed by a processor, the computer program implements the steps of the SLAM method based on visual inertial guidance and laser cascade registration as described in any one of the first aspects.

[0016] Compared with the prior art, the beneficial effects of the present invention are: This invention improves the stability of pose estimation in weak texture and high-speed motion scenes through IMU-guided visual front-end prediction; significantly enhances the convergence and accuracy of laser registration through a multi-stage registration mechanism; achieves dynamic collaboration among multiple modalities through visual-guided laser registration; and effectively reduces the false loop closure rate and improves global consistency and system robustness through visual and laser dual-modal loop closure consistency verification. This invention significantly improves the positioning accuracy and mapping stability of fused SLAM systems in degraded, dynamic, and occluded environments, and has good engineering applicability and promotion value. Attached Figure Description

[0017] Figure 1 This is a flowchart of the SLAM method based on visual inertial guidance and laser cascade registration of the present invention. Figure 2 This is a flowchart of the visual-inertial pose output of the present invention; Figure 3 This is a flowchart of the cascaded registration process when selecting multi-stage registration based on confidence feedback according to the present invention; Figure 4 This is a flowchart of the multi-stage cascaded registration process of the present invention; Figure 5 This is a flowchart of the dual-modal loop closure detection process of the present invention; Figure 6 This is a comparison chart of trajectory estimation and ground truth trajectories performed using the present invention and existing algorithms on different datasets. Detailed Implementation

[0018] The present invention will be further described below with reference to the accompanying drawings. The following embodiments are only used to more clearly illustrate the technical solutions of the present invention and should not be used to limit the scope of protection of the present invention. It should be noted that the term "comprising" and any variations thereof in the specification, claims and the above-mentioned drawings of the present invention are intended to cover non-exclusive inclusion. For example, a process, method, system, product or device that includes a series of steps or units is not necessarily limited to those steps or units explicitly listed, but may include other steps or units not explicitly listed or inherent to these processes, methods, products or devices.

[0019] Example 1 like Figure 1 As shown, a SLAM method based on visual inertial guidance and laser cascade registration is provided, including the following steps: S1: Acquire and align multi-source data, including camera images, LiDAR point cloud data, and IMU data.

[0020] In this embodiment, the specific data acquisition method and alignment method can refer to the existing technology. Specifically, the point cloud in the lidar coordinate system is transformed to the camera coordinate system through the extrinsic parameter matrix, and the three-dimensional points are projected onto the image plane using the camera intrinsic parameters to achieve multimodal spatial alignment. In this embodiment, it is also necessary to perform preprocessing and time synchronization of the multi-source data according to conventional operations.

[0021] S2: Based on IMU data, the pose is predicted using the IMU pre-integration method. This predicted pose is used as the initial value. Combined with camera images, the robot's visual-inertial pose is output using visual-inertial odometry technology. The visual-inertial factor is constructed based on the pose constraint relationship of the visual-inertial pose.

[0022] like Figure 2 As shown, step S2 specifically includes the following sub-steps: S21: Pre-integrate the IMU data between adjacent image frames to obtain the inter-frame predicted pose.

[0023] Specifically, the acceleration and angular velocity information of the inertial measurement unit (IMU) are used to perform integral prediction on continuous image frames, and the pre-integration method refers to existing technology.

[0024] S22: The inter-frame predicted pose is used as the initial value for optical flow feature matching. The Lucas-Kanade method based on the pyramid structure is used to extract and match features from adjacent image frames. During the matching process, abnormal feature matching points are removed.

[0025] The Lucas–Kanade method based on a pyramid structure is used to extract and match features from adjacent image frames, which can enhance the stability of feature tracking in weak texture or fast motion scenes. When abnormal feature matching points are found, geometric consistency constraints can be introduced to remove them based on the projection residuals and reprojection errors of feature points in adjacent frames, thereby ensuring the consistency of visual observation. For specific removal of abnormal feature matching points, existing technologies can be referenced.

[0026] S23: Combine the constraints of inter-frame predicted pose and the visual reprojection constraints after removing anomalous feature matching points into a nonlinear least squares optimization problem, and solve it to obtain the visual-inertial pose.

[0027] When solving the nonlinear least squares optimization problem, the inter-frame predicted pose is used as the initial value. The constraint relationship of the inter-frame predicted pose and the visual reprojection constraint after removing abnormal matching points can refer to existing technologies. Constructing the nonlinear least squares optimization problem with these two is equivalent to integrating IMU prediction and visual observation, which can ensure the robustness of solving the visual inertial pose.

[0028] S24: Construct visual inertia factor.

[0029] In this embodiment, the visual inertia factor is a unified model of the constraints of inter-frame predicted pose and visual reprojection constraints on the same set of keyframe states, and is regarded as a whole visual inertia factor.

[0030] S241: Construct the inertial pre-integral factor for inter-frame predicted pose constraints.

[0031] For any two adjacent keyframes k and k+1, within the time interval The internal IMU provides linear acceleration measurement. With angular velocity measurement Given a zero initial value Under these conditions, the relative motion prediction is obtained through pre-integration: ; represent position increment, velocity increment, and attitude increment, respectively. The inertial pre-integration residual can then be expressed as: Among them, the positional residual is: Velocity residual: The attitude residual can be written as a vectorized form of the quaternion error: ;in, For quaternions The corresponding rotation matrix, The vector of gravitational acceleration. , To represent quaternion multiplication, This indicates taking the imaginary part vector. During pre-integration, a first-order linear approximation can also be established for the zero-bias transformation, thus introducing an adjacency into the residual. Jacobi. The above. This is the residual vector corresponding to the inertial preintegration factor, used to constrain the relative motion and zero-bias drift between two keyframes.

[0032] S242: Visual reprojection subfactor construction.

[0033] Let the position of the i-th feature point in the world coordinate system be . The body posture of keyframe j is The camera's external parameters relative to the body are The coordinates of this feature point in the camera coordinate system are: Let the projection model of the camera be... For example, the pinhole model: The observed pixel coordinates are: The visual reprojection residual is: In actual implementation, feature points Inverse depth parameterization can be used, for example, denoted as in its first observation camera coordinate system. And restore it to the world coordinate system through the corresponding relationship. For the above residual pairs and Linearization yields the Jacobian of the visual reprojection sub-factors, which can be used for optimization in the factor graph.

[0034] S243: Unified modeling of visual inertia factor.

[0035] In the factor graph, for any adjacent keyframe (k, k+1) and its observed set of feature points... The inertial pre-integration residual With all visual reprojection residuals within the corresponding frame They are uniformly considered as a set of visual inertial constraints that collectively act on the state set of the factor graph. Specifically, it can be formally written as: The weighted L2 norm corresponding to the residual vector is the cost contribution of the visual inertia factor.

[0036] S3: Using the visual-inertial pose as the initial value for registration, perform multi-stage registration on the input lidar point cloud frame, output the estimated lidar pose, and construct the lidar factor based on the pose constraint relationship of the estimated lidar pose.

[0037] like Figure 3 As shown, multi-stage registration includes two-stage registration or three-stage registration. Two-stage registration includes local fine registration and probabilistic optimization; three-stage registration includes global coarse registration, local fine registration and probabilistic optimization.

[0038] In this embodiment, before performing multi-stage registration on the input LiDAR point cloud frame, the confidence level corresponding to the robot's visual-inertial pose output by the visual-inertial odometry is obtained. When the confidence level is lower than a set threshold, a three-stage registration method is used to perform multi-stage registration on the input LiDAR point cloud frame. When the confidence level is higher than or equal to the set threshold, a two-stage registration method is used to perform multi-stage registration on the input LiDAR point cloud frame.

[0039] The confidence level corresponding to the robot vision-inertial pose output can be calculated by referring to existing technologies. Specifically, it can be jointly evaluated and calculated based on the number of features, matching residuals, and IMU prediction drift.

[0040] Perform multi-stage registration and output laser estimated pose, specifically: step ac or steps b and c.

[0041] Step a: Use the Super4PCS algorithm to perform global coarse registration between the current frame point cloud and the historical keyframe point cloud to obtain the initial pose transformation.

[0042] Step b: Using the initial pose transformation as the initial value, the ICP algorithm based on point-to-surface constraints is used to perform local fine registration of the current frame point cloud and the historical key frame point cloud to obtain the rigid body transformation matrix.

[0043] Step c: Based on the rigid body transformation matrix, use the NDT algorithm to construct a probability density model for fine optimization and output the laser estimated pose.

[0044] The Super4PCS algorithm accelerates registration through feature point subset selection and random sampling mechanisms. ICP uses a point-to-surface error metric to improve convergence accuracy, and NDT employs a resolution-adaptive voxel model to balance speed and accuracy. The method for constructing the laser factor can directly refer to existing techniques.

[0045] In some other embodiments, such as Figure 4As shown, when performing multi-stage registration on the input lidar point cloud frames, an adaptive trigger registration stage backoff strategy is introduced. Specifically, the ICP convergence residual is detected in real time. When the residual of two consecutive frames exceeds the threshold, it automatically backoffs to Super4PCS global registration, thereby ensuring that the registration stability can still be maintained in the degradation environment.

[0046] S4: Visual loop closure detection and laser loop closure detection are performed based on the feature similarity between historical keyframes and the current frame. The effective loop closure is determined based on the spatiotemporal consistency of the two loop closure detection results, and the loop closure factor is constructed based on the loop closure constraint relationship.

[0047] like Figure 5 As shown, step S4 specifically includes: S41: Visual features of historical keyframes and current frames are extracted using the visual bag-of-words model, and laser features of historical keyframes and current frames are extracted using the laser 3D bag-of-words model.

[0048] The visual bag-of-words model and the laser 3D bag-of-words model can refer to existing technologies, and the specific calculation of the number of words is shared.

[0049] S42: Select the first candidate loop closure frame that satisfies visual loop closure and the second candidate loop closure frame that satisfies laser loop closure based on the similarity of features extracted from historical keyframes and the current frame.

[0050] Specifically, cosine similarity is used for selection, and the selection method is as follows: whether the similarity score between the current frame and each historical keyframe is greater than the similarity score threshold. If the value is greater than 0, then the visual loop closure or laser loop closure is satisfied; otherwise, the loop closure is not satisfied.

[0051] That is, for each historical keyframe, determine whether its visual feature similarity score with the current frame meets a threshold. If yes, then the visual loop closure is satisfied; otherwise, the loop closure is not satisfied, and the historical keyframe is removed. All historical keyframes that satisfy the visual loop closure are the first candidate loop closure frames. Similarly, the second candidate loop closure frames can be obtained.

[0052] S43: Filter the intersection of the first candidate loopback frame and the second candidate loopback frame, and apply spatial consistency constraints to a set number of loopback frames in the intersection. If the constraints are met, it is a valid loop closure; otherwise, it is not a loop closure. The formula for the spatial consistency constraint is: ; in, For the set spatial distance threshold, and It is the feature vector corresponding to loopback frames i and j in the intersection of the first candidate loopback frame and the second candidate loopback frame.

[0053] Steps S41-S43 can effectively reduce the false loop closure rate and improve the loop closure robustness under dynamic occlusion or drastic view changes.

[0054] S44: Construct cyclic factors.

[0055] The method for constructing cyclic factors is based on existing techniques.

[0056] In some other embodiments, a similarity score threshold is used. The algorithm adaptively adjusts based on the maximum shared word count and information entropy. Specifically, in visual loop closure detection, the BoW shared word count is calculated for the current frame i and all historical frames j. A set of distributions was obtained. The process of setting the adaptive threshold can be described as follows: Statistically analyze the distribution and maximum value of shared word counts; calculate the number of shared words for the current frame i and all historical frames j: Calculate its maximum and mean values: .

[0057] Constructing a distribution histogram and calculating information entropy: The intervals are divided into several bins to obtain a frequency histogram, which is then normalized to a probability distribution. : Calculate information entropy: Simultaneously determine the maximum entropy under the current group. ( K (where the number of bins is ), and the normalized entropy is obtained: Adaptive threshold Defined as a weighted interpolation of the maximum and mean values, with the weights controlled by the normalized entropy.

[0058] Specifically, .

[0059] S5: Using the robot's pose, velocity, and IMU bias as state variables as variable nodes, and the visual inertial factor, laser factor, and loop closure factor as factor nodes, construct a factor graph and perform graph optimization to obtain a globally consistent trajectory and map points.

[0060] The construction of factor graphs and the execution of graph optimization can refer to existing technologies. Specifically, the iSAM2 incremental smoothing algorithm can be used to optimize the global graph structure in real time. The sliding window mechanism is used to retain neighboring keyframes to improve real-time performance, and finally, a globally consistent trajectory estimate and a high-precision map are output.

[0061] The state vector and the overall optimization objective are: Let the state vector of keyframe k be: ;in, The position of the machine in the world coordinate system; Machine speed; Quaternions describing the attitude of the world to the organism; Accelerometer and gyroscope zero bias.

[0062] Maintain several keyframe states within a sliding window. It can also include parameters such as inverse depth of feature points and camera-IMU extrinsic parameters. The overall optimization objective can be written as a nonlinear least squares function in the form of a factor graph: ; By performing joint nonlinear optimization of all visual-inertial factors, laser factors, and loop closure factors within a sliding window, keyframe pose, velocity, IMU zero bias, and necessary map point parameters can be corrected simultaneously, thereby achieving multimodal tightly coupled SLAM.

[0063] To verify the effectiveness of this invention, functional testing was conducted on the publicly available M2DGR dataset. The selected test sequences included Gate01, Gate02, and Gate03, all of which are representative dynamic outdoor scenes. During the testing process, A-LOAM, LeGO-LOAM, and the algorithm of this invention were run respectively to collect their respective pose estimation trajectories, and the EVO evaluation tool was used to perform temporal and spatial alignment with the ground truth trajectories provided in the dataset. Subsequently, the absolute trajectory error (ATE) and relative pose error (RPE) were calculated, and multi-dimensional indicators such as maximum, minimum, average, median, and mean square values ​​were statistically analyzed. The results are shown in Table 1.

[0064] Table 1 Comparison of absolute trajectory errors of different SLAM algorithms on the M2DGR gate sequence.

[0065] Figure 6 The 'ac' values ​​in Table 1 and 'ac' represent comparisons of trajectory estimation and ground truth trajectories using the present invention and existing algorithms, based on the Gate01, Gate02, and Gate03 datasets. Figure 6 It can be seen that the method of the present invention exhibits superior performance in terms of trajectory accuracy and continuity in different dynamic scenarios.

[0066] In the Gate01 scenario, the method of this invention has the lowest mean square error, small overall trajectory fluctuation, and smoother positioning results. In the Gate02 scenario, the algorithm maintains high accuracy and stability in a moderately dynamic environment, with a concentrated error distribution. In the Gate03 scenario, the method of this invention can still achieve continuous trajectory output in degraded and partially occluded environments, and the registration error is significantly reduced.

[0067] The visual-inertial guidance and laser multi-stage registration coordination mechanism of this invention plays a significant role in multimodal information complementarity. Specifically, visual-IMU prediction provides reliable initial values ​​and constraints, enabling registration to remain stable in fast-moving scenes; the Super4PCS-ICP-NDT cascade structure enhances the convergence of point cloud registration in degraded regions; and the dual-modal loop closure verification mechanism effectively reduces the probability of false loop closures and maintains global accuracy. Figure 1 To the point of being responsive.

[0068] Comprehensive test results show that, compared with traditional single laser or visual SLAM algorithms, the present invention can obtain more stable positioning trajectories and higher mapping accuracy in dynamic, weak texture and feature degradation scenarios, and the overall robustness of the system is significantly enhanced.

[0069] Example 2 A SLAM system based on visual-inertial guidance and laser cascade registration includes: The data acquisition module is used to acquire and align multi-source data, including camera images, lidar point cloud data, and IMU data. The visual-inertial front-end module is used to predict the pose based on IMU data using the IMU pre-integration method. The predicted pose is used as the initial value, combined with camera images, and the visual-inertial odometry technology is used to output the robot's visual-inertial pose. The visual-inertial factor is constructed based on the pose constraint relationship of the visual-inertial pose. The laser cascaded registration module is used to perform multi-stage registration on the input lidar point cloud frame using the visual-inertial pose as the initial value for registration, output the estimated laser pose, and construct the laser factor based on the pose constraint relationship of the estimated laser pose. The dual-modal loop closure detection module is used to perform visual loop closure detection and laser loop closure detection respectively by using the feature similarity between historical keyframes and the current frame. The effective loop closure is determined based on the spatiotemporal consistency of the two loop closure detection results, and the loop closure factor is constructed based on the loop closure constraint relationship. The factor graph optimization module is used to construct a factor graph and perform graph optimization using the robot's pose, velocity, and IMU bias as state variables, and the visual inertial factor, laser factor, and loop closure factor as factor nodes, to obtain a globally consistent trajectory and map points.

[0070] For more specific details about the processes corresponding to the above systems, please refer to the relevant content disclosed in the foregoing embodiments, which will not be repeated here.

[0071] Example 3 The present invention provides a computer device, including a processor and a memory; wherein, when the processor executes a computer program stored in the memory, it implements the steps of the above-described SLAM method based on visual inertial guidance and laser cascade registration.

[0072] For a more detailed explanation of the above method, please refer to the relevant content disclosed in the foregoing embodiments, which will not be repeated here.

[0073] Example 4 The present invention provides a computer-readable storage medium for storing a computer program; when the computer program is executed by a processor, it implements the steps of the above-described SLAM method based on visual inertial guidance and laser cascade registration.

[0074] For a more detailed explanation of the above method, please refer to the relevant content disclosed in the foregoing embodiments, which will not be repeated here.

[0075] The various embodiments in this specification are described in a progressive manner, with each embodiment focusing on its differences from other embodiments. Similar or identical parts between embodiments can be referred to interchangeably. The systems, devices, and storage media disclosed in the embodiments are described simply because they correspond to the methods disclosed in the embodiments; relevant details can be found in the method section.

[0076] Those skilled in the art will clearly understand that the techniques in the embodiments of the present invention can be implemented using software plus necessary general-purpose hardware platforms. Based on this understanding, the technical solutions in the embodiments of the present invention, or the parts that contribute to the prior art, can be embodied in the form of a software product. This computer software product can be stored in a storage medium, such as ROM / RAM, magnetic disk, optical disk, etc., and includes several instructions to cause a computer device (which may be a personal computer, server, or network device, etc.) to execute the methods described in various embodiments or certain parts of the embodiments of the present invention.

[0077] The above are merely preferred embodiments of the present invention. The scope of protection of the present invention is not limited to the above embodiments. All technical solutions falling within the scope of the present invention's concept are within the scope of protection of the present invention. It should be noted that for those skilled in the art, any improvements and modifications made without departing from the principles of the present invention should be considered within the scope of protection of the present invention.

Claims

1. A SLAM method based on visual-inertial odometry and laser cascade registration, characterized in that, The method comprises the following steps: acquiring multi-source data and aligning, the multi-source data comprising camera images, lidar point cloud data and IMU data; based on the IMU data, predicting a pose by using an IMU pre-integration method, taking the predicted pose as an initial value, combining the camera images, outputting a robot visual-inertial pose by using a visual-inertial odometry technology, and constructing a visual-inertial factor according to the pose constraint relationship of the visual-inertial pose; taking the visual-inertial pose as an initial value of registration, performing multi-stage registration on the input lidar point cloud frame, outputting a laser estimated pose, and constructing a laser factor according to the pose constraint relationship of the laser estimated pose; performing visual loop detection and laser loop detection respectively according to the feature similarity of the historical key frame and the current frame, determining an effective loop according to the spatio-temporal consistency of the two loop detection results, and constructing a loop factor according to the loop constraint relationship of the effective loop; taking the state variable composed of the pose, velocity and IMU bias of the robot as a variable node, taking the visual-inertial factor, the laser factor and the loop factor as a factor node, constructing a factor graph and performing graph optimization to obtain a globally consistent trajectory and map point.

2. The SLAM method based on visual-inertial guidance and laser cascade registration according to claim 1, characterized in that, The method of taking the predicted pose as an initial value, combining the camera images, and outputting a robot visual-inertial pose by using a visual-inertial odometry technology comprises the following steps: performing pre-integration on the IMU data between adjacent image frames to obtain an inter-frame predicted pose; taking the inter-frame predicted pose as an initial value of optical flow feature matching, performing feature extraction and matching processing on adjacent image frames by using a Lucas-Kanade method based on a pyramid structure, and eliminating abnormal feature matching points in the matching process; jointly constructing the constraint relationship of the inter-frame predicted pose and the visual re-projection constraint after eliminating the abnormal feature matching points into a non-linear least squares optimization problem, and solving to obtain a visual-inertial pose.

3. The SLAM method based on visual-inertial guidance and laser cascade registration according to claim 1, characterized in that, The multi-stage registration comprises two-stage registration or three-stage registration, the two-stage registration comprises local fine registration and probability optimization, and the three-stage registration comprises global coarse registration, local fine registration and probability optimization; Before performing multi-stage registration on the input lidar point cloud frame, the confidence corresponding to the robot visual-inertial pose output by the visual-inertial odometry is acquired, when the confidence is lower than a set threshold, the input lidar point cloud frame is registered by using a three-stage registration method, and when the confidence is higher than or equal to the set threshold, the input lidar point cloud frame is registered by using a two-stage registration method.

4. The SLAM method based on visual-inertial guidance and laser cascade registration according to claim 3, characterized in that, The method of performing multi-stage registration on the input lidar point cloud frame by using a three-stage registration method comprises the following steps: performing global coarse registration on the current frame point cloud and the historical key frame point cloud by using a Super4PCS algorithm to obtain an initial pose transformation; taking the initial pose transformation as an initial value, performing local fine registration on the current frame point cloud and the historical key frame point cloud by using an ICP algorithm based on point-to-plane constraint to obtain a rigid transformation matrix; based on the rigid transformation matrix, constructing a probability density model by using an NDT algorithm to perform fine optimization, and outputting a laser estimated pose.

5. The SLAM method based on visual-inertial odometry and laser cascade registration according to claim 4, characterized in that, In the multi-stage registration of the input laser radar point cloud frame, an adaptive trigger registration stage rollback strategy is introduced; specifically, the ICP convergence residual is detected in real time, and when the residual of two consecutive frames exceeds the threshold, the Super4PCS global registration is automatically rolled back.

6. The SLAM method based on visual-inertial odometry and laser cascade registration according to claim 1, characterized in that, The visual loop detection and laser loop detection are performed respectively according to the feature similarity of the historical key frame and the current frame, and the effective loop is determined according to the spatiotemporal consistency of the two loop detection results, specifically: The visual features of the historical key frame and the current frame are extracted by using the visual bag-of-words model respectively, and the laser features of the historical key frame and the current frame are extracted by using the laser three-dimensional bag-of-words model respectively; The first candidate loop frame meeting the visual loop and the second candidate loop frame meeting the laser loop are selected based on the similarity of the features extracted from the historical key frame and the current frame; The intersection of the first candidate loop frame and the second candidate loop frame is screened, and a spatial consistency constraint is imposed on a certain number of loop frames in the intersection, if the constraint is met, it is an effective loop, if not, it is a non-loop; The spatial consistency constraint formula is: ； wherein, is a set spatial distance threshold, and is a feature vector corresponding to the loopback frames i and j in the intersection of the first candidate loopback frame and the second candidate loopback frame.

7. The SLAM method based on visual-inertial odometry and laser cascade registration according to claim 6, characterized in that, The condition for judging whether the visual loop is satisfied and whether the laser loop is satisfied is whether the similarity score of the current frame and each historical key frame is greater than a similarity score threshold If yes, the visual loop is satisfied or the laser loop is satisfied, otherwise, the loop is not satisfied; the similarity score threshold is adaptively adjusted according to the maximum number of shared words and information entropy.

8. A SLAM system based on visual-inertial odometry and laser cascade registration, characterized in that, It comprises: A data acquisition module is configured to acquire and align multi-source data, including camera images, laser radar point cloud data and IMU data; A visual-inertial front-end module is configured to predict a pose based on the IMU data using an IMU pre-integration method, use the predicted pose as an initial value, combine the camera images, output a robot visual-inertial pose using a visual-inertial odometry technology, and construct a visual-inertial factor based on the pose constraint relationship of the visual-inertial pose; A laser cascade registration module is configured to use the visual-inertial pose as an initial value for registration, perform multi-stage registration on the input laser radar point cloud frame, output a laser estimated pose, and construct a laser factor based on the pose constraint relationship of the laser estimated pose; A dual-mode loop detection module is configured to perform visual loop detection and laser loop detection respectively based on the feature similarity of the historical key frame and the current frame, determine an effective loop according to the spatiotemporal consistency of the two loop detection results, and construct a loop factor based on the loop constraint relationship. A factor graph optimization module is configured to use the pose, velocity and IMU bias of the robot as variable nodes, use the visual-inertial factor, the laser factor and the loop factor as factor nodes, construct a factor graph and perform graph optimization to obtain a globally consistent trajectory and map point.

9. A computer device, comprising: It comprises a processor and a memory; wherein the processor executes the computer program saved in the memory to realize the steps of the SLAM method based on visual-inertial guidance and laser cascade registration according to any one of claims 1-7.

10. A computer-readable storage medium, characterized in that, It is used for storing a computer program; the computer program is executed by the processor to realize the steps of the SLAM method based on visual-inertial guidance and laser cascade registration according to any one of claims 1-7.

Images