Multi-sensor fusion repositioning method based on real-time positioning confidence

Abstract

The invention belongs to the technical field of logistics robot positioning, and particularly relates to a multi-sensor fusion repositioning method based on real-time positioning confidence, and the method comprises the following steps: constructing a two-dimensional grid map through laser SLAM, fusing a depth camera image and radar point cloud data to construct an associated data set of a vision-laser image and a current corresponding pose, and carrying out the fusion of the depth camera image and the radar point cloud data; when the real-time positioning confidence coefficient is lower than a threshold value for the first time, a radar scanning matching mode is adopted for repositioning, if the real-time positioning confidence coefficient is still lower than the threshold value after repositioning, a vision and radar fusion method is used for repositioning, firstly, fine screening is conducted on poses through the vision and radar fusion method, and then the poses are screened; and taking the pose corresponding to the fine screening as the initial pose of a radar scanning matching algorithm for fine positioning, and taking the result of the fine positioning as the current positioning pose of the robot. According to the method, the pose information of the robot can be quickly obtained, and the real-time requirement of a navigation scene is met.

Description

TECHNICAL FIELD

[0001] The present application belongs to the technical field of logistics robot positioning, and particularly relates to a multi-sensor fusion repositioning method based on real-time positioning reliability. BACKGROUND

[0002] Mobile robot repositioning refers to estimating the position and pose of the robot in the known global map only by relying on its own sensor data without any prior information. Repositioning usually occurs after simultaneous localization and mapping (SLAM) is completed and enters the autonomous navigation stage, and is the basis for realizing autonomous navigation. When the robot is powered on or encounters a sudden situation (such as device restart), the initial position needs to be estimated through repositioning. The kidnapping phenomenon refers to the fact that during the robot navigation process, due to external interference (such as human moving, collision, etc.), the pose of the robot changes dramatically, causing the positioning algorithm that relies on continuous pose changes to fail, and the pose needs to be estimated again. The kidnapping problem is particularly prominent in dynamic or unstructured environments, and puts higher requirements on the robustness and autonomy of the robot system.

[0003] Traditional repositioning methods mostly rely on single sensor data, such as laser radar or visual information, and it is difficult to maintain high precision and high robustness in complex scenes.

[0004] Therefore, how to effectively fuse multi-sensor information and combine the real-time and accuracy of the positioning algorithm has become the key to solving the repositioning problem. SUMMARY

[0005] The present application aims to overcome the above-mentioned problems in the prior art and provide a multi-sensor fusion repositioning method based on real-time positioning reliability, which can improve the repositioning performance of the robot in complex environments and improve the accuracy and robustness of positioning.

[0006] To achieve the above technical purposes and effects, the present application is implemented by the following technical solutions:

[0007] The present application provides a multi-sensor fusion repositioning method based on real-time positioning reliability, and the specific steps are as follows:

[0008] S1, first, simultaneous localization and mapping based on laser is performed, after the completion of the two-dimensional grid map, the mobile robot is controlled to synchronously collect RGB-D images and laser point cloud data, a composite image of fused visual-laser features is generated through registration, the captured images are trained into words by the bag-of-words search training method, forming a mapping relationship data set of <image feature vector, pose coordinates>, for use in the following repositioning;

[0009] S2, whether the robot is kidnapped is judged based on real-time positioning reliability;

[0010] S3, when the current threshold is lower than the set threshold for the first time, a radar scan matching method is started to relocate, and the radar scan matching method is to match real-time laser scan points with pre-processed map features, i.e., a gradient mask map, to perform iterative search near an initial pose, find an optimal pose matching, and use the optimal pose matching as a current pose of the robot for subsequent navigation work;

[0011] S4, if the current threshold is still lower than the set threshold, radar and vision are used for relocation; in order to improve the relocation speed, a composite image composed of a current frame image captured by a depth camera and a radar graph at the current time is used to perform preliminary candidate matching with previously collected images to obtain possible candidate image groups, then ORB descriptors of the candidate images and the current frame image are matched, a method of FLANN algorithm and RANSAC algorithm is used to find an image most similar to the current image, and a corresponding pose is used as an initial pose in the radar scan matching algorithm to find a current optimal pose, which is used as a current pose of the robot for subsequent navigation work.

[0012] Further, in step S1, the correspondence between the fusion visual-laser feature composite image and the current frame pose is obtained by triggering data collection at intervals of 5 cm displacement or 5° rotation by driving the mobile robot through the motion control module, synchronously acquiring the RGB-D image of the depth camera and the laser radar point cloud data, and recording the pose information of the current positioning.

