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

Abstract

The invention discloses a laser navigation position verification and repositioning method, a robot and a storage medium, and the method comprises the steps: obtaining current pose information, and searching first data closest to the current pose information from KDTree data; if the pose change between the current pose information and the first data is smaller than or equal to a pose threshold value, performing navigation position verification according to the first data and the current image data; if the pose change between the current pose information and the first data is greater than a pose threshold value or the navigation position verification fails, calculating a VLAD descriptor according to the current image data; searching a plurality of pieces of second data matched with the VLAD descriptor from the HNSW data; and performing navigation position repositioning according to the current image data and each piece of second data. According to the method, the error problem of homologous data verification positioning is avoided, and the accuracy of laser navigation positioning is greatly improved.

Description

Technical Field

[0001] The present invention relates to robot navigation and positioning technology, and in particular to a laser navigation position verification and repositioning method based on visual features, a robot and a storage medium. Background Art

[0002] In the field of robotics applications, the confirmation and repositioning of the positioning results of the laser navigation and positioning algorithm are key links to ensure the precise operation of the robot and improve its environmental adaptability and reliability. Robot repositioning generally refers to the robot estimating its position and posture in a known global map by relying solely on its own sensors without prior information. Repositioning occurs during the autonomous navigation process after the completion of synchronous positioning and mapping, and is an important prerequisite for autonomous navigation. The robot needs to be repositioned when the initial posture is unknown or "kidnapping" occurs. Among them, the initial posture of the robot needs to be estimated when the robot is initially powered on or after it is forced to restart under emergency conditions; robot "kidnapping" refers to the sudden change in the robot's posture during navigation due to some external factors (such as human removal, external collision, etc.), which causes the original positioning algorithm that relies on continuous changes in posture to fail.

[0003] Laser relocalization is a common technology used in robot navigation and positioning. It matches laser radar scan data with known map data to identify feature points (corners, edges, etc.) and adjust the robot's posture based on the deviation of the feature points. First, this relocalization method relies on homologous data. Using homologous data to verify positioning errors can easily result in false negatives due to problems with the data source or data characteristics. Second, laser radars that support non-repetitive scanning are more likely to cover the entire environment. Methods that identify feature points can have greater differences when using methods such as multi-line laser radars. Finally, when passing through long, narrow channels, laser data degrades. This is because laser data at different locations have a high degree of similarity. Under single conditions, even if the human eye views the laser radar data, it is difficult to determine the robot's position, resulting in an inability to confirm positioning.

[0004] Using Scan Context data for loop detection in laser navigation mainly includes the following steps: computing a ScanContext from a 3D point cloud; generating a RingKey index vector for the ScanContext; searching the KDTree for a RingKey that matches the current one and retrieving the corresponding ScanContext; comparing the ScanContext indexed by the KDTree with the current ScanContext to confirm whether there is a loop, and then adding it to the KDTree. Because the ScanContext feature greatly reduces point cloud information, when the environment structure is easily repeated, the matched ScanContext may produce multiple results with high matching scores; the matching principle of ScanContext is similar to that of laser positioning point cloud registration, which can easily lead to the ScanContext result being consistent with the positioning result but incorrect; using different LiDARs or LiDAR installations can lead to different occlusion situations, which has a significant impact on the ScanContext algorithm, resulting in poor versatility. Summary of the Invention

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

[0006] The present invention solves the above technical problems through the following technical solutions: a laser navigation position verification and repositioning method, comprising:

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

[0008] Obtain current posture information, and search the KDTree data for the first data closest to the current posture information;

[0009] If a posture change between the current posture information and the first data is less than or equal to a posture threshold, performing navigation position verification 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 the navigation position verification fails, a VLAD descriptor is calculated based on the current image data;

[0011] Searching for a plurality of second data matching the VLAD descriptor from the HNSW data;

[0012] The navigation position is relocated according to the current image data and each piece of second data.

[0013] The present invention does not rely on homologous data. It obtains the current correct positioning (first data or second data) and image data to determine whether the navigation position verification is successful, and uses it as a basis for successful repositioning, thereby avoiding the error problem of homologous data verification positioning; uses visual image features to perform laser navigation position verification, which greatly improves the accuracy of laser navigation positioning; uses image features to determine whether repositioning is needed, and uses VLAD descriptors for repositioning, thereby avoiding the reduction of point cloud information and the situation where the results are consistent with the positioning but wrong.

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

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

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

[0017] Step S1.2: Get current posture 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 posture change between the previous image data and the current posture information is greater than the posture threshold. If so, proceed to step S1.5; if not, 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 and VLAD descriptors of the current image data.

[0021] Furthermore, performing navigation position verification according to the first data and the current image data specifically includes:

[0022] Calculate image features of current image data;

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

[0024] Whether the navigation position verification is successful is determined based on the first matching degree.

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

