SLAM (Simultaneous Localization and Mapping)-based visual inertial map combined positioning method in complex environment

Abstract

The invention discloses an SLAM (Simultaneous Localization and Mapping)-based visual inertial map combined positioning method in a complex environment, which comprises the following steps of: 1, acquiring multi-source data, and aligning; 2, off-line map processing is carried out, and three-dimensional line features are extracted; step 3, performing front-end processing of online vision to obtain motion information between continuous frames; step 4, obtaining accurate motion prediction information through IMU pre-integration calculation; 5, finishing feature matching by adopting a cross-modal dual judgment mechanism; step 6, implementing IMU motion prediction optical flow verification to obtain accurate motion information; and step 7, implementing joint optimization, realizing global state output, and completing positioning. The invention belongs to the technical field of simultaneous localization and map construction, and solves the problem that adaptive switching and dynamic feature elimination of extreme environments such as high dynamic environments and weak texture environments are difficult to realize in the prior art; the utilization of map information is limited, and system drift is difficult to avoid.

Description

TECHNICAL FIELD

[0001] The application belongs to the technical field of simultaneous localization and mapping, and relates to a visual-inertial map joint positioning method in a complex environment based on SLAM. BACKGROUND

[0002] With the wide application of robots, unmanned aerial vehicles and intelligent vehicles in complex and dynamic environments, higher requirements are put forward for positioning systems with high precision and strong robustness. Traditional single sensor (such as pure vision or pure inertia) SLAM method often leads to decreased positioning accuracy or even failure due to limitations of scene conditions such as light, violent motion, sparse texture and the like. For example, pure vision SLAM in low light or high dynamic scene, image quality is reduced, feature points are lost, and drift is prone to occur; and pure inertial positioning can estimate short-time attitude at high frequency, but has large long-term cumulative error, and is difficult to meet actual navigation requirements.

[0003] In recent years, multi-sensor fusion has become the mainstream direction of SLAM development, and the tight coupling of vision and IMU inertial navigation, and the cooperative optimization with sparse map structure constraint are considered as effective means to improve the robustness and accuracy of the system. However, most of the existing systems are still dominated by single sensors in the front end, and it is difficult to realize adaptive switching and dynamic feature elimination in extreme environments such as high dynamic and weak texture; in addition, the traditional vision-inertial SLAM has limited use of map information, and lacks a unified optimization mechanism, so system drift is inevitable.

[0004] Therefore, it is urgent to develop a new multi-sensor positioning system which can dynamically fuse vision and IMU information in the front end and improve global consistency through light map structure constraint, so as to meet the application requirements of robot autonomous positioning in complex environments. SUMMARY

[0005] The purpose of the application is to provide a visual-inertial map joint positioning method in a complex environment based on SLAM, which solves the problems that the prior art is difficult to realize adaptive switching and dynamic feature elimination in extreme environments such as high dynamic and weak texture, and has limited use of map information and is difficult to avoid system drift.

[0006] The technical scheme adopted by the application is that the visual-inertial map joint positioning method in a complex environment based on SLAM is implemented according to the following steps: Step 1, acquiring multi-source data and aligning processing; Step 2, implementing offline map processing and extracting three-dimensional line features; Step 3, online visual front-end processing to obtain motion information between consecutive frames; Step 4, obtaining accurate motion prediction information through IMU pre-integration calculation; Step 5, complete feature matching by using a cross-modal double judgment mechanism; Step 6, implement IMU motion prediction optical flow verification to obtain accurate motion information; Step 7, implement joint optimization to realize global state output and complete positioning.

[0007] The present application has the advantages of high-precision positioning and navigation, good environmental adaptability, stable operation in complex and dynamic environments, and application in autonomous positioning scenarios such as robots, drones and autonomous driving. BRIEF DESCRIPTION OF DRAWINGS

[0008] Figure 1 is the overall flow architecture diagram of the method of the present application; Figure 2 is a 2D, 3D line feature extraction diagram of the method of the present application in operation. DETAILED DESCRIPTION

[0009] The present application will be described in detail below in conjunction with the drawings and specific embodiments.

[0010] Referring to Figure 1 The visual-inertial map joint positioning method of the present application based on SLAM in a complex environment is implemented according to the following steps: Step 1, acquire multi-source data and align processing, This step innovatively proposes a multi-source data synchronization and alignment method, which ensures seamless fusion of data from IMU, camera and three-dimensional map through accurate time synchronization and spatial alignment technology, avoiding positioning errors caused by inconsistent data in time and space in traditional methods.

[0011] 1.1) Obtain multi-source data through an IMU acquisition unit, a camera acquisition unit and a map acquisition unit, wherein the IMU acquisition unit provides three-axis angular velocity and three-axis acceleration data of the current pose of the robot, with a sampling frequency of 200Hz, accurately measuring the motion state in a dynamic environment; the camera acquisition unit acquires high-resolution images with a resolution of 1920x1200 and a frame rate of 40fps, for obtaining visual information of the environment; the map acquisition unit uses the prior three-dimensional map generated by Cartographer to provide a static environment reference for online positioning; 1.2) Transfer the above multi-source data to the main control computing platform, and perform accurate time synchronization through the message_filters module in ROS Noetic under the Ubuntu 20.04 operating system, to ensure that the data of each acquisition unit is aligned in time and space.