[0013] Further, in step S1, when constructing a visual positioning database based on a Bag-of-Words model, an offline training strategy is adopted to encode the continuously collected image sequence into a visual dictionary, and a pose index file (pose.txt) strictly spatially and temporally aligned with the image frames is generated, forming a mapping relationship dataset of <image feature vector, pose coordinates>.

[0014] Further, in step S2, the specific operation of calculating the real-time positioning confidence is as follows:

[0015] A real-time confidence evaluation model based on IMU, odometry, and radar multi-sensor fusion is designed; whether relocation is needed is determined by the real-time confidence score of multi-sensor fusion, because in actual scenes, there are some temporary obstacles in the scene, so the judgment of the radar point cloud and the map cannot only use the grid map, but also add a local cost map, if the confidence score is continuously lower than the adaptive threshold, it is determined that the robot encounters a positioning problem, and the hierarchical relocation module is activated.

[0016] Further, in step S3, when the current threshold is lower than the set threshold for the first time, a radar scan matching method is started to relocate, and the specific steps are as follows:

[0017] The core process of radar scan matching includes map processing, pose initialization, laser data processing, iterative optimization matching, convergence judgment and result publishing; the system first receives and processes map data, trims the effective area and creates a gradient mask; when receiving the relocalization signal of the relocalization detection and triggering module, it is judged whether it is the first trigger or the second trigger, if it is the first trigger, the pose provided by / amcl_pose is obtained, if it is the second trigger, the pose provided by / Dbow_pose is obtained, and then the relocalization process is started.

[0018] Further, the relocalization process: convert the lidar data into point cloud in the map coordinate system, maximize the matching score of the laser point cloud and the map by iterative optimization of the pose (translation and rotation); when the pose change converges (the change is less than the threshold for 10 times in succession), the optimized pose is published to the / initialpose topic, and the TF transformation is updated. The whole process realizes the optimization from the rough initial pose to the accurate pose.

[0019] Further, in step S4, if the current threshold is still lower than the set threshold, radar and vision are used for relocalization, and the specific steps are as follows:

[0020] Based on the DBoW3 visual bag-of-words model, the current frame of RGB-Lidar fusion image (depth camera RGB frame and laser point cloud projection synthesis) is quickly matched with the reference image collected in advance. By constructing the inverted index structure of the visual dictionary, the top 5 candidate image groups are screened out, the subsequent calculation complexity is effectively reduced, the fine feature alignment is implemented in the candidate image set, the ORB feature descriptors of the current frame and the candidate images are extracted, the feature correspondence is established based on the FLANN (fast approximate nearest neighbor) algorithm, the RANSAC (random sample consensus) algorithm is used for false matching elimination, the matching confidence is evaluated by the maximum number of inliers, the optimal reference frame is selected and the associated initial pose estimation is obtained, the candidate image ID with the maximum number of inliers is selected as the best match, and the pose of the image in the pose saving text is retrieved as the initial pose of the radar scan matching algorithm for fine matching.

[0021] Further, the hierarchical architecture realizes the balance between positioning efficiency and accuracy: the fast retrieval based on the bag-of-words model in the front end ensures the real-time requirement, and the optimization process based on the radar scan matching in the back end can control the positioning error within ±2cm / ±1°.

[0022] The beneficial effects of the present application are:

[0023] 1. By fusing RGB-D images and laser point cloud data, a high-precision associated dataset is constructed, and the Bag of Words model is used to extract visual features, effectively improving the accuracy and robustness of relocalization.

[0024] 2. A real-time positioning confidence assessment model is introduced, combined with IMU, odometry and radar data, to realize dynamic monitoring of positioning reliability, significantly enhancing the adaptability of robots in complex environments.

[0025] 3. A hierarchical relocalization strategy is proposed, using radar matching and vision-assisted radar matching methods under different confidence thresholds, significantly improving relocalization efficiency and success rate, thereby ensuring stable navigation of robots in the presence of temporary obstacles and other interference conditions.

[0026] 4. Through the cooperative application of local cost maps and grid maps, the adaptability and accuracy of radar point cloud matching are optimized, solving the problem of positioning deviation caused by temporary obstacles.

[0027] 5. A multi-sensor data acquisition and tightly coupled processing mechanism is used to improve the overall response speed and data processing stability of the system, providing reliable support for long-term and large-scale robot operation.

[0028] 6. The system architecture is designed modularly, with good portability and expandability, suitable for various robot platforms and complex environment application scenarios, significantly improving the practical value and promotion prospects of relocalization technology.

[0029] Of course, implementing any product of the present application does not necessarily require all the above advantages to be achieved simultaneously. BRIEF DESCRIPTION OF DRAWINGS

