Robot vision-inertia SLAM method and device and medium

Abstract

The invention discloses a robot vision-inertia SLAM method and device and a medium, and belongs to the technical field of computer vision and robot navigation. The method comprises the following steps: synchronously acquiring images and inertial data through a robot binocular camera and an IMU, performing feature enhancement on an original image in combination with a pre-trained deep learning model, introducing an adaptive brightness compensation mechanism and designing an image light supplementing module based on a generative adversarial network (GAN), recovering low-illumination image details, and improving the image quality of a low-illumination area; an entropy-based adaptive dynamic interference rejection algorithm is provided, and dynamic interference feature points are rejected in combination with IMU (Inertial Measurement Unit) data; a low-rank approximate improved graph optimization algorithm is adopted, and global map construction and pose optimization are accelerated; and through entropy-based nonlinear dynamic smoothing coefficient adjustment, the track stability is improved. According to the method, the positioning precision and robustness in a low-light environment are improved, the calculation efficiency is improved, and dynamic interference is effectively resisted.

Description

Technical Field

[0001] The present invention belongs to the technical field of computer vision and robot navigation, and specifically relates to a robot vision-inertial SLAM method, device and medium. Background Art

[0002] In practical application scenarios, such as low-light conditions at night and in dark indoor environments, existing SLAM methods have many problems. Vision-based SLAM methods have difficulty accurately extracting image features under low-light conditions, resulting in feature point mismatches or loss, affecting positioning accuracy and map construction. At the same time, moving objects in dynamic environments (such as pedestrians and vehicles, which correspond to the dynamic feature points mentioned above) can also interfere with the SLAM system, further reducing the robustness of the system. Although studies have attempted to improve SLAM performance through deep learning or visual-inertial fusion, existing methods still have shortcomings in low-light adaptability, dynamic interference detection, and computational efficiency. Traditional methods may only use histogram equalization to improve image brightness, but fail to fully utilize the feature enhancement capabilities of deep learning models; dynamic interference detection methods may rely on complex scene segmentation, which is computationally intensive and ineffective; traditional graph optimization algorithms have high computational complexity and cannot meet real-time requirements.

[0003] The existing VIO-Dual-PoseNet approach uses a CNN to extract image features, then combines them with pre-integrated IMU features and performs temporal fusion via a Transformer. While this approach improves dynamic adaptability, it suffers from high computational complexity and cannot meet real-time requirements. Therefore, developing a SLAM method that can achieve high-precision positioning in low light conditions while effectively resisting dynamic interference is of great practical significance.

[0004] Prior art publication CN105258702A discloses a global positioning method for a mobile robot based on SLAM navigation. The method includes the following steps: selecting sub-areas of the mobile robot's application environment; collecting data points; analyzing and determining whether the sub-area selection is reasonable; and finally, achieving global positioning of the mobile robot based on ICP. This method is used for global positioning of mobile robots, particularly AGVs (automated guided vehicles), in complex environments based on laser SLAM navigation. However, this method is not adaptable to complex environments, is costly, suffers from poor positioning accuracy, and is extremely inconvenient.

[0005] Publication No. CN114187424A discloses a 3D laser SLAM method based on histogram and surface feature matching, including the following steps: S1. Constructing a histogram HISTt of the current laser frame of a 3D laser radar; S2. Rotating the histogram HISTt based on NUM angle candidates to obtain a rotated histogram HISTtnum; S3. Matching the histogram HISTtnum with the histogram HISTt-1 at the previous moment, with the angle candidate yaw∧ having the highest score as the initial pose value at the current moment; S4. Extracting the set of planar feature points of the current laser frame; S5. Optimizing the pose increment of the initial pose value based on surface feature matching between the current laser frame and the previous laser frame; S6. Applying quadratic constraints based on the accumulated planar feature point cloud map to obtain the current pose of the laser radar. The robot's initial pose is determined using the histogram, and the robot's pose is optimized and its position is determined based on inter-frame surface feature constraints and map surface feature quadratic constraints. This method is computationally complex, has certain environmental requirements, and has poor versatility. Summary of the Invention

[0006] In response to the shortcomings of the above technologies, a robot visual-inertial SLAM method, equipment and medium are provided to solve problems such as feature degradation, sensitivity to dynamic interference, and insufficient computational efficiency in low-light scenes, and significantly improve the positioning accuracy and robustness in low-light environments.

[0007] To achieve the above technical objectives, the present invention discloses a robot visual-inertial SLAM method, which uses a robot with a binocular camera and an inertial sensor IMU, and the steps are as follows:

[0008] The binocular camera collects continuous frame images of the road in front of the robot, and the IMU is used to synchronously collect inertial data to obtain the robot's position information aligned in time and space;

[0009] Adaptive brightness compensation mechanism and generative adversarial network are used to fill the low-light image and generate low-light adaptation feature maps;

[0010] Generate multi-resolution feature levels through Gaussian pyramid decomposition, adaptively assign weights based on scene dynamics and lighting conditions, and generate a fused feature map after weighted fusion.

[0011] Extract and match visual feature points from adjacent frames and fuse inertial data to estimate the robot's pose. Visual feature points are significant local features extracted from the low-light adaptation feature map, including dynamic feature points representing moving objects and static feature points representing fixed environments.

[0012] Using the entropy-based dynamic interference elimination algorithm, the dynamic entropy value and static confidence of the visual feature points are quantified, the dynamic feature point interference in the visual feature point set is eliminated, and the static feature points in the inertial data are retained;

[0013] A low-rank approximation improved graph optimization algorithm is used to construct a global map of static feature points to optimize the robot's pose;

[0014] The trajectory jitter is eliminated by adjusting the dynamic smoothing coefficient based on entropy and optimizing the pose fusion by weighted averaging of the robot's historical trajectory.

[0015] Furthermore, the adaptive brightness compensation mechanism is used to restore low-light image details as follows:

[0016] Preprocess binocular images for binocular matching and time series frame matching;

[0017] Grayscale the binocular camera image and calculate its grayscale histogram distribution;

[0018] Calculate the brightness gain factor of each pixel in the binocular camera image based on the histogram statistical characteristics Among them I target is the target brightness value, I current is the current pixel brightness value;

[0019] Perform pixel-level brightness compensation for low-light areas.

[0020] Furthermore, the adversarial network is used to enhance the image quality of low-light areas in binocular camera images. The process of generating low-light adaptation feature maps is as follows:

[0021] The adversarial network consists of a sequentially connected generator G and a discriminator D. The generator G adopts a U-Net structure, with an adaptive brightness compensation module integrated at the input end and an adaptive brightness compensation mechanism running. The discriminator D adopts a PatchGAN structure.

