Robot repositioning method in dynamic environment based on laser visual complementation

Abstract

The invention provides a robot repositioning method in a dynamic environment based on laser visual complementation. The robot repositioning method comprises two stages of offline mapping and online repositioning. In the off-line stage, two-dimensional and three-dimensional elimination of a dynamic target is realized through a lightweight detection network and geometric projection, and the purity of a prior map is improved; visual and laser radar static features are respectively extracted, a complementary double descriptor database is constructed, and laser radar descriptors fuse geometry and reflection intensity information to enhance discrimination. In the online stage, a coarse-to-fine strategy is adopted, rapid and robust global matching is achieved by combining GPS coarse positioning and radius search-based database retrieval, and scene recognition and accurate pose recovery are completed by using vision and radar dual channels. According to the method, a high-purity priori library is constructed through dynamic target collaborative elimination and multi-modal feature fusion, and autonomous high-precision global repositioning without manual initial pose input is realized in combination with GPS coarse positioning and a self-adaptive channel switching mechanism.

Description

Technical Field

[0001] This invention belongs to the field of robot autonomous navigation, specifically involving a global relocalization method based on dynamic object removal and fusion of visual images and LiDAR features, which is particularly suitable for long-term inspection and navigation tasks of robots in large-scale dynamic environments. Background Technology

[0002] In mobile robot navigation systems, localization is a crucial step in ensuring that the robot accurately obtains its own position in a known map environment. When a robot loses its localization due to reasons such as power-on restart, human interference, or sensor failure during navigation tasks, it needs to have the ability to relocalize. This involves comparing the environmental feature information collected by the current sensors with the previously constructed offline prior map to determine its current position on the map and achieve localization recovery. This technology directly determines the continuity and reliability of the robot in performing long-term inspections and autonomous navigation tasks in large-scale dynamic environments.

[0003] Currently, the mainstream robot relocalization methods mainly include LiDAR-based relocalization methods, vision-based relocalization methods, and deep learning-based relocalization methods.

[0004] For LiDAR-based relocalization methods, traditional algorithms such as AMCL, NDT, and ICP require manual provision of initial poses; otherwise, they may encounter difficulties in global convergence. Therefore, a method is needed to accurately represent the characteristics of different scenes, enabling robots to autonomously complete scene recognition and coarse localization without initial poses. Although some scene recognition methods based on LiDAR descriptors (such as Scan Context) have emerged in recent years, capable of global scene matching through geometric features to provide coarse poses for subsequent fine registration, these methods primarily rely on static environment assumptions and have weak ability to distinguish scenes with similar geometric structures. When there are many dynamic objects in the environment (such as pedestrians and vehicles), the geometric structure of the LiDAR point cloud is easily disturbed, leading to scene recognition failures or matching errors.

[0005] For vision-based relocalization methods, traditional approaches primarily rely on image features (such as corners and edges) for scene recognition and relocalization. However, due to the lack of reliable depth information in images, texture matching alone is insufficient to accurately recover 3D pose. Furthermore, changes in illumination, shadow interference, and the presence of dynamic targets significantly reduce the stability and robustness of image feature matching. In addition, the repeatability of image features is easily affected by environmental factors such as seasonal and temporal variations, leading to decreased localization accuracy.

[0006] In recent years, some studies have attempted to use deep learning methods to fuse geometric and semantic information in order to improve the accuracy of scene recognition and relocalization. However, these methods often rely on large-scale neural network models for feature extraction and semantic segmentation, which incurs significant computational overhead and requires high computing power, making them difficult to deploy on resource-constrained embedded mobile robot platforms.

[0007] Currently, there are some multi-sensor fusion solutions, but most of them simply overlay LiDAR and visual data without establishing a dynamic target collaborative elimination mechanism. The visual feature points and LiDAR point clouds formed by dynamic objects will cause interference, leading to online relocalization matching deviations. At the same time, most methods still rely on manual input of initial pose, resulting in low automation and limiting the robot's autonomous operation capabilities.

[0008] Therefore, developing a lightweight global relocalization method that can effectively eliminate dynamic environmental interference, achieve complementary advantages between LiDAR and vision, has adaptive channel switching capability, and does not require manual input of initial pose has become a key problem that urgently needs to be solved in the field of robot autonomous navigation. Summary of the Invention

