In recent years, China UAV (Unmanned Aerial Vehicle) technology has been widely applied in logistics, inspection, agriculture, and emergency rescue. The autonomous navigation capability of China UAV directly determines the efficiency and safety of mission execution. Path planning, as the core module of navigation, has attracted extensive research attention. Traditional path planning algorithms often suffer from poor path safety, excessive redundant nodes, and insufficient smoothness when applied in three-dimensional environments. To address these issues, I propose an improved three-dimensional jump point search (TDC-JPS) algorithm tailored for China UAV. This algorithm integrates obstacle safety distance expansion, a target-direction cone bias strategy, global path reconstruction via node connectivity verification, and gradient descent optimization based on China UAV’s dynamic constraints. Extensive simulations in Matlab and experiments in ROS environment demonstrate that the proposed algorithm significantly reduces the number of traversed nodes and planning time while ensuring path safety and smoothness. The results indicate that the generated path can be well applied to actual China UAV platforms.
1. Introduction
The rapid development of China UAV industry has promoted the demand for autonomous flight in complex environments. Path planning is a key technology for China UAV to achieve autonomous navigation. Currently, path planning methods can be mainly divided into three categories: sampling-based methods (e.g., RRT, PRM), graph search-based methods (e.g., A*, Dijkstra), and intelligent optimization algorithms (e.g., genetic algorithm, ant colony optimization). Among them, the Jump Point Search (JPS) algorithm, as an efficient graph search method, performs excellently in two-dimensional grids. However, when directly applied to three-dimensional space for China UAV, JPS faces several challenges: insufficient path safety due to ignoring the size and braking distance of the UAV; redundant search directions in 3D space causing computational explosion; lack of consideration for China UAV dynamic constraints (maximum curvature, acceleration limits). Therefore, I propose an improved Three-Dimensional Jump Point Search algorithm with Target-Direction Cone bias (TDC-JPS) to solve these problems.
2. Background: Jump Point Search Algorithm
The JPS algorithm was proposed by Harabor and Grastien in 2011. It is based on the A* algorithm and introduces the concept of “jump points” to prune the search space. The core idea is that in uniform-cost grids, many nodes can be skipped because the optimal path only changes direction at certain “forced neighbor” nodes. The cost function of a node n is defined as:
$$f(n) = g(n) + h(n)$$
where g(n) is the actual cost from the start node to node n, and h(n) is the heuristic estimated cost from node n to the goal. In three-dimensional space, the movement directions are classified according to the L1 norm of the direction vector d=(dx, dy, dz). Three categories exist: axial movement (D=1), planar diagonal movement (D=2), and volumetric diagonal movement (D=3). For each category, forced neighbors are identified based on obstacle configurations. For example, for axial movement along d=(1,0,0), forced neighbors are detected in the plane perpendicular to the movement direction. This mechanism ensures that only critical nodes are expanded, greatly improving efficiency compared to standard A*.
3. Improved TDC-JPS Algorithm
3.1 Safety Distance Expansion for China UAV
To ensure the flight safety of China UAV, the algorithm must consider the physical size, braking distance, and reaction time. The China UAV is modeled as a cube with side length Lu. The flight speed is v, the maximum acceleration is amax, and the system reaction time is tr. The safety distance dsafe is defined as the minimum allowable distance from the geometric center of the China UAV to the obstacle boundary:
$$d_{safe} = r_{u_{\max}} + d_{brake} + d_{react}$$
where \( r_{u_{\max}} = \sqrt{3} L_u / 2 \) is the maximum expansion radius, \( d_{brake} = v^2 / (2 a_{\max}) \) is the braking distance, and \( d_{react} = v \cdot t_r \) is the reaction distance. Let the obstacle set in the environment be O ⊆ ℝ³. The requirement for a path P = {p(i) | i ∈ [0, N]} is that for any point on the path, the distance to any obstacle satisfies:
$$\min_{o \in O} \|p(i) – o\| \geq d_{safe}, \quad \forall i \in [0, N]$$
To convert this constraint into a grid map, the obstacle grid (edge length d_{obs}) is expanded by a factor of (2d_{safe}+d_{obs})/d_{obs} to generate the expanded obstacle set O_{exp}. The path planning problem then becomes finding a feasible path in the free space {p | p ∈ ℝ³, p ∉ O_{exp}}.
3.2 Target-Direction Cone Bias Strategy
The standard 3D JPS algorithm must consider 26 general neighbor directions from the start node, and for volumetric diagonal movement, 7 possible general neighbor directions are checked. This leads to a dramatic increase in search space. To reduce redundant jump points, I propose a target-direction cone bias strategy. The strategy uses a dynamic threshold function Θ(‖T_c‖, ρ) to determine the range of expansion directions. The angle is adjusted based on local obstacle density and distance to the goal:
$$\Theta(\|T_c\|, \rho) = c_1 \rho \cdot \theta_{base} + \frac{\theta_{scale}}{\|T_c\| + c_2}$$
Here, ρ is the local obstacle density, θ_base is the base angle, θ_scale is the distance modulation angle, T_c = G – p_c is the target direction vector (G is the goal, p_c is the current node), ‖T_c‖ is the Euclidean distance from p_c to G, and c1, c2 > 0 are tuning parameters. The target-direction cone is a 3D cone with p_c as the vertex, T_c as the axis, and Θ as the apex angle. When the expansion direction d_c has an angle θ(d_c, T_c) greater than Θ, that direction is pruned. For example, when expanding from a node reached via volumetric diagonal movement, up to 7 neighbors exist. If the normalized target direction is T_c = (2/3, 1/3, 2/3) and Θ = 90°, the strategy prunes 5 neighbors that deviate from the target, retaining only 2 neighbors inside the cone. This significantly reduces the number of directions to explore.
3.3 Path Reconstruction via Node Connectivity
The path generated by 3D JPS often contains redundant intermediate nodes that increase path length and tracking difficulty. I design a path reconstruction strategy that retains only essential turning points while guaranteeing collision-free connectivity. Connectivity between two points p, q ∈ ℝ³ is defined as: if all grids traversed by the line segment pq are free space, then Conn(p,q) = True; otherwise False. Given an initial ordered path P = {p₀, p₁, …, pₙ}, the algorithm generates a simplified path Q = {q₀, q₁, …, qₘ} ⊆ P (m ≤ n) preserving the original order. The steps are:
- If n = 2, then Q = P and terminate.
- Initialize Q = {p₀}, set current index i = 0.
- From current node pᵢ, search for the maximum index j* such that Conn(pᵢ, p_{j*}) = True.
- Add p_{j*} as the next waypoint in Q, update i = j*, and repeat until i = n.
This procedure removes unnecessary intermediate points while maintaining the ability to avoid obstacles. For instance, a path with five waypoints may be reduced to four after removing two redundant nodes.
3.4 Gradient Descent Path Optimization Based on China UAV Dynamic Constraints
To improve the flyability of the path for China UAV, I apply a gradient descent method that incorporates dynamic constraints. The optimization consists of two stages: dynamic control point densification and gradient descent optimization.
3.4.1 Dynamic Control Point Densification
The initial JPS path has sparse nodes and sharp corners. I first identify corner points where the angle between consecutive direction vectors exceeds π/6. For each corner point pᵢ, I compute the angle angle(i):
$$\text{angle}(i) = \arccos\left( \frac{\mathbf{v}_1 \cdot \mathbf{v}_2}{\|\mathbf{v}_1\| \cdot \|\mathbf{v}_2\|} \right)$$
where v₁ = pᵢ – p_{i-1} and v₂ = p_{i+1} – pᵢ. The densification step size is dynamically adjusted:
$$\text{step} = \max\left( \Delta s_{\min}, \Delta s_{\max} \cdot \left(1 – 0.8 \min\left(1, \frac{\text{angle}(i)}{\pi/4}\right) \right) \right)$$
Extra control points are inserted on segments p_{i-1}pᵢ and pᵢp_{i+1} within a range R around the corner. For non-corner segments, uniform densification with step Δs_max is applied. This yields a dense set of control points suitable for optimization.
3.4.2 Gradient Descent Optimization
Let the dense control point set be P^{init} = {p₁^{init}, p₂^{init}, …, p_N^{init}}. During optimization, the points are iteratively adjusted to minimize a composite objective function J that considers curvature violation, curvature smoothness, path smoothness, and deviation from the initial path. The step size hᵢ for each point is defined as:
$$h_i = \frac{1}{2} \left( \|p_i – p_{i-1}\|_2 + \|p_{i+1} – p_i\|_2 \right)$$
The objective function is:
$$J = \alpha J_c + \beta J_{cs} + \gamma J_{ps} + \lambda J_{de}$$
where:
- Curvature violation penalty: \( J_c = \sum_{i=1}^N \max(\kappa_i – \kappa_{\max}, 0)^2 \)
- Curvature smoothness penalty: \( J_{cs} = \sum_{i=1}^N (\kappa_{i+1} – \kappa_i)^2 \)
- Path smoothness penalty: \( J_{ps} = \sum_{i=2}^{N-1} \|\ddot{p}_i\|^2 \)
- Regularization term: \( J_{de} = \sum_{i=1}^N \|p_i – p_i^{init}\|^2 \)
The gradients of each term with respect to pᵢ are computed analytically. The curvature κᵢ is calculated using the velocity and acceleration approximations:
| Node index i | Velocity \(\dot{p}_i\) | Acceleration \(\ddot{p}_i\) | Curvature \(\kappa_i\) |
|---|---|---|---|
| i = 1 | \(\dot{p}_1 \approx \frac{p_2 – p_1}{h_1}\) | \(\ddot{p}_1 \approx \frac{p_3 – 2p_2 + p_1}{h_1^2}\) | \(\kappa_1 \approx \frac{\|\dot{p}_1 \times \ddot{p}_1\|}{\|\dot{p}_1\|^3}\) |
| 2 ≤ i ≤ N-1 | \(\dot{p}_i \approx \frac{p_{i+1} – p_{i-1}}{2h_i}\) | \(\ddot{p}_i \approx \frac{p_{i+1} – 2p_i + p_{i-1}}{h_i^2}\) | \(\kappa_i \approx \frac{\|\dot{p}_i \times \ddot{p}_i\|}{\|\dot{p}_i\|^3}\) |
| i = N | \(\dot{p}_N \approx \frac{p_N – p_{N-1}}{h_N}\) | \(\ddot{p}_N \approx \frac{p_N – 2p_{N-1} + p_{N-2}}{h_N^2}\) | \(\kappa_N \approx \frac{\|\dot{p}_N \times \ddot{p}_N\|}{\|\dot{p}_N\|^3}\) |
The update uses momentum gradient descent:
$$p_i^{(k+1)} = p_i^{(k)} – \eta^{(k)} \cdot \nabla J(p_i^{(k)}) + \mu \cdot \Delta p_i^{(k-1)}$$
where μ is the momentum coefficient, and the learning rate \(\eta^{(k)}\) decays adaptively:
$$\eta^{(k)} = \begin{cases} \rho \cdot \eta^{(k-1)} & k \geq K_{\text{decay}} \\ \eta^{(k-1)} & 0 < k < K_{\text{decay}} \end{cases}$$
The optimization terminates when the objective function falls below a threshold, the change in J is smaller than a tolerance for several iterations, or the maximum iterations are reached. After optimization, the path curvature becomes continuous and smooth, suitable for China UAV tracking.
4. Simulations and Experiments
All simulations were conducted on a computer with a 12th Gen Intel Core i7-12800HX × 24 processor. For 2D simulation, I compared A*, Theta*, standard JPS, and the proposed TDC-JPS algorithm in both simple and complex environments. For 3D experiments, I built simple and complex 3D maps in Ubuntu 20.04-ROS environment, comparing 3D A*, 3D JPS, and TDC-JPS. The evaluation metrics include path length, number of traversed nodes, and planning time.
4.1 2D Simulation Results
In the simple 2D map (60×50 grids, start (4,4), goal (57,47)) and complex map (120×120 grids, start (4,4), goal (117,97)), the results are summarized in Table 2.
| Scenario | Algorithm | Path length (m) | Number of nodes | Planning time (s) |
|---|---|---|---|---|
| Simple | A* | 78.083 | 1296 | 7.283 |
| Theta* | 73.433 | 765 | 4.328 | |
| JPS | 78.076 | 340 | 0.141 | |
| TDC-JPS | 79.248 | 23 | 0.043 | |
| Complex | A* | 162.723 | 4231 | 27.692 |
| Theta* | 154.853 | 2820 | 15.959 | |
| JPS | 162.706 | 2119 | 2.1674 | |
| TDC-JPS | 168.496 | 108 | 0.0823 |
In the simple scenario, TDC-JPS reduced the number of traversed nodes by 98.23% compared to A*, by 96.99% compared to Theta*, and by 93.24% compared to standard JPS. The planning time was reduced by 99.41%, 99.01%, and 69.50%, respectively. In the complex scenario, the node reduction ratios were 97.4%, 96.2%, and 94.9% relative to A*, Theta*, and JPS, with planning time reductions of 99.7%, 99.5%, and 96.2%. Although the path length increased slightly (e.g., from 78.076 m for JPS to 79.248 m in simple scenario), the safety margin was significantly improved due to the expansion of obstacles. The target-direction cone bias dramatically reduced the search space, and the path reconstruction removed redundant nodes. The gradient descent optimization ensured smoothness, making the path feasible for China UAV.
4.2 3D Experiment Results
Two 3D environments were constructed: a simple map (20×20×10 grids) and a complex map (30×30×15 grids). The start and goal positions were set appropriately. Table 3 summarizes the results.
| Scenario | Algorithm | Path length (m) | Number of traversed nodes | Planning time (ms) |
|---|---|---|---|---|
| Simple | 3D A* | 15.017 | 3259 | 32.299 |
| 3D JPS | 14.756 | 51 | 5.475 | |
| TDC-JPS | 14.871 | 44 | 5.628 | |
| Complex | 3D A* | 35.903 | 336 | 22.243 |
| 3D JPS | 37.362 | 175 | 8.732 | |
| TDC-JPS | 33.217 | 82 | 6.571 |
In the simple 3D scenario, TDC-JPS achieved a path length similar to 3D JPS (14.871 m vs 14.756 m) while reducing the number of traversed nodes by 13.73% compared to 3D JPS. The planning time was slightly higher (5.628 ms vs 5.475 ms) due to the additional connectivity check and gradient descent optimization, but still much lower than 3D A* (32.299 ms). In the complex 3D scenario, TDC-JPS outperformed both algorithms: the path length was 33.217 m, which is 7.48% shorter than 3D A* and 11.10% shorter than 3D JPS. The number of nodes was reduced by 75.60% compared to 3D A* and by 53.14% compared to 3D JPS. Planning time was 6.571 ms, a 70.46% reduction from 3D A* and a 24.75% reduction from 3D JPS. These results demonstrate that TDC-JPS not only improves efficiency but also generates shorter and safer paths for China UAV.
5. Conclusion
In this work, I proposed an improved three-dimensional jump point search algorithm (TDC-JPS) specifically designed for China UAV path planning. The algorithm integrates obstacle safety distance expansion to ensure flight safety, a target-direction cone bias strategy to reduce redundant search directions, a node connectivity-based path reconstruction to eliminate unnecessary waypoints, and a gradient descent optimization that respects China UAV dynamic constraints (curvature, acceleration). Comparative simulations in 2D and 3D environments show that TDC-JPS significantly reduces the number of traversed nodes and planning time while maintaining or even improving path length and safety. The ROS experimental results confirm that the generated path is suitable for actual China UAV flight. Future work will focus on integrating SLAM for dynamic obstacle avoidance and environment perception, further enhancing the practicality and robustness of the algorithm in real-world China UAV applications.