Navigation positioning method and system based on triangle features and two-level search tree

Abstract

The application belongs to the technical field of navigation positioning. A navigation positioning method and system based on triangle features and two-level search trees are provided. A first search tree is established by constructing triangles using prior map landmarks and generating a three-side length feature vector. A candidate check point is obtained by estimating a two-dimensional rigid body pose transformation through searching for candidate matches in the triangle features. A second search tree is then established to query the nearest neighbor landmarks of the check point, and a geometric consistency error is calculated. Finally, the global position and heading correction are determined according to the error, and the navigation positioning state is updated. The application effectively improves the accuracy and robustness of positioning through two-level search and geometric checking.

Description

Technical Field

[0001] This invention relates to the field of navigation and positioning technology, specifically to a navigation and positioning method and system based on triangular features and a two-level retrieval tree. Background Technology

[0002] With the rapid development of autonomous mobile robots, drones, and autonomous driving technologies, accurate and reliable navigation and positioning have become the core foundation for ensuring the safe operation of these systems. In complex environments where satellite signals are limited or denied, visual or lidar positioning technologies based on environmental features have gradually become a research hotspot. These technologies typically rely on pre-built environmental maps, achieving pose estimation by matching real-time perceived data with the prior map. Among these, methods based on geometric features have attracted widespread attention due to their strong robustness to changes in ambient lighting and texture. Triangles, as stable geometric primitives, possess excellent rotation and translation invariance in their feature vectors formed by the combination of their three side lengths, and are widely used in landmark matching and location recognition tasks, providing crucial technical support for passive navigation.

[0003] However, existing triangular feature-based localization methods often face the challenge of balancing retrieval efficiency and matching reliability when dealing with large-scale landmark sets. Traditional methods struggle to combine observed triangles with massive amounts of data. Figure 3 When matching triangles, the lack of an efficient indexing mechanism leads to lengthy candidate match searches. Furthermore, relying solely on single triangle feature matching is prone to false matches, making it difficult to guarantee the accuracy of global positioning results. Although some methods introduce verification steps, they fail to effectively combine a two-level retrieval structure to balance computational load and verification accuracy. This makes it difficult for the system to quickly obtain reliable global position and heading corrections when dynamically updating navigation status, limiting its application in scenarios with high real-time requirements. Summary of the Invention

[0004] To address the shortcomings of existing technologies, this invention provides a navigation and positioning method and system based on triangular features and a two-level retrieval tree. Through the coordinated operation of the two-level retrieval tree, the robustness of global positioning is improved while ensuring a certain level of real-time performance. It also effectively balances the efficiency and accuracy requirements of feature matching under large-scale landmark sets, making it suitable for complex environment positioning scenarios where navigation continuity is required.

[0005] To achieve the above objectives, the present invention adopts the following technical solution: In a first aspect, the present invention provides a navigation and positioning method based on triangular features and a two-level retrieval tree.

[0006] A navigation and localization method based on triangle features and a two-level retrieval tree includes the following process: Obtain the set of prior standard map landmarks and the current locally observed landmark set, and construct a map that satisfies geometric constraints based on the prior standard map landmark set. Figure 3 The triangle is then used to generate feature vectors of the lengths of the three sides in ascending order, and a first-level search tree is built based on these feature vectors. Based on the current set of local observation landmarks, generate feature vectors for the lengths of the three sides of the observation triangle, and then search for candidate locations using these feature vectors in the first-level search tree. Figure 3 Angular; According to candidate locations Figure 3 The correspondence between the vertices of the triangle and the observed triangle is estimated by the two-dimensional rigid body pose transformation. The two-dimensional rigid body pose transformation is used to transform the vertices of the observed triangle to the standard map coordinate system to obtain candidate verification points. A secondary search tree is established based on the prior standard map landmark set. The nearest neighbor landmark of the candidate verification point is queried in the secondary search tree to calculate the geometric consistency error. The global position correction and heading correction are determined based on the geometric consistency error, and the current navigation and positioning status is updated using the global position correction and heading correction.

[0007] In one implementation of the first aspect of the invention, a ground plane satisfying geometric constraints is constructed. Figure 3 Angular shapes, including: Enumerate the combinations of three points from the prior standard map landmark set to obtain the set of candidate triangle side lengths; Arrange the set of lengths of the three sides of the candidate triangle in ascending order to obtain the shortest side, the second shortest side, and the longest side; Candidate triangles are removed when the difference between any two side lengths is less than the side length difference threshold, the minimum interior angle is less than the angle threshold, or the longest side is greater than the observation association distance threshold. The retained candidate triangles are used as the ground Figure 3 Angular shape.

[0008] In one implementation of the first aspect of the present invention, after generating the feature vectors of the three side lengths arranged in ascending order, the method further includes: Definitely Figure 3 The unified vertex order of the triangle is as follows: the first vertex is the common corner point of the shortest side and the second shortest side, the second vertex is the common corner point of the second shortest side and the longest side, and the third vertex is the common corner point of the shortest side and the longest side. Write the vertex identifier, three-side length feature vector and standard map coordinate index corresponding to the unified vertex sorting into the compact coded data unit; Compact coded data units are stored as node association information in the first-level search tree.

[0009] In one implementation of the first aspect of the present invention, candidate locations for the three-side length feature vectors are retrieved in a first-level search tree. Figure 3Angular shapes, including: Calculate the absolute value of the difference between the three-side length feature vector and the corresponding side length of the three-side length feature vector of each node in the first-level search tree; The maximum value among the three absolute values ​​is selected as the maximum absolute deviation of the three sides; When the maximum absolute deviation of the three sides is less than or equal to the maximum single-sided deviation threshold, read the compact coded data unit associated with the corresponding node; Candidate locations are recovered based on compact coded data units. Figure 3 The vertex identifiers of the triangle, the unified vertex sorting, and the standard map coordinate information are used as candidate locations. Figure 3 Angular shape.

[0010] In one implementation of the first aspect of the invention, based on the candidate site Figure 3 The correspondence between the vertices of the triangle and the observed triangle is used to estimate the pose transformation of a two-dimensional rigid body, including: Based on the unified vertex sort recovered from the compact coded data unit, the unified vertex sort corresponding to the observed triangle is matched to establish the relationship between the vertices of the observed triangle and candidate locations. Figure 3 One-to-one correspondence between the vertices of the triangle; With the goal of minimizing the sum of squared Euclidean distances between three sets of corresponding vertices, the two-dimensional rotation matrix and two-dimensional translation vector are solved by the least squares method to obtain the initial two-dimensional rigid body pose transformation. The calculation logic for the optimization target is as follows: traverse the three sets of matching vertices, calculate the distance between each set of observed vertices and the corresponding map vertices after rotation and translation, obtain the sum of the squares of all distances, and select the rotation matrix and translation vector that minimize the summation value as the component parameters of the initial two-dimensional rigid body pose transformation. Among them, the observation vertices are the vertices of the observation triangle arranged in a uniform vertex order, and the map vertices are candidate vertices that correspond one-to-one with the observation vertices. Figure 3 Angular vertex.