[0022] Synchronously optimize the generator G and the discriminator D through a joint loss function: L total =L GAN +λL bright , L GAN Denotes the adversarial loss, L bright Indicates the loss of brightness consistency;

[0023]

[0024] Where pdata(x) refers to the probability distribution of the real data, D(x) represents the probability that the discriminator believes that the sample x comes from the real data, and E x～pdata(x) [logD(x)] refers to sampling sample x from pdata(x) and calculating the average value of logD(x); p z (z) refers to the noise distribution, G(z) refers to the light-filled image output by the generator, and D(G(z)) represents the discriminator’s judgment result on the generated image. Refers to pz (z) sampling noise z, calculate the average value of log(1-D(G(z)); λ represents the weight coefficient, which is 10. If L bright If the value is too large, increase λ;

[0025]

[0026] Where N is the number of samples in the training batch, I target Refers to the target brightness value, G(z i ) refers to the generator's response to the i-th low-light image z i Output brightness;

[0027] Fix the discriminator D and minimize L total Update the parameters of generator G; fix generator G and maximize L GAN Update the discriminator D parameters; iterate alternately until convergence, and output the target brightness fill-light image.

[0028] Furthermore, we generate a multi-resolution feature hierarchy through Gaussian pyramid decomposition, adaptively assign weights based on scene dynamics and lighting conditions, and generate a fused feature map after weighted fusion as follows:

[0029] 1) Input the low-light enhanced binocular image into the pre-trained model to generate a multi-scale pyramid feature map; Gaussian smoothing and downsampling are performed on the feature map to construct a three-layer Gaussian pyramid, with the layer weights assigned to 0.6, 0.3, and 0.1 from high to low resolution;

[0030] 2) The feature maps of different scales are weighted and summed according to the weights, and spliced ​​in the channel dimension to generate a multi-scale fusion feature map;

[0031] 3) Acceleration a of IMU k and angular velocity ω k Perform pre-integration to calculate the displacement change and rotation change at adjacent moments. The displacement change is: where v k and a k Represent the linear velocity and acceleration at the kth moment respectively, Δt represents the sampling time interval of IMU; rotation change: Δθ = ω k Δt, where ω k represents the angular velocity at the kth moment;

[0032] 4) Update the state vector based on the original visual feature points and pre-integration results The two-dimensional state vector Contains position (x, y) and heading angle θ, F k is the state transfer matrix, B k is the control input matrix, u kis the IMU measurement value; update the error covariance: where Q k Refers to the process noise covariance, including IMU noise and model approximation error: Q k =Cov IMU +Cov model ; Output initial pose estimate Used for global map construction and navigation.

[0033] Furthermore, the process of removing high entropy and low confidence dynamic feature point interference from inertial data is as follows:

[0034] Based on the visual feature points extracted by the binocular camera, the entropy value of the velocity distribution of the feature points in the sliding time window is calculated ν i Refers to the velocity vector of the i-th feature point p(ν i ) is the probability distribution of the feature point velocity, N refers to the total number of feature points in the window; the threshold θ=θ is adjusted according to the entropy value base (1+αH), where α=0.1 is the adjustment coefficient, θ base is the basic threshold, determined by experimental calibration method;

[0035] Using the formula: Calculate the dynamic adjustment confidence of the visual feature points, when C d When it exceeds the preset threshold θ, it is determined to be a dynamic feature point and removed, otherwise it is retained as a static feature point for positioning and mapping; where v i is the velocity vector of the i-th feature point, is the mean velocity of all feature points in the sliding time window, Cov IMU and Cov flow is the IMU and optical flow noise covariance matrix.

[0036] Furthermore, the specific steps of using the low-rank approximation improved graph optimization algorithm to construct a global map of the static feature point set and optimize the robot's posture are as follows:

[0037] 1) In the two-dimensional plane coordinate system, the information matrix Ω is constructed by combining the static feature point coordinate change error and the IMU pre-integration error ij , for Ω ij Perform singular value decomposition Ω ij =UΣV T Get the left singular vector matrix U, singular value matrix Σ=diag(σ1,σ2,···,σ m )(singular values ​​in descending order) and the transpose of the right singular vector matrix V T , determine the value of the number of retained singular values ​​k by the energy proportion method: calculate the sum of squares of the first k singular values ​​and select The minimum k value, if k>10, then take k=10;

[0038] 2) Keep the first k largest singular values ​​Σ in Σ k =diag(σ1,σ2,···,σ k ), reconstruct the approximate matrix Among them U k and V k The first k columns of U and V respectively;

[0039] 3) Input graph optimization algorithm to construct linear system in It is a vector formed by the weighted combination of the linearized residual error of the static feature point coordinate error and the IMU pre-integration error, and the approximate pseudo-inverse is calculated. in Solving Increment And iteratively update the pose and map point coordinates until the residual norm ||b||<10 -4 Or when the maximum number of iterations reaches 20, the optimization results are output.

[0040] Furthermore, the constructed global map is subjected to two-dimensional loop closure detection and global correction based on deep feature descriptors as follows:

[0041] 1) Use GAN to enhance image input to pre-train convolutional networks, extract deep semantic features, build a scene description sub-database, and store keyframe feature maps and their two-dimensional poses (x, y, θ), where (x, y) is the position and θ is the heading angle;

[0042] 2) Calculate the cosine similarity between the current frame and the historical key frames in real time: Among them, F i and F j are the low-light enhanced feature maps of the current frame and the historical key frame respectively; if S i,j >0.8, then geometric consistency verification is triggered:

[0043] Use the static feature points after dynamic interference removal to match and obtain the coordinate matching point pair between the current frame and the historical key frame (p k ,q k ), where p k and q k Refers to the coordinates p of the matching points between the current frame and the historical frame k =[u k ,v k ] T ,q k =[u′ k ,v′ k ] T , by minimizing the coordinate transformation error Where N represents the total number of static feature point pairs involved in matching, and the optimal posture transformation matrix of the robot is solved Where R is the rotation matrix t=[Δx,Δy] T is the translation vector;

[0044] Calculate all (p k ,q k ) Average translation error and the average rotation error If the average translation error is ≤ 0.1m and the average rotation error is ≤ 1°, then it is considered that one of the geometric consistency verification conditions is met;

[0045] Calculate each (p k ,q k )'s single-point residual e k =||p k -(R·q k +t)||, if e k <2 pixels, the matching point pair is determined to be an inlier; the inlier ratio is counted. If the proportion of interior points exceeds 70%, it is considered that the second geometric consistency verification condition is met;

[0046] Verify the orthogonality of the rotation matrix and calculate the magnitude of the translation vector t Is it ≤1m? If all of them are met, the third geometric consistency verification condition is met;

