A hierarchical path planning method and system based on a three-dimensional graph network

Abstract

The present application relates to the technical field of unmanned system autonomous navigation and path planning, in particular to a hierarchical path planning method and system based on three-dimensional graph network, a three-dimensional environment semantic graph containing geometric and risk semantic information is constructed; then a down-sampling strategy and an improved AStar algorithm are used in the global layer, a ray collision detection mechanism is used to quickly generate a global waypoint sequence under a coarse resolution grid, solving the search efficiency bottleneck in a large-scale three-dimensional space; in the local layer, a Q learning local decision method guided by artificial potential field is proposed, a lazy loading sparse storage is realized by initializing a Q table with expert experience, and global waypoint guidance and local obstacle avoidance logic are combined. The present application realizes the decoupling and fusion of global optimality and local real-time obstacle avoidance through hierarchical architecture, effectively solves the problems of calculation explosion, local extremum and non-connected space traversal in the three-dimensional complex environment of traditional algorithms, and improves the planning efficiency and safety of the agent.

Description

Technical Field

[0001] This invention relates to the field of autonomous navigation and path planning technology for unmanned systems, and in particular to a hierarchical path planning method and system based on three-dimensional graph networks. Background Technology

[0002] In existing technologies, 3D path planning is widely used in the autonomous navigation of unmanned systems such as autonomous underwater vehicles, unmanned aerial vehicles, and complex space inspection platforms. Given a known global map, existing methods typically employ strategies such as direct search in the grid space, end-to-end decision-making based on reinforcement learning, or downsampling approximate planning. Specifically, the A* search algorithm obtains feasible paths by traversing and expanding nodes in a 3D grid; reinforcement learning-based methods learn obstacle avoidance strategies through interaction with the environment; and some methods further introduce downsampling mechanisms to compress the state space dimension, attempting to alleviate computational pressure in large-scale scenarios.

[0003] However, the aforementioned methods are prone to problems such as high search dimensionality, large computational overhead, coarse paths, or insufficient local obstacle avoidance adaptability in large-scale 3D environments. In complex 3D environments (such as dense urban building clusters, complex underwater mining areas, forestry resource surveys, logistics, and transportation), path planning for unmanned aerial vehicles (UAVs) or underwater unmanned vehicles (AUVs) faces significant challenges. Traditional A* search, when dealing with large-scale 3D meshes, experiences an exponential increase in search space with increasing dimensionality, resulting in computation time that cannot meet real-time requirements, and the generated paths are coarse and lack local adaptability. Reinforcement learning-based methods are prone to falling into local extremum traps when lacking global information guidance, and their convergence speed is extremely slow in large-scale environments. Furthermore, while existing downsampling planning methods can improve search speed, coarse resolution can easily lead to path connections penetrating thin-walled obstacles, creating safety hazards. Meanwhile, existing methods typically use map information only for geometric space occupancy, failing to fully utilize the graph structure to uniformly organize traversable relationships, obstacle semantics, and passage costs. This results in difficulties in effectively balancing global guidance, local real-time obstacle avoidance, and computational efficiency. At present, a hierarchical path planning method and system based on 3D graph networks is needed. Summary of the Invention

[0004] To address the problems of low search efficiency, local planning getting trapped in local extrema, and insufficient utilization of graph structure map information in large-scale known maps, this invention provides a hierarchical path planning method and system based on 3D graph networks.

[0005] In a first aspect, the present invention provides a hierarchical path planning method based on a three-dimensional graph network, which adopts the following technical solution: A hierarchical path planning method based on 3D graph networks includes: Acquire 3D environmental data of the target area, represent free space, obstacle boundaries, start point and end point using graph network, and construct a 3D environmental semantic graph. Based on the 3D environment semantic graph, a downsampling A* global search is performed, and a coarse-resolution guided path composed of several global waypoints is output through a dynamic resolution strategy and a collision detection mechanism. Using the global waypoint sequence as the local sub-target sequence, a Q-learning local planner based on artificial potential field prior is constructed, and the action Q value in the current state is initialized according to the potential energy difference; The local planner executes local planning actions in real time, performs collision detection during the state transition process, and updates the action Q value online based on the reward function and Bellman update formula. When the agent enters the preset switching threshold range of the current global waypoint, it automatically switches to the next global waypoint. Iteratively execute the local planning and sub-objective switching process until the final target point is reached, and output a complete continuous 3D path.

[0006] Furthermore, the graph network representation of free space, obstacle boundaries, start point, and end point includes: Acquire 3D environmental point cloud or voxel data of the target area, extract environmental boundaries, obstacle geometric features, start coordinates and end coordinates, and establish a 3D spatial index structure; Based on the three-dimensional spatial index structure, voxel rasterization is performed on the target area, the continuous three-dimensional space is discretized into several voxel units, and the free space is extracted according to the occupancy relationship between each voxel unit and the obstacle geometry and the environmental boundary. The free space is discretized into passable sampling nodes, with the start point and end point as the starting node and the target node, respectively, and the three-dimensional spatial coordinates of each sampling node as the basic attribute of the node. Perform neighborhood connectivity analysis on accessible sampling nodes, establish graph edges for node pairs that meet the collision-free connectivity condition after collision detection, and use the spatial distance between nodes as the basic attribute of the edges. The obstacle type, boundary constraints, distance to the target, and waypoint hierarchy information are encoded as node attributes or edge attributes to obtain a three-dimensional environment semantic graph.

[0007] Furthermore, the output coarse-resolution guided path, composed of several global waypoints, includes: The coarse resolution step size is adaptively determined based on the map physical scale and resolution scaling factor, and twenty-six neighborhood node expansion is performed on the coarse resolution grid. The priority of the expanded nodes is calculated using a weighted heuristic function. The node search order is determined by the weighted sum of the cumulative cost from the starting point to the current node and the heuristically estimated cost from the current node to the end point. During node expansion, collision detection is performed on the connections between the current node and candidate nodes. Candidate nodes that are verified to be collision-free are included in the global search process, and a coarse-resolution guided path consisting of several global waypoints is output. The formula for calculating the coarse-resolution step size is:

[0008] in, For coarse resolution step size, The physical scale of the current map. To impose an upper limit constraint on the calculation results, This is the resolution scaling factor. To impose a lower bound constraint on the calculation results.