[0012] Wherein, the message_filters module is a module in ROS Noetic, used to synchronize messages of multiple data streams, ensuring the consistency of different sensors (such as IMU, camera, etc.) in time, and achieving time alignment of messages through the TimeSynchronizer class, allowing data of different sensors to be processed at the same time, thereby improving the accuracy and stability of data fusion. The message_filters module can effectively handle the synchronization of multiple sensors, avoiding positioning errors caused by different time synchronization.

[0013] The main functions and configurations of the hardware platform modules used in the method of the application are shown in Table 1.

[0014] Table 1, main functions and configurations of multi-source data acquisition and alignment processing

[0015] At this point, step 1 obtains multi-source data after synchronization processing.

[0016] Step 2, implement offline map processing to extract three-dimensional line features, the specific process is: 2.1) Import the prior three-dimensional map into PCL (Point Cloud Library) to enter the initial filtering process and noise removal, remove redundant points and noise in the environment; the data processed by PCL enters the depth filtering process, which uses VoxelGrid voxel filtering to downsample the point cloud, reducing the computational complexity; PCL is an open source library specifically for processing and analyzing 3D point cloud data, through a series of tools for point cloud acquisition, filtering, segmentation, feature extraction, registration, etc. VoxelGrid voxel filtering is a point cloud downsampling method, which divides the point cloud space into small cubes called voxels, and then uses the average value of all points in each voxel to represent the points in that voxel, which helps to reduce the computational complexity of the point cloud while maintaining its approximate geometric shape.

[0017] 2.2) Use the Statistical Outlier Removal algorithm to calculate the number of neighborhood points for each point, and remove those points whose neighborhood point number is significantly less than the average value. These removed points are usually noise or outliers, and the abnormal data points far from the overall distribution of the point cloud are removed.

[0018] 2.3) After two filtering and denoising processes, the point cloud data is registered by the global ICP (Iterative Closest Point) algorithm to align the point cloud with the world coordinate system. Stable three-dimensional line features are extracted using the LSD v1.6 algorithm, which represent the main geometric structures in the environment (such as walls, door frames, etc.). These three-dimensional line features are stored in a lightweight data format, preserving necessary geometric information while reducing storage and computational burden, providing a basis for subsequent real-time feature matching.

[0019] ICP is a widely used point cloud registration algorithm that aligns two point clouds by iteratively finding the nearest point in one point cloud to the other and computing a transformation matrix that minimizes the distance, ultimately aligning the two point clouds. Global ICP considers a broader optimization objective and is typically used to handle large-scale point cloud registration problems. LSD v1.6 is a version of the LSD (Line Segment Detector) algorithm that can extract stable and reliable line segment features from images. It can extract straight line structures in the environment under complex backgrounds and different lighting conditions, making it suitable for feature extraction in low-texture and complex environments.

[0020] From here, the offline map processing is complete, and the next step of real-time localization is entered.

[0021] Step 3, online visual front-end processing, obtaining motion information between consecutive frames, the specific process is: 3.1) Each frame of the camera acquisition unit image is de-warped to correct the geometric distortion caused by the lens; 3.2) Use the ORB (Oriented FAST and Rotated BRIEF) feature detector in ORB-SLAM3 v0.4 to extract no more than 1000 two-dimensional point features from the image. These features are key points for tracking and positioning. At the same time, the LSD algorithm is used to extract two-dimensional line features, which can provide additional geometric information, especially suitable for scenes with sparse texture; The ORB feature detector is an algorithm for image feature extraction that combines the FAST corner detector and the BRIEF descriptor, allowing efficient extraction of feature points in images and invariance to rotation.

[0022] 3.3) All extracted two-dimensional point features and two-dimensional line features are processed as follows: The first step is line feature tracking, where the extracted two-dimensional line features are tracked using the Lucas-Kanade sparse optical flow method to track the endpoints of the line features in consecutive frames, ensuring the temporal consistency of all two-dimensional line features. The Lucas-Kanade sparse optical flow method is a classic sparse optical flow method used to estimate motion between consecutive frames in a video. This method is based on the grayscale consistency assumption within a local area, assuming that within a short period of time, the motion of all points within a small area of ​​the image is consistent. The Lucas-Kanade sparse optical flow method calculates the motion (i.e., optical flow) of each feature point in the image by solving the image brightness equation, thereby determining the direction and speed of motion of different areas in the image. The second step is to implement IMU verification, combining the extracted two-dimensional point features and two-dimensional line features with the data from the IMU acquisition unit. The combination conditions are: first, all visual feature points and line features must be strictly synchronized with the IMU data in time to ensure the temporal consistency of each data stream; then, the visual feature points and line features need to be aligned with the IMU coordinate system through external parameter calibration; finally, the rotation, velocity, and displacement predictions provided by the IMU pre-integration are compared with the optical flow tracking results of the visual features. If the residual between the two is less than the set threshold, they are judged to be consistent. Through these conditions, the geometric consistency of the visual features and IMU data is ensured, and accurate feature fusion is achieved.

[0023] At this point, step 3 obtains preliminary motion information between consecutive frames through the Lucas-Kanade sparse optical flow method and IMU verification.