[0047] 3) If all three conditions are met, the geometric consistency verification is considered to be successful, triggering global pose graph optimization, updating the two-dimensional distribution of map points and the covariance matrix Cov(x,y), and outputting the corrected map.

[0048] Furthermore, the weight distribution is performed based on the historical trajectory information of the robot movement and combined with weighted average to dynamically adjust the smoothing coefficient. The specific steps are as follows:

[0049] 1) Use static feature points to reflect the robot's movement trajectory, and calculate the velocity component ν of each static feature point in the two-dimensional plane x and ν y The speed range is divided into M = 30 intervals, where M is the number of speed bins, and the speed probability distribution p(ν x )、p(ν y ), and calculate the kinetic entropy

[0050] 2) Adjust the smoothing coefficient α according to the motion entropy H: Among them H max is the maximum entropy value, determined by the number of bins M: H max =ln(M),α min and αmax is the minimum and maximum value of the smoothing coefficient, usually α min =0.2;α max =0.8;

[0051] 3) Calculate the weight of each frame in the sliding window according to the weight distribution rule ω i represents the weight of the historical pose of the i-th frame, where N represents the total number of frames in the sliding window;

[0052] 4) Weighted fusion window pose to generate smoothed pose Where T i =(x i ,y i θ i ) represents the pose of the i-th frame, and T smooth As the final trajectory output, it is used for positioning and navigation tasks.

[0053] A computer device includes a processor and a memory, wherein the processor is electrically connected to the memory, the memory is used to store instructions and data, and the processor is used to execute a robot visual-inertial SLAM method.

[0054] A computer-readable storage medium stores a computer program, which is suitable for being loaded by a processor and executing a robot visual-inertial SLAM method.

[0055] Beneficial effects: This method achieves the high efficiency and robustness of dynamic anti-interference SLAM; through the dynamic interference elimination and low-light enhanced visual-inertial SLAM method, it significantly improves the positioning accuracy in low-light environments and effectively resists the interference of dynamic objects. The specific steps include: using binocular cameras and IMU to collect data, and through the adaptive brightness compensation mechanism and the generative adversarial network adaptive brightness compensation, pixel-level correction is performed on low-light areas to restore texture details, improve the stability and matching accuracy of feature point extraction, build an initial pose estimation model, and adopt a dynamic object recognition algorithm with adaptive spatiotemporal consistency constraints. According to the scene dynamics and sensor noise level, the confidence threshold is adjusted in real time to avoid the limitations of fixed thresholds and improve the robustness of dynamic detection; the improved graph optimization algorithm is used for global map construction and pose optimization, and finally the trajectory smoothing correction is performed through nonlinear dynamic smoothing coefficient adjustment and weighted average strategy. In addition, this method has simple steps, occupies little hardware resources, and is suitable for embedded devices and mobile robots; it uses a visual-inertial joint optimization model to replace the traditional single sensor solution, enhancing the system's adaptability in complex dynamic scenarios; this method achieves breakthroughs in the three core indicators of dynamic anti-interference, low-light adaptation, and computational efficiency; this method has high precision and strong real-time performance, and can effectively support application scenarios such as autonomous navigation, virtual reality, and augmented reality.

[0056] The SLAM method in this application includes sensor data preprocessing, feature extraction, fusion of inertial data and optimization algorithms. Inertial data fusion and SLAM framework are prior art, but this patent application makes the following improvements:

[0057] Improvements to inertial data fusion: By eliminating interference from dynamic feature points and retaining only static feature points consistent with the static environment for IMU data fusion, the pose estimation bias introduced by dynamic objects is reduced, thereby reducing the IMU pre-integration cumulative error. Low-rank approximation is used to accelerate sparse matrix solutions and improve real-time performance.

[0058] Improvements to the SLAM framework: Based on low-light enhanced images, deep feature descriptors with semantic information are extracted through pre-trained deep convolutional neural networks to replace traditional manual features (such as SIFT), thereby improving the recall rate of feature matching in complex scenarios. Motion entropy-driven smoothing coefficient adjustment is introduced (by analyzing the information entropy value of the motion trajectory distribution of static feature points and dynamically adjusting the weight distribution of historical frames and current frames) to solve the trajectory jitter problem of traditional fixed-weight algorithms.

[0059] Although low-light enhancement involves video processing technology, its purpose is to provide higher-quality image data for the visual-inertial SLAM system (restoring texture details through adaptive brightness compensation and generative adversarial networks), thereby improving the accuracy and stability of feature extraction.

[0060] The dynamic interference removal algorithm was proposed to address the lack of robustness of SLAM systems in dynamic environments. Using an adaptive dynamic interference removal algorithm based on velocity entropy (calculating the information entropy of the velocity distribution of feature points within a sliding window and dynamically adjusting the confidence threshold based on IMU data and optical flow noise covariance), it effectively identifies and removes dynamic feature points, thereby improving the accuracy of pose estimation. This involves quantifying and adaptively processing the motion complexity of dynamic environments, not just video processing. BRIEF DESCRIPTION OF THE DRAWINGS

[0061] Figure 1 Schematic diagram of the process of the robot visual-inertial SLAM method in an embodiment of the present invention;

[0062] Figure 2 is a flowchart for implementing the adaptive brightness compensation mechanism in an embodiment of the present invention;

[0063] Figure 3 This is a structural diagram of adaptive brightness compensation based on a generative adversarial network in an embodiment of the present invention;

[0064] Figure 4 Schematic diagram of the working principle of the entropy-based adaptive dynamic interference removal algorithm in an embodiment of the present invention;

[0065] Figure 5 Schematic diagram of the process of adjusting the historical trajectory weights by adaptive coefficients for smoothing in an embodiment of the present invention; DETAILED DESCRIPTION

[0066] The embodiments of the present invention are further described below with reference to the accompanying drawings:

[0067] The present invention provides a robotic visual-inertial SLAM method. This method combines image sequences captured by a binocular camera with inertial measurement unit data, utilizes a deep learning model to perform feature enhancement processing on raw images in low-light environments, and improves image quality in low-light environments through an adaptive brightness compensation mechanism, thereby significantly improving positioning accuracy and anti-interference capabilities. The following describes specific embodiments of the present invention in detail with reference to the accompanying drawings.

[0068] Visual feature points: Feature points extracted from the enhanced image through image processing (such as multi-scale pyramid decomposition and feature matching) for pose estimation and map construction.

