A hierarchical dynamic scene graph navigation method based on probability distribution perception

Abstract

The application discloses a kind of hierarchical dynamic scene graph navigation methods based on probability distribution perception, belong to robot and embodied intelligent technical field.The application is aimed at the technical problems such as poor dynamic adaptability of existing semantic mapping and navigation technology in high dynamic large-scale scene, not robust to sensor noise, lack of long-term memory management, the following solutions are proposed: first, construct the double-layer map architecture of static abstract map skeleton and dynamic map, the hierarchical structure of floor-room-anchor point is adopted in static map, and Voronoi navigation map is constructed, and dynamic map is used to describe high dynamic objects in a small range.The application can improve the environmental perception ability, navigation robustness and task execution efficiency of robot in high dynamic large-scale scene, and can be widely applied in the fields of service robot, guide robot, cleaning robot and the like.

Description

Technical Field

[0001] This invention relates to the field of robotics and embodied intelligence, and in particular to a hierarchical dynamic scene graph navigation method based on probability distribution perception. Background Technology

[0002] With the rapid development of robotics and embodied intelligence technologies, robot applications are gradually shifting from industrial scenarios to complex service sectors, such as public services, commercial services, and home services. In these application scenarios, robots need to achieve high-precision multi-target search, navigation planning, and task execution in highly dynamic and large-scale environments such as large shopping malls and exhibition halls.

[0003] Among existing semantic mapping and semantic navigation technologies, hierarchical 3D scene graph methods can effectively represent large-scale scene spatial structures with multiple floors and rooms, but they usually implicitly assume a static environment, resulting in limited dynamic adaptability when facing highly dynamic scenes. On the other hand, while object-level online mapping and updating methods can respond to dynamic environments, object updates often rely on simple geometric overlap or weighted averaging, which is not robust to sensor noise and is prone to incorrect node fusion and redundancy. Furthermore, most existing methods lack long-term memory management, leading to incorrect reliance on outdated information during robot navigation. Summary of the Invention

[0004] The purpose of this invention is to overcome the shortcomings of the prior art and provide a hierarchical dynamic scene graph navigation method based on probability distribution perception. By constructing a two-layer map architecture that combines a static abstract graph and a dynamic map, a probability distribution is introduced to describe dynamic nodes and implement update and forgetting mechanisms. Combined with a large language model to process semantic instructions, this method enables robots to achieve efficient semantic navigation in highly dynamic and large-scale scenes.

[0005] To achieve the above objectives, the present invention provides the following technical solution: A hierarchical dynamic scene graph navigation method based on probability distribution awareness includes the following steps: A two-layer map architecture is constructed that combines a static abstract map skeleton with a dynamic map. The static abstract map is used to describe long-term stable static information in large-scale scenes, while the dynamic map is used to describe highly dynamic objects in a small area. Probabilistic modeling of nodes in dynamic maps is performed, Gaussian distribution is used to describe the state of nodes, and the association, updating and forgetting of nodes are realized based on Mahalanobis distance and Kalman filtering. By parsing natural language commands using a large language model and combining hierarchical retrieval of static maps with confidence assessment of dynamic maps, the robot achieves semantic navigation and active exploration.

[0006] Furthermore, the construction of the static abstract map skeleton includes: Construct an abstract undirected graph containing a set of anchor vertices and a set of topological edges; The abstract graph is hierarchically divided into floors, rooms, and anchor points. Floor nodes, room nodes, and anchor point nodes are defined respectively, and the membership relationships between nodes are established. A Voronoi navigation map is constructed within each floor based on the free passage space, and cross-floor connections are achieved through stairwell areas to form an executable navigation map.

[0007] Furthermore, the generation and fusion of the anchor nodes includes: For newly generated anchor points, their point cloud geometric overlap with that of existing anchor points is calculated and matched. If the overlap exceeds a set threshold, the point cloud and semantic features are fused using a weighted average method. Otherwise, add the new anchor point as an independent node to the anchor point set.

