A robot autonomous navigation global topology map construction and management system

Abstract

The application discloses a kind of robot autonomous navigation global topology atlas construction and management system, it is related to robot navigation technical field, global optimum and global consistency, in the single connected network of covering entire passable space with A* from start point to end point the whole journey search is carried out, the path obtained is global (near) optimum, fundamentally avoid the detour and dead angle reciprocation caused by local planning due to limited field of view;Multi-source / multi-section fusion maintains global topology consistency, task point back-throw binding guarantees that semantics and geometry are strictly consistent globally, so as to realize global cooperation of whole journey path and along-the-way task arrangement.Meanwhile, the construction cost is significantly reduced, and the efficiency is improved: based on the incremental automatic network of pose trajectory, manual annotation is avoided, combined with space resolution criterion and IKD-Tree de-redundancy, node density is controllable, connection relationship is reliable, and construction period is reduced from hour level to build as you go.

Description

Technical Field

[0001] This invention relates to the field of robot navigation technology, and in particular to a global topology map construction and management system for autonomous robot navigation. Background Technology

[0002] When mobile robots perform autonomous navigation in structured and semi-structured environments (such as parks, buildings, and streets), they need a compact representation of the traversable space on which to perform global path planning and semantic action triggering. Existing global environment representation methods are mainly divided into two categories.

[0003] The first category is a representation method based on grid maps. This method discretizes the environment into uniform grids, with each grid marked as passable or obstructed, and runs search algorithms such as Dijkstra's or A* on the grids. As a well-known technique that forms the basis of this invention and helps in understanding it, the heuristic cost of the A* algorithm at node a is given by the Euclidean distance:

[0004] Its total valuation function is ,in From the starting point to The cumulative cost.

[0005] The second category is a representation method based on manually labeled topological nodes. A topological graph is formed by manually or offline tools labeling nodes and their connections on a map, and the robot retrieves paths on this graph. This is the closest existing technical solution to the present invention. Its device structure typically includes: a graph structure storing node coordinates and adjacency relationships, and a path search module; its method steps are: (1) offline node labeling; (2) manual edge connection; (3) online matching the robot's current position to the nearest node; (4) performing graph search.

[0006] The existing technology has the following technical shortcomings: (1) Raster maps have high memory overhead and maintenance costs. The complexity of raster storage increases with the square of the environment area, and resolution and memory usage are mutually restrictive. As the resolution increases, the number of rasters increases dramatically, which in turn leads to a significant increase in memory usage and search time. Therefore, it is difficult to balance accuracy and efficiency in large-scale environments.

[0007] (2) Manual annotation of topology maps is inefficient and inconsistent. Existing topology methods rely on manual annotation of nodes and edges. Because of this reliance on manual annotation, the density of nodes is uneven and the connection relationships are prone to errors and omissions, which in turn leads to unreliable road network accessibility and long construction cycle, resulting in low construction efficiency.

[0008] (3) The road network and semantic tasks are separated and the binding robustness is poor. In existing methods, path nodes and task semantic points are usually stored in separate databases and compared point by point. Because the task points and the final path geometry are not consistent, the task landing point deviates from the actual trajectory, which leads to inaccurate triggering position, missed triggering or false triggering, so the binding robustness is poor.

[0009] (4) Interval tasks cannot be triggered in pairs or in an orderly manner. For interval tasks that need to be triggered in pairs, such as "going up a step - going down a step" and "entering the gate - exiting the gate", existing methods lack modeling of the order and pairing relationship of tasks on the path. Due to the lack of pairing and ordering mechanisms, the entry and exit actions of the same segment are mismatched or lost, which leads to errors in the actions of high-risk segments, resulting in poor safety and reliability. Summary of the Invention

[0010] In view of the above problems, the present invention provides a robot autonomous navigation global topology map construction and management system to overcome or at least partially solve the above problems, so as to solve the technical problems of road network construction relying on manual labor, large memory consumption, separation of road network and task semantics, and inability to trigger interval-type tasks in pairs in an orderly manner, thereby reducing construction costs, improving task triggering accuracy and navigation safety in high-risk sections.

[0011] This invention provides the following solution: A robot autonomous navigation global topology map construction and management system includes: The positioning input module is used to output the real-time pose from at least one positioning source after normalizing it to the global world coordinate system through a pre-calibrated rigid body external parameter transformation. The topology network construction module is used to receive the real-time pose, discretize the continuous pose flow into a set of nodes based on the spatial resolution criterion, establish an edge set and Euclidean distance weight based on the temporal adjacency relationship, and maintain an incremental IKD-Tree to provide nearest neighbor retrieval. The semantic point construction module is used to construct a navigation point set and a task point set, and to project the semantic points to the nearest road network node through the IKD-Tree to complete the pose anchoring. At the same time, it maintains a task segment pairing table to provide semantic input for fusion planning. The task segment pairing table is used to define the mapping relationship between entry task code and exit task code. The semantic point fusion planning module is used to attach the starting point and the ending point to the road network node, search for the shortest topological path on the global road network, reorganize the path points and construct a temporary KD-Tree, bind the task point to the nearest point on the path, and calculate the azimuth angle of the path point. The task segment pairing and encoding module is used to perform nearest pairing on the ordered task sequence on the path according to the task segment pairing table, and to encode the offset of the exiting task overlay segment of the successfully paired task, and output a complete global navigation path with task identifier and azimuth angle. The persistence and lifecycle management module is used to assign a globally unique identifier to the topology network, archive the network data and its associated semantic point set and task segment pairing table in a strong association, and provide network switching, merging and deletion operations. A robot motion control module is used to receive the complete global navigation path and perform motion control and trajectory tracking.

[0012] Preferably: the topology road network construction module constructs the road network incrementally based on pose trajectory, and the construction method includes: Define the minimum interval between nodes. For a new pose, generate a new node only if the minimum spatial distance between the new pose and the current set of nodes is not less than the minimum interval. For nodes generated sequentially at adjacent time points, weights are assigned using three-dimensional Euclidean distance and undirected edges are established; Each time a node is generated, it is synchronously inserted into the IKD-Tree to keep the graph construction and retrieval structure consistent.