[0069] Dynamic feature points: These are not directly derived from the IMU but are extracted from visual data (images). These are feature points of moving objects in the scene (such as pedestrians and vehicles). Their motion trajectories are inconsistent with the static environment, interfering with positioning accuracy and requiring filtering using a dynamic interference rejection algorithm. Examples of these include pedestrian outlines, vehicle headlights, and the edges of moving objects.

[0070] Static feature points: Feature points extracted from visual data (images) that belong to fixed objects in the scene (such as walls and floors). Their motion trajectories are consistent with the static environment motion predicted by the IMU. These points are retained for global map construction and pose optimization. Examples include corner points of walls, floor textures, and the geometric structure of fixed objects.

[0071] Visual feature points: Significant feature points, such as corners and edge intersections, extracted from images captured by a binocular camera using image processing techniques (such as multi-scale pyramid decomposition and feature matching). These are used for pose estimation and map construction. These include both dynamic and static feature points.

[0072] Dynamic feature points: These are feature points generated by moving objects (such as pedestrians and vehicles) in the scene. Their motion trajectories are inconsistent with the static background environment, which can interfere with positioning accuracy and must be filtered out using a dynamic interference removal algorithm.

[0073] Static feature points: These are feature points generated by fixed environmental elements in the scene (such as walls, floors, and buildings). Their motion trajectories are consistent with the static environmental motion predicted by the IMU and are the core input for SLAM systems used for mapping and positioning.

[0074] Dynamic feature points are not noise points. Noise points are usually invalid feature points caused by sensor noise or image processing errors. Dynamic feature points are real feature points that belong to moving objects (such as vehicle lights or pedestrian outlines). Their dynamic nature comes from the object's motion and must be eliminated through algorithms to reduce interference.

[0075] The function of dynamic feature points is to reflect the existence of moving objects in the scene. However, because their movement is inconsistent with the static environment, they need to be filtered through a dynamic interference removal algorithm to avoid contaminating the global map.

[0076] Dynamic feature points are real feature points of moving objects and need to be eliminated to avoid interference; static feature points are stable feature points of environmental elements and are the core input of navigation maps; both are not noise points, but feature points of different categories distinguished by algorithms.

[0077] like Figure 1 As shown, the present invention discloses a robot visual-inertial SLAM method, the steps of which are as follows:

[0078] Step 1: Use a binocular camera to capture a scene image sequence and simultaneously obtain the acceleration and angular velocity data from the IMU to ensure synchronization between the two on the time axis. The captured image sequence is input into a pre-trained deep learning model to perform feature enhancement on the original image in a low-light environment, generating a highly robust low-light adaptation feature map.

[0079] In this process, adaptive brightness compensation (pixel-level correction) is combined with the GAN fill-light module to restore the texture details of low-light images and generate high-quality fill-light images, providing clearer input for subsequent feature extraction.

[0080] like Figure 2 As shown: The specific steps of the adaptive brightness compensation mechanism are:

[0081] (1) Analyze the binocular camera input image, extract the brightness information of each pixel, convert the color image into a grayscale image using grayscale processing, and calculate its grayscale histogram distribution;

[0082] (2) Calculate the ratio of target brightness to current brightness to obtain the brightness gain factor Among them I target is the target brightness value, I current is the current pixel brightness value; the obtained brightness gain factor g is applied to each pixel of the image to perform pixel-level brightness correction on low-light areas, thereby improving image contrast and retaining key texture information;

[0083] (3) Dynamically adjust the brightness compensation strategy based on the actual application scenario. When the ambient light changes, the compensation effect is further optimized by combining the ambient light sensor data.

[0084] like Figure 3 As shown in Figure 2, the specific steps of adaptive brightness compensation based on generative adversarial networks are:

[0085] (1) By training the generator G and the discriminator D, the brightness and details of the input image are restored, and the generator G and the discriminator D are optimized synchronously through the joint loss function. The joint loss function is defined as: L total =L GAN +λL bright , L GAN Refers to fighting losses,

[0086] L bright Refers to the loss of brightness consistency: Among them, E x～pdata(x) [logD(x)] represents the discriminant's ability to discriminate true samples, which means calculating the average value of logD(x) for samples x sampled from the true data distribution pdata(x). pdata(x) refers to the probability distribution of the true data, and D(x) represents the probability that the discriminator believes that sample x comes from the true data (in the range [0,1]). Indicates the discriminant ability of the discriminator to generate samples, which means the discriminant ability of the discriminator to generate samples from the noise distribution p z(z) The noise z sampled, the generator generates samples G(z), calculates the average value of log(1-D(G(z)), D(G(z)) represents the probability that the discriminator believes that the generated sample G(z) is the real data; λ is the weight coefficient, usually 10, if the brightness deviation of the generated image is serious, then increase λ, L bright Defined as Ensure that the brightness of the generated image meets the expected target and avoid local overexposure or underexposure, where N refers to the number of samples in the training batch, I target Refers to the target brightness value, G(z i ) refers to the generator's response to the i-th low-light image z i Output brightness;

[0087] (2) During the training process, the generator G optimizes the fidelity and brightness distribution of the generated image through a joint loss function, and the discriminator D improves its discrimination ability by distinguishing between real and fake images;

[0088] (3) The two are iterated alternately, and the optimization goal of generator G is to minimize L total , so that G(z) approaches the real image distribution and maintains brightness consistency; the optimization goal of the discriminator D is to maximize L GAN , improve the ability to distinguish the generated images; ultimately, it can generate a fill-light image with a brightness distribution that meets the expected target from the low-light image.

[0089] Step 2: Perform multi-scale pyramid decomposition on the enhanced feature map and build an initial pose estimation model based on IMU data.

[0090] (1) The feature map F enhanced by the deep learning model base Input the pre-trained deep learning model to generate a multi-scale pyramid feature map; Gaussian smoothing and downsampling are performed on the feature map to construct a three-layer pyramid (number of levels N = 3), and the layer weights are distributed from high to low resolution as 0.6, 0.3, and 0.1;

[0091] (2) Fuse feature maps of different scales, assign weights to feature maps of each scale according to their importance, and perform weighted summation. The weighted multi-scale feature maps are concatenated in the channel dimension to obtain the final multi-scale fused feature map.

[0092] (3) Perform pre-integration processing on the IMU data to calculate the displacement and rotation changes between two consecutive moments. The displacement change is: where v k and a k Represent the linear velocity and acceleration at the kth moment respectively, Δt represents the sampling time interval of IMU; rotation change: Δθ = ω k Δt, where ω k represents the angular velocity at the kth moment;

