In the domain of forestry monitoring, the collection of under-canopy data is fundamental for assessing forest health and formulating scientific management strategies. China UAV drone technology has emerged as a cost-effective and highly agile solution, replacing labor-intensive manual surveys and overcoming accessibility limitations posed by complex terrain and dense vegetation. However, the cluttered under-canopy environment presents significant challenges for autonomous navigation. Existing trajectory planning methods often prioritize high speed at the expense of safety and data quality, leading to issues such as blurred imagery or point cloud distortion. To address these challenges, we propose a hierarchical speed-adaptive B-spline trajectory planning algorithm tailored specifically for China UAV drones operating in under-canopy environments. Our method unifies smoothness and safety through a two-stage optimization framework. In the front-end, we search for the shortest safe path and perform variable-step goal extension based on a comprehensive risk weight that considers both distance to obstacles and velocity direction. In the back-end, we optimize B-spline node velocities and accelerations with penalty terms to achieve smooth speed adaptation. Simulation results in a custom AirSim forest environment demonstrate that our algorithm not only realizes dynamic speed regulation but also maintains high trajectory smoothness, short flight path, and robust collision avoidance even at elevated speeds. This work provides a practical solution for China UAV drones to efficiently and safely perform under-canopy data acquisition.
1. Introduction
The application of China UAV drones in forestry has grown rapidly due to their ability to cover large areas quickly and capture high-resolution data. Under-canopy environments, characterized by dense tree trunks, branches, and foliage, impose strict constraints on flight safety and data quality. Traditional trajectory planners designed for open spaces often fail in such settings because they either ignore the need for speed adaptation or produce jerky motions that degrade sensor readings. For China UAV drones engaged in ecological monitoring tasks, both safety and smoothness are paramount. Safety ensures collision-free operation, while smoothness reduces energy consumption, improves tracking accuracy, and enhances the quality of collected imagery and LiDAR point clouds.
Several recent studies have attempted to introduce speed adaptation based on environmental risk. For instance, methods that penalize high velocity near obstacles or use velocity-direction-aware risk weights have been proposed. However, these approaches often rely on discretized state sequences, leading to poor trajectory smoothness. In contrast, continuous parametric representations such as B-spline curves offer inherent continuity and easy derivative computation. Our work bridges this gap by combining B-spline parameterization with risk-aware speed and acceleration penalties, achieving both safety and smoothness in a unified optimization framework. This is particularly important for China UAV drones operating in dense forests where frequent maneuvering is required.
2. Problem Formulation
2.1 B-Spline Trajectory Representation
We represent the trajectory as a cubic B-spline curve defined by control points q0, q1, …, qN and a knot vector. The strong convex hull property of B-splines ensures that the entire curve lies within the convex hull of its control points, facilitating collision checking via control point constraints. The derivative properties allow us to compute velocity and acceleration at knot instants efficiently. For a uniform cubic B-spline with knot spacing Δt, the position p(ti), velocity v(ti), and acceleration a(ti) at knot ti are given by:
$$ \mathbf{p}(t_i) = \frac{1}{6} (\mathbf{q}_{i-1} + 4\mathbf{q}_i + \mathbf{q}_{i+1}) $$
$$ \mathbf{v}(t_i) = \frac{\mathbf{q}_{i+1} – \mathbf{q}_{i-1}}{2\Delta t} $$
$$ \mathbf{a}(t_i) = \frac{\mathbf{q}_{i-1} – 2\mathbf{q}_i + \mathbf{q}_{i+1}}{\Delta t^2} $$
These relationships allow us to formulate optimization problems directly on the control points while implicitly satisfying smoothness constraints.
2.2 Risk-Weighted Speed Adaptation
The risk weight η(β) is defined as:
$$ \eta(\beta) = \frac{2}{1 + e^{\alpha \beta}} $$
where β = cos(θ) is the cosine of the angle between the velocity vector v and the gradient of the Euclidean signed distance field ∇c. When the drone flies toward an obstacle, v and ∇c are opposite, leading to a large η and thus penalizing high speed. Conversely, when flying away, η becomes small, encouraging faster flight. This mechanism naturally achieves speed adaptation: slowing down near obstacles and speeding up in open areas.
3. Hierarchical Speed-Adaptive B-Spline Planning
3.1 Front-End Search
The front-end first uses A* to find the shortest collision-free geometric path within the local sensor range. Key waypoints along this path are extracted. Then, a variable-step goal extension is performed using the risk weight η. The step size step(pos) at waypoint pos is computed as:
$$ \text{step}(\mathbf{pos}) = \begin{cases} S_{\max}, & d(\mathbf{pos}) > l_{\text{reduce}} \\ (S_{\max} – S_{\min}) \cdot \eta(\beta) + S_{\min}, & d(\mathbf{pos}) \leq l_{\text{reduce}} \end{cases} $$
where d(pos) is the distance to the nearest obstacle, l_reduce is the speed regulation activation distance, and S_max, S_min define the step size bounds (proportional to desired speed). Each step corresponds to a constant time interval, so the step size directly regulates the average speed along that segment. After extension, the discrete points are fitted to an initial B-spline trajectory with uniform knot spacing Δt.
To ensure continuity with the current drone state, the first three control points are computed from the initial position p0, velocity v0, and acceleration a0:
$$ \mathbf{q}_0 = \mathbf{p}_0 – \frac{\Delta t}{2} \mathbf{v}_0 – \frac{\Delta t^2}{6} \mathbf{a}_0 $$
$$ \mathbf{q}_1 = \mathbf{v}_0 \Delta t + \mathbf{p}_0 – \frac{\Delta t^2}{3} \mathbf{a}_0 $$
$$ \mathbf{q}_2 = \mathbf{v}_0 \Delta t + \mathbf{p}_0 + \frac{\Delta t^2}{3} \mathbf{a}_0 $$
If any of these control points lies inside an obstacle, the knot spacing Δt is adjusted locally to shift the points to safe positions.
3.2 Back-End Optimization
The back-end refines the initial B-spline trajectory by minimizing a composite cost function:
$$ \min_{\mathbf{q}_0,\dots,\mathbf{q}_N} J = \lambda_1 J_s + \lambda_2 J_c + \lambda_3 J_d + \lambda_4 J_e + \lambda_a J_{ea} $$
with the following components:
- Smoothness cost J_s: penalizes the integral of squared jerk (jerk magnitude). For cubic B-spline, this is equivalent to:
$$ J_s = \sum_{i=0}^{N-3} || – \mathbf{q}_i + 3\mathbf{q}_{i+1} – 3\mathbf{q}_{i+2} + \mathbf{q}_{i+3} ||^2 $$
- Collision cost J_c: penalizes control points that are too close to obstacles:
$$ J_c = \sum_{i=3}^{N-1} F_c(d(\mathbf{q}_i), d_{thr1}) $$
where F_c(x, d) = (x – d)^2 if x ≤ d, else 0.
- Dynamic feasibility cost J_d: penalizes velocity and acceleration exceeding limits:
$$ J_d = \sum_{i=0}^{N-1} F_u(\mathbf{v}_i, v_{\max}) + \sum_{i=0}^{N-1} F_u(\mathbf{a}_i, a_{\max}) $$
with F_u(u, u_max) = (||u|| – u_max)^2 if ||u|| > u_max, else 0.
- Speed penalty J_e: enforces speed adaptation at knot points:
$$ J_e = \sum_{i=3}^{N-1} \eta(\beta_i) \cdot F_c(d(\mathbf{p}(t_i)), d_{thr0}) \cdot (||\mathbf{v}(t_i)|| – v_{thr})^2 $$
where d_thr0 is the activation distance for speed reduction, and v_thr is a small lower bound to avoid zero speed.
- Acceleration penalty J_{ea}: newly introduced to suppress abrupt acceleration changes near obstacles:
$$ J_{ea} = \sum_{i=3}^{N-1} \frac{F_c(d(\mathbf{p}(t_i)), d_{thr0})}{d(\mathbf{p}(t_i))^2} \cdot ||\mathbf{a}(t_i)||^2 $$
The acceleration term is distance-weighted: near obstacles, the penalty is stronger, discouraging rapid acceleration/deceleration and thus improving smoothness. The weights λ1, λ2, λ3, λ4, λa are tuned hierarchically: dynamic feasibility and collision costs are prioritized, followed by smoothness and acceleration penalty, with speed penalty being the weakest guide.
The optimization is solved using a quasi-Newton method (e.g., L-BFGS) with analytical gradients derived from B-spline derivative chains. The average solving time is about 10 ms for typical scenarios, satisfying real-time requirements for China UAV drones.
4. Simulation Results and Analysis
4.1 Custom AirSim Forest Environment
We built two custom forest environments in AirSim (based on Unreal Engine): a sparse forest and a dense forest. The environments are populated with realistic tree models with collision meshes. The China UAV drone is equipped with a depth camera and odometry. The planning algorithm runs onboard at 10–20 Hz, receiving ground-truth pose and depth images. The simulation setup parameters are listed in Table 1.
| Parameter | Value |
|---|---|
| Start position | (0,0,0) |
| Goal position | (36,1,3) |
| Default knot spacing Δt | 0.3 s |
| Map resolution | 0.1 m |
| Speed lower bound S_min | 0.8 m/s |
| Speed upper bound S_max | 2 m/s (or 3 m/s in high-speed tests) |
| Max acceleration a_max | 2 m/s² |
| Obstacle collision radius | 0.5 m |
| Speed regulation distance l_reduce | 1.0 m |
| λ1 (smoothness) | 10 |
| λ2 (collision) | 2 |
| λ3 (dynamics) | 0.00001 |
| λ4 (speed penalty) | 0.1 |
| λa (acceleration penalty) | 1 |
4.2 Speed Adaptation in Sparse Forest
In the sparse forest environment, our algorithm demonstrated clear speed adaptation. As the China UAV drone approached a tree, it decelerated; when moving toward open regions, it accelerated. The averaged results over 10 runs were: trajectory length 36.9 m, flight time 22.9 s, average speed 1.6 m/s, and smoothness cost (integral of squared jerk) 30.1 m²/s⁵. This confirms that the risk-weight-based speed adaptation works effectively in benign clutter.
4.3 Comparison in Dense Forest
We compared our method with three state-of-the-art planners: Ego-Planner, Fast-Planner, and EVA-Planner. Table 2 summarizes the average metrics over 10 runs in the dense forest (v_max = 2 m/s).
| Method | Length (m) | Time (s) | Smoothness Cost (m²/s⁵) | Planning Time (ms) | Success Rate |
|---|---|---|---|---|---|
| Ego-Planner | 39.5 | 25.1 | 120.0 | 0.56 | 100% |
| Fast-Planner | 41.8 | 27.5 | 27.5 | 2.15 | 100% |
| EVA-Planner | 44.3 | 29.5 | 128.3 | 17.3 | 100% |
| Ours (with J_ea) | 38.5 | 27.5 | 49.2 | 10.42 | 100% |
| Ours (without J_ea) | 39.3 | 28.6 | 83.0 | 10.41 | 100% |
Our full algorithm achieved the shortest path (38.5 m) thanks to the A* front-end, while maintaining a competitive flight time (27.5 s). The smoothness cost of 49.2 m²/s⁵ was substantially lower than Ego-Planner (120) and EVA-Planner (128.3), though higher than Fast-Planner (27.5) because Fast-Planner sacrifices path length for smoothness. The removal of the acceleration penalty term J_ea resulted in a higher smoothness cost (83.0), confirming its effectiveness in suppressing abrupt velocity changes. All methods achieved 100% success rate at 2 m/s.
4.4 High-Speed Test (v_max = 3 m/s)
We increased the maximum speed to 3 m/s. The results are shown in Table 3.
| Method | Length (m) | Time (s) | Smoothness Cost (m²/s⁵) | Success Rate |
|---|---|---|---|---|
| Ego-Planner | 38.7 | 18.9 | 148 | 20% |
| Fast-Planner | 43.5 | 18.5 | 54.6 | 50% |
| EVA-Planner | 43.6 | 28.1 | 140 | 100% |
| Ours (with J_ea) | 37.3 | 22.9 | 101.1 | 100% |
At higher speed, Ego-Planner and Fast-Planner suffered significant failure rates due to tracking errors. Our method maintained 100% success because the speed adaptation mechanism reduced velocity near obstacles, and the acceleration penalty limited dynamic overshoot. The smoothness cost increased compared to the 2 m/s case but remained lower than EVA-Planner and Ego-Planner. This demonstrates the robustness of our approach for high-speed China UAV drone flight in cluttered forests.
4.5 Dynamic Obstacle Scenario
To evaluate adaptability to moving obstacles (e.g., swaying branches, small animals), we added a cubic dynamic obstacle moving perpendicular to the drone’s path. Results showed that when the obstacle speed was low (0.5 m/s) and drone speed was reduced to 1 m/s, our algorithm could detect and avoid it using local replanning, as long as the obstacle was fully within the sensor field of view. However, due to the lack of motion prediction and the static map assumption, performance degraded with faster obstacle or drone speeds. This highlights the need for future integration of dynamic mapping and predictive planning.
5. Conclusion
In this paper, we presented a hierarchical speed-adaptive B-spline trajectory planning algorithm for China UAV drones performing under-canopy data collection. By combining B-spline parameterization with risk-weighted speed and acceleration penalties, our method achieves both high trajectory smoothness and dynamic speed regulation. The front-end variable-step extension based on distance and velocity-direction risk ensures a safe initial guess, while the back-end optimization refines the trajectory to be collision-free, dynamically feasible, and smooth. Simulations in dense and sparse AirSim forests demonstrated that our method yields the shortest path, competitive flight times, superior smoothness, and 100% success rate even at elevated speeds, outperforming existing planners. Future work will focus on extending the algorithm to dynamic obstacle environments with motion prediction, multi-drone coordination, and real-world field tests in natural forests to further validate the practicality for China UAV drone forestry monitoring.