[0013] Preferably: take the union of the nodes of each sub-road network, and merge the same nodes whose spatial distance is less than the fusion threshold to obtain the deduplicated global node set, and establish an index mapping from each sub-road network node to the global node; The edge index mapping of each sub-network is rewritten to the global node, and the Euclidean weights are recalculated, while self-loops and repeated edges are removed. For adjacent node pairs located in different sub-networks after merging and with a spatial spacing less than the bridging threshold, supplementary connection edges are added to enable the originally independently collected sub-networks to automatically connect at overlapping or adjacent points, thereby achieving multi-network fusion. After the integration is completed, the IKD-Tree will be reconstructed as a whole for the unified global road network.

[0014] Preferably, the method for constructing the semantic point construction module includes: During the road network recording or interactive annotation process, the real-time pose input by the positioning input module is used as a reference to provide a six-degree-of-freedom pose for each semantic point. The nearest neighbor projection is then performed by the IKD-Tree of the topology road network construction module to anchor the projected point to the road network node and determine its belonging position on the road network. The navigation point set and the task point set are constructed and obtained respectively in the form of a set of key indexes; the navigation points are indexed by keywords and can specify the navigation destination pose, serving as the endpoint source for path search; the task points carry task identifier codes and speed levels, marking the positions where the robot needs to perform specific actions; For interval-type tasks that require pairing, a task segment pairing table is constructed, defined by key-value pairs of entry and exit codes, to describe the semantic correspondence between the entry and exit of each task segment, so that the task segment pairing and encoding module can perform nearest pairing and decodeable encoding.

[0015] Preferably, an adsorption threshold is set to determine whether the semantic point is effectively located near the road network; when the projection distance is greater than the adsorption threshold, an off-network alarm is given to ensure that all constructed semantic points are effectively located within the neighborhood of the passable road network.

[0016] Preferably, the fusion planning method of the semantic point fusion planning module includes: The real-time pose and the target navigation point are respectively snapped to the node through the nearest neighbor projection of the road network IKD-Tree to obtain the start index and the end index; A* search is performed on the global road network to obtain the node sequence, and the heuristic function is three-dimensional Euclidean distance. The node coordinates of the node sequence are recombined with the target navigation point to form a path point sequence, and a temporary KD-Tree is created for this sequence; For each task point, retrieve its nearest point on the path on the temporary KD-Tree; when the distance is less than the binding threshold, bind the task identifier to the corresponding path point index and establish a path index to task mapping; For path points of a bound task, the starting task heading of the task segment is directly recorded using the task point, and the ending task heading of the task segment is calculated by the difference between adjacent points.

[0017] Preferably, the task pairing and encoding method of the task segment pairing and encoding module includes: Define a task identifier space, which includes point tasks and segment tasks; Let the ordered sequence of tasks along the path be... The corresponding path indices are in ascending order; the algorithm maintains an ordered stack-like list of tasks to be matched. Scan from front to back: Point-to-point task If so, then keep it directly. Represents a set of point tasks; For segmented tasks, from the list The tail moves forward and back, when And look for matching relationships Recent tasks If it hits, it will retain the same value. And Remove :

[0018] like Then push into the list Awaiting further pairings; This indicates entry into the task code set. Represents the set of valid exit codes; For tasks that are determined to exit a segment, an additional segment offset is added for encoding, while incoming tasks retain their original code. The codes and azimuths of all retained tasks are backfilled in the original path index order to obtain a complete navigation path with task identifiers, which can be used by the motion control terminal to trigger actions in sequence, in pairs, and in a decodeable manner.

[0019] Preferably, the persistence and lifecycle management module is used to perform the following operations: Each topological road network is assigned a globally unique identifier, and an incremental detection strategy is used to generate non-conflicting identifiers. Maintain the currently selected road network identifier in persistent metadata; when switching, first verify the validity of the target identifier in the identifier set, and after the verification is successful, update the selected identifier and write it to disk so that the previously selected road network can still be restored after the system restarts; After obtaining the global road network by calling the multi-road network fusion, a new unique identifier is assigned to the fusion result and written to disk. Atomic road networks can be retained or merged according to a strategy, and the fusion relationship is recorded in the metadata. When deleting, the storage directory and the identifier-name mapping entry corresponding to the identifier are also removed; if the deleted identifier happens to be the currently selected road network, the metadata of the selected identifier is cleared simultaneously.

[0020] Preferably, the positioning source includes a single positioning source or a multi-source fusion positioning source, and the external interface is consistent with the coordinate reference.

[0021] According to specific embodiments provided by the present invention, the present invention discloses the following technical effects: This invention provides a robot autonomous navigation global topology map construction and management system, which achieves global optimization and global consistency: a full-range search from the starting point to the end point is performed using A* on a single connected road network covering the entire traversable space, and the resulting path is globally (near) optimal, fundamentally avoiding detours and dead ends caused by limited vision in local planning; multi-source / multi-segment fusion maintains global topology consistency, and task point return binding ensures strict consistency of semantics and geometry globally, thereby achieving global coordination of the entire path and the arrangement of tasks along the way.

[0022] Construction costs are significantly reduced and efficiency is improved: Incremental automatic mesh construction based on pose trajectory eliminates manual annotation. Combined with spatial resolution criteria and IKD-Tree redundancy removal, node density is controllable and connection relationships are reliable. The construction cycle is reduced from hours to on-the-go construction.

[0023] Highly efficient retrieval and planning: IKD-Tree is used to realize dynamic addition and deletion of nodes and O(log n) nearest neighbor retrieval, which significantly reduces memory usage and retrieval time compared with grid traversal, and is suitable for large-scale environments.

[0024] Large-scale scalability: Multi-network geometric registration and stitching fusion support segmented mapping and overall planning. The network can be gradually expanded as the collection range expands while maintaining topological connectivity and geometric consistency.