[0024] Step 4: Obtain accurate motion prediction information through IMU pre-integration calculation. This step 4 innovatively divides the IMU data sequence between two adjacent frames into dynamic time windows according to the timestamp. In each dynamic time window, an adaptive integration step is used to perform short-time integration, calculate the rotation amount, velocity increment, and displacement increment respectively, and accumulate them step by step between the dynamic time windows to form the IMU pre-factor. The specific process is: 4.1) Using the angular velocity and acceleration measured by the IMU acquisition unit, the rotation, velocity increment, and displacement increment within each dynamic time window are calculated. These increments form the IMU rotation prediction matrix. 4.2) A segmented accumulation strategy is adopted, that is, separate integration is performed within each dynamic time window, which avoids the error linearization problem caused by the one-time integration of the entire segment in the traditional method.

[0025] By this method, more high-frequency motion details can be preserved in high-speed motion or severe acceleration, thereby improving the accuracy of motion prediction. The pre-integration result provides accurate motion prediction information for subsequent feature matching and optimization, ensuring stable operation in dynamic environments. However, the noise and integration error of the IMU can cause cumulative error in the IMU pre-integration data, especially in long-time or severe acceleration motion, the error may gradually increase, affecting the subsequent motion prediction and positioning accuracy, which requires the fusion processing of step 5.

[0026] From this, step 4 obtains accurate motion prediction information through IMU pre-integration.

[0027] Step 5, using a cross-modal double judgment mechanism, completes feature matching, The motion prediction information obtained by the IMU pre-integration is matched with the two-dimensional line feature extracted by the visual front-end, and the endpoints of the two-dimensional line feature in the continuous frame are tracked to ensure the continuity of the feature in time; in order to ensure the accuracy of the match, a cross-modal double judgment mechanism is adopted, only when both kinds of judgment meet the conditions, it is considered that the match is effective, the two kinds of judgment are described as follows: Judgment method one: through the perspective projection model, the line feature endpoints in the three-dimensional map are projected into the current frame, and the re-projection error is calculated for re-projection error judgment. Perspective projection model is a method of mapping three-dimensional scene to two-dimensional image plane, commonly used in computer vision to describe how the camera projects three-dimensional object points onto the image. Through the camera's internal and external parameter matrix, three-dimensional points can be converted to two-dimensional image points. This process is crucial for tasks such as three-dimensional reconstruction and image registration. However, due to inaccurate camera internal and external parameters or image feature matching errors, there may be some visual re-projection errors in this part.

[0028] Judgment method two: calculate the Hamming distance through the binary line descriptor (BLD) to determine the descriptor similarity. Binary line descriptor (BLD) is a descriptor specifically designed for line features, which encodes the local geometric features of line segments (such as length, direction, endpoints, etc.) into binary format, and evaluates the similarity between line features by calculating the Hamming distance. This method is efficient and suitable for line feature matching in low-texture or occluded environments, helping to improve the accuracy of feature matching.

[0029] The above double judgment mechanism greatly reduces the false match rate in low-texture environments or local occlusions.

[0030] From this, step 5 completes the matching of line features, and the matching result is added to the global optimization objective function in the form of a unified line feature factor, providing accurate positioning.

[0031] Step 6, implement IMU motion prediction optical flow check, get accurate motion information, 6.1) The motion prediction information calculated by IMU pre-integration is compared with the motion information between consecutive frames obtained by Lucas-Kanade sparse optical flow method and IMU check; the position of the feature points in the previous frame is predicted to the position in the current frame by rotation transformation using the rotation prediction matrix in the motion prediction information, and the motion information between the two consecutive frames obtained by tracking is compared to calculate the optical flow residual; 6.2) In order to improve the robustness, this step innovatively introduces an adaptive residual threshold adjustment mechanism, which dynamically adjusts the threshold of optical flow residual according to the dynamic degree and depth information of the scene: in dynamic complex scenes, such as student campus scenes and complex traffic scenes, the threshold is appropriately relaxed to retain more effective points; in static or high-texture scenes, such as closed classroom scenes, the threshold is tightened to eliminate potential abnormal points; Through this adaptive optical flow residual check, effective feature points can be better retained in dynamic scenes, while abnormal points that do not meet the conditions are eliminated, improving the accuracy of feature matching.

[0032] From now on, step 6 obtains accurate motion information through IMU pre-integration and adaptive residual threshold adjustment of optical flow residual mechanism.

[0033] Step 7, implement joint optimization to achieve global state output, the specific process is: 7.1) Build a factor graph optimization framework (GTSAM v4.1.1), which models the problem as a graph, where each node represents a state variable (such as the pose of the robot), and each edge represents a constraint or observation data (such as visual and IMU observation errors). Use this factor graph optimization framework to unify visual re-projection errors and IMU pre-integration errors into an optimization objective function, which combines the errors of multiple sensor data and provides multiple constraints for global optimization; 7.2) Use Ceres Solver to complete the nonlinear least squares optimization of the optimization objective function, iteratively solve the optimization problem, and get the six-degree-of-freedom pose of the camera, three-axis velocity and map state. Ceres Solver is a high-efficiency C++ library for solving nonlinear least squares optimization problems, and has been widely used in computer vision and robotics.

[0034] 7.3) The optimized results are output in real time through the ROS message interface, completing the localization.

[0035] The subsequent can also be passed to the navigation and path planning module, which is responsible for the autonomous movement and path calculation of the robot or unmanned aerial vehicle. The path calculation selects A* algorithm or Dijkstra algorithm, uses the optimized pose information, tracks the position and attitude of the robot in real time to navigate, and provides high-precision pose estimation and path planning support for the robot or unmanned aerial vehicle.

[0036] Finally, the method of the application significantly improves the positioning accuracy and system robustness by efficiently combining visual, IMU and map data, especially in dynamic environments.

