A scene graph semantic navigation method for dynamic outdoor environment

Abstract

The application discloses a scene graph semantic navigation method for a dynamic outdoor environment, comprising the following steps: S1, constructing a multi-level embodied graph, wherein the multi-level embodied graph comprises multiple types of nodes connected by hierarchical clustering relationships; S2, receiving a natural language instruction, and performing hierarchical semantic retrieval and reasoning on the natural language instruction and node descriptions of the multi-level embodied graph to determine a target node corresponding to the natural language instruction from the multi-level embodied graph; S3, acquiring a current robot self node, and performing global path planning according to the target node and the current robot self node; S4, acquiring local real-time perception data, and performing local trajectory planning and safety control decision based on the planned global path and the local real-time perception data to generate a control instruction. The application can realize semantic navigation guided by a natural language instruction in a large-scale dynamic outdoor environment.

Description

Technical Field

[0001] This invention relates to the field of robot navigation technology, and in particular to a scene graph semantic navigation method for dynamic outdoor environments. Background Technology

[0002] In complex outdoor environments such as urban roads, campuses, and industrial parks, mobile robots (such as unmanned shuttle buses, delivery robots, and patrol robots) need to perform long-distance, multi-target navigation tasks amidst numerous dynamic targets (pedestrians, vehicles, bicycles) and environmental changes (construction, fencing, temporary road closures). Traditional navigation primarily addresses the geometric path planning problem of "from point A to point B," while real-world applications increasingly emphasize the understanding and locating of semantically meaningful locations, reasoning about open-ended linguistic targets, and the ability to remember and adapt to environmental changes during long-term operation.

[0003] Most existing outdoor navigation methods adopt a modular architecture: they use GNSS (Global Navigation Satellite System) / RTK (Real-Time Kinematic) and SLAM (Simultaneous Localization and Mapping) to complete positioning and mapping, rely on HD (High Definition) maps for global path planning, and use laser / visual obstacle avoidance in local areas. Such methods have good interpretability and engineering maturity, but generally have the following limitations: (1) they rely heavily on detailed static maps, have high costs for early surveying and maintenance, and are not sensitive to environmental changes; (2) they have weak semantic understanding capabilities, cannot directly understand natural language instructions, and can only navigate between predefined "coordinate points"; (3) they lack a unified representation and retrieval capability for open vocabulary targets.

[0004] On the other hand, in recent years, a large number of deep learning-based visual navigation and visual-language navigation (VLN) methods have emerged, attempting to directly output control commands through end-to-end policy networks. Although these methods have achieved good results in simulated environments, they still have the following problems: (1) they are highly dependent on large-scale training data and have difficulty generalizing to new environments; (2) they lack explicit topology and semantic structure, making it difficult to perform long-term planning and interpretable decision-making; and (3) they are not sufficiently validated in real, large-scale, dynamic outdoor scenes.

[0005] Recently, semantic navigation methods based on scene graphs have made progress in indoor scenes, such as Hydra, VLMaps (Visual Language Maps), and LExis, which integrate 3D geometry and semantics into a structured graph for path planning and semantic retrieval. However, these methods are mostly limited to static indoor environments and are difficult to apply directly to larger-scale, more dynamic outdoor scenes.

[0006] The above background information is provided only to aid in understanding the concept and technical solution of this invention. It does not necessarily belong to the prior art of this patent application. In the absence of clear evidence that the above information was disclosed on the filing date of this patent application, the above background information should not be used to evaluate the novelty and inventiveness of this application. Summary of the Invention

[0007] To address the aforementioned technical problems, this invention proposes a scene graph semantic navigation method for dynamic outdoor environments, which enables robots to reliably understand open-vocabulary natural language instructions and achieve safe, robust, and long-term autonomous navigation in large-scale, dynamic outdoor environments.

[0008] To achieve the above objectives, the present invention adopts the following technical solution:

[0009] In a first aspect, the present invention discloses a scene graph semantic navigation method for dynamic outdoor environments, comprising the following steps:

[0010] S1: Construct a multi-level embodied graph, which includes nodes of various types associated through hierarchical clustering relationships;

[0011] S2: Receive natural language instructions, and perform hierarchical semantic retrieval and reasoning on the natural language instructions and the node descriptions of the multi-level embodied graph, and determine the target node corresponding to the natural language instructions from the multi-level embodied graph;

[0012] S3: Obtain the current robot's own node, and perform global path planning based on the target node and the current robot's own node;

[0013] S4: Acquire local real-time sensing data, and based on the planned global path and the local real-time sensing data, perform local trajectory planning and safety control decisions to generate control commands.

[0014] Preferably, S1 specifically includes:

[0015] S11: Collect multimodal sensor data and perform time synchronization and spatial coordinate alignment processing on the modal sensor data;

[0016] S12: Based on the processed multimodal sensor data, construct the multi-level embodied graph, wherein the various types of nodes include nodes representing multi-level static environmental semantic information and nodes representing the robot's own historical trajectory information.

[0017] Preferably, the multimodal sensor data in S11 includes RGB images, LiDAR point clouds, and inertial and pose-aided information; S12 specifically includes:

[0018] S121: Based on the open vocabulary detection model, reason about the RGB image to obtain open vocabulary target detection results containing text description and segmentation mask, and perform temporal correlation of the open vocabulary target detection results through a multi-target tracking algorithm to form a target trajectory;