[0025] Improved task triggering accuracy: The KD-Tree return binding method ensures that the task point falls strictly on the planned path, eliminating the deviation of the landing point geometrically and significantly reducing missed triggers and false triggers.

[0026] Enhanced safety in high-risk sections: The task segment pairing model, ordered proximity matching, and decodeable encoding enable paired, ordered, and decodeable triggering of tasks within a section, and support for segment cross-nesting, thereby improving the correctness of actions and driving safety in complex scenarios.

[0027] The project boasts strong maintainability: unified semantic point construction and unique identifiers for storage media, strong association persistence, and complete lifecycle management. It supports switching, merging, adding, deleting, and modifying multiple networks, facilitating large-scale deployment.

[0028] Of course, any product implementing this invention does not necessarily need to achieve all of the advantages described above at the same time. Attached Figure Description

[0029] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the embodiments will be briefly described below. Obviously, the drawings described below are merely some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without any creative effort.

[0030] Figure 1 This is a schematic diagram of a robot autonomous navigation global topology map construction and management system provided in an embodiment of the present invention; Figure 2 This is a schematic diagram of the topology network construction process provided in an embodiment of the present invention; Figure 3 This is a flowchart of semantic point construction provided in an embodiment of the present invention. Detailed Implementation

[0031] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only a part of the embodiments of the present invention, and not all of them. All other embodiments obtained by those skilled in the art based on the embodiments of the present invention are within the scope of protection of the present invention.

[0032] See Figure 1 This invention provides a robot autonomous navigation global topology map construction and management system, such as... Figure 1 As shown, the system may include: The positioning input module is used to output the real-time pose from at least one positioning source after normalizing it to the global world coordinate system through a pre-calibrated rigid body parameter transformation. In specific implementation, the positioning source can include a single positioning source or a multi-source fusion positioning source, and the external interface is consistent with the coordinate reference.

[0033] A topology road network construction module is used to receive the real-time pose, discretize the continuous pose flow into a set of nodes based on a spatial resolution criterion, establish an edge set and Euclidean distance weights based on temporal adjacency relationships, and maintain an incremental IKD-Tree to provide nearest neighbor retrieval. Specifically, embodiments of the present invention can provide an incremental road network construction method based on pose trajectories, comprising: Define the minimum interval between nodes. For a new pose, generate a new node only if the minimum spatial distance between the new pose and the current set of nodes is not less than the minimum interval. For nodes generated sequentially at adjacent time points, weights are assigned using three-dimensional Euclidean distance and undirected edges are established; Each time a node is generated, it is synchronously inserted into the IKD-Tree to keep the graph construction and retrieval structure consistent.

[0034] Also includes: Take the union of the nodes of each sub-road network, and merge the nodes of the same position whose spatial distance is less than the fusion threshold to obtain the deduplicated global node set, and establish the index mapping from each sub-road network node to the global node. The edge index mapping of each sub-network is rewritten to the global node, and the Euclidean weights are recalculated, while self-loops and repeated edges are removed. For adjacent node pairs located in different sub-networks after merging and with a spatial spacing less than the bridging threshold, supplementary connection edges are added to enable the originally independently collected sub-networks to automatically connect at overlapping or adjacent points, thereby achieving multi-network fusion. After the integration is completed, the IKD-Tree will be reconstructed as a whole for the unified global road network.

[0035] A semantic point construction module is used to construct a navigation point set and a task point set, and to project the semantic points to the nearest road network nodes through the IKD-Tree to complete pose anchoring. Simultaneously, it maintains a task segment pairing table to provide semantic input for fusion planning. The task segment pairing table is used to define the mapping relationship between entry and exit task codes. Specifically, in this embodiment, the construction method of the semantic point construction module can include: During the road network recording or interactive annotation process, the real-time pose input by the positioning input module is used as a reference to provide a six-degree-of-freedom pose for each semantic point. The nearest neighbor projection is then performed by the IKD-Tree of the topology road network construction module to anchor the projected point to the road network node and determine its belonging position on the road network. The navigation point set and the task point set are constructed and obtained respectively in the form of a set of key indexes; the navigation points are indexed by keywords and can specify the navigation destination pose, serving as the endpoint source for path search; the task points carry task identifier codes and speed levels, marking the positions where the robot needs to perform specific actions; For interval-type tasks that require pairing, a task segment pairing table is constructed, defined by key-value pairs of entry and exit codes, to describe the semantic correspondence between the entry and exit of each task segment, so that the task segment pairing and encoding module can perform nearest pairing and decodeable encoding.

[0036] An adsorption threshold is set to determine whether a semantic point is effectively located near the road network. When the projection distance is greater than the adsorption threshold, an off-network alarm is given to ensure that all constructed semantic points are effectively located within the neighborhood of the passable road network.

[0037] A semantic point fusion planning module is used to attach the starting point and ending point to road network nodes, search for the shortest topological path on the global road network, reorganize path points and construct a temporary KD-Tree, bind task points to the nearest point on the path, and calculate the azimuth angle of the path points. Specifically, embodiments of the present invention can provide a fusion planning method for the semantic point fusion planning module including: The real-time pose and the target navigation point are respectively snapped to the node through the nearest neighbor projection of the road network IKD-Tree to obtain the start index and the end index; A* search is performed on the global road network to obtain the node sequence, and the heuristic function is three-dimensional Euclidean distance. The node coordinates of the node sequence are recombined with the target navigation point to form a path point sequence, and a temporary KD-Tree is created for this sequence; For each task point, retrieve its nearest point on the path on the temporary KD-Tree; when the distance is less than the binding threshold, bind the task identifier to the corresponding path point index and establish a path index to task mapping; For path points of a bound task, the starting task heading of the task segment is directly recorded using the task point, and the ending task heading of the task segment is calculated by the difference between adjacent points.

