We present a comprehensive framework for real-time motion planning and trajectory tracking control of fixed-wing drones. The proposed system addresses the critical challenge of autonomous obstacle avoidance during cruise flight, combining a collision-avoidance-force-based path planning algorithm with a decoupled position and attitude control strategy. Our approach is validated through software-in-the-loop (SITL) simulations using the PX4 autopilot platform, demonstrating real-time performance and safe trajectory execution for fixed-wing drones in complex environments.
1. Introduction
Fixed-wing drones are widely used in reconnaissance, mine detection, and remote sensing tasks due to their superior endurance, range, and cruise speed compared to rotary-wing platforms. However, these characteristics also limit their ability to hover and perform agile maneuvers, making real-time obstacle avoidance a significant challenge. To enable autonomous flight in cluttered environments, an efficient real-time motion planning algorithm and a robust trajectory tracking controller are essential. In this work, we focus on developing a complete pipeline for fixed-wing drones, integrating gradient-based trajectory optimization with L1 distance and total energy control system (TECS) for position and attitude regulation. The entire system is tested in a PX4-based SITL simulation environment to ensure feasibility and real-time performance.
2. Real-time Trajectory Planning Algorithm
2.1 Collision-Avoidance-Force-Based Path Planning
Our planning algorithm adopts a collision-avoidance-force concept to handle obstacles detected during flight. The decision variables are the control points Q of a B-spline curve. Initially, an obstacle-free B-spline trajectory A is generated that satisfies terminal constraints. When collisions are detected, local replanning is performed to produce a collision-free trajectory B. For each control point Q_i obstructed by an obstacle, we assign an anchor point p_{ij} on the obstacle surface and a direction vector v_{ij} from Q_i to p_{ij}, forming a {p,v} pair. The signed distance from Q_i to the j-th obstacle is defined as:
$$ d_{ij} = (Q_i – p_{ij})^T v_{ij} $$
The danger threshold is set such that when d_{ij} exceeds zero, a new collision is registered. To avoid duplicate {p,v} pairs, we only generate a new pair if all existing distances d_{ij} are positive. Table 1 summarizes the key parameters for trajectory optimization.
| Parameter | Symbol | Value |
|---|---|---|
| Smoothness weight | λ_s | 0.5 |
| Collision weight | λ_c | 2.0 |
| Dynamics weight | λ_d | 1.0 |
| Safety distance | d_s | 3.8 m |
| Cruise speed | V_c | 15 m/s |
2.2 {p,v} Pair Selection Using A* Algorithm
Direct anchor point localization on obstacle surfaces can be inaccurate and computationally expensive. To overcome this, we design an A* algorithm based method for {p,v} pair selection. The steps are: (i) generate an initial collision-free path B using A* search that bypasses the obstacle; (ii) for each colliding control point Q_k, compute a tangent plane to trajectory A and intersect it with path B to obtain anchor p_{kj}; (iii) define the unit vector v_{kj} pointing from Q_k to p_{kj}. This approach ensures robust pair generation even in the presence of mapping inaccuracies.
2.3 Gradient-Based Trajectory Optimization
The trajectory is represented as a uniform B-spline of order P with knot vector {t_1, t_2, …, t_M} and control points {Q_1, Q_2, …, Q_N}, where Q_i ∈ R^3 and t_M ∈ R with M=N+P. The time interval Δt = D_c / V_c, where D_c = 2 m is the distance between control points. The optimization problem is formulated as:
$$ \min_{Q} J = \lambda_s J_s + \lambda_c J_c + \lambda_d J_d $$
The smoothness penalty J_s penalizes the acceleration and jerk of the B-spline control points. The velocity V_i, acceleration A_i, and jerk J_i at each control point are computed from the differences of successive control points divided by Δt. The collision penalty J_c is defined using a piecewise penalty function:
$$ j_c(i,j) =
\begin{cases}
0 & \text{if } \Delta d_{ij} \le 0 \\
\Delta d_{ij}^3 & \text{if } 0 < \Delta d_{ij} \le d_s \\
3 d_s \Delta d_{ij}^2 – 3 d_s^2 \Delta d_{ij} + d_s^3 & \text{if } \Delta d_{ij} > d_s
\end{cases} $$
where Δd_{ij} = d_s – d_{ij}. The total collision cost is J_c = Σ_i Σ_j j_c(i,j). Its gradient with respect to Q_i is:
$$ \frac{\partial J_c}{\partial Q_i} =
\begin{cases}
0 & \text{if } \Delta d_{ij} \le 0 \\
\sum_{j} v_{ij} \cdot (-3 \Delta d_{ij}^2) & \text{if } 0 < \Delta d_{ij} \le d_s \\
\sum_{j} v_{ij} \cdot (-6 d_s \Delta d_{ij} + 3 d_s^2) & \text{if } \Delta d_{ij} > d_s
\end{cases} $$
The dynamics penalty J_d enforces feasibility constraints on velocity, acceleration, and jerk. For example, the velocity penalty function f_v(V_i) is designed to keep the speed near the cruise value V_c:
$$ f_v(V_{ir}) =
\begin{cases}
k_v (V_{ir} – V_c – V_O)^2 & \text{if } V_{ir} \ge V_c + V_O \\
0 & \text{if } V_c – V_O < V_{ir} < V_c + V_O \\
(V_c – V_O) – V_{ir}^3 & \text{if } V_j \le V_{ir} < V_c – V_O \\
a_1 V_{ir}^2 + b_1 & \text{if } 0 \le V_{ir} < V_j
\end{cases} $$
where V_O is the allowable speed interval radius, V_j is the transition point, and a_1, b_1 are coefficients ensuring smooth second derivatives. This formulation forces fixed-wing drones to quickly return to cruise speed when deviating.
2.4 Numerical Solution
We employ the Limited-Memory BFGS (L-BFGS) algorithm to solve the unconstrained optimization problem. The update for the inverse Hessian approximation H_k is given by:
$$ H_{k+1} = V_k^T H_k V_k + \rho_k s_k s_k^T $$
where ρ_k = (y_k^T s_k)^{-1}, V_k = I – ρ_k y_k s_k^T, s_k = x_{k+1} – x_k, and y_k = ∇f_{k+1} – ∇f_k. L-BFGS stores only a limited number of correction pairs, making it memory-efficient for real-time applications. The iterative update allows fast convergence for the trajectory optimization of fixed-wing drones.
2.5 Time Reallocation
After optimization, the trajectory length may increase, requiring time adjustment. For each interval [t_i, t_{i+1}], we compute the overshoot ratio:
$$ r_e = \max\{ V_{ir}/v_{\max}, A_{ir}/a_{\max}, J_{ir}/j_{\max}, 1 \} $$
If r_e > 1, the time interval is scaled by Δt’ = λ_t r_e Δt, where λ_t is a coefficient slightly larger than 1. This ensures all segments satisfy the dynamic constraints of fixed-wing drones.
3. Trajectory Tracking Control
3.1 Position Control
Position control for fixed-wing drones is achieved using a hybrid L1 distance algorithm and TECS. The L1 algorithm generates lateral acceleration to follow the desired path. It selects a target point from the trajectory based on the aircraft’s position and heading, as described in Figure 3 of the original paper. The lateral acceleration command a_sc is:
$$ a_{sc} = 2 \frac{V^2}{L} \left( \frac{d}{L} + \frac{\dot{d}}{V} \right) $$
where V is the airspeed, L is the distance to the target point, and d is the cross-track error. The commanded roll and yaw angles are:
$$ \phi_c = \arctan\left( \frac{a_{sc}}{g} \right), \quad \psi_c = \eta $$
where η is the angle between the velocity vector and the line to the target point.
TECS decouples speed and altitude control by managing total energy. The total energy per unit mass is:
$$ E_1 = \frac{V^2}{2g} + H $$
The throttle command δ_T is derived from the total energy error and its derivative:
$$ \delta_T = K_{ET} \left[ (K_{Tp} + \frac{K_{Ti}}{s}) \Delta E_1 + K_{Td} \Delta \dot{E}_1 \right] + T_{pre} $$
where T_{pre} is a predicted throttle based on the desired energy rate. Similarly, the elevator command δ_e (which serves as pitch angle command θ_c) is:
$$ \delta_e = K_{Le} \left[ (K_{ep} + \frac{K_{ei}}{s}) \Delta E_{L1} + K_{ed} \Delta \dot{E}_{L1} + K_{et} L_{1s} \right] $$
Table 2 lists the key gains for the position control system.
| Gain | Symbol | Value |
|---|---|---|
| Energy to throttle coefficient | K_ET | 0.8 |
| Throttle proportional gain | K_Tp | 1.2 |
| Throttle integral gain | K_Ti | 0.1 |
| Elevator proportional gain | K_ep | 1.0 |
| Elevator integral gain | K_ei | 0.05 |
3.2 Attitude Control
The attitude controller computes desired angular rates from the commanded roll, pitch, and yaw angles. The angular rate commands in the body frame are obtained via coordinate transformation:
$$ \dot{\phi}_s = \dot{\phi}_{sg} – \dot{\psi}_{sg} \sin \theta_r $$
$$ \dot{\theta}_s = \dot{\theta}_{sg} \cos \phi_r + \dot{\psi}_{sg} \sin \phi_r \cos \theta_r $$
$$ \dot{\psi}_s = -\dot{\theta}_{sg} \sin \phi_r + \dot{\psi}_{sg} \cos \phi_r \cos \theta_r $$
The roll attitude control law is:
$$ \dot{\phi}_c = [K_t (\phi_s – \phi_r) – \dot{\phi}_r] (K_{\phi p} + \frac{K_{\phi i}}{s}) + K_{\phi f} \phi_s $$
where K_t = 1/t_c, and K_{\phi p}, K_{\phi i}, K_{\phi f} are proportional, integral, and feedforward gains that vary inversely with airspeed. The yaw rate command is computed from the coordinated turn equation:
$$ \dot{\psi}_s = \frac{g \tan \phi_s}{V \cos \theta_s} $$
The yaw rate control law is:
$$ \dot{\psi}_c = (\dot{\psi}_s – \dot{\psi}_r) (K_{\psi p} + \frac{K_{\psi i}}{s}) + K_{\psi f} \psi_s $$
Table 3 summarizes the attitude control gains.
| Gain | Symbol | Value |
|---|---|---|
| Roll proportional gain | K_φp | 2.0 |
| Roll integral gain | K_φi | 0.5 |
| Yaw proportional gain | K_ψp | 1.5 |
| Yaw integral gain | K_ψi | 0.3 |
4. Simulation Experiments
4.1 PX4 SITL Simulation Framework
We build our simulation framework on the PX4 SITL (Software-in-the-Loop) functionality, ROS (Robot Operating System), and the Gazebo simulator. The framework consists of three main components: (i) a high-level computer running the trajectory planner and controller, (ii) the PX4 flight stack simulating the autopilot, and (iii) the Gazebo simulator providing realistic sensor data including GPS and IMU. The position controller (L1 + TECS) computes desired angular rates, which are sent to the PX4 attitude controller via offboard mode. This setup ensures a seamless transfer from simulation to real hardware for fixed-wing drones.
4.2 Online Obstacle Avoidance Trajectory Simulation
We simulate a fixed-wing drone with a wingspan of L_w = 1.2 m, a perception range of 150 m in a 45° forward-facing cone, and a cruise speed of 15 m/s. The trajectory is replanned whenever a collision is detected or after a fixed time interval. The minimum distance to obstacles D_min is maintained greater than D_s = 1.5L_w + D_c = 3.8 m, ensuring safety. Table 4 shows the planning time and speed statistics for the different phases of the simulation.
| Phase | Duration (s) | Avg Speed (m/s) | Max Speed (m/s) | Avg Planning Time (ms) |
|---|---|---|---|---|
| Phase 1 (initial to first obstacle) | 9.4 | 14.7 | 15.8 | 0.89 |
| Phase 2 (first to second obstacle) | 10.1 | 14.5 | 16.1 | 1.24 |
| Phase 3 (after second obstacle to goal) | 4.6 | 14.7 | 15.2 | 0.62 |
The planning times are consistently below 1.5 ms, demonstrating the real-time capability of the gradient-based optimization for fixed-wing drones. The speed remains close to the cruise speed throughout the mission.
4.3 Trajectory Tracking Performance
The tracking controller is evaluated during the obstacle avoidance flight. The total tracking error e remains below 1.5 m in the ground frame. Considering the minimum obstacle distance D_min = 11.48 m, the effective safety margin D_t = D_min – e > 9.98 m, which still exceeds D_s = 3.8 m. This confirms that even with tracking errors, the fixed-wing drones maintain safe distances from obstacles.
Attitude tracking performance is shown in Figure 9 of the original paper. The maximum angular tracking error is 0.13 rad, but the error quickly converges to below 0.02 rad in steady state. Angular rate tracking errors are consistently less than 0.07 rad/s. These results indicate that the combined L1 and TECS controller provides accurate and stable tracking for the dynamically replanned trajectories of fixed-wing drones.
5. Conclusion
We have presented a complete framework for real-time motion planning and trajectory tracking control of fixed-wing drones. The collision-avoidance-force-based planning algorithm, solved via L-BFGS optimization, generates smooth and dynamically feasible trajectories in milliseconds. The hybrid position controller using L1 and TECS decouples lateral and longitudinal dynamics, while the attitude controller ensures precise angular tracking. SITL simulations on the PX4 platform validate the real-time performance, safety, and tracking accuracy of the proposed system, demonstrating its practical applicability for autonomous fixed-wing drones.