[0093] According to the original visual feature points extracted from the enhanced image before dynamic interference removal and the IMU pre-integration results, the state vector and error covariance matrix are predicted; the two-dimensional state vector is defined Contains position (x, y) and heading angle θ, and the state vector is updated: Among them F k is the state transfer matrix, which is dynamically updated by the angular velocity and acceleration measurements of the IMU and is used to predict the posture state at the next moment. k is the control input matrix, u k is the IMU measurement value; the error covariance matrix is ​​updated: Used to quantify the uncertainty of the estimate at the current moment, where Q k Refers to the process noise covariance, including IMU noise and model approximation error: Q k =Cov IMU +Cov model ; The fused initial pose estimation result As the robot's two-dimensional plane pose output, it is used for global map construction and navigation.

[0094] Step 3: Dynamic interference detection and elimination are performed on the feature map fused in Step 2. Based on the visual feature points extracted by the binocular camera in the two-dimensional plane, an adaptive dynamic object recognition algorithm based on spatiotemporal consistency constraints is designed. Combining the pose uncertainty predicted by the IMU and the optical flow tracking error, the dynamic confidence threshold is dynamically adjusted and the dynamic confidence of each feature point is calculated. If the dynamic confidence exceeds the set threshold, the feature point is determined to be a dynamic object and is eliminated. This step effectively reduces the interference of dynamic objects on positioning accuracy.

[0095] like Figure 4 As shown in Figure 2, the specific steps of the entropy-based adaptive dynamic interference removal algorithm are:

[0096] (1) Calculate the entropy value of the velocity distribution of feature points in the window Among them, the feature point refers to the key observation point used to quantify the motion state during the dynamic object recognition process, ν i Refers to the velocity vector of the i-th feature point in the two-dimensional plane p(ν i ) is the probability distribution of the feature point velocity, N refers to the total number of feature points in the window; the threshold θ=θ is dynamically adjusted according to the entropy value base (1+αH), where α=0.1 is the adjustment coefficient, θ base It is the basic threshold value, which can be determined by experimental calibration method: in a purely static environment, the dynamic confidence distribution of static feature points is collected by binocular camera and IMU data, the dynamic confidence mean μ and standard deviation 3σ are calculated, and the basic threshold θ is set. base=μ+3σ, verify and optimize the basic threshold in dynamic scenes to ensure the missed rejection rate is less than 5%;

[0097] (2) Calculating dynamic confidence where v i is the velocity vector of the i-th feature point, is the mean velocity of all feature points, Cov IMU and Cov flow is the IMU and binocular eye flow noise covariance matrix, when C d When the value exceeds the preset threshold, it is determined to be a dynamic object feature point and removed. If it is lower than the threshold, it is determined to be a static point and retained for positioning and mapping.

[0098] Step 4: Using the improved graph optimization algorithm to construct a global map and optimize the pose of the feature point set after removing dynamic interference in Step 3. Building on traditional graph optimization, a low-rank approximation method is introduced to accelerate sparse matrix solutions. Low-rank decomposition techniques are used to approximate the information matrix as the product of two low-rank matrices, significantly reducing computational complexity while ensuring the accuracy of the optimization results.

[0099] The specific steps of decomposition and reconstruction of the low-rank approximation method in the improved graph optimization algorithm are:

[0100] (1) In the two-dimensional plane coordinate system of the binocular camera, the information matrix is ​​constructed by combining the projection error of the static feature points on the two-dimensional plane and the IMU pre-integration error in is the Jacobian matrix of the visual error (n is the number of feature points, each point corresponds to two observations x and y); is the Jacobian matrix of the IMU pre-integration error (g is the total number of key frames, including the translation and heading angle in the two-dimensional plane); are the inverse of the visual and IMU noise covariance matrices, respectively.

[0101] Perform singular value decomposition of the information matrix Ω ij =UΣV T , we get three parts: the left singular vector matrix U, the singular value matrix Σ=diag(σ1,σ2,···,σ m ) where the singular values ​​are arranged in descending order, and the transpose of the right singular vector matrix V T , determine the value of the number of retained singular values ​​k by the energy proportion method, and calculate the square sum E of the first n singular values total =σ1 2 +σ2 2 +···+σ m 2 , accumulate the squares of the singular values ​​from large to small, and find the minimum k value that satisfies: (σ1 2 +σ2 2 +···+σk 2 )≥η·E total , where the threshold η is 95%, corresponding to 95% energy retention. If k>10, k=10 is forced;

[0102] (2) Calculate the proportion of the sum of the squares of the first K singular values ​​to the total energy, and select the minimum K value that meets the formula conditions: The threshold is usually set based on experience, usually 95% or 99%); retain the first k largest singular values ​​Σ in ∑ k =diag(σ1,σ2,···,σ k ), and take the first k columns of U to form the corresponding left singular vector matrix U k , take the first k columns of V to form the corresponding right singular vector matrix V k , the remaining singular values ​​are set to zero, and the approximate matrix is ​​reconstructed This step achieves a low-rank approximation by truncating the singular value matrix.

[0103] (3) The reconstructed low-rank approximation matrix Used in graph optimization algorithms to replace the original information matrix Ω ij , in order to speed up the sparse matrix solution process, the pose increment Δx is solved by the following steps: Construct a linear system in It is a vector composed of the weighted combination of the linearized residuals of all constraints (visual coordinate transformation error of static feature points and IMU pre-integration error); based on the low-rank decomposition result, the approximate pseudo-inverse is directly calculated. in Solving Increment Iteratively update the robot pose and map point coordinates according to Δx until the convergence condition is met (residual norm ||b||<10 -4 or reaches a maximum number of iterations of 20 times), and finally outputs the optimized global map and robot pose.

[0104] Step 5: Perform loop detection and global correction based on deep feature descriptors on the global map obtained in step 4, extract the deep semantic features of the key frames in the global map, build a scene descriptor database, and calculate the scene similarity between the current frame and the historical key frames in real time. When the similarity exceeds the threshold, perform geometric consistency verification. After verification, trigger global pose graph optimization, correct the accumulated error, and ensure the global consistency and positioning accuracy of the map. The specific steps include:

[0105] Deep semantic features are extracted from the keyframes in the global map to build a scene description sub-database. The keyframe insertion conditions for the binocular camera include: when the robot displacement exceeds 5cm or the heading angle changes by more than 30°, if the above thresholds are not met, a keyframe is forcibly inserted every 5 seconds; the cosine similarity between the current frame and the historical keyframes is calculated in real time, and the formula is: Among them, F i and F j They are the feature maps of the current frame and the historical key frame respectively; when the similarity S i,j When it exceeds 0.8, the steps for performing geometric consistency verification are:

[0106] By minimizing the coordinate transformation error where p k and q k Refers to the two-dimensional coordinates p of the matching point between the current frame and the historical frame k =[u k ,v k ] T ,q k =[u′ k ,v′ k ] T , N represents the total number of static feature point pairs involved in matching, and solves the optimal posture transformation matrix of the robot Where R is the rotation matrix t=[Δx,Δy] T is the translation vector;

