Laser navigation position verification and relocation method, robot and storage medium

Abstract

The application discloses a laser navigation position checking and repositioning method, a robot and a storage medium, and the method comprises the following steps: acquiring current pose information, searching out first data most adjacent to the current pose information from KDTree data; if the pose change between the current pose information and the first data is less than or equal to a pose threshold, performing navigation position checking according to the first data and current image data; if the pose change between the current pose information and the first data is greater than the pose threshold, or the navigation position checking fails, calculating a VLAD descriptor according to the current image data; searching out multiple pieces of second data matched with the VLAD descriptor from HNSW data; and performing navigation position repositioning according to the current image data and each piece of second data. The application avoids the error problem of homologous data checking positioning, and greatly improves the accuracy of laser navigation positioning.

Description

Technical Field

[0001] This invention pertains to robot navigation and positioning technology, and particularly relates to a laser navigation position verification and repositioning method based on visual features, a robot, and a storage medium. Background Technology

[0002] In the field of robotics applications, the confirmation and relocalization of laser navigation and positioning algorithms are crucial for ensuring accurate robot operation and improving environmental adaptability and reliability. Robot relocalization generally refers to the process where, without prior information, the robot estimates its pose in a known global map solely based on its own sensors. Relocalization occurs during autonomous navigation after simultaneous localization and mapping (SMR) and is an important prerequisite for autonomous navigation. Relocalization is necessary when the initial pose is unknown or when a robot is "hijacked." Specifically, the initial pose needs to be estimated when the robot is initially powered on or after a forced restart due to unforeseen circumstances. "Hacked-up" refers to a sudden change in the robot's pose during navigation due to external factors (such as human intervention or external collisions), causing the original positioning algorithm, which relies on continuous pose changes, to fail.

[0003] Laser relocalization is a commonly used technique in robot navigation and localization. It matches data scanned by a LiDAR scanner with known map data to identify feature points (corners, edges, etc.) and adjusts the robot's pose based on deviations from these feature points. First, this relocalization method relies on data from the same source. However, using this data to verify localization accuracy is prone to false negatives due to issues with the data source or its inherent characteristics. Second, LiDAR scanners that support non-repeating scans can more easily cover the entire environment, while feature point identification methods show significant differences when using multi-line LiDAR. Finally, laser data degrades when traversing long, narrow channels because the similarity between laser data from different locations is high, making it difficult to determine the robot's position even with the naked eye, thus hindering localization confirmation.

[0004] Loop closure detection in laser navigation using Scan Context data mainly includes the following steps: Computing ScanContext from 3D point cloud data; generating a RingKey index vector for ScanContext; searching for a similar RingKey in KDTree and retrieving the corresponding ScanContext; comparing the ScanContext indexed by KDTree with the current ScanContext to confirm loop closure and adding it to KDTree. Because ScanContext features significantly reduce point cloud information, multiple ScanContexts with high matching scores may appear when the environmental structure is easily repeated; the matching principle of ScanContext is similar to that of laser positioning point cloud registration, which can easily lead to situations where the ScanContext result matches the positioning result but is incorrect; using different LiDARs or varying LiDAR installation occlusion conditions greatly affects the ScanContext algorithm, resulting in poor versatility. Summary of the Invention

[0005] The purpose of this invention is to provide a laser navigation position verification and repositioning method, robot, and storage medium to solve the problems of poor verification and repositioning accuracy and poor versatility of the Scan Context loop closure detection method.

[0006] This invention solves the above-mentioned technical problems through the following technical solution: a laser navigation position verification and repositioning method, comprising:

[0007] Acquire or construct a scene route feature data model; wherein, the scene route feature data model includes KDTree data based on location index and HNSW data based on VLAD descriptor index;

[0008] Obtain the current pose information and search for the first data that is closest to the current pose information from the KDTree data;

[0009] If the pose change between the current pose information and the first data is less than or equal to the pose threshold, then the navigation position is verified based on the first data and the current image data.

[0010] If the pose change between the current pose information and the first data is greater than the pose threshold, or if the navigation position verification fails, then the VLAD descriptor is calculated based on the current image data.

[0011] Search the HNSW data for multiple second data entries that match the VLAD descriptor;

[0012] Navigation location repositioning is performed based on the current image data and each second data point.