[0030] In order to more clearly illustrate the technical solutions of the embodiments of the present application, the following will briefly introduce the drawings needed to be used for the embodiment description. Obviously, the drawings in the following description are only some embodiments of the present application, and other drawings can also be obtained by those skilled in the art without creative labor on the basis of these drawings.

[0031] Figure 1 The overall architecture diagram of the robot relocalization based on real-time positioning confidence of the present application;

[0032] Figure 2 The flowchart of image acquisition and generation of visual words of the present application;

[0033] Figure 3 The histogram of image acquisition and visual word distribution of the present application;

[0034] Figure 4 The flowchart of real-time positioning confidence score of the present application;

[0035] Figure 5 This is a flowchart of the radar score calculation module of the present invention;

[0036] Figure 6 This is a laser radar scan image of the present invention;

[0037] Figure 7 This is a flowchart of the integrated health scoring module of the present invention;

[0038] Figure 8 This is a flowchart of the visual-assisted relocation based on bag-of-words search according to the present invention;

[0039] Figure 9 This is a schematic diagram of ORB matching feature points between the current image and the images in the bag of words in this invention;

[0040] Figure 10 These are the ORB+FLANN+RANSAC feature points of the image and bag-of-words image of this invention;

[0041] Figure 11 This is a flowchart of the radar scanning matching process of the present invention;

[0042] Figure 12 This is a flowchart of the raster map preprocessing process of the present invention;

[0043] Figure 13 This is a flowchart of the radar scanning process of the present invention. Detailed Implementation

[0044] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0045] The specific embodiments of the present invention are as follows:

[0046] like Figure 1 As shown, this embodiment provides a multi-sensor fusion relocalization method based on real-time location confidence, including the following steps:

[0047] S1. Multi-sensor Data Acquisition and Map Building: Environmental point cloud data is acquired via LiDAR, and robot motion information is obtained by combining it with IMU sensors. LiDAR SLAM is then performed to build a global grid map for navigation and a local cost map containing information on temporary obstacles. Simultaneously, the robot is controlled to synchronously acquire RGB-D images at set distance or angle intervals (e.g., every 5cm movement or every 5° rotation) during movement, ensuring that the image acquisition density matches the grid map resolution to achieve continuous coverage of spatial features.

[0048] S2. Visual Feature Learning and Bag-of-Words Model Establishment: Environmental features are extracted using visual sensors. By fusing RGB-D images with synchronously acquired radar point cloud data, a dataset relating visual-laser composite images to poses is constructed. Subsequently, a bag-of-words search training method is used to extract visual features from the composite images, generating visual word histograms. Based on this, a bag-of-words model for fast image retrieval is established.

[0049] S3. Real-time Positioning Reliability Assessment and Relocation Trigger Decision: A real-time confidence assessment model based on IMU, odometry, and radar multi-sensor fusion is proposed. This model comprehensively considers the matching errors between radar point cloud and grid map, local cost map, and the consistency of data from various sensors, outputting a comprehensive confidence score. By dynamically evaluating this positioning reliability score and setting an adaptive threshold, when the score continuously falls below the threshold, it is determined that the robot has encountered a positioning failure or "hijacking" problem, and a hierarchical relocation strategy is activated.

[0050] S4. Multi-sensor fusion localization and accuracy improvement: Based on the radar scanning matching algorithm, the robot's current pose is initially calculated by matching real-time laser point clouds with a pre-built grid map. At the same time, combined with the visual auxiliary information provided by the bag-of-words model established in S2, the initial pose is verified and optimized, thereby improving the accuracy and robustness of in-situ localization.

[0051] S5. Hierarchical Global Search and Tightly Coupled Relocalization: After relocalization is triggered due to low confidence, a hierarchical global search mechanism is initiated. First, when the confidence level falls below the threshold for the first time, pure radar scan matching relocalization is initiated. By iteratively searching near the initial pose, matching real-time laser scanning and preprocessed map features (such as gradient mask maps) is performed to find the optimal pose. If the confidence level is still below the threshold, tightly coupled radar and vision relocalization is initiated: first, DBoW3 is used to quickly search the current composite image to obtain candidate image groups and corresponding poses; then, ORB feature matching is performed between the candidate images and the current frame, and the most similar image is found by combining FLANN and RANSAC algorithms. Its pose is then used as a high-precision initial value and injected into the radar scan matching algorithm. Finally, the robot's current optimal pose is determined through multi-sensor fusion, and relocalization is successfully completed.