[0011] In one implementation of the first aspect of the present invention, candidate verification points are obtained by transforming the vertices of the observed triangle to a standard map coordinate system using two-dimensional rigid body pose transformation, including: Retrieve all local observation landmarks contained in the current local observation landmark set; Substitute each local observation landmark into the initial two-dimensional rigid body pose transformation, and perform rotation and translation operations in sequence to obtain the corresponding candidate verification points in the standard map coordinate system. Candidate check points obtained from the calculations of all local observation landmarks are combined to form a set of candidate check points.

[0012] In one implementation of the first aspect of the present invention, a secondary search tree is established based on a priori standard map landmark set, and the nearest neighbor landmarks of candidate verification points are queried in the secondary search tree to calculate the geometric consistency error, including: In the offline processing stage, the two-dimensional global coordinates corresponding to each of the prior standard map landmarks are used as node information to construct a global two-dimensional coordinate retrieval tree as a secondary retrieval tree; During the online processing phase, each candidate verification point is traversed sequentially, and the prior standard map landmark point with the closest spatial distance to the current candidate verification point is retrieved within the secondary search tree. Calculate the Euclidean distance between the current candidate verification point and its nearest landmark, and use this distance as the geometric cost of a single point. The global geometric consistency error is obtained by summing up the values ​​of all individual point geometric costs. The calculation logic for global geometric consistency error is as follows: sum up all the single-point geometric costs generated for each local observation landmark, and use the summation result as the geometric consistency error.

[0013] In one implementation of the first aspect of the present invention, determining the global position correction and heading correction based on the geometric consistency error includes: Traverse all candidate locations output by the first-level search tree Figure 3 For each triangle, solve the corresponding initial two-dimensional rigid body pose transformation and calculate the corresponding geometric consistency error; Candidate sites that simultaneously satisfy both types of constraints are selected. Figure 3 For the triangle, the first type of constraint is that after adding a fixed minimum constant to the geometric consistency error corresponding to the current candidate, the value does not exceed the product of the single-point deviation threshold and the total number of observation points. The second type of constraint is that the number of effective punctuation correspondences formed by the current candidate matching is not less than three sets. From the selected valid candidate sites Figure 3 In the triangle, the candidate with the smallest geometric consistency error is selected as the optimal candidate; Retrieve all valid punctuation correspondences formed by the optimal candidate matching, and then use the least squares method to solve the final two-dimensional rigid body pose transformation again. Extract the translation components contained in the final two-dimensional rigid body pose transformation, and calculate the global position correction. Extract the rotational components contained in the final two-dimensional rigid body pose transformation, and calculate the heading correction.

[0014] In one implementation of the first aspect of the present invention, a matching result determination process is also included: If there are no candidate locations that simultaneously satisfy both types of constraints Figure 3 Triangular, or all candidate sites Figure 3If the number of valid marker correspondences generated by the angular matching is less than three, the map matching is deemed invalid, no global position correction and heading correction are generated, and the navigation and positioning status update operation is skipped. If the optimal candidate is obtained through screening, four types of matching result information are output simultaneously. The four types of matching result information are global position correction, heading correction, compact coded data unit corresponding to the optimal candidate, correspondence of all effective punctuation points, geometric consistency error and final two-dimensional rigid body pose transformation.

[0015] Secondly, the present invention provides a navigation and positioning system based on triangular features and a two-level retrieval tree.

[0016] A navigation and positioning system based on triangular features and a two-level retrieval tree includes: The map database building unit is configured to: acquire the prior standard map landmark set and the current locally observed landmark set, and construct a map database that satisfies geometric constraints based on the prior standard map landmark set. Figure 3 The triangle is then used to generate feature vectors of the lengths of the three sides in ascending order, and a first-level search tree is built based on these feature vectors. The candidate retrieval unit is configured to: generate a feature vector of the three side lengths of an observation triangle based on the current set of local observation landmarks, and retrieve candidate locations with the three side length feature vectors in the first-level retrieval tree. Figure 3 Angular; The pose estimation unit is configured to: based on candidate locations Figure 3 The correspondence between the vertices of the triangle and the observed triangle is estimated by the two-dimensional rigid body pose transformation. The two-dimensional rigid body pose transformation is used to transform the vertices of the observed triangle to the standard map coordinate system to obtain candidate verification points. The state update unit is configured to: establish a secondary search tree based on the prior standard map landmark set, query the nearest neighbor landmark of the candidate verification point in the secondary search tree to calculate the geometric consistency error, determine the global position correction and heading correction based on the geometric consistency error, and update the current navigation and positioning state using the global position correction and heading correction.

[0017] Compared with the prior art, the beneficial effects of the present invention are: This invention constructs a first-level retrieval tree based on the feature vectors of the lengths of the three sides, providing a retrieval tree for all regions. Figure 3 A dedicated indexing system is built around triangles, allowing for rapid filtering and matching of candidate locations after acquiring the features of observed triangles. Figure 3 The triangular shape eliminates redundant calculations from global traversal, significantly reducing the computation time of the feature matching process. Based on candidate locations... Figure 3The method solves the 2D rigid body pose transformation by matching the vertices of the triangle and the observed triangle, and uniformly transforms all observed vertices to the standard map coordinate system to generate candidate verification points, providing a unified coordinate reference for subsequent matching verification. Simultaneously, a two-level search tree is built based on prior standard map landmarks to retrieve the nearest neighbor landmarks corresponding to each candidate verification point and solve for the geometric consistency error. This verifies the rationality of candidate matching from a spatial dimension, reducing erroneous matching caused by similar local geometric features. Finally, the navigation state is updated by solving for position and heading correction parameters based on the geometric consistency error. Through the collaborative work of the two-level search tree, the method simultaneously considers search speed and matching accuracy, enhancing global positioning stability while ensuring real-time navigation capabilities. This effectively resolves the challenge of balancing matching efficiency and accuracy in large-scale landmark scenarios, making it suitable for passive positioning scenarios in complex environments where continuous navigation operation is required.

[0018] Advantages of additional aspects of the invention will be set forth in part in the description which follows, and in part will be obvious from the description, or may be learned by practice of the invention. Attached Figure Description

[0019] The accompanying drawings, which form part of this invention, are used to provide a further understanding of the invention. The illustrative embodiments of the invention and their descriptions are used to explain the invention and do not constitute an improper limitation of the invention.