[0009] This invention provides a robot relocalization method based on laser vision complementarity in dynamic environments, aiming to solve the problems of existing technologies such as reliance on initial pose, weak anti-interference in dynamic environments, and insufficient robustness of single sensors. Through dynamic target elimination, multimodal feature fusion, and adaptive channel switching mechanisms, it achieves autonomous high-precision global relocalization without human intervention.

[0010] This invention is implemented as follows: a robot relocalization method based on laser vision complementarity in a dynamic environment, comprising the following steps:

[0011] S1. Offline mapping stage: This includes the following steps:

[0012] S11. Selection and Alignment of Vision and Laser Keyframes: Acquire keyframe data of the robot's LiDAR and keyframe images from multiple directional cameras aligned with its timestamps.

[0013] S12. Image dynamic target detection: The lightweight YOLO-Nano target detection algorithm is used to detect dynamic targets in keyframes of the image and generate two-dimensional detection boxes.

[0014] S13. Feature Extraction and Visual Descriptor Construction: Calculate the Fast corner points of the keyframes of the image as visual feature points. Treat the visual feature points within the two-dimensional detection box as dynamic visual feature points and the visual feature points outside the two-dimensional detection box as static visual feature points. Use the BRIEF visual descriptor to encode the static visual feature points and construct a visual dictionary for subsequent retrieval and matching.

[0015] S14. Dynamic laser point removal: The camera-LiDAR extrinsic parameter matrix projects the LiDAR point cloud onto the camera image plane, assigns corresponding depth information to the visual feature points, obtains the three-dimensional coordinates of the dynamic visual feature points within the two-dimensional detection box, constructs a three-dimensional dynamic region, and removes the dynamic laser points within the region.

[0016] S15, LOAM localization and mapping: The LOAM algorithm is executed on the key frames of the LiDAR after removing dynamic laser points. The pose of the key frames is obtained through point cloud registration and a global offline map is generated.

[0017] S16. Construction of LiDAR descriptor: Fuse radar spatial height structure information and laser reflection intensity information to construct a LiDAR descriptor matrix containing multi-dimensional information;

[0018] S17. Prior data fusion and index construction: Associate the LiDAR descriptor with the visual descriptor and their corresponding keyframes and keyframe poses to construct a prior scene index library containing the LiDAR database and the visual database.

[0019] S2, Online Relocation Phase: This includes the following steps:

[0020] S21. Online data acquisition and coarse retrieval: Acquire current visual camera images, LiDAR point clouds and GPS observation data. When GPS signal is available, determine the robot's approximate position on the offline map based on GPS coordinates to reduce the global retrieval space.

[0021] S22, Dual-channel retrieval and scene recognition: Parallel retrieval of the LiDAR database and visual database to obtain the optimal candidate scene matching results for each channel;

[0022] S23. Adaptive Channel Switching: Compare the confidence levels between the best matching results of the vision and LiDAR channels, and use an adaptive channel switching mechanism to select the one with higher confidence as the candidate result.

[0023] S24. Fine matching and recovery localization: The selected candidate matching results are further optimized using NDT or PnP fine matching algorithms to obtain the final pose and complete the fine localization.

[0024] Preferably, in step S11, the system timestamps of the lidar and the camera are generated by the same master clock. The lidar keyframes are selected every other frame based on the complete scan frame. The visual keyframes are obtained by nearest neighbor matching of timestamps. The reflection points in the lidar point cloud are processed for distortion correction based on the point-level timestamps. Each lidar keyframe and its corresponding visual keyframe are combined into a set of synchronized keyframes.

[0025] Preferably, in step S14, the three-dimensional points in the lidar coordinate system are projected onto the camera image plane through the camera-lidar extrinsic parameter matrix, the RANSAC algorithm is introduced to filter outliers of the three-dimensional dynamic feature points, and a three-dimensional cube bounding box is generated based on the x, y, and z direction boundaries to construct the dynamic region.

[0026] Preferably, in step S15, the point cloud of the LiDAR keyframe after removing dynamic laser points is height filtered, and then the LOAM algorithm is used for inter-frame registration to generate a global offline map and record the pose set in the offline map corresponding to the LiDAR keyframe; based on the extrinsic parameters between the camera and the LiDAR, the pose set in the offline map corresponding to the visual keyframe is obtained through the transformation matrix.