[0052] First, simultaneous localization and mapping (SMR) based on laser technology is performed. After the 2D grid map is completed, the mobile robot is controlled to move 5cm or rotate 5° to acquire a composite image of the current frame image and the current frame radar point cloud image, as well as the pose information at the current moment, forming a correspondence between the composite image and the current pose. For each acquired image, the ORB algorithm is used to extract key points and feature descriptors. During the extraction process, multi-threading is used to extract image features to improve processing speed. A vocabulary is constructed based on all extracted feature descriptors. The image descriptors are matched with words in the vocabulary to generate the visual word distribution (BoW representation) of the image, which is used by the subsequent hierarchical relocalization module. The flowchart is as follows. Figure 2 As shown:

[0053] Feature extraction is the foundation of the BoW model, used to extract local features from an image and generate descriptors.

[0054] The ORB descriptor is a binary feature, represented as an N×D matrix:

[0055]

[0056] Where: N is the number of feature points, and D is the descriptor dimension.

[0057] The ORB descriptor is generated using image gradients and BRIEF:

[0058]

[0059] It refers to the direction of the key point, used for rotational invariance compensation.

[0060] Vocabulary Construction: The vocabulary is constructed by clustering the descriptors of all images into visual words using k-means clustering. Specifically, the set of descriptors extracted from all images is as follows:

[0061]

[0062] Where M is the total number of descriptors in all images, coming from multiple images.

[0063] The descriptor set D is divided into k clusters using k-means clustering:

[0064]

[0065] The center point of each cluster is a visual word:

[0066]

[0067] A vocabulary is a collection of cluster centers:

[0068]

[0069] Image word (database) generation involves matching image descriptors with words in a vocabulary to generate a visual word distribution (BoW representation) for the image. Then, the frequency of each word is counted to create a histogram, such as... Figure 3 As shown.

[0070] First, descriptors for each image Find its nearest visual word:

[0071]

[0072] in, It is a descriptor The word number.

[0073] After that Count the occurrences of each word in the image and generate a histogram.

[0074] To make the histogram independent of the number of descriptors in the image, n is normalized. The formula for the normalized histogram is:

[0075]

[0076] These are the normalized histogram values. It is the sum of the number of times all words appear.

[0077] Normalization makes In other words, the normalized histogram is a probability distribution. The advantage of this approach is that histogram features can be directly compared between different images, regardless of how the number of descriptors in the image changes.

[0078] Secondly, a multi-sensor fusion robot real-time relocation detection and triggering module was implemented. This module includes three sub-modules: a radar scoring module, an IMU scoring module, and an odom scoring module. The relocation detection and triggering module calculates and outputs a fused average score by weighted IMU score, weighted odometry score, and weighted laser score. When the fused average score is lower than a set threshold, relocation is triggered. The specific flowchart of the relocation detection and triggering module's score calculation is shown below. Figure 4 :

[0079] The radar scoring module calculates the distance from a laser-hit grid to its nearest obstacle grid. This quantifies the accuracy of each frame of laser data. After receiving the grid map, the system calculates the Euclidean distance between the laser-hit grid and its nearest obstacle grid, thus accurately describing the... Because the radar has an error of 3cm to 5cm at 10m when we are mapping, in order to increase the fault tolerance, when the laser hits a grid, the Euclidean distance from the nearest obstacle grid is less than or equal to... If the laser beam hits an obstacle at a given point, it is considered to have struck the obstacle. To address the impact of dynamic obstacles and human interference on laser detection accuracy in real-world scenarios, this system optimizes the obstacle detection process by integrating local cost map construction and resolution adaptive adjustment mechanisms (e.g., ...). Figure 5 (As shown).

[0080] Because the resolution of the map constructed in this embodiment is 0.05, and after a large number of tests, the above distance is considered to be the optimal threshold distance.

[0081] The radar model selected in this embodiment is the LS01B laser radar from LeiShen Intelligent Technology. This laser radar can achieve 360° two-dimensional planar scanning, generating planar point cloud information of the scanned space, which can be used for robot grid map creation and robot autonomous localization. The laser radar scan image is shown below. Figure 6 :

[0082] from Figure 6 As can be seen, the radar scan starts clockwise, scans the surrounding environment 360° and then returns to the origin. However, the robot follows the right-hand rule, so the radar 0° direction corresponds to the positive x-axis, 90° corresponds to the negative half of the y-axis, and 270° corresponds to the positive half of the y-axis.

[0083] Based on the robot's pose information, the radar point cloud is transformed from the radar coordinate system to the world coordinate system. The coordinates of the points in the lidar coordinate system are shown in the following formula:

[0084]

[0085]

[0086] in , These are the coordinates of points in the point cloud within the lidar coordinate system. It is the distance to the current point measured by the lidar. It is the angle of the current point.

[0087] The calculation is as follows:

[0088]

[0089] It is the first laser radar scan Angle of a point It is the starting angle of the scan. It is the angle increment between each point. This is the total number of lidar points. It is the index of the current point, traversed from high to low.

[0090] The transformation formula from the lidar coordinate system to the robot coordinate system is:

[0091]

[0092]

[0093] , These are the coordinates of the point in the robot's coordinate system. , These are the coordinates of the point in the lidar coordinate system. It represents the offset of the LiDAR relative to the robot's coordinate system. Since it determines which grid cell of the 2D map the LiDAR point cloud is in, it is zero on the z-axis.

[0094] The conversion from the robot coordinate system to the global map coordinate system is as follows:

[0095]

[0096] in , These are the coordinates of a point in the global map coordinate system. , It is the robot's position in the global map coordinate system. It is the robot's orientation angle on the global map.

[0097] Because the radar's scanning range is 360°, calculating and analyzing the entire range would result in a significant waste of resources. Therefore, during scanning, the radar point cloud was cropped, scanning only 0° to 90° and 270° to 360°, scanning only half. When calculating the position of the point cloud in the raster map, we used a method of calculating the position of one point every four points and then analyzing it to reduce resource consumption.

[0098] The formula for calculating the index of radar point clouds in a raster map is:

[0099]

[0100]

[0101]

[0102] The location of the map origin in the global coordinate system. The resolution of the map, i.e., the side length of each grid cell.

[0103] The grid width of the map. : The column and row numbers of a point in a raster map.

[0104] The raster index corresponding to the point. (In calculation) Previously, it was necessary to determine whether the point was within the map's boundaries: , If the index is outside the range, it is an invalid index.

[0105] To determine the distance between a point in the radar point cloud and its nearest obstacle, it is necessary to traverse the grid neighborhood surrounding the radar point cloud. Since real-world scenarios involve dynamic obstacles, such as people and temporary obstacles placed randomly, relying solely on the global map would introduce a significant risk of misjudgment during relocation. Therefore, this embodiment introduces both a local cost map and a global map. First, it checks if the radar point is in the local cost map. If it is in both the local and global maps, a score is awarded. Similarly, if the radar point is not in the local map but hits an obstacle, a score is also awarded. This experiment uses an eight-neighborhood check, with the step size determined by the resolution. If the neighborhood index is valid and represents an obstacle, the distance between the current point and the obstacle point is calculated.

[0106]

[0107] When d is less than the threshold, the click is considered to have hit an obstacle. If the click hits an obstacle, the score is 100 points. To more clearly see the score and determine whether the robot has been hijacked, the score is normalized. The calculation formula is as follows:

[0108]

[0109] The formula for calculating the IMU health score (heading consistency) is as follows:

[0110]

[0111] The formula for calculating the odometer health score is as follows:

[0112]

[0113] The real-time positioning confidence module (repositioning detection and triggering module) first aligns the odometry and IMU data streams through the time synchronization module. In the combinedSensorCallback callback, it uses the TensorFlow library to parse the robot's pose and Euler angles. At the same time, it calculates the body's linear velocity and angular velocity using the pose difference method. When any velocity exceeds 0.05 m / s, it is marked as a motion state. After downsampling, the LiDAR point cloud is equipped with offset compensation and global coordinate transformation. It adopts a two-level obstacle detection strategy: first, it performs obstacle matching with a 0.1m expansion radius on the local cost map. If no match is found, it queries the global map. Finally, it calculates the obstacle hit rate of the effective points as the basis for LiDAR scoring. In the multi-sensor fusion stage, IMU health is evaluated using a Gaussian decay function (20-degree tolerance threshold) based on the difference between its heading angle and that of the odometry. Odometry health is assigned full marks in static states, while in moving states, an exponential decay function (0.3m tolerance radius) based on pose offset is used, with weights dynamically allocated according to the motion state: 60% for laser, 30% for IMU, and 10% for odometry in moving states; and 70% for laser, 10% for IMU, and 20% for odometry in static states. A 30% penalty is applied to the total score when an IMU-odometry heading conflict exceeding 1 radian is detected. The evaluation results are temporally filtered using an 8-frame moving average window. A continuous decrease in the matching score indicates a positioning anomaly, triggering relocalization. The calculation flowchart is shown below. Figure 7 .

[0114] The dynamic weight allocation is shown in Table 1:

[0115] Table 1

[0116]

[0117] Conflict scenarios:

[0118] The odometer is malfunctioning, but the IMU is functioning normally (the robot is actually rotating, but the odometer is not detecting it).

[0119] The IMU is drifting, but the odometer is working correctly (the IMU is providing an incorrect heading).