[0107] Calculate all matching point pairs (p k ,q k )’s average translation error Represents the average value of the position error of all matching point pairs after transformation, as well as the average rotation error Denotes the estimated heading angle change Δθ k The average angle deviation from the true value. k ,q k ) if the average translation error is ≤0.1m and the average rotation error is ≤1°, then it is considered to meet one of the geometric consistency verification conditions;

[0108] Calculate each matching point pair (p k ,q k )'s single-point residual e k =||p k -(R·q k +t)||, if e k <2 pixels, the matching point pair is determined to be an inlier; the inlier ratio is counted. If the proportion of interior points exceeds 70%, it is considered that the second geometric consistency verification condition is met;

[0109] Verify that the rotation matrix is ​​orthogonal (R T R is equal to the identity matrix), check the modulus of the translation vector t Is it ≤1m? If all of them are satisfied, it means that the transformation matrix T is reasonable and meets the third geometric consistency verification condition;

[0110] If all the above conditions are met, the system determines that the geometric consistency verification has passed, triggers global pose graph optimization, corrects the accumulated errors, and ensures the global consistency and positioning accuracy of the map.

[0111] Step 6: Post-process the optimized pose estimation results, use the adaptive coefficient to adjust the historical trajectory weight, and perform smoothing;

[0112] like Figure 5 As shown, static feature points are used to reflect the movement trajectory of the robot, and the motion velocity component ν of each static feature point in the two-dimensional plane is recorded. x and ν y , divide the speed range into M = 30 intervals, calculate the number of speed values ​​in each interval, and obtain the probability distribution p(ν x )、p(ν y );Calculate the motion entropy of static feature points Dynamically adjust the smoothing coefficient according to the motion entropy H Among them H max is the maximum entropy value, determined by the number of bins M: H max =ln(M),α min and α max is the minimum and maximum value of the smoothing coefficient, usually α min =0.2;α max = 0.8, H represents motion entropy; the weights of historical frames and current frames are adjusted according to the smoothing coefficient α Perform weighted fusion on the poses within the window to generate a smoothed pose Where T i Represents the 2D pose of the i-th frame (including translation and heading, and output the smoothed pose as the final trajectory.

[0113] Example 1: Robot visual-inertial SLAM method is as follows:

[0114] Step 1: Data acquisition: Use the binocular camera to capture a sequence of scene images and simultaneously obtain the acceleration and angular velocity data from the inertial measurement unit. The binocular camera's image acquisition frequency is set to 30 frames per second, and the IMU's data sampling frequency is set to 100 Hz to ensure synchronization between the two on the time axis. The binocular camera image is grayscaled and its grayscale histogram distribution is calculated.

[0115] Step 2: Image feature enhancement processing: the collected image sequence is input into the pre-trained deep learning model to perform feature enhancement processing on the original image in the low-light environment, generating a low-light adaptation feature map with high robustness. In this process, an adaptive brightness compensation mechanism is introduced to calculate the brightness gain factor of each pixel based on the histogram statistical characteristics of the input image. Among them I target is the target brightness value, I current is the current pixel brightness value; g is applied to each pixel in the image to complete brightness correction. This method can significantly improve visibility in low-light areas while preserving key texture information and reducing the probability of feature point mismatching.

[0116] Step 3: Image light filling processing, design an adaptive brightness compensation based on the generative adversarial network, optimize the generator G and the discriminator D synchronously through the joint loss function, restore the brightness and details of the input image, and define the joint loss function L total =L GAN +λL bright , L GAN Refers to the loss of resistance, L bright Refers to brightness consistency loss, and the adversarial loss function is defined as Where E is the mathematical expectation, z is the low-light image, x~pdata(x) represents the real data distribution of the real normal-light image x, z~p z (z) represents the input distribution of the low-light image z, G(z) is the light-filled image output by the generator, D(x) represents the probability estimate of the discriminator that the real image x is “true”, and D(G(z)) represents the probability estimate of the discriminator that the generated image G(z) is “true”; λ is the weight coefficient; L bright is the brightness mean square error between the corrected image and the normal illumination image: Where N is the number of samples in the training batch, I target Refers to the target brightness value (preset or reference value), G(z i ) The generator generates the i-th low-light image z i The output of the generator G is a U-Net structure, and the discriminator D is designed using PatchGAN. During the training process, by alternately updating the parameters of the generator and the discriminator, the generator G optimizes the realism and brightness distribution of the generated image through a joint loss function, and the discriminator D improves its discrimination ability by distinguishing between real and fake images; the optimization goal of the generator G is to minimize L total , so that G(z) approaches the real image distribution and maintains brightness consistency; the optimization goal of the discriminator D is to maximize L GAN , improve the ability to distinguish the generated images; ultimately, it can generate a fill-light image with a brightness distribution that meets the expected target from the low-light image.

[0117] Step 4: Fusion of visual and inertial data, multi-scale pyramid decomposition of the enhanced feature map, and construction of the initial pose estimation model in combination with IMU data. Specifically, the extended Kalman filter is used to loosely couple the visual feature points with the IMU data, and the formula is used. in is the estimated state value at the kth moment, including position, velocity and attitude; F k The state transfer matrix is ​​dynamically updated by the angular velocity and acceleration measurements of the IMU and is used to predict the posture state at the next moment. k is the control input matrix, u k The IMU measurement value is output as the initial pose estimation result after fusion, and the preliminary fusion of visual and inertial data is achieved through this formula.

[0118] Step 5: Dynamic interference detection and elimination. Design an entropy-based adaptive dynamic interference elimination algorithm, calculate the entropy value H of the velocity distribution of feature points in the window, and dynamically adjust the threshold; calculate the dynamic confidence of each feature point. where v i is the velocity vector of the i-th feature point, is the mean velocity of all feature points, Σ IMU and Σ 光流 is the covariance of IMU and optical flow noise, if C d If the threshold is exceeded, the feature point is determined to be a dynamic object and is removed. This step can effectively reduce the interference of dynamic objects on positioning accuracy.

[0119] Step 6: Global map construction and pose optimization. Based on the feature point set after removing dynamic interference, the improved graph optimization algorithm is used to construct the global map and optimize the pose. Based on the traditional graph optimization, the low-rank approximation method is introduced to accelerate the sparse matrix solution, and the information matrix Ω is converted to ij Perform singular value decomposition and decompose it into U∑V T , thereby greatly reducing the computational complexity while ensuring the accuracy of the optimization results.

[0120] Step 7: Loop detection and global correction. Dynamically adjust the key frame selection frequency according to the motion distance and angle. Build a scene description sub-database based on the deep feature descriptor. Calculate the similarity between the current frame and the historical key frames in real time. Among them, F i and F j are the feature maps of the current frame and the historical key frame respectively, φ(·) is the deep feature encoder based on convolutional neural network; when the similarity S i,j When the preset threshold is exceeded, the geometric consistency verification steps are as follows:

[0121] By minimizing the coordinate transformation error formula min∑ k||p k -T·q k || 2 Solve the optimal pose transformation matrix T, where p k Refers to the kth matching 3D map point coordinates (observation values) in the current frame, q k Refers to the corresponding 3D map point coordinates (reference values) in the historical keyframe, and calculates all matching point pairs (p k ,q k )’s average coordinate transformation error If the average error is lower than the preset threshold (for pixel coordinate system, the error is ≤ 2 pixels; for 3D coordinate system, the translation error is ≤ 0.1m, and the rotation error is ≤ 1°), the geometry is considered consistent.

[0122] Calculate the residual ||p for each matching point pair k -T·q k If the residual is less than a preset threshold (such as pixel level or 3D distance threshold), the point is determined to be an inlier. If the ratio of inliers exceeds the set threshold, the geometric consistency verification is considered to have passed;

[0123] Verify the rationality of the transformation matrix T: the orthogonality of the rotation matrix (R T R=I); whether the modulus of the translation amount t is within a reasonable range;

[0124] If all the above conditions are met, the system determines that the geometric consistency verification has passed, triggers global pose graph optimization, corrects the accumulated errors, and ensures the global consistency and positioning accuracy of the map.

[0125] Step 8: Trajectory smoothing correction: process the optimized pose estimation results, adjust the weight distribution of historical trajectories through dynamic smoothing coefficients, and smooth the historical trajectories in combination with weighted averaging.

[0126] Specifically: save the latest N frames of pose {T1, T2, ..., T N}, calculate the relative transformation ΔT of adjacent poses i =T i - 1 T i+1 , ΔT i Convert to translation vector t i and the rotation angle θ i , statistics of its distribution p(t i ,θ i ), calculate the entropy value Calculate the smoothing coefficient based on the entropy value H Among them H max is the predefined maximum entropy value, α min and α maxis the minimum and maximum value of the smoothing coefficient; weighted fusion is performed on the pose within the window to generate the smoothed pose The weight of the i-th frame in the window is T i Represents the pose of the i-th frame; the smaller α is, the faster the weight of the historical frames decays, focusing on the current frame and responding to changes quickly; the larger α is, the more evenly the weight of the historical frames is distributed, enhancing the smoothing effect.

[0127] Step 9: Output the system results. The smoothed and corrected pose estimation results are output to the navigation system for real-time positioning and path planning.

Claims

1. A robot visual-inertial SLAM method using a robot with a binocular camera and an inertial sensor IMU, characterized in that: Here are the steps: The binocular camera collects continuous frame images of the road in front of the robot, and the IMU is used to synchronously collect inertial data to obtain the robot's position information aligned in time and space; Adaptive brightness compensation mechanism and generative adversarial network are used to fill the low-light image and generate low-light adaptation feature maps; Generate multi-resolution feature levels through Gaussian pyramid decomposition, adaptively assign weights based on scene dynamics and lighting conditions, and generate a fused feature map after weighted fusion. Extract and match visual feature points from adjacent frames and fuse inertial data to estimate the robot's pose. Visual feature points are significant local features extracted from the low-light adaptation feature map, including dynamic feature points representing moving objects and static feature points representing fixed environments. Using the entropy-based dynamic interference elimination algorithm, the dynamic entropy value and static confidence of the visual feature points are quantified, the dynamic feature point interference in the visual feature point set is eliminated, and the static feature points in the inertial data are retained; A low-rank approximation improved graph optimization algorithm is used to construct a global map of static feature points to optimize the robot's pose; Trajectory jitter is eliminated by adjusting the dynamic smoothing coefficient based on entropy and optimizing the posture fusion by combining the weighted average of the robot's historical trajectory.

2. The robot visual-inertial SLAM method according to claim 1, wherein The process of restoring low-light image details using the adaptive brightness compensation mechanism is as follows: Preprocess binocular images for binocular matching and time series frame matching; Grayscale the binocular camera image and calculate its grayscale histogram distribution; Calculate the brightness gain factor of each pixel in the binocular camera image based on the histogram statistical characteristics Among them I target is the target brightness value, I current is the current pixel brightness value; Perform pixel-level brightness compensation for low-light areas.

3. The robot visual-inertial SLAM method according to claim 2, wherein The process of using the adversarial network to enhance the image quality of low-light areas in binocular camera images and generate low-light adaptation feature maps is as follows: The adversarial network consists of a sequentially connected generator G and a discriminator D. The generator G adopts a U-Net structure, with an adaptive brightness compensation module integrated at the input end and an adaptive brightness compensation mechanism running. The discriminator D adopts a PatchGAN structure. Synchronously optimize the generator G and the discriminator D through a joint loss function: L total =L GAN +λL bright , L GAN Denotes the adversarial loss, L bright Indicates the loss of brightness consistency; Where pdata(x) refers to the probability distribution of the real data, D(x) represents the probability that the discriminator believes that the sample x comes from the real data, and E x～pdata(x) [logD(x)] refers to sampling sample x from pdata(x) and calculating the average value of logD(x); p z (z) refers to the noise distribution, G(z) refers to the light-filled image output by the generator, and D(G(z)) represents the discriminator’s judgment result on the generated image. Refers to p z (z) sampling noise z, calculate the average value of log(1-D(G(z)); λ represents the weight coefficient, which is 10. If L bright If the value is too large, increase λ; Where N is the number of samples in the training batch, I target Refers to the target brightness value, G(z i ) refers to the generator's response to the i-th low-light image z i Output brightness; Fix the discriminator D and minimize L total Update the parameters of generator G; fix generator G and maximize L GAN Update the discriminator D parameters; iterate alternately until convergence, and output the target brightness fill-light image.

4. The robot visual-inertial SLAM method according to claim 3, wherein The steps for generating a fused feature map after weighted fusion are as follows: 1) Input the low-light enhanced binocular image into the pre-trained model to generate a multi-scale pyramid feature map; Gaussian smoothing and downsampling are performed on the feature map to construct a three-layer Gaussian pyramid, with the layer weights assigned to 0.6, 0.3, and 0.1 from high to low resolution; 2) The feature maps of different scales are weighted and summed according to the weights, and spliced ​​in the channel dimension to generate a multi-scale fusion feature map; 3) Acceleration a of IMU k and angular velocity ω k Perform pre-integration to calculate the displacement change and rotation change at adjacent moments. The displacement change is: where v k and a k Represent the linear velocity and acceleration at the kth moment, respectively, and Δt represents the sampling time interval of the IMU; Rotational change: Δθ = ω k Δt, where ω k represents the angular velocity at the kth moment; 4) Update the state vector based on the original visual feature points and pre-integration results The two-dimensional state vector Contains position (x, y) and heading angle θ, F k is the state transfer matrix, B k is the control input matrix, u k is the IMU measurement value; update the error covariance: where Q k Refers to the process noise covariance, including IMU noise and model approximation error: Q k =Cov IMU +Cov model ; Output initial pose estimate Used for global map construction and navigation.

