In recent years, the rapid development of low-altitude economy has propelled the large-scale application of UAV drones in fields such as logistics, agriculture, and emergency response. However, as operational scenarios become more complex, UAV drones increasingly face challenging environments like mountain valleys, urban building gaps, and industrial pipeline interiors. These narrow spaces severely compress safe flight volumes, posing significant challenges to trajectory planning and collision avoidance for UAV drones. Traditional planning algorithms, such as A* or rapidly-exploring random trees (RRT), often treat UAV drones as point masses, ignoring kinematic constraints or generating suboptimal, non-smooth paths. Similarly, obstacle avoidance methods like artificial potential fields may suffer from local minima, while machine learning approaches lack rigorous safety guarantees. Therefore, developing reliable and precise collision avoidance algorithms for UAV drones in confined settings is of critical theoretical and practical importance.
In this article, I address these challenges by proposing a numerical optimization-based framework for exact collision avoidance in narrow tubes. My approach leverages geometric modeling, convex optimization, and numerical methods to construct differentiable constraints that ensure safe and smooth trajectories for UAV drones. The core idea is to formulate the trajectory planning problem as an optimal control problem (OCP), discretize it into a nonlinear programming (NLP) problem, and solve it using efficient solvers. This method not only enhances planning success rates but also provides high-quality paths suitable for real-world deployment of UAV drones.
The remainder of this article is organized as follows: First, I introduce the geometric modeling of UAV drones, obstacles, and environments using polyhedra. Next, I construct the OCP framework incorporating collision avoidance requirements. Then, I derive explicit differentiable constraints for containment and separation based on geometric principles and the hyperplane separation theorem. Subsequently, I define the kinematic model and objective function for UAV drones, followed by discretization into an NLP using multiple shooting and Runge-Kutta methods. Finally, I present simulation case studies in narrow tube environments to validate the effectiveness of the proposed algorithm for UAV drones.
Geometric Modeling of UAV Drones, Obstacles, and Environments
Accurate geometric representation is fundamental for precise collision avoidance in UAV drones. To capture the complex shapes of UAV drones, obstacles, and confined environments while maintaining mathematical tractability, I model all entities as convex polyhedra. This representation allows for efficient computation and differentiable constraint formulation, which are essential for numerical optimization of UAV drones.
For UAV drones and obstacles, I use vertex representation, where a polyhedron is defined as the convex hull of a finite set of points. Let \( P \) denote a polyhedron with vertex set \( V_P \):
$$ V_P = [v_{1,P}, \ldots, v_{N_P, P}]^\top \in \mathbb{R}^{N_P \times N}, $$
where \( v_{i,P}^\top \) is the state vector of the \( i \)-th vertex, \( N_P \) is the number of vertices, and \( N \) is the spatial dimension (e.g., \( N=3 \) for 3D space). For a UAV drone \( B \) and an obstacle \( O \), their vertex matrices are:
$$ V_B \in \mathbb{R}^{3 \times N_B}, \quad V_O \in \mathbb{R}^{3 \times N_O}. $$
The environment \( W \), such as a narrow tube, is modeled as a convex polyhedron described by linear inequalities:
$$ W := \{ s \in \mathbb{R}^3 \mid G s \leq h \}, $$
where \( G = [g_1, \ldots, g_{N_W}]^\top \in \mathbb{R}^{N_W \times 3} \) is the constraint matrix, \( h \in \mathbb{R}^{N_W} \) is the offset vector, and \( s \) is a position vector within the environment. This formulation ensures that the environment boundaries are explicitly defined, facilitating containment constraints for UAV drones.
To summarize, the geometric parameters for typical UAV drones and obstacles in narrow tubes are listed in Table 1.
| Entity | Representation | Dimensions (cm) | Vertices Count |
|---|---|---|---|
| UAV Drone (B) | Convex Polyhedron | 6 × 6 × 2 | 8 |
| Obstacle (O) | Convex Polyhedron | 40 × 3 × 25 | 8 |
| Environment (W) | Linear Inequalities | 40 × 200 × 40 | 6 faces |
Optimal Control Problem Framework for UAV Drone Trajectory Planning
To formalize the collision avoidance problem for UAV drones, I construct an optimal control problem (OCP) that seeks to minimize a performance index while satisfying dynamic, boundary, and safety constraints. The state vector \( \xi \) represents the UAV drone’s position and possibly other states, and the control input \( u \) includes velocities or accelerations. The system dynamics are described by an ordinary differential equation:
$$ \dot{\xi} = f(\xi, u), \quad \xi(0) = \xi_{\text{init}}, $$
where \( f \) is the kinematic model, and \( \xi_{\text{init}} \) is the initial state. The OCP for UAV drones in narrow tubes is formulated as:
$$
\begin{aligned}
\min_{\xi, u} & \quad w \int_{0}^{t_f} \ell(\xi(t), u(t)) \, dt \\
\text{s.t.} & \quad \xi(0) = \xi_{\text{init}}, \quad \xi(t_f) = \xi_{\text{final}} \\
& \quad \dot{\xi}(t) = f(\xi(t), u(t)) \\
& \quad \xi(t) \in \mathcal{X}, \quad u(t) \in \mathcal{U} \\
& \quad B(\xi) \subset W \\
& \quad B(\xi) \cap O = \emptyset \quad \forall t \in [0, t_f],
\end{aligned}
$$
where \( \ell \) is a running cost, \( t_f \) is the final time, \( \mathcal{X} \) and \( \mathcal{U} \) are admissible sets for states and controls, \( B(\xi) \) denotes the UAV drone’s geometry at state \( \xi \), and \( W \) and \( O \) are the environment and obstacle, respectively. The constraints \( B(\xi) \subset W \) and \( B(\xi) \cap O = \emptyset \) ensure containment within the tube and collision avoidance for UAV drones. However, these constraints are implicit and must be transformed into explicit, differentiable forms for numerical optimization.
Differentiable Collision Avoidance Constraints for UAV Drones
The key innovation in my approach is the derivation of explicit differentiable constraints for containment and separation, which are crucial for efficient NLP solving. I leverage geometric properties and convex optimization theory to achieve this for UAV drones.
Differentiable Containment Constraint
The containment constraint \( B(\xi) \subset W \) requires that the UAV drone remains entirely inside the environment. Since \( W \) is convex and \( B \) is a convex polyhedron, this is equivalent to requiring all vertices of \( B \) to lie within \( W \). Using the vertex matrix \( V_B \) and environment inequalities, the constraint becomes:
$$ G V_B \leq h, $$
where the inequality is applied element-wise. This formulation is explicit and differentiable with respect to the UAV drone’s state, as \( V_B \) depends on \( \xi \) (e.g., through translation and rotation). For simplicity, I assume the UAV drone is rigid, so \( V_B(\xi) = R(\xi) V_{B,0} + t(\xi) \), where \( R \) is rotation and \( t \) is translation. The Jacobians can be computed for optimization, ensuring smooth integration into NLP solvers for UAV drones.
Differentiable Separation Constraint
The separation constraint \( B(\xi) \cap O = \emptyset \) ensures no collision between the UAV drone and obstacles. By the hyperplane separation theorem, two convex sets are disjoint if and only if there exists a hyperplane that strictly separates them. Let the hyperplane be defined as:
$$ H(\lambda, \mu) = \{ s \in \mathbb{R}^3 \mid \lambda^\top s = \mu \}, $$
where \( \lambda \neq 0 \) is the normal vector and \( \mu \in \mathbb{R} \) is the offset. The separation condition can be written as:
$$ B \subset H^-(\lambda, \mu), \quad O \subset H^+(\lambda, \mu), $$
where \( H^- \) and \( H^+ \) are the half-spaces \( \lambda^\top s \leq \mu \) and \( \lambda^\top s \geq \mu \), respectively. Since \( B \) and \( O \) are convex polyhedra, this is equivalent to requiring all vertices of \( B \) to lie in \( H^- \) and all vertices of \( O \) to lie in \( H^+ \):
$$ \lambda^\top V_B \leq \mu \mathbf{1}^\top, \quad \lambda^\top V_O \geq \mu \mathbf{1}^\top, \quad \|\lambda\| > 0, $$
where \( \mathbf{1} \) is a vector of ones. The norm condition \( \|\lambda\| > 0 \) prevents trivial solutions. To handle this in optimization, I impose \( \|\lambda\| \geq \epsilon \) for a small \( \epsilon > 0 \). Thus, the separation constraint becomes a set of linear inequalities in \( \lambda \) and \( \mu \), which are differentiable. For multiple obstacles, this can be extended by introducing separate hyperplanes for each obstacle, ensuring comprehensive collision avoidance for UAV drones.
The advantages of these differentiable constraints for UAV drones are summarized in Table 2.
| Constraint Type | Mathematical Form | Differentiability | Computational Complexity |
|---|---|---|---|
| Containment | \( G V_B \leq h \) | Yes (linear in vertices) | \( O(N_B N_W) \) |
| Separation | \( \lambda^\top V_B \leq \mu \mathbf{1}^\top \), \( \lambda^\top V_O \geq \mu \mathbf{1}^\top \), \( \|\lambda\| \geq \epsilon \) | Yes (linear in \( \lambda, \mu \)) | \( O(N_B + N_O) \) per obstacle |
Kinematic Model and Objective Function for UAV Drones
To complete the OCP formulation, I define the kinematic model and objective function tailored for UAV drones in narrow tubes. For simplicity, I consider a point-mass model with position states \( (x, y, z) \) and velocity controls \( (v_x, v_y, v_z) \). However, the framework can be extended to more detailed dynamics. Since the final time \( t_f \) is unknown, I introduce a normalized time variable \( \tau \in [0, 1] \) such that \( t = t_f \tau \). The kinematic model is:
$$
\frac{dx}{d\tau} = t_f v_x, \quad \frac{dy}{d\tau} = t_f v_y, \quad \frac{dz}{d\tau} = t_f v_z.
$$
The state and control vectors are \( \xi = [x, y, z]^\top \) and \( u = [v_x, v_y, v_z]^\top \), with feasible sets:
$$ |x|, |y| \leq 20 \, \text{cm}, \quad |z| \leq 100 \, \text{cm}, \quad |v_x|, |v_y|, |v_z| \leq 2.7 \, \text{cm/s}. $$
The objective function combines time minimization and control effort regularization to promote smooth trajectories for UAV drones:
$$ J(u, t_f) = t_f \left( r + \int_{0}^{1} \| u(\tau) \|^2_P \, d\tau \right), $$
where \( r = 0.01 \) is a weighting factor balancing time and control costs, and \( P = \text{diag}(10, 10, 10) \) is a diagonal weighting matrix. This encourages UAV drones to reach the goal quickly while minimizing aggressive maneuvers, which is crucial in confined spaces.
Discretization into Nonlinear Programming Problem for UAV Drones
To solve the continuous-time OCP numerically, I discretize it into a finite-dimensional nonlinear programming (NLP) problem using direct methods. This involves approximating states and controls over a time grid, transforming the infinite-dimensional problem into a form suitable for standard NLP solvers like IPOPT, which is efficient for UAV drones.
I discretize the normalized time interval \( [0, 1] \) into \( K \) subintervals with step size \( \Delta \tau = 1/K \). The control input is parameterized as piecewise constant over each subinterval: \( u(\tau) = u_k \) for \( \tau \in [\tau_k, \tau_{k+1}] \), where \( \tau_k = k \Delta \tau \). The states are discretized using multiple shooting, which splits the trajectory into segments and enforces continuity constraints. Within each segment, I employ the fourth-order Runge-Kutta (RK4) method to integrate the dynamics accurately. For the kinematic model, the RK4 update from \( \xi_k \) to \( \xi_{k+1} \) is:
$$ \xi_{k+1} = \xi_k + \Delta \tau \sum_{i=1}^{4} \mu_i \lambda_i, $$
with slopes \( \lambda_i \) and weights \( \mu_i \) given by:
$$
\begin{aligned}
\mu_1 &= \frac{1}{6}, \quad \mu_2 = \frac{1}{3}, \quad \mu_3 = \frac{1}{3}, \quad \mu_4 = \frac{1}{6}, \\
\lambda_1 &= f(\xi_k, u_k, \tau_k), \\
\lambda_2 &= f\left( \xi_k + \frac{\Delta \tau}{2} \lambda_1, u_k, \tau_k + \frac{\Delta \tau}{2} \right), \\
\lambda_3 &= f\left( \xi_k + \frac{\Delta \tau}{2} \lambda_2, u_k, \tau_k + \frac{\Delta \tau}{2} \right), \\
\lambda_4 &= f\left( \xi_k + \Delta \tau \lambda_3, u_k, \tau_k + \Delta \tau \right).
\end{aligned}
$$
The discretized OCP becomes an NLP with decision variables including states \( \xi_k \), controls \( u_k \), final time \( t_f \), and hyperplane parameters \( \lambda, \mu \) for each obstacle. The constraints include dynamics via RK4, boundary conditions, state/control limits, and the differentiable collision avoidance constraints evaluated at each discretization point. The full NLP formulation is:
$$
\begin{aligned}
\min_{\xi_k, u_k, t_f, \lambda, \mu} & \quad t_f \left( r + \sum_{k=0}^{K-1} \| u_k \|^2_P \Delta \tau \right) \\
\text{s.t.} & \quad \xi_0 = \xi_{\text{init}}, \quad \xi_K = \xi_{\text{final}} \\
& \quad \xi_{k+1} = \xi_k + \Delta \tau \sum_{i=1}^{4} \mu_i \lambda_i \quad \text{(RK4)} \\
& \quad |x_k|, |y_k| \leq 20, \quad |z_k| \leq 100 \\
& \quad |v_{x,k}|, |v_{y,k}|, |v_{z,k}| \leq 2.7 \\
& \quad G V_B(\xi_k) \leq h \quad \text{(containment)} \\
& \quad \lambda^\top V_B(\xi_k) \leq \mu \mathbf{1}^\top, \quad \lambda^\top V_O \geq \mu \mathbf{1}^\top, \quad \|\lambda\| \geq \epsilon \quad \text{(separation)} \\
& \quad \text{for } k = 0, \ldots, K-1.
\end{aligned}
$$
This NLP can be efficiently solved using interior-point methods, enabling real-time trajectory planning for UAV drones in narrow tubes.
Simulation Case Studies and Results for UAV Drones
To validate the proposed algorithm, I conducted simulations in a narrow tube environment with static obstacles. The scenario involves a UAV drone navigating from a start point to an end point while avoiding collisions and staying within tube boundaries. The environment is a rectangular tube of dimensions 40 cm × 200 cm × 40 cm, with two box-shaped obstacles placed along the path. The UAV drone is modeled as a small rectangular polyhedron of size 6 cm × 6 cm × 2 cm. Parameters are summarized in Table 3.
| Parameter | Value |
|---|---|
| Tube Dimensions (W) | 40 cm × 200 cm × 40 cm |
| Obstacle Dimensions (O) | 40 cm × 3 cm × 25 cm (each) |
| UAV Drone Dimensions (B) | 6 cm × 6 cm × 2 cm |
| Start Point | (0, -70, 0) cm |
| End Point | (6, 80, 5) cm |
| State Limits | |x|, |y| ≤ 20 cm, |z| ≤ 100 cm |
| Control Limits | |v_x|, |v_y|, |v_z| ≤ 2.7 cm/s |
| Discretization Points (K) | 50 |
| Weighting Matrix P | diag(10, 10, 10) |
| Time Weight r | 0.01 |
I implemented the NLP using CasADi with IPOPT solver in MATLAB. For comparison, I also tested a traditional RRT algorithm with 1000 maximum nodes and a step size of 4 cm. The initial guess for the optimization was a straight-line trajectory between start and end points, with controls set to half the maximum speed. Hyperplane parameters were initialized as \( \lambda = [0.5, 0.5, 0.5]^\top \) and \( \mu = 0.1 \).
The optimization-based algorithm successfully planned a smooth, collision-free trajectory for the UAV drone, as shown in the results. The trajectory adhered strictly to tube boundaries and avoided obstacles with clear separation. In contrast, the RRT path was jagged and closer to obstacles, reflecting its probabilistic nature. Key performance metrics are compared in Table 4.
| Metric | Proposed Optimization Algorithm | Traditional RRT Algorithm |
|---|---|---|
| Total Cost J | 23.334 | 192.691 |
| Trajectory Smoothness | High (continuous derivatives) | Low (piecewise linear) |
| Collision Avoidance | Exact (constraints satisfied) | Approximate (may have narrow margins) |
| Computation Time | 1.497 seconds | 1.514 seconds |
| Success Rate | 100% (in tested scenarios) | Variable (depends on sampling) |
The proposed method achieved a significantly lower cost, indicating more efficient trajectories for UAV drones. The computation times are comparable, but the optimization approach provides guaranteed constraint satisfaction, which is critical for safety in narrow tubes.
Ablation Study on Collision Avoidance Constraints
To demonstrate the necessity of both containment and separation constraints, I performed an ablation study by disabling each constraint individually. The results, summarized in Table 5, confirm that both constraints are essential for successful trajectory planning for UAV drones. Without containment, the UAV drone may exit the tube; without separation, collisions occur; only with both constraints enabled does the algorithm produce feasible paths.
| Containment Constraint | Separation Constraint | Trajectory Feasibility |
|---|---|---|
| Disabled | Disabled | Failed (exits tube and collides) |
| Enabled | Disabled | Failed (collides with obstacles) |
| Disabled | Enabled | Failed (exits tube boundaries) |
| Enabled | Enabled | Successful (safe and contained) |
Conclusion and Future Work for UAV Drones
In this article, I presented a numerical optimization-based framework for exact collision avoidance of UAV drones in narrow tubes. By modeling geometries as convex polyhedra and deriving explicit differentiable constraints through geometric methods and hyperplane separation, I enabled efficient solving of trajectory planning as an NLP. The proposed algorithm generates smooth, safe paths that respect environmental boundaries and avoid obstacles, outperforming traditional methods like RRT in terms of cost and reliability for UAV drones.
The key contributions include precise geometric modeling and differentiable constraint formulation, which provide a technical foundation for reliable trajectory planning in confined environments for UAV drones. The simulation results validate the effectiveness and robustness of the approach, demonstrating its potential for real-world applications such as pipeline inspection, urban delivery, and search-and-rescue missions involving UAV drones.
For future work, I plan to extend this framework in several directions. First, incorporating more accurate dynamics models, such as full quadrotor dynamics, could enhance realism for UAV drones. Second, handling dynamic obstacles and uncertain environments would increase applicability. Third, integrating real-time perception data and adaptive replanning could enable autonomous navigation in unknown narrow spaces for UAV drones. Finally, exploring distributed optimization techniques may allow scalable multi-UAV drone coordination in confined areas. These advancements will further solidify the role of numerical optimization in enabling safe and efficient operations for UAV drones in challenging environments.