[0013] This invention does not rely on source data. It determines whether the navigation position verification is successful by acquiring the current correct positioning (first data or second data) and image data, and uses this as the basis for successful repositioning, thus avoiding the error problem of positioning verification based on source data. It uses visual image features for laser navigation position verification, which greatly improves the accuracy of laser navigation positioning. It uses image features to determine whether repositioning is needed and uses VLAD descriptors for repositioning, avoiding the reduction of point cloud information and the situation where the result is consistent with the positioning but incorrect.

[0014] Furthermore, if a scene route feature data model has been constructed and the scene route remains unchanged, then the constructed scene route feature data model is obtained; if a scene route feature data model has not been constructed or the scene route has changed, then a scene route feature data model is constructed.

[0015] Furthermore, the specific construction steps of the scene line feature data model include:

[0016] Step S1.1: Determine whether data collection is complete; if yes, construct KDTree data based on location index and HNSW data based on VLAD descriptor index based on the recorded data; if no, proceed to step S1.2.

[0017] Step S1.2: Obtain current pose information;

[0018] Step S1.3: Calculate the pose change between the previous image data and the current pose information;

[0019] Step S1.4: Determine whether the pose change between the previous image data and the current pose information is greater than the pose threshold. If yes, proceed to step S1.5; otherwise, proceed to step S1.1.

[0020] Step S1.5: Calculate the image features and VLAD descriptors of the current image data, and record the current data; wherein, the current data includes the current pose information, the image features of the current image data, and the VLAD descriptors.

[0021] Furthermore, the step of verifying the navigation position based on the first data and the current image data specifically includes:

[0022] Calculate the image features of the current image data;

[0023] Calculate the first matching degree between the image features of the current image data and the image features of the first data;

[0024] The success of the navigation location verification is determined based on the first matching degree.

[0025] Furthermore, the first matching degree between the image features of the current image data and the image features of the first data is calculated using Euclidean distance.

[0026] Furthermore, the step of repositioning the navigation location based on the current image data and each piece of second data specifically includes:

[0027] Calculate the image features of the current image data;

[0028] Calculate the second matching degree between the image features of the current image data and the image features of each second data point;

[0029] Select the optimal second matching degree from all the second matching degrees;

[0030] The success of navigation location relocation is determined based on the optimal second matching degree.

[0031] Furthermore, the SIFT algorithm is used to calculate the image features of the current image data.

[0032] Based on the same concept, the present invention also provides a robot, including a memory, a processor, and a computer program / instructions stored in the memory, wherein the processor executes the computer program / instructions to implement the laser navigation position verification and repositioning method as described above.

[0033] Based on the same concept, the present invention also provides a computer-readable storage medium having a computer program / instruction stored thereon, which, when executed by a processor, implements the laser navigation position verification and repositioning method as described above.

[0034] Compared with the prior art, the advantages of the present invention are as follows:

[0035] This invention does not rely on source data. It determines the success of navigation position verification by acquiring the current correct positioning and image data, and uses this as the basis for successful repositioning, thus avoiding the error problem of positioning verification based on source data. This invention uses visual image features for laser navigation position verification, which greatly improves the accuracy of laser navigation positioning. It uses image features to determine whether repositioning is needed and uses VLAD descriptors for repositioning, avoiding situations where point cloud information is reduced or the result is consistent with the positioning but incorrect. This greatly reduces the impact of using different lidars or lidar installation obstruction on the repositioning effect, improves the accuracy of laser navigation repositioning, and enhances its versatility. Attached Figure Description

[0036] To more clearly illustrate the technical solution of the present invention, the drawings used in the following description of the embodiments will be briefly introduced. Obviously, the drawings described below are only one embodiment of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0037] Figure 1 This is a flowchart of the laser navigation position verification and relocation method in an embodiment of the present invention;

[0038] Figure 2 This is a flowchart illustrating the construction process of the scene line feature data model in this embodiment of the invention;

[0039] Figure 3 This refers to KDTree data based on location indexing in this embodiment of the invention;

[0040] Figure 4 This refers to HNSW data based on the VLAD descriptor index in this embodiment of the invention;

[0041] Figure 5 This is a single data structure in the scene line feature data model in this embodiment of the invention. Detailed Implementation

[0042] The technical solutions of the present invention will be clearly and completely described below with reference to the accompanying drawings of the embodiments. 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.