[0038] The task segment pairing and encoding module is used to perform nearest-neighbor pairing on the ordered task sequence on the path according to the task segment pairing table, and to encode the offset of the exiting task overlay segment of the successfully paired task, outputting a complete global navigation path with task identifier and azimuth angle; in specific implementation, the task pairing and encoding method of the task segment pairing and encoding module of the present invention can include: Define a task identifier space, which includes point tasks and segment tasks; Let the ordered sequence of tasks along the path be... The corresponding path indices are in ascending order. The algorithm maintains an ordered stack-like list of tasks to be matched. Scan from front to back: Point-to-point task If so, then keep it directly. Represents a set of point tasks; For segmented tasks, from the list The tail moves forward and back, when And look for matching relationships Recent tasks If it hits, it will retain the same value. And Remove :

[0039] like Then push into the list Awaiting further pairings; This indicates entry into the task code set. Represents the set of valid exit codes; For tasks that are determined to exit a segment, an additional segment offset is added for encoding, while incoming tasks retain their original code. The codes and azimuths of all retained tasks are backfilled in the original path index order to obtain a complete navigation path with task identifiers, which can be used by the motion control terminal to trigger actions in sequence, in pairs, and in a decodeable manner.

[0040] The persistence and lifecycle management module is used to assign globally unique identifiers to the topological road network, strongly associate and archive the road network data and its associated semantic point sets and task segment pairing tables, and provide road network switching, merging, and deletion operations. Specifically, in this embodiment of the invention, the persistence and lifecycle management module can be used to perform the following operations: Each topological road network is assigned a globally unique identifier, and an incremental detection strategy is used to generate non-conflicting identifiers. Maintain the currently selected road network identifier in persistent metadata; when switching, first verify the validity of the target identifier in the identifier set, and after the verification is successful, update the selected identifier and write it to disk so that the previously selected road network can still be restored after the system restarts; After obtaining the global road network by calling the multi-road network fusion, a new unique identifier is assigned to the fusion result and written to disk. Atomic road networks can be retained or merged according to a strategy, and the fusion relationship is recorded in the metadata. When deleting, the storage directory and the identifier-name mapping entry corresponding to the identifier are also removed; if the deleted identifier happens to be the currently selected road network, the metadata of the selected identifier is cleared simultaneously.

[0041] A robot motion control module is used to receive the complete global navigation path and perform motion control and trajectory tracking.

[0042] The robot autonomous navigation global topology map construction and management system provided in this invention uses the minimum spatial resolution distance criterion to control node generation, which is equivalent to applying voxel grid constraints to continuous pose trajectories to achieve an approximately uniform distribution of nodes. It combines IKD-Tree incremental redundancy removal, Euclidean weighting for automatic edge connection and temporal adjacency edge construction, and supports geometric registration and splicing fusion of multiple sub-networks to construct a single connected and geometrically consistent global topology network covering the entire traversable space. This supports optimal path planning covering the entire journey from the start point to the end point, which is different from local methods that only make decisions within the neighborhood window. Thus, it achieves global network construction without manual annotation, with physically interpretable edge weights, and progressive expansion capability.

[0043] Using real-time pose as the reference navigation point and mission point, six degrees of freedom pose is provided, which is then projected via IKD-Tree nearest neighbor. Anchor to road network nodes and determine ownership according to adsorption threshold, issue off-network alarms, classify and construct key index sets, and construct a task segment pairing table that is semantically decoupled from task codes and is scalable.

[0044] Reorganize the A* output path and create a new temporary KD-Tree. Return the task point to the nearest point on the path to complete the binding, ensuring that the task trigger point is geometrically strictly on the driving trajectory, and calculate the azimuth of each path point based on the difference between adjacent points.

[0045] Pair mapping Define task segment semantics, and adopt ordered nearest-neighbor matching by taking the nearest available pair from the end of the list to be matched (supporting task segment intersection and nesting), and use segment offset. Construct a hierarchical interval encoding This enables unambiguous decoding of task categories using only a single integer field.

[0046] It generates globally unique identifiers using incremental probing, atomically archives road network nodes / edges and all their semantic auxiliary data (navigation points, task points, task segment pairing tables), and provides consistent lifecycle management for switching, merging, and deleting.

[0047] The robot autonomous navigation global topology map construction and management system, which consists of the above-mentioned positioning input, five core functional modules and motion control, takes the normalization of multi-source poses to the same global world coordinate system as a unified benchmark. Its features are global coordinate consistency, synchronous mapping-retrieval structure, strict consistency of path-task geometry, end-to-end collaboration of paired readable and triggerable task segments, and the formation of a positioning-planning-execution closed loop through motion control pose feedback.

[0048] The following is a detailed description of the robot autonomous navigation global topology map construction and management system provided in the embodiments of the present invention.

[0049] The global topology map construction and management system provided by this invention includes five core components: topology network construction, semantic point construction, semantic point fusion planning, task segment pairing and encoding, and persistence and lifecycle management. The following details the system and method steps.

[0050] The system of the present invention is as follows Figure 1 As shown, it consists of a positioning input module (pre-input), a robot motion control module (post-execution), and five functional modules corresponding to the five core components mentioned above. The module composition and data connection relationships are as follows: The positioning input module (pre-input) receives real-time pose data from positioning modules (SLAM, RTK-IMU, or spatial positioning modules). Regardless of the data source, it normalizes the data to the same global world coordinate system through pre-calibrated rigid body parameter transformation, providing real-time pose data in a unified global reference system for subsequent modules. This serves as the data source for topology network construction and fusion planning. Each positioning source can be used independently or fused from multiple sources, and the external interface remains consistent with the coordinate reference. Topology network construction module: It contains a graph structure node set, a global set of adjacency relationships, and an incremental KD-Tree (IKD-Tree) maintained synchronously with it; it discretizes real-time poses into nodes based on spatial resolution criteria, automatically assigns weights to edges, and inserts nodes into the IKD-Tree synchronously, and provides an integrated nearest neighbor retrieval capability of "graph construction-retrieval" to the outside world; Semantic point construction module: Using pose data, it constructs navigation point sets and task point sets respectively using key indexing, and maintains a task segment pairing table; it uses the IKD-Tree of the topology road network construction module to complete the nearest neighbor projection and attribution determination of each semantic point to the road network node; Semantic point fusion planning module: Using the A* search algorithm, it receives the indexes of the start and end nodes after adsorption, searches the global road network to obtain the shortest topological path; it reorganizes the path and constructs a temporary KD-Tree, projects the task points to the nearest point on the path, and calculates the azimuth of each path point. Task segment pairing and encoding module: Based on the pairing table, the ordered task sequence output by the fusion planning module is paired and decoded, and the complete global navigation path with task identifier and ordered pairs is output and input to the robot motion control. The persistence and lifecycle management module is responsible for reading, writing, and strongly associated archiving of the topology network, each semantic point set, and selected map identifiers on the storage medium, and provides unique identifier management and lifecycle operations such as adding, switching, merging, and deleting. Robot motion control module (post-execution): Receives the complete global navigation path with task identifier and azimuth angle output from the core functional module, and uses it as reference input for motion control and trajectory tracking.