[0009] Furthermore, the collision detection for the connection between the current node and the candidate node includes, during the 26-neighborhood expansion of the downsampling A* global search, discretizing the connection between the current node and the candidate node with a fixed step size using a parameterized ray equation, verifying the geometric intersection relationship between each sampling point and the obstacle one by one, and determining that the connection has collided when there is a sampling point located inside the obstacle. The parameterized ray equation is: , in, The coordinates of the sampling points on the line segment are... and These are the three-dimensional coordinates of the start and end points of the line segment, respectively. These are the normalized parameters.

[0010] Furthermore, the construction of the Q-learning local planner based on artificial potential field priors includes: The three-dimensional environment is segmented into voxels according to a preset resolution, the state of the agent is identified by discrete grid coordinates, and the current state coordinates are used as the query key. A set of Q-values ​​is established using a sparse storage structure based on hash tables. The corresponding action value function is dynamically initialized only when the agent explores a new state, thus implementing a lazy loading mechanism. Establish an action space containing multiple discrete directions, wherein the action vector is represented as follows: The values ​​of each dimension component are taken from And they are not all zero at the same time; Based on the current state coordinates and the selected action vector, the predicted state coordinates for the next moment are calculated through a linear mapping. The expression for the linear mapping is: , in, Current state coordinates For the selected action Components along each coordinate axis direction, This is the control resolution of the local planner. Predicting the state for the next moment The coordinates.

[0011] Furthermore, the initialization of the action Q-value in the current state based on the potential energy difference includes calculating the distance from the current state to the current sub-target waypoint, and the distance from the next state after executing the candidate action to the current sub-target waypoint, to obtain the potential energy difference. The Q-value of the corresponding action is initialized based on the potential energy difference and the potential field guidance weight coefficient. When the state transition corresponding to the candidate action collides with an obstacle, the Q-value of that action is initialized to a preset negative value to form an obstacle avoidance prior. The expression for initializing the Q-value of the corresponding action is: , in, Actions under state s The initial Q value, The Euclidean distance from the current state s to the current sub-goal waypoint is... To perform the action Next state Euclidean distance to the current sub-target waypoint The potential field guide weighting coefficient.

[0012] Furthermore, the initialization of the action Q value in the current state based on the potential energy difference also includes constructing a gravitational potential field based on the coordinates of the currently activated global waypoint, constructing a repulsive potential field based on the locally detected obstacle center, and using the superposition result of the gravitational and repulsive potential fields as the basis for initializing the state Q value. The potential field initialization for global waypoint-guided local Q learning satisfies the following equation: , in, Let Q be the initial Q value for state s. This is the gravitational gain coefficient. The repulsive force gain coefficient, The coordinates of the currently active global waypoint. For the center of the i-th obstacle detected locally, Let be the Euclidean distance function. To prevent the numerically stable term from being divided by zero, This represents the number of obstacles detected locally.

[0013] Furthermore, the online update of the action Q value includes selecting an action from the action space based on an ε-greedy strategy and performing a state transition, wherein an action is randomly selected from the action space with probability ε, and the action with the largest Q value in the current state is selected with probability 1-ε. Ray collision detection is performed on the line connecting the current state to the next state. When the detection result is a collision, a collision penalty is imposed and the current state is maintained. When the detection result is no collision, a convergence reward is imposed based on the distance changes between the current state and the next state and the endpoint and the current sub-target, respectively. When the agent enters the preset threshold range of the current global waypoint, a waypoint switching reward is given; when the agent reaches the preset threshold range of the final destination, a destination reward is given. The Q-value of the selected action in the current state is updated online based on the Bellman update formula.

[0014] Furthermore, the automatic switching to the next global waypoint includes determining a preset switching threshold based on the global planning resolution and obstacle complexity coefficient. When a locally planned path enters the distance threshold range of the current global waypoint, the current global waypoint is switched to the next global waypoint. During local planning, when a local path is detected to be oscillating or repeatedly visited, a random action perturbation is introduced to break out of the local deadlock region. The expression for the random action perturbation is: , in, Select an action in state s The strategy probability, For the currently selected action, This is the current state. An exploration factor, used to balance the ratio of exploration to utilization. For the action space, Candidate actions in the action space, To select the action from action space A that maximizes the value of the state action.

[0015] Secondly, a hierarchical path planning system based on a three-dimensional graph network includes: The map atlas construction module is configured to: acquire 3D environmental data of the target area, perform graph network representation of free space, obstacle boundaries, start point and end point, and construct a 3D environmental semantic graph; The global planning module is configured to perform downsampling global search based on the 3D environment semantic graph and output a coarse-resolution guided path composed of several global waypoints through a dynamic resolution strategy and collision detection mechanism. The local decision-making module is configured to: use the global waypoint sequence as the local sub-target sequence, construct a Q-learning local planner based on the prior of the artificial potential field, initialize the action Q value in the current state according to the potential energy difference, and execute the local planning action in real time. The collision detection module is configured to perform collision detection on state transitions during local planning and on node connections during global search. The path output module is configured to automatically switch to the next global waypoint when the agent enters the preset switching threshold range of the current global waypoint, and continue to execute the local planning and sub-goal switching process until the final goal point is reached, and output a complete continuous three-dimensional path.

[0016] In summary, the present invention has the following beneficial technical effects: 1. This invention uses graph network representation to express the three-dimensional environment, unifying passable nodes, obstacle geometric relationships, boundary constraints, and passage costs into a three-dimensional environment semantic graph. This enhances the structure and interpretability of map representation and provides a unified data foundation for subsequent knowledge graph extraction, graph network reasoning, and hierarchical path planning.

[0017] 2. This invention significantly reduces the number of search nodes in a large-scale 3D map by setting a downsampling A* global planning layer, while ensuring path connectivity. Combined with a dynamic resolution strategy and a ray collision detection mechanism, it improves planning speed while avoiding obstacle crossing problems under coarse resolution conditions.

[0018] 3. This invention sets up a Q-learning local planning layer based on artificial potential field priors, uses global waypoints to provide directional priors for the local obstacle avoidance process, and adopts a lazy-loaded Q-table initialization strategy to reduce storage consumption and initialization overhead in the three-dimensional state space, effectively improving the convergence efficiency of local planning.

