SLAM positioning method and system based on multi-sensor fusion

Abstract

The invention provides an SLAM positioning method and system based on multi-sensor fusion, and the method comprises the steps: firstly receiving data of a plurality of sensors, and obtaining initial data including visual feature data, I MU pre-integration data and UWB distance data through anomaly detection and processing; initializing by using visual feature data and I MU pre-integration data, estimating an initial pose, and predicting a UWB anchor point position as a fixed constraint in combination with UWB distance data; inputting the initial data into a sliding window, outputting drifting-free odometer data, judging the motion state of the SLAM system according to the data, and if the motion state is static, executing zero-speed updating to obtain target pose data; and finally, determining a new key frame based on the parallax or visual feature data quantity of the current frame and the previous key frame. According to the invention, the problems of relatively weak robustness and relatively low positioning accuracy of the system facing extreme scenes are solved; by means of motion state judgment and zero-speed updating, the robustness of the system in long-term operation is improved.

Description

Technical Field

[0001] The present application relates to a positioning method, in particular to a SLAM positioning method and system based on multi-sensor fusion, belonging to the technical field of computer vision sensing. Background Art

[0002] The booming development of robotics and artificial intelligence has led to their wide application in fields such as industry, production, and entertainment. Accurate and consistent position estimation is a key factor for mobile robots in applications such as search and rescue. However, the application of position estimation technology in indoor and satellite positioning signal-free areas still faces challenges. The positioning problem of drones in these environments has not been solved; the augmented reality system is affected by camera motion and dynamic objects, making it difficult to achieve stable virtual-real fusion; indoor robots and autonomous vehicles also have problems with positioning, path planning, and obstacle avoidance. The key lies in that these technologies have not yet achieved the true autonomous functions of robots, especially in dynamic environments, the problems of self-positioning and surrounding environment map construction. Therefore, Simultaneous Localization and Mapping (SLAM) is regarded as one of the key technologies for robots to achieve autonomy.

[0003] In recent years, Visual Simultaneous Localization and Mapping (Visual SLAM) has become a cutting-edge technology in robot positioning research, and can use a single camera to achieve continuous six-degree-of-freedom pose estimation. However, as application scenarios and tasks become more and more complex, Visual SLAM systems face limitations such as sensitivity to lighting changes and motion speed interference. Therefore, SLAM technologies integrating multiple information sources have received increasing attention. The complementary characteristics of cameras and Inertial Measurement Units (IMUs) have prompted them to be integrated into Visual SLAM systems. This integration forms a Visual Inertial SLAM (VI-SLAM) or Visual Inertial Odometry (VIO) system, significantly enhancing the robustness of pose estimation. However, sensor noise and computational errors will still cause the system to drift over time. Recent research has shown that Ultra-Wideband (UWB) can provide reliable global constraints for indoor Visual Inertial Odometry (VIO) systems. However, there is still a need to detect and process sensor anomalies to address inaccuracies and low robustness during long-term operation.

[0004] Therefore, it is necessary to propose appropriate methods for detecting and processing sensor outliers to improve the positioning accuracy and robustness of system operation. Summary of the Invention

[0005] In view of this, the present application provides a SLAM positioning method and system based on multi-sensor fusion to solve or alleviate the technical problems existing in the prior art, and at least provide a beneficial option.

[0006] The technical solution of the embodiment of the present application is implemented as follows: A SLAM positioning method based on multi-sensor fusion is provided, including:

[0007] S100: Receive multiple sensor data, and respectively perform anomaly detection and processing on the multiple sensor data to obtain preliminary data corresponding to each sensor; wherein, the preliminary data at least includes visual feature data, IMU pre-integration data, and UWB distance data.

[0008] S200: Initialize based on the visual feature data and IMU pre-integration data to estimate the initial pose, and combine the initial pose and UWB distance data to predict the UWB anchor position, and use the UWB anchor position as a fixed constraint.

[0009] S300: Based on the fixed constraint, input the preliminary data into a sliding window to output drift-free odometer data.

[0010] S400: Determine the motion state of the SLAM system based on the drift-free odometer data, and when it is detected that the motion state is static, perform zero-velocity update to obtain target pose data.

[0011] S500: Based on the target pose data, determine whether the parallax between the current frame and the previous key frame exceeds a first preset threshold or determine whether the number of visual feature data is lower than a second preset threshold, and when the determination result is exceeding or lower, confirm the current frame as a new key frame.