[0020] Figure 1 A flowchart illustrating the overall process of a navigation and positioning method based on triangular features and a two-level retrieval tree, provided as an exemplary embodiment of the present invention; Figure 2 Prior standard provided for an exemplary embodiment of the present invention Figure 3 Flowchart of angular geometry filtering and compact coding; Figure 3 Standard form provided for an exemplary embodiment of the present invention Figure 3 Flowchart for constructing the KdTree with triangular side length feature; Figure 4 A flowchart of the current local observation triangle generation and unified sorting is provided as an exemplary embodiment of the present invention; Figure 5 Candidate locations based on a first KdTree and the maximum one-sided deviation, as provided in an exemplary embodiment of the present invention Figure 3 Triangular retrieval flowchart; Figure 6 A flowchart for verifying the consistency between two-dimensional rigid body pose estimation and prior standard map global coordinates is provided as an exemplary embodiment of the present invention. Figure 7 A schematic diagram illustrating triangle unified sorting, compact encoding, and two-level KdTree map matching as an exemplary embodiment of the present invention; Figure 8 A schematic diagram of the results of two-level KdTree map matching and navigation positioning correction provided as an exemplary embodiment of the present invention; Figure 9 A standard location based on the classic Manhattan distance is provided as an exemplary embodiment of the present invention. Figure 3 A schematic diagram illustrating the search performance of the first-level candidate KdTree based on the triangular side length feature. Figure 10 A schematic diagram illustrating the effect of second-level nearest neighbor search and geometric consistency verification based on the global two-dimensional coordinates KdTree of a priori standard map, provided as an exemplary embodiment of the present invention; Figure 11 This is a schematic diagram of a navigation and positioning system based on triangular features and a two-level retrieval tree, provided as an exemplary embodiment of the present invention. Detailed Implementation

[0021] The present invention will be further described below with reference to the accompanying drawings and embodiments.

[0022] It should be noted that the following detailed descriptions are exemplary and intended to provide further illustration of the invention. Unless otherwise specified, all technical and scientific terms used herein have the same meaning as commonly understood by one of ordinary skill in the art to which this invention pertains.

[0023] With the rapid development of technologies such as mobile robots and automated guided vehicles, Simultaneous Localization and Mapping (SLAM) plays a crucial role in long-term robot operation, repeated cross-regional navigation, and large-scale map construction. In SLAM systems, due to factors such as sensor observation noise, dynamic environmental changes, and the accumulation of long-term operational errors, the system is prone to cumulative localization drift as the operating time increases and the mapping range expands. Loop closure detection is a core module in SLAM systems, capable of constructing global constraints by identifying locations previously visited by the robot. It is a key step in reducing cumulative drift and maintaining the consistency of the global map structure. In navigation and localization scenarios with a prior standard map, the loop closure detection or relocalization process can be transformed into determining which region in the prior standard map matches the current local observation, and based on this, obtaining the robot's position and heading correction relationship relative to the prior standard map.

[0024] Existing loop closure detection methods for mobile robots typically utilize visual appearance, grid correlation, point cloud descriptors, or local geometric features for matching. These methods often require keyframe or sub-map management, descriptor voting, point cloud registration, and backend optimization. Candidate ambiguity may still arise in environments with weak textures, varying lighting, or repetitive structures, and the computational chain is long, which is unfavorable for lightweight online navigation and localization of mobile robots with limited computing resources. In industrial environments such as factories and warehouses, reflective pillars are typically used as artificial landmarks that can be reliably and repeatedly observed by LiDAR. The global identifier and 2D coordinates of each reflective pillar are stored in a priori standard map. When the robot runs online, it acquires a set of reflective pillar observation points in a local coordinate system. It cannot directly know which global landmark point in the priori standard map each observation point corresponds to. Therefore, a map matching process is needed to determine the correspondence between local observation points and global landmark points. When the number of reflective pillars in the prior standard map increases, or when reflective pillars are repeated or approximately arranged near regular shelves, if unconstrained combination matching is performed directly between local observation points and global landmark points, the number of candidate correspondences will increase, and the online matching pressure will also increase. At the same time, multiple local landmark combinations may have similar distance or shape features, which can easily lead to candidate ambiguity.

[0025] Therefore, for prior standard maps composed of sparse artificial landmarks such as reflectors, existing technologies still have shortcomings in effectively filtering candidate correspondences during the online phase and verifying the global consistency of matching results under conditions of repeated or similar landmark arrangements. Especially when there are multiple reflectors or landmarks in the global map, and local landmark arrangements exhibit repeated or similar structures, a map matching and positioning method is needed that can utilize pre-constructed geometric features to retrieve candidate map regions and combine this with global two-dimensional coordinate information from the prior standard map to verify the consistency of candidate matching relationships. This would reduce the probability of incorrect matching in repetitive structure environments while controlling the online matching pressure, thus obtaining reliable position and heading correction results.

[0026] In view of the problems existing in the existing solutions, this invention proposes a navigation and positioning method based on triangular features and a two-level retrieval tree. Using a prior navigation standard map containing sparse landmark coordinates as a benchmark, the method filters the landmark combinations in the standard map to construct a map that satisfies geometric constraints. Figure 3 Triangles are normalized into triples of side length features arranged in ascending order and compact encoded data units, establishing a standard... Figure 3 Triangle candidate feature library and its KdTree retrieval index; during the navigation operation phase, based on the current local observation landmarks acquired by the mobile robot, observation triangles are generated according to the same rules, and the maximum absolute deviation of the three sides is used to compare the observation triangles with the ground. Figure 3 Triangles are used for similarity screening, and in standard locations Figure 3Candidate similar triangles are retrieved from the triangular feature database. Then, based on the vertex correspondence of the candidate triangles, the two-dimensional rigid body pose transformation from the current local observation coordinate system to the prior navigation standard map coordinate system is estimated using least squares. This transformation is then combined with the Euclidean distance error and geometric consistency error between the transformed observed landmarks and the standard map landmarks. Calculate the map matching score, determine the optimal map matching result, and output the position and heading of the mobile robot in the prior navigation standard map to correct the navigation and positioning results.

[0027] Specifically, such as Figure 1 As shown, the process is divided into an offline modeling stage and an online matching and optimization stage. First, the feature point set of the prior standard map is read. Then, all three-point combinations are enumerated, and legal triangles are selected based on geometric constraints (such as geometric constraint filtering). These triangles are then sorted to generate standardized triangle side length features. Subsequently, compact encoded data units are constructed, and a two-level KdTree index structure is established: the first level is the standard map... Figure 3 The local index for the triangle side length feature is used, with a second-level global two-dimensional coordinate index. Upon entering online processing: the local observation point set of the current frame is obtained, and it is determined whether the number of local observation points is ≥3; if not, the process terminates; if so, the local observation triangle side length feature is generated, and candidate matching results within the nearest threshold are searched in the first-level KdTree (i.e., the first-level search tree), outputting the candidate triangle set and its corresponding local observation triangle; then, the two-dimensional rigid body transformation (rotation + translation) is solved using the least squares method according to vertex order, and the results are mapped back to the standard map wire system; subsequently, the global geometric consistency error E is calculated based on the second-level KdTree (i.e., the second-level search tree). geo ; Determine E geo Does the threshold hold? If not, return to continue searching for other candidates; if yes, select E. geo The minimum solution is taken as the optimal solution. The closed-loop confirmation result and matching information are output, and the data required for subsequent localization, map correction, or closed-loop constraints are generated, thus ending the process. The entire process embodies a complete closed-loop registration logic: feature extraction → index construction → coarse matching → fine registration → consistency verification → closed-loop decision.