5. The robot visual-inertial SLAM method according to claim 4, wherein The process of removing high entropy and low confidence dynamic feature point interference from inertial data is as follows: Based on the visual feature points extracted by the binocular camera, the entropy value of the velocity distribution of the feature points in the sliding time window is calculated ν i Refers to the velocity vector of the i-th feature point p(ν i ) is the probability distribution of the feature point velocity, N refers to the total number of feature points in the window; the threshold θ=θ is adjusted according to the entropy value base (1+αH), where α=0.1 is the adjustment coefficient, θ base is the basic threshold, determined by experimental calibration method; Using the formula: Calculate the dynamic adjustment confidence of the visual feature points, when C d When it exceeds the preset threshold θ, it is determined to be a dynamic feature point and removed, otherwise it is retained as a static feature point for positioning and mapping; where v i is the velocity vector of the i-th feature point, is the mean velocity of all feature points in the sliding time window, Cov IMU and Cov flow is the IMU and optical flow noise covariance matrix.

6. The robot visual-inertial SLAM method according to claim 5, characterized in that The specific steps of using the low-rank approximation improved graph optimization algorithm to construct a global map of the static feature point set and optimize the robot's posture are as follows: 1) In the two-dimensional plane coordinate system, the information matrix Ω is constructed by combining the static feature point coordinate change error and the IMU pre-integration error ij , for Ω ij Perform singular value decomposition Ω ij =UΣV T Get the left singular vector matrix U, singular value matrix Σ=diag(σ1,σ2,···,σ m )(singular values ​​in descending order) and the transpose of the right singular vector matrix V T , determine the value of the number of retained singular values ​​k by the energy proportion method: calculate the sum of squares of the first k singular values ​​and select The minimum k value, if k>10, then take k=10; 2) Keep the first k largest singular values ​​Σ in Σ k =diag(σ1,σ2,···,σ k ), reconstruct the approximate matrix Among them U k and V k The first k columns of U and V respectively; 3) Input graph optimization algorithm to construct linear system in It is a vector formed by the weighted combination of the linearized residual error of the static feature point coordinate error and the IMU pre-integration error, and the approximate pseudo-inverse is calculated. in Solving Increment And iteratively update the pose and map point coordinates until the residual norm ||b||<10 -4 Or when the maximum number of iterations reaches 20, the optimization results are output.