[0043] The technical solutions of this application will be described in detail below with specific embodiments. The following specific embodiments can be combined with each other, and the same or similar concepts or processes may not be described again in some embodiments.

[0044] Example 1

[0045] To address the issues of confirming and relocating robot laser navigation and positioning algorithm results, this invention provides a laser navigation position verification and relocation method, such as... Figure 1 As shown, the laser navigation position verification and repositioning method includes the following steps:

[0046] Step S1: Obtain or construct a scene line feature data model.

[0047] Before the robot can navigate autonomously, a scene route feature data model needs to be constructed based on the robot's current pose information and image features during path navigation. If a scene route feature data model has already been constructed and the scene route remains unchanged, the constructed scene route feature data model is used during autonomous navigation; if a scene route feature data model has not been constructed or the scene route has changed, a new scene route feature data model needs to be constructed. The robot's pose information includes its position and yaw angle.

[0048] In a specific embodiment of the present invention, such as Figure 2 As shown, the specific steps for constructing the scene line feature data model include:

[0049] Step S1.1: Determine whether data collection is complete; if yes, construct KDTree data (K-dimension tree) based on location index and HNSW data (HierarchicalNavigable Small World) based on VLAD descriptor index according to the recorded data; if no, proceed to step S1.2.

[0050] The scene line feature data model of this invention includes two data structures: KDTree data based on location index and HNSW data based on VLAD descriptor index, such as... Figure 3 and Figure 4 As shown.

[0051] The operator determines whether the robot has collected all the data along the path it needs to walk. If so, the operator constructs KDTree data based on the location index and HNSW data based on the VLAD descriptor index based on the recorded data, thus obtaining the scene route feature data model.

[0052] Step S1.2: Obtain the current pose information.

[0053] The current pose information includes the current position and the current yaw angle, which can be obtained based on the robot's laser navigation and positioning algorithm.

[0054] Step S1.3: Calculate the pose change between the previous image data and the current pose information.

[0055] For the first frame of image data and the first current pose information, proceed directly to step S1.5; steps S1.3 and S1.4 are not required. For non-first current pose information, calculate the distance difference between the previous image data and the current pose information based on their positions in the previous image data and the current pose information, and calculate the angle difference between the previous image data and the current pose information based on their yaw angles in the previous image data and the current pose information.

[0056] Step S1.4: Determine whether the pose change between the previous image data and the current pose information is greater than the pose threshold. If yes, proceed to step S1.5; otherwise, proceed to step S1.1.

[0057] The pose threshold includes a distance threshold and an angle threshold. If the distance difference between the previous image data and the current pose information is greater than the distance threshold, and the angle difference between the previous image data and the current pose information is greater than the angle threshold, it indicates that the pose change between the previous image data and the current pose information is greater than the pose threshold. Steps S1.3 and S1.4 prevent the data in the scene line feature data model from becoming too dense. In this embodiment, the distance threshold is set to 0.5m, and the angle threshold is set to 30°.

[0058] Step S1.5: Calculate the image features and VLAD descriptors of the current image data, and record the current data.

[0059] The system acquires current image data from an image sensor, calculates image features using feature extraction algorithms, and then uses the VLAD (Vector of Locally Aggregated Descriptors) algorithm to calculate the VLAD descriptor. The VLAD algorithm can capture local information in image data while simultaneously generating a global feature description, facilitating the retrieval and recognition of large-scale images and videos.

[0060] In a specific embodiment of this invention, the SIFT (Scale Invariant Feature Transform) algorithm is used to calculate the image features of the current image data. Image features calculated using the SIFT algorithm (SIFT features for short) are invariant to rotation, scaling, brightness changes, etc., and are therefore very stable local features. The VLAD algorithm is then used to calculate the SIFT features to obtain the VLAD descriptor of the current image data.

[0061] The current data includes current pose information, image features of the current image data, and VLAD descriptors. That is, each piece of data in the scene line feature data model includes pose information, image features, and VLAD descriptors, such as... Figure 5 As shown.

[0062] Step S2: Obtain the current pose information and search for the first data that is closest to the current pose information from the KDTree data.

[0063] During autonomous navigation, the current pose information can be obtained based on the laser navigation and positioning algorithm. A nearest neighbor search algorithm is then used to search for the first data point in the KDTree dataset that is closest to the current pose information.

[0064] Step S3: Determine whether the pose change between the current pose information and the first data is less than or equal to the pose threshold.