[0028] Before performing loop closure detection, the system reads the set of sparse environmental landmarks that have been calibrated or databaseed in the prior standard map. Each standard map landmark At least include a unique identifier and standard Figure 2 3D coordinates The prior standard map serves as the navigation reference map for map matching, pose estimation, and geometric verification. The current running map or local observation points of the current frame generated by the sensors during the robot's online operation do not have a global correspondence that is completely consistent with the standard map in advance. Instead, they are confirmed to form a loop with the corresponding area in the prior standard map through subsequent triangle side length retrieval and two-dimensional nearest neighbor verification.

[0029] In a typical case, a prior standard map stores several reflective pillar locations, their IDs, and 2D map coordinates. A mobile robot equipped with a 2D LiDAR observes highly reflective landmarks in the environment in real time during online operation, forming a local set of observation points (however, at this stage, only the 2D coordinates of the landmarks are obtained through sensors, and they have not yet been matched with the prior standard map). For 3D mapping environments, stable landmarks can also be projected onto a 2D horizontal plane to obtain 2D coordinates used for the construction and verification of the KdTree in this method.

[0030] In this invention, the set of current local observation points or the set of current local observation landmarks, which are obtained in advance by the sensing front end and extracted in two-dimensional coordinate form during the robot's online operation, are denoted as... ,in, , This indicates a local observation number or a temporary number, and it is not required that it correspond one-to-one with the standard map ID before the loopback confirmation; and This represents the horizontal two-dimensional coordinates of the observed landmark in the robot's local coordinate system. Although the robot's environment is three-dimensional, for features commonly used by indoor mobile robots, such as reflective pillars and landmarks, this invention preferably establishes geometric features and a KdTree retrieval structure in the horizontal two-dimensional map plane. represent It constantly senses the total number of local landmarks currently observed by the front end.

[0031] like Figure 2 As shown, this is the a priori standard ground. Figure 3 The flowchart for angular geometry filtering and compact coding begins by inputting a prior standard map feature point set. Then, three distinct points are selected, their three side lengths are calculated, and the lengths are sorted in ascending order. Next, it is determined whether the combination of these three points satisfies three constraints: side length distinctiveness constraint (i.e., the minimum difference between any two sides is greater than a threshold δ1), and angle degradation constraint (minimum interior angle θ). min Greater than the threshold δ θThe system includes constraints on vertex spacing (the longest side l3 is less than R, where R is typically set to twice the maximum radar recognition distance); if any constraint is not satisfied, the current triangle selection is abandoned, and the next set of three points is selected; if all constraints are satisfied, the side length features of the triangle are generated, the order of the three vertices is determined according to a unified rule, efficient and compact encoded data such as uint64_t is generated, and written into a valid standard location. Figure 3 The set of triangles is then determined; it is then checked whether all three-point combinations have been traversed. If not, the loop continues to select the next set of three points; if all combinations have been traversed, the final valid standard land is output. Figure 3 The process assembles the triangles and terminates the process. The entire process ensures that the selected triangles have good geometric stability and distinguishability, providing high-quality feature primitives for subsequent registration.

[0032] This invention enumerates three-point combinations from a priori standard map landmark set, calculates the lengths of the three sides, and obtains the results in ascending order. In a number of sparse environmental landmarks, not all points enumerated into triangles can be included in the first-layer KdTree. For the selection of triangles, if the differences between the three sides are too small, less than a threshold, the triangle is close to an equilateral structure and is easily confused with other triangles in regular shelving or regular reflective column arrays, therefore it is discarded. If the smallest interior angle is less than an angle threshold, it indicates that the three points are approximately collinear or the triangle area is too small, resulting in geometric constraint degradation, therefore it is discarded. If the longest side or the side associated with a specified observation exceeds twice the effective observation distance of the sensor, for example, exceeding 20m, the triangle is considered difficult to form stably within the same local observation frame and is also discarded.

[0033] More specifically, let the set of the three side lengths of the candidate triangle be... Sort them in ascending order as follows The eigenvectors of the triangle side lengths are obtained as follows: (1); in, The shortest side, The second shortest side, The longest side. A triangle should satisfy constraints on side length difference, minimum interior angle, and observation distance, for example: (2); in, The threshold for the difference in side lengths. The minimum angle threshold, The threshold for the observation correlation distance is generally selected as twice the radar scanning radius. In a preferred embodiment, it is combined with the effective detection range of the reflective column by the lidar (e.g., the effective detection range of some radars is 10m). The value can be 20m or determined by the parameter configuration of the components in the sensing map. The above constraints are used to eliminate triangles with sides that are too similar, nearly collinear, too small in scale, or beyond the actual observation range, thereby improving the recognition accuracy of the candidate feature library from the source.

[0034] For the set of triangles that meet the above requirements, since the validity of the triangles has been verified according to the aforementioned criteria, this invention can establish a unique vertex sort based on the relationship between the lengths of the three sides: the common angle point of the shortest and second shortest sides is the first vertex, the common angle point of the second shortest and longest sides is the second vertex, and the common angle point of the shortest and longest sides is the third vertex. This sorting ensures that the triangles on the map and the triangles on the observation side have a consistent vertex arrangement during encoding, effectively reducing duplicate expressions or corresponding ambiguities caused by the original enumeration order. Through this rule, the same triangle is not affected by the original enumeration order, and a stable and consistent vertex sort can be obtained.

[0035] The system writes the uniformly sorted vertex ID sequence, side length features, and standard map coordinate association information into a uint64_t or an equivalent compact encoded data unit. For example, compact encoding can use fixed-length bit segments to store the three vertex IDs, or it can use uint64_t as the primary index and associate the side features and coordinate information through a contiguous memory table. Subsequently, the system... As three-dimensional node coordinates, construct standard land Figure 3 The KdTree, featuring triangular side lengths, uses compact encoded data units as node association information to form a standard map. Figure 3 Triangular candidate feature library.

[0036] More specifically, compact coded data units are formed using uint64_t or equivalent fixed-length bit-field structures in the stored data. These compact coded data units are used to store standard location data. Figure 3 The KdTree algorithm uses triangle vertex IDs, a uniformly sorted vertex sequence, side length feature indexes, and standard map coordinate indexes to quickly recover candidate triangle information after a successful retrieval. In practice, a uint64_t (or other equivalent long bit-segment structure forming a compact encoded data unit) can be used as the primary index, and side lengths, vertex coordinates, and other related information can be stored in a contiguous memory table or adjacency array to improve cache locality and retrieval efficiency.

[0037] In the aforementioned compact encoding process, the system first constructs triangles from map landmarks and real-time observed points, and obtains vertex ID sequences, side length features, and corresponding coordinate indices according to unified rules. Then, the point information, side length features, point coordinates, and index information are compressed and written into uint64_t or equivalent compact encoded data units. This encoding process is used to unify and solidify the relationship between the points, side lengths, and indices of the triangle into a data structure that can be associated with and quickly accessed by KdTree nodes.