[0019] S122: Using camera intrinsic parameters and camera-radar extrinsic parameters, the lidar point cloud and the segmentation mask are aligned in 2D-3D to determine the position of the open vocabulary target detection result in three-dimensional space and generate object nodes;

[0020] S123: Generate cluster nodes by clustering multiple object nodes through spatial-semantic hierarchy;

[0021] S124: Obtain the current robot pose and offline map based on inertial and pose assistance information, and generate the robot's own trajectory nodes and building nodes representing the macroscopic structure;

[0022] S125: The building nodes, object nodes, and clustering nodes representing multi-level static environmental semantic information, as well as the robot's own trajectory nodes representing the robot's own historical trajectory information, are fused to construct the multi-level embodied graph.

[0023] Preferably, S12 further includes the following steps in constructing the multi-level embodied atlas:

[0024] The motion trajectory of the dynamic target is obtained, and a spatiotemporal occupancy region representation of the dynamic target is constructed based on the motion trajectory. Nodes within the spatiotemporal occupancy region of the dynamic target are filtered out from various types of nodes representing multi-level static environmental semantic information according to the spatiotemporal occupancy region of the dynamic target.

[0025] Preferably, S2 specifically includes:

[0026] S21: Receive natural language commands;

[0027] S22: Calculate the semantic matching probability between the natural language instruction and the descriptions of nodes at different levels in the multi-level embodied graph based on the large language model;

[0028] S23: Based on semantic matching probability and hierarchical relationship between nodes, determine candidate nodes and calculate the overall path score from the current node to the candidate node;

[0029] S24: Determine the target node corresponding to the natural language instruction based on the overall path score.

[0030] Preferably, S3 specifically includes: obtaining the current robot's own node, executing a path search algorithm on the multi-level embodied graph, obtaining the path from the current robot's own node to the target node, and obtaining global path planning.

[0031] Preferably, S4 specifically includes:

[0032] S41: Acquire local real-time perception data, and construct a local feasible region containing real-time dynamic obstacle information based on the local real-time perception data;

[0033] S42: Generate a local reference trajectory based on the local feasible region and the planned global path;

[0034] S43: Based on the safety constraints of nonlinear model predictive control combined with the control barrier function, the local reference trajectory is tracked to generate control commands.

[0035] Preferably, S42 specifically includes: generating an initial path within the local feasible region using a planning algorithm based on the planned global path, and smoothing the initial path to generate the local reference trajectory;

[0036] S43 specifically includes: constructing an optimization problem of nonlinear model predictive control using the robot's own state and control input as variables, with the optimization objective being to track the local reference trajectory and minimize the control cost; and solving the optimization problem to obtain the optimal control sequence and extracting the control command at the current moment; wherein, a safety constraint represented in the form of a control barrier function is introduced into the optimization problem of the nonlinear model predictive control, and the safety constraint is used to ensure that the robot itself maintains a safe distance from the obstacle.

[0037] Preferably, during navigation, the multi-level embodied graph is continuously updated online. The online update includes: inserting newly observed static or quasi-static nodes into the multi-level embodied graph, updating the information of existing nodes, deleting nodes marked as dynamic, and periodically reconstructing hierarchical clustering relationships.

[0038] In a second aspect, the present invention discloses a computer-readable storage medium storing a computer program, wherein the computer program is configured to be run by a processor to perform the scene graph semantic navigation method for dynamic outdoor environments as described in the first aspect.

[0039] Compared with existing technologies, the beneficial effects of this invention are as follows: The scene graph semantic navigation method for dynamic outdoor environments disclosed in this invention first constructs a multi-level embodied graph including various types of nodes associated through hierarchical clustering relationships, and then performs hierarchical semantic retrieval and reasoning. This enables the robust parsing of natural language instructions into target nodes in the embodied graph to further obtain global paths. By combining global paths with local real-time perception data, the robustness, security, and practicality of semantic navigation guided by natural language instructions in large-scale dynamic outdoor environments are ensured.

[0040] In a further embodiment, the present invention also has the following beneficial effects:

[0041] (1) By constructing a multi-level embodied graph, offline maps, online perception, and robot historical trajectories are unified in a structured representation, realizing multi-scale fusion of environmental semantic information. This enables the robot to simultaneously understand macroscopic building structures and microscopic object details, thus providing rich contextual information for the semantic parsing of natural language instructions. Furthermore, the accuracy and robustness of environmental perception are improved through the fusion processing of multimodal sensor data; the identification and localization of unknown categories of objects are achieved through open vocabulary detection and 2D-3D alignment technology; and meaningful regional semantic nodes are generated through spatial-semantic hierarchical clustering.

[0042] (2) Based on the hierarchical semantic retrieval and reasoning mechanism of the large language model, the powerful semantic understanding capability of the language model is used to perform multi-level matching of natural language instructions of open vocabulary with node descriptions in the embodied graph. By calculating the semantic matching probability and the overall path score, the fuzzy language instructions can be accurately mapped to specific navigation target nodes, thus solving the problem that traditional navigation systems cannot understand natural language instructions.

[0043] (3) By using the spatiotemporal occupancy area representation and filtering mechanism of dynamic targets, short-term dynamic targets are removed from the long-term static map, while maintaining real-time perception of dynamic obstacles in local planning. This ensures both the stability of the long-term environmental representation and the safety of instant navigation, effectively improving the robustness and adaptability of navigation in dynamic outdoor environments.