[0065] If the pose change between the current pose information and the first data is less than or equal to the pose threshold, it indicates that the current pose information matches the data in the scene line feature data model. Navigation position verification is then performed based on the matched first data and the current image data. In a specific embodiment of the present invention, navigation position verification based on the first data and the current image data specifically includes:

[0066] Step S3.1: Calculate the image features of the current image data;

[0067] Step S3.2: Calculate the first matching degree between the image features of the current image data and the image features of the first data;

[0068] Step S3.3: Determine whether the navigation position verification was successful based on the first matching degree.

[0069] In a specific embodiment of the present invention, in step S3.1, the SIFT algorithm is used to calculate the image features of the current image data, and these image features are called SIFT features. In another specific embodiment of the present invention, other algorithms can also be used to calculate the image features of the current image data. Image features are divided into four categories: corner points, gradient feature points, edge features, and texture features. The image feature extraction methods corresponding to these four categories are as follows:

[0070] Corner points: Harris operator, SUSAN operator, FAST operator; Gradient feature points: SIFT, SURF, GLOH, ASIFT, PSIFT operators, etc.; Edge features (line type): Canny operator, Marr operator; Texture features: Gray-level co-occurrence matrix, wavelet Gabor operator.

[0071] In a specific embodiment of the present invention, in step S3.2, a first matching degree is calculated between the image features of the current image data and the image features of the first data using Euclidean distance. The first matching degree is the Euclidean distance between the image features of the current image data and the image features of the first data; the smaller the distance, the higher the matching degree. If the first matching degree is less than or equal to the matching degree threshold, it indicates that the current navigation position has not deviated, the navigation position verification is successful, and repositioning is not required; if the first matching degree is greater than the matching degree threshold, it indicates that the current navigation position has deviated, the navigation position verification fails, and repositioning is required.

[0072] If the pose change between the current pose information and the first data is greater than the pose threshold, it indicates that the current pose information is not covered by the scene line feature data model, the localization is incorrect, and relocalization is required.

[0073] If the pose change between the current pose information and the first data exceeds the pose threshold, or if navigation position verification fails, it indicates that relocation is required, and a VLAD descriptor is calculated based on the current image data. In a specific embodiment of the present invention, the VLAD algorithm is used to calculate the SIFT features of the current image data to obtain the VLAD descriptor of the current image data.

[0074] Step S4: Search the HNSW data for multiple second data entries that match the VLAD descriptor.

[0075] A VLAD descriptor is a multidimensional fixed-size vector. Based on the VLAD descriptor, a vector search is performed in the HNSW data to find multiple second data that match the VLAD descriptor of the current image data.

[0076] Step S5: Relocate the navigation position based on the current image data and each second data point.

[0077] In a specific embodiment of the present invention, navigation position relocation is performed based on the current image data and each piece of second data, specifically including:

[0078] Step S5.1: Calculate the image features of the current image data;

[0079] Step S5.2: Calculate the second matching degree between the image features of the current image data and the image features of each second data point;

[0080] Step S5.3: Select the optimal second matching degree from all second matching degrees;

[0081] Step S5.4: Determine whether the navigation position relocation was successful based on the optimal second matching degree.

[0082] In this embodiment, Euclidean distance is used to calculate the second matching degree between the image features of the current image data and the image features of each second data point. The second matching degree is the Euclidean distance between the image features of the current image data and the image features of each second data point; the smaller the distance, the higher the matching degree. The smallest second matching degree is selected from all the second matching degrees. If the smallest second matching degree is less than or equal to the matching degree threshold, it indicates that the navigation position repositioning is successful, and the pose information matched by the smallest second matching degree is fed back to the laser navigation and positioning algorithm; if the smallest second matching degree is greater than the matching degree threshold, it indicates that the navigation position repositioning has failed.

[0083] VLAD descriptors are used solely for fast searching within HNSW data. Compared to image features, using VLAD descriptors for fast searching significantly reduces the amount of data required. Image features are used for navigation position verification and relocation, greatly improving the accuracy of laser navigation positions.

[0084] Example 2

[0085] This invention also provides a robot, which includes a memory, a processor, and a computer program / instructions stored in the memory. The processor executes the computer program / instructions to implement the laser navigation position verification and repositioning method in Embodiment 1 of this invention.