[0019] 4. This invention integrates global waypoint guidance with local Q-learning decision-making, and uses global topology information to give the local planner a global perspective, effectively overcoming the problem that reinforcement learning alone is prone to getting trapped in local extrema, and achieving the decoupling and integration of global optimality and local real-time obstacle avoidance.

[0020] 5. This invention achieves coordinated optimization of global search efficiency and local obstacle avoidance robustness in large-scale three-dimensional space through dynamic sub-target switching and online update mechanism of local action Q value under hierarchical architecture. It effectively solves the problems of computational explosion, local extrema and non-connected space traversal in traditional algorithms in complex three-dimensional environment, thereby improving the planning efficiency and safety of intelligent agents. Attached Figure Description

[0021] Figure 1 This is a schematic diagram of the overall process of a hierarchical path planning method based on a three-dimensional graph network according to an embodiment of the present invention.

[0022] Figure 2 This is a schematic diagram of the construction of a three-dimensional environmental semantic graph according to an embodiment of the present invention.

[0023] Figure 3 This is a schematic diagram of the downsampling A* global planning process according to an embodiment of the present invention.

[0024] Figure 4 This is a schematic diagram of the APF initialization Q-learning local planning process according to an embodiment of the present invention.

[0025] Figure 5 This is a schematic diagram illustrating the trap map and local extremum avoidance effect of an embodiment of the present invention.

[0026] Figure 6 This is a comparative diagram of planning results under the first obstacle scenario in a 50×50×50 random map according to an embodiment of the present invention.

[0027] Figure 7 This is a comparative diagram of planning results under the second obstacle scenario in a 50×50×50 random map according to an embodiment of the present invention.

[0028] Figure 8 This is a comparative diagram of planning results under the first obstacle scenario in a 20×20×20 random map according to an embodiment of the present invention.

[0029] Figure 9 This is a comparative diagram of planning results under the second obstacle scenario in a 20×20×20 random map according to an embodiment of the present invention. Detailed Implementation

[0030] The present invention will be further described in detail below with reference to the accompanying drawings.

[0031] Example 1 Reference Figure 1 This embodiment of a hierarchical path planning method based on a three-dimensional graph network includes: S1. Acquire the three-dimensional environmental data of the target area, and represent the free space, obstacle boundaries, start point and end point using a graph network to construct a three-dimensional environmental semantic graph; S2. Perform downsampling A* global search based on the 3D environment semantic graph, and output a coarse-resolution guided path composed of several global waypoints through dynamic resolution strategy and collision detection mechanism. S3. Using the global waypoint sequence as the local sub-target sequence, construct a Q-learning local planner based on the prior of the artificial potential field, and initialize the action Q value in the current state according to the potential energy difference. S4. The local planner performs local planning actions in real time, performs collision detection during the state transition process, and updates the action Q value online according to the reward function and Bellman update formula. When the agent enters the preset switching threshold range of the current global waypoint, it automatically switches to the next global waypoint. S5. Iteratively execute the local planning and sub-objective switching process until the final target point is reached, and output the complete continuous three-dimensional path.

[0032] Specifically, a hierarchical path planning method based on 3D graph networks includes the following: like Figure 1 , Figure 2 As shown, S1: Acquire 3D environmental point cloud or voxel data of the target area, extract environmental boundaries, obstacle geometric features, start coordinates, and end coordinates, and establish a 3D spatial index structure. In this embodiment, the environment consists of boundaries, start points, end points, and various types of obstacles, including axis-aligned bounding box (AABB) obstacles, spherical obstacles, and directed bounding box (OBB) obstacles. The environment module supports 20×20×20, 50×50×50, and trap terrain. Figure 3 A random obstacle map of various scales, where the number of obstacles can be adjusted proportionally, and the map scene can be reproduced using a fixed random number seed, by introducing parameters. With spatial constraints The coupling mechanism transforms the unpredictable random generation process into a verifiable deterministic simulation. This mechanism addresses the technical deficiency of existing technologies where experimental results are not reproducible due to different test scenarios when comparing algorithm performance in complex environments. The formula for the fixed random number seed is: , in, For the generated 3D environment map, Generate functions for the environment. The input is a fixed random number seed. The preset number of obstacles, As a spatial scale constraint, this function ensures that for a given... The topological structure of the environment is uniquely determined.

[0033] The environment generation function Based on a pseudo-random number generator (PRNG), its mapping process is defined as follows:

[0034] in: : indicates using seeds Using the state baseline, generate a pseudo-random sequence of obstacle parameter sets. ; : Indicates that collision checking logic is executed to check whether the generated obstacles cover the start or end point, and to remove obstacles that conflict with the start or end point (default is 5 units in length) to ensure the feasibility of the obstacle set; : Defines the spatial boundary constraints for the distribution of obstacles.

[0035] For any map scene, its boundaries are represented as: , in, Indicates the environmental boundary at , , Minimum coordinates in the axial direction, Represents the coordinates of the maximum values ​​of the environment boundary along the x, y, and z axes; Indicates the starting coordinates. Indicates the coordinates of the endpoint. , and These represent the coordinate components of the starting point in the x, y, and z axes of three-dimensional space. , and These are the coordinate components of the endpoint in the x-axis, y-axis, and z-axis directions in three-dimensional space.

[0036] Based on the three-dimensional spatial index structure, voxel rasterization is performed on the target region, discretizing the continuous three-dimensional space into several voxel units. Free space is extracted based on the occupancy relationship between each voxel unit and the obstacle geometry and the environmental boundary. The free space is discretized into passable sampling nodes, with the start point and end point as the starting node and the target node, respectively. The three-dimensional spatial coordinates of each sampling node are used as the basic attribute of the node. Twenty-six neighborhood connectivity analysis is performed on the passable sampling nodes. Node pairs that meet the collision-free connectivity condition after collision detection are used to establish graph edges, and the spatial distance between nodes is used as the basic attribute of the edge.

[0037] To represent the known global map as a graph structure, this embodiment abstracts traversable space as graph nodes, the twenty-six neighborhood connectivity relationships and their costs as graph edges, and uses obstacle type, boundary constraints, distance to the target, and waypoint hierarchy information as graph attributes, thereby constructing a three-dimensional environmental semantic graph. Any node... and The Euclidean distance between them is defined as: , in, and Representing nodes respectively and nodes In three-dimensional space, the aforementioned Euclidean distance is used for edge cost calculation, heuristic evaluation, and reward shaping.