[0120] Sensor hardware failure

[0121] The calculation formula is as follows (fusion score includes anomaly penalty):

[0122]

[0123] To prevent the robot from mistakenly believing it has been kidnapped due to sudden changes, a sliding window is proposed to output the average score, that is, to calculate the average of the most recent N scores to reduce the above situation. Here, average_score is the average of the most recent N scores, score_history is the queue storing the most recent scores, and N is the size of the queue (maximum is max_history_size). In this embodiment, N is set to 8, and users can increase or decrease it more or less according to their own chip power consumption.

[0124] If the average score is below the threshold for M consecutive times, it is considered that the robot's position has shifted and needs to be repositioned to correct the robot's position.

[0125] To address the relocalization requirements in complex dynamic environments, this system constructs a hierarchical localization framework that integrates visual and laser features (see flowchart for details). Figure 8 Its core processing flow is as follows:

[0126] Based on the DBoW3 visual bag-of-words model, the RGB-Lidar fused image of the current frame (synthesized from the RGB frame of the depth camera and the laser point cloud projection) is quickly matched with a pre-acquired reference image for similarity. By constructing an inverted index structure of the visual dictionary, the top 5 candidate image groups with the highest similarity are selected, effectively reducing the computational complexity of subsequent steps. Fine-grained feature alignment is performed within the candidate image set, and ORB feature descriptors of the current frame and candidate images are extracted. Feature correspondences are established based on the FLANN (Fast Approximate Nearest Neighbor) algorithm, and mismatches are eliminated using the RANSAC (Random Sample Consensus) algorithm. The matching confidence is evaluated using the maximum number of inliers criterion. The optimal reference frame is selected, and its associated initial pose estimate is obtained. The initial pose is injected as prior information into the radar scan matching of the variant ICP for fine matching, and a high-precision pose estimate is output.

[0127] This layered architecture achieves a balance between positioning efficiency and accuracy: the front-end fast retrieval based on the bag-of-words model ensures real-time requirements, while the back-end radar scan matching based on a variant of ICP can control the positioning error within ±2cm / ±1°.

[0128] The specific steps are as follows: First, a BoW (Bag of Words) coarse screening is performed, using a bag-of-words model to quickly retrieve images from the query image and the database. This initially filters out potentially similar images, reducing the complexity of subsequent calculations. Then, feature points and descriptors are extracted from the candidate images, and descriptor matching is performed with the query image. Next, FLANN and RANSAC are used for validation, verifying matching points based on geometric relationships (homography matrix), eliminating false matches, and selecting the candidate image with the most inliers as the most similar image to the current image. Figure 9 , Figure 10 .

[0129] BoW coarse screening quantizes the image descriptor ORB into discrete visual words, constructs a bag-of-words model, encodes images in the image database, and calculates the similarity between the query image and the database images. Based on the BoW matching scoring formula, N relatively similar images are first selected according to their scores. The scoring formula is as follows:

[0130]

[0131] in Is the first in the query image The frequency of each word It is the first candidate image The frequency of a word, N: the size of the visual vocabulary. If the score is greater than a threshold, it is added to the candidate image for further refinement.

[0132] The fine screening first uses FLANN (Fast Library for Approximate Nearest Neighbors) matching. The formula for FLANN to search for nearest neighbors based on the LSH method is as follows:

[0133]

[0134] Then, Lowe's ratio test is used to compare the distances between the nearest and second nearest neighbors. If the ratio is less than a set threshold, the matching point is retained, as shown in the following formula:

[0135]

[0136] in They are the nearest neighbors. This is a close neighbor.

[0137] Next, RANSAC is used to verify the matching points. The process involves converting the matching points into two-dimensional coordinate point sets points1 and points2, and then using the RANSAC method to calculate the homography matrix.

[0138] Randomly select 4 pairs of points and estimate the homography matrix H. The homography matrix H describes the transformation relationship between the two images.

[0139] Predict matching points based on H, calculate the error, eliminate outliers based on the error, repeat the iteration, and return the verified interior point matches.

[0140] Record the candidate image ID with the maximum number of inliers as the best match, and retrieve the pose of that point from the pose storage text of that image as the initial pose for the radar scan matching algorithm for fine matching.