[0038] like Figure 3 As shown, this is a standard location. Figure 3 Flowchart for constructing the KdTree with triangular side length features, inputting valid standard land... Figure 3 After assembling the triangles, the side length features of each triangle are read sequentially, along with its corresponding compact encoded data unit. Then, the three side lengths of each triangle (arranged in the order of shortest side l1, second shortest side l2, and longest side l3, i.e., F=(l1,l2,l3)) are used as a three-dimensional feature vector to construct a first-level KdTree node index (i.e., establishing a spatial index using the side length triplet as the key). Subsequently, the KdTree coordinates are associated with the compact data encoding, achieving the binding of geometric features and attribute data. Afterward, it is determined whether the writing operation for all valid triangles has been completed. If not, the loop returns to process the next triangle; if yes, the final constructed standard map is output. Figure 3 The KdTree structure for triangular side length features is now complete. The entire process aims to establish an efficient and searchable triangular feature index to support subsequent matching and pose estimation.

[0039] This invention uses the side length feature vectors of legal triangles in standard maps (i.e., candidate features) ) Constructing standard land Figure 3 An angular candidate feature library and its KdTree retrieval index. During robot operation, sensors acquire the set of currently visible local observation landmarks within the current frame in real time or periodically. Alternatively, it can extract a local set of points from the currently running map. When At that time, the system constructs observation triangles from local observation points and obtains query features using filtering, sorting, and encoding rules that are completely consistent with those of the standard map. ,in, This represents the lengths of the three sides of the observation triangle after it has been uniformly sorted. If there are fewer than three local observation points, or if the constructed observation triangle does not meet the validity constraints, the loop closure result may not be output in this frame.

[0040] like Figure 4As shown, the process first obtains the local observation point set of the current frame and determines whether the number of observation points is no less than 3. If not, the process ends directly. If so, three points are selected, their three side lengths are calculated, and it is determined whether the combination of these three points meets the triangle feature conditions. If not, the current combination is skipped, and the next set of triangle points is selected. If it meets the conditions, the three side lengths are arranged in ascending order, and the corresponding order of the vertices is determined according to the actual spatial distribution of the observation points, thereby generating the features and encoding information of the local triangle. Then, it is determined whether the traversal of all possible triangle combinations has been completed. If not, the next set of triangle points is selected and the above steps are repeated. If completed, the final set of local observation triangle features is output, and the process ends. The entire process ensures that only geometrically valid and ordered encoded local triangles are retained, providing reliable input for subsequent matching with standard map features.

[0041] This invention will query the features of the locally observed triangle. Input is a standard map constructed from prior standard maps. Figure 3 The KdTree, with its triangular side length feature, is used... As the retrieval distance. The maximum absolute difference among the three corresponding side lengths; if this maximum absolute difference does not exceed the threshold. If the deviation of any corresponding side length exceeds the threshold, then add it to the candidate triangle set; If the number of nearest neighbors is specified, the candidate triangle is excluded. Specifying the number of nearest neighbors can be used as a search range or efficiency control parameter when performing KdTree retrieval, but it is not used as an acceptance condition for bypassing the maximum one-sided bias threshold.

[0042] More specifically, in the standard Figure 3 In the KdTree triangular feature, the maximum absolute deviation of the three sides is used as the first-level retrieval metric, that is, the currently observed query features are calculated separately. All candidate features recorded in the standard map The absolute values ​​of the differences between the three corresponding side lengths are used, and the maximum value among them is selected as the candidate matching range judgment value for each comparison: (3); in, , and Representing each Figure 3 The lengths of the three sides of a triangle after it has been sorted according to the same rule. If and only if At that time, the corresponding standard will be Figure 3 Angle is considered as a candidate triangle, among which This is the maximum permissible single-sided deviation threshold among the three corresponding side lengths. If the deviation of any corresponding side length is greater than... ,but The triangle pair is directly excluded. In practice, KdTree can be used to return several nearest neighbors to improve retrieval efficiency, but the triangles that finally enter the candidate set still need to meet the above maximum single-side deviation threshold. The maximum absolute deviation of the three sides can directly exclude the candidate when the deviation of any corresponding side exceeds the threshold, avoiding the large deviation of one side in the classic Manhattan distance being canceled or masked by the smaller deviations of the other two sides, and making it easier to control the side-by-side consistency of the triangle.

[0043] like Figure 5 As shown, candidate locations are based on the first KdTree and the maximum one-sided deviation. Figure 3 The triangle retrieval flowchart is as follows: Input the set of locally observed triangle features in the current frame and read the first-level KdTree data from the standard map; then, select a triangle feature in the current frame sequentially and compare it with its corresponding KdTree data in the standard map to find the most suitable candidate triangle feature point; then, decode the feature points that meet the requirements according to distance order and calculate the side length deviation; finally, determine whether the maximum side length deviation is within a preset threshold. within (i.e. ), or whether the matching point is located at Within a range of nearby results; if the condition is met, the global triangle ID is recorded, and it continues to determine whether all traversal of the required feature points in the current frame has been completed; if not, it returns to process the next feature point; if completed, it further determines whether all triangles in the current frame have been traversed; if not, it returns to select the next triangle; if yes, it ends the process. The entire process realizes robust triangle feature matching and candidate selection based on side length tolerance constraints, providing a reliable correspondence for subsequent pose solving.

[0044] For the standard location to enter the candidate set Figure 3 For triangles, the system reads the uint64_t or equivalent compact encoded data unit associated with the triangle in the KdTree node, and decodes it to recover the candidate triangle vertex ID, unified vertex order, side length features, and standard map coordinate information. The candidate set will be retained and entered into global pose estimation and global two-dimensional coordinate space verification. Specifically, when the standard map... Figure 3 After the KdTree angular feature returns candidate nodes, the system reads the uint64_t or equivalent compact coded data unit associated with the candidate node, and uses this coded data unit as an index to perform decoding or candidate information recovery to obtain the candidate standard location. Figure 3 The original vertex IDs, unified vertex order, side length features, and corresponding standard map coordinates of the triangle are obtained. Decoding does not involve inferring map point coordinates from the side length features; instead, it recovers the previously written or associated candidate information based on the original encoded data units and their associated tables. The recovered vertex order and coordinate information are used to subsequently establish local observation triangles and candidate standard maps. Figure 3The correspondence between vertices of triangles.

[0045] This invention further uses a global coordinate retrieval structure constructed from the coordinates of prior standard map landmarks to verify candidate triangles. In this embodiment, the system verifies candidate triangles based on the locally observed triangles and the coordinates of candidate standard map landmarks. Figure 3 The correspondence between the three pairs of vertices of the triangle is used to estimate the initial two-dimensional rigid body pose transformation using the least squares method. To minimize the sum of squared Euclidean distances between the three observed vertices after transformation and their corresponding standard map vertices: (4); in, The first vertex arranged in a uniform vertex order One observation triangle vertex, For its corresponding candidate standard location Figure 3 angular vertex; and These are the two-dimensional rotation matrix and translation vector to be optimized during the solution process; and Let be the two-dimensional rotation matrix and translation vector that minimize the sum of the squared Euclidean distances between three pairs of corresponding vertices. The initial pose is obtained from the correspondence of three pairs of vertices.

[0046] The above initial pose It is used only as a pose assumption for the current candidate triangle and not as a basis for loop closure confirmation; it is used to transform all landmarks in the current local observation to the prior standard map coordinate system for global coordinate consistency verification.