[0086] Although not shown, the robot includes a processor that can perform various appropriate operations and processes based on programs and / or data stored in read-only memory (ROM) or loaded from a storage portion into random access memory (RAM). The processor can be a multi-core processor or may contain multiple processors. In some embodiments, the processor may include a general-purpose main processor and one or more specialized coprocessors, such as a central processing unit, graphics processing unit (GPU), neural network processor (NPU), digital signal processor (DSP), etc. Various programs and data required for device operation are also stored in the RAM. The processor, ROM, and RAM are interconnected via a bus. Input / output (I / O) interfaces are also connected to the bus.

[0087] The processor and memory described above are used together to execute programs / instructions stored in the memory. When the program / instructions are executed by the computer, they can implement the methods, steps, or functions described in the above embodiments.

[0088] Although not shown, embodiments of the present invention also provide a computer-readable storage medium having a computer program / instruction stored thereon, which, when executed by a processor, implements the laser navigation position verification and repositioning method in Embodiment 1 of the present invention.

[0089] The above description only discloses specific embodiments of the present invention, but the scope of protection of the present invention is not limited thereto. Any changes or modifications that can be easily conceived by those skilled in the art within the scope of the technology disclosed in the present invention should be included within the scope of protection of the present invention.

Claims

1. A laser navigation position verification and repositioning method, characterized in that, The method includes: Acquire or construct a scene route feature data model; wherein, the scene route feature data model includes KDTree data based on location index and HNSW data based on VLAD descriptor index; Obtain the current pose information and search for the first data that is closest to the current pose information from the KDTree data; If the pose change between the current pose information and the first data is less than or equal to the pose threshold, then the navigation position is verified based on the first data and the current image data. If the pose change between the current pose information and the first data is greater than the pose threshold, or if the navigation position verification fails, then the VLAD descriptor is calculated based on the current image data. Search the HNSW data for multiple second data entries that match the VLAD descriptor; Navigation location repositioning is performed based on the current image data and each second data point.

2. The laser navigation position verification and repositioning method according to claim 1, characterized in that, If a scene route feature data model has been built and the scene route remains unchanged, then the built scene route feature data model is obtained; if a scene route feature data model has not been built or the scene route has changed, then a scene route feature data model is built.

3. The laser navigation position verification and repositioning method according to claim 1, characterized in that, The specific steps for constructing the scene line feature data model include: Step S1.1: Determine whether data collection is complete; if yes, construct KDTree data based on location index and HNSW data based on VLAD descriptor index based on the recorded data; if no, proceed to step S1.

2. Step S1.2: Obtain the current pose information; Step S1.3: Calculate the pose change between the previous image data and the current pose information; Step S1.4: Determine whether the pose change between the previous image data and the current pose information is greater than the pose threshold. If yes, proceed to step S1.5; otherwise, proceed to step S1.

1. Step S1.5: Calculate the image features and VLAD descriptors of the current image data, and record the current data; wherein, the current data includes the current pose information, the image features of the current image data, and the VLAD descriptors.

4. The laser navigation position verification and repositioning method according to claim 1, characterized in that, The navigation position verification based on the first data and the current image data specifically includes: Calculate the image features of the current image data; Calculate the first matching degree between the image features of the current image data and the image features of the first data; The success of the navigation location verification is determined based on the first matching degree.

5. The laser navigation position verification and repositioning method according to claim 4, characterized in that, The first matching degree between the image features of the current image data and the image features of the first data is calculated using Euclidean distance.

6. The laser navigation position verification and repositioning method according to any one of claims 1 to 5, characterized in that, The navigation position relocation based on the current image data and each piece of second data specifically includes: Calculate the image features of the current image data; Calculate the second matching degree between the image features of the current image data and the image features of each second data point; Select the optimal second matching degree from all the second matching degrees; The success of navigation location relocation is determined based on the optimal second matching degree.

7. The laser navigation position verification and repositioning method according to claim 6, characterized in that, The SIFT algorithm is used to calculate the image features of the current image data.

8. A robot, characterized in that, The robot includes a memory, a processor, and a computer program / instructions stored in the memory. The processor executes the computer program / instructions to implement the laser navigation position verification and repositioning method as described in any one of claims 1 to 7.

9. A computer-readable storage medium having a computer program / instructions stored thereon, characterized in that, When the computer program / instruction is executed by the processor, it implements the laser navigation position verification and relocation method as described in any one of claims 1 to 7.

Images