[0027] Preferably, in step S16, with the robot's location as the center, the horizontal plane of the LiDAR keyframe point cloud is divided into N equal-angle sectors and M concentric rings to form M×N sub-regions; the maximum height value and maximum reflection intensity value of the point cloud in each sub-region are calculated and normalized respectively to form a height matrix and a reflection matrix, which are then spliced ​​together in the feature dimension to obtain the LiDAR descriptor sub-matrix.

[0028] Preferably, in step S22, the visual channel uses the bag-of-words model to measure similarity, and the lidar channel uses matrix cosine similarity to measure similarity, and the key frame with the highest score is selected as the candidate matching result.

[0029] Preferably, in step S23, when a sensor degrades or fails, causing a significant decrease in similarity exceeding a set minimum threshold, the adaptive channel switching mechanism automatically switches to another sensor channel.

[0030] Preferably, in step S24, the visual channel estimates the relative pose transformation using the PnP algorithm, and the lidar channel performs point cloud registration using the NDT algorithm. Both are combined with the global pose of the candidate frames to calculate the global pose of the current frame.

[0031] Beneficial effects

[0032] Compared with the prior art, the beneficial effects of the present invention are: the present invention provides a robot relocalization method based on laser vision complementarity in a dynamic environment.

[0033] 1. This invention achieves simultaneous 2D and 3D removal of dynamic targets using only a lightweight target detection network and projection mechanism, effectively improving the purity and stability of the prior map. This mechanism maintains stable map building and pose recovery capabilities even in complex scenes with dynamic objects, thereby significantly improving the robot's autonomous navigation and task execution performance in dynamic environments.

[0034] 2. This invention employs a complementary fusion strategy of visual images and LiDAR features. The visual component is responsible for dynamic object detection and assists the LiDAR in 3D dynamic point removal; the LiDAR provides absolute distance and depth information, compensating for the depth uncertainty in visual images and improving the estimation accuracy of camera pose during subsequent online localization. By constructing a dual feature database of visual and LiDAR features, both the visual texture information of the environment and the geometric structure information of the LiDAR are preserved, thereby significantly enhancing the richness of the scene feature database.

[0035] 3. This invention utilizes the geometric spatial height information and laser reflection intensity information of key frames of LiDAR to construct a multi-dimensional descriptor matrix, which significantly enhances the scene representation capability of LiDAR descriptors and improves the accuracy and stability of scene recognition and matching.

[0036] 4. This invention employs a coarse-to-fine dual-channel retrieval strategy to recover the robot's precise pose in a priori map. When GPS signals are available, GPS information is first used to determine the robot's approximate position on the map, and a distance search algorithm based on radius constraints is used to perform local scene retrieval, thereby significantly accelerating database query efficiency. This is particularly suitable for efficient scene recognition and matching in large-scale environments.

[0037] 5. When the system starts up or experiences a loss of positioning (such as during a power-on restart, human interference, or sensor failure), this invention eliminates the need for manual provision of initial pose. It automatically achieves global scene recognition and precise repositioning through a dual-database retrieval and confidence level switching mechanism. Even in the event of a single sensor failure or temporary unavailability, the system maintains stable repositioning capabilities, demonstrating high robustness and autonomy. Attached Figure Description

[0038] Figure 1 This is a flowchart of the method of the present invention;

[0039] Figure 2 This is a schematic diagram of the system in this invention;

[0040] Figure 3 This is a schematic diagram of the detection and projection combined mechanism in this invention;

[0041] Figure 4 This is a schematic diagram of the lidar descriptor construction process in this invention. Detailed Implementation

[0042] To make the objectives, technical solutions, and advantages of this invention clearer, the invention will be further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative and not intended to limit the invention.

[0043] This invention is based on a robot. Regarding the sensor configuration of the entire robot relocalization system, four color cameras (RGB cameras) are evenly installed on the robot's horizontal plane. This combination of four cameras (front, rear, left, and right) can cover almost all directions around the robot, achieving omnidirectional visual coverage of the robot's surroundings. Furthermore, to obtain accurate spatial depth information and point cloud maps, the system is equipped with a 3D LiDAR for measuring geometric distance information and mapping the environment. Simultaneously, the system includes a GPS device to assist the robot in performing coarse localization.