[0037] Embodiment 1 The application scenario of this embodiment 1 is set in a complex indoor environment, such as a sports goods storage room full of goods, with various types and sizes of goods stacked. The change of light and sparse texture in the indoor environment makes the traditional visual SLAM method face challenges, so it is necessary to realize high-precision indoor positioning through data fusion of vision, IMU and prior map, and finally realize indoor navigation and path planning.

[0038] The foregoing step process of the application is implemented as follows: Step 1, data acquisition and synchronization: First, data acquisition is performed by multiple sensors. IMU (Inertial Measurement Unit) provides high-frequency motion data through accelerometer and gyroscope, and camera collects environment images for feature extraction.

[0039] Then, in order to ensure the spatio-temporal consistency of system data, the data of all sensors are precisely synchronized through the message_filters module of ROS Noetic, ensuring the timestamp alignment of different data streams, and avoiding the positioning error caused by the inconsistency of sensor data collected at different times.

[0040] Step 2, offline map processing and feature extraction: In the offline processing stage, the system imports the prior three-dimensional map data into PCL (Point Cloud Library) for filtering and denoising. Voxel filter (VoxelGrid) is used to downsample the point cloud to reduce the computational complexity, and statistical outlier removal algorithm (Statistical Outlier Removal) is used to remove noise points. Then, the LSD (Line Segment Detector) algorithm is used to extract stable three-dimensional line features in the environment, such as walls and door frames. These line features will be used as geometric structure information of the environment for subsequent matching and positioning.

[0041] Step 3, online visual front-end processing and feature extraction: In the real-time localization stage, the camera first performs distortion correction to correct the geometric distortion caused by the lens after capturing each frame of image. Then, ORB-SLAM3 is used to extract two-dimensional feature points, and the LSD algorithm is used to extract two-dimensional line features, as shown in Figure 2 The running screen shows the pose optimization process in the camera view during real-time localization. The line with nodes in the figure is the three-dimensional line feature in the current frame view, and the thick line without nodes is the two-dimensional line feature detected in the image. The extracted two-dimensional point features and two-dimensional line features are respectively transmitted to different processing modules: the two-dimensional line features are sent to the tracking module, and the two-dimensional point features are sent to the IMU verification module.

[0042] Step 4, IMU pre-integration and motion prediction: IMU data is divided into multiple dynamic time windows according to the timestamp. Within each window, the system uses the angular velocity and acceleration measured by the IMU to perform short-time integration to calculate the rotation, velocity increment and displacement increment. These increments constitute the pre-integration result of the IMU, providing accurate motion prediction information for subsequent feature matching and optimization.

[0043] Step 5, using a cross-modal double judgment mechanism to complete the fusion of visual and IMU data and feature matching: Through the consistency test between the visual re-projection error and the IMU pre-integration data, the system fuses the visual features and the IMU data. Specifically: first, project the line feature endpoints in the three-dimensional map into the current image through the perspective projection model, calculate the re-projection error, and compare it with the motion prediction result provided by the IMU data; at the same time, use the binary line descriptor (BLD) to calculate the Hamming distance to evaluate the accuracy of line feature matching. Only when both geometric consistency and descriptor matching meet the conditions, is the feature matching considered valid.

[0044] Step 6, implement IMU motion prediction optical flow verification, and get accurate motion information through the mechanism of IMU pre-integration and adaptive residual threshold adjustment of optical flow residual.

[0045] Step 7, joint optimization and global state output, path planning and dynamic obstacle avoidance: The visual re-projection error, IMU pre-integration error and map feature alignment error are unified to construct an optimization objective function, which is solved by the factor graph optimization framework (GTSAM). Nonlinear least squares optimization is performed using Ceres Solver to obtain the accurate pose of the robot. The optimized pose information is transmitted to the path planning module to plan the shortest path and ensure that the robot avoids obstacles.

[0046] Based on the optimized pose, an A* algorithm is used for path planning to ensure that the robot can accurately reach the target position from the current position. According to the corresponding state results obtained in the previous steps, the path is adjusted in real time to ensure the safe driving of the robot in complex environments.

[0047] The actual test results show that in a complex indoor environment, the positioning error of this method is 2 cm, and in a dynamic environment (such as personnel, furniture, etc.), a high accuracy can be maintained. The path planning success rate is 98%, and the robot can complete the task without collision in a complex obstacle environment. The system can complete feature extraction, matching and optimization calculation for each frame of image within 30 ms, realizing real-time positioning. During the test, the robot can accurately avoid 97% of the obstacles.

[0048] Embodiment 2 This embodiment 2 is suitable for positioning and navigation of an autonomous vehicle in a school road environment, which has a large number of dynamic elements, including vehicles, pedestrians, etc., and has high dynamicity and complex road structure.

[0049] The foregoing steps of the present application are implemented as follows: Step 1, data acquisition and synchronization: The autonomous vehicle is equipped with IMU, camera and laser radar for data acquisition. The IMU is responsible for providing high-frequency motion data of the vehicle, the camera captures road images and extracts visual features, and the laser radar is used to generate a three-dimensional point cloud map of the environment. During data acquisition, the data of all sensors are time-synchronized and coordinate-aligned by ROS Noetic, ensuring the consistency of visual, inertial and map data in space and time.