[0051] Topology network construction method: This invention employs an incremental approach to road network construction based on pose trajectory. It assumes that the robot's motion trajectory within the passable space is a continuous pose sequence. A topological road network is defined as an undirected weighted graph. , where the set of nodes Edge set Weighting function The goal of network construction is to discretize continuous pose flow into a topological network with minimal redundancy while ensuring complete spatial coverage.

[0052] Step S1 (Node generation based on spatial resolution constraints): Define the minimum node interval (spatial resolution). 。 For the new position Only if its minimum spatial distance to the current set of nodes is not less than A new node is generated only when the node generation criterion is met:

[0053] minimum distance Nearest neighbor retrieval by IKD-Tree The complexity is obtained. This criterion is equivalent to applying a time complexity of 1 / 2 to a continuous trajectory. The spatial voxel grid constraint with radius makes the nodes approximately uniformly distributed in space, fundamentally suppressing node redundancy caused by dense trajectory sampling.

[0054] Step S2 (Automatic edge connection based on temporal adjacency): Connect nodes generated sequentially at adjacent times. Weighted by three-dimensional Euclidean distance and with undirected edges established:

[0055] Step S3 (Incremental Synchronization): Each time a node is generated, it is synchronously inserted into the IKD-Tree, ensuring a consistent graph construction and retrieval structure and supporting an online working mode of simultaneous data collection and planning. Thus, a global road network with consistent topology, controllable node density, and physically interpretable edge weights can be automatically obtained without any manual annotation. A complete flowchart is shown below. Figure 2 As shown.

[0056] Step S4 (Multi-network fusion): For engineering requirements that necessitate partitioning, segmentation, and time-based data acquisition in large-scale scenes, this invention can integrate several sub-networks. Merged into a unified global road network The fusion process, conducted in a unified coordinate system, involves three steps. First, node union and deduplication: the union of nodes from each sub-network is taken, and nodes with spatial distances less than the fusion threshold are deduplicated. Merge the nodes in the same position to obtain the deduplicated global node set:

[0057] And establish an index mapping from each sub-network node to the global node. Secondly, edge remapping and weight recalculation: The edges of each sub-network are rewritten to the global nodes via index mapping, and the Euclidean weights are recalculated. Self-loops and duplicate edges are also removed.

[0058]

[0059] Third, connectivity bridging, for merged sub-networks with spatial spacing less than the bridging threshold. The neighboring nodes are supplemented with connecting edges, so that the originally independently collected sub-networks are automatically connected at overlapping or adjacent points:

[0060] After the fusion is completed, The IKD-Tree is reconstructed as a whole, thereby enabling segmented mapping and overall planning, so that the global road network can maintain topological connectivity and geometric consistency as the collection range expands progressively.

[0061] Semantic point construction methods: This step is responsible for generating, spatially anchoring, and organizing two types of semantic points: navigation points and task points. It also constructs a task segment pairing table to provide semantic input for fusion planning. The construction method is as follows.

[0062] Step S1 (Semantic Point Acquisition and Pose Anchoring): During road network recording or interactive annotation, based on the real-time pose input by the positioning input module, a six-degree-of-freedom pose is provided for each semantic point. And the nearest neighbor projection is performed by the IKD-Tree of the topology road network construction module. This involves anchoring the data to road network nodes and determining its location within the road network.

[0063] And set an adsorption threshold. Determine whether a semantic point effectively falls near the road network; when the projection distance... It provides off-network alarms in a timely manner to ensure that all constructed semantic points are effectively located within the neighborhood of the passable road network.

[0064] Step S2 (Classification and Construction of Two Types of Semantic Points): Navigation points are indexed by keywords and can specify the navigation destination pose, serving as the endpoint source for path searching; task points carry task identifier codes and speed levels, marking locations where the robot needs to perform specific actions (such as traffic lights, turnstiles, steps, stairs, manhole covers, hazard signs, etc.). The two types of semantic points are independently organized as sets of key indexes, without interfering with each other and can be independently added, deleted, modified, and queried.

[0065] Step S3 (Task Segment Pairing Table Construction): For interval-type tasks that need to be triggered in pairs, construct a task segment pairing table defined by entry code → exit code key-value pairs. This table describes the semantic correspondence between the entry and exit of each task segment, providing the task segment pairing and encoding module with the basis for performing nearest-neighbor pairing and decodeable encoding. This pairing table is decoupled from the task code semantics, allowing for the expansion of task segment categories as needed without affecting existing logic. The entire set of semantic points and the task segment pairing table are archived and managed by the persistence module along with the associated topology network. A schematic diagram of the semantic point construction process is shown below. Figure 3 As shown.

[0066] Semantic point fusion planning method: To address the shortcomings of the disconnect between road network and task allocation, this invention proposes a KD-Tree-based fusion binding method that ensures task points strictly fall within the planned path. The steps are as follows: Step S1 (Start and End Point Snapping): Snap the real-time pose and the target navigation point to the node via the nearest neighbor projection of the road network IKD-Tree, respectively, to obtain the start point index. With Endpoint Index .