[0044] Please see Figure 1-4 This invention provides a technical solution: a robot relocalization method based on laser vision complementarity in a dynamic environment, comprising the following steps:

[0045] S1. Offline Mapping Stage: In the offline mapping stage, the system inputs data from a 3D LiDAR and four RGB camera sensors (front, rear, left, and right). Sensor time alignment is used to select keyframes for both vision and LiDAR. The YOLO-Nano image dynamic target detection algorithm is used to detect dynamic visual feature points on the selected visual keyframes, and static visual feature points are extracted to construct a visual descriptor. Simultaneously, based on the detected dynamic visual feature points, a 3D projection mechanism is used to synchronously remove dynamic LiDAR points. Subsequently, the LOAM localization and mapping algorithm is executed on the LiDAR keyframes with removed dynamic LiDAR points to obtain an offline map and pose information, and a LiDAR descriptor is constructed. Finally, based on all prior information, the system obtains both a visual database and a LiDAR database, which are then combined into a high-purity prior scene index library.

[0046] Specifically, it includes the following steps:

[0047] S11. Selection and Alignment of Vision and Laser Keyframes: The lidar and camera independently acquire data at their respective frequencies, and assign a high-precision system timestamp to each frame of data. Preferably, the system timestamp is generated from the same master clock (ROS system time) to ensure time source consistency.

[0048] Using the complete scan frame of the LiDAR as a baseline, LiDAR keyframes are selected every other frame. For each selected LiDAR keyframe, a keyframe correspondence method based on timestamp nearest neighbor matching is used to retrieve the image frame with the closest time from the camera image sequence, which is then used as the corresponding visual keyframe. Simultaneously, each reflection point received in the LiDAR point cloud data undergoes distortion correction based on its point-level timestamp to ensure strict alignment between the visual image and the point cloud on the time axis. Finally, each LiDAR keyframe and its corresponding visual keyframe are combined into a set of synchronized keyframe data and stored in a database, providing a high-precision, temporally consistent input data foundation for subsequent dynamic target detection, point cloud culling, and multimodal feature alignment.

[0049] S12. Image dynamic target detection: The lightweight YOLO-Nano target detection algorithm is used to detect dynamic targets in keyframes of the image and generate two-dimensional detection boxes.

[0050] In the dynamic object detection stage of this invention, to ensure the purity and stability of the prior map, dynamic targets are identified and removed from the multi-view image sequence. First, during the offline mapping task, the robot simultaneously acquires multi-view image sequences using multi-view cameras installed in the front, back, left, and right directions of the robot, achieving omnidirectional coverage in the horizontal view. For each laser keyframe's corresponding timestamp, the set of multi-view image frames that are at the same time or closest in time is obtained, denoted as I. in, These represent the front, back, left, and right camera images of the robot at time t, respectively.

[0051] During the mapping phase, the robot acquires image sequences from multiple cameras (front, back, left, and right). For each keyframe image... t The lightweight object detection algorithm YOLO-Nano is used for dynamic object detection (pedestrians, vehicles, etc.). YOLO-Nano can reduce computational complexity while maintaining high detection accuracy, making it suitable for embedded low-power computing platforms. The detection result can be represented as a set of detection boxes:

[0052] B t ={b1,b2,…,b n}

[0053] The detected dynamic target region b i This will be used to guide the dynamic point culling process of subsequent LiDAR point clouds.

[0054] S13. Feature Extraction and Visual Descriptor Construction: After completing dynamic target detection in multi-camera images, image feature points are obtained by calculating Fast corner points in the images. Visual feature points within the 2D detection box are considered dynamic visual feature points, while visual feature points outside the 2D detection box are considered static visual feature points. Based on the set of feature points within the detection box region, the dynamic feature point set F is extracted. d The complete set of feature points F is used to obtain static feature points F outside the detection box. s =FF d .