[0050] Step 2, offline map processing and feature extraction: This method uses PCL to process the three-dimensional point cloud data provided by the laser radar, removes noise points and performs point cloud registration. The three-dimensional line features in the environment are extracted by the LSD algorithm, which represent important elements such as road markings and building edges, providing environmental reference for subsequent positioning and path planning of the vehicle.

[0051] Step 3, online visual front-end processing and feature extraction: In real-time positioning, the images captured by the camera are extracted by ORB-SLAM3 to extract two-dimensional feature points, and the LSD algorithm is used to extract two-dimensional line features, enhancing the positioning ability of this method in low-texture or high-dynamic scenes. The extracted two-dimensional features are combined with IMU data to track the feature points in the image through the Lucas-Kanade optical flow method, ensuring the consistency of visual features in time.

[0052] Step 4, IMU pre-integration and motion prediction: The method uses the acceleration and angular velocity data of the IMU to pre-integrate using an adaptive step size integration method, calculating the rotation, velocity, and displacement increments between adjacent frames. These motion prediction results provide accurate motion information for subsequent visual data matching and optimization.

[0053] Step 5: Complete visual and IMU data fusion and feature matching using a cross-modal double judgment mechanism: Match the IMU pre-integrated data with the two-dimensional features extracted by the camera. First, project the line features in the three-dimensional map into the image using the perspective projection model, calculate the re-projection error, and compare it with the motion estimation provided by the IMU. In addition, use the binary line descriptor (BLD) for feature matching, and through the geometric consistency test and descriptor matching double judgment mechanism, ensure the accuracy of feature matching.

[0054] Step 6: Implement IMU motion prediction optical flow verification, obtain accurate motion information through IMU pre-integration and adaptive residual threshold adjustment of optical flow residual.

[0055] Step 7: Joint optimization and global state output, path planning and dynamic obstacle avoidance: Model the visual re-projection error, IMU pre-integration error, and map feature alignment error through the factor graph optimization framework, and use Ceres Solver for nonlinear least squares optimization to solve the optimal vehicle pose. The optimization result is passed to the navigation and path planning module to calculate the shortest driving path.

[0056] Based on the optimized pose, the system uses Dijkstra algorithm to calculate the shortest path and ensures safe driving in complex urban traffic through dynamic obstacle avoidance technology. The system can adjust the path in real time to avoid dynamic obstacles (such as pedestrians and other vehicles), ensuring the stability and safety of the autonomous vehicle.

[0057] Implementation results: The optimized positioning accuracy is 1.5 cm, and even in high dynamic environments (such as during class dismissal and lunch time), it can still maintain high accuracy. The error between the path successfully planned by the vehicle and the actual driving path is 3 cm, successfully avoiding all obstacles and dynamic elements. The system can complete positioning and path calculation within 50 ms under 30 frames per second image update. The system successfully avoids 98% of dynamic obstacles such as moving vehicles and pedestrians, ensuring safe driving.

[0058] Example 3 This embodiment 3 is suitable for positioning and flying of UAV in forest environment. Forest environment has the characteristics of dense trees, uneven light and uneven ground, which makes it difficult for traditional vision systems to extract features. The positioning system requires strong adaptability and robustness to ensure that the UAV can successfully complete the flight mission.

[0059] The foregoing step process according to the present application is implemented as follows: Step 1, data acquisition and synchronization: The UAV is equipped with IMU, camera and prior map data for positioning. The IMU provides acceleration and angular velocity data, the camera collects environmental features through image acquisition, and the laser scanner provides the prior map of the forest environment. All sensor data is synchronized through ROS to ensure consistency in time and space for subsequent feature extraction and matching.

[0060] Step 2, offline map processing and feature extraction: The point cloud data collected by the laser scanner is filtered and denoised by PCL, and three-dimensional line features in the forest (such as tree trunks, ridges, etc.) are extracted to construct a static map of the forest environment, providing a reference for subsequent real-time positioning.

[0061] Step 3, online visual front-end processing and feature extraction: In the process of real-time positioning, the camera collects a frame of image, first performs distortion correction to correct the geometric distortion in the image, and then extracts two-dimensional feature points and line features. The extracted features are tracked by Lucas-Kanade optical flow method to ensure the consistency of the features in consecutive frames and enhance the robustness of the method.

[0062] Step 4, IMU pre-integration and motion prediction: The IMU data is integrated for a short time to calculate the rotation, velocity increment and displacement increment between each two frames. The pre-integration result provides accurate motion prediction information for subsequent visual feature matching and optimization, especially when the UAV is flying fast or accelerating sharply, which can maintain high motion accuracy.

[0063] Step 5, using cross-modal double judgment mechanism to complete visual and IMU data fusion and feature matching: By fusing the IMU pre-integration result with the two-dimensional features extracted by the camera, the system calculates the re-projection error through the perspective projection model to ensure the consistency of the visual and IMU data. In the matching process, a binary line descriptor is used for efficient feature matching to ensure the accuracy of the feature points in the dynamic scene.

[0064] Step 6, implement IMU motion prediction optical flow check, through the mechanism of IMU pre-integration and adaptive residual threshold adjustment, the accurate motion information is obtained.

[0065] Step 7, joint optimization and global state output, flight control and path planning: The visual re-projection error, IMU pre-integration error, and map feature alignment error are unified through a factor graph optimization framework. Ceres Solver is used for optimization to obtain accurate UAV pose, which is passed to the flight control module.

[0066] Based on the optimized pose, the system uses A* algorithm for path planning to ensure that the UAV can avoid trees and other obstacles, fly stably, and dynamically adjust the flight path according to environmental changes.