[0047] Suppose that the current local observation landmark set contains One observation landmark, For the first Each local observation point is transformed into a candidate verification point in the standard map coordinate system: (5); For each candidate verification point Query the nearest standard map landmark in the global 2D coordinate KdTree ,calculate: (6); (7); in, For the first The cost of each observation point is included in the total score.

[0048] The system processes each candidate location Figure 3 Initial pose solution using angle And calculate Choose from the solvable candidates The smallest. Correspondingly, only when Continue processing as needed, among which This is a small amount used during code comparison to exclude invalid results where all observations are scored at the maximum cost. This is a cost threshold, which can be configured based on the allowable deviation at a single point, sensor noise, and map accuracy settings. For example, when the allowable deviation at a single point is 0.1m, it can be set to... If there are fewer than three valid one-to-one correspondences, the loop closure is deemed invalid; if there are at least three valid correspondences, the final pose is re-estimated using all valid correspondences. .

[0049] In an equivalent implementation, the global coordinate retrieval structure can be a two-dimensional KdTree. For each candidate verification point... The 2D KdTree returns the nearest standard map landmark; when the nearest neighbor is determined to be a valid match, the nearest neighbor distance or the matching cost obtained from that distance is used as the nearest neighbor distance. .

[0050] This invention generates map matching and loop closure confirmation outputs based on the global coordinate consistency verification results. When there are no solvable candidates, or the minimum geometric consistency cost does not meet the acceptance criteria, or there are fewer than three valid one-to-one correspondences, a matching failure or loop closure invalidation flag is output; otherwise, the observed triangle vertex number and candidate standard location are output. Figure 3 Triangular coding information, effective punctuation correspondence, and cumulative cost of all observation points. and final pose .

[0051] The final 2D rigid body pose transformation is re-estimated based on all valid one-to-one correspondences in the optimal map matching results. This allows us to obtain the translation and rotation relationships of the current local observation coordinate system relative to the prior standard map coordinate system. Among these, Used to determine the position or position correction amount of a mobile robot in a priori standard map. Used to determine the heading angle or heading correction; thus outputting map matching relationships, position, heading, and corresponding navigation and positioning correction information. Based on the pre-calibrated extrinsic parameter relationships between the sensing sensors and the robot body, the final pose transformation can be converted into the position and heading of the robot body in the prior standard map.

[0052] Specifically, such as Figure 6The diagram shows the flowchart for consistency verification between 2D rigid body pose estimation and prior standard map global coordinates. The input consists of a set of triangles filtered by a first-level KdTree as candidate triangles, and the 2D coordinates of feature points from the prior standard map. Then, a candidate triangle is selected sequentially, and its corresponding vertex ID and related encoding information in the standard map are decoded. Finally, based on a unified vertex order, the current candidate triangle and the standard map coordinates are established. Figure 3 The process establishes a one-to-one correspondence between the vertices of the triangles. Next, a two-dimensional rigid body pose transformation is performed using the least squares method to project the candidate triangles onto the standard map coordinate system. The sum of the Euclidean distances between each of the candidate triangles' three vertices and the corresponding standard point is calculated as the geometric error. Then, it is determined whether this geometric consistency error meets a preset acceptance threshold. If not, the candidate triangle is marked as invalid, and the process continues to check if all candidates have been traversed. If all candidates have been traversed and no qualified candidates exist, the process ends. If all candidates have been traversed but a candidate meeting the threshold requirement exists, the candidate triangle with the smallest geometric error is output. Finally, the closed-loop result corresponding to the optimal candidate and related information are output, and the process ends. The entire process ensures high geometric consistency and reliability of the final closed-loop detection result through step-by-step verification, elimination of invalid matches, and optimization of the optimal solution.

[0053] For better illustration, such as Figure 7 As shown, this invention proposes a unified triangle sorting, compact encoding, and two-level KdTree map matching schematic diagram. First, in the triangle sorting stage, based on the original three-side relationships... Determine the uniform vertex order Arrange them in ascending order of side length as follows: The corresponding triangular features are denoted as ; then proceed Encode the vertex ID Vertex order Trilateral features The coordinate index and associated table pointers are integrated into compact data units, serving as KdTree node association information to support efficient retrieval and triangle reconstruction; then, a first-level 3D feature KdTree is constructed, using the three side lengths... Organize point clouds into 3D spatial coordinates and input query features. Candidates are then retrieved using KdTree and based on the side length deviation. A coarse screening is performed, followed by reading the corresponding codes to recover complete information; then, in the global coordinate verification stage, the initial pose is estimated from the three pairs of vertices of the candidate triangles. All local observation points are arranged according to Transform to a standard map coordinate system and obtain the single-point cost of each observation point using a sparse raster cost map or an equivalent two-dimensional KdTree. and the cumulative cost across all observation points Candidates are selected; finally, the final pose is re-estimated using no fewer than three pairs of valid one-to-one correspondence points. It outputs whether the loop closure is true or false, the candidate triangle encoding information, the effective punctuation correspondence, and the final pose. With cumulative cost Waiting for the results.

[0054] Figure 8 This is a visualization of the loop closure matching results, showing how the currently observed triangle, after pose transformation, matches the global triangle in a single loop closure detection (loop=1). Figure 3 Triangle matching status: Gray dots represent standard points in the global map, orange triangles are candidate triangles formed by the transformed observation interior points, and blue solid-line triangles are confirmed successful matches. Figure 3 The triangle is marked with a red cross, indicating the rejected outer points. The figure shows a total of 5 inner points, of which 4 were successfully matched (corresponding to the vertex numbers of the matching triangle [4, 3, 2]). The loop closure result is PASS, indicating that the detection passed the geometric consistency verification and achieved effective loop closure.

[0055] Figure 9 This is a three-dimensional spatial diagram of the KdTree retrieval process with first-order three-sided features. The horizontal and vertical axes represent the shortest sides, respectively. and the second shortest side (Unit: m), vertical axis is the longest side (Unit: m); The gray point cloud represents the feature vectors of the three sides of all triangles in the map, and the red pentagram represents the query feature. The blue rhombuses represent candidate features. The maximum side length deviation is marked in the diagram. And satisfy (Threshold), so it was judged as PASS, which reflects the ability of the coarse screening stage based on the invariant length of the three sides to efficiently screen out geometrically similar candidates.

[0056] Figure 10 This is a graph showing the verification results of a two-level global coordinate KdTree system. In this coordinate system, the horizontal axis represents the horizontal coordinate in meters, and the vertical axis represents the vertical coordinate in meters. Gray dots represent global map points, blue squares represent the nearest standard map point matched to the corresponding transformed observation point, and orange triangles represent the transformed observation points after the initial two-dimensional rigid body pose transformation to the standard map coordinate system. The six transformed observation points correspond to single-point geometric costs, namely c1=0.113m, c2=0.005m, c3=0.010m, c4=0.015m, and c5. The single-point geometric cost value of the transformed observation point c5 is greater than the single-point allowable deviation threshold C. maxThe points marked as invalid are not included. The geometric costs of the remaining transformed observation points from c1 to c4 do not exceed the threshold and can be included in the cumulative calculation of global geometric consistency error.