[0044] (4) The global-local hierarchical planning framework combines nonlinear model predictive control and safety constraints of control barrier functions to perform high-level semantic path planning on the embodied graph and generate safe trajectories using real-time perception data locally. This achieves a balance between long-range task execution and immediate safe obstacle avoidance, improving the overall performance and reliability of the navigation system. Furthermore, efficient path planning on the semantic graph is achieved through path search algorithms; safe navigation in dynamic environments is ensured through local feasible region construction and trajectory optimization; and safe distances between the robot and obstacles are guaranteed through optimized control of safety constraints.

[0045] (5) The online update mechanism of the embodied graph enables the system to continuously adapt to environmental changes. By inserting new nodes, updating existing information, deleting dynamic nodes and reconstructing hierarchical relationships, the system maintains the timeliness and accuracy of environmental representation, and supports the robot's autonomous learning and environmental adaptation during long-term operation.

[0046] In summary, this invention significantly improves the practicality, robustness, and environmental adaptability of mobile robots performing long-term semantic navigation tasks based on natural language in complex and dynamic outdoor environments.

[0047] Other beneficial effects of the embodiments of the present invention will be further described below. Attached Figure Description

[0048] Figure 1 This is a flowchart of a scene graph semantic navigation method for dynamic outdoor environments disclosed in a preferred embodiment of the present invention;

[0049] Figure 2 This is a flowchart of a scene graph semantic navigation method for dynamic outdoor environments disclosed in a preferred embodiment of the present invention. Detailed Implementation

[0050] The embodiments of the present invention will be described in detail below. It should be emphasized that the following description is merely exemplary and not intended to limit the scope and application of the present invention.

[0051] It should be noted that when a component is referred to as "fixed to" or "set on" another component, it can be directly on or indirectly on that other component. When a component is referred to as "connected to" another component, it can be directly connected to or indirectly connected to that other component. Furthermore, a connection can be used for both fixing and circuit / signal connectivity.

[0052] It should be understood that the terms "length", "width", "up", "down", "front", "back", "left", "right", "vertical", "horizontal", "top", "bottom", "inner", and "outer" indicate the orientation or positional relationship based on the orientation or positional relationship shown in the accompanying drawings. They are only for the convenience of describing the embodiments of the present invention and simplifying the description, and do not indicate or imply that the device or element referred to must have a specific orientation, or be constructed and operated in a specific orientation. Therefore, they should not be construed as limitations on the present invention.

[0053] Furthermore, the terms "first" and "second" are used for descriptive purposes only and should not be construed as indicating or implying relative importance or implicitly specifying the number of technical features indicated. Thus, a feature defined as "first" or "second" may explicitly or implicitly include one or more of that feature. In the description of embodiments of the present invention, "a plurality of" means two or more, unless otherwise explicitly specified.

[0054] For ease of understanding, the main abbreviations and key terms involved in this invention are explained below:

[0055] (1) VLN (Vision-Language Navigation): refers to the task and method of robot navigation under the joint constraints of visual perception and natural language instructions.

[0056] (2) SLAM (Simultaneous Localization and Mapping): refers to the technology of a mobile platform estimating its own pose and building an environmental map in an unknown environment at the same time.

[0057] (3) HD Map (High Definition Map): refers to a high-precision map containing detailed geometric and semantic information such as lanes, curbs, and traffic signs. Traditional autonomous driving systems often rely on this type of map for navigation.

[0058] (4) Embodied Graph: refers to the representation of a robot's "embodied experience" in a real environment as a multi-level scene graph structure. In this invention, the embodied graph consists of building nodes, clustering nodes (region nodes), object nodes, and vehicle trajectory nodes, which uniformly represent offline map data and online perception results.

[0059] (5) LLM (Large Language Model): refers to a language model based on Transformer and trained on a large-scale corpus, such as the LLaMA series and Qwen series. In this invention, it is mainly used for semantic retrieval and text generation, rather than for traditional dialogue purposes.

[0060] (6) RAG (Retrieval-Augmented Generation): refers to combining the retrieval results of an external knowledge base (Embodied Graph in this invention) with LLM to improve the relevance and accuracy of the generated content to the current environment.

[0061] (7) RH-Map (Region-wise Hash Map): A 3D region hash map construction method used to quickly complete map building, dynamic object removal, and feasible region extraction in local space. In this invention, it is used for local planning and dynamic obstacle avoidance.

[0062] (8) NMPC (Nonlinear Model Predictive Control): refers to a class of methods that optimize the control input sequence for a future period of time within a finite prediction time domain, using the nonlinear model of the system as a constraint, so as to obtain the current optimal control output.

[0063] (9) CBF (Control Barrier Function): A function form used to explicitly ensure that the system state remains within a safe set during the control process. It is often combined with NMPC to form an optimization control framework with safety constraints.

[0064] (10) Embodied Navigation: refers to a navigation method in which a robot, in a real physical environment, combines its own historical trajectory, environmental semantics, offline maps and online perception to achieve long-term semantic understanding and task execution.

[0065] (11) Open-vocabulary detection / segmentation: refers to detection or segmentation models that are not limited to a fixed set of categories. They can detect targets in a scene based on any text description, such as "library", "trash can", "bus stop sign", etc.

[0066] (12) LIO (LiDAR-Inertial Odometry): refers to an algorithm framework that estimates the six-degree-of-freedom pose of a carrier in real time by fusing LiDAR point cloud data with inertial measurement unit (IMU) data.