[0067] Step S2 (A* Global Search): Perform an A* search on the global road network to obtain the node sequence. The heuristic function uses three-dimensional Euclidean distance. Updated to:

[0068] Step S3 (Path Reorganization and Secondary Tree Construction): ... The node coordinates, together with the target navigation point, are recombined into a path point sequence, and a temporary KD-Tree is created for this sequence.

[0069] Step S4 (Task Point Redirect Binding): For each task point, retrieve its nearest point on the path on the temporary KD-Tree; when the distance is less than the binding threshold, bind the task identifier to the path point index and establish a path index to task mapping. This ensures that the task trigger point is geometrically strictly located on the driving trajectory, fundamentally eliminating landing point deviation.

[0070] Step S5 (azimuth calculation): For the waypoints of the bound task, the starting task heading of the task segment is directly recorded using the task point, and the ending task heading of the task segment is calculated using the difference between adjacent points:

[0071] For the first point of the path , The calculation is based on the difference between the real-time pose and the initial position, ensuring the correct starting orientation. Ultimately, this meets the requirements of actual use.

[0072] Task segment pairing and encoding method To address the drawback of interval-type tasks not being able to be triggered in pairs in an orderly manner, this invention proposes a task segment pairing model and a decodable task encoding method, which organizes discrete semantic actions on the path into paired, orderly, and decodable task flows.

[0073] (1) Task segment pairing model: Define the task identifier space and divide it into two categories: point tasks (instantaneous actions triggered independently) and segment tasks (interval actions that need to be triggered in pairs). The set of point tasks is used... The segment task is represented by a pairing map. Define the mapping between its entry code and exit code:

[0074] in, To enter the task code set, This is a set of valid exit codes.

[0075] (2) Ordered proximity pairing algorithm: Let the task sequence arranged in order along the path be . The corresponding path indices are in ascending order. The algorithm maintains an ordered stack-like list of tasks to be matched. Scan from front to back: Point-to-point task If so, then keep it directly; For segmented tasks, from the list The tail moves forward and back, when And look for matching relationships Recent tasks If it hits, it will retain the same value. And Remove :

[0076] like Then push into the list Awaiting further pairing.

[0077] Because it uses a backtracking method to obtain the nearest matching task, this model not only strictly supports stack-style matching, but also directly supports the intersection and nesting of task segments, overcoming the limitation of traditional first-in-last-out stacks in being unable to handle intersection segments.

[0078] Finally, if there are unpaired isolated pairs They will be discarded to ensure the paired integrity of the output task flow.

[0079] (3) Decodeable task encoding: For tasks determined to be segment exits, add a segment offset. Encode the code, keeping the original code for incoming tasks:

[0080] according to The range of values ​​can be used to decode the task category without ambiguity.

[0081] (4) Backfilling the output in the original order: Backfill all the codes and azimuths of the retained tasks in the original index order of the path to obtain a complete navigation path with task identifiers, which can be used by the motion control terminal to trigger actions in sequence, in pairs, and in a decodeable manner. Thus, the present invention realizes paired triggering of task segments, cross-segment sequencing, cross-nesting support and decodeable encoding, and ensures the correctness and reliability of actions in high-risk segments from the model level.

[0082] Persistence and Lifecycle Management Methods To address the engineering management needs of road network data, this invention provides a method for persistent topology maps and lifecycle management for storage media, enabling strong association persistence, unique identifier management, and lifecycle operations such as construction, switching, merging, and deletion of topology road networks, semantic points, and other data.

[0083] (1) Unique Identifier and Strong Association Persistence: Assign a globally unique identifier to each topological road network. An incremental probing strategy is used to generate conflict-free identifiers:

[0084] in This is the set of occupied identifiers. A strong association is established on the storage medium using an identifier-name mapping file and an identifier-data directory: the set of nodes, the set of edges, and their associated navigation point / task point sets, as well as the task segment pairing table of the topological road network, are uniformly archived in a storage directory named after this identifier, ensuring that the road network data and all its semantic auxiliary data coexist atomically and traceably.

[0085] (2) Selection and switching: Maintain the currently selected road network identifier in persistent metadata; when switching, first verify the validity of the target identifier in the identifier set. After the verification is passed, update the selected identifier and write it to disk so that the previously selected road network can still be restored after the system restarts.

[0086] (3) Merging and association: After calling the multi-network fusion to obtain the global network, a new unique identifier is assigned to the fusion result and stored on the disk. Atomic networks can be retained or merged according to the strategy. The fusion relationship is recorded in the metadata to ensure that the fusion operation is traceable and can be rolled back.

[0087] (4) Delete association. When deleting, the storage directory and the identifier-name mapping item corresponding to the identifier are also removed. If the deleted one is the currently selected road network, the metadata of the selected identifier is cleared at the same time to avoid dangling references and ensure the consistency of the storage medium status. Thus, the topology road network obtains unique identifier, strong association and consistent management of the entire life cycle (establishment, loading, switching, merging and deletion) on the storage medium.

[0088] It is understood that, for the technical solution provided by this invention, there are the following alternative implementation methods that can achieve the same purpose: Search structure replacement: The IKD-Tree used for nearest neighbor retrieval can be replaced with Octree, R-Tree or VP-Tree without changing the main process of "automatic network construction - fusion binding - pairwise ordering".

[0089] Search algorithm alternatives: A* can be replaced with Dijkstra's algorithm, bidirectional A*, Theta*, or JPS with jump points; heuristic function. The Euclidean distance can be replaced by a weighted Euclidean distance or a diagonal distance.

[0090] Task coding substitution: segment offset The interval encoding can be replaced with an independent Boolean flag, a highest-order flag, or an independent enumeration field to distinguish between entry and exit actions.

[0091] Alternative pairing strategies: Ordered proximity matching can degenerate into strict stacked pairing in scenarios where overlap is not required; Pairing mapping It can be expanded from a static table into configuration items that can be edited online and loaded by road network.