[0008] Furthermore, the construction of the dynamic map includes: Construct a dynamic node set and an associated edge set, where the dynamic node contains a 3D point cloud and semantic features; The association edges between dynamic nodes and between them and static anchor points are established by spatial distance and overlap.

[0009] Furthermore, the probabilistic modeling of dynamic nodes includes: Define state variables for each dynamic node, including spatial location and semantic features; The state variables are described using a Gaussian distribution, and their mean and covariance matrices are maintained to express the uncertainty of the nodes.

[0010] Furthermore, the association and updating of the dynamic nodes include: For new observations, calculate their Mahalanobis distance to existing dynamic nodes; If the distance is less than the set threshold, it is considered a match, and the mean and covariance of the nodes are updated using Kalman filtering. Otherwise, a new dynamic node is generated.

[0011] Furthermore, the forgetting mechanism of the dynamic node includes: For dynamic nodes that have not been observed for a long time, their covariance matrix increases linearly over time to increase their uncertainty and reduce their weight in subsequent navigation and fusion.

[0012] Furthermore, the semantic navigation includes the following sub-steps: The natural language instruction is decomposed into three levels of query clauses: floor, room, and target, using a large language model, and then encoded into query vectors aligned with semantic features. Search the static map level by level to determine candidate floors, rooms and anchor points, and perform global path planning on the executable navigation map; Upon reaching the target area, semantic similarity and confidence are calculated based on dynamic map nodes, and the optimal target is selected for local planning. If the target is not determined, proactive exploration is guided based on the uncertainty of the nodes until the target is confirmed.

[0013] Furthermore, the confidence level of the dynamic node is defined as a weighted combination of semantic similarity and uncertainty, which is used to guide target selection and task execution.

[0014] Furthermore, the active exploration mechanism includes: The exploration reward function for reachable nodes is defined as the maximum value of the uncertainty of nearby dynamic nodes; Select the node with the highest reward as the exploration target, perform local planning and observation, and repeat until the target is confirmed.

[0015] The beneficial effects of this invention are: Two-layer map architecture: By constructing a two-layer map architecture that combines a static abstract map and a dynamic map, the dynamic adaptability of semantic navigation is maintained in large-scale scenarios, which can achieve efficient global planning and support high-precision local positioning. Probabilistic modeling and update mechanism: Gaussian distribution is introduced to describe the uncertainty of dynamic nodes, Mahalanobis distance is used for node association, and Kalman filtering is used to update nodes, which improves robustness to sensor noise and reduces node erroneous fusion and redundancy. Forgetting Mechanism and Long-Term Memory Management: By leveraging the increasing uncertainty over time, the system enables the forgetting of nodes that have not been observed for a long time, preventing the robot from making incorrect reliance on outdated information and improving the reliability of the system in dynamic environments. Semantic understanding and hierarchical retrieval: By combining a large language model to process natural language commands, hierarchical retrieval of floors, rooms and anchor points can be achieved, which can quickly locate the target area and improve navigation efficiency; Uncertainty-driven proactive exploration: When the target is uncertain, the uncertainty of dynamic nodes is used to guide the robot to actively explore, which improves search efficiency and task completion rate. Attached Figure Description

[0016] Figure 1 This is a flowchart of the semantic mapping and navigation strategy for high-dynamic, large-scale scenes according to the present invention. Figure 2 This is a flowchart illustrating the construction of the static abstract map skeleton of the present invention; Figure 3 This is a flowchart of the dynamic map node construction and updating process of the present invention; Figure 4This is a flowchart illustrating the semantic navigation strategy and task implementation of the present invention. Detailed Implementation

[0017] 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 some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0018] Specific implementation examples are given below.

[0019] Example 1: System Overall Architecture like Figure 1 As shown, the technical solution of the present invention consists of three core parts: map construction, map updating, and semantic navigation.

[0020] During the map building phase, the robot constructs a semantic map of the environment, creating a dual-layer structure of static and dynamic elements. The static map skeleton maps large-scale scenes according to a hierarchical structure of floors, rooms, and anchor points, while the dynamic map nodes retain 3D point clouds and semantic labels of small-scale objects.