[0141] This embodiment proposes a variant of the ICP radar scan matching relocation algorithm. The core process of radar scan matching includes map processing, pose initialization, laser data processing, iterative optimization matching, convergence judgment, and result publication. The system first receives and processes map data, cropping out the effective region and creating a gradient mask. Upon receiving a relocation signal from the relocation detection and triggering module, it determines whether it is the first or second trigger. If it is the first trigger, it obtains the pose provided by / amcl_pose; if it is the second trigger, it obtains the pose provided by / Dbow_pose. Then, it initiates the relocation process: converting the laser radar data into a point cloud in the map coordinate system, iteratively optimizing the pose (translation and rotation) to maximize the matching score between the laser point cloud and the map. When the pose change converges (10 consecutive changes less than a threshold), the optimized pose is published to the / initialpose topic, and the TF transform is updated. The entire process achieves optimization from a coarse initial pose to a precise pose.

[0142] The overall process of radar scanning and matching is as follows: Figure 11 .

[0143] First, the map is traversed to find the boundaries (xMin / xMax / yMin / yMax) of all obstacles (pixel value = 100). This reduces the computational load in subsequent processing, focuses on the effective area, and extends the boundary center point by 50 pixels to crop out the effective area (map_cropped). ROI information (map_roi_info) is recorded to prevent the ROI from exceeding the map's boundaries. Then, a 101×101 circular gradient mask is generated for each obstacle point (the further away from the center, the smaller the pixel value). The mask is overlaid on the cropped map to generate map_temp (used for subsequent matching score calculation). This makes the matching process more focused on obstacle edges, improving the robustness of laser point cloud matching. See the detailed process below. Figure 12 .

[0144] Radar scanning matching first converts polar coordinates (range, angle) to map pixel coordinates to unify the coordinate system and facilitate direct matching with the map. Then, candidate poses are generated using a three-set point method: the current pose point set (transform_points), the point set rotated 1 degree clockwise (clockwise_points), and the point set rotated 1 degree counterclockwise (counter_points), with five translation offsets: {0,0}, {±1,0}, and {0,±1} (in pixels). Next, matching scores are calculated by projecting the point sets onto map_temp, accumulating the pixel values ​​of the covered area, and using a gradient mask to make the matching more sensitive to edge alignment. Convergence is then determined by maintaining a historical queue of 10 poses; if the continuous changes are all less than a threshold (5 pixels, 5 degrees), convergence is achieved. This avoids getting trapped in local optima and ensures stability. If convergence is achieved, the corresponding pose is output as the robot's optimal pose. See the detailed process below. Figure 13 .

[0145] This invention experimentally validates the method proposed in this embodiment under real-world conditions. First, for similar real-world scenarios, this embodiment uses the proposed laser environment perception and visual environment perception methods to model the environment, ultimately generating a grid map with visual information. Next, this embodiment conducts experiments on location reliability calculation under different conditions and performs a separate kidnapping experiment to verify the sensitivity, effectiveness, robustness, and feasibility of the proposed location reliability calculation method. Following this, to verify the effectiveness and feasibility of the proposed relocalization method based on multi-sensor data, this embodiment kidnaps the robot to several points in the real-world environment and uses the proposed method for relocalization. The results show that the proposed method can accurately identify the visual label most similar to the current synthesized image and obtain accurate global positioning information for the robot based on this pose. Finally, this embodiment conducts comparative experiments on different relocalization methods for the same scenario. The experimental results demonstrate the superiority of the proposed method compared to other methods and prove its effectiveness in service scenarios.

[0146] The core of this invention lies in introducing a real-time positioning confidence mechanism to dynamically evaluate the reliability of the positioning system and trigger a repositioning process when the confidence level falls below a set threshold. Furthermore, it utilizes a camera and radar for collaborative positioning optimization, thereby effectively improving the accuracy and efficiency of repositioning and avoiding the impact of incorrect poses on navigation tasks. Experimental results show that this method exhibits excellent robustness and adaptability in typical indoor service scenarios. Especially in the "kidnapping" test, the robot can quickly detect pose abnormalities and initiate the repositioning mechanism after being moved, allowing the system to quickly return to normal operation. Moreover, in disturbance scenarios such as crossing slopes and obstacles, the system can quickly converge and maintain stable positioning after a brief drop in score, demonstrating the effectiveness of this method in dealing with complex environmental interference. Compared to traditional particle filter-based AMCL methods, positioning schemes relying solely on visual tags, or scanning and matching positioning schemes relying solely on LiDAR, the method proposed in this invention shows significant advantages in repositioning success rate and response speed.

[0147] This invention enables real-time monitoring of the positioning status by dynamically evaluating the confidence level of sensor data. When pose anomalies are detected, a repositioning mechanism is rapidly triggered, ensuring the robot system can quickly resume normal operation after encountering sudden interference. Furthermore, by fusing camera and radar data and optimizing the collaborative positioning strategy, the limitations of a single sensor in complex scenarios are overcome, effectively solving the "hijacking" problem and other sudden positioning failure scenarios.