[0067] like Figure 1As shown, a preferred embodiment of the present invention discloses a scene graph semantic navigation method for dynamic outdoor environments. This method achieves long-term outdoor semantic navigation through embodied graph + LLM semantic retrieval + dynamic filtering + global-local planning. The scene graph semantic navigation method for dynamic outdoor environments includes the following steps:

[0068] S1: Construct a multi-level embodied graph, which includes multiple types of nodes associated through hierarchical clustering relationships;

[0069] In this step S1, a multi-level embodied scene atlas that can be updated over a long period of time is constructed in a large-scale, dynamic outdoor environment, unifying offline maps, online perception, and robot historical trajectories in the same structure. Specifically, this includes:

[0070] S11: Acquire multimodal sensor data and perform time synchronization and spatial coordinate alignment processing on the modal sensor data;

[0071] The multimodal sensor data used in this step includes RGB images, LiDAR point clouds, and inertial and pose-aided information.

[0072] Specifically, refer to Figure 2 In this step, a color camera is used to acquire RGB image sequences. Point cloud sequences were acquired using 3D LiDAR. The system uses IMU (Inertial Measurement Unit) and GNSS (Global Navigation Satellite System) / RTK (Real-Time Kinematic) to collect inertial and pose-aided information; then, through timestamp synchronization and extrinsic parameter calibration, the data from each sensor are aligned to a unified time axis and a unified world coordinate system.

[0073] S12: Based on the processed multimodal sensor data, construct a multi-level embodied graph, in which various types of nodes are included, such as nodes representing multi-level static environmental semantic information and nodes representing the robot's own historical trajectory information.

[0074] Step S12 specifically includes:

[0075] S121: Based on the open vocabulary detection model, inference is performed on RGB images to obtain open vocabulary target detection results containing text descriptions and segmentation masks. The open vocabulary target detection results are then temporally correlated using a multi-target tracking algorithm to form target trajectories.

[0076] Specifically, refer to Figure 2Using open vocabulary detection models (such as the YOLO-World AI model for recognizing objects from images) to analyze RGB image sequences Perform inference to obtain open-vocabulary target detection results:

[0077] (1)

[0078] in, for t Time (the first) t A set of open-vocabulary object detection results corresponding to RGB images (frames). For the set of i An example of the detection results for a single target. For the set of i A textual description of the target. For the set of i Segmentation mask for each target, For the set of i The 2D bounding box of the target in the current RGB image, index i It means t The first time detected i Index of each target;

[0079] Multi-target tracking algorithms such as ByteTrack The detection results are correlated over time to form a stable target trajectory.

[0080] S122: Using camera intrinsic parameters and camera-radar extrinsic parameters, the lidar point cloud and segmentation mask are aligned in 2D-3D to determine the position of the open vocabulary target detection result in three-dimensional space and generate object nodes;

[0081] Specifically, using camera intrinsic parameters and camera-radar external parameters To use lidar point clouds Projected onto the image plane; and falling into the segmentation mask. Points within the object constitute the point cloud. ; then Fitting the minimum volume 3D bounding box By taking its centroid as the three-dimensional position of the object, the object nodes are obtained.

[0082] S123: Clustering multiple object nodes through spatial-semantic hierarchy to generate cluster nodes;

[0083] S124: Obtain the current robot pose and offline map based on inertial and pose assistance information, and generate the robot's own trajectory nodes and building nodes representing the macroscopic structure;

[0084] Specifically, LIO (LiDAR-Inertial Odometry) is used to obtain the robot's current pose. Transform the object pose from the radar coordinate system to the world coordinate system to obtain... When the robot's displacement exceeds the threshold At that time, record a new trajectory node of the robot itself. Furthermore, edges are added between adjacent trajectory nodes in the multi-level embodied graph to form a historical trajectory subgraph.

[0085] Before fusing the nodes to construct a multi-level embodied graph, the process includes obtaining the motion trajectory of the dynamic target, constructing a spatiotemporal occupancy region representation of the dynamic target based on the motion trajectory, and filtering out nodes in the spatiotemporal occupancy region from various types of nodes representing multi-level static environmental semantic information according to the spatiotemporal occupancy region of the dynamic target.

[0086] Specifically, dynamic objects are filtered in a space-time manner. First, the dynamic object trajectory and corridor are represented as follows:

[0087] For each detected target, its 3D bounding box sequence is collected within a time window:

[0088] (2)

[0089] In the formula, This is a set of sequences of "target identifier + 3D bounding box + timestamp" for a dynamic target within a specified time window. Let be the pose transformation matrix of the target in the world coordinate system. For this dynamic target within the time window i The 3D bounding box corresponding to each time point. For the corresponding timestamp;

[0090] Find the joint envelope of the sequence in three-dimensional space to form a space-time corridor extending along the time axis. This is used to characterize the area that the target may occupy in the near future.

[0091] Secondly, dynamic node removal and static graph construction are as follows: If the target displacement or number of steps exceeds the threshold... If an object is classified as a typical dynamic / quasi-dynamic object, its corresponding corridor region is removed from the long-term static map construction; object nodes falling into the corridor region are marked as dynamic nodes. It will no longer participate in the construction of static multi-level embodied graphs.

[0092] S125: Integrate building nodes, object nodes, and cluster nodes that represent multi-level static environmental semantic information, as well as robot trajectory nodes that represent the robot's own historical trajectory information, to construct a multi-level embodied graph.