[0038] Furthermore, the environment construction process is essentially a semantic object instantiation process based on deterministic parameter mapping. Specifically, the system receives a set of input parameters. A three-dimensional environment semantic graph is generated through the following deterministic mapping mechanism: Deterministic seed initialization: Call the pseudo-random number generator This generates a series of independent deterministic random sequences. Geometric semantic mapping: based on The sequence will contain the number of obstacles in the input. Mapped to a specific set of geometric properties ,in As the geometric center, This is the extension of the half-axis of the enclosure box. Rotation matrix generated based on Euler angles; constraint filtering and instantiation; execution of safety determination function. This function iterates through all potential obstacles, removes objects that conflict with the start and end point safety thresholds (default is 5 units), and finally outputs objects that conform to the spatial boundaries. 3D environment semantic graph .

[0039] like Figure 3 As shown, S2, the coarse resolution step size is adaptively determined based on the spatial physical scale and resolution ratio coefficient of the 3D environment semantic graph, and a 26-neighborhood node expansion is performed on the coarse resolution grid; specifically, in the global planning stage, in order to reduce the number of search nodes in the large-scale 3D space and reduce the state space dimension, the coarse resolution step size is adaptively set according to the spatial physical scale of the 3D environment semantic graph to accelerate the global search speed. The coarse resolution step size is defined as: , in, For coarse resolution step size, The physical scale of the current map. To impose an upper limit constraint on the calculation results, ensuring that the step size does not exceed 5.0 units, This is the resolution scaling factor. To impose a lower limit constraint on the calculation results, the step size is not less than 1.0 unit length.

[0040] On a coarse-resolution 3D mesh, with the current node Expanding the candidate nodes of the 26-neighborhood The candidate nodes satisfy ,in, The direction vector is the 26-neighborhood extension vector. and Meanwhile, by pruning the graph and filtering key nodes, the dimensionality of the state space is further reduced while maintaining the topological features, thus generating a set of candidate nodes.

[0041] Based on the candidate node set, a weighted heuristic function is used to calculate the priority of the expanded nodes. The node search order is determined by the weighted sum of the cumulative cost from the starting point to the current node and the heuristically estimated cost from the current node to the ending point. During the node search process, the weighted heuristic function is used to calculate the priority, and the heuristic function is expressed as follows: , in, Indicates the coordinates of the current node. Indicates the coordinates of the endpoint. This provides a heuristic estimate of the cost from the current node to the destination.

[0042] Then, a priority function is constructed based on the heuristic function to evaluate the estimated total cost from the starting point through the current node to the ending point. The priority function is a weighted sum of the actual cumulative cost from the starting point to the current node and the heuristically estimated cost, expressed as: , in, Indicates the distance from the starting point to the node. The cumulative cost, Indicates from node Heuristic cost estimation to the destination The weighting coefficient is 5.0 in this embodiment. This is used to adjust the weight of heuristic estimation costs in priority evaluation, if... Then update The parent node is And write it into the open list, where, Indicates from node To the node Path cost, From the starting point through the current node to candidate node The new cumulative cost is used to repeat the above expansion and priority calculation process until the open list contains the target node or the list is empty. Then, the target node is traced back to the starting node along the parent node pointer to obtain the topological sequence of the coarse-resolution guided path composed of several global waypoints.

[0043] Furthermore, in this embodiment, during the node expansion process, ray collision detection is performed on the connections between the current node and candidate nodes, and candidate nodes that are verified to be collision-free are included in the global search process. Considering that downsampling can introduce obstacle-crossing risks, especially in scenarios with thin-walled obstacles, where the obstacles are too thin to detect whether there are obstacles near the landing point (essentially, an obstacle-crossing event has occurred), this embodiment performs collision detection on the connections between the current node and candidate nodes. If the starting point of the line segment is... The endpoint is Then its distance is ,in, The Euclidean distance formula is used to calculate the Euclidean distance between the endpoints of a line segment. In this embodiment, during the expansion of the 26-neighborhood, not only is the collision state of the landing node detected, but also the connection between the current node and the candidate node is discretized and sampled using a parameterized ray equation. The parameterized ray equation is: , in, and Representing the start and end points of a line segment, they are typically tuples or arrays containing three elements: , . The normalization parameter is a scalar (real number) that determines the point. The relative position on the line segment. During downsampling A* expansion, for and The lines connecting them are of fixed step size. The default value should be less than 1 / 2 of the minimum thickness of the obstacle. In order to ensure that it does not pass through thin-walled obstacles, iterative sampling is performed to check whether each sampling point falls within the range of a sphere, axisymmetric bounding box, or directed bounding box. This solves the problem of "instantly" passing through thin-walled obstacles when the downsampling resolution is too large.

[0044] For axis-aligned bounding box obstacles, their center is denoted as The length of the half-axis is expressed as ,in, Indicates the center coordinates of the bounding box. Indicates the edge of the enclosure , , The length of the half-shaft of the shaft, , and Let x, y, and z represent the minimum coordinates of the bounding box along the x, y, and z axes, respectively. , and These are the coordinates of the maximum values ​​of the bounding box along the x, y, and z axes, respectively. , and These represent the semi-axis lengths along each coordinate axis.

[0045] The collision determination formula for spherical obstacles is as follows: , The collision determination formula for axis-aligned bounding box obstacles is: , The collision determination formula for directed bounding box obstacles is: , in To the center coordinates of the bounding box, Let the radius of the spherical obstacle be . It is the semi-major axis vector. Let be a rotation matrix. Let be the coordinates of the center of the spherical obstacle, and e be the semi-major axis vector of the directed bounding box. Represents the coordinates of the center of the spherical obstacle. Indicates the coordinates of the current node. These are the center coordinates of the bounding box.

[0046] For spherical obstacles, when A collision is detected when a sampling point is found to be inside an obstacle. If such a point exists within the sampling point, the connection is considered to have collided, and the candidate node is not included in the open list. Only when the connection satisfies boundary constraints and does not intersect with any type of obstacle is the candidate node allowed to be added to the open list. This ray collision detection mechanism effectively avoids the risk of penetrating thin-walled obstacles due to downsampling resolution, ensuring that the coarse-resolution guide path output from the backtracking has strict physical traversability.