[0148] The preferred embodiments of the present invention disclosed above are merely illustrative of the invention. These preferred embodiments do not exhaustively describe all details, nor do they limit the invention to specific implementations. Clearly, many modifications and variations can be made based on the content of this specification. This specification selects and specifically describes these embodiments to better explain the principles and practical applications of the invention, thereby enabling those skilled in the art to better understand and utilize the invention. The invention is limited only by the claims and their full scope and equivalents.

Claims

1. A multi-sensor fusion repositioning method based on real-time positioning reliability, characterized in that, The specific steps are as follows: S1, first, laser-based simultaneous localization and mapping is carried out, after completion of the two-dimensional grid map, the mobile robot is controlled to synchronously collect RGB-D image and laser point cloud data, a composite image of fused visual-laser features is generated through registration, the captured images are trained into words through bag-of-words search training method, and a mapping relationship data set of <image feature vector, pose coordinate> is formed; S2, whether the robot is kidnapped is judged based on real-time positioning confidence; S3, when the current threshold is lower than the set threshold for the first time, a radar scanning matching method is started for repositioning; S4, if the current threshold is still lower than the set threshold, radar plus vision is used for repositioning.

2. The multi-sensor fusion repositioning method of claim 1, wherein, In step S1, the correspondence between the composite image of fused visual-laser features and the current frame pose is triggered by driving the mobile robot by the motion control module at an interval of 5cm displacement or 5° rotation to trigger data collection, synchronously acquiring depth camera RGB-D image and laser radar point cloud data, and recording the pose information of the current positioning.

3. The multi-sensor fusion repositioning method of claim 2, wherein, In step S1, when constructing the visual positioning database based on the bag-of-words model, an offline training strategy is adopted to encode the continuously collected image sequence into a visual dictionary, and a pose index file strictly spatiotemporally aligned with the image frame is generated, forming a mapping relationship data set of <image feature vector, pose coordinate>.

4. The multi-sensor fusion repositioning method of claim 1, wherein, In step S2, the specific operation of calculating real-time positioning confidence is as follows: An IMU, odometer and radar multi-sensor fusion based real-time confidence evaluation model is designed; whether repositioning is needed is judged by the real-time confidence score of multi-sensor fusion, if the confidence score is continuously lower than the adaptive threshold, it is determined that the robot encounters positioning inaccuracy problem, and the hierarchical repositioning module is activated.

5. The multi-sensor fusion repositioning method of claim 1, wherein, In step S3, when the current threshold is lower than the set threshold for the first time, a radar scanning matching method is started for repositioning, and the specific steps are as follows: The core process of radar scanning matching includes map processing, pose initialization, laser data processing, iterative optimization matching, convergence judgment and result publishing; the system first receives and processes map data, crops the effective area and creates a gradient mask; after receiving the repositioning signal from the repositioning detection and trigger module, it is judged whether it is the first trigger or the second trigger, if it is the first trigger, the pose provided by / amcl_pose is obtained, if it is the second trigger, the pose provided by / Dbow_pose is obtained, and then the repositioning process is started.

6. The multi-sensor fusion repositioning method of claim 5, wherein, The repositioning process: the laser radar data is converted into point cloud in the map coordinate system, and the pose is iteratively optimized to maximize the matching score of the laser point cloud and the map; when the pose change converges, the optimized pose is published to the / initialpose topic, and the TF transformation is updated.

7. The multi-sensor fusion repositioning method of claim 1, wherein, In step S4, if the current threshold is still lower than the set threshold, radar plus vision is used for repositioning, and the specific steps are as follows: Based on the DBoW3 visual bag-of-words model, the RGB-Lidar fusion image of the current frame is quickly matched with the reference image collected in advance; by constructing the inverted index structure of the visual dictionary, the top 5 candidate image groups are screened out, the subsequent calculation complexity is effectively reduced, the fine feature alignment is implemented within the candidate image set, the ORB feature descriptors of the current frame and the candidate images are extracted, the feature correspondence is established based on the FLANN algorithm, the RANSAC algorithm is used for false matching elimination, the matching confidence is evaluated by the maximum number of inliers, the optimal reference frame is selected and the initial pose estimation associated therewith is obtained, the candidate image ID with the maximum number of inliers is taken as the best match, and the pose of the image in the pose saving text is taken as the initial pose of the radar scan matching algorithm for fine matching.

8. The multi-sensor fusion repositioning method of claim 7, wherein, The front end of the layered architecture ensures the real-time requirement based on the fast retrieval of the bag-of-words model, and the optimization process based on the radar scan matching in the back end controls the positioning error within the range of ±2cm / ±1°.

Images