[0012] Further preferably: The multiple sensor data at least includes camera images, IMU information, and UWB information; the anomaly detection and processing in step S100 includes:

[0013] Extract point features and line features based on the camera image, combine the optical flow backtracking method and the gray-scale verification method to remove outliers, and filter out feature points on dynamic objects through dynamic target recognition to output visual feature data; perform pre-integration on the IMU information to output IMU pre-integration data; and perform sliding window anomaly detection and Gaussian smoothing on the UWB information, and use the anomaly-detected UWB information and the Gaussian smoothing processing result as UWB distance data.

[0014] Further preferably: The extraction of point features and line features includes:

[0015] Obtain point features based on the camera image through the Shi-Tomasi method and use KLT tracking and RANSAC confocal geometric constraints to screen inliers; and obtain line features based on the camera image through the LSD algorithm and combine the LBD descriptor and KNN matching.

[0016] Further preferably, the estimating of the initial pose and the predicting of the UWB anchor position in step S200 include:

[0017] Estimating the initial pose through visual-inertial alignment based on the visual feature data and the IMU pre-integration data; predicting the UWB anchor position through non-linear optimization by combining the initial pose and the UWB distance data.

[0018] Further preferably, the outputting of the drift-free odometer data in step S300 includes:

[0019] Minimizing the weighted sum of the prior information residual, the IMU residual, the point feature projection residual, the line feature reprojection residual, and the UWB residual to obtain the maximum a priori probability problem; and outputting the drift-free odometer data based on the preliminary data and the maximum a priori probability problem.

[0020] Further preferably, the obtaining of the target pose data in step S400 includes:

[0021] Based on the drift-free odometer data, combining the IMU generalized likelihood ratio verification, the sliding window variance analysis of the acceleration variance and the angular velocity variance, and simultaneously calculating the average parallax through visual parallax; if the IMU generalized likelihood ratio is lower than the first preset threshold, or the acceleration variance and the angular velocity variance are lower than the second threshold, and the average parallax is lower than the third preset threshold, it is determined that the SLAM system is in a stationary state, triggering zero velocity update and correcting the pose and velocity within the sliding window to obtain the target pose data.

[0022] Further preferably, step S500 further includes:

[0023] Activating loop closure detection based on the bag-of-words model to correct the global trajectory according to the loop closure detection result.

[0024] Further preferably, the correcting of the global trajectory includes:

[0025] Based on the new key frame, matching the visual feature descriptors of the historical key frames through the bag-of-words model to detect loop closure; if loop closure is detected, extracting the relative pose constraints between the loop closure frames, correcting the global trajectory through the pose graph, and feeding back the correction result to the sliding window.

[0026] Based on the same concept, the present application further provides a SLAM positioning system based on multi-sensor fusion, including:

[0027] Anomaly detection module: used to receive multiple sensor data, and respectively perform anomaly detection and processing on the multiple sensor data to obtain the preliminary data corresponding to each sensor; wherein, the preliminary data at least includes visual feature data, IMU pre-integration data, and UWB distance data.

[0028] Initialization module: It is used to perform initialization based on the visual feature data and IMU pre-integration data to estimate the initial pose, and combine the initial pose and UWB distance data to predict the UWB anchor position, and use the UWB anchor position as a fixed constraint.

[0029] Sliding window optimization module: It is used to input the preliminary data into the sliding window based on the fixed constraint to output drift-free odometer data.

[0030] Zero-velocity update module: It is used to determine the motion state of the SLAM system based on the drift-free odometer data, and when it detects that the motion state is static, perform zero-velocity update to obtain the target pose data.

[0031] Loop closure module: It is used to judge whether the parallax between the current frame and the previous key frame exceeds a first preset threshold or judge whether the quantity of visual feature data is lower than a second preset threshold based on the target pose data. When the judgment result is exceeding or lower, the current frame is confirmed as a new key frame.

[0032] Further preferably: The loop closure module is further used to activate loop closure detection based on the bag-of-words model to correct the global trajectory according to the loop closure detection result.

[0033] Since the embodiments of the present application adopt the above technical solutions, they have the following advantages:

[0034] The present application proposes anomaly detection and processing of each sensor under extreme conditions, solves the problems of weak robustness and low positioning accuracy of the system in the face of extreme scenarios; improves the robustness of the system under long-term operation through system motion state judgment and zero-velocity update.