[0093] Among them, building nodes Derived from offline maps, such as campus GIS (Geographic Information System) or open-source maps, including building names, functional descriptions, and geometric locations; object nodes. Data from online open vocabulary detection and 3D localization includes terms such as "library bench" and "bus stop sign"; clustering nodes. Using a spatial-semantic hierarchical clustering algorithm, several object nodes are aggregated into region nodes (e.g., "Information Building Front Plaza"); vehicle trajectory nodes Record the robot's historical key poses and motion information for subsequent planning and interpretation based on historical paths.

[0094] By jointly measuring spatial distance and semantic similarity Clustering of object nodes generates cluster nodes; Large Language Model (LLM) is used for each cluster. By summarizing the member node descriptions, a more abstract semantic description of the region can be obtained. The cluster nodes and building nodes are further aggregated to form a multi-level embodied graph from the building layer to the region layer and then to the object layer.

[0095] In other embodiments, the multi-level embodied graph is not limited to the four-layer structure of "building-region-object-trajectory". It can also employ a structure of "city block-road segment-point of interest-trajectory", "building-floor-functional area-object", or a hierarchical structure automatically learned by a graph neural network (GNN). Relationships between graph nodes can also be replaced by topological adjacency (e.g., connectivity), visibility relationships (e.g., FOV (Field of View) visible area), semantic similarity, traffic rule constraints, etc. These alternatives still achieve the goal of "multi-scale semantic structure + long-term memory".

[0096] S2: Receive natural language instructions and perform hierarchical semantic retrieval and reasoning on the natural language instructions and the node descriptions of the multi-level embodied graph to determine the target node corresponding to the natural language instructions from the multi-level embodied graph;

[0097] In this step S2, open vocabulary awareness and LLM-RAG are used to robustly parse human natural language instructions into target nodes or target regions in the embodied graph; specifically, this includes:

[0098] S21: Receive natural language commands;

[0099] S22: Calculate the semantic matching probability between natural language instructions and node descriptions at different levels in a multi-level embodied graph based on a large language model;

[0100] Specifically, refer to Figure 2 For each node Constructing text description This includes node names, functions, and relationships between adjacent nodes; using a Large Language Model (LLM) or text embedding model to... Convert to vectors for subsequent semantic similarity measurement. Given user natural language instructions. For example, "I want to borrow a storybook, please take me to a place where I can borrow books"; at each level of the multi-level embodiment map. Above, compute nodes through Large Language Model (LLM) Selection probability:

[0101] (3)

[0102] In the formula, For nodes In the query The probability of choice, hierarchical A candidate node in, For queries entered by the user (such as natural language navigation instructions). Indicates direct proportion. It is a natural exponential function. For temperature coefficient, For the inference function of the Large Language Model, For nodes Contextual description;

[0103] Path from top layer to target layer Calculate the overall score:

[0104] (4)

[0105] in, This represents a hierarchical node path from the top layer to the target layer. For path The top-level node in the middle, For path The underlying nodes in For path The Middle Hierarchical nodes, For path The overall score, The multiplication symbol is used. This indicates the validity of the relationship between nodes in different layers.

[0106] S23: Based on semantic matching probability and hierarchical relationship between nodes, determine candidate nodes and calculate the overall path score from the current node to the candidate node;

[0107] Specifically, if the approximate current position of the robot is known, then for nodes on the candidate path... Calculate the mixed score:

[0108] (5)

[0109] in, Candidate nodes The mixed score, Candidate nodes to be evaluated. These are the weighting coefficients for spatial similarity. For spatial similarity functions, This represents the current position of the robot.

[0110] S24: Determine the target node corresponding to the natural language instruction based on the overall path score.

[0111] Specifically, in this step, the node or path with the highest score is selected as the building / area / object where the navigation target is located.

[0112] In the above embodiments, the present invention employs a Large Language Model (LLM) to perform hierarchical scoring of node descriptions (C(n)) and instructions (q). In some other embodiments, it may also be based on a graph-structured retrieval enhancement model, a combination of a small LLM and a large vector database, or a "structured reasoning" scheme that directly trains a language model to generate target nodes from a graph structure. As long as the idea of ​​"using graph structures for hierarchical semantic retrieval" is still satisfied, it is within the scope of protection of this application.

[0113] S3: Obtain the current robot's own nodes, and perform global path planning based on the target node and the current robot's own nodes;

[0114] This step specifically includes: obtaining the current robot's own node, executing a path search algorithm on a multi-level embodied graph, obtaining the path from the current robot's own node to the target node, and obtaining global path planning.

[0115] Specifically, based on the target node obtained in step S2, algorithms such as Dijkstra are executed on the multi-level embodied graph to obtain the high-level path from the current robot's own node to the target node; if the target is not within the connected components of the historical trajectory, an architectural-level global route is generated using an offline road network or external map service, and then mapped back to the multi-level embodied graph.

[0116] S4: Acquire local real-time perception data, and based on the planned global path and local real-time perception data, perform local trajectory planning and safety control decisions to generate control commands.

[0117] Specifically, step S4 includes:

[0118] S41: Acquire local real-time perception data and construct a local feasible region containing real-time dynamic obstacle information based on the local real-time perception data;

[0119] Specifically, a local three-dimensional voxel map is constructed around the robot using LiDAR to remove dynamic obstacles and their afterimages in real time, thereby obtaining the local feasible region. As a constraint space for local trajectory planning.

[0120] S42: Generate a local reference trajectory based on the local feasible region and the planned global path;