[0057] In summary, this invention transforms map matching between current local observed landmarks and prior standard maps into candidate retrieval in the feature space of triangle side lengths, reducing the need for full combination matching between local observed points and all standard map points, thus improving the efficiency of online navigation map matching. This invention utilizes side length distinguishability, minimum interior angle, and maximum associative distance constraints to filter... Figure 3 Triangular shapes reduce matching ambiguities caused by unstable side length order, near-collinearity of three points, or exceeding the effective observation range in regularly arranged landmarks.

[0058] This invention establishes observation triangles and candidate locations through a unified vertex sorting. Figure 3 The invention establishes stable vertex correspondences between triangles and associates vertex identifiers, three-sided features, and standard map coordinates through compact coding or master index, facilitating candidate information recovery and two-dimensional pose estimation. The invention uses the maximum single-sided deviation of the three sides as the candidate acceptance condition, so that candidates whose deviation of any corresponding side length exceeds the threshold are excluded, avoiding the large deviation of a single side being masked when relying solely on the cumulative difference of the three sides.

[0059] After candidate retrieval in the triangle feature space, this invention further utilizes a global coordinate KdTree constructed from the two-dimensional coordinates of prior standard map landmarks to perform nearest-neighbor geometric consistency verification on the vertices of the transformed observed triangles, ensuring that the candidates simultaneously satisfy both shape feature consistency and standard map coordinate position consistency. This invention uses the obtained two-dimensional coordinates of sparse landmarks as input and does not limit the underlying perception extraction method of the landmarks, making it applicable to reflective pillars, highly reflective road signs, stable corner points, or other map landmarks that can be repeatedly observed. Based on the optimal map matching result, this invention outputs a two-dimensional rigid body pose transformation as well as position and heading corrections, which can be used for mobile robot navigation and positioning correction, map calibration, or closed-loop constraint construction.

[0060] Figure 11 A navigation and positioning system based on triangular features and a two-level retrieval tree is shown, including: Map database unit 1101 is configured to: acquire a priori standard map landmark set and a current local observation landmark set, and construct a map database that satisfies geometric constraints based on the priori standard map landmark set. Figure 3 The triangle is then used to generate feature vectors of the lengths of the three sides in ascending order, and a first-level search tree is built based on these feature vectors. Candidate retrieval unit 1102 is configured to: generate a feature vector of the three side lengths of an observation triangle based on the current set of local observation landmarks, and retrieve candidate locations with the three side length feature vectors in the first-level retrieval tree. Figure 3 Angular; The pose estimation unit 1103 is configured to: based on candidate locations Figure 3 The correspondence between the vertices of the triangle and the observed triangle is estimated by the two-dimensional rigid body pose transformation. The two-dimensional rigid body pose transformation is used to transform the vertices of the observed triangle to the standard map coordinate system to obtain candidate verification points. The state update unit 1104 is configured to: establish a secondary search tree based on the prior standard map landmark set, query the nearest neighbor landmark of the candidate verification point in the secondary search tree to calculate the geometric consistency error, determine the global position correction and heading correction based on the geometric consistency error, and update the current navigation and positioning state using the global position correction and heading correction.

[0061] It is understood that the aforementioned units can be individually or entirely merged into one or more other units, or some of the units can be further divided into multiple functionally smaller units. This achieves the same operation without affecting the technical effects of the embodiments of the present invention. The aforementioned units are based on logical functional division. In practical applications, the function of one unit can be implemented by multiple units, or the function of multiple units can be implemented by one unit. In other embodiments of the present invention, the system may also include other units. In practical applications, these functions can also be implemented with the assistance of other units, and can be implemented collaboratively by multiple units.

[0062] According to another embodiment of the present invention, the system of this embodiment can be constructed by running a computer program (including program code) capable of performing the steps involved in the corresponding method of the present invention on a general-purpose computing device, such as a computer, which includes processing elements and storage elements such as a central processing unit (CPU), random access memory (RAM), and read-only memory (ROM). The computer program can be recorded on, for example, a computer-readable recording medium, loaded into the aforementioned computing device through the computer-readable recording medium, and run therein.

[0063] Those skilled in the art will recognize that the units and algorithm steps of the various examples described in conjunction with the embodiments disclosed in this invention can be implemented in electronic hardware, or a combination of computer software and electronic hardware. Whether these functions are implemented in hardware or software depends on the specific application and design constraints of the technical solution. Those skilled in the art can implement the described functions using different methods for each specific application, but such implementations should not be considered beyond the scope of this invention.

[0064] In the above embodiments, implementation can be achieved, in whole or in part, through software, hardware, firmware, or any combination thereof. When implemented in software, it can be implemented, in whole or in part, as a computer program product. A computer program product includes one or more computer instructions. When the computer program instructions are loaded and executed on a computer, all or part of the flow or function according to the embodiments of the present invention is generated. The computer can be a general-purpose computer, a special-purpose computer, a computer network, or other programmable device. The computer instructions can be stored in or transmitted through a computer-readable storage medium. The computer instructions can be transmitted from one website, computer, server, or data center to another website, computer, server, or data center via wired (e.g., coaxial cable, fiber optic cable, digital cable) or wireless (e.g., infrared, wireless, microwave, etc.). The computer-readable storage medium can be any available medium that a computer can access or a data processing device such as a server or data center that integrates one or more available media. The available medium can be a magnetic medium (e.g., floppy disk, hard disk, magnetic tape), an optical medium (e.g., DVD), or a semiconductor medium (e.g., solid-state drive), etc.

[0065] The above description is merely a preferred embodiment of the present invention and is not intended to limit the invention. Various modifications and variations can be made to the present invention by those skilled in the art. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the scope of protection of the present invention.

Claims