[0067] Implementation results: In the forest environment, the positioning accuracy reached 3 cm, accurately tracking the UAV's pose even in low light and low texture environments. The flight path error was 5 cm, and the UAV successfully avoided 100% of trees and other obstacles. The system can complete positioning and path adjustment within 20 ms, maintaining flight stability and accuracy, especially under changing wind speed and dynamic obstacle interference. The path planning success rate was 100%, and the UAV could accurately avoid all obstacles and complete the task safely.

[0068] Example 4 This example 4 is suitable for positioning and navigation of cleaning robots in urban environments. In urban cleaning tasks, cleaning robots need to move autonomously in complex urban streets, pedestrian-intensive areas, and various dynamic obstacles. These environments have irregular road structures, frequent traffic and pedestrian flow, and traditional SLAM systems often cause positioning errors due to dynamic environmental interference. To ensure that the cleaning robot can efficiently and accurately complete the task, this example combines IMU, camera, and lidar, and uses multi-source data synchronization and fusion technology to enhance the positioning accuracy and robustness of the robot in urban environments.

[0069] The following is implemented according to the foregoing steps of the present application: Step 1, data acquisition and synchronization: The cleaning robot is equipped with IMU, camera, and lidar. The IMU provides motion data of the robot in complex environments, the camera captures images of the surrounding urban street environment, and the lidar generates a three-dimensional point cloud map of the urban road and obstacles. The data of all sensors is synchronized in time and space through the ROS system to ensure consistency in time and space.

[0070] Step 2, offline map processing and feature extraction: PCL is used to denoise and filter the three-dimensional point cloud data collected by the lidar, extracting features such as sidewalks, curbs, traffic signs, etc. on the urban road, and constructing a static reference map of the urban environment.

[0071] (3) Online visual processing and feature extraction: After distortion correction of camera images, ORB-SLAM3 is used to extract two-dimensional feature points, and LSD algorithm is used to extract stable line features such as building edges and road edges to help the robot identify key landmarks.

[0072] Step 4, IMU pre-integration and motion prediction: Through short-time integration of IMU acceleration and angular velocity data, the rotation, speed and displacement increment of the robot in the dynamic environment are calculated to provide accurate motion prediction information and ensure the stability of the cleaning path.

[0073] Step 5, using cross-modal double judgment mechanism, completing visual and IMU data fusion and feature matching: Match the motion information obtained by IMU pre-integration with the visual features captured by the camera, calculate the re-projection error using the perspective projection model, and perform feature matching using the binary line descriptor to ensure the consistency and accuracy of visual and IMU data.

[0074] Step 6, implement IMU motion prediction optical flow verification, through the mechanism of IMU pre-integration and adaptive residual threshold adjustment of optical flow residual, accurate motion information is obtained.

[0075] Step 7, joint optimization and global state output, path planning and dynamic obstacle avoidance: Through the factor graph optimization framework, the visual re-projection error, IMU pre-integration error and map feature alignment error are jointly optimized, and Ceres Solver is used for global optimization to obtain accurate pose and motion information of the robot. Use A* algorithm to plan the moving path of the cleaning robot, and adjust the path according to real-time sensor data to ensure that the robot avoids pedestrians, vehicles and other obstacles and completes the cleaning task.

[0076] Implementation results: In urban street environment, 2cm positioning accuracy is achieved, and the cleaning robot can accurately avoid pedestrians, vehicles and other dynamic obstacles to successfully complete the cleaning task. In complex street environment, the robot can adjust the path according to real-time sensor data to avoid collision with obstacles. Through accurate path planning and dynamic obstacle avoidance, the cleaning efficiency is improved by about 30% compared with traditional methods. In different weather conditions, the robot can still maintain high-precision positioning and complete the scheduled cleaning area, finally ensuring that the cleaning effect reaches more than 95% coverage rate.

[0077] Example 5 This embodiment 5 is suitable for automatic driving vehicle positioning and navigation in urban underground parking lot. In urban underground parking lot, complex structure, narrow lane and dynamic obstacles (such as pedestrians and other vehicles) make the positioning and navigation of automatic driving vehicle face greater challenges. In order to ensure that the automatic driving vehicle can park smoothly and avoid collision in such environment, this embodiment combines IMU, camera and laser radar, realizes accurate vehicle positioning and dynamic path planning through multi-sensor data fusion technology.

[0078] The foregoing procedure according to the present application is implemented as follows: Step 1, data acquisition and synchronization: the automatic driving vehicle is equipped with IMU, camera and laser radar. IMU provides acceleration and angular velocity data of the vehicle, camera is used to capture real-time images of the parking lot, and laser radar generates three-dimensional point cloud map of the parking lot. All sensor data are accurately synchronized through ROS Noetic, ensuring the spatio-temporal consistency of data.

[0079] Step 2, offline map processing and feature extraction: using three-dimensional point cloud data collected by laser radar, the parking lot is filtered and denoised, and features such as lane edge, obstacle and marking line are extracted, to construct a static reference map of the parking lot.

[0080] Step 3, online visual front-end processing and feature extraction: after distortion correction of camera images, two-dimensional feature points are extracted using ORB-SLAM3, and line features are extracted using LSD algorithm, to enhance the positioning ability of the system in complex and dynamic environment.

[0081] Step 4, IMU pre-integration and motion prediction: through short-time integration of angular velocity and acceleration data collected by IMU, rotation, velocity increment and displacement increment are calculated, to provide accurate motion prediction and support for subsequent positioning.