[0026] Furthermore, the repositioning of the navigation position according to the current image data and each piece of second data specifically includes:

[0027] Calculate image features of current image data;

[0028] Calculating a second matching degree between the image feature of the current image data and the image feature of each piece of second data;

[0029] Selecting the best second matching degree from all second matching degrees;

[0030] Whether the navigation position relocation is successful 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 comprising 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:

[0035] The present invention does not rely on homologous data. It determines whether the navigation position verification is successful by obtaining the current correct positioning and image data, and uses it as a basis for successful repositioning, thereby avoiding the error problem of homologous data verification and positioning. The present invention uses visual image features to perform 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, thereby avoiding the reduction of point cloud information and the situation where the results are consistent with the positioning but wrong, greatly reducing the impact of using different laser radars or laser radar installation occlusion on the repositioning effect, improving the accuracy of laser navigation repositioning, and improving versatility. BRIEF DESCRIPTION OF THE DRAWINGS

[0036] In order to more clearly illustrate the technical solution of the present invention, the following briefly introduces the drawings required for use in the description of the embodiments. Obviously, the drawings described below are only one embodiment of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying any creative work.

[0037] Figure 1 This is a flow chart of the laser navigation position verification and repositioning method according to an embodiment of the present invention;

[0038] Figure 2 This is a flow chart for constructing a scene line feature data model in an embodiment of the present invention;

[0039] Figure 3 It is the KDTree data based on the position index in the embodiment of the present invention;

[0040] Figure 4 is HNSW data based on the VLAD descriptor index in an embodiment of the present invention;

[0041] Figure 5 It is a single data structure in the scene line feature data model in the embodiment of the present invention. DETAILED DESCRIPTION

[0042] The following is a clear and complete description of the technical solutions of the present invention in conjunction with the accompanying drawings of the embodiments of the present invention. Obviously, the embodiments described are only part of the embodiments of the present invention, not all of the embodiments. All other embodiments obtained by ordinary technicians in this field based on the embodiments of the present invention without making any creative efforts are within the scope of protection of the present invention.

[0043] The following specific embodiments are used to describe the technical solution of the present application in detail. The following specific embodiments can be combined with each other, and the same or similar concepts or processes may not be described in detail in some embodiments.

[0044] Example 1

[0045] In order to solve the problem of confirming and repositioning the results of the robot laser navigation positioning algorithm, the present invention provides a laser navigation position verification and repositioning method, such as Figure 1 As shown, the laser navigation position verification and repositioning method includes the following steps:

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

[0047] Before the robot can navigate autonomously, it must first construct a scene route feature data model based on the robot's current pose information and image features as it navigates the path. If the scene route feature data model has already been constructed and the scene route remains unchanged, the existing model will be used during autonomous navigation. If the model has not been constructed or the scene route has changed, a new model must be constructed. The robot's pose information includes its position and yaw angle.

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

[0049] Step S1.1: Determine whether data collection is completed; if so, construct KDTree data (K-dimension tree) based on location index and HNSW data (Hierarchical Navigable Small World) based on VLAD descriptor index based on the recorded data; if not, go to step S1.2.

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

[0051] Manually determine whether the robot has collected all the data on the path it needs to walk. If so, construct KDTree data based on location index and HNSW data based on VLAD descriptor index based on the recorded data, and obtain the scene line feature data model.

[0052] Step S1.2: Get current pose information.

[0053] The current posture information includes the current position and the current yaw angle, which can be obtained according to 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, directly execute step S1.5 without executing steps S1.3 and S1.4. For current pose information other than the first, calculate the distance difference between the previous image data and the current pose information based on the 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 the yaw angles in the previous image data and the current pose information.

[0056] Step S1.4: Determine whether the posture change between the previous image data and the current posture information is greater than the posture threshold. If so, proceed to step S1.5; if not, 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 scene route feature data model from becoming overly 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 current image data is acquired from the image sensor, and a feature extraction algorithm is used to calculate the image features of the current image data. The VLAD algorithm (Vector of Locally Aggregated Descriptors) is then used to calculate the VLAD descriptor for the current image data. The VLAD algorithm captures local information in the image data while generating a global feature description, facilitating the retrieval and recognition of large-scale images and videos.

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

[0061] The current data includes the current posture information, the image features of the current image data and the VLAD descriptor, that is, each data in the scene line feature data model includes posture information, image features and VLAD descriptor, such as Figure 5 shown.

[0062] Step S2: Obtain the current posture information, and search the KDTree data for the first data closest to the current posture information.

[0063] During autonomous navigation, the current position information can be obtained according to the laser navigation positioning algorithm. The nearest neighbor search algorithm is used to search the KDTree data for the first data that is closest to the current position information.

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