[0055] To improve system efficiency and reduce the size of the visual database, this invention uses only keyframe images from the robot's forward-facing camera as input. The static feature point set F outside the detection box... s Calculate its local BRIEF visual descriptor D vBRIEF visual descriptors store the intensity relationships of feature point neighborhoods in binary form, offering advantages such as low computational overhead and fast matching speed, making them suitable for deployment in embedded systems. Subsequently, the BRIEF visual descriptors for each frame are quantized and indexed using the Bag-of-Words (BoW2) model to construct a visual feature bag-of-words database. This database uses a visual dictionary as its index structure and represents image feature distribution through histogram-based feature vectors, enabling rapid scene retrieval and matching during online relocalization.

[0056] S14. Dynamic laser point removal: based on the calibrated camera-LiDAR extrinsic parameter matrix. The three-dimensional point (x) in the lidar coordinate system l ,y l ,z l The projection onto the camera image plane is as follows:

[0057]

[0058] Where R is the 3D rotation matrix, t is the 3D displacement vector, K is the camera intrinsic parameter matrix, (u,v) are the pixel coordinates, and s is the scale factor. Through the above projection, the pixel positions of the LiDAR points in the image can be obtained. Based on the nearest neighbor search algorithm, corresponding depth information is assigned to neighboring visual feature points. On this basis, the 3D coordinates of the dynamic visual feature points within the 2D detection box are inversely calculated in the LiDAR coordinate system based on the depth information. Simultaneously, to improve the robustness of region estimation, the Random Sample Consensus Algorithm (RANSAC) is introduced to fit and filter out outliers from the inversely calculated 3D dynamic feature points. Finally, a 3D bounding box is generated based on the boundary ranges of the 3D dynamic feature points in the x, y, and z directions, constructing a 3D dynamic region. And from the associated original point cloud P... t Remove the dynamic laser points R within the range of this three-dimensional dynamic region. d Obtain the 3D point cloud P after removing dynamic points. ' t ,like Figure 3 As shown, adaptive detection and removal of dynamic targets are achieved:

[0059] P ' t =P t -R d

[0060] Through the above method, this invention can achieve integrated processing of two-dimensional detection and three-dimensional removal of dynamic targets without relying on complex semantic segmentation or high-performance GPUs, using only a lightweight detection network and geometric projection relationships. This improves the static purity and mapping stability of the prior map, providing a foundation for subsequent scene recognition and relocalization.