[0092] Replacement of network construction criteria: Spatial resolution criteria can be determined by a fixed threshold. Replace with an adaptive threshold (dynamically adjusted based on curvature or velocity) to achieve scene-adaptive distribution of node density.

[0093] Alternative positioning sources: Positioning input can be provided by SLAM, RTK-IMU or other global spatial positioning modules, or multi-source fusion can be used without changing the subsequent network construction and planning process.

[0094] In summary, the robot autonomous navigation global topology map construction and management system provided by this invention achieves global optimization and global consistency: a full-range search from the starting point to the end point is performed using A* on a single connected road network covering the entire traversable space, and the resulting path is globally (near) optimal, fundamentally avoiding detours and dead ends caused by limited vision in local planning; multi-source / multi-segment fusion maintains global topology consistency, and task point return binding ensures strict consistency of semantics and geometry globally, thereby achieving global coordination of the entire path and the arrangement of tasks along the way.

[0095] Construction costs are significantly reduced and efficiency is improved: Incremental automatic mesh construction based on pose trajectory eliminates manual annotation. Combined with spatial resolution criteria and IKD-Tree redundancy removal, node density is controllable and connection relationships are reliable. The construction cycle is reduced from hours to on-the-go construction.

[0096] Highly efficient retrieval and planning: IKD-Tree is used to realize dynamic addition and deletion of nodes and O(log n) nearest neighbor retrieval, which significantly reduces memory usage and retrieval time compared with grid traversal, and is suitable for large-scale environments.

[0097] Large-scale scalability: Multi-network geometric registration and stitching fusion support segmented mapping and overall planning. The network can be gradually expanded as the collection range expands while maintaining topological connectivity and geometric consistency.

[0098] Improved task triggering accuracy: The KD-Tree return binding method ensures that the task point falls strictly on the planned path, eliminating the deviation of the landing point geometrically and significantly reducing missed triggers and false triggers.

[0099] Enhanced safety in high-risk sections: The task segment pairing model, ordered proximity matching, and decodeable encoding enable paired, ordered, and decodeable triggering of tasks within a section, and support for segment cross-nesting, thereby improving the correctness of actions and driving safety in complex scenarios.

[0100] The project boasts strong maintainability: unified semantic point construction and unique identifiers for storage media, strong association persistence, and complete lifecycle management. It supports switching, merging, adding, deleting, and modifying multiple networks, facilitating large-scale deployment.

[0101] Information that helps in understanding this invention includes: Main data flow: localization input (SLAM / RTK-IMU / spatial localization) pose → topology network construction (graph structure + IKD-Tree) → semantic point construction (pose anchoring + pairing table) → origin and destination snapping → A* planning → path reorganization + secondary tree construction → task feedback binding → task segment pairing and encoding (segment offset Ω) → azimuth angle calculation → task-oriented path → motion control (pose feedback closed loop); network and semantic data are archived and managed for lifecycle by the persistence module.

[0102] Key model parameters: Topological road network Nearest neighbor projection Pairing mapping Task attribution criteria and segment offset Fusion threshold Bridging threshold .

[0103] Encoding range parameter implementation example conventions: For entry into / normal tasks, For point tasks, This is to exit the task.

[0104] Definitions of abbreviations and key terms

[0105] It should be noted that, in this document, relational terms such as "first" and "second" are used only to distinguish one entity or operation from another, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Furthermore, the terms "comprising," "including," or any other variations thereof are intended to cover non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements includes not only those elements but also other elements not expressly listed, or elements inherent to such a process, method, article, or apparatus. Without further limitations, an element defined by the phrase "comprising one..." does not exclude the presence of other identical elements in the process, method, article, or apparatus that includes said element.

[0106] As can be seen from the above description of the embodiments, those skilled in the art can clearly understand that the present invention can be implemented by means of software plus necessary general-purpose hardware platforms. Based on this understanding, the technical solution of the present invention, or the part that contributes to the prior art, can be embodied in the form of a software product. This computer software product can be stored in a storage medium, such as ROM / RAM, magnetic disk, optical disk, etc., and includes several instructions to cause a computer device (which may be a personal computer, server, or network device, etc.) to execute the methods described in various embodiments or some parts of the embodiments of the present invention.

[0107] The various embodiments in this specification are described in a progressive manner. Similar or identical parts between embodiments can be referred to mutually. Each embodiment focuses on describing the differences from other embodiments. In particular, for system or system embodiments, since they are basically similar to method embodiments, the description is relatively simple, and relevant parts can be referred to the descriptions in the method embodiments. The systems and system embodiments described above are merely illustrative. The units described as separate components may or may not be physically separate. The components shown as units may or may not be physical units; that is, they may be located in one place or distributed across multiple network units. Some or all of the modules can be selected to achieve the purpose of this embodiment according to actual needs. Those skilled in the art can understand and implement this without creative effort.

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

Claims

1. A robot autonomous navigation global topology map construction and management system, characterized in that, include: The positioning input module is used to output the real-time pose from at least one positioning source after normalizing it to the global world coordinate system through a pre-calibrated rigid body external parameter transformation. The topology network construction module is used to receive the real-time pose, discretize the continuous pose flow into a set of nodes based on the spatial resolution criterion, establish an edge set and Euclidean distance weight based on the temporal adjacency relationship, and maintain an incremental IKD-Tree to provide nearest neighbor retrieval. The semantic point construction module is used to construct a navigation point set and a task point set, and to project the semantic points to the nearest road network node through the IKD-Tree to complete the pose anchoring. At the same time, it maintains a task segment pairing table to provide semantic input for fusion planning. The task segment pairing table is used to define the mapping relationship between entry task code and exit task code. The semantic point fusion planning module is used to attach the starting point and the ending point to the road network node, search for the shortest topological path on the global road network, reorganize the path points and construct a temporary KD-Tree, bind the task point to the nearest point on the path, and calculate the azimuth angle of the path point. The task segment pairing and encoding module is used to perform nearest pairing on the ordered task sequence on the path according to the task segment pairing table, and to encode the offset of the exiting task overlay segment of the successfully paired task, and output a complete global navigation path with task identifier and azimuth angle. The persistence and lifecycle management module is used to assign a globally unique identifier to the topology network, archive the network data and its associated semantic point set and task segment pairing table in a strong association, and provide network switching, merging and deletion operations. A robot motion control module is used to receive the complete global navigation path and perform motion control and trajectory tracking.