[0047] like Figure 4 As shown, S3 uses the global waypoint sequence as a local sub-goal and performs Q-learning local programming with artificial potential field initialization.

[0048] This embodiment's local planner employs a Q-learning framework, using the next waypoint in the global waypoint sequence as the current sub-goal. In this embodiment's Q-learning framework, the state... The environment is identified using discrete grid coordinates based on three-dimensional Euclidean space. Specifically, the three-dimensional environment is mapped according to a preset resolution. Voxel segmentation is performed, and the intelligent agent is in The state at any given moment Represented as a triple To address the memory consumption issue caused by state explosion in three-dimensional space, this embodiment does not employ a pre-allocated dense matrix storage for the Q-table, but instead uses a sparse storage structure based on a hash table. The coordinates of the current state are used as the query key, and the corresponding action value function is dynamically initialized only when the agent explores a new state, implementing a 'lazy loading' mechanism. This significantly reduces the algorithm's memory requirements.

[0049] Local action space in this embodiment Defined as a set containing 26 discrete directions, encompassing the current node in the three-dimensional mesh space. The directions of its and all its adjacent nodes.

[0050] Action vector is represented as ,in, , and These represent the components of the action vector along the x, y, and z axes, respectively, with each component taking values ​​within a range of... And exclude the zero vector These 26 directions specifically include: Axial movements (6): along Movement in the positive and negative directions of the axis, such as wait; Planar angular movements (12): In Moving diagonally within a plane, such as wait; Spatial angular movements (8): Movement along the diagonal of a volume in three-dimensional space, such as... wait.

[0051] The analytical mapping relationship between action space and state transition is as follows: Let the current state of the agent be The selected action is ,in Then the predicted state at the next moment. The coordinates are calculated using the following linear mapping equation: , in, Current state coordinates For the selected action Components along each coordinate axis direction, This is the control resolution of the local planner. Predicting the state for the next moment Before executing the state transition, the system will call the collision detection module (ray detection mechanism) to determine the coordinates of the current state. arrive The connection segments between the points are subjected to a safety check. If the check passes, the state is updated; if a collision occurs, the current state is maintained and a negative reward value is assigned.

[0052] To avoid memory consumption issues caused by initializing a complete Q-table in a large three-dimensional space, this embodiment employs a lazy loading strategy, creating the action Q-value set only when state s is accessed for the first time. The action set uses 26 discrete actions, and any action is represented as... ,in And not all of them are zero at the same time, corresponding to the next state satisfying ,in, This indicates the unit step size (spatial resolution) of the grid map. Indicates action The next state coordinates after execution.

[0053] During Q-value initialization, the concept of an artificial potential field is introduced. The difference between the distance from the current position to the sub-target and the distance after executing the candidate action is used as the potential energy difference, and the corresponding action Q-value is initialized with this potential energy difference. Let the current sub-target be... ,but ,in, Indicates the current state coordinates. Indicates candidate actions, This indicates the coordinates of the next state after the action is performed. Indicates the coordinates of the current sub-target. This is the Euclidean distance function, multiplied by 10.0 as a scaling factor. Specifically, the formula for initializing the Q value of the potential energy difference is:

[0054] in Indicates the current state Waypoint distance, Indicates the next state Waypoint distance, The weights are guided by the potential field. In this way, the agent tends to explore along the direction of decreasing potential (i.e., away from obstacles and closer to the target) during the early stages of training. The Q-table is initialized using potential differences as prior knowledge to address the slow convergence speed of reinforcement learning. The agent moves according to the Q-table, and when it approaches... When switching targets to .

[0055] The trap map utilizes global waypoints provided by the 3D environmental semantic map to guide the local Q-learning planning process, bypassing the local extremum regions of the artificial potential field. The formula for global waypoint-guided local Q-learning is:

[0056] in and These are the gravitational and repulsive gain coefficients, respectively. The coordinates of the currently active global waypoint. For the locally detected first The center of an obstacle, For Euclidean distance To prevent infinitesimal constants from being divided by zero, if an action causes a state transition and collision with an obstacle, the Q value of that action is initialized to a preset negative value, preferably -1000.0, to reflect the repulsive force constraint of the obstacle.

[0057] S4. Based on the ε-greedy strategy, select actions from the action space and perform state transitions. In each local decision step, randomly select an action from the action space with probability ε for exploration, and select the action with the largest Q value in the current state with probability 1-ε for utilization, perform a state transition, and then perform ray collision detection on the state transition process, and assign corresponding rewards based on the detection results and position changes. Discretize and sample the connection from the current state to the next state based on the parameterized ray equation.

[0058] If a collision occurs between the current state and the next state, a collision penalty is applied and the state remains in place. The collision reward is preferably set to [value missing]. If there is no collision, a proximity reward is set based on the changes in distance between the current position, the next position, and the destination. The base reward is expressed as follows: .in, Indicates the coordinates of the endpoint. and These represent the Euclidean distances between the current position and the destination, and the next position and the destination, respectively. If the local path enters the threshold range of the current global waypoint, a waypoint reward is given and the path switches to the next global waypoint; if the final target threshold range is reached, a destination reward is given, and the reward update formula is as follows: , in, The distance from the target point in the previous state. The distance from the current state to the target point. This is the amplification factor for the distance control effect.

[0059] In large-scale 3D space, graph networks often require sparsification (downsampling) to ensure search efficiency. However, sparse graph nodes can lead to the loss of high-frequency geometric features of the environment (such as thin walls and small obstacles) in the graph representation. This "resolution bias" can easily cause spurious topological connectivity, meaning that two points are considered reachable by the graph search but are physically infeasible. To address the topological risks caused by graph network downsampling, a ray-casting edge verification mechanism is introduced in the graph search stage. It no longer only verifies the safety of individual "points" but also verifies the physical connectivity of "edges" between graph nodes in real time using ray equations. This ensures that even in extremely sparse graph structures, the generated path sequences possess strict physical traversability.

[0060] Based on the reward function, the Q-value of the selected action in the current state is updated online using the Bellman update formula, which is: , in, For learning rate, , This is called the discount factor. , State at time t Next action The value of state and action. The immediate reward obtained at time t+1; The agent is in a state at all times The value of a state action is After taking action Time to reach state And receive a reward At this point, the agent will find the state in the Q-table. The most valuable move is the next move. In order to obtain Thus, to Update.

[0061] Based on the local planning results, the risk level and distance attributes encoded in the 3D environmental semantic map are used to dynamically adjust the sub-objective switching threshold of the local planning; when the local planned path enters the current global waypoint, a preset switching threshold is set. When within range, the current global waypoint is switched to the next global waypoint. Graph networks establish static topological connections, while actual path generation needs to consider spatial continuity. By perceiving the semantic environment complexity around graph nodes, it adaptively seeks a balance between following the global topology and local autonomous obstacle avoidance, ensuring the global connectivity of the escort path in complex graph environments. The basic resolution and coarse-grained step size satisfy the following formula: , , in, , and These represent the physical lengths of the environmental space along each coordinate axis. The preset number of global grid divisions, The base resolution represents the unit length after dividing the largest dimension of the environment space into N equal parts. The step size is coarse-grained, representing the actual global planning resolution after adjustment for obstacle complexity coefficients. This is the obstacle complexity coefficient, used to adaptively adjust the step size based on the obstacle density in the environment. If the obstacle density in the area is high, it is reduced. To enhance the ability to traverse small, confined spaces, and to increase the ability to navigate open spaces. To achieve rapid jumping; This is a rounding operation used to round the adjusted resolution to the discrete grid step size.

[0062] In addition, this implementation has a preset switching threshold. With global planning resolution A positive correlation is usually taken. When the agent enters a region centered at the current waypoint with a radius of... When the neighboring node is found, the topology guidance objective for this stage is considered complete, and the next path point is automatically attached as the gravitational source of the artificial potential field. During local planning, if oscillations or repeated visits are detected in the local path, random action perturbation or greedy correction is executed to escape the local deadlock region and improve the success rate of path generation. The random action perturbation adopts an ε-greedy strategy, and its strategy probability satisfies the following formula: , in, Select an action in state s The strategy probability, For the currently selected action, This is the current state. An exploration factor, used to balance the ratio of exploration to utilization. For the action space, Candidate actions in the action space, To select the action from action space A that maximizes the value of the state action.

[0063] S5. Based on the local planning results, iteratively execute local planning actions, state transition ray collision detection, action Q-value Bellman online update, and global waypoint sub-target switching. When the agent reaches the preset threshold range of the final target point, the local planning iteration process is terminated, and a state coordinate sequence from the starting point to the end point is obtained. The global waypoint guidance results and local decision results are integrated, and the state coordinates of each iteration step are concatenated in time order to output a complete continuous three-dimensional path. The complete continuous three-dimensional path consists of the three-dimensional coordinate points at each moment in the local planning process, reflecting the actual movement trajectory of the agent from the starting point to the end point. The planning results of the complete continuous three-dimensional path are evaluated, and the evaluation indicators include planning success rate, path length, path smoothness, and average time consumption.

[0064] Example 2 The difference between this embodiment and embodiment 1 is that this embodiment provides a method for constructing a 3D environment graph for knowledge graph / graph network extraction scenarios; In this embodiment, the start point, end point, free space nodes, obstacle entities, node adjacency relationships, edge travel costs, obstacle types, risk levels, and waypoint hierarchical relationships in the 3D environment are organized into graph triples or attribute graph data. For example, relationships such as "node-connected-node", "node-nearby obstacle-obstacle", and "node-belonging hierarchy-global channel segment" can be constructed to support subsequent graph database storage, graph mining analysis, and graph neural network training.

[0065] Based on the above graph representation, preprocessing operations such as graph trimming, key node filtering, and semantic annotation of important obstacles can be performed before planning; or the generated path can be written back to the graph after planning to form relational data such as "path-passage-node" and "path-avoidance-obstacle", providing an engineering foundation for the extension of patent solutions to graph networks and knowledge graphs.

[0066] To verify the performance of a hierarchical path planning method based on a 3D graph network, we conducted random seed evaluations on trap maps, 20*20*20 maps, and 50*50*50 maps. The experiments evaluated traditional Q-Learning, artificial potential field algorithms, and RRT* algorithms as comparative baselines for the performance evaluation of the hierarchical path planning method based on a 3D graph network proposed in this embodiment.

[0067] Table 1 Evaluation results of each algorithm on the trap map

[0068] like Figure 5 As shown in Table 1, four algorithms were compared based on a trap map. Their evaluation results indicate that, in terms of planning success rate, the presence of traps caused Q-learning to frequently hit walls and engage in meaningless random movements, further increasing path length, making the path less smooth, and increasing time consumption. In this situation, the APF algorithm reached an extreme value; if the attraction was strong, it would oscillate upwards until avoiding obstacles; if the repulsion was strong, it would stop. RRT* performed better in terms of path smoothness and path length, while the hierarchical path planning method based on a 3D graph network proposed in this invention had a shorter time consumption and superior overall performance. It should be noted that in the multiple rounds of evaluation tests using the random seeds, the planning success rate of all algorithms did not reach 100%. The objective reason for the very few failures (approximately 5%) is that, under a purely random generation mechanism, there is an extremely low probability of generating 'extremely unsolvable topologies' (such as the starting or ending point being completely surrounded by a group of obstacles).

[0069] Table 2 Evaluation results of each algorithm on a 20*20*20 map;

[0070] like Figure 8 , Figure 9As shown in Table 2, the four algorithms are compared using a 20*20*20 map. The evaluation results are shown in Table 2. Thanks to the relatively small and complex map size, the Q-Learning Q-table can be stored within memory. Furthermore, when the number of episodes is limited, although the target point can still be reached, the path length increases, path smoothness decreases, and the running time is shortened. The APF algorithm, benefiting from its movement based on the attraction of the target point, the repulsion of obstacles, and potential energy difference, has a simpler calculation process and shorter running time, but it is also more susceptible to parameter influences. The performance varies significantly depending on the parameters and map. The RRT* algorithm, benefiting from its random sampling mechanism, exhibits strong adaptability to high-dimensional spaces. Its incremental optimization mechanism, independent of gridded modeling, broadens its applicability, and its paths are shorter and smoother compared to APF and Q-Learning. While the proposed algorithm, implemented in this paper, doesn't show a significant performance advantage in smaller spaces, its lazy loading mechanism and potential difference initialization of the local Q-table reduce memory burden. It also demonstrates strong overall performance in terms of success rate, path smoothness, path length, and average time consumption. It should be noted that in the multi-round evaluation tests using the aforementioned random seeds, the planning success rate of all algorithms did not reach 100%. The objective reason for the very few failures (approximately 5%) is that, under a purely random generation mechanism, there is an extremely low probability of generating 'extremely unsolvable topologies' (such as the starting or ending point being completely surrounded by a group of obstacles), or failing to find a feasible solution within the set maximum algorithm iteration steps / time threshold.

[0071] Table 3 Evaluation results of each algorithm on a 50*50*50 map;

[0072] like Figure 6 , Figure 7As shown in Table 3, the four algorithms were compared using a 50*50*50 map. The evaluation results are shown in Table 3. By controlling the number of obstacles on the map with a random number seed, it is clear that under the same environment, the significantly larger map environment and the increased number of spatial states lead to a "dimensionality explosion" in Q-Learning. Its memory becomes overburdened due to the excessive number of states, and because of the excessive number of states, Q-Learning has to spend more episodes trying new paths to successfully plan a path, thus accumulating rewards to update the value of each state. As a result, its performance in terms of average time, path length, and path smoothness significantly decreases. In contrast, the RRT* algorithm benefits from its random sampling. The point-based mechanism is highly adaptable to high-dimensional spaces. It employs a progressive optimization mechanism and does not rely on gridded modeling, thus demonstrating strong performance in significantly larger environments like 50x50x50 maps. In contrast, the APF algorithm's performance varies depending on parameter configuration, making it difficult to achieve optimal overall results. The algorithm proposed in this embodiment benefits from lazy loading and potential difference initialization of the local Q-table, reducing memory burden compared to Q-Learning which loads all state values ​​into the Q-table at once. Furthermore, the layered global guidance combined with local path planning ensures strong overall performance in terms of success rate, path smoothness, path length, and average time on a 50x50x50 map. It should be noted that in the multi-round evaluation tests using the random seed algorithm, none of the algorithms achieved a 100% success rate. The objective reason for the very few failures (about 5%) is that, under the pure random generation mechanism, the map itself is relatively large and there is a very low probability of generating an 'extremely unsolvable topology' (such as the starting point or ending point being completely closed and surrounded by a group of obstacles), or no feasible solution is found within the set maximum number of algorithm iterations / time threshold.

[0073] Example 3 A hierarchical path planning system based on a 3D graph network includes: The map atlas construction module is configured to: acquire 3D environmental data of the target area, perform graph network representation of free space, obstacle boundaries, start point and end point, and construct a 3D environmental semantic graph; The global planning module is configured to perform downsampling global search based on the 3D environment semantic graph and output a coarse-resolution guided path composed of several global waypoints through a dynamic resolution strategy and collision detection mechanism. The local decision-making module is configured to: use the global waypoint sequence as the local sub-target sequence, construct a Q-learning local planner based on the prior of the artificial potential field, initialize the action Q value in the current state according to the potential energy difference, and execute the local planning action in real time. The collision detection module is configured to perform collision detection on state transitions during local planning and on node connections during global search. The path output module is configured to automatically switch to the next global waypoint when the agent enters the preset switching threshold range of the current global waypoint, and continue to execute the local planning and sub-goal switching process until the final goal point is reached, and output a complete continuous three-dimensional path.

[0074] The above are all preferred embodiments of the present invention and are not intended to limit the scope of protection of the present invention. Therefore, all equivalent changes made in accordance with the structure, shape and principle of the present invention should be covered within the scope of protection of the present invention.

Claims

1. A hierarchical path planning method based on three-dimensional graph networks, characterized in that, include: Acquire 3D environmental data of the target area, represent free space, obstacle boundaries, start point and end point using graph network, and construct a 3D environmental semantic graph. Based on the 3D environment semantic graph, a downsampling A* global search is performed, and a coarse-resolution guided path composed of several global waypoints is output through a dynamic resolution strategy and a collision detection mechanism. Using the global waypoint sequence as the local sub-target sequence, a Q-learning local planner based on artificial potential field prior is constructed, and the action Q value in the current state is initialized according to the potential energy difference; The initialization of the action Q-value based on the potential energy difference includes calculating the distance from the current state to the current sub-target waypoint, and the distance from the next state after executing the candidate action to the current sub-target waypoint, to obtain the potential energy difference. The Q-value of the corresponding action is initialized based on the potential energy difference and the potential field guidance weight coefficient. When the state transition corresponding to the candidate action collides with an obstacle, the Q-value of that action is initialized to a preset negative value to form an obstacle avoidance prior. The expression for initializing the Q-value of the corresponding action is: ， in, Actions under state s The initial Q value, The Euclidean distance from the current state s to the current sub-goal waypoint is... To perform the action Next state Euclidean distance to the current sub-target waypoint The weighting coefficient is used to guide the potential field. The initialization of the action Q value in the current state based on the potential energy difference also includes constructing a gravitational potential field based on the coordinates of the currently activated global waypoint and constructing a repulsive potential field based on the locally detected obstacle center. The superposition result of the gravitational and repulsive potential fields is used as the basis for initializing the state Q value. The potential field initialization for global waypoint-guided local Q learning satisfies the following equation: ， in, Let Q be the initial Q value for state s. This is the gravitational gain coefficient. The repulsive force gain coefficient, The coordinates of the currently active global waypoint. For the center of the i-th obstacle detected locally, Let be the Euclidean distance function. To prevent the numerically stable term from being divided by zero, This represents the number of obstacles detected locally. The local planner executes local planning actions in real time, performs collision detection during the state transition process, and updates the action Q value online based on the reward function and Bellman update formula. When the agent enters the preset switching threshold range of the current global waypoint, it automatically switches to the next global waypoint. The preset switching threshold is positively correlated with the global planning resolution. Its benchmark value is determined based on the global planning resolution and obstacle complexity coefficient, and is dynamically adjusted during the local planning process using the risk level and distance attributes encoded in the three-dimensional environment semantic graph. Iteratively execute the local planning and sub-objective switching process until the final target point is reached, and output a complete continuous 3D path.

2. The hierarchical path planning method based on a three-dimensional graph network according to claim 1, characterized in that, The graph network representation of free space, obstacle boundaries, start point, and end point includes: Acquire 3D environmental point cloud or voxel data of the target area, extract environmental boundaries, obstacle geometric features, start coordinates and end coordinates, and establish a 3D spatial index structure; Based on the three-dimensional spatial index structure, voxel rasterization is performed on the target area, the continuous three-dimensional space is discretized into several voxel units, and the free space is extracted according to the occupancy relationship between each voxel unit and the obstacle geometry and the environmental boundary. The free space is discretized into passable sampling nodes, with the start point and end point as the starting node and the target node, respectively, and the three-dimensional spatial coordinates of each sampling node as the basic attribute of the node. Perform neighborhood connectivity analysis on accessible sampling nodes, establish graph edges for node pairs that meet the collision-free connectivity condition after collision detection, and use the spatial distance between nodes as the basic attribute of the edges. The obstacle type, boundary constraints, distance to the target, and waypoint hierarchy information are encoded as node attributes or edge attributes to obtain a three-dimensional environment semantic graph.

3. The hierarchical path planning method based on a three-dimensional graph network according to claim 1, characterized in that, The output consists of a coarse-resolution guided path composed of several global waypoints, including: The coarse resolution step size is adaptively determined based on the map physical scale and resolution scaling factor, and twenty-six neighborhood node expansion is performed on the coarse resolution grid. The priority of the expanded nodes is calculated using a weighted heuristic function. The node search order is determined by the weighted sum of the cumulative cost from the starting point to the current node and the heuristically estimated cost from the current node to the end point. During node expansion, collision detection is performed on the connections between the current node and candidate nodes. Candidate nodes that are verified to be collision-free are included in the global search process, and a coarse-resolution guided path consisting of several global waypoints is output. The formula for calculating the coarse-resolution step size is: ， in, For coarse resolution step size, The physical scale of the current map. To impose an upper limit constraint on the calculation results, This is the resolution scaling factor. To impose a lower bound constraint on the calculation results.

4. The hierarchical path planning method based on a three-dimensional graph network according to claim 3, characterized in that, The collision detection of the connection between the current node and the candidate node includes, during the 26-neighborhood expansion of the downsampling A* global search, discretizing the connection between the current node and the candidate node with a fixed step size using a parameterized ray equation, and verifying the geometric intersection relationship between each sampling point and the obstacle one by one. When there is a sampling point located inside the obstacle, it is determined that the connection has collided. The parameterized ray equation is: ， in, Let these be the coordinates of the sampling points on the line segment. and These are the three-dimensional coordinates of the start and end points of the line segment, respectively. These are the normalized parameters.

5. The hierarchical path planning method based on a three-dimensional graph network according to claim 1, characterized in that, The construction of the Q-learning local planner based on artificial potential field priors includes: The three-dimensional environment is segmented into voxels according to a preset resolution, the state of the agent is identified by discrete grid coordinates, and the current state coordinates are used as the query key. A set of Q-values ​​is established using a sparse storage structure based on hash tables. The corresponding action value function is dynamically initialized only when the agent explores a new state, thus implementing a lazy loading mechanism. Establish an action space containing multiple discrete directions, wherein the action vector is represented as follows: The values ​​of each dimension component are taken from And they are not all zero at the same time; Based on the current state coordinates and the selected action vector, the predicted state coordinates for the next moment are calculated through a linear mapping. The expression for the linear mapping is: ， in, Current state coordinates For the selected action Components along each coordinate axis direction, This is the control resolution of the local planner. Predicting the state for the next moment The coordinates.

6. The hierarchical path planning method based on a three-dimensional graph network according to claim 1, characterized in that, The online update of the action Q value includes selecting an action from the action space and performing a state transition based on an ε-greedy strategy, wherein an action is randomly selected from the action space with probability ε, and the action with the largest Q value in the current state is selected with probability 1-ε. Ray collision detection is performed on the line connecting the current state to the next state. When the detection result is a collision, a collision penalty is imposed and the current state is maintained. When the detection result is no collision, a convergence reward is imposed based on the distance changes between the current state and the next state and the endpoint and the current sub-target, respectively. When the agent enters the preset threshold range of the current global waypoint, a waypoint switching reward is given; when the agent reaches the preset threshold range of the final destination, a destination reward is given. The Q-value of the selected action in the current state is updated online based on the Bellman update formula.

7. The hierarchical path planning method based on a three-dimensional graph network according to claim 6, characterized in that, The automatic switching to the next global waypoint includes determining a preset switching threshold based on the global planning resolution and obstacle complexity coefficient. When a locally planned path enters the distance threshold range of the current global waypoint, the current global waypoint is switched to the next global waypoint. During local planning, when a local path is detected to be oscillating or repeatedly visited, a random action perturbation is introduced to break out of the local deadlock region. The expression for the random action perturbation is: ， in, Select an action in state s The strategy probability, For the currently selected action, This is the current state. An exploration factor, used to balance the ratio of exploration to utilization. For the action space, Candidate actions in the action space, To select the action from action space A that maximizes the value of the state action.

8. A hierarchical path planning system based on a three-dimensional graph network, comprising the method described in any one of claims 1 to 7, characterized in that, include: The map atlas construction module is configured to: acquire 3D environmental data of the target area, perform graph network representation of free space, obstacle boundaries, start point and end point, and construct a 3D environmental semantic graph; The global planning module is configured to perform downsampling global search based on the 3D environment semantic graph and output a coarse-resolution guided path composed of several global waypoints through a dynamic resolution strategy and collision detection mechanism. The local decision-making module is configured to: use the global waypoint sequence as the local sub-target sequence, construct a Q-learning local planner based on the prior of the artificial potential field, initialize the action Q value in the current state according to the potential energy difference, and execute the local planning action in real time. The collision detection module is configured to perform collision detection on state transitions during local planning and on node connections during global search. The path output module is configured to automatically switch to the next global waypoint when the agent enters the preset switching threshold range of the current global waypoint, and continue to execute the local planning and sub-goal switching process until the final goal point is reached, and output a complete continuous three-dimensional path.

Images