7. The robot visual-inertial SLAM method according to claim 6, wherein: The process of performing two-dimensional loop detection and global correction based on deep feature description on the constructed global map is as follows: 1) Use GAN to enhance image input to pre-train convolutional networks, extract deep semantic features, build a scene description sub-database, and store keyframe feature maps and their two-dimensional poses (x, y, θ), where (x, y) is the position and θ is the heading angle; 2) Calculate the cosine similarity between the current frame and the historical key frames in real time: Among them, F i and F j are the low-light enhanced feature maps of the current frame and the historical key frame respectively; if S i,j >0.8, then geometric consistency verification is triggered: Use the static feature points after dynamic interference removal to match and obtain the coordinate matching point pair between the current frame and the historical key frame (p k ,q k ), where p k and q k Refers to the coordinates p of the matching points between the current frame and the historical frame k =[u k ,v k ] T ,q k =[u′ k ,v′ k ] T , by minimizing the coordinate transformation error Where N represents the total number of static feature point pairs involved in matching, and the optimal posture transformation matrix of the robot is solved Where R is the rotation matrix t=[Δx,Δy] T is the translation vector; Calculate all (p k ,q k ) Average translation error and the average rotation error If the average translation error is ≤ 0.1m and the average rotation error is ≤ 1°, then it is considered that one of the geometric consistency verification conditions is met; Calculate each (p k ,q k )'s single-point residual e k =||p k -(R·q k +t)||, if e k <2 pixels, the matching point pair is determined to be an inlier; the inlier ratio is counted. If the proportion of interior points exceeds 70%, it is considered that the second geometric consistency verification condition is met; Verify the orthogonality of the rotation matrix and calculate the magnitude of the translation vector t Is it ≤1m? If all of them are met, the third geometric consistency verification condition is met; 3) If all three conditions are met, the geometric consistency verification is considered to be successful, triggering global pose graph optimization, updating the two-dimensional distribution of map points and the covariance matrix Cov(x,y), and outputting the corrected map.

8. The robot visual-inertial SLAM method according to claim 7, wherein: According to the historical trajectory information of the robot's movement, weight distribution is performed and combined with weighted average to dynamically adjust the smoothing coefficient. The specific steps are as follows: 1) Use static feature points to reflect the robot's movement trajectory, and calculate the velocity component ν of each static feature point in the two-dimensional plane x and ν y The speed range is divided into M = 30 intervals, where M is the number of speed bins, and the speed probability distribution p(ν x )、p(ν y ), and calculate the kinetic entropy 2) Adjust the smoothing coefficient α according to the motion entropy H: Among them H max is the maximum entropy value, determined by the number of bins M: H max =ln(M),α min and α max is the minimum and maximum value of the smoothing coefficient, usually α min =0.2;α max =0.8; 3) Calculate the weight of each frame in the sliding window according to the weight distribution rule ω i represents the weight of the historical pose of the i-th frame, where N represents the total number of frames in the sliding window; 4) Weighted fusion window pose to generate smoothed pose Where T i =(x i ,y i θ i ) represents the pose of the i-th frame, and T smooth As the final trajectory output, it is used for positioning and navigation tasks.

9. A computer device, characterized in that: The invention comprises a processor and a memory, wherein the processor is electrically connected to the memory, the memory is used to store instructions and data, and the processor is used to execute the robot visual-inertial SLAM method according to any one of claims 1 to 8.

10. A computer-readable storage medium, characterized in that The computer-readable storage medium stores a computer program, which is suitable for being loaded by a processor and executing the robot visual-inertial SLAM method according to any one of claims 1 to 8.

Images