[0021] During the map update phase, a Gaussian distribution is introduced to describe node probabilities, including the mean and covariance of semantic features. Dynamic map nodes are updated using the Mahalanobis distance between new observations and old nodes, and forgetting is achieved by increasing the variance of long-unobserved objects.

[0022] In the semantic navigation phase, semantics are processed through a large language model, static map hierarchical retrieval is performed, and an uncertainty-driven active exploration process is used to achieve semantic navigation.

[0023] Example 2: Static Abstract Map Skeleton Construction like Figure 2 As shown, the construction process of the static abstract map skeleton is as follows: First, define the abstract map as an undirected graph. ,in For the set of anchor vertices, This is the set of topological edges between anchor points. Each anchor point... Includes a collection of 3D point clouds and semantic feature vectors .

[0024] When new observations generate new anchor points At this time, the point cloud geometric overlap is calculated and matched with existing anchor points. The formula for calculating the overlap is: , If anchor points exist, If the condition is met, the new anchor point is merged with the existing anchor points, and the semantic features are updated by weighting according to the point cloud size. The merged point clouds are then downsampled. Otherwise, the new anchor point is added to the anchor point set.

[0025] Based on this, the abstract diagram is hierarchically structured as floors, rooms, and anchor points. (Definition) From the set of floor nodes Room node set With anchor node set Composition. Floor nodes It includes floor point clouds and aggregated semantic features. Room nodes are obtained by clustering the point clouds within a floor, and anchor nodes are associated with room nodes by calculating the overlap or distance with the room point clouds.

[0026] Finally, a Voronoi navigation map is constructed within each floor based on free passage space. It also connects navigation maps of adjacent floors through the stairwell area to construct an executable navigation map across floors. .

[0027] Example 3: Dynamic Map Construction and Probabilistic Modeling like Figure 3 As shown, the process of dynamic map construction and updating is as follows: Dynamic map representation ,in For a dynamic set of nodes, This is the set of associated edges. Each dynamic node... It includes 3D point clouds and semantic features.

[0028] To characterize uncertainty, a state variable is added to each dynamic node. And described using a Gaussian distribution: ,in This indicates the uncertainty of the node's state.

[0029] When new observations At that time, calculate its Mahalanobis distance with the existing dynamic nodes: , If a node exists that makes If the result is a match, then a Kalman filter is used to update the mean and covariance of the nodes. Otherwise, a new dynamic node is generated.

[0030] For dynamic nodes that have not been observed for a long time, a forgetting mechanism is introduced, and the covariance increases over time: , Example 4: Semantic Navigation and Active Exploration like Figure 4 As shown, the implementation process of semantic navigation is as follows: First, the natural language instruction q is decomposed into three levels of query clauses: floor, room, and target, using a large language model. And mapped to a query vector through encoder() .

[0031] Perform a step-by-step search on a static map: calculate the characteristics of floor nodes and... The cosine similarity is used to select the floor with the highest similarity, f; within f, the room nodes are calculated to... The similarity is calculated, and the room with the highest similarity (r*) is selected; the anchor node is calculated with... Based on the similarity, the top K anchor points are selected to form a candidate set. .

[0032] Map candidate anchor points to the executable navigation graph The target node set is obtained. .exist The global path is obtained by using heuristic search for global planning.

[0033] After the robot reaches the target area along the path, it performs precise localization on a dynamic map. This involves calculating the dynamic nodes and... cosine similarity Confidence level is defined in conjunction with uncertainty. The node with the highest confidence level is selected as the final target.

[0034] If the target is not determined, an active exploration mechanism is triggered. Define the exploration reward function R(u) = max_{v_k^dN(u)}_k^d for reachable nodes u, select the node with the highest reward as the exploration target, perform local planning and observation, and repeat until the target is confirmed.

[0035] This invention can be widely applied to navigation systems for various autonomous mobile robots, especially robots that need to perform service tasks in high-dynamic, large-scale scenarios such as shopping malls, exhibition halls, airports, and hospitals, such as tour guide robots, cleaning robots, and delivery robots. It has significant engineering application value and economic value.