[0121] Specifically, step S42 involves: based on the planned global path, generating an initial path within the local feasible region using a planning algorithm, and smoothing the initial path to generate a local reference trajectory.

[0122] Specifically, in the local feasible region The initial path is generated using sampling planning methods such as Informed-RRT* (an asymptotically optimal path planning algorithm that first quickly finds a feasible path and then 'shrinks' all subsequent samples into a potentially better ellipse). The path is smoothed using B-spline interpolation, and an orientation is assigned to each path point to obtain a directed local reference trajectory. .

[0123] S43: Based on nonlinear model predictive control combined with safety constraints of control barrier functions, it tracks the local reference trajectory and generates control commands.

[0124] Specifically, step S43 involves: constructing an optimization problem for nonlinear model predictive control using the robot's own state and control input as variables, with the optimization objective being to track the local reference trajectory and minimize the control cost; solving the optimization problem to obtain the optimal control sequence and extracting the control command at the current moment; wherein, a safety constraint, expressed in the form of a control barrier function, is introduced into the optimization problem of nonlinear model predictive control, and the safety constraint is used to ensure that the robot maintains a safe distance from the obstacle.

[0125] Specifically, based on the robot's own state and control input A nonlinear model predictive control (NMPC) optimization problem is constructed for the variables, with the objective of tracking the reference trajectory and minimizing the control cost. A safety constraint of the form of a control barrier function (CBF) is introduced to ensure a safe distance between the robot and obstacles. The optimal control sequence over a time period is obtained by solving the NMPC problem.

[0126] (6)

[0127] In the formula, the subscript N represents the prediction time domain length of NMPC.

[0128] Extract the current control input from it. Send to the robot's underlying controller.

[0129] In this embodiment, a hierarchical global-local planning framework is designed in steps S3 to S4, which enables the global path to be generated based on embodied maps and offline road information, while the local planning is based on dynamic maps and NMPC-CBF safety control, taking into account both long-term tasks and immediate safety.

[0130] In some embodiments, during navigation, the multi-level embodied graph is continuously updated online. Online updates include: inserting newly observed static or quasi-static nodes into the multi-level embodied graph, updating information of existing nodes, deleting nodes marked as dynamic, and periodically reconstructing hierarchical clustering relationships. This step handles dynamic objects at the graph level, ensuring that short-term moving objects do not pollute long-term memory, while still being promptly perceived and avoided during local planning.

[0131] Specifically, refer to Figure 2 The multi-level embodied map is designed as a long-term, online-updable "scene memory container". Its update mechanism includes: (1) Node insertion and update: Newly observed static or quasi-static object nodes are inserted into the map; existing nodes are updated with their position and semantic descriptions based on the new observations. (2) Dynamic node deletion and corridor removal: For objects marked as dynamic and continuously moving by the spatial-temporal corridor, their nodes and corresponding paths are deleted from the static map to avoid polluting long-term memory. (3) Periodic hierarchical clustering reconstruction: Hierarchical clustering is performed periodically at the object layer and the clustering layer to update the hierarchical relationship between regional nodes and buildings-regions-objects. (4) Historical trajectory node maintenance: The vehicle trajectory nodes are sampled and maintained according to displacement and time to form a concise and representative navigation experience.

[0132] In the above embodiments, the present invention employs a "spatial-temporal corridor + dynamic point filtering" approach to filter dynamic objects. In some other embodiments, the following methods can also be used to filter dynamic objects: dynamic detection based on frame-by-frame optical flow, constructing a dynamic occupancy grid using Bayesian Occupancy Filtering, detecting dynamic objects based on a learned temporal consistency model (such as 3D MOT Transformer), or using a depth model to predict passable regions and directly generate local feasible regions to replace the point cloud voxel map. The core principle remains "dynamic objects are not written into the long-term atlas, maintaining a static representation."

[0133] The preferred embodiment of this invention discloses a scene graph semantic navigation method for dynamic outdoor environments. First, in the construction method of a multi-level embodied scene graph, semantics, topology, and historical experience are unified into a structured graph, simultaneously integrating offline building maps, online open-vocabulary object detection, and the robot's own historical trajectories. It possesses a multi-scale hierarchical relationship of "building-region-object-trajectory" and can be continuously updated online to reflect the spatiotemporal structure of the dynamic outdoor environment. Second, in LLM-RAG-driven hierarchical semantic retrieval and target reasoning, hierarchical semantic alignment (building → region → object) is performed on user natural language commands (q); LLM is used to calculate the "semantic matching probability" between node text descriptions and commands; and target localization is completed through graph hierarchy constraints and path scores. This enables the robot to understand open-vocabulary targets, such as "a place to study," "a bench at the library entrance," and "a square near the information building." Third, in the dynamic object filtering mechanism based on space-time corridors, since dynamic objects can pollute long-term memory, a space-time region of dynamic objects is constructed and the corridor region is removed from the map, so that the long-term static map remains clean and stable; at the same time, it ensures that local planning can use the latest dynamic obstacle information; this mechanism significantly improves the robustness of navigation in dynamic outdoor environments.

[0134] The preferred embodiment of this invention discloses a scene graph semantic navigation method for dynamic outdoor environments. It encodes offline maps, multimodal online perception, and the robot's long-term historical trajectory into a dynamically updatable multi-level embodied scene graph, and combines it with LLM-RAG hierarchical semantic retrieval to achieve long-term outdoor navigation for open-vocabulary semantic targets. At the same time, it uses dynamic filtering and a global-local planning framework to achieve safe obstacle avoidance and stable path tracking.