[0035] The above summary is only for the purpose of the specification and is not intended to be limiting in any way. In addition to the above-described illustrative aspects, embodiments and features, other aspects, embodiments and features of the present application will be readily apparent by reference to the drawings and the following detailed description. BRIEF DESCRIPTION OF THE DRAWINGS

[0036] In order to more clearly illustrate the technical solutions in the embodiments of the present application or the prior art, the following will briefly introduce the drawings required for use in the description of the embodiments or the prior art. Obviously, the following drawings are only some embodiments of the present application. For those of ordinary skill in the art, other drawings can be obtained based on these drawings without creative efforts.

[0037] Figure 1 It is a flowchart of the SLAM positioning method based on multi-sensor fusion described in the present application.

[0038] Figure 2 This is the framework diagram of the SLAM positioning system based on multi-sensor fusion described in this application.

[0039] Figure 3 This is the schematic diagram of the comparison results of VINS-Mono, VIR-SLAM, PL-VINS described in this application and this application.

[0040] Figure 4 This is the schematic diagram of the comparison results of VINS-w / o L, VIR-W / 0LW, PL-w / o L described in this application and this application.

[0041] Figure 5 This is the schematic diagram of the estimated and ground truth trajectories described in this application.

[0042] Figure 6 This is the schematic diagram of the mobile platform of the sensor described in this application.

[0043] Figure 7 This is the schematic diagram of the comparison results of VINS-Mono, PL-VINS, VIR-SLAM described in this application and this application.

[0044] Figure 8 This is the schematic diagram of the trajectory visualization of the example sequence described in this application. Detailed implementation manners

[0045] In the following, only some exemplary embodiments are simply described. As those skilled in the art can recognize, the described embodiments can be modified in various different ways without departing from the spirit or scope of this application. Therefore, the drawings and descriptions are considered to be exemplary in nature rather than restrictive.

[0046] The embodiments of this application will be described in detail below with reference to the accompanying drawings.

[0047] As Figure 1 shown, the embodiments of this application provide a SLAM positioning method based on multi-sensor fusion, including:

[0048] S100: Receive multiple sensor data, and respectively perform anomaly detection and processing on the multiple sensor data to obtain the preliminary data corresponding to each sensor; wherein, the preliminary data at least includes visual feature data, IMU pre-integration data, and UWB distance data.

[0049] In this embodiment, specifically: the multiple sensor data at least includes camera images, IMU information, and UWB information; the anomaly detection and processing in step S100 includes:

[0050] Extract point features and line features from camera images, remove outliers by combining the optical flow backtracking method and the gray-scale verification method, and filter out feature points on dynamic objects through dynamic target recognition to output visual feature data; pre-integrate the IMU information to output IMU pre-integrated data; and, perform sliding window anomaly detection and Gaussian smoothing on the UWB information, and use the UWB information after anomaly detection and the result of Gaussian smoothing processing as UWB distance data.

[0051] Among them, the IMU information includes the information of the IMU accelerometer and the gyroscope angular velocity meter, and the UWB information includes the distance information between the UWB sensor and the anchor point.

[0052] It should be noted that in order to improve the robustness of the optical flow method and perform feature filtering, first use the optical flow backtracking method to reverse the optical flow order between two frames. Only those feature points that are successfully tracked in two iterations and show adjacent distances below the specified threshold, and at the same time meet the boundary and gray value checks, will be retained for further processing.

[0053] Furthermore, in a dynamic environment, use YOLOv3 for dynamic target recognition to identify objects such as humans, animals, and bicycles before extracting point features. After completing the target detection, the points on the dynamic objects are removed in the point feature extraction stage, thus achieving dynamic point filtering.

[0054] It should also be noted that two challenges in dealing with UWB anomalies include the abnormal distance information in the non-line-of-sight (NLOS) environment and the existence of system biases. To solve these problems, two strategies are introduced: UWB sliding window anomaly detection and Gaussian smoothing.

[0055] Among them, for UWB sliding window anomaly detection, a sliding window of size 10 is established for the range data. When a new UWB range measurement arrives, calculate the deviation between the current range measurement and the average of the last 10 measurements stored in the sliding window. If this deviation exceeds the specified threshold, the range measurement is identified as an outlier. Then the range measurement is corrected to match the last range measurement in the sliding window and inserted into the window for further processing. In addition, timeout processing is also implemented: if the difference between the timestamp of the received range measurement and the timestamp of the last processed range measurement exceeds 1 second, the data in the sliding window will be reset, indicating that the reception of range measurements starts again.