[0036] The above description is only a preferred embodiment of the present invention, but the scope of protection of the present invention is not limited thereto. Any equivalent substitutions or modifications made by those skilled in the art within the scope of the technology disclosed in the present invention, based on the technical solution and inventive concept of the present invention, should be covered within the scope of protection of the present invention.

Claims

1. A hierarchical dynamic scene graph navigation method based on probability distribution perception, characterized in that, Includes the following steps: A two-layer map architecture is constructed that combines a static abstract map skeleton with a dynamic map. The static abstract map is used to describe long-term stable static information in large-scale scenes, while the dynamic map is used to describe highly dynamic objects in a small area. Probabilistic modeling of nodes in dynamic maps is performed, Gaussian distribution is used to describe the state of nodes, and the association, updating and forgetting of nodes are realized based on Mahalanobis distance and Kalman filtering. By parsing natural language commands using a large language model and combining hierarchical retrieval of static maps with confidence assessment of dynamic maps, the robot achieves semantic navigation and active exploration.

2. The method according to claim 1, characterized in that, The construction of the static abstract map skeleton includes: Construct an abstract undirected graph containing a set of anchor vertices and a set of topological edges; The abstract graph is hierarchically divided into floors, rooms, and anchor points. Floor nodes, room nodes, and anchor point nodes are defined respectively, and the membership relationships between nodes are established. A Voronoi navigation map is constructed within each floor based on the free passage space, and cross-floor connections are achieved through stairwell areas to form an executable navigation map.

3. The method according to claim 2, characterized in that, The generation and fusion of the anchor nodes include: For newly generated anchor points, their point cloud geometric overlap with that of existing anchor points is calculated and matched. If the overlap exceeds a set threshold, the point cloud and semantic features are fused using a weighted average method. Otherwise, add the new anchor point as an independent node to the anchor point set.

4. The method according to claim 1, characterized in that, The construction of the dynamic map includes: Construct a dynamic node set and an associated edge set, where the dynamic node contains a 3D point cloud and semantic features; The association edges between dynamic nodes and between them and static anchor points are established by spatial distance and overlap.

5. The method according to claim 1 or 4, characterized in that, The probabilistic modeling of dynamic nodes includes: Define state variables for each dynamic node, including spatial location and semantic features; The state variables are described using a Gaussian distribution, and their mean and covariance matrices are maintained to express the uncertainty of the nodes.

6. The method according to claim 5, characterized in that, The association and updating of the dynamic nodes include: For new observations, calculate their Mahalanobis distance to existing dynamic nodes; If the distance is less than the set threshold, it is considered a match, and the mean and covariance of the nodes are updated using Kalman filtering. Otherwise, a new dynamic node is generated.

7. The method according to claim 5, characterized in that, The forgetting mechanism of the dynamic nodes includes: For dynamic nodes that have not been observed for a long time, their covariance matrix increases linearly over time to increase their uncertainty and reduce their weight in subsequent navigation and fusion.

8. The method according to claim 1, characterized in that, The semantic navigation includes the following sub-steps: The natural language instruction is decomposed into three levels of query clauses: floor, room, and target, using a large language model, and then encoded into query vectors aligned with semantic features. Search the static map level by level to determine candidate floors, rooms and anchor points, and perform global path planning on the executable navigation map; Upon reaching the target area, semantic similarity and confidence are calculated based on dynamic map nodes, and the optimal target is selected for local planning. If the target is not determined, proactive exploration is guided based on the uncertainty of the nodes until the target is confirmed.

9. The method according to claim 8, characterized in that, The confidence level of the dynamic node is defined as a weighted combination of semantic similarity and uncertainty, which is used to guide target selection and task execution.

10. The method according to claim 8, characterized in that, The active exploration mechanism includes: The exploration reward function for reachable nodes is defined as the maximum value of the uncertainty of nearby dynamic nodes; Select the node with the highest reward as the exploration target, perform local planning and observation, and repeat until the target is confirmed.

Images