Unmanned platform path planning method and system

Abstract

The application relates to the technical field of autonomous navigation and path planning of unmanned systems, and discloses an unmanned platform path planning method and system, which comprises the following steps: constructing a three-dimensional adaptive map; defining a state vector and motion constraint parameter of the unmanned platform, and initializing an open list and a closed list; constructing a finite set of three-dimensional motion primitives, taking a current node as a starting point, parameterizing sampling of each type of motion primitive, generating a candidate continuous motion segment, taking an end state of the candidate continuous motion segment as a candidate sub-node, and dynamically adjusting a current expansion step length through an adaptive step length model; checking the generated candidate continuous motion segment, calculating an actual cost increment from the starting point to the candidate node which passes the check, and adding the candidate node to the open list, thereby completing one expansion iteration of a three-dimensional hybrid A* algorithm; checking the candidate continuous motion segment, and eliminating unqualified candidate items; and updating the open list and the closed list, so that the optimal path can be efficiently planned.

Description

Technical Field

[0001] This invention relates to the field of autonomous navigation and path planning technology for unmanned systems, and particularly to a path planning method and system for unmanned platforms. Background Technology

[0002] In complex 3D operational scenarios such as field exploration, mountain logistics, and urban three-dimensional security, the autonomous navigation capability of unmanned platforms is crucial. The core challenge lies in the fact that the planning algorithm must not only generate geometrically collision-free paths in environments with drastic elevation changes and three-dimensional obstacles, but also ensure that the path conforms to the platform's own nonholonomic constraints (such as the inability to move laterally) and dynamic constraints (such as maximum turning rate and climb angle). In other words, the path must be continuous and directly executable. Traditional 2D path planning methods suitable for simple environments are completely ineffective in these situations, and research focuses on two types of 3D planning techniques:

[0003] The first category is sampling-based stochastic programming algorithms, such as Rapid Expanding Random Tree (RRT) and its variants. These methods explore the environment by randomly sampling and connecting nodes in the configuration space, and their advantage lies in their ability to relatively easily incorporate complex kinematic constraints. However, their inherent drawback is that the randomness of the planning leads to unstable and unpredictable quality of the generated paths (such as length and smoothness), and it is difficult to guarantee global optimality under all circumstances; convergence speed can be extremely slow in open regions.

[0004] The second category is deterministic algorithms based on gridded search, such as the 3D A* algorithm. This type of method discretizes the environment into a uniform 3D grid and finds a path by systematically searching the grid nodes, ensuring global optimality within the discretized space. However, its fundamental limitation lies in the severe mismatch between the discrete, isotropic grid node expansion method (typically allowing movement to 26 neighborhoods) and the continuous, anisotropic real-world motion capabilities of the unmanned platform. The optimal path found by the algorithm often consists of a series of polyline segments that ignore the platform's turning and climbing capabilities; it is geometrically optimal but kinematically infeasible. Although subsequent processing can be achieved using trajectory smoothing techniques, this post-processing step is computationally complex and easily compromises the collision-free and optimal nature of the path.

[0005] To reconcile the conflict between motion feasibility and planning optimality, the hybrid A* algorithm has achieved success in the two-dimensional domain. It directly generates paths satisfying vehicle kinematic constraints during the search process by searching in a continuous state space but using a discrete control set for node expansion. However, when extending it directly to three-dimensional space, the following structural challenges lead to the poor performance of existing three-dimensional hybrid A* schemes:

[0006] First, the design of motion primitives is flawed. Most solutions fail to construct native 3D motion primitives that truly reflect the platform's coupled motion characteristics in 3D space, resulting in extended path segments that are still kinematically crude or even infeasible.

[0007] Secondly, the search guidance mechanism is inefficient. Traditional heuristic functions perform a large number of invalid searches in the huge three-dimensional state space, resulting in low planning efficiency.

[0008] Finally, the environmental representation lacks intelligence. When using fixed-resolution 3D raster maps for collision detection, it is impossible to achieve an adaptive balance between computational efficiency and environmental modeling accuracy: in complex terrain areas, fine rasters lead to computational overload; in flat areas, coarse rasters may lose details.

[0009] Therefore, how to design a method that can efficiently plan a path in a complex three-dimensional scene that satisfies both the kinematic and dynamic constraints of the platform and ensures global optimality has become an urgent problem for those skilled in the art. Summary of the Invention

[0010] To address the aforementioned technical problems, this invention provides a path planning method and system for unmanned platforms, which can efficiently plan paths that satisfy both the platform's kinematic and dynamic constraints and ensure global optimality.

[0011] The first aspect of this invention provides a path planning method for an unmanned platform, comprising the following steps: Construct a 3D adaptive map that includes terrain elevation and obstacle information; Define the state vector of the unmanned platform, define the starting state and the ending state, and set the platform motion constraint parameters; initialize the open list and the closed list, where the open list uses a priority queue to store nodes to be expanded, and the closed list stores valid nodes that have been expanded. Based on the defined state vector, the defined start state and end state, and motion constraints, a finite set of three-dimensional motion primitives is constructed, with each motion primitive corresponding to a continuous motion segment. Taking the current node as the starting point, parameterized sampling is performed on each type of motion primitive to generate candidate continuous motion segments and their end states are used as candidate child nodes. According to the distance between the current node and the target state, the terrain slope, and the three-dimensional distance to the nearest obstacle, the current expansion step size is dynamically adjusted through an adaptive step size model. Three-dimensional collision detection and motion constraint verification are performed on the generated candidate continuous motion segments. For candidate nodes that pass the verification, the actual cost increment from the starting point to the node is calculated and added to the open list, completing one expansion iteration of the three-dimensional hybrid A* algorithm. For each candidate node in the open list, calculate its actual cost and heuristic cost. The sum of the two is the total cost. Select the node with the smallest cost as the next expansion node and move it into the closed list. Continue until the 3D coordinate error between the current expansion node and the endpoint is less than a set threshold. When the endpoint convergence condition is met, the current node is used to backtrack to the starting node based on the parent node information to generate a continuous path from the starting point to the endpoint.

[0012] Optionally, construct a 3D adaptive map that includes terrain elevation and obstacle information, including the following steps: A three-dimensional adaptive map is constructed using digital elevation maps, and the grid size of the map is dynamically adjusted according to the terrain slope and obstacle distribution density. The grid base size is set according to the maximum slope of the environment, and the adaptive grid size is adjusted according to the slope of the local area.

[0013] Optionally, the state vector of the unmanned platform is: ,in, For three-dimensional position coordinates, Yaw angle The pitch angle; Motion constraint parameters include: minimum turning radius Maximum climb angle and safe distance .

[0014] Optionally, the three-dimensional motion elements include ten basic motion modes: horizontal forward straight line, horizontal forward left turn, horizontal forward right turn, vertical forward climb, vertical forward dive, horizontal backward straight line, horizontal backward left turn, horizontal backward right turn, vertical backward climb, and vertical backward dive. The trajectory generation of each basic motion mode follows the constraints of the platform's turning speed, vertical speed of climb or dive, and attitude change angle.

[0015] Optional, expand step size This can be expressed by the following formula: , in, The initial reference step size, This is the step size adjustment factor. This represents the current node state. This is the final state. For terrain slope, The 3D distance between the current node and the nearest obstacle; step size adjustment factor. The system dynamically adjusts its step size based on the distance between the current node and the target, the terrain slope, and the distance to obstacles, so that the step size increases when the node is far away from the target, the terrain is flat, and the obstacle is far away, and decreases when the node is far away from the obstacle.

[0016] Optional step size adjustment factor Determined using the linear weighting formula: , in, Let be the weighting coefficient, satisfying , The target distance normalization factor, The terrain slope normalization factor, This is the obstacle distance normalization factor.

[0017] Optionally, the state of candidate nodes is updated using the following general model: , in, The three-dimensional coordinates of the candidate nodes For dynamic step size, The yaw angle of the candidate node. Candidate node pitch angle, This represents the change in yaw angle. This represents the change in pitch angle.

[0018] Optional, actual cost Combined path length and pose smoothness penalty; heuristic cost ,in, To simulate the motion cost of a platform under nonholonomic constraints in an obstacle-free environment, The shortest grid path length from the current node to the destination, obtained using the 3D A* algorithm; total cost. .

[0019] Optionally, when the extended node meets the endpoint convergence condition, an initial path from the starting point to the endpoint is generated by backtracking through the parent node information. This also includes: verifying the feasibility and smoothing the path; wherein the endpoint convergence condition is that the positional error between the current extended node and the endpoint in three-dimensional space is less than a preset threshold. threshold Related to map scale and baseline step size: , in, As the reference step size, 3D maps in Dimensions of direction.

[0020] A second aspect of the present invention provides a system for path planning of an unmanned platform, comprising: The map building module is used to build a 3D adaptive map that includes terrain elevation and obstacle information; The state initialization module is used to define the state vector of the unmanned platform, define the starting state and the ending state, and set the platform motion constraint parameters; it also initializes the open list and the closed list, where the open list uses a priority queue to store nodes to be expanded, and the closed list stores valid nodes that have been expanded. The node expansion module is used to construct a finite set of three-dimensional motion primitives based on the defined state vector, the defined start state and end state, and motion constraints. Each motion primitive corresponds to a continuous motion segment. Starting from the current node, it performs parameterized sampling on each type of motion primitive to generate candidate continuous motion segments and uses their end states as candidate child nodes. Based on the distance between the current node and the target state, the terrain slope, and the three-dimensional distance to the nearest obstacle, it dynamically adjusts the current expansion step size through an adaptive step size model. It performs three-dimensional collision detection and motion constraint verification on the generated candidate continuous motion segments. For candidate nodes that pass the verification, it calculates the actual cost increment from the start point to the node and adds it to the open list, completing one expansion iteration of the three-dimensional hybrid A* algorithm. The node filtering module is used to calculate the actual cost and heuristic cost of each candidate node in the open list. The sum of the two is the total cost. The node with the lowest cost is selected as the next expansion node and moved into the closed list until the three-dimensional coordinate error between the current expansion node and the endpoint is less than the set threshold. The path backtracking module is used to generate a continuous path from the starting point to the ending point by gradually backtracking from the current node to the starting point node based on the parent node information when the endpoint convergence condition is met.

[0021] The technical solution provided by the embodiments of the present invention has the following advantages compared with the prior art: This invention provides a path planning method and system for unmanned platforms. By introducing a 3D adaptive map construction, a node expansion mechanism with consistent 3D constraints, and a matching cost filtering strategy, it systematically solves the problem that existing 3D path planning methods struggle to balance motion feasibility, planning efficiency, and environmental adaptability. During node expansion, a finite set of 3D motion primitives conforming to the 3D spatial motion characteristics of the unmanned platform is constructed. By ensuring that each expanded continuous motion segment satisfies the platform's nonholonomic constraints and dynamic feasibility, the search process directly generates executable paths, avoiding the unreliable cycle of planning-smoothing-verification in traditional methods, significantly improving path usability and execution success rate. By parametrically sampling the motion primitives during node expansion and combining 3D collision detection and constraint verification, a large number of invalid candidates are effectively eliminated, increasing the proportion of effective expansions. Simultaneously, the cost function filters and optimizes nodes, guiding the search towards directions that satisfy constraints and offer better costs, thereby achieving efficient and directional searching in a vast 3D state space. By constructing a 3D adaptive map, the map's representation accuracy can be dynamically adjusted according to the complexity of the terrain (such as slope and obstacle density). By using a coarser resolution in flat areas to reduce computational overhead and a finer resolution in complex areas to ensure obstacle avoidance accuracy, the overall allocation of computational resources is optimized, providing an efficient and accurate environmental model for subsequent path search. Attached Figure Description

[0022] Figure 1 A flowchart of an unmanned platform path planning method provided in an embodiment of the present invention; Figure 2 A structural diagram of a system for path planning of an unmanned platform provided in an embodiment of the present invention; Figure 3 The simulation results of the planned path provided in the embodiment of the present invention are shown in the figure. Detailed Implementation

[0023] The following detailed description of a specific embodiment of the present invention is provided in conjunction with the accompanying drawings. However, it should be understood that the scope of protection of the present invention is not limited to the specific embodiment.

[0024] In the description of this invention, it should be understood that the terms "center," "longitudinal," "lateral," "length," "width," "thickness," "upper," "lower," "front," "rear," "left," "right," "vertical," "horizontal," "top," "bottom," "inner," "outer," "axial," "radial," and "circumferential" 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 technical solution of this 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 this invention.

[0025] The present invention will be described below through several specific embodiments. To keep the following description of the embodiments clear and concise, detailed descriptions of known functions and components may be omitted. When any component of an embodiment of the present invention appears in more than one drawing, the component may be represented by the same reference numerals in each drawing.

[0026] The first part of the embodiments of the present invention provides a path planning method for an unmanned platform, including the following steps: Construct a 3D adaptive map that includes terrain elevation and obstacle information; Define the state vector of the unmanned platform, define the starting state and the ending state, and set the platform motion constraint parameters; initialize the open list and the closed list, where the open list uses a priority queue to store nodes to be expanded, and the closed list stores valid nodes that have been expanded. Based on the defined state vector, the defined start state and end state, and motion constraints, a finite set of three-dimensional motion primitives is constructed, with each motion primitive corresponding to a continuous motion segment. Taking the current node as the starting point, parameterized sampling is performed on each type of motion primitive to generate candidate continuous motion segments and their end states are used as candidate child nodes. According to the distance between the current node and the target state, the terrain slope, and the three-dimensional distance to the nearest obstacle, the current expansion step size is dynamically adjusted through an adaptive step size model. Three-dimensional collision detection and motion constraint verification are performed on the generated candidate continuous motion segments. For candidate nodes that pass the verification, the actual cost increment from the starting point to the node is calculated and added to the open list, completing one expansion iteration of the three-dimensional hybrid A* algorithm. For each candidate node in the open list, calculate its actual cost and heuristic cost. The sum of the two is the total cost. Select the node with the smallest cost as the next expansion node and move it into the closed list. Continue until the 3D coordinate error between the current expansion node and the endpoint is less than a set threshold. When the endpoint convergence condition is met, the current node is used to backtrack to the starting node based on the parent node information to generate a continuous path from the starting point to the endpoint.

[0027] This invention provides a path planning method for unmanned platforms. Its core lies in extending the traditional hybrid A* node expansion mechanism from a two-dimensional plane to a three-dimensional space. Under set dynamic constraints, it constructs motion primitives that satisfy three-dimensional kinematics and dynamic constraints. Based on this set of motion primitives, feasible node expansion and three-dimensional collision verification are performed, ensuring that the path segments generated during the search process are inherently executable. Furthermore, by combining a three-dimensional adaptive map with constraint-aware cost evaluation, stable planning and continuous executable path output are achieved in complex three-dimensional scenarios. This makes it suitable for the three-dimensional autonomous navigation needs of various unmanned platform tasks, such as security patrols, material transportation, and emergency rescue. The method has a clear structure, flexible parameter settings, and strong adaptability. It can be widely applied to three-dimensional autonomous navigation tasks of various types of unmanned platforms, including ground, air, surface, and underwater environments, demonstrating good engineering practicality and scalability.

[0028] Optionally, terrain and elevation data of the target's 3D environment can be acquired using environmental sensing devices, and a 3D adaptive map can be constructed using a digital elevation map (DEM). During this process, the raster reference size... The setting depends primarily on the maximum slope of the terrain. The calculation formula is as follows:

[0029] , in, The angle (in degrees) representing the maximum slope in the environment determines the accuracy requirements of the raster. When the slope is large, the terrain changes more drastically, so a smaller raster size is needed to depict the undulations of the terrain in detail. This is a constant representing the scaling factor of the grid reference size, measured in meters. Its value typically ranges from 1 to 10, and the specific value can be determined based on the platform's operating environment, performance, and computing power. For relatively flat environments, this constant... It can be relatively large, but for complex terrain, it should be appropriately reduced. To improve accuracy.

[0030] In 3D adaptive map construction, the raster size depends not only on the baseline size but also needs to be adjusted according to the specific terrain features of the environment. Specifically, the raster size... At grid reference size Based on the slope of the terrain The distribution density of obstacles changes dynamically to adapt to the needs of different areas. For relatively flat areas, if... When the grid is small and the distribution density is low, the grid size can be larger, which helps reduce computational complexity and improve planning efficiency; however, in complex areas or areas with dense obstacles, if When the grid size is large and the distribution density is high, the grid size needs to be reduced to ensure detailed terrain depiction and obstacle avoidance accuracy. The specific calculation method is as follows:

[0031] , in: It is an adaptive grid size. It is the grid reference size. It is the maximum slope angle in the environment. It refers to the slope of a local area.

[0032] By constructing a 3D adaptive map, the computational scale can be controlled while ensuring the accuracy of the representation of 3D structures and obstacles.

[0033] Optionally, define the state vector of the unmanned platform, and define the state vector of the unmanned platform search node as follows: , in, For three-dimensional position coordinates, Yaw angle This refers to the pitch angle (or equivalent slope attitude parameter). Set the starting state. With the final state And clearly define the set of motion constraint parameters for the unmanned platform, including the minimum turning radius. Maximum climb angle and safe distance Among them, the maximum climb angle safe distance The minimum turning radius is determined by the dynamic constraints of the unmanned platform itself. Satisfy the following formula:

[0034] , in, It is the acceleration due to gravity. For the instantaneous speed of unmanned platforms, The maximum roll angle of the unmanned platform is set to ensure that the motion constraint parameters match the actual physical characteristics of the unmanned platform. Initialize the open and closed lists: the open list uses a priority queue to store nodes to be expanded, and the nodes to be expanded are randomly expanded outwards using motion primitives. Node information includes 3D coordinates, attitude angle, and total cost. Actual cost Heuristic Costs And the parent node index; disable the list storage for valid nodes that have completed expansion to avoid duplicate calculations.

[0035] Under the premise of satisfying the motion attributes of an omnidirectional unmanned platform, ten basic motion modes are defined: horizontal forward straight line, horizontal forward left turn, horizontal forward right turn, vertical forward climb, vertical forward dive, horizontal backward straight line, horizontal backward left turn, horizontal backward right turn, vertical backward climb, and vertical backward dive. The trajectory generation of each basic motion mode follows the following constraints to ensure the feasibility and safety of the motion: , in: It's the turning speed. Maximum permissible turning speed; It is the vertical speed of climbing or diving. Maximum permissible climb or dive speed; It is the angle of attitude change. These three parameters, representing the maximum permissible angle of attitude change, are all determined by the physical properties of the unmanned platform itself.

[0036] All motion modes uniformly follow a constraint-oriented candidate node generation logic. By selecting motion modes that conform to the platform's kinematic and dynamic constraints, and simultaneously expanding nodes according to an adaptive step size, the generated paths are ensured to be physically feasible and meet obstacle avoidance requirements. A multi-factor coupled adaptive step size model is constructed, and the dynamic step size is calculated using the following formula:

[0037] , in, The initial reference step size is set similarly to the grid reference size, and is related to the maximum slope of the map and the map size. The larger the map size, the longer the reference step size should be. This is the step size adjustment factor. This represents the current node state. This is the final state. For terrain slope, This represents the 3D distance between the current node and the nearest obstacle. Step size adjustment factor. The step size is dynamically adjusted based on the distance between the current state and the target state, the terrain slope, and the distance to obstacles. The target distance factor increases the step size when the platform is far from the target and decreases it when it is close to the target; the terrain slope factor increases the step size when the terrain is flat and decreases it when the terrain is steep; and the obstacle distance factor increases the step size when the platform is far from an obstacle and decreases it when it is close to an obstacle. These factors are integrated into the calculation of the step size adjustment factor to ensure that the step size adjustment balances path planning efficiency and obstacle avoidance safety under different environmental and task requirements.

[0038] Specifically, step size adjustment factor Calculated using the following linear weighting formula: , in, Let be the weighting coefficient, satisfying The three weighting coefficients are used to reflect the degree of importance attached to different factors and can be set directly according to task requirements: if more emphasis is placed on approaching the target as quickly as possible, the weight corresponding to the target distance should be increased appropriately. If a greater emphasis is placed on the detailed depiction of complex terrain and safe passage, then the weight corresponding to the terrain slope should be appropriately increased. If a greater emphasis is placed on obstacle avoidance safety margins and detailed searching near obstacles, then the weight corresponding to the distance to obstacles should be appropriately increased. By adjusting the weighting coefficients, a balance can be struck between planning efficiency, terrain adaptability, and obstacle avoidance accuracy. This is the target distance normalization factor, representing the distance from the current node to the target. The smaller the value, the smaller the step size. This is the terrain slope normalization factor, representing the gentleness of the terrain; the smaller the value, the larger the step size. This is the obstacle distance normalization factor, representing the distance from the current node to the nearest obstacle. The smaller the value, the smaller the step size. In this way, the step size can be intelligently adjusted to respond to different conditions of the target, terrain, and obstacles, ensuring the accuracy and safety of the path.

[0039] terrain slope It is calculated using the local gradient of the current node in the Digital Elevation Model (DEM). Specifically, the calculation method involves performing central difference on the elevation data to obtain... and The gradient of the direction is calculated, and then the slope angle is calculated based on the gradient:

[0040] This terrain slope measurement The value of directly affects the step size adjustment factor, ensuring that the platform can better avoid obstacles and optimize the efficiency of path planning in complex terrain.

[0041] For all basic motion patterns, a general candidate node state update model is designed, with the following expression: , in, The three-dimensional coordinates of the candidate nodes For dynamic step size, The yaw angle of the candidate node. Candidate node pitch angle, This represents the change in yaw angle. This refers to the change in pitch angle; during a horizontal left turn. Take negative values, take positive values ​​when making a horizontal right turn, and take positive values ​​when climbing vertically. Take positive values; take negative values ​​for vertical dives.

[0042] Optionally, a two-stage collision detection strategy combining grid state determination and 3D distance verification can be adopted. First, the grid state of the candidate node is queried; if the grid is occupied by an obstacle, it is determined to be an invalid node. Next, the shortest 3D distance between the candidate node and surrounding obstacles is calculated. If this distance is greater than or equal to a preset safety distance (primarily related to actual requirements and the specific physical properties of the unmanned platform), the node is considered a valid candidate node. For valid candidate nodes, their total cost is calculated, which consists of the actual cost of the path and a heuristic cost.

[0043] , Actual cost The combined path length and attitude smoothness penalty is calculated using the following formula: , in, This represents the path length from the starting point to the candidate node. The attitude penalty coefficient is... , This represents the change in the attitude angle between the candidate node and its parent node. The value can be set to a number between 0 and 1, adjusted according to the path smoothness requirements, reflecting the smoothness requirements of the path. The larger the value, the more the system tends to choose a smooth path to avoid excessive attitude changes.

[0044] Non-holonomic constraint heuristic cost function: , in, The optimal motion path of the platform in an obstacle-free environment is simulated. The costs of each primitive element—horizontal turning, horizontal straight line, vertical rise and fall, and vertical straight line—are calculated and summarized to reflect the motion cost under nonholonomic constraints. The calculation formula is as follows:

[0045] , in, The starting point and the target point are at - Horizontal distance on a plane The minimum turning radius of the unmanned platform. The current node and the target node are in Height difference along the axis. Represents the change in the platform's height in the vertical direction. This is the maximum pitch angle set.

[0046] The shortest grid path length from the current node to the destination is calculated using the 3D A* algorithm, considering only grid accessibility to reflect the shortest path cost in obstacle-prone environments. The maximum of the two values ​​is then taken as the heuristic cost. .

[0047] Check if a valid candidate node already exists in the open or closed list: if not in either list, add it directly to the open list; if already in the open list and the new calculation has a better cost, update the node's cost information and parent node index; if already in the closed list, do not update to avoid duplicate calculations and path backtracking confusion. After updating, sort the open list in ascending order of total cost, select the node with the lowest cost as the next expanded node, and move it to the closed list.

[0048] Optionally, when the extended node meets the endpoint convergence condition, an initial path from the starting point to the endpoint is generated by backtracking through the parent node information. This also includes: verifying the feasibility and smoothing the path; wherein the endpoint convergence condition is that the positional error between the current extended node and the endpoint in three-dimensional space is less than a preset threshold. threshold Related to map scale and reference step size: , in, As the reference step size, 3D maps in The dimension of the direction. If this condition is not met, return to the extended step to continue. Path backtracking starts from the endpoint node and backtracks step by step based on the parent node information until the starting node is reached. Finally, a path from the starting point to the endpoint is generated, retaining the position and attitude information of each step.

[0049] After the initial path is generated, feasibility verification is required. First, verify the path's motion constraints, checking whether the turning radius, climb or dive angle, and attitude changes between each node are within the platform's physical limitations. Second, perform collision detection to ensure that each node on the path is located in a passable area and meets safety distance requirements. If any non-compliance with constraints or collision risks are found, return to the node expansion and filtering steps for adjustments until the path meets all constraints, ensuring its feasibility.

[0050] like Figure 2 As shown, the second part of the embodiments of the present invention provides a system for path planning of an unmanned platform, comprising: The map building module is used to build a 3D adaptive map that includes terrain elevation and obstacle information; The state initialization module is used to define the state vector of the unmanned platform, define the starting state and the ending state, and set the platform motion constraint parameters; it also initializes the open list and the closed list, where the open list uses a priority queue to store nodes to be expanded, and the closed list stores valid nodes that have been expanded. The node expansion module is used to construct a finite set of three-dimensional motion primitives based on the defined state vector, the defined start state and end state, and motion constraints. Each motion primitive corresponds to a continuous motion segment. Starting from the current node, it performs parameterized sampling on each type of motion primitive to generate candidate continuous motion segments and uses their end states as candidate child nodes. Based on the distance between the current node and the target state, the terrain slope, and the three-dimensional distance to the nearest obstacle, it dynamically adjusts the current expansion step size through an adaptive step size model. It performs three-dimensional collision detection and motion constraint verification on the generated candidate continuous motion segments. For candidate nodes that pass the verification, it calculates the actual cost increment from the start point to the node and adds it to the open list, completing one expansion iteration of the three-dimensional hybrid A* algorithm. The node filtering module is used to calculate the actual cost and heuristic cost of each candidate node in the open list. The sum of the two is the total cost. The node with the lowest cost is selected as the next expansion node and moved into the closed list until the three-dimensional coordinate error between the current expansion node and the endpoint is less than the set threshold. The path backtracking module is used to generate a continuous path from the starting point to the ending point by gradually backtracking from the current node to the starting point node based on the parent node information when the endpoint convergence condition is met.

[0051] This invention provides a system for path planning on unmanned platforms, used to generate continuous, executable trajectories that satisfy platform kinematic and dynamic constraints in environments with three-dimensional obstacles and varying height levels. Based on a hybrid A* search framework, a set of multiple motion primitives is constructed under dynamic constraints, and during node expansion, minimum turning radius, maximum climb or dive capability, and three-dimensional collision constraints are simultaneously satisfied, thereby extending hybrid A* from two-dimensional planar planning to three-dimensional spatial planning. Simultaneously, an adaptive grid is used for three-dimensional map modeling, and a heuristic cost function incorporating nonholonomic constraints and an adaptive step-size expansion mechanism are employed in the three-dimensional hybrid A*. This invention comprehensively balances motion feasibility, planning efficiency, and path accuracy, and is applicable to three-dimensional path planning tasks for various types of unmanned platforms, including ground, air, surface, and underwater platforms.

[0052] Example: like Figure 1 As shown, this embodiment of the invention provides a path planning method for an unmanned platform. In this embodiment, the map size is set to be... The reference grid is However, in practice, 1 kilometer is used as an approximate grid length. The platform's starting point is set as follows: The endpoint is set as The platform's yaw angle range is ±30°, pitch angle range is ±30°, minimum turning radius is set at 50 meters, and maximum climb and dive angles are limited to 30°.

[0053] First, environmental sensing devices are used to collect terrain elevation data of the target's 3D environment, generating a digital elevation map (DEM). During map construction, the grid size is set based on the maximum slope of the terrain. Calculations show the maximum slope to be 36.7°, so the grid reference size is set to 970 meters. Simultaneously, based on the calculation method for adaptive raster maps, the grid size for local areas is obtained, facilitating subsequent node expansion.

[0054] Next, we perform the 3D hybrid A* algorithm initialization, with the platform's state vector as follows: This includes three-dimensional coordinates, yaw angle, and pitch angle. The starting state is... The final state is The platform's motion constraints include a minimum turning radius of 50 meters, a safe collision distance of 25 meters, and a maximum climb and dive angle of 30°. These parameters will affect subsequent node expansion and path generation.

[0055] During node expansion, the initial baseline step size is set to 25 meters. Subsequent expansions are performed using a dynamic step size calculation method to determine the distance of each node expansion. A finite set of three-dimensional motion primitives, constructed based on the platform's motion characteristics in three-dimensional space, is used. These primitives include horizontal straight-line movement, horizontal turning, vertical climbing, and vertical diving. The trajectory generated by each motion primitive must satisfy the platform's nonholonomic and dynamic constraints.

[0056] For each candidate node generated, its cost function is calculated, consisting of two parts: the actual cost and the heuristic cost. The actual cost considers penalties for path length and pose changes, while the heuristic cost is based on the shortest grid path length from the current node to the destination, calculated using the 3D A* algorithm. During this process, collision detection is performed to ensure that candidate nodes maintain a sufficient safe distance from surrounding obstacles, avoiding collision risks along the path.

[0057] Path selection and updates are based on cost function evaluation. If a candidate node has a better cost, it is added to the open list, and its corresponding cost information is updated. If a candidate node already exists in the open list and its newly calculated cost is better, the node's cost information is updated. After each node expansion, nodes are sorted according to their total cost, and the node with the lowest cost is selected for the next expansion, until the target node converges.

[0058] When the expanded node meets the endpoint convergence condition, node expansion stops and path backtracking begins. The backtracking process starts from the endpoint node and gradually traces back to the starting node using parent node information, ultimately generating a path from the starting point to the endpoint. The backtracked path needs to undergo motion constraint verification and collision feasibility verification. If the path does not meet the constraints, the process returns to the expansion step for adjustments until the path meets all constraints and is safe.

[0059] Ultimately, the generated path is in The map can effectively avoid obstacles, meet the platform's kinematic and dynamic constraints, and ensure the feasibility and safety of the path.

[0060] The above path planning algorithm was implemented using the MATLAB simulation platform, and the simulation results are as follows: Figure 3 As shown in the figure, the path planned using the 3D hybrid A* algorithm between the specified start and end points avoids obstacles and meets all platform constraints. The path is smooth and feasible, suitable for autonomous navigation tasks of unmanned platforms in real-world environments.

[0061] The above inventions are merely a few specific embodiments of the present invention. However, the embodiments of the present invention are not limited thereto, and any variations that can be conceived by those skilled in the art should fall within the protection scope of the present invention.

Claims

1. A path planning method for an unmanned platform, characterized in that, Includes the following steps: Construct a 3D adaptive map that includes terrain elevation and obstacle information; Define the state vector of the unmanned platform, define the starting state and the ending state, and set the platform motion constraint parameters; initialize the open list and the closed list, where the open list uses a priority queue to store nodes to be expanded, and the closed list stores valid nodes that have been expanded. Based on the defined state vector, the defined start state and end state and motion constraints, a finite set of three-dimensional motion primitives is constructed, and each motion primitive corresponds to a continuous motion segment. Starting from the current node, parameterize the sampling of each type of motion primitive to generate candidate continuous motion segments and use their end states as candidate child nodes; dynamically adjust the current expansion step size through an adaptive step size model based on the distance between the current node and the target state, the terrain slope, and the 3D distance to the nearest obstacle; perform 3D collision detection and motion constraint verification on the generated candidate continuous motion segments, calculate the actual cost increment from the starting point to the node for the candidate nodes that pass the verification and add them to the open list, thus completing one expansion iteration of the 3D hybrid A* algorithm; For each candidate node in the open list, calculate its actual cost and heuristic cost. The sum of the two is the total cost. Select the node with the smallest cost as the next expansion node and move it into the closed list. Continue until the 3D coordinate error between the current expansion node and the endpoint is less than a set threshold. When the endpoint convergence condition is met, the current node is used to backtrack to the starting node based on the parent node information to generate a continuous path from the starting point to the endpoint.

2. The unmanned platform path planning method as described in claim 1, characterized in that, The construction of a 3D adaptive map containing terrain elevation and obstacle information includes the following steps: A three-dimensional adaptive map is constructed using digital elevation maps, and the grid size of the map is dynamically adjusted according to the terrain slope and obstacle distribution density. The grid base size is set according to the maximum slope of the environment, and the adaptive grid size is adjusted according to the slope of the local area.

3. The unmanned platform path planning method as described in claim 1, characterized in that, The state vector of the unmanned platform is: ,in, For three-dimensional position coordinates, Yaw angle The pitch angle; The motion constraint parameters include: minimum turning radius. Maximum climb angle and safe distance .

4. The unmanned platform path planning method as described in claim 1, characterized in that, The three-dimensional motion elements include ten basic motion modes: horizontal forward straight line, horizontal forward left turn, horizontal forward right turn, vertical forward climb, vertical forward dive, horizontal backward straight line, horizontal backward left turn, horizontal backward right turn, vertical backward climb, and vertical backward dive. The trajectory generation of each basic motion mode follows the constraints of the platform's turning speed, vertical speed of climb or dive, and attitude change angle.

5. The unmanned platform path planning method as described in claim 1 or 4, characterized in that, The extended step size This can be expressed by the following formula: ， in, The initial reference step size, This is the step size adjustment factor. This represents the current node state. This is the final state. For terrain slope, The step size adjustment factor is the three-dimensional distance between the current node and the nearest obstacle. The system dynamically adjusts its step size based on the distance between the current node and the target, the terrain slope, and the distance to obstacles, so that the step size increases when the node is far away from the target, the terrain is flat, and the obstacle is far away, and decreases when the node is far away from the obstacle.

6. The unmanned platform path planning method as described in claim 5, characterized in that, The step size adjustment factor Determined using the linear weighting formula: ， in, Let be the weighting coefficient, satisfying , The target distance normalization factor, The terrain slope normalization factor, This is the obstacle distance normalization factor.

7. The unmanned platform path planning method as described in claim 1, characterized in that, The state of the candidate node is updated using the following general model: ， in, The three-dimensional coordinates of the candidate nodes For dynamic step size, The yaw angle of the candidate node. Candidate node pitch angle, This represents the change in yaw angle. This represents the change in pitch angle.

8. The unmanned platform path planning method as described in claim 1, characterized in that, The actual cost Combine path length and attitude smoothness penalty; Heuristic Cost ,in, To simulate the motion cost of a platform under nonholonomic constraints in an obstacle-free environment, This represents the shortest grid path length from the current node to the destination, obtained using the 3D A* algorithm. The total cost .

9. The unmanned platform path planning method as described in claim 1, characterized in that, When the extended node meets the endpoint convergence condition, an initial path from the starting point to the endpoint is generated by backtracking through the parent node information. This also includes: performing feasibility verification and smoothing on the path; wherein the endpoint convergence condition is that the positional error between the current extended node and the endpoint in three-dimensional space is less than a preset threshold. The threshold Related to map scale and baseline step size: ， in, As the reference step size, 3D maps in Dimensions of direction.

10. A system for path planning of unmanned platforms, characterized in that, include: The map building module is used to build a 3D adaptive map that includes terrain elevation and obstacle information; The state initialization module is used to define the state vector of the unmanned platform, define the starting state and the ending state, and set the platform motion constraint parameters; it also initializes the open list and the closed list, where the open list uses a priority queue to store nodes to be expanded, and the closed list stores valid nodes that have been expanded. The node extension module is used to construct a finite set of three-dimensional motion primitives based on the defined state vector, the defined start state and end state and motion constraints, and each motion primitive corresponds to a continuous motion segment. Starting from the current node, parameterize the sampling of each type of motion primitive to generate candidate continuous motion segments and use their end states as candidate child nodes; dynamically adjust the current expansion step size through an adaptive step size model based on the distance between the current node and the target state, the terrain slope, and the 3D distance to the nearest obstacle; perform 3D collision detection and motion constraint verification on the generated candidate continuous motion segments, calculate the actual cost increment from the starting point to the node for the candidate nodes that pass the verification and add them to the open list, thus completing one expansion iteration of the 3D hybrid A* algorithm; The node filtering module is used to calculate the actual cost and heuristic cost of each candidate node in the open list. The sum of the two is the total cost. The node with the lowest cost is selected as the next expansion node and moved into the closed list until the three-dimensional coordinate error between the current expansion node and the endpoint is less than the set threshold. The path backtracking module is used to generate a continuous path from the starting point to the ending point by gradually backtracking from the current node to the starting point node based on the parent node information when the endpoint convergence condition is met.

Images