1. A navigation and positioning method based on triangular features and a two-level retrieval tree, characterized in that, Includes the following processes: Obtain the set of prior standard map landmarks and the set of current local observation landmarks. Construct a map triangle that satisfies geometric constraints based on the set of prior standard map landmarks and generate the three-side length feature vectors arranged in ascending order. Build a first-level search tree based on the three-side length feature vectors. After generating the eigenvectors of the three side lengths in ascending order, the process also includes: Determine a uniform vertex order for the map triangle, where the common corner point of the shortest and second shortest sides is the first vertex, the common corner point of the second shortest and longest sides is the second vertex, and the common corner point of the shortest and longest sides is the third vertex. Write the vertex identifier, three-side length feature vector and standard map coordinate index corresponding to the unified vertex sorting into the compact coded data unit; Compact coded data units are stored as node association information in the first-level retrieval tree; Based on the current set of local observation landmarks, generate feature vectors for the lengths of the three sides of the observation triangle. Then, retrieve candidate map triangles with matching feature vectors in the first-level search tree, including: Calculate the absolute value of the difference between the three-side length feature vector and the corresponding side length of the three-side length feature vector of each node in the first-level search tree; The maximum value among the three absolute values ​​is selected as the maximum absolute deviation of the three sides; When the maximum absolute deviation of the three sides is less than or equal to the maximum single-sided deviation threshold, read the compact coded data unit associated with the corresponding node; The vertex identifiers, unified vertex sorting, and standard map coordinate information of the candidate map triangles are recovered based on the compact coded data units as candidate map triangles; The two-dimensional rigid body pose transformation is estimated based on the vertex correspondence between the candidate map triangle and the observation triangle. The two-dimensional rigid body pose transformation is then used to transform the vertices of the observation triangle to the standard map coordinate system to obtain candidate verification points. A secondary search tree is built based on a prior standard map landmark set, and the nearest neighbor landmarks of candidate verification points are queried in the secondary search tree to calculate the geometric consistency error, including: In the offline processing stage, the two-dimensional global coordinates corresponding to each of the prior standard map landmarks are used as node information to construct a global two-dimensional coordinate retrieval tree as a secondary retrieval tree; During the online processing phase, each candidate verification point is traversed sequentially, and the prior standard map landmark point with the closest spatial distance to the current candidate verification point is retrieved within the secondary search tree. Calculate the Euclidean distance between the current candidate verification point and its nearest landmark, and use this distance as the geometric cost of a single point. The global geometric consistency error is obtained by summing up the values ​​of all individual point geometric costs. The calculation logic for global geometric consistency error is as follows: sum up all the single-point geometric costs generated for each local observation landmark, and use the summation result as the geometric consistency error; After obtaining the global geometric consistency error of each candidate map triangle, the candidate map triangles are screened according to the following constraints: Candidate map triangles that satisfy both types of constraints are screened. The first type of constraint is that after adding a fixed minimum constant to the geometric consistency error of the current candidate, the value does not exceed the product of the single-point deviation threshold and the total number of observation points. The second type of constraint is that the number of effective point correspondences formed by the current candidate matching is not less than three sets. The global position correction and heading correction are determined based on the geometric consistency error, and the current navigation and positioning status is updated using the global position correction and heading correction.

2. The navigation and positioning method based on triangular features and a two-level retrieval tree as described in claim 1, characterized in that, Construct map triangles that satisfy geometric constraints, including: Enumerate the combinations of three points from the prior standard map landmark set to obtain the set of candidate triangle side lengths; Arrange the set of candidate triangle side lengths in ascending order to obtain the shortest side, the second shortest side, and the longest side; Candidate triangles are removed when the difference between the shortest and second shortest sides is less than the side length difference threshold, the minimum interior angle is less than the angle threshold, or the longest side is greater than the observation association distance threshold. The selected candidate triangles will be used as map triangles.

3. The navigation and positioning method based on triangular features and a two-level retrieval tree as described in claim 1, characterized in that, Estimate the 2D rigid body pose transformation based on the vertex correspondence between candidate map triangles and observed triangles, including: Based on the unified vertex sorting recovered from the compact coded data unit, the unified vertex sorting corresponding to the observed triangle is matched to establish a one-to-one correspondence between the vertices of the observed triangle and the vertices of the candidate map triangle. With the goal of minimizing the sum of squared Euclidean distances between three sets of corresponding vertices, the two-dimensional rotation matrix and two-dimensional translation vector are solved by the least squares method to obtain the initial two-dimensional rigid body pose transformation. The calculation logic for the optimization target is as follows: traverse the three sets of matching vertices, calculate the distance between each set of observed vertices and the corresponding map vertices after rotation and translation, obtain the sum of the squares of all distances, and select the rotation matrix and translation vector that minimize the summation value as the component parameters of the initial two-dimensional rigid body pose transformation. Among them, the observation vertices are the vertices of the observation triangle arranged in a uniform vertex order, and the map vertices are the candidate map triangle vertices that correspond one-to-one with the observation vertices.

4. The navigation and positioning method based on triangular features and a two-level retrieval tree as described in claim 1, characterized in that, By utilizing two-dimensional rigid body pose transformation, the vertices of the observed triangle are transformed to the standard map coordinate system to obtain candidate verification points, including: Retrieve all local observation landmarks contained in the current local observation landmark set; Substitute each local observation landmark into the initial two-dimensional rigid body pose transformation, and perform rotation and translation operations in sequence to obtain the corresponding candidate verification points in the standard map coordinate system. Candidate check points obtained from the calculations of all local observation landmarks are combined to form a set of candidate check points.

5. The navigation and positioning method based on triangular features and a two-level retrieval tree as described in claim 1, characterized in that, The global position and heading corrections are determined based on the geometric consistency error, including: Traverse all candidate map triangles output by the first-level search tree, solve the corresponding initial two-dimensional rigid body pose transformation and calculate the corresponding geometric consistency error for each triangle. Candidate map triangles that satisfy both types of constraints are selected. The first type of constraint is that the geometric consistency error corresponding to the current candidate, after being increased by a fixed minimum constant, does not exceed the product of the single-point deviation threshold and the total number of observation points. The second type of constraint is that the number of effective point correspondences formed by the current candidate matching is not less than three sets. From the selected valid candidate map triangles, the candidate with the smallest geometric consistency error is selected as the optimal candidate. Retrieve all valid punctuation correspondences formed by the optimal candidate matching, and then use the least squares method to solve the final two-dimensional rigid body pose transformation again. Extract the translation components contained in the final two-dimensional rigid body pose transformation, and calculate the global position correction. Extract the rotational components contained in the final two-dimensional rigid body pose transformation, and calculate the heading correction.

6. The navigation and positioning method based on triangular features and a two-level retrieval tree as described in claim 1, characterized in that, It also includes the matching result determination process: If there are no candidate map triangles that simultaneously satisfy both types of constraints, or if the number of valid marker correspondences generated by matching all candidate map triangles is less than three, then map matching is deemed invalid, global position correction and heading correction are not generated, and navigation and positioning status update operations are skipped. If the optimal candidate is obtained through screening, four types of matching result information are output simultaneously. The four types of matching result information are global position correction, heading correction, compact coded data unit corresponding to the optimal candidate, correspondence of all effective punctuation points, geometric consistency error and final two-dimensional rigid body pose transformation.

7. A navigation and positioning system based on triangle features and a two-level search tree, employing the navigation and positioning method based on triangle features and a two-level search tree as described in any one of claims 1-6, characterized in that, include: The map database building unit is configured to: obtain the set of prior standard map landmarks and the set of current local observation landmarks; construct a map triangle that satisfies geometric constraints based on the set of prior standard map landmarks and generate the three-side length feature vectors arranged in ascending order; and build a first-level search tree based on the three-side length feature vectors. The candidate retrieval unit is configured to: generate a feature vector of the three sides length of the observation triangle based on the current set of local observation landmarks, and retrieve candidate map triangles with the feature vector of the three sides length in the first-level retrieval tree; The pose estimation unit is configured to: estimate the two-dimensional rigid body pose transformation based on the vertex correspondence between the candidate map triangle and the observed triangle, and use the two-dimensional rigid body pose transformation to transform the vertices of the observed triangle to the standard map coordinate system to obtain candidate verification points. The state update unit is configured to: establish a secondary search tree based on the prior standard map landmark set, query the nearest neighbor landmark of the candidate verification point in the secondary search tree to calculate the geometric consistency error, determine the global position correction and heading correction based on the geometric consistency error, and update the current navigation and positioning state using the global position correction and heading correction.

Images