[0082] Step 5, cross-modal double judgment mechanism is adopted to complete visual and IMU data fusion and feature matching: IMU data and visual features are fused, line features in three-dimensional map are projected into image through perspective projection model, re-projection error is calculated, and accurate matching is performed through binary line descriptor, to ensure positioning accuracy.

[0083] Step 6, IMU motion prediction optical flow verification is implemented, accurate motion information is obtained through IMU pre-integration and adaptive residual threshold adjustment mechanism of optical flow residual.

[0084] Step 7, joint optimization and global state output, path planning and dynamic obstacle avoidance: Through the factor graph optimization framework, the visual re-projection error, IMU pre-integration error and map feature alignment error are jointly optimized, and the Ceres Solver is used for global optimization to obtain accurate vehicle pose and motion information. A* algorithm is used for path planning, and dynamic obstacle avoidance technology is used to adjust the path in real time to ensure that the vehicle can safely pass through the parking lot and avoid collisions with obstacles such as parked vehicles or pedestrians.

[0085] Implementation results: In the urban underground parking lot environment, a positioning accuracy of 3 cm is achieved, and the autonomous vehicle can successfully avoid 100% of dynamic obstacles and efficiently complete the parking task. The system can adapt to the complex layout of the parking lot, ensuring that the vehicle can stably drive and smoothly park in narrow lanes and crowded spaces.

[0086] Example 6 This example 6 is suitable for the positioning and navigation of automatic planting equipment in agricultural plantations. In agricultural plantations, the irregular shape of the land, the diversity of planted crops, and the changes in external environments such as weather pose challenges to the positioning and navigation of automatic planting equipment. In order to ensure that the automatic planting equipment can smoothly perform work tasks in the field, this example combines IMU, cameras and lidar, and realizes high-precision positioning and autonomous navigation of the equipment in complex agricultural environments through multi-sensor data fusion technology.

[0087] According to the foregoing step process of the present application, the following is implemented: Step 1, data acquisition and synchronization: The automatic planting equipment is equipped with IMU, camera and lidar. The IMU provides real-time motion data of the equipment, monitors the acceleration and attitude change of the equipment; the camera is used to capture images of the agricultural environment, especially to identify crop planting positions and ground features; the lidar is used to scan the ground, crop distribution and surrounding obstacles of the field, generating three-dimensional point cloud data. All sensor data are synchronized in time and space through the integrated control system to ensure accurate data fusion and avoid positioning errors caused by inconsistent data.

[0088] Step 2, offline map processing and feature extraction: The system uses three-dimensional point cloud data collected by lidar to model the agricultural environment. First, the system removes unnecessary noise data and extracts crop row spacing, terrain undulations, field obstacles and other features to generate a preliminary environmental map of the field. This map provides a static environmental reference for the equipment and helps the system identify crop growth areas, irrigation pipelines and other important areas.

[0089] Step 3, online visual front-end processing and feature extraction: For each frame of image captured by the camera, first, distortion correction is performed to correct lens distortion. Then, ORB-SLAM3 algorithm is used to extract two-dimensional feature points from the image, and combined with LSD algorithm to extract line features in the field (such as row spacing, irrigation pipeline, etc.), enhancing the device's perception ability of the environment. These features are used to dynamically update the device's pose and current working area.

[0090] Step 4, IMU pre-integration and motion prediction: The device records acceleration and angular velocity data in real time through the IMU sensor, and performs short-time integration to calculate the device's rotation, speed increment and displacement increment. This process provides real-time motion prediction of the device, and helps the device maintain stable positioning and navigation in the agricultural environment, even in uneven fields or variable weather conditions.

[0091] Step 5, cross-modal double judgment mechanism is adopted to complete the fusion of visual and IMU data and feature matching: In order to further improve the positioning accuracy, the system matches the motion prediction information of the IMU with the environmental features extracted by the camera and lidar through visual-IMU data fusion technology. Through the perspective projection model, the features in the three-dimensional environment are projected into the image, and the re-projection error is calculated. At the same time, the binary line descriptor (BLD) is used to efficiently match the extracted line features, ensuring the consistency of visual and IMU data and further improving the positioning accuracy of the system.

[0092] Step 6, IMU motion prediction optical flow verification, through the mechanism of IMU pre-integration and adaptive residual threshold adjustment, accurate motion information is obtained.

[0093] Step 7, joint optimization and global state output, path planning and dynamic obstacle avoidance: The system uses the factor graph optimization framework (GTSAM) to model the visual re-projection error, IMU pre-integration error and map feature alignment error, and performs global optimization through Ceres Solver. After optimization, the system can output accurate device pose information and update the device's position in the field in real time.

[0094] During the device's travel, the system uses A* algorithm for path planning according to the current pose information, ensuring that the device can smoothly avoid obstacles (such as trees, stones, etc.) and avoid repeated routes. The device perceives the surrounding environment in real time, and when encountering new obstacles, it can quickly adjust the path and continue working, ensuring efficient and safe work.

[0095] Implementation results: In the agricultural plantation environment, a positioning accuracy of 3 cm is achieved, and the automated planting equipment can successfully avoid 100% of dynamic obstacles and efficiently complete the crop planting task. The system can effectively deal with complex changes in the agricultural environment, such as uneven ground, variable weather, and other factors, and operates stably in different crop areas and land conditions. The work efficiency of the equipment is improved by 25%, and the need for manual intervention is successfully reduced, ensuring precise agricultural operations and crop management.