[0056] Gaussian smoothing is applicable to all acceptable measurement methods and is performed during the duration of the long window loop module. For gaps in the measurement timeline, if the adjacent endpoints are close in time, interpolation is performed based on them. Then the smoothed values corresponding to the key frame times are used to create the UWB factors in the long-term window closure module.

[0057] In this embodiment, specifically, the extraction of point features and line features includes:

[0058] Based on the camera image, using the Shi-Tomasi method, and screening inliers by KLT tracking and RANSAC confocal geometry constraints to obtain point features; and, based on the camera image, using the LSD algorithm, and combining the LBD descriptor and KNN matching to obtain line features.

[0059] Among them, the improved LSD algorithm is used to obtain line features, and its running speed is at least three times that of the original LSD algorithm. The line features provide additional constraint conditions for the scene structure, and the residuals of the point and line features are both incorporated into the factor graph for joint optimization.

[0060] S200: Initialize based on the visual feature data and IMU pre-integration data to estimate the initial pose, and combine the initial pose and UWB distance data to predict the UWB anchor position, and use the UWB anchor position as a fixed constraint.

[0061] In this embodiment, specifically, the estimation of the initial pose and the prediction of the UWB anchor position in step S200 include:

[0062] Based on the visual feature data and IMU pre-integration data, estimate the initial pose through visual-inertial alignment; combine the initial pose and UWB distance data, and predict the UWB anchor position through nonlinear optimization.

[0063] Among them, in the initialization stage, let the robot move around the UWB anchor. Therefore, the state vector to be estimated initially is The cost function and factors remain unchanged. Among them, includes the state x of all n frames and the inverse depth l of m p visual features. Represents the orthogonal parameter representation of the three-dimensional line feature. m p 、m l and s respectively represent the total number of spatial points and lines in the sliding window and the size of the long sliding window. w represents the state carried in the long sliding window, including only the position of the robot in the world coordinate system After the initialization stage, The optimization result of is saved as a fixed value, that is, the fixed constraint.

[0064] S300: Based on the fixed constraint, input the preliminary data into the sliding window to output drift-free odometry data.

[0065] In this embodiment, specifically, the output of the drift-free odometry data in step S300 includes:

[0066] Minimize the weighted sum of the prior information residual, the IMU residual, the point feature projection residual, the line feature reprojection residual, and the UWB residual to obtain the maximum a priori probability problem; and, output drift-free odometer data based on the preliminary data and the maximum a priori probability problem.

[0067] Among them, minimize the sum of the prior norm and the Mahalanobis norm of all measurement residuals to obtain the maximum a priori probability problem as follows:

[0068]

[0069] The formula includes the prior information {r p , H p} after excluding one frame in the sliding window, the IMU residual the point feature projection residual the line feature reprojection residual and the UWB residual ρ is the Cauchy robust function used to mitigate the influence of outliers.

[0070] S400: Determine the motion state of the SLAM system based on the drift-free odometer data, and when it is detected that the motion state is static, perform zero-velocity update to obtain the target pose data.

[0071] In this embodiment, specifically: obtaining the target pose data in the step S400 includes:

[0072] Based on the drift-free odometer data, combine the IMU generalized likelihood ratio verification, the sliding window variance analysis of the acceleration variance and the angular velocity variance, and at the same time calculate the average parallax through the visual parallax; if the IMU generalized likelihood ratio is lower than the first preset threshold, or the acceleration variance and the angular velocity variance are lower than the second threshold, and the average parallax is lower than the third preset threshold, it is determined that the SLAM system is in a stationary state, trigger zero-velocity update and correct the pose and velocity within the sliding window to obtain the target pose data.

[0073] Among them, estimating the motion state of the system, the method includes:

[0074] IMU generalized likelihood ratio verification: A method for the system to use an accelerated motion variance detector based on the generalized likelihood ratio test (GLRT) to determine whether there is sufficient motion excitation. The formula is as follows:

[0075]

[0076] Among them is the original measurement value of the IMU. W is the number of IMU measurements within the sliding window. ψ represents the window range, is the average acceleration.