[0135] Another preferred embodiment of the present invention discloses a scene graph semantic navigation system for dynamic outdoor environments, comprising:

[0136] The multi-level embodied graph construction module is used to construct multi-level embodied graphs, which include various types of nodes associated through hierarchical clustering relationships;

[0137] The hierarchical semantic retrieval and reasoning module based on the large language model is used to receive natural language instructions and perform hierarchical semantic retrieval and reasoning on the natural language instructions and the node descriptions of the multi-level embodied graph based on the large language model. The module determines the target node corresponding to the natural language instruction from the multi-level embodied graph. Specifically, the natural language instructions and the node descriptions of the embodied graph are input into the large language model (LLM) to perform hierarchical node selection and path scoring to determine the node or region where the semantic target is located.

[0138] The global path planning module is used to obtain the current robot's own nodes and perform global path planning based on the target node and the current robot's own nodes; specifically, it uses the embodied graph for global path planning.

[0139] The control command generation module is used to perform local trajectory planning and safety control decisions based on the planned global path and local real-time perception data, and generate control commands. Specifically, it combines online voxel map updates from LiDAR to obtain the local feasible region in real time, uses methods such as Informed-RRT* to generate the initial trajectory, and optimizes it using NMPC-CBF to obtain safety control commands that meet dynamic obstacle constraints.

[0140] The multi-level embodied graph construction module includes:

[0141] The multi-module perception and time synchronization submodule is used to acquire RGB images, LiDAR point clouds and IMU / GNSS data, and complete time synchronization and coordinate alignment.

[0142] The open vocabulary object detection and vehicle pose estimation submodule is used to realize real-time estimation of object nodes and vehicle nodes based on open vocabulary detection / segmentation network, multi-object tracking methods such as ByteTrack and LIO (LiDAR-Inertial Odometry).

[0143] The Dynamic Object Space-Time Filtering and Static Environment Extraction submodule is used to remove highly dynamic objects from the long-term graph by employing a trajectory-based space-time corridor mechanism, while retaining relatively stable static structures.

[0144] The multi-level embodied graph construction submodule is used to construct a multi-level embodied graph containing building nodes, cluster nodes, object nodes, and vehicle trajectory nodes by utilizing offline building maps, online object nodes, and clustering results.

[0145] In a further embodiment, the scene graph semantic navigation system for dynamic outdoor environments also includes an embodied graph online update module, which is used to continuously insert, update and delete nodes during robot operation, perform hierarchical clustering, and ensure the long-term adaptability of the graph to environmental changes.

[0146] Another preferred embodiment of the present invention discloses a computer-readable storage medium storing a computer program, wherein the computer program is configured to be run by a processor to perform the scene graph semantic navigation method for dynamic outdoor environments described in the preferred embodiment above.

[0147] Optionally, the aforementioned storage media may include, but are not limited to, USB flash drives, read-only memory (ROM), random access memory (RAM), portable hard drives, magnetic disks, or optical disks, and other media capable of storing computer programs.

[0148] The embodiments of the present invention have the following advantages:

[0149] (1) A multi-level embodied scene graph suitable for dynamic outdoor environments was constructed: offline building maps, online open vocabulary object detection, and vehicle historical trajectories were unified into a multi-level embodied graph to achieve multi-scale semantic representation from the building layer to the region layer and then to the object layer.

[0150] (2) It provides natural language navigation capabilities combined with LLM-RAG: through hierarchical semantic retrieval and path scoring mechanism, it can robustly map open natural language instructions (such as "where to borrow books" or "where to buy coffee") into graph target nodes, and realize language navigation in real environment.

[0151] (3) Unified handling of dynamic objects at the map and planning levels: Through the space-time corridor mechanism, short-term dynamic targets are removed from the long-term map, while local obstacle avoidance safety is ensured in online voxel map updates and NMPC-CBF, which greatly improves the robustness of navigation in dynamic environments.

[0152] (4) Support for long-term operation and adaptation to environmental changes: The multi-level embodied map is continuously updated online. Nodes can be inserted / deleted and cluster structures can be reconstructed as the environment changes. This supports robots to operate in campuses, parks and other scenarios for a long time without over-reliance on static HD maps.

[0153] (5) Convenient engineering implementation and easy integration with existing systems: The present invention adopts a modular design at both the perception and planning levels, and can be integrated with existing LIO (LiDAR-Inertial Odometry), local planner and other components; LLM can be deployed locally using open source models, reducing dependence on cloud services and having good engineering feasibility.

[0154] The background section of this invention may include background information about the problems or circumstances surrounding the invention, rather than a description of prior art by others. Therefore, the content included in the background section is not an admission of prior art by the applicant.

[0155] The above description provides a further detailed explanation of the present invention in conjunction with specific / preferred embodiments, and it should not be construed that the specific implementation of the present invention is limited to these descriptions. For those skilled in the art, various substitutions or modifications can be made to these described embodiments without departing from the concept of the present invention, and all such substitutions or modifications should be considered within the scope of protection of the present invention. In the description of this specification, the reference to terms such as "an embodiment," "some embodiments," "preferred embodiment," "example," "specific example," or "some examples," etc., indicates that the specific features, structures, materials, or characteristics described in connection with that embodiment or example are included in at least one embodiment or example of the present invention. In this specification, the illustrative expressions of the above terms do not necessarily refer to the same embodiment or example. Moreover, the specific features, structures, materials, or characteristics described can be combined in any suitable manner in one or more embodiments or examples. Furthermore, those skilled in the art can combine and integrate different embodiments or examples and features of different embodiments or examples described in this specification without contradiction. Although the embodiments of the present invention and their advantages have been described in detail, it should be understood that various changes, substitutions, and modifications can be made herein without departing from the scope defined by the appended claims.