2. The robot autonomous navigation global topology map construction and management system according to claim 1, characterized in that, The topology network construction module constructs the network incrementally based on pose trajectories, and the construction method includes: Define the minimum interval between nodes. For a new pose, generate a new node only if the minimum spatial distance between the new pose and the current set of nodes is not less than the minimum interval. For nodes generated sequentially at adjacent time points, weights are assigned using three-dimensional Euclidean distance and undirected edges are established; Each time a node is generated, it is synchronously inserted into the IKD-Tree to keep the graph construction and retrieval structure consistent.

3. The robot autonomous navigation global topology map construction and management system according to claim 2, characterized in that, Also includes: Take the union of the nodes of each sub-road network, and merge the nodes of the same position whose spatial distance is less than the fusion threshold to obtain the deduplicated global node set, and establish the index mapping from each sub-road network node to the global node. The edge index mapping of each sub-network is rewritten to the global node, and the Euclidean weights are recalculated, while self-loops and repeated edges are removed. For adjacent node pairs located in different sub-networks after merging and with a spatial spacing less than the bridging threshold, supplementary connection edges are added to enable the originally independently collected sub-networks to automatically connect at overlapping or adjacent points, thereby achieving multi-network fusion. After the integration is completed, the IKD-Tree will be reconstructed as a whole for the unified global road network.

4. The robot autonomous navigation global topology map construction and management system according to claim 1, characterized in that, The construction method of the semantic point construction module includes: During the road network recording or interactive annotation process, the real-time pose input by the positioning input module is used as a reference to provide a six-degree-of-freedom pose for each semantic point. The nearest neighbor projection is then performed by the IKD-Tree of the topology road network construction module to anchor the projected point to the road network node and determine its belonging position on the road network. The navigation point set and the task point set are constructed and obtained respectively in the form of a set of key indexes; the navigation points are indexed by keywords and can specify the navigation destination pose, serving as the endpoint source for path search; the task points carry task identifier codes and speed levels, marking the positions where the robot needs to perform specific actions; For interval-type tasks that require pairing, a task segment pairing table is constructed, defined by key-value pairs of entry and exit codes, to describe the semantic correspondence between the entry and exit of each task segment, so that the task segment pairing and encoding module can perform nearest pairing and decodeable encoding.

5. The robot autonomous navigation global topology map construction and management system according to claim 4, characterized in that, An adsorption threshold is set to determine whether a semantic point is effectively located near the road network. When the projection distance is greater than the adsorption threshold, an off-network alarm is given to ensure that all constructed semantic points are effectively located within the neighborhood of the passable road network.

6. The robot autonomous navigation global topology map construction and management system according to claim 1, characterized in that, The fusion planning method of the semantic point fusion planning module includes: The real-time pose and the target navigation point are respectively snapped to the node through the nearest neighbor projection of the road network IKD-Tree to obtain the start index and the end index; A node sequence is obtained by performing an A* search on the global road network, with the three-dimensional Euclidean distance as the heuristic function. The node coordinates of the node sequence are recombined with the target navigation point to form a path point sequence, and a temporary KD-Tree is created for this sequence; For each task point, retrieve its nearest point on the path on the temporary KD-Tree; when the distance is less than the binding threshold, bind the task identifier to the corresponding path point index and establish a path index to task mapping; For path points of a bound task, the starting task heading of the task segment is directly recorded using the task point, and the ending task heading of the task segment is calculated by the difference between adjacent points.

7. The robot autonomous navigation global topology map construction and management system according to claim 1, characterized in that, The task pairing and encoding method of the task segment pairing and encoding module includes: Define a task identifier space, which includes point tasks and segment tasks; Let the ordered sequence of tasks along the path be... The corresponding path indices are in ascending order; the algorithm maintains an ordered stack-like list of tasks to be matched. Scan from front to back: Point-to-point task If so, then keep it directly. Represents a set of point tasks; For segmented tasks, from the list The tail moves forward and back, when And look for matching relationships Recent tasks If it hits, it will retain the same value. And Remove : like Then push into the list Awaiting further pairings; This indicates entry into the task code set. Represents the set of valid exit codes; For tasks that are determined to exit a segment, an additional segment offset is added for encoding, while incoming tasks retain their original code. The codes and azimuths of all retained tasks are backfilled in the original path index order to obtain a complete navigation path with task identifiers, which can be used by the motion control terminal to trigger actions in sequence, in pairs, and in a decodeable manner.

8. The robot autonomous navigation global topology map construction and management system according to claim 1, characterized in that, The persistence and lifecycle management module is used to perform the following operations: Each topological road network is assigned a globally unique identifier, and an incremental detection strategy is used to generate non-conflicting identifiers. Maintain the currently selected road network identifier in persistent metadata; when switching, first verify the validity of the target identifier in the identifier set, and after the verification is successful, update the selected identifier and write it to disk so that the previously selected road network can still be restored after the system restarts; After obtaining the global road network by calling the multi-road network fusion, a new unique identifier is assigned to the fusion result and stored on disk. Atomic road networks can be retained or merged according to a strategy, and the fusion relationship is recorded in the metadata. When deleting, the storage directory corresponding to the identifier and the identifier-name mapping entry are also removed. If the deleted network is the currently selected road network, the selected identifier metadata will be cleared simultaneously.

9. The robot autonomous navigation global topology map construction and management system according to claim 1, characterized in that, The positioning source includes a single positioning source or a multi-source fusion positioning source, and its external interface is consistent with the coordinate reference.

Images