[0077] IMU Sliding Variance Verification: Using only GLRT to determine the motion state of the system is not sensitive enough. Therefore, an IMU sliding variance verification method is proposed. When the carrier is in a stationary state, the variances of the accelerometer outputs along the three axes and the gyroscope outputs along the three axes in the IMU should be approximately zero. This application implements a sliding window of size 20. When new IMU accelerometer and gyroscope data enter the window, the system checks whether the number of data points in the window exceeds 20. If so, the oldest IMU accelerometer and gyroscope data are deleted from the window. Once the sliding window is full, the zero-velocity state is detected by comparing the variances of the specific forces and angular rates of the three axes of the accelerometer and gyroscope with their respective values. The formula is as follows:

[0078]

[0079] where n is the window size. x i represents the output value of an axis of the accelerometer or gyroscope within the window, while represents the average value of all output values of the accelerometer or gyroscope on a specific axis within the window. When both the sliding variance of the accelerometer and the sliding direction difference of the gyroscope are lower than their respective thresholds, the IMU sliding window is determined to be in a static state.

[0080] Visual Verification: When the carrier is stationary, the actual position of the feature points relative to the carrier remains unchanged. Therefore, the pixel positions of the feature points imaged by the two cameras should be very close in the horizontal direction, resulting in a parallax close to zero. Feature points are extracted from the latest frame and matched with the images in the sliding window. Then, the average visual parallax can be expressed as:

[0081]

[0082] where r is the number of matched feature points between the i-th image in the sliding window and the latest image. is the two-dimensional coordinate of the j-th feature point in the latest image frame.

[0083] By combining G, S, and V, the motion state can be detected as:

[0084]

[0085] where the thresholds of α, β, and γ are determined by experimental methods. In the identified stationary state, the first camera frame is set as the local world frame, and the z-axis is aligned with the direction of gravity. Subsequently, all other poses within the sliding window are aligned with the first pose, and the velocity is set to zero. During the optimization process, the velocity, position, and orientation of each frame in the sliding window are set as constant blocks. During the entire optimization process, motion state judgment and zero-velocity update are applied.

[0086] S500: Based on the target pose data, determine whether the parallax between the current frame and the previous key frame exceeds a first preset threshold or determine whether the quantity of visual feature data is lower than a second preset threshold. When the determination result is exceeding or lower, confirm the current frame as a new key frame.

[0087] In this embodiment, specifically: The step S500 further includes:

[0088] Activate loop closure detection based on the bag-of-words model to correct the global trajectory according to the loop closure detection result.

[0089] In this embodiment, specifically: The correcting the global trajectory includes:

[0090] Based on the new key frame, match the visual feature descriptors of historical key frames through the bag-of-words model to detect loop closures; if a loop closure is detected, extract the relative pose constraints between the loop closure frames, correct the global trajectory through the pose graph, and feedback the correction result to the sliding window.

[0091] Based on the same concept, as Figure 2 shown, the present application further provides a multi-sensor fusion-based SLAM positioning system, including:

[0092] Abnormality detection module: configured to receive multiple sensor data, and respectively perform abnormality detection and processing on the multiple sensor data to obtain preliminary data corresponding to each sensor; wherein, the preliminary data at least includes visual feature data, IMU pre-integration data, and UWB distance data.

[0093] Initialization module: configured to perform initialization based on the visual feature data and IMU pre-integration data to estimate an initial pose, and predict a UWB anchor position in combination with the initial pose and UWB distance data, and use the UWB anchor position as a fixed constraint.

[0094] Sliding window optimization module: configured to input the preliminary data into a sliding window based on the fixed constraint to output drift-free odometer data.

[0095] Zero velocity update module: configured to determine the motion state of the SLAM system based on the drift-free odometer data. When it is detected that the motion state is static, perform zero velocity update to obtain target pose data.

[0096] Loop closure module: configured to determine whether the parallax between the current frame and the previous key frame exceeds a first preset threshold or determine whether the quantity of visual feature data is lower than a second preset threshold based on the target pose data. When the determination result is exceeding or lower, confirm the current frame as a new key frame.

[0097] Further preferably, the loop module is further configured to activate loop detection based on the bag-of-words model to correct the global trajectory according to the loop detection result.

[0098] The algorithm of this application has been demonstrated through experimental comparison as follows:

[0099] To verify the effectiveness of the algorithm of this application, it is evaluated through public datasets and real-scene experiments respectively. The positioning performance of our system in various extreme cases is tested in the real scene.