Claims

1. A scene graph semantic navigation method for dynamic outdoor environments, characterized in that, Includes the following steps: S1: Construct a multi-level embodied graph, which includes nodes of various types associated through hierarchical clustering relationships; S2: Receive natural language instructions, and perform hierarchical semantic retrieval and reasoning on the natural language instructions and the node descriptions of the multi-level embodied graph, and determine the target node corresponding to the natural language instructions from the multi-level embodied graph; S3: Obtain the current robot's own node, and perform global path planning based on the target node and the current robot's own node; S4: Acquire local real-time sensing data, and based on the planned global path and the local real-time sensing data, perform local trajectory planning and safety control decisions to generate control commands; Specifically, S1 includes: S11: Acquire multimodal sensor data and perform time synchronization and spatial coordinate alignment processing on the multimodal sensor data; the multimodal sensor data includes RGB images, LiDAR point clouds, and inertial and pose assistance information; S12: Based on the processed multimodal sensor data, construct the multi-level embodied graph, wherein the various types of nodes include nodes representing multi-level static environmental semantic information and nodes representing the robot's own historical trajectory information; S12 specifically includes: S121: Based on the open vocabulary detection model, reason about the RGB image to obtain open vocabulary target detection results containing text description and segmentation mask, and perform temporal correlation of the open vocabulary target detection results through a multi-target tracking algorithm to form a target trajectory; S122: Using camera intrinsic parameters and camera-radar extrinsic parameters, the lidar point cloud and the segmentation mask are aligned in 2D-3D to determine the position of the open vocabulary target detection result in three-dimensional space and generate object nodes; S123: Generate cluster nodes by clustering multiple object nodes through spatial-semantic hierarchy; S124: Obtain the current robot pose and offline map based on inertial and pose assistance information, and generate the robot's own trajectory nodes and building nodes representing the macroscopic structure; S125: The building nodes, object nodes, and clustering nodes representing multi-level static environmental semantic information, as well as the robot's own trajectory nodes representing the robot's own historical trajectory information, are fused to construct the multi-level embodied graph.

2. The scene graph semantic navigation method for dynamic outdoor environments according to claim 1, characterized in that, S12 also includes the following in the process of constructing the multi-level embodied graph: The motion trajectory of the dynamic target is obtained, and a spatiotemporal occupancy region representation of the dynamic target is constructed based on the motion trajectory. Nodes within the spatiotemporal occupancy region of the dynamic target are filtered out from various types of nodes representing multi-level static environmental semantic information according to the spatiotemporal occupancy region of the dynamic target.

3. The scene graph semantic navigation method for dynamic outdoor environments according to claim 1, characterized in that, S2 specifically includes: S21: Receive natural language commands; S22: Calculate the semantic matching probability between the natural language instruction and the descriptions of nodes at different levels in the multi-level embodied graph based on the large language model; S23: Based on the semantic matching probability and the hierarchical relationship between nodes, determine candidate nodes and calculate the overall path score from the current node to the candidate node; S24: Determine the target node corresponding to the natural language instruction based on the overall path score.

4. The scene graph semantic navigation method for dynamic outdoor environments according to claim 1, characterized in that, S3 specifically includes: obtaining the current robot's own node, executing a path search algorithm on the multi-level embodied graph, obtaining the path from the current robot's own node to the target node, and obtaining global path planning.

5. The scene graph semantic navigation method for dynamic outdoor environments according to claim 1, characterized in that, S4 specifically includes: S41: Acquire local real-time perception data, and construct a local feasible region containing real-time dynamic obstacle information based on the local real-time perception data; S42: Generate a local reference trajectory based on the local feasible region and the planned global path; S43: Based on the safety constraints of nonlinear model predictive control combined with the control barrier function, the local reference trajectory is tracked to generate control commands.

6. The scene graph semantic navigation method for dynamic outdoor environments according to claim 5, characterized in that, S42 specifically includes: based on the planned global path, generating an initial path within the local feasible region using a planning algorithm, and smoothing the initial path to generate the local reference trajectory; S43 specifically includes: constructing an optimization problem of nonlinear model predictive control using the robot's own state and control input as variables, with the optimization objective being to track the local reference trajectory and minimize the control cost; and solving the optimization problem to obtain the optimal control sequence and extracting the control command at the current moment; wherein, a safety constraint represented in the form of a control barrier function is introduced into the optimization problem of the nonlinear model predictive control, and the safety constraint is used to ensure that the robot itself maintains a safe distance from the obstacle.

7. The scene graph semantic navigation method for dynamic outdoor environments according to claim 1, characterized in that, During navigation, the multi-level embodied graph is continuously updated online. The online update includes: inserting newly observed static or quasi-static nodes into the multi-level embodied graph, updating the information of existing nodes, deleting nodes marked as dynamic, and periodically reconstructing hierarchical clustering relationships.

8. A computer-readable storage medium, characterized in that, The computer-readable storage medium stores a computer program, wherein the computer program is configured to be run by a processor to perform the scene graph semantic navigation method for dynamic outdoor environments as described in any one of claims 1 to 7.

Images