[0065] If the posture change between the current posture information and the first data is less than or equal to the posture threshold, it indicates that the current posture information matches the data in the scene line feature data model, and the navigation position verification is performed based on the matched first data and the current image data. In a specific embodiment of the present invention, performing the 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: Calculating a 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 is 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. The image features are 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 degree of matching between the image features of the current image data and the image features of the first data is calculated using Euclidean distance. The first degree of matching 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 degree of matching. If the first degree of matching is less than or equal to a matching 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 degree of matching is greater than the matching threshold, it indicates that the current navigation position has deviated, the navigation position verification has failed, and repositioning is required.

[0072] If the posture change between the current posture information and the first data is greater than the posture threshold, it indicates that the current posture information is not covered by the scene line feature data model, the positioning is wrong, and repositioning is required.

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

[0074] Step S4: Search the HNSW data for a plurality of second data matching the VLAD descriptor.

[0075] The VLAD descriptor is a multi-dimensional fixed-size vector. Based on the VLAD descriptor, a vector search is performed in the HNSW data to search for multiple pieces of second data that match the VLAD descriptor of the current image data.

[0076] Step S5: relocating the navigation position according to the current image data and each piece of second data.

[0077] In a specific embodiment of the present invention, relocating the navigation position according to the current image data and each piece of second data specifically includes:

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

[0079] Step S5.2: Calculating a second matching degree between the image feature of the current image data and the image feature of each piece of second data;

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

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

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

[0083] The VLAD descriptor is used only for fast searches within the HNSW data. Compared to image features, using the VLAD descriptor for fast searches significantly reduces the amount of search data. Image features are used for navigation position verification and repositioning, significantly improving the accuracy of laser navigation positions.

[0084] Example 2

[0085] An embodiment of the present invention also provides a robot, which includes: a memory, a processor, and a computer program / instructions stored in the memory, and the processor executes the computer program / instructions to implement the laser navigation position verification and repositioning method in embodiment 1 of the present 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 a read-only memory (ROM) or programs and / or data loaded from a storage portion into a random access memory (RAM). The processor can be a multi-core processor or can include multiple processors. In some embodiments, the processor can include a general-purpose main processor and one or more special coprocessors, such as a central processing unit, a graphics processing unit (GPU), a neural network processor (NPU), a 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 connected to each other via a bus. An input / output (I / O) interface is also connected to the bus.

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

[0088] Although not shown, an embodiment of the present invention further 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 in the first embodiment of the present invention.

[0089] The above disclosure is only a specific embodiment of the present invention, but the scope of protection of the present invention is not limited thereto. Any technician familiar with this technical field can easily think of changes or modifications within the technical scope disclosed in the present invention, and they should all be covered by the scope of protection of the present invention.

Claims

1. A laser navigation position verification and repositioning method, characterized in that: The method comprises: Acquire or construct a scene line feature data model; wherein the scene line feature data model includes KDTree data based on location index and HNSW data based on VLAD descriptor index; Obtain current posture information, and search the KDTree data for the first data closest to the current posture information; If a posture change between the current posture information and the first data is less than or equal to a posture threshold, performing navigation position verification 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 the navigation position verification fails, a VLAD descriptor is calculated based on the current image data; Searching for a plurality of second data matching the VLAD descriptor from the HNSW data; The navigation position is relocated according to the current image data and each piece of second data.

2. The laser navigation position calibration and repositioning method according to claim 1, characterized in that: If the scene line feature data model has been constructed and the scene line remains unchanged, the constructed scene line feature data model is obtained; if the scene line feature data model has not been constructed or the scene line has changed, the scene line feature data model is constructed.

3. The laser navigation position verification and repositioning method according to claim 1, characterized in that: The specific steps of constructing the scene line feature data model include: Step S1.1: Determine whether data collection is completed; if so, construct KDTree data based on location index and HNSW data based on VLAD descriptor index based on the recorded data; if not, proceed to step S1.2; Step S1.2: Get current posture information; Step S1.3: Calculate the pose change between the previous image data and the current pose information; Step S1.4: Determine whether the posture change between the previous image data and the current posture information is greater than the posture threshold. If so, proceed to step S1.5; if not, 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 and VLAD descriptors of the current image data.

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

5. The laser navigation position verification and repositioning method according to claim 4, characterized in that: A first matching degree between the image feature of the current image data and the image feature 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 repositioning of the navigation position according to the current image data and each piece of second data specifically includes: Calculate image features of current image data; Calculating a second matching degree between the image feature of the current image data and the image feature of each piece of second data; Selecting the best second matching degree from all second matching degrees; Whether the navigation position relocation is successful 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 / instruction stored in the memory, and the processor executes the computer program / instruction to implement the laser navigation position verification and repositioning method according to any one of claims 1 to 7.

9. A computer-readable storage medium having a computer program / instruction stored thereon, characterized in that: When the computer program / instruction is executed by a processor, the laser navigation position verification and repositioning method according to any one of claims 1 to 7 is implemented.

Images