Claims

1. A visual-inertial map joint positioning method in complex environments based on SLAM, characterized by: Follow these steps to implement: Step 1: Obtain multi-source data and align them; Step 2: Implement offline map processing to extract 3D line features; Step 3: Front-end processing of online vision to obtain motion information between consecutive frames; Step 4: Obtain accurate motion prediction information through IMU pre-integration calculation; Step 5: Use a cross-modal dual judgment mechanism to complete feature matching; Step 6: Implement IMU motion prediction optical flow verification to obtain accurate motion information; Step 7: Implement joint optimization to achieve global state output and complete positioning.

2. The visual-inertial map joint positioning method in a complex environment based on SLAM according to claim 1, characterized in that: In step 1, the specific process is: 1.1) Multi-source data is obtained through the IMU acquisition unit, camera acquisition unit, and map acquisition unit. The IMU acquisition unit provides three-axis angular velocity and three-axis acceleration data of the robot's current position, accurately measuring its motion state in a dynamic environment. The camera acquisition unit is used to obtain visual information of the environment. The map acquisition unit generates a priori three-dimensional maps, providing a static environment reference for online positioning. 1.2) Transmit the above multi-source data to the main control computing platform for time synchronization to ensure that the data of each acquisition unit is aligned in time and space.

3. The visual-inertial map joint positioning method in a complex environment based on SLAM according to claim 1, characterized in that: In step 2, the specific process is: 2.1) Importing the prior 3D map into PCL for initial filtering and denoising to remove redundant points and noise in the environment; the PCL-processed data then enters the deep filtering process to reduce computational complexity; 2.2) Use a statistical outlier removal algorithm to calculate the number of neighborhood points for each point and remove points whose number of neighborhood points is significantly less than the average. These removed points are usually noise or outliers, removing abnormal data points that are far away from the overall distribution of the point cloud; 2.3) After two filtering and denoising steps, the point cloud data is registered using the global ICP algorithm to align the point cloud with the world coordinate system. The LSD v1.6 algorithm is then used to extract stable 3D line features, which represent the main geometric structures in the environment. These 3D line features are stored in a lightweight data format.

4. The visual-inertial map joint positioning method in a complex environment based on SLAM according to claim 1, characterized in that: In step 3, the specific process is: 3.1) Dedistort each frame of the camera acquisition unit’s image to correct for the geometric distortion caused by the lens; 3.2) Use the ORB feature detector to extract 2D point features from the image; at the same time, use the LSD algorithm to extract 2D line features; 3.3) All extracted 2D point features and 2D line features are processed as follows: First, enter line feature tracking, and use the Lucas-Kanade sparse optical flow method to track the line feature endpoints in continuous frames. The second is to implement IMU verification, combining the extracted two-dimensional point features and two-dimensional line features with the data of the IMU acquisition unit to ensure the geometric consistency of the visual features and IMU data and complete accurate feature fusion.

5. The visual-inertial map joint positioning method in a complex environment based on SLAM according to claim 1, characterized in that: In step 4, the specific process is: 4.1) Using the angular velocity and acceleration measured by the IMU acquisition unit, the rotation, velocity increment, and displacement increment within each dynamic time window are calculated. These increments form the IMU rotation prediction matrix. 4.2) A segmented accumulation strategy is adopted, that is, separate integration is performed within each dynamic time window.

6. The visual-inertial map joint positioning method in a complex environment based on SLAM according to claim 1, characterized in that: In step 5, the specific process is: A cross-modal dual judgment mechanism is used. A match is considered valid only when both judgment methods meet the conditions. The two judgment methods are described as follows: Determination method 1: Project the endpoints of the line features in the 3D map into the current frame through the perspective projection model, calculate the reprojection error, and determine the reprojection error; Determination method 2: Calculate the Hamming distance through the binary line descriptor to determine the descriptor similarity.

7. The visual-inertial map joint positioning method in a complex environment based on SLAM according to claim 1, characterized in that: In step 6, the specific process is: 6.1) Compare the motion prediction information obtained from IMU pre-integration with the motion information between consecutive frames obtained using the Lucas-Kanade sparse optical flow method and IMU verification. Use the rotation prediction matrix in the motion prediction information to predict the position of the feature points in the previous frame to the position of the current frame through rotation transformation. Compare this with the motion information between the two consecutive frames obtained by tracking, and calculate the optical flow residual. 6.2) An adaptive residual threshold adjustment mechanism is introduced to dynamically adjust the threshold of the optical flow residual based on the scene dynamics and depth information. In dynamic and complex scenes, the threshold is appropriately relaxed to retain more valid points; in static or highly textured scenes, the threshold is tightened to eliminate potential outliers.

8. The visual-inertial map joint positioning method in a complex environment based on SLAM according to claim 1, characterized in that: In step 7, the specific process is: 7.1) Construct a factor graph optimization framework. This framework models the problem as a graph, where each node represents a state variable and each edge represents a constraint or observation. This factor graph optimization framework unifies the visual reprojection error and the IMU pre-integration error into a single optimization objective function. 7.2) Perform nonlinear least squares optimization on the optimization objective function and iteratively solve the optimization problem to obtain the camera's six-degree-of-freedom pose, three-axis velocity, and map state. 7.3) The optimized results are output in real time through the ROS message interface to complete the positioning.

Images