[0061] S15, LOAM Localization and Mapping: In this invention, to further improve the geometric stability and descriptor expression accuracy of the map, after dynamic region culling, ground filtering and geometric feature extraction are performed on the laser point cloud data. Considering that the robot may encounter uneven terrain (such as stairs, steps, etc.) and ground clutter during navigation, to reduce the impact of ground noise on map construction, height filtering is performed on each frame of point cloud to remove near-ground points below 30cm to ignore ground plane interference. Subsequently, the processed keyframe point cloud sequence {P ' t The LOAM algorithm is used for inter-frame registration, gradually stitching together to form a dense static prior map. Simultaneously, the poses (including the 3D rotation matrix R and the 3D translation vector t) of the corresponding offline map for each LiDAR keyframe are recorded. Based on the extrinsic parameters between the camera and LiDAR, the pose set in the offline map corresponding to the associated visual keyframes can be obtained through the transformation matrix.

[0062] S16. LiDAR Descriptor Construction: After removing dynamic laser points, to achieve high-precision scene recognition and relocalization, local LiDAR features are extracted from each keyframe of the LiDAR, and the LiDAR geometric descriptor D is calculated. l .like Figure 4 As shown. First, the point cloud of the currently acquired LiDAR keyframe is spatially divided with the robot's location as the center. The horizontal plane is divided into N equal-angle sectors, and further divided into M concentric rings based on distance, thus forming M×N sub-regions in polar coordinate space. For the point cloud set within each sub-region, its maximum height value and maximum reflection intensity value are calculated and normalized to the [0,1] interval. The maximum height values ​​of all sub-regions are arranged in spatial order to form a height matrix H∈R. M×N The reflection matrix R ∈ R is formed by composing all the reflection intensity values. M×N Then, the two are concatenated along the feature dimension to obtain the joint descriptor matrix:

[0063]

[0064] This lidar descriptor matrix not only preserves the geometric structure of the lidar point cloud (reflecting scene undulations through height variations), but also integrates reflection intensity distribution information (different materials have different reflection intensity values), enhancing the ability to distinguish between different materials and object surfaces, and providing a robust feature foundation for subsequent scene matching and localization.

[0065] S17. Prior data fusion and index construction: Associate the LiDAR descriptors with the visual descriptors and their corresponding keyframes and keyframe poses to construct a prior scene index library. This index library includes the LiDAR database and the visual database, which are used to achieve fast retrieval and matching during the online relocalization stage.

[0066] Through the aforementioned steps, the system obtained the visual database. and lidar database

[0067]

[0068] Where M v M is a set of keyframes for an image. l This is a set of keyframes for LiDAR. When constructing the prior scene index library, the keyframe index structure is used as the core to uniformly organize and associate the multimodal information under each keyframe. Specifically, the visual / LiDAR descriptor under each keyframe is associated and stored with its corresponding visual / LiDAR sensor pose, as well as the visual image and LiDAR point cloud under that keyframe. Among them, the visual descriptor D... v Using the BoW2 bag-of-words model as the index structure, fast similarity retrieval based on descriptors can be achieved. (LiDAR descriptor D) l Using keyframes as spatial indexes and combining them with the lidar descriptor matrix, efficient search based on spatial structure similarity is achieved, facilitating querying and retrieval in subsequent relocation stages.

[0069] S2. Online Relocalization Phase: In the online relocalization phase, the system input includes RGB images from the forward feed, laser point cloud information, and data observed by the GPS sensor. When GPS signal is available, the system utilizes the coarse localization results provided by GPS to narrow down the search range through distance radius search. Subsequently, through feature extraction, the system calculates the visual and LiDAR descriptors for the current frame, performs searches and matches in the visual and LiDAR databases respectively, and calculates the similarity. Based on the similarity score, the system selects the most matching image frame or LiDAR scene from the search results and adaptively switches to the corresponding matching channel through a channel switching mechanism: if visual features are more reliable, the PnP algorithm is used for visual localization; if the laser scene is more similar, LiDAR localization is performed based on NDT matching. This helps the robot achieve proactive scene recognition and pose recovery, completing high-precision global relocalization in complex dynamic environments.

[0070] Specifically, it includes the following steps:

[0071] S21. Online Data Acquisition and Coarse Search: Acquire the data received by the current sensors, and when GPS signal is available, first determine the robot's approximate location on the offline map based on GPS coordinates to reduce the global search space.

[0072] After the robot restarts, loses its localization, or the task is interrupted, the system of this invention enters the online relocalization phase. At this time, the system acquires current sensor data in real time, including visual camera image sequences and LiDAR point cloud frames, and calculates and extracts the corresponding visual descriptors respectively. With laser descriptor This serves as a representation of the multimodal environment at the current moment.

[0073] When GPS signals are available, the system prioritizes using the current GPS coordinates to estimate the robot's approximate position in the prior map coordinate system. Considering the meter-level error in GPS measurements, this invention employs a radius search strategy based on spatial constraints to estimate the pose setting radius r (e.g., 20m) and retrieve a set of candidate keyframes from a priori database.

[0074]

[0075] Where t k This represents the pose vector of the k-th keyframe. Through this GPS-constrained coarse search mechanism, the system can significantly reduce the global search space and improve matching efficiency, making it particularly suitable for rapid scene recognition and relocalization in large-scale or long-cycle inspection scenarios.

[0076] S22. Dual-channel retrieval and scene recognition: Parallel retrieval of the LiDAR database and the visual database to obtain the optimal candidate scene matching results for each channel.

[0077] For the set of candidate keyframes retrieved in the coarse search phase A dual-channel retrieval mechanism is adopted to perform local scene descriptor matching in the visual database and the LiDAR database respectively, and to calculate the similarity score between the current frame descriptor and each candidate descriptor in the database.

[0078] In visual channel matching, the visual descriptor extracted from the current frame is used. A similarity measurement method based on the bag-of-words model and visual descriptors for each keyframe in the visual database are used. Calculate the similarity distance and obtain the most similar (shortest distance) image keyframes by sorting them. For the lidar channel, the geometric descriptor of the current frame is used. LiDAR descriptors for each candidate keyframe in the LiDAR database Calculate the cosine similarity of matrices:

[0079]

[0080] Select the keyframe with the highest score As a candidate frame for LiDAR matching.

[0081] S23. Adaptive Channel Switching: Compare the confidence levels between the best matching results of the vision and LiDAR channels, and use an adaptive channel switching mechanism to select the one with higher confidence as the candidate result.

[0082] To improve the system's robustness under different environments and sensor conditions, this system introduces an adaptive channel switching mechanism. This mechanism dynamically selects the most reliable recognition result based on the similarity scores of the visual and LiDAR channels. After two channels have completed matching, the system retrieves the optimal results for each and compares their similarity. If sim(v i )>sim(l i If the visual channel result is satisfactory, the LiDAR channel result is used as the final candidate frame; otherwise, the LiDAR channel result is used as the relocalization basis. Furthermore, when the system detects degradation of a sensor (such as the visual channel being affected by illumination or blurring, or the LiDAR being affected by strong reflection or sparse point clouds) causing a significant decrease in similarity exceeding the set minimum threshold, the adaptive channel switching mechanism will automatically switch to another sensor channel to ensure the continuity and reliability of scene recognition. Through this mechanism, the system can adaptively select the optimal feature channel under different environments and sensor conditions, achieving robust scene matching and relocalization capabilities.

[0083] S24. Fine Matching and Re-localization: The selected candidate matching results are further optimized using NDT or PnP fine matching algorithms to obtain the final pose, complete the fine localization, and realize automatic scene recognition and global relocalization without manual input of the initial pose.

[0084] After completing coarse matching, switching, and candidate frame selection in both channels, this system selects either the visual channel or the LiDAR channel for fine matching and pose optimization to further improve repositioning accuracy.

[0085] When selecting the visual channel, since the system has already obtained the rigid body transformation matrix from the LiDAR coordinate system to the camera coordinate system through extrinsic parameter calibration, it can obtain the true 3D spatial coordinates corresponding to each pixel feature point in the current image frame, thereby eliminating the problem of scale uncertainty in monocular vision features. Subsequently, the reprojection error is constructed using the PnP algorithm to estimate the relative pose transformation of the current frame (cur) relative to the candidate frame (ref). Based on the offline map global pose associated with candidate keyframes in the image database. The global pose of the current camera frame in the offline map can be calculated as follows:

[0086]

[0087] Similarly, when selecting a lidar channel, the system selects the current keyframe point cloud p. curThe point cloud p with the highest similarity to the candidate frame in the database ref The NDT (Normal Distributions Transform) point cloud matching method is used for accurate registration to obtain the optimal pose transformation between two LiDAR frames. Based on the offline map global pose associated with candidate keyframes in the database. The global pose of the current camera frame in the offline map can be calculated as follows:

[0088]

[0089] This mechanism maintains stable relocation performance and restores the robot's position in the offline map even under conditions of visual degradation (such as lighting or occlusion) or interference with the LiDAR (such as dynamic objects or sparse reflections).

[0090] This method combines offline mapping with online relocalization to achieve stable scene recognition and pose recovery under conditions of no initial pose, sensor degradation, or dynamic environmental changes.

[0091] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, and improvements made within the spirit and principles of the present invention should be included within the protection scope of the present invention.

Claims

1. A robot relocalization method based on laser vision complementarity in a dynamic environment, characterized in that: Includes the following steps: S1. Offline mapping stage: This includes the following steps: S11. Selection and Alignment of Vision and Laser Keyframes: Acquire keyframe data of the robot's LiDAR and keyframe images from multiple directional cameras aligned with its timestamps. S12. Image dynamic target detection: The lightweight YOLO-Nano target detection algorithm is used to detect dynamic targets in keyframes of the image and generate two-dimensional detection boxes. S13. Feature Extraction and Visual Descriptor Construction: Calculate the Fast corner points of the keyframes of the image as visual feature points. Treat the visual feature points within the two-dimensional detection box as dynamic visual feature points and the visual feature points outside the two-dimensional detection box as static visual feature points. Use the BRIEF visual descriptor to encode the static visual feature points and construct a visual dictionary for subsequent retrieval and matching. S14. Dynamic laser point removal: The laser radar point cloud is projected onto the camera image plane through the camera-laser extrinsic parameter matrix, and the corresponding depth information is assigned to the visual feature points. The three-dimensional coordinates of the dynamic visual feature points within the two-dimensional detection box are obtained, a three-dimensional dynamic region is constructed, and the dynamic laser points within the region are removed. S15, LOAM localization and mapping: The LOAM algorithm is executed on the key frames of the LiDAR after removing dynamic laser points. The pose of the key frames is obtained through point cloud registration and a global offline map is generated. S16. Construction of LiDAR descriptor: Fuse radar spatial height structure information and laser reflection intensity information to construct a LiDAR descriptor matrix containing multi-dimensional information; S17. Prior data fusion and index construction: Associate the LiDAR descriptor with the visual descriptor and their corresponding keyframes and keyframe poses to construct a prior scene index library containing the LiDAR database and the visual database. S2, Online Relocation Phase: This includes the following steps: S21. Online data acquisition and coarse retrieval: Acquire current visual camera images, LiDAR point clouds and GPS observation data. When GPS signal is available, determine the robot's approximate position on the offline map based on GPS coordinates to reduce the global retrieval space. S22, Dual-channel retrieval and scene recognition: Parallel retrieval of LiDAR database and visual database. The optimal candidate scene matching results for each channel were obtained respectively; S23. Adaptive Channel Switching: Compare the confidence levels between the best matching results of the vision and LiDAR channels, and use an adaptive channel switching mechanism to select the one with higher confidence as the candidate result. S24. Fine matching and recovery localization: The selected candidate matching results are further optimized using NDT or PnP fine matching algorithms to obtain the final pose and complete the fine localization.

2. The robot relocalization method based on laser vision complementarity in a dynamic environment as described in claim 1, characterized in that: In step S11, the system timestamps of the lidar and the camera are generated by the same master clock. The lidar keyframes are selected every other frame based on the complete scan frame. The visual keyframes are obtained by nearest neighbor matching of timestamps. The reflection points in the lidar point cloud are processed for distortion correction based on the point-level timestamps. Each lidar keyframe and its corresponding visual keyframe are combined into a set of synchronized keyframe data.

3. The robot relocalization method based on laser vision complementarity in a dynamic environment as described in claim 1, characterized in that: In step S14, the three-dimensional points in the lidar coordinate system are projected onto the camera image plane through the camera-lidar extrinsic parameter matrix. The RANSAC algorithm is introduced to filter outliers of the three-dimensional dynamic feature points. Based on the x, y, and z direction boundaries, a three-dimensional cube bounding box is generated to construct the dynamic region.

4. The robot relocalization method based on laser vision complementarity in a dynamic environment as described in claim 1, characterized in that: In step S15, the point cloud of the LiDAR keyframe after removing dynamic laser points is height filtered, and then the LOAM algorithm is used for inter-frame registration to generate a global offline map and record the pose set in the offline map corresponding to the LiDAR keyframe; based on the extrinsic parameters between the camera and the LiDAR, the pose set in the offline map corresponding to the visual keyframe is obtained through the transformation matrix.

5. The robot relocalization method based on laser vision complementarity in a dynamic environment as described in claim 1, characterized in that: In step S16, with the robot's location as the center, the horizontal plane of the LiDAR keyframe point cloud is divided into N equal-angle sectors and M concentric rings to form M×N sub-regions. The maximum height value and maximum reflection intensity value of the point cloud in each sub-region are calculated and normalized to form a height matrix and a reflection matrix. The LiDAR descriptor sub-matrix is ​​obtained by splicing them together in the feature dimension.

6. The robot relocalization method based on laser vision complementarity in a dynamic environment as described in claim 1, characterized in that: In step S22, the visual channel uses the bag-of-words model to measure similarity, while the lidar channel uses matrix cosine similarity. The keyframe with the highest score is selected as the candidate matching result.

7. The robot relocalization method based on laser vision complementarity in a dynamic environment as described in claim 1, characterized in that: In step S23, when a sensor degrades or fails, causing a significant decrease in similarity exceeding the set minimum threshold, the adaptive channel switching mechanism automatically switches to another sensor channel.

8. The robot relocalization method based on laser vision complementarity in a dynamic environment as described in claim 1, characterized in that: In step S24, the visual channel estimates the relative pose transformation using the PnP algorithm, and the lidar channel performs point cloud registration using the NDT algorithm. Both are combined with the global pose of the candidate frames to calculate the global pose of the current frame.

Images