[0100] Simulation using the EuRoC dataset: The state estimation system established on the EuRoC database is tested, which includes three levels of state estimation data: easy, medium, and difficult, covering 11 scenarios. To simulate ultra-wideband measurements, UWB ranging measurements are simulated from the ground truth data. Static anchors are assumed to be at the origin of the frame created during robot initialization. Gaussian white noise N(0, 0.03) is added to simulate the error of the ultra-wideband sensor.

[0101] For ablation experiments, this application compares VINS-Mono, VIR-SLAM, PL-VINS, and this application, denoted as VINS-full, VIR-full, PL-full, and ours-full respectively. The comparison results are as Figure 3 shown. In addition, the system of this application without the loop closure module is also compared with the system of VINS-Mono without the loop closure module, VIR-SLAM without the long-window loop module, and PL-VINS without the loop module, denoted as VINS-w / oL, VIR-W / 0LW, PL-w / o L, and Ours (w / o LW and w / o L) respectively. The comparison results are as Figure 4 shown. In all 11 scenarios of the EuRoC dataset, all methods converge and provide positioning information, but obviously, this application provides more accurate state estimation. This is because this application enriches features, provides additional constraints, and corrects the visual odometry drift through loop detection and UWB state constraints in the backend optimization process. In addition, by comparing the loop module and the long-window module of visual recognition based on the bag-of-words model, the loop method based on the bag-of-words is finally selected.

[0102] As the state estimation difficulty increases from MH_01 to MH_05, the error of this application also increases. However, this application is still superior to the other three methods, and the error remains below 15.5 cm, demonstrating the robustness of this application. Figure 5 Some estimated and ground truth trajectories are depicted.

[0103] Real-scene experiment: To verify the feasibility and effectiveness of this application, a device equipped with Figure 6Several real - world experiments were conducted on the mobile platform of the shown sensor. These experiments were carried out indoors, using the HTC - VIVE motion capture system as the ground truth. In all experiments, a UWB antenna was placed at a fixed position to provide UWB measurements.

[0104] This application introduces specific sequences to evaluate the robustness and performance of our method under various sensor anomalies: Sequence Dynamic was recorded in a dynamic indoor environment where people were constantly moving; Sequence DarkRoom was recorded in a dimly lit environment, which greatly affected the visual front - end feature extraction and tracking process; Sequence WeakTexture involved moving in front of a clean, white wall with weak texture, making it almost impossible for the visual system to extract and track valid features; Sequence ZeroVelocity had special motions where it would suddenly stop after moving a certain distance; Sequence Loop involved returning to the starting position in a loop; Sequences UWB1 and UWB2 involved long - term and continuous movement around UWB anchors, where UWB1 experienced intermittent UWB signal blockages, resulting in anomalies. In these sequences, this application was compared with benchmark algorithms such as VINS - Mono, PL - VINS, and VIR - SLAM, and the comparison results are summarized in Figure 7 and the trajectory visualizations of some example sequences are in Figure 8

[0105] The method proposed in this application demonstrated superior positioning accuracy in all test sequences. The comprehensive evaluation emphasized the robustness and effectiveness of this application in handling various sensor anomalies and environmental challenges. These results verified its potential in reliable indoor positioning applications.

[0106] As described above, it is only the specific implementation manner of this application, but the protection scope of this application is not limited thereto. Any person skilled in the art in the technical field disclosed in this application can easily think of various changes or substitutions within the technical scope disclosed in this application, and these should all be covered within the protection scope of this application. Therefore, the protection scope of this application should be subject to the protection scope of the claimed rights.​

Claims

1. A SLAM positioning method based on multi-sensor fusion, characterized in that, Including: S100: Receive multiple sensor data, and respectively perform anomaly detection and processing on the multiple sensor data to obtain preliminary data corresponding to each sensor; wherein, the preliminary data at least includes visual feature data, IMU pre-integration data, and UWB distance data; S200: Initialize based on the visual feature data and IMU pre-integration data to estimate an initial pose, and combine the initial pose and UWB distance data to predict the UWB anchor position, and use the UWB anchor position as a fixed constraint; S300: Based on the fixed constraint, input the preliminary data into a sliding window to output drift-free odometry data; S400: Determine the motion state of the SLAM system based on the drift-free odometry data, and when it is detected that the motion state is static, perform zero-velocity update to obtain target pose data; S500: Based on the target pose data, determine whether the parallax between the current frame and the previous key frame exceeds a first preset threshold or determine whether the quantity of visual feature data is lower than a second preset threshold, and when the judgment result is exceeding or lower, confirm the current frame as a new key frame.

2. The SLAM positioning method based on multi-sensor fusion according to claim 1, characterized in that, The multiple sensor data at least includes camera images, IMU information, and UWB information; the anomaly detection and processing in step S100 includes: Extract point features and line features based on the camera images, combine the optical flow backtracking method and the gray-scale verification method to remove outliers, and filter out feature points on dynamic objects through dynamic target recognition to output visual feature data; Perform pre-integration on the IMU information to output IMU pre-integration data; And, perform sliding window anomaly detection and Gaussian smoothing on the UWB information, and use the anomaly-detected UWB information and the Gaussian smoothing processing result as UWB distance data.

3. The SLAM positioning method based on multi-sensor fusion according to claim 2, wherein The extraction of point features and line features includes: Based on the camera images, use the Shi-Tomasi method, and adopt KLT tracking and RANSAC confocal geometric constraints to screen inliers to obtain point features; And, based on the camera images, use the LSD algorithm, and combine the LBD descriptor and KNN matching to obtain line features.

4. The SLAM positioning method based on multi-sensor fusion according to claim 2, characterized in that, The estimation of the initial pose and the prediction of the UWB anchor position in step S200 include: Estimate the initial pose based on the visual feature data and IMU pre-integration data through visual-inertial alignment; Combine the initial pose and UWB distance data to predict the UWB anchor position through non-linear optimization.

5. The SLAM positioning method based on multi-sensor fusion according to claim 4, wherein The output of the drift-free odometry data in step S300 includes: Minimize the weighted sum of the prior information residual, IMU residual, point feature projection residual, line feature reprojection residual, and UWB residual to obtain a maximum a priori probability problem; And, output drift-free odometry data based on the preliminary data and the maximum a priori probability problem.

6. The SLAM positioning method based on multi-sensor fusion according to claim 5, wherein, The obtaining of the target pose data in step S400 includes: Based on the drift-free odometry data, combine IMU generalized likelihood ratio verification, sliding window variance analysis of acceleration variance and angular velocity variance, and simultaneously calculate the average parallax through visual parallax calculation; If the IMU generalized likelihood ratio is lower than the first preset threshold, or the acceleration variance and angular velocity variance are lower than the second threshold, and the average parallax is lower than the third preset threshold, it is determined that the SLAM system is in a stationary state, triggering zero-velocity update and correcting the pose and velocity within the sliding window to obtain the target pose data.

7. The SLAM positioning method based on multi-sensor fusion according to claim 1, wherein, The step S500 further includes: Activating loop closure detection based on the bag-of-words model to correct the global trajectory according to the loop closure detection result.

8. The SLAM positioning method based on multi-sensor fusion according to claim 7, characterized in that, The correcting the global trajectory includes: Based on the new key frame, matching the visual feature descriptors of the historical key frames through the bag-of-words model to detect loop closures; If a loop closure is detected, extracting the relative pose constraints between the loop closure frames, correcting the global trajectory through the pose graph, and feeding back the correction result to the sliding window.

9. A system using the multi-sensor fusion-based SLAM positioning method according to any one of claims 1-8, characterized in that, including: Anomaly detection module: used to receive multiple sensor data, and respectively perform anomaly detection and processing on the multiple sensor data to obtain the preliminary data corresponding to each sensor; wherein, the preliminary data at least includes visual feature data, IMU pre-integration data, and UWB distance data; Initialization module: used to initialize based on the visual feature data and IMU pre-integration data to estimate the initial pose, and predict the UWB anchor position in combination with the initial pose and UWB distance data, taking the UWB anchor position as a fixed constraint; Sliding window optimization module: used to input the preliminary data into the sliding window based on the fixed constraint to output drift-free odometer data; Zero-velocity update module: used to determine the motion state of the SLAM system based on the drift-free odometer data, and when it is detected that the motion state is static, perform zero-velocity update to obtain the target pose data; Loop closure module: used to judge whether the parallax between the current frame and the previous key frame exceeds the first preset threshold or judge whether the quantity of the visual feature data is lower than the second preset threshold, and when the judgment result is exceeding or lower, confirm the current frame as a new key frame.

10. The system according to claim 9, wherein The loop closure module is further used to activate loop closure detection based on the bag-of-words model to correct the global trajectory according to the loop closure detection result.

Images