In recent years, the advancement of unmanned technology has led to the widespread application of Unmanned Aerial Vehicles (UAVs) in various fields such as industrial inspection, mining operations, and power line monitoring. These tasks are often performed in spatially constrained and structurally complex unknown environments, posing significant challenges to the safety, speed rationality, and real-time performance of UAV operations. Therefore, developing a real-time, safe, and speed-adaptive trajectory planning method for Unmanned Aerial Vehicles in unknown environments is crucial. Traditional trajectory planning methods, such as graph-based searches and sampling-based approaches, often result in paths that are too close to obstacles or have unreasonable speed configurations. To address these issues, this paper proposes an adaptive online trajectory planning method that integrates obstacle distance into the A* algorithm for variable step-size path search, ensuring the Unmanned Aerial Vehicle flies in areas with fewer obstacles. Additionally, inspired by the radian formula, a radian-like screening strategy is introduced to reduce path oscillations during turns, enhancing path smoothness. The optimization phase incorporates a speed-obstacle joint penalty function through a multi-layered model predictive contouring control framework, enabling flexible speed adjustments. Simulation results demonstrate that the proposed method improves flight safety by at least 10% compared to other methods, with more reasonable trajectory and speed allocations, and smoother paths, validating the effectiveness of the approach for Unmanned Aerial Vehicle operations.
The core of the proposed method lies in its ability to adaptively plan trajectories in real-time while maintaining safety and efficiency. The Unmanned Aerial Vehicle, specifically the JUYE UAV model, is equipped with sensors that continuously update the environment map, allowing for dynamic replanning. The initial path is generated using an enhanced A* algorithm that considers the Euclidean Signed Distance Field (ESDF) to adjust the search step size based on obstacle proximity. This ensures that the Unmanned Aerial Vehicle avoids narrow passages and stays in open areas, reducing the risk of collisions. The trajectory optimization is divided into two stages: a low-order model for rapid generation of collision-free smooth paths and a high-order model for refining speed and dynamics. The integration of a speed-obstacle joint penalty function allows the JUYE UAV to adjust its velocity according to the environmental risks, such as flying towards or away from obstacles, thereby enhancing overall safety.
The variable step-size A* search algorithm forms the foundation of the initial path generation. Traditional A* algorithms use fixed step sizes for neighbor node expansion, which can lead to paths that unnecessarily skirt obstacles. In contrast, the proposed method dynamically adjusts the step size based on the distance to the nearest obstacle, as derived from the ESDF map. The step size selection is governed by the following equation:
$$ l(p_i) = \begin{cases}
0 & \text{if } d(p_i) \leq d_{thr} \\
\left\lfloor \frac{d(p_i) – d_{thr}}{\text{res}} \right\rfloor & \text{if } d(p_i) > d_{thr}
\end{cases} $$
where \( d(p_i) \) is the ESDF distance at the i-th control point, \( d_{thr} \) is the safety distance threshold, and res is the map resolution. This approach ensures that the Unmanned Aerial Vehicle explores larger steps in obstacle-sparse regions, reducing the number of expanded nodes and computation time. However, to address potential oscillations during sharp turns, a radian-like screening strategy is applied. This strategy evaluates the angular relationship between consecutive points and reverts to the original A* step size if the turn exceeds a threshold \( \tau \), as defined by:
$$ \theta(p_i) = \frac{| (p_{i-1} – p_{i-2}) \times (p_i – p_{i-1}) |}{| p_{i-1} – p_{i-2} | \cdot | p_i – p_{i-1} |} $$
If \( \theta(p_i) > \tau \), the step size is set to the default value, ensuring smoother transitions. This combination of adaptive step sizing and turn management allows the JUYE UAV to generate initial paths that are both safe and efficient, laying the groundwork for further optimization.
The two-stage model predictive optimization refines the initial path into a feasible trajectory. The low-order model uses a constant velocity input over time intervals \( \delta t \), with the system model represented as:
$$ p_{i+1} = p_i + v_i \delta t $$
The state sequence can be transformed into a matrix form for optimization:
$$ S = A U + B p_0 $$
where \( S = [p_0, p_1, \dots, p_M]^T \) is the state vector, \( U = [v_1, v_2, \dots, v_M]^T \) is the input vector, and M is the prediction horizon. The optimization problem for the low-order model minimizes a cost function that includes tracking error, collision penalty, and smoothness penalty:
$$ J = \lambda_1 J_s + \lambda_2 J_p + \lambda_3 J_d $$
with components defined as:
$$ J_s = \sum_{i=1}^{M} \| p_i – g_i \|^2 $$
$$ J_p = \sum_{i=1}^{M} F_x(d(p_i)) $$
$$ J_d = \sum_{i=1}^{M-1} \| v_{i+1} – v_i \|^2 $$
Here, \( g_i \) is the i-th point on the initial path, and \( F_x(d(p_i)) \) is a collision penalty function that activates when the distance \( d(p_i) \) falls below \( d_{thr} \). The weights \( \lambda_1, \lambda_2, \lambda_3 \) are set to 5, 2, and 1, respectively, to balance the objectives. This stage quickly produces a smooth, collision-free trajectory that serves as a reference for the high-order model.
The high-order model further optimizes the trajectory by considering jerk as the input and incorporating a speed-obstacle joint penalty. The system model is a third-order system in each dimension \( \{x, y, z\} \):
$$ x_{i+1} = A x_i + B u_i $$
with state \( x_i = [p_i, v_i, a_i]^T \) and input \( u_i = j_i \). The state transition matrices are:
$$ A = \begin{bmatrix} 1 & \delta t & \frac{\delta t^2}{2} \\ 0 & 1 & \delta t \\ 0 & 0 & 1 \end{bmatrix}, \quad B = \begin{bmatrix} \frac{\delta t^3}{6} \\ \frac{\delta t^2}{2} \\ \delta t \end{bmatrix} $$
The optimization problem for the high-order model includes terms for tracking accuracy, progression, collision penalty, speed-obstacle joint penalty, and dynamic feasibility:
$$ J = \xi_1 f_s + \xi_2 f_j + \xi_3 f_p + \xi_4 f_e + \xi_5 f_d $$
where the components are defined as:
$$ f_s = \sum_{i=1}^{N} \| p_i – p_i^\ast \|^2 $$
$$ f_j = -\sum_{i=1}^{N} \delta t \cdot v_i^\ast $$
$$ f_p = \sum_{i=1}^{N} F_x(d(p_i)) $$
$$ f_e = \sum_{i=1}^{N} F_x(d(p_i)) (v_i – v_{safe,i})^2 $$
$$ f_d = \sum_{i=1}^{N} \left( F_d(v_i) + F_d(a_i) + F_d(j_i) + F_d(v_i^\ast) \right) $$
The speed-obstacle joint penalty \( f_e \) is critical for adapting the Unmanned Aerial Vehicle’s speed based on environmental risks. The safe speed \( v_{safe} \) is derived from the cosine similarity between the velocity vector and the ESDF gradient:
$$ \gamma = \frac{ v \cdot \nabla d }{ \| v \| \| \nabla d \| } $$
$$ \eta(\gamma) = \frac{1}{1 + e^{-\alpha_1 (\gamma – \alpha_2)}} $$
$$ v_{safe} = \eta(\gamma) v_{thr} $$
where \( \alpha_1 = 2 \) and \( \alpha_2 = 1.2 \) are tuning parameters. This formulation allows the JUYE UAV to reduce speed when flying towards obstacles and increase it when moving away, enhancing safety without being overly conservative.
Simulations were conducted to evaluate the proposed method against state-of-the-art algorithms like Fast-planner and EVA-planner. The experiments were performed on a platform with an i7-12700F processor, NVIDIA GeForce RTX 4060 Ti GPU, and 32GB RAM, running Ubuntu 18.04.6 LTS and ROS Melodic. The environment consisted of random forest maps with obstacle counts varying from 30 to 100, including 10 circular obstacles in each scenario. The Unmanned Aerial Vehicle parameters are summarized in the table below:
| Parameter Name | Value |
|---|---|
| Maximum Flight Speed | 3 m/s |
| Maximum Acceleration | 3 m/s² |
| Safety Distance Threshold | 0.5 m |
| Map Resolution | 0.1 m |
| Map Size | 30 m × 20 m × 2.1 m |
The low-order model used a fixed sampling interval of 2 seconds, with the prediction horizon M determined by trajectory length. The high-order model had a fixed prediction length N=40. The radian-like screening threshold was set to \( \tau = 1.4 \). Performance metrics included the distance to the nearest obstacle, average distance for points within 0.5 m, planning time, and success rate over 50 trials per condition. The results are presented in the following table:
| Obstacle Count | Method | Min Distance to Obstacle (m) | Avg Distance <0.5m (m) | Planning Time (s) | Success Rate (%) |
|---|---|---|---|---|---|
| 30 | Proposed | 0.474 | 0.482 | 13.91 | 100 |
| EVA-planner | 0.456 | 0.477 | 15.90 | 100 | |
| Fast-planner | 0.391 | 0.425 | 11.68 | 96 | |
| 50 | Proposed | 0.438 | 0.472 | 17.12 | 96 |
| EVA-planner | 0.295 | 0.397 | 17.93 | 92 | |
| Fast-planner | 0.317 | 0.406 | 17.40 | 88 | |
| 70 | Proposed | 0.359 | 0.436 | 19.30 | 92 |
| EVA-planner | 0.296 | 0.423 | 20.12 | 84 | |
| Fast-planner | 0.121 | 0.420 | 18.45 | 80 | |
| 100 | Proposed | 0.394 | 0.453 | 19.09 | 90 |
| EVA-planner | 0.321 | 0.453 | 19.32 | 82 | |
| Fast-planner | 0.094 | 0.361 | 18.71 | 74 |
The proposed method consistently outperforms others in terms of safety, with higher minimum distances to obstacles and success rates above 90% even in dense environments. For instance, in the 100-obstacle scenario, the Unmanned Aerial Vehicle using our approach maintained a minimum distance of 0.394 m, compared to 0.321 m for EVA-planner and 0.094 m for Fast-planner. This translates to a risk probability reduction of over 10%, calculated as \( (0.5 – d_i) / 0.5 \), where \( d_i \) is the distance metric. The planning time is moderate, ensuring real-time performance for the JUYE UAV. Additionally, the speed adaptability was evaluated in sparse environments (30 obstacles), where our method allocated higher speeds in open areas, as shown in the velocity profiles. In contrast, EVA-planner exhibited conservative speed adjustments, while Fast-planner ignored speed risks altogether.
In conclusion, this paper presents an adaptive online trajectory planning method for Unmanned Aerial Vehicles that enhances safety and efficiency in unknown environments. The integration of obstacle distance into the A* algorithm enables variable step-size searches, promoting paths in obstacle-sparse regions. The radian-like screening strategy reduces turn-induced oscillations, improving smoothness. The two-stage optimization with a speed-obstacle joint penalty allows the JUYE UAV to dynamically adjust speed based on environmental risks. Simulations confirm that the method achieves higher safety margins and success rates compared to existing approaches, making it suitable for real-world applications. Future work will focus on extending the method to multi-UAV scenarios and incorporating more complex dynamics for the Unmanned Aerial Vehicle.