Stochastic Path Planning with Obstacle Avoidance for UAVs Using Covariance Control

Abstract

Unmanned aerial vehicles (UAVs) operating in uncertain environments must plan safe and efficient trajectories while avoiding obstacles. This work addresses this challenge by formulating UAV path planning as a stochastic optimal control problem using covariance control. The objective is to generate a closed-loop guidance policy that steers both the mean and covariance of the UAV’s state toward a desired target distribution while ensuring probabilistic collision avoidance with ellipsoidal obstacles. The stochastic problem is convexified and reformulated as a sequence of deterministic optimization problems, enabling efficient computation even from coarse initial guesses. Simulation results demonstrate that the proposed method successfully produces robust trajectories and feedback policies that satisfy chance constraints on obstacle avoidance and reach the target with prescribed statistical characteristics.

1. Introduction

Unmanned Aerial Vehicles (UAVs) have become increasingly prevalent in a wide range of applications, including environmental monitoring, infrastructure inspection, surveillance, and delivery services. Their agility, low cost, and ease of deployment make them ideal for operating in complex, cluttered, or dynamically changing environments. To accomplish these tasks safely and effectively, robust and adaptive path planning methods are essential.

Path planning, or guidance, involves computing an optimal, collision-free trajectory from a start to a goal location, taking into account constraints such as path length, energy consumption, smoothness, and computational cost [1]. Classical methods include Rapid-exploring Random Trees (RRTs) [2], Voronoi Diagrams (VDs) [3], Artificial Potential Fields (APFs) [4], Visibility Graphs (VGs) [5], Dijkstra’s Algorithm [6], and Probabilistic Roadmaps (PRMs) [7]. These techniques perform well in static environments with simple obstacles but can struggle in dynamic or highly constrained scenarios.

Heuristic methods such as A* [8] provide efficient solutions but may not scale well for large problems or dynamic environments. Meta-heuristic approaches, including Genetic Algorithms (GAs) [9], Particle Swarm Optimization (PSO) [10], and Ant Colony Optimization (ACO) [11], have shown effectiveness in complex and uncertain environments due to their adaptability, though at the cost of a higher computational effort.

Recently, machine learning methods have gained attention in UAV path planning [12], particularly in dynamic and partially observable settings. These include imitation learning, which leverages expert demonstrations [13], and reinforcement learning, where policies are learned from interaction with the environment [14]. Reinforcement learning, in particular, has been applied to tasks where model-based planning is infeasible, such as real-time navigation in changing environments [15] or highly uncertain domains like Mars [16]. However, its high computational requirements and data demands, together with a lack of formal verification tools, limit their use in practical contexts.

To provide robustness to disturbances, uncertainties, and changing environments, UAV path planning can be formulated as a closed-loop guidance problem, where the UAV’s trajectory (or outer-loop control) is adjusted on the basis of real-time measurements of its state, such as position and velocity. One popular approach in aerospace applications is Model Predictive Control (MPC), which repeatedly solves an optimization problem over a finite horizon to synthesize control inputs that optimize performance, satisfy constraints, and inherently adapt to disturbances and uncertainties [17]. However, for UAVs, MPC is typically employed for trajectory tracking rather than path planning, as the computational cost of solving a full path planning problem at each time step is prohibitive for real-time operation. Moreover, MPC may yield conservative policies when uncertainties are not explicitly modeled [18].

Stochastic optimal control frameworks address these limitations by explicitly modeling uncertainty distributions and incorporating chance constraints to ensure probabilistic constraint satisfaction [19]. Unlike robust optimization, which assumes bounded disturbances and often results in overly conservative solutions, stochastic methods account for the true nature of disturbances, such as wind gusts, which are better represented by unbounded distributions [20].

Chance-constrained optimization has been applied to UAV path planning with probabilistic obstacle avoidance; however, most existing approaches focus on feedforward control policies that do not adapt to disturbances in real time. To address this limitation, these methods are often embedded within an MPC framework, which allows feedback-based replanning but increases computational demands and may require frequent onboard optimization [21]. When combined with convexification techniques, the underlying optimization problems can be solved more efficiently, enabling higher replanning frequencies and making real-time implementation more feasible [22]. In Ref. [23], a feedback control law is considered, but the feedback gains are designed using a classical method such as the linear quadratic regulator (LQR), rather than being jointly optimized with the feedforward control within the chance-constrained framework.

However, when the UAV navigates in cluttered airspaces, disturbances such as wind can significantly affect not only the nominal trajectory but also the dispersion of the state distribution. Ensuring safety thus requires explicitly shaping both the mean and the uncertainty of the UAV’s position. Existing chance-constrained MPC methods either require online optimization, which is computationally demanding, or rely on fixed feedback laws that cannot adapt optimally to uncertainty. This gap motivates the exploration of covariance control for UAV guidance, as it enables the design of closed-loop policies that simultaneously regulate the mean and covariance while remaining computationally efficient.

Covariance control is a prominent stochastic optimal control method that generates closed-loop policies to steer both the mean and covariance of the system state toward a desired distribution while optimizing a performance index [24]. Unlike MPC-based approaches, which require repeated online optimization, covariance control solves for both the feedforward control sequence and the state-feedback gains offline, yielding an explicit closed-loop policy. For linear systems, Chen et al. [25,26] and Bakolas [27,28] have shown that finite-horizon covariance control problems can be reformulated as deterministic convex optimization problems, enabling efficient computation.

Recent advances have extended covariance control to several aerospace scenarios using convexification techniques, including applications to spacecraft rendezvous [29,30], low-thrust interplanetary transfers [31,32,33], powered descent [34,35], station-keeping [36], launch vehicle guidance [37], asteroid missions [38], and planetary entry [39]. Covariance control has also been used for vehicle path planning. For example, Ref. [40] proposes a stacked-matrix formulation for generating optimal paths of a robot through non-convex obstacles modeled as unions of polytopes.

By leveraging covariance control, this work brings a robust planning capability to UAVs, ensuring reliable obstacle avoidance in uncertain and cluttered environments where conventional approaches may fail. Specifically, this paper introduces a novel UAV path planning method that combines covariance control with chance-constrained optimization to generate robust trajectories and control laws that ensure safety and performance under uncertainty. The UAV is modeled as a stochastic linear system with additive white Gaussian noise representing environmental disturbances such as wind. The vehicle must navigate from an initial Gaussian-distributed position to a target location while avoiding static ellipsoidal obstacles.

The proposed closed-loop path planning strategy optimizes not only the mean trajectory but also the probability distribution of position and velocity to ensure safety margins around obstacles with a specified probability and to arrive at the target with bounded uncertainty. State-of-the-art convexification methods are used to reformulate the covariance control problem as a sequence of deterministic convex programs, solvable with low computational cost even from coarse initial guesses.

The remainder of this paper is organized as follows. Section 2 formulates the UAV path planning problem as a covariance control problem with chance constraints. Section 3 outlines the mathematical framework and convexification techniques used to pose the problem as a sequence of deterministic semidefinite programs. Section 4 demonstrates the performance of the proposed method through numerical simulations, and Section 5 concludes the paper.

2. Problem Statement

We address the problem of trajectory planning for a UAV operating in a three-dimensional environment populated by multiple static ellipsoidal obstacles. The vehicle is subject to stochastic disturbances, which reflect realistic sources of uncertainty such as wind gusts or unmodeled aerodynamic effects.

The scenario, illustrated in Figure 1, involves navigating the UAV from an initial location to a desired terminal position while ensuring obstacle avoidance. The UAV operates in an inertial reference frame ${\mathcal{F}}_I=\{O;{\mathit{x}}^I,{\mathit{y}}^I,{\mathit{z}}^I\}$, with the geometric centers of the obstacles located at fixed positions ${\mathit{o}}_i$, for $i=1,\dots ,n$. A body-fixed reference frame ${\mathcal{F}}_B=\{O^B;{\mathit{x}}^B,{\mathit{y}}^B,{\mathit{z}}^B\}$, centered in the vehicle center of mass, is also depicted.

2.1. Deterministic Dynamics

As we focus on the UAV path planning problem, a three-degrees-of-freedom (3-DOF) translational model is considered under the assumption that the attitude control system is capable of tracking the commanded attitude, which is retrieved from the thrust vector (being aligned with the $\mathit{z}$ body-axis). This modeling approach is consistent with the standard inner–outer loop control architecture, commonly adopted for smooth, non-aggressive UAV trajectories such as those used by PX4 [41], where the outer-loop planner generates the desired attitude and thrust norm, and a standard inner-loop controller ensures accurate tracking of these commands [42]. The fully nonlinear six-degrees-of-freedom (6-DOF) UAV model used for validation is described in Appendix A. A comprehensive validation using a higher-fidelity model incorporating sensor and estimation effects, as well as HIL/SITL or real-world testing, lies beyond the scope of this manuscript and is left for future work.

The UAV’s state at time t is defined by its inertial position $\mathit{r}\left(t\right)\in {\mathbb{R}}^3$ and velocity $\mathit{v}\left(t\right)\in {\mathbb{R}}^3$. The control input is the thrust vector $\mathit{u}\left(t\right)={[u_x\left(t\right),u_y\left(t\right),u_z\left(t\right)]}^{\top }\in {\mathbb{R}}^3$, with $(u_x,u_y)$ acting in the horizontal plane and $u_z$ representing the vertical component. The full system state is denoted as $x\left(t\right)={[\mathit{r}{\left(t\right)}^{\top },\mathit{v}{\left(t\right)}^{\top }]}^{\top }\in {\mathbb{R}}^6$.

Under deterministic conditions (i.e., in the absence of disturbances), the translational dynamics of the UAV are written as

$$\begin{array}{cc}\hfill \dot{\mathit{r}}\left(t\right)& \hfill =\mathit{v}\left(t\right)\hfill \end{array}$$

(1)

$$\begin{array}{cc}\hfill \dot{\mathit{v}}\left(t\right)& \hfill ={\displaystyle \frac{1}{m}}\mathit{u}\left(t\right)-g{\mathit{e}}_3+{\displaystyle \frac{\mathit{D}}{m}}\hfill \end{array}$$

(2)

where m is the vehicle’s mass, g is the gravitational constant, and ${\mathit{e}}_3={[0,0,1]}^{\top }$ is the vertical unit vector. The aerodynamic drag force $\mathit{D}$ is defined as

$$\mathit{D}=-{\displaystyle \frac{1}{2}}{k}_{\mathrm{drag}}\left|\right|\mathit{v}\left|\right|\mathit{v}$$

(3)

where $k_{\mathrm{drag}}=\rho S{C}_D$ is a positive constant that encapsulates the drag coefficient $C_D$, air density $\rho$, and reference area S.

2.2. Stochastic Dynamics

To model environmental uncertainties, we consider additive white Gaussian noise and express the system dynamics as a linear stochastic differential Equation (SDE)

$$d\mathit{x}\left(t\right)=\left(Ax\left(t\right)+B\mathit{u}\left(t\right)+\mathit{c}\right)dt+Gd\mathit{w}\left(t\right)$$

(4)

where the matrices A, B, and vector $\mathit{c}$ are defined as follows:

$$A={\displaystyle \frac{\partial \mathit{f}}{\partial \mathit{x}}}{|}_{\mathit{x}=\widehat{\mathit{x}},\mathit{u}=\widehat{\mathit{u}}}B={\displaystyle \frac{\partial \mathit{f}}{\partial \mathit{u}}}{|}_{\mathit{x}=\widehat{\mathit{x}},\mathit{u}=\widehat{\mathit{u}}}\mathit{c}=\mathit{f}(\widehat{\mathit{x}},\widehat{\mathit{u}})-A\widehat{\mathit{x}}-B\widehat{\mathit{u}}$$

(5)

Here, $\mathit{f}(\mathit{x},\mathit{u})$ represents the deterministic dynamics of the system as in Equations (1) and (2), and $(\widehat{\mathit{x}},\widehat{\mathit{u}})$ is a reference point around which the system is linearized. The analytical expressions of the system matrices A and B are

$$A=\left[\begin{array}{cc}{\mathbf{0}}_{3\times 3}& I_3\\ {\mathbf{0}}_{3\times 3}& A_v\end{array}\right]B={\displaystyle \frac{1}{m}}\left[\begin{array}{c}{\mathbf{0}}_{3\times 3}\\ I_3\end{array}\right]$$

(6)

where

$$A_v=-{\displaystyle \frac{k_{\mathrm{drag}}}{2m}}\left({∥\widehat{\mathit{v}}∥}_2{I}_3+{\displaystyle \frac{\widehat{\mathit{v}}{\widehat{\mathit{v}}}^{\top }}{{∥\widehat{\mathit{v}}∥}_2}}\right)$$

(7)

and the disturbance matrix G injects process noise into the velocity states

$$G=g_w\left[\begin{array}{c}{\mathbf{0}}_{3\times 3}\\ I_3\end{array}\right]$$

(8)

being $I_n$ the $n\times n$ identity matrix and ${\mathbf{0}}_{m\times n}$ the $m\times n$ zero matrix.

The vector $\mathit{w}\left(t\right)\in {\mathbb{R}}^6$ represents a Wiener process, which is a real-valued continuous-time stochastic process with $\mathbb{E}\left[\mathit{w}\right(t\left)\right]=\mathbf{0}$ and covariance $\mathbb{C}\mathrm{ov}[\mathit{w}\left(t\right)-\mathit{w}\left(s\right)]=|t-s|I_6$. The matrix G is a constant matrix that scales the noise and determines how disturbances affect the system dynamics. In this case, we assume that the disturbances are perturbing accelerations, so we set

$$G=g_v\left[\begin{array}{cc}{\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}\\ {\mathbf{0}}_{3\times 3}& I_3\end{array}\right]$$

(9)

where $g_v$ is a positive constant that quantifies the intensity of the disturbances.

2.3. Stochastic Optimal Control Problem

Due to the stochastic nature of the dynamics, we formulate the control problem as a stochastic optimal control problem, specifically a covariance control problem. The objective is to find a control policy $\mathit{u}\left(t\right)$ that steers the system state from an initial distribution to a desired target distribution at a final time $t_f$.

We assume both the initial and target state distributions are Gaussian: the initial state follows $\mathit{x}\left(t_0\right)\sim \mathcal{N}({\overline{\mathit{x}}}_0,P_{x,0})$ and the target state follows $\mathit{x}\left(t_f\right)\sim \mathcal{N}({\overline{\mathit{x}}}_f,P_{x,f})$. Here, $\overline{\mathit{x}}$ denotes the mean (expected value) of the state, and $P_x$ denotes its covariance matrix. This modeling choice is particularly relevant for UAV applications, where navigation systems typically provide state estimates with Gaussian uncertainty, due to the propagation of sensor noise through linear or linearized estimation filters (e.g., Kalman filters). Additionally, specifying a target as a Gaussian distribution reflects realistic operational requirements, where final positions and velocities are only required within acceptable bounds rather than as exact points. This Gaussian assumption is further motivated as the solution of a linear stochastic differential equation remains Gaussian at all times when initialized from a Gaussian distribution [43]. Moreover, since Gaussian distributions are fully characterized by their first two moments (mean and covariance), we do not need to consider higher-order statistical moments in our analysis.

To achieve the desired mean and covariance steering, the control policy must incorporate both feedforward and feedback components. For this work, we consider a simple linear state feedback control law of the form

$$\mathit{u}\left(t\right)=\overline{\mathit{u}}+K\left(t\right)(\mathit{x}\left(t\right)-\overline{\mathit{x}}\left(t\right))$$

(10)

where $\overline{\mathit{u}}\left(t\right)$ is the feedforward control input, which also equates to the expected value of the control input $\mathbb{E}\left[\mathit{u}\right(t\left)\right]$, and $\mathit{K}\left(t\right)$ is the time-varying feedback gain matrix that adjusts the control input based on the deviation of the current state $\mathit{x}\left(t\right)$ from the expected state $\overline{\mathit{x}}\left(t\right)$.

By replacing the control input in Equation (4) with the control law in Equation (10), the time-evolution of the statistical moments of the state is given by the following set of ordinary differential equations (ODEs) [43]

$$\begin{array}{cc}\hfill \dot{\overline{\mathit{x}}}& \hfill =A\overline{\mathit{x}}+B\overline{\mathit{u}}+\mathit{c}\hfill \end{array}$$

(11a)

$$\begin{array}{cc}\hfill {\dot{P}}_x& \hfill =(A+BK)P_x+P_x{(A+BK)}^T+G{G}^T\hfill \end{array}$$

(11b)

Here, the feedforward term is responsible for driving the mean state $\overline{\mathit{x}}\left(t\right)$ from its initial value ${\overline{\mathit{x}}}_0$ to the target ${\overline{\mathit{x}}}_f$, while the feedback term shapes the covariance evolution $P_x\left(t\right)$ to reach the desired final covariance $P_{x,f}$.

To reflect physical limitations, the thrust vector $\mathit{u}\left(t\right)$ is constrained by a maximum allowable norm $u_{max}$, so that

$$\mathrm{Pr}\{\parallel \mathit{u}\left(t\right){\parallel }_2\le u_{max}\}\ge p$$

(12)

Equation (12) is enforced as chance constraints with probability p to account for the stochastic nature of the control input. Indeed, since the control $\mathit{u}\left(t\right)$ depends on the Gaussian state $\mathit{x}\left(t\right)$, the control itself becomes a Gaussian random variable and is, therefore, unbounded. Therefore, hard constraints on the control input, such as ${\parallel \mathit{u}\left(t\right)\parallel }_2\le u_{max}$, cannot be enforced in a strict sense, as they would lead to infeasibility in the optimization problem.

The probability level p directly regulates the conservatism of the chance constraints: higher values of p enforce stricter safety margins, leading to more conservative control actions, while lower values reduce conservatism at the expense of safety guarantees. This parameter can thus be tuned according to the requirements of a specific application.

In addition to the obstacle avoidance constraints, a tilt-angle constraint is enforced to ensure that the UAV maintains a controllable attitude along the trajectory. Indeed, this constraint limits the angle between the thrust direction and the vertical axis, reflecting the mechanical limitations of the UAV’s propulsion system. Mathematically, this is enforced by requiring

$$\parallel {\overline{\mathit{u}}}_{xy}{\left(t\right)\parallel }_2\le tan\left({\theta }_{max}\right){\overline{u}}_z\left(t\right)$$

(13)

where ${\theta }_{max}$ denotes the maximum allowable tilt angle, and ${\mathit{u}}_{xy}\left(t\right)$ and $u_z\left(t\right)$ are the (mean) lateral and vertical components of the thrust vector, respectively. Note that this constraint is convex for ${\theta }_{max}<\pi /2$; more precisely, Equation (13) belongs to the class of second-order cone (SOC) constraints.

For the sake of simplicity, the tilt-angle constraint was enforced only on the mean trajectory, rather than being formulated as a chance constraint. As demonstrated in Section 4, the majority of the trajectory remains well below the maximum allowable tilt angle, making a probabilistic formulation unnecessary. Enforcing the constraint on the mean not only suffices for safety but also preserves the convexity of the optimization problem, avoiding the need for iterative linearizations required by a chance-constrained approach.

The UAV must avoid collisions with static obstacles, which are modeled as ellipsoidal exclusion zones. Collision avoidance is ensured by requiring the UAV’s position to remain outside each exclusion zone with high probability. Specifically, the chance constraint is formulated as

$$\mathrm{Pr}\{{(\mathit{r}\left(t\right)-{\mathit{o}}_i)}^{\top }{\Omega }_i^{-1}(\mathit{r}\left(t\right)-{\mathit{o}}_i)\ge 1\}\ge p,i=1,\dots ,n$$

(14)

where ${\mathit{o}}_i$ is the center of the i-th ellipsoid and ${\Omega }_i$ is a positive-definite matrix that defines the shape and orientation of the i-th ellipsoid. In the present manuscript, all obstacles are considered fixed. The extension of the proposed formulation to dynamic obstacles, which requires accounting for their reconstructed trajectories, is left for future work.

It is also worth noting that the ellipsoidal model is a versatile representation that can model other shapes by appropriately choosing the matrix ${\Omega }_i$ or its inverse. For instance, if ${\Omega }_i=d_{min}^2{I}_3$, then Equation (14) reduces to the spherical case, where the UAV must maintain a minimum distance $d_{min}$ from the center of each obstacle ${\mathit{o}}_i$. As another example, by setting ${\Omega }_i^{-1}=\mathrm{diag}(1/r_{ix}^2,1/r_{iy}^2,0)$, the chance constraint can be adapted to model cylindrical obstacles aligned with the vertical axis, which is a common representation for tall obstacles such as buildings or trees.

The control policy $\mathit{u}\left(t\right)$ is optimized to minimize the control effort, which is defined as the integral of the thrust magnitude over a finite time horizon $[0,t_f]$, so that

$$J={\int }_{t_0}^{t_f}{\parallel \mathit{u}\left(t\right)\parallel }_{2}dt.$$

(15)

The optimization problem can thus be formulated as

$$\begin{array}{cc}\hfill \underset{\mathit{u}\left(t\right)}{min}& \hfill {\int }_{t_0}^{t_f}{\parallel \mathit{u}\left(t\right)\parallel }_{2}dt\hfill \end{array}$$

(16a)

$$\begin{array}{cc}\hfill \mathrm{s}.\mathrm{t}.& \hfill d\mathit{x}\left(t\right)=\left(A\mathit{x}\right(t)+B\mathit{u}(t)+\mathit{c})dt+Gd\mathit{w}\left(t\right)\hfill \end{array}$$

(16b)

$$\begin{array}{cc}\hfill & \hfill \mathit{x}\left(t_0\right)\sim \mathcal{N}({\overline{\mathit{x}}}_0,P_{x,0})\hfill \end{array}$$

(16c)

$$\begin{array}{cc}\hfill & \hfill \mathit{x}\left(t_f\right)\sim \mathcal{N}({\overline{\mathit{x}}}_f,P_{x,f})\hfill \end{array}$$

(16d)

$$\begin{array}{cc}\hfill & \hfill Pr\left({\parallel \mathit{u}\left(t\right)\parallel }_2\le u_{max}\right)\ge p\hfill \end{array}$$

(16e)

$$\begin{array}{cc}\hfill & \hfill Pr\left({(\mathit{r}\left(t\right)-{\mathit{o}}_i)}^{\top }{\Omega }_i^{-1}(\mathit{r}\left(t\right)-{\mathit{o}}_i)\ge 1\right)\ge p\hfill \end{array}$$

(16f)

$$\begin{array}{cc}\hfill & \hfill \parallel {\overline{u}}_{xy}{\left(t\right)\parallel }_2\le tan\left({\theta }_{max}\right){\overline{u}}_z\left(t\right)\hfill \end{array}$$

(16g)

Note that the constraints hold for all $t\in [0,t_f]$ and for all obstacles indexed by $i=1,\dots ,n$.

3. Convex Formulation

This section details the transformation of the nonlinear covariance control problem in Equation (16) into a deterministic convex optimization framework. In addition to ensuring tractability of the problem, this reformulation leverages the well-documented computational advantages of convex optimization over general nonlinear programming approaches, which have been repeatedly demonstrated in aerospace applications [44].

The proposed methodology combines lossless relaxation techniques [45] with successive convexification strategies [44], reformulating the original problem as a sequence of convex optimization problems solvable by highly efficient numerical solvers. While theoretical convergence guarantees exist only under specific conditions [46,47], extensive numerical evidence suggests that this approach converges in a wide range of practical applications, including UAV guidance [48,49,50,51].

3.1. Discretization

By considering Equation (11) in place of the SDE in Equation (4), we can reformulate the covariance control problem in Equation (16) in terms of mean state $\overline{\mathit{x}}\left(t\right)$, covariance state matrix $P_x\left(t\right)$, feedforward control $\overline{\mathit{u}}\left(t\right)$, and feedback gain matrix $K\left(t\right)$ as decision variables. However, when addressing covariance control problems where both the feedforward component $\overline{\mathit{u}}\left(t\right)$ and the feedback gain matrix $K\left(t\right)$ serve as decision variables, direct numerical collocation of the ODE constraints in Equation (11) can be computationally challenging. This difficulty arises because Equation (11b) exhibits stiffness for certain configurations of the feedback gain matrix $K\left(t\right)$, thus requiring a sufficiently accurate numerical integration scheme to ensure convergence.

In the authors’ experience, a discrete-time representation of the system dynamics is more suitable for numerical optimization, as it is more robust to stiffness issues. In this respect, we adopt a zero-order hold (ZOH) control strategy, where $\mathit{u}\left(t\right)=\mathit{u}\left(t_j\right)\forall t\in [t_j,t_{j+1})$. By applying the ZOH strategy and integrating both sides of Equation (4) over the time interval $[t_j,t_{j+1})$, we obtain the following time-evolution equation for the state

$$\mathit{x}\left(t\right)=\Phi (t_j,t)\mathit{x}\left(t_j\right)+{\int }_{t_j}^t\Phi (s,t)B\left(s\right)ds\mathit{u}\left(t_j\right)+{\int }_{t_j}^t\Phi (s,t)\mathit{c}\left(s\right)ds+{\int }_{t_j}^t\Phi (s,t)G\left(s\right)d\mathit{w}\left(s\right)$$

(17)

where $\Phi (t_a,t_b)$ represents the state transition matrix from time $t_a$ to $t_b$. The state transition matrix satisfies the matrix differential equation

$${\displaystyle \frac{d}{dt}}\Phi (t_j,t)=A\left(t\right)\Phi (t_j,t)$$

(18)

subject to the initial condition $\Phi (t_j,t_j)=I_{n_x}$, with $A\left(t\right)$ as defined in Equation (6).

Equation (17) can be rewritten in a more compact form as

$${\mathit{x}}_{j+1}=A_j{\mathit{x}}_j+B_j{\mathit{u}}_j+{\mathit{c}}_j+G_j{\mathit{w}}_j\mathrm{for}j=1,\dots ,N-1$$

(19)

where ${\mathit{x}}_j=\mathit{x}\left(t_j\right)$, ${\mathit{u}}_j=\mathit{u}\left(t_j\right)$, and N denotes the number of uniformly distributed temporal nodes ${\left\{t_j\right\}}_{j=1}^N$ spanning the interval from $t_0$ to $t_f$. The discrete system matrices $A_j$, $B_j$, ${\mathit{c}}_j$ in Equation (19) are given by

$$\begin{array}{cc}\hfill A_j& \hfill =\Phi (t_j,t_{j+1})\hfill \end{array}$$

(20)

$$\begin{array}{cc}\hfill B_j& \hfill ={\int }_{t_j}^{t_{j+1}}\Phi (s,t_{j+1})B\left(s\right)ds\hfill \end{array}$$

(21)

$$\begin{array}{cc}\hfill {\mathit{c}}_j& \hfill ={\int }_{t_j}^{t_{j+1}}\Phi (s,t_{j+1})\mathit{c}\left(s\right)ds\hfill \end{array}$$

(22)

The matrix $G_j$ is constructed such that $G_j{\mathit{w}}_j$ represents an $n_x$-dimensional Gaussian random vector with covariance equal to

$$Q_j={\int }_{t_j}^{t_{j+1}}\Phi (s,t_{j+1})G\left(s\right)G^T\left(s\right){\Phi }^T(s,t_{j+1})ds$$

(23)

By considering the expectation and covariance of Equation (19), we can derive the mean and covariance dynamics in discrete time

$$\begin{array}{cccc}\hfill {\overline{\mathit{x}}}_{j+1}& \hfill =A_j{\overline{\mathit{x}}}_j+B_j{\overline{\mathit{u}}}_j+{\mathit{c}}_j\hfill & \hfill & \hfill \mathrm{for}j=1,\dots ,N-1\hfill \end{array}$$

(24a)

$$\begin{array}{cccc}\hfill P_{x,j+1}& \hfill =(A_j+B_j{K}_j)P_{x,j}{(A_j+B_j{K}_j)}^T+Q_j\hfill & \hfill & \hfill \mathrm{for}j=1,\dots ,N-1\hfill \end{array}$$

(24b)

The main advantage of this reformulation is that it allows us to work with deterministic ODEs instead of stochastic differential equations, which simplifies the analysis and allows us to apply well-known numerical methods for solving optimal control problems subject to ODE constraints.

3.2. Lossless Covariance Propagation

The covariance propagation in Equation (24b) is nonlinear in the variables $P_{x,j}$ and $K_j$. To address this nonlinearity, we change the optimization variable $K_j$ to $Y_j$, which is defined as

$$Y_j=K_j{P}_{x,j}$$

(25)

where $Y_j$ is a matrix of the same size as $K_j$, leaving the problem dimension unchanged. Furthermore, after the problem is solved, $K_j$ can be retrieved from $Y_j$ as $K_j=Y_j{P}_{x,j}^{-1}$.

Then, we introduce the auxiliary variable $H_j$, defined as

$$H_j=K_j{P}_{x,j}{K}_j^T$$

(26)

which can be substituted together with Equation (25) in Equation (24b) to obtain a linear constraint in $P_{x,j}$, $Y_j$, and $H_j$, that is

$$P_{x,j+1}=A_j{P}_{x,j}{A}_j^T+B_j{Y}_j{A}_j^T+A_j{Y}_j^T{B}_j^T+B_j{H}_j{B}_j^T+Q_j$$

(27)

for $j=1,\dots ,N-1$.

By replacing $Y_j$ in Equation (26), we obtain

$$H_j=Y_j{P}_{x,j}^{-1}{Y}_j^T$$

(28)

Equation (28) should be added to the optimization problem as an additional, non-convex, constraint since it is an equality constraint nonlinear in the variables $Y_j$ and $P_{x,j}$. Nevertheless, its relaxation can be written in a convex form. Using the notation $A⪰B$ to indicate that the matrix $A-B$ is positive semidefinite, one has

$$H_j⪰Y_j{P}_{x,j}^{-1}{Y}_j^T$$

(29)

that can be written as a convex semidefinite cone constraint using Schur’s lemma

$$\left[\begin{array}{cc}{H}_j& Y_j\\ Y_j^T& P_{x,j}\end{array}\right]⪰0$$

(30)

Intuitively, Equation (29) states that all eigenvalues of the matrix $H_j$ must be greater than or equal to the eigenvalues of the matrix $Y_j{P}_{x,j}^{-1}{Y}_j^T$, meaning that $H_j$ acts as an upper bound for the control covariance represented by the matrix $Y_j{P}_{x,j}^{-1}{Y}_j^T$. Thus, provided that a measure of $H_j$ (specifically, its trace) is minimized in the cost function, the solution of the relaxed problem will be the same as the original problem, and the constraint Equation (29) will be satisfied with the equality sign [52]. The lossless nature of this transformation will be evaluated and confirmed numerically in the following sections.

3.3. Convexification of the Maximum Control Norm Constraint

The maximum control norm constraint in Equation (12) is a chance constraint that must be reformulated into a tractable, convex form. To achieve this, we use the following sufficient condition for the chance constraint [32]

$$\mathrm{Pr}\{\parallel {\mathit{u}}_j{\parallel }_2\le u_{max}\}\ge p\Leftarrow \parallel {\overline{\mathit{u}}}_j{\parallel }_2+\gamma \left(p\right)\sqrt{\rho \left(H_j\right)}\le u_{max}$$

(31)

where $\gamma \left(p\right)=\sqrt{Q_{{\chi }_{n_u}^2}\left(p\right)}$ is the square root of the quantile function of the chi-squared distribution with $n_u$ degrees of freedom, and $\rho \left(H_j\right)$ is the spectral radius (i.e., the largest eigenvalue) of the matrix $H_j$, which is equal to the control input covariance at time $t_j$, as per Equations (26) and (10).

The sufficient condition is a conservative approximation of the chance constraint; however, it provides a tractable convex expression of the constraint that can be incorporated into the optimization problem with low computational complexity. An analysis of the tightness of this approximation is provided in [33]. It is worth noting that this conservatism is necessary, as the condition remains computationally tractable and can be efficiently convexified, ensuring practical applicability in real-time optimization.

The spectral radius $\rho \left(H_j\right)$ can be computed as the largest eigenvalue of the matrix $H_j$. In this respect, we can introduce an auxiliary variable ${\rho }_j$ that must satisfy the following convex constraint for each time step $j=1,\dots ,N-1$

$${\rho }_j{I}_{n_u}⪰H_j$$

(32)

This constraint ensures that ${\rho }_j$ is greater than or equal to the largest eigenvalue of $H_j$, thus providing a valid upper bound for the spectral radius. In Section 4 section, we will show that the optimal solution converges to the tightest possible value of ${\rho }_j$ that satisfies Equation (32).

Due to the square root in Equation (31), a further step is required to obtain a convex constraint. Specifically, we introduce an auxiliary variable ${\tau }_j$ defined as

$${\tau }_j^2={\rho }_j$$

(33)

By linearizing Equation (33) around a reference value ${\widehat{\tau }}_j$, we obtain the following linearized constraint

$${\widehat{\tau }}_j^2+2{\widehat{\tau }}_j({\tau }_j-{\widehat{\tau }}_j)+{\xi }_{\tau ,j}={\rho }_j$$

(34)

for $j=1,\dots ,N-1$. In Equation (34), ${\xi }_{\tau ,j}$ is a virtual buffer variable that ensures that the linearized constraint does not become infeasible for any value of ${\widehat{\tau }}_j$. The virtual buffer can assume any value in the real numbers, and it is minimized in the optimization problem to ensure that the converged optimal solution does not use it to relax the constraint.

To ensure the validity of the linearization, we introduce a soft trust region around the reference value ${\widehat{\tau }}_j$ by enforcing the following constraint

$$\parallel {\tau }_j-{\widehat{\tau }}_j\parallel \le {\delta }_{\tau ,j},$$

(35)

for $j=1,\dots ,N-1$. This constraint ensures that the variable ${\tau }_j$ remains in the vicinity of the reference value ${\widehat{\tau }}_j$, thus maintaining the validity of the linearization. The trust region radius ${\delta }_{\tau ,j}$ is a positive additional decision variable that is minimized in the optimization cost.

Finally, we introduce an auxiliary variable ${\nu }_j\ge 0$ to characterize the norm of the mean control input ${\overline{\mathit{u}}}_j$ at time $t_j$

$$\parallel {\overline{\mathit{u}}}_j{\parallel }_2\le {\nu }_j$$

(36)

for $j=1,\dots ,N-1$. This is a convex second-order cone constraint that the optimal solution is guaranteed to satisfy as an equality, as long as ${\nu }_j$, which represents the norm of the mean control input, is minimized in the optimization problem.

By combining Equations (31), (32), (34), and (36), we obtain the following linear deterministic expression of the chance constraint in Equation (12)

$${\nu }_j+\gamma \left(p\right){\tau }_j\le u_{max}$$

(37)

for $j=1,\dots ,N-1$.

3.4. Convexification of the Collision Avoidance Constraint

The collision avoidance constraint in Equation (14) is also a chance constraint and must be reformulated into a deterministic convex form. In the deterministic case, the collision avoidance condition in Equation (14) corresponds to a concave inequality constraint on the state. Liu and Lu [47] proposed a successive convexification method that guarantees convergence of concave inequality constraints linearized via first-order Taylor expansion around a nominal trajectory. A similar linearization strategy is adopted in this manuscript. Specifically, for each obstacle $i=1,\dots ,n$ and each time step $j=1,\dots ,N$, the constraint in Equation (14) is linearized around the reference trajectory $\widehat{\mathit{r}}\left(t\right)$, leading to

$$\mathrm{Pr}\{{(\widehat{\mathit{r}}\left(t\right)-{\mathit{o}}_i)}^{\top }{\Omega }_i^{-1}(\widehat{\mathit{r}}\left(t\right)-{\mathit{o}}_i)+2{(\widehat{\mathit{r}}\left(t\right)-{\mathit{o}}_i)}^{\top }{\Omega }_i^{-1}(\mathit{r}\left(t\right)-\widehat{\mathit{r}}\left(t\right))\ge 1\}\ge p$$

(38)

By isolating the only decision variable ${\mathit{r}}_j$ in Equation (38), we obtain the following equivalent linearized chance constraint

$$\mathrm{Pr}\{{\mathit{a}}_{i,j}^{\top }{\mathit{r}}_j\ge b_{i,j}\}\ge p$$

(39)

where

$$\begin{array}{cc}\hfill {\mathit{a}}_i& \hfill =2{\Omega }_i^{-1}(\widehat{\mathit{r}}\left(t\right)-{\mathit{o}}_i)\hfill \end{array}$$

(40a)

$$\begin{array}{cc}\hfill b_i& \hfill =1-{(\widehat{\mathit{r}}\left(t\right)-{\mathit{o}}_i)}^{\top }{\Omega }_i^{-1}(\widehat{\mathit{r}}\left(t\right)-{\mathit{o}}_i)+{\mathit{a}}_i^{\top }\widehat{\mathit{r}}\left(t\right)\hfill \end{array}$$

(40b)

Since ${\mathit{r}}_j$ is a Gaussian random variable, the following conditions are equivalent

$$\mathrm{Pr}\{{\mathit{a}}_{i,j}^{\top }{\mathit{r}}_j\ge b_{i,j}\}\ge p\iff {\mathit{a}}_{i,j}^{\top }{\overline{\mathit{r}}}_j+z_p\sqrt{{\mathit{a}}_{i,j}^{\top }{P}_{r,j}{\mathit{a}}_{i,j}}\ge b_{i,j}$$

(41)

where $z_p$ is the quantile of the standard normal distribution corresponding to the probability p, ${\overline{\mathit{r}}}_j$ is the expected value of the position at time $t_j$, and $P_{r,j}$ is the covariance matrix of the position at time $t_j$, which can be computed as the first three rows and columns of the covariance matrix $P_{x,j}$.

The equation on the right-hand side of Equation (41) is nonlinear in the decision variable $P_{r,j}$, due to the presence of the square root. To address this nonlinearity, we introduce an auxiliary variable $s_{i,j}$, defined as

$$s_{i,j}^2={\mathit{a}}_{i,j}^{\top }{P}_{r,j}{\mathit{a}}_{i,j}$$

(42)

Note that the number of auxiliary variables $s_{i,j}$ is equal to the number of obstacles n multiplied by the number of time steps N, thus resulting in a total of $n·N$ auxiliary variables.

By linearizing Equation (42) around a reference value ${\widehat{s}}_{i,j}$, we obtain the following linearized constraint:

$${\widehat{s}}_{i,j}+2{\widehat{s}}_{i,j}(s_{i,j}-{\widehat{s}}_{i,j})+{\xi }_{s,ij}={\mathit{a}}_{i,j}^{\top }{P}_{r,j}{\mathit{a}}_{i,j}$$

(43)

where ${\xi }_{s,ij}$ is a virtual buffer variable that, similarly to ${\xi }_{\tau ,j}$ in Equation (34), ensures the feasibility of the linearized constraint. Furthermore, similarly to ${\xi }_{\tau ,j}$, a penalty term that is a function of the norm of the virtual buffer variable ${\xi }_{s,ij}$ is added to the optimization cost.

A trust region around the reference value ${\widehat{s}}_{i,j}$ is enforced to ensure the validity of the linearization

$$|s_{i,j}-{\widehat{s}}_{i,j}|\le {\delta }_{s,ij}$$

(44)

for $i=1,\dots ,n$ and $j=1,\dots ,N$. The radius of the trust region ${\delta }_{s,ij}$ is an additional positive decision variable. Its norm is added to the cost function to ensure that the converged solution remains within the vicinity of the reference value ${\widehat{s}}_{i,j}$.

By replacing $s_{i,j}$ in Equation (41), we obtain the following linear deterministic constraint

$${\mathit{a}}_{i,j}^{\top }{\overline{\mathit{r}}}_j+z_p{\widehat{s}}_{i,j}\ge b_{i,j}$$

(45)

which, together with Equation (43), imposes the collision avoidance constraint in Equation (14).

3.5. State Trust Region

To enhance the convergence properties of the successive convexification algorithm and to ensure the validity of linearizations around reference trajectories, we introduce a trust region constraint on the state deviation from a reference trajectory. Specifically, we enforce the following constraint at each time node

$$\parallel {\overline{\mathit{x}}}_j-{\widehat{\mathit{x}}}_j{\parallel }_2\le {\delta }_{x,j}$$

(46)

where ${\delta }_{x,j}$ is a positive scalar variable that is penalized in the cost function.

3.6. Cost Function and Final Problem Formulation

The cost function in Equation (15) depends on the control input $\mathit{u}\left(t\right)$, which is a stochastic variable. As a result, the cost is generally intractable in its original form. To address this, we adopt the deterministic convex sufficient condition introduced in Equation (31), which provides a conservative yet tractable approximation of the control norm’s p-quantile. While minimizing the norm of the mean control input ${\nu }_j$ would yield a convex and tractable objective, it fails to account for the variability introduced by feedback, resulting in suboptimal performance. Instead, we define the following convex cost function as

$$J=\sum _{j=1}^{N-1}\left({\nu }_j+\gamma \left(p\right){\tau }_j\right)$$

(47)

where the linearized expression in Equation (37) is used to express the cost function as a linear function of the decision variables ${\nu }_j$ and ${\tau }_j$.

The cost function in Equation (47) must be augmented with several penalization terms to ensure that the optimization problem is well-posed and that the converged solution satisfies the original requirements. Specifically, the following penalization terms are added, that is

$$\begin{array}{cc}\hfill J_H& \hfill ={\alpha }_H\sum _{j=1}^{N-1}\mathrm{Tr}\left(H_j\right)\hfill \end{array}$$

(48a)

$$\begin{array}{cc}\hfill J_{{\xi }_{\tau }}& \hfill ={\alpha }_{{\xi }_{\tau }}\sum _{j=1}^{N-1}\left|{\xi }_{\tau ,j}\right|\hfill \end{array}$$

(48b)

$$\begin{array}{cc}\hfill J_{{\xi }_s}& \hfill ={\alpha }_{{\xi }_s}\sum _{i=1}^n\sum _{j=1}^{N-1}\left|{\xi }_{s,ij}\right|\hfill \end{array}$$

(48c)

$$\begin{array}{cc}\hfill J_{{\delta }_{\tau }}& \hfill ={\alpha }_{{\delta }_{\tau }}\sum _{j=1}^{N-1}{\delta }_{\tau ,j}\hfill \end{array}$$

(48d)

$$\begin{array}{cc}\hfill J_{{\delta }_s}& \hfill ={\alpha }_{{\delta }_s}\sum _{i=1}^n\sum _{j=1}^{N-1}{\delta }_{s,ij}\hfill \end{array}$$

(48e)

$$\begin{array}{cc}\hfill J_{{\delta }_x}& \hfill ={\alpha }_{{\delta }_x}\sum _{j=1}^N{\delta }_{x,j}\hfill \end{array}$$

(48f)

where $J_H$ penalizes the trace of the covariance matrix $H_j$ to ensure that the relaxation of Equation (29) is lossless, as discussed in Section 3.2. $J_{{\xi }_{\tau }}$ and $J_{{\xi }_s}$ penalize the virtual buffer variables ${\xi }_{\tau ,j}$ and ${\xi }_{s,ij}$, respectively, while $J_{{\delta }_{\tau }}$ and $J_{{\delta }_s}$ penalize the trust region radii ${\delta }_{\tau ,j}$ and ${\delta }_{s,ij}$. $J_{{\delta }_x}$ penalizes the state trust region radii ${\delta }_{x,j}$ to promote convergence of the successive convexification algorithm. The weights ${\alpha }_H$, ${\alpha }_{{\xi }_{\tau }}$, ${\alpha }_{{\xi }_s}$, ${\alpha }_{{\delta }_{\tau }}$, ${\alpha }_{{\delta }_s}$, and ${\alpha }_{{\delta }_x}$ are positive scalars that balance the penalization terms in the cost function.

Finally, the complete convex optimization problem can be formulated as

$$\begin{array}{cc}\hfill \underset{\mathcal{X}}{min}& J+J_H+J_{{\xi }_{\tau }}+J_{{\xi }_s}+J_{{\delta }_{\tau }}+J_{{\delta }_s}+J_{{\delta }_x}\hfill \\ \hfill \mathrm{s}.\mathrm{t}.& {\overline{\mathit{x}}}_1={\mathit{x}}_0\hfill \\ & P_{x,1}=P_{x,0}\hfill \\ & {\overline{\mathit{x}}}_N={\mathit{x}}_f\hfill \\ & P_{x,N}⪯P_{x,f}\hfill \\ & {\overline{\mathit{x}}}_{j+1}=A_j{\overline{\mathit{x}}}_j+B_j{\overline{\mathit{u}}}_j+{\mathit{c}}_j\hfill & \mathrm{for}j=1,\dots ,N-1\hfill \\ & P_{x,j+1}=A_j{P}_{x,j}{A}_j^T+B_j{Y}_j{A}_j^T+A_j{Y}_j^T{B}_j^T+B_j{H}_j{B}_j^T+Q_j\hfill & \mathrm{for}j=1,\dots ,N-1\hfill \\ & \left[\begin{array}{cc}{H}_j& Y_j\\ Y_j^T& P_{x,j}\end{array}\right]⪰0\hfill & \mathrm{for}j=1,\dots ,N-1\hfill \\ & {\nu }_j+\gamma \left(p\right){\tau }_j\le u_{max}\hfill & \mathrm{for}j=1,\dots ,N-1\hfill \\ & \parallel {\overline{u}}_{xy,j}{\parallel }_2\le tan\left({\theta }_{max}\right){\overline{u}}_{z,j}\hfill & \mathrm{for}j=1,\dots ,N-1\hfill \\ & \parallel {\overline{\mathit{u}}}_j{\parallel }_2\le {\nu }_j\hfill & \mathrm{for}j=1,\dots ,N-1\hfill \\ & {\widehat{\tau }}_j+2{\widehat{\tau }}_j({\tau }_j-{\widehat{\tau }}_j)+{\xi }_{\tau ,j}={\rho }_j\hfill & \mathrm{for}j=1,\dots ,N-1\hfill \\ & |{\tau }_j-{\widehat{\tau }}_j|\le {\delta }_{\tau ,j}\hfill & \mathrm{for}j=1,\dots ,N-1\hfill \\ & {\rho }_j{I}_{n_u}⪰H_j\hfill & \mathrm{for}j=1,\dots ,N-1\hfill \\ & \parallel {\overline{\mathit{x}}}_j-{\widehat{\mathit{x}}}_j{\parallel }_2\le {\delta }_{x,j}\hfill & \mathrm{for}j=1,\dots ,N\hfill \\ & {\mathit{a}}_{i,j}^{\top }{\overline{\mathit{r}}}_j+z_p{\widehat{s}}_{i,j}\ge b_{i,j}\hfill & \mathrm{for}j=1,\dots ,N,i=1,\dots ,n\hfill \\ & {\widehat{s}}_{i,j}+2{\widehat{s}}_{i,j}(s_{i,j}-{\widehat{s}}_{i,j})+{\xi }_{s,ij}={\mathit{a}}_{i,j}^{\top }{P}_{r,j}{\mathit{a}}_{i,j}\hfill & \mathrm{for}j=1,\dots ,N,i=1,\dots ,n\hfill \\ & |s_{i,j}-{\widehat{s}}_{i,j}|\le {\delta }_{s,ij}\hfill & \mathrm{for}j=1,\dots ,N,i=1,\dots ,n\hfill \end{array}$$

(49)

where $\mathcal{X}=\{{\overline{\mathit{x}}}_j,P_{x,j},{\overline{\mathit{u}}}_j,K_j,Y_j,H_j,{\rho }_j,{\tau }_j,{\nu }_j,s_{i,j},{\xi }_{\tau ,j},{\xi }_{s,j},{\delta }_{\tau ,j},{\delta }_{s,ij},{\delta }_{x,j}\}$ is the set of decision variables.

It is important to note that the terminal covariance constraint $P_{x,N}⪯P_{x,f}$ is formulated as a linear matrix inequality (LMI), rather than an equality constraint as in Equation (16d). This relaxed formulation provides greater flexibility, which is particularly beneficial in practical scenarios where the exact terminal covariance may be uncertain or difficult to specify. In such cases, it is sufficient—and often preferable—to ensure that the final state distribution remains within the bounds of a prescribed target distribution.

The optimization problem in Equation (49) is formulated as a convex semidefinite program (SDP) that must be solved iteratively. At each iteration, the reference variables ${\widehat{\mathit{x}}}_j$, ${\widehat{\tau }}_j$, and ${\widehat{s}}_{i,j}$ are updated based on the most recent solution until convergence is achieved. The convergence criterion is defined in terms of the maximum relative change in the decision variables

$$\begin{array}{c}\hfill \underset{j}{max}\left(|{\widehat{\mathit{x}}}_j-{\mathit{x}}_j|\right)\le {ϵ}_x\end{array}$$

(50a)

$$\begin{array}{c}\hfill \underset{j}{max}\left(|{\widehat{\tau }}_j-{\tau }_j|\right)\le {ϵ}_{\tau }\end{array}$$

(50b)

$$\begin{array}{c}\hfill \underset{i,j}{max}\left(|{\widehat{s}}_{i,j}-s_{i,j}|\right)\le {ϵ}_s\end{array}$$

(50c)

where ${ϵ}_x$, ${ϵ}_{\tau }$, and ${ϵ}_s$ are small positive scalars that specify the convergence thresholds. In addition to this criterion, the final solution must also satisfy a constraint on the virtual buffer variables, ensuring they remain below a specified tolerance, hence

$$\begin{array}{c}\hfill \underset{i,j}{max}\left(|{\xi }_{\tau ,j}|,|{\xi }_{s,ij}|\right)\le {ϵ}_{\xi }\end{array}$$

(51)

where ${ϵ}_{\xi }$ is a user-defined threshold that guarantees the feasibility of the buffered constraints.

4. Numerical Results

This section presents two case studies in which the UAV navigates through a three-dimensional environment populated with static obstacles. In the first case, the environment comprises three ellipsoidal obstacles placed at a moderate distance from one another. The proposed guidance and control strategy is first evaluated in a 3-DOF dynamical framework, consistent with the assumptions of the optimal control formulation. To further assess its robustness and practical relevance, the same control law is then implemented in a fully nonlinear 6-DOF simulation environment. This higher-fidelity model explicitly accounts for the UAV’s attitude dynamics, aerodynamic coupling, and actuator constraints, thereby providing a preliminary external validation of the algorithm’s effectiveness under more realistic flight conditions.

The flexibility of the algorithm is further shown by considering a second study case, in which the UAV must navigate among cylindrical obstacles placed close to each other. These obstacles extend vertically across the entire computational domain, thereby allowing us to assess the UAV’s capability to perform obstacle avoidance relying exclusively on precise lateral maneuvers.

The main UAV properties and simulation parameters shared by the two cases are presented in

Table 1. UAV and problem parameters.

Parameter	Description	Value
m	UAV mass	1 kg
$k_{\mathrm{drag}}$	drag parameter	0.5 kg/m
g	gravitational acceleration	9.8 m/s2
$u_{max}$	maximum thrust	10 N
${\theta }_{max}$	maximum tilt angle	45°
N	number of discretization nodes	101
$g_v$	disturbance intensity scale	2 × 10−2 m/s3/2
${\mathit{r}}_{\mathrm{init}}$	initial position	[1, 1, 2] m
${\mathit{r}}_{\mathrm{fin}}$	final position	[6, 6, 2.2] m
${\mathit{v}}_{\mathrm{init}}$	initial velocity	[0, 0, 0] m/s
${\mathit{v}}_{\mathrm{fin}}$	final velocity	[0, 0, 0] m/s
p	probability level	0.95
${\sigma }_p^2$	initial/final position covariance	10−4 m2
${\sigma }_v^2$	initial/final velocity covariance	10−4 m2 s−2

. The initial and final covariance matrices are set as

$$\begin{array}{cc}\hfill P_{x,0}& \hfill =P_{x,N}=\mathrm{diag}\left({\sigma }_p^2{\mathbf{1}}_3,{\sigma }_p^2{\mathbf{1}}_3\right)\hfill \end{array}$$

(52)

where ${\mathbf{1}}_3$ is the all-ones vector of dimension three. To improve the numerical stability of the optimization problem, the covariance matrices are scaled by a diagonal matrix D such that the diagonal terms of the scaled covariance matrix ${\tilde{P}}_{x,j}=D{P}_{x,j}{D}^T$ are as close as possible to unity, following the approach detailed in [31]. Specifically, the scaling matrix was selected as $D={10}^2{I}_6$. This scaling ensures that the mean state and control variables are on a similar scale to the covariance variables, thereby reducing the risk of numerical ill-conditioning or dominance by variables with large absolute magnitudes. Importantly, because the covariance dynamics are decoupled from the mean dynamics in this formulation, the scaling does not affect the mean dynamics and constraints.

The sequential convex programming (SCP) algorithm was implemented in MATLAB R2023a using the CVX toolbox for disciplined convex programming [53,54], with the SDPT3 solver for semidefinite programs [55]. All numerical experiments were conducted on a laptop equipped with an AMD Ryzen 7 6800HS processor (3.2 GHz). The optimization weights were set to ${\alpha }_H={10}^2$, ${\alpha }_{{\xi }_{\tau }}={10}^4$, ${\alpha }_{{\xi }_s}={10}^1$, ${\alpha }_{{\delta }_{\tau }}=1$, ${\alpha }_{{\delta }_s}={10}^1$, and ${\alpha }_{{\delta }_x}={10}^2$, while the convergence thresholds were chosen as ${ϵ}_x={10}^{-5}$, ${ϵ}_{\tau }={10}^{-3}$, ${ϵ}_s={10}^{-5}$, and ${ϵ}_{\xi }={10}^{-5}$. The initial guess for the state trajectory was obtained via a straight-line interpolation between the initial and final states. The initial reference values for the auxiliary variable ${\widehat{\tau }}_j$ were uniformly set to 0.1. The initial values for the auxiliary variable ${\widehat{s}}_{i,j}$ were computed by evaluating Equation (42) along the initial guess trajectory, assuming a covariance matrix $P_{r,j}$ equal to the initial covariance matrix for all time steps and obstacles.

Note that, in the present study the disturbance model employed in the optimization and in the Monte Carlo simulations was kept consistent (Gaussian) to ensure that the predicted mean and covariance evolution matched the simulated trajectories. Using different disturbance models in simulation (e.g., pulsed wind shear or sensor faults) would result in deviations from the predicted statistics, and will be investigated in future work.

4.1. Case Study 1: Ellipsoidal Obstacles

In the first case study, the UAV navigates around three elliptical obstacles, whose positions and shapes of the obstacles are specified by their centers ${\mathit{o}}_i$ and shape matrices ${\Omega }_i^{-1}$ as

$$\begin{array}{cccc}\hfill {\mathit{o}}_1& \hfill =(2,2,2.5)\mathrm{m}\hfill & \hfill & \hfill {\Omega }_1=\mathrm{diag}(0.5^2,0.5^2,3^2){\mathrm{m}}^2\hfill \end{array}$$

(53)

$$\begin{array}{cccc}\hfill {\mathit{o}}_2& \hfill =(3,3,1.8)\mathrm{m}\hfill & \hfill & \hfill {\Omega }_2=\mathrm{diag}(0.5^2,0.5^2,3^2){\mathrm{m}}^2\hfill \end{array}$$

(54)

$$\begin{array}{cccc}\hfill {\mathit{o}}_3& \hfill =(4,4,2.5)\mathrm{m}\hfill & \hfill & \hfill {\Omega }_3=\mathrm{diag}(0.5^2,0.5^2,3^2){\mathrm{m}}^2\hfill \end{array}$$

(55)

The flight time for reaching the target position is fixed and equal to $t_f=10s$. The number of control points was set to $N=101$, which corresponds to a 10 Hz update rate for the considered flight time. This update frequency is representative of practical UAV guidance loops.

Figure 2 presents the (mean) optimal trajectory (blue line) and the associated feedforward control vectors (red arrows) computed via the SCP algorithm, together with 1000 Monte Carlo realizations (gray lines). The realizations are generated by numerically integrating the stochastic dynamics in Equation (4) using the Euler–Maruyama method with the optimal feedback gains obtained from the SCP algorithm. Finally, the ellipsoidal obstacles are instead represented in green.

A three-dimensional view of the trajectory is presented in Figure 2a, while the projections in the XY and XZ planes are presented in Figure 2c and Figure 2d, respectively. As apparent in Figure 2a, the initial trajectory violates the collision avoidance constraints. Nevertheless, the SCP algorithm successfully converged to a feasible solution that satisfies all imposed constraints within a few iterations, with a total computation time of approximately 3 s for the entire trajectory optimization problem. Figure 2b provides a zoomed view of the trajectory, showing that the dispersion of the Monte Carlo trajectories is consistent with the predicted state uncertainty, which is represented by the 95% confidence ellipsoids (black contours) computed from the state covariance matrices $P_{x,j}$ at each time step.

In the same fashion, Figure 3 reports the x, y, and z velocity components over time together with their 95% confidence intervals, computed from the diagonal entries of the velocity covariance matrix $P_x$. The sampled trajectories lie well within the predicted confidence bounds, confirming that the covariance is accurately captured and that the optimal control strategy effectively accounts for uncertainty during planning.

Figure 4 shows the distance to the closest obstacle at each time step when the collision avoidance constraint is enforced as a deterministic-mean constraint (Figure 4a) or as a chance-constraint (Figure 4b). When enforcing the collision avoidance constraint deterministically on the nominal (mean) trajectory only, several Monte Carlo realizations violate the safety threshold, even though the mean trajectory remains safe. Conversely, the chance-constrained formulation guarantees that the distance to the closest obstacle ${\mathit{o}}_i$ stays strictly positive, thereby providing probabilistic safety assurances throughout the flight. This is illustrated by the p-quantile of the obstacle distance across the Monte Carlo realizations (magenta line), which remains below the threshold.

The tilt angle $\theta$, defined as the angle between the thrust vector and the vertical axis, over time is shown in Figure 5. Both the mean solution computed via the SCP algorithm (black line) and the Monte Carlo realizations (gray lines) are presented. The MC samples are so close to the nominal trajectory that they are visually indistinguishable. This supports the modeling choice of imposing the tilt constraint only on the mean trajectory, rather than introducing it as a chance constraint.

Figure 6 shows the thrust control norm over time, normalized by $u_{max}$. The plot presents the nominal control input (i.e., the mean control input $\overline{\mathit{u}}\left(t\right)$ as a black line, while the gray lines show the norms corresponding to the 1000 Monte Carlo realizations under disturbances. The red curve depicts the p-quantile of the control norm, obtained from the sufficient condition in Equation (31). It is apparent that the MC realizations remain below the red quantile curve for the entire duration of the trajectory, confirming the (slightly) conservative nature of the sufficient condition. It is also worth noting that the thrust norm resulting from the optimization consistently lies in the range of approximately 85% to 100% of $u_{\max }$, indicating a high utilization of the available control authority. However, $u_{\max }$ in this formulation does not correspond to the physical saturation limit of the UAV’s actuators. In practical applications, $u_{\max }$ should be selected as a fraction of the actual maximum thrust capability, (e.g., around 50–75%) to preserve sufficient control margin for disturbance rejection, emergency maneuvers, and to avoid operating the motors near their efficiency limits. Therefore, the high thrust levels observed in the figure reflect the optimizer working near the imposed constraint, not the true physical limits of the system.

Furthermore, note that the thrust signal exhibits sharp variations, which result from minimizing the control norm in the cost function. To enable a smoother real-time implementation, an alternative, yet convex, formulation based on the minimization of the squared control norm may be considered in future works.

Figure 7 illustrates the terminal state distribution in terms of pairwise projections of the final position (Figure 7a) and velocity (Figure 7b) components. The red ellipsoids represent the target distribution, defined by its mean and 95%-confidence region. The dashed black ellipsoids correspond to the solution of the optimization problem, and they are fully contained within the red ellipsoids, indicating that the optimal solution satisfies the terminal covariance constraint in Equation (16d). The Monte Carlo end states are shown as blue dots, with their empirical mean and 95%-confidence ellipsoid shown as a black point and blue ellipse, respectively. The close alignment among the target distribution, the optimized terminal state, and the Monte Carlo results confirms that the proposed method reliably achieves the desired terminal distribution both in expectation and under stochastic realization. This demonstrates the effectiveness of the proposed framework in regulating both the mean and uncertainty of the final state.

A quantitative comparison between the optimal closed-loop (CL) solution and the results obtained from the Monte Carlo (MC) simulations is presented in

Table 2. Case 1. Comparison between the optimal 3-DoF solution and the Monte Carlo 3-DoF simulations in terms of cost function and final state distribution.

Quantity	CL Solution	CL MC	Units
J	101.59	100.4578	Ns
$\Delta \overline{x}\left(t_f\right)$	-	−4.938 × 10−4	$\mathrm{m}$
$\Delta \overline{y}\left(t_f\right)$	-	−1.50 × 10−3	$\mathrm{m}$
$\Delta \overline{z}\left(t_f\right)$	-	4.169 × 10−4	$\mathrm{m}$
$\Delta {\overline{v}}_x\left(t_f\right)$	-	−1.60 × 10−3	$\mathrm{m}/\mathrm{s}$
$\Delta {\overline{v}}_y\left(t_f\right)$	-	−2.737 × 10−4	$\mathrm{m}/\mathrm{s}$
$\Delta {\overline{v}}_z\left(t_f\right)$	-	−4.688 × 10−4	$\mathrm{m}/\mathrm{s}$
${\sigma }_x^2\left(t_f\right)$	0.880 × 10−4	0.888 × 10−4	${\mathrm{m}}^2$
${\sigma }_y^2\left(t_f\right)$	0.899 × 10−4	0.869 × 10−4	${\mathrm{m}}^2$
${\sigma }_z^2\left(t_f\right)$	0.985 × 10−4	1.040 × 10−4	${\mathrm{m}}^2$
${\sigma }_{v_x}^2\left(t_f\right)$	0.6716 × 10−4	0.694 × 10−4	${\mathrm{m}}^2 {\mathrm{s}}^{-2}$
${\sigma }_{v_y}^2\left(t_f\right)$	0.676 × 10−4	0.854 × 10−4	${\mathrm{m}}^2 {\mathrm{s}}^{-2}$
${\sigma }_{v_z}^2\left(t_f\right)$	0.826 × 10−4	0.854 × 10−4	${\mathrm{m}}^2 {\mathrm{s}}^{-2}$

, which reports the cost function, the final state mean deviations, and terminal state covariances. For the optimal solution, the cost function J is computed as the sum over time of the left-hand side of the sufficient condition in Equation (31), while for the MC simulations it is evaluated as the integral of the p-quantiles of the closed-loop control norm. The cost function J associated with the optimal solution is computed as the sum over time of the left-hand side of the sufficient condition in Equation (31). For the Monte Carlo simulations, the cost is computed as the integral of the p-quantiles of the closed-loop control norm over time. The final state mean deviations, denoted $\Delta \overline{x}\left(t_f\right)$, $\Delta \overline{y}\left(t_f\right)$, etc., are zero by construction in the optimal solution, as the terminal mean state is constrained to match the target. In contrast, the mean of the Monte Carlo results exhibits small deviations from the desired final state, on the order of 10⁻³ m and 10⁻³ m/s, which are within the solver’s numerical precision and consistent with the specified convergence tolerances. The table also reports the diagonal elements of the terminal state covariance matrix $P_{x,N}$. The values from the optimal solution closely match the prescribed target distribution covariances, confirming that the terminal covariance constraint in Equation (16d) is met. Similarly, the Monte Carlo simulations yield comparable covariance values, further validating the robustness and effectiveness of the proposed control framework in achieving the desired terminal state distribution under uncertainty.

Figure 8 illustrates the errors introduced by the relaxation and linearization operations applied to reformulate the original non-convex constraints into convex approximations, as discussed in Section 3. These approximations are critical to enabling tractable optimization while preserving the problem’s underlying structure. Figure 8a displays the relaxation error on the mean control norm, defined as the difference between the two sides of Equation (36), normalized by $u_{max}$. The small magnitude of the error across time confirms that the relaxation is lossless and that the auxiliary variable ${\nu }_j$ accurately captures the true mean control norm. This is consistent with the design of the cost function, which promotes tightness by minimizing the auxiliary variable ${\nu }_j$. Figure 8b shows the relaxation error associated with the control covariance, comparing the matrix $H_j$ with the term $Y_j{P}_{x,j}^{-1}{Y}_j^T$, which constitute the two sides of the inequality in Equation (29). The near-zero error validates the effectiveness of this relaxation. This outcome aligns with the theoretical guarantee of tightness provided by augmenting the cost function with the trace regularization term $J_H$, as proposed in [52]. Finally, Figure 8c reports the linearization error related to the presence of the virtual buffer variable ${\xi }_{\tau ,j}$ in Equation (33), required for the numerical stability of the computation of the square root (${\tau }_j$) of the spectral radius of the control matrix (${\rho }_j$). The low error magnitude confirms that the virtual buffer vanishes, hence the computation of the spectral radius is effective.

Validation in a 6-DOF Environment

To further validate the robustness and practical viability of the proposed methodology, the synthesized guidance policy was implemented within a high-fidelity 6-DOF UAV simulation model that fully captures the vehicle’s nonlinear rotational dynamics. This provides a more rigorous test of the algorithm’s performance.

Specifically, at each time step $t_j$, the 3-DOF thrust command ${\mathit{u}}_{\mathrm{cmd},j}$ computed via Equation (10) is interpreted as a reference for a low-level geometric attitude controller [56]. The command is decomposed into a desired thrust magnitude, $T_d=∥{\mathit{u}}_{\mathrm{cmd},j}∥$, and a desired attitude orientation, $R_d\in SO\left(3\right)$. Assuming a fixed-yaw policy (${\psi }_d=0$), the desired attitude matrix $R_d=\left[\begin{array}{ccc}{\mathit{b}}_{1d}& {\mathit{b}}_{2d}& {\mathit{b}}_{3d}\end{array}\right]$ is constructed by aligning the body’s thrust axis with the normalized command vector, ${\mathit{b}}_{3d}={\mathit{u}}_{\mathrm{cmd},j}/∥{\mathit{u}}_{\mathrm{cmd},j}∥$. The remaining axes are subsequently computed to form a right-handed orthonormal frame:

$$\begin{array}{c}{\mathit{b}}_{2d}={\displaystyle \frac{{\mathit{b}}_{3d}\times {[1,0,0]}^T}{∥{\mathit{b}}_{3d}\times {[1,0,0]}^T∥}}\hfill \\ {\mathit{b}}_{1d}={\mathit{b}}_{2d}\times {\mathit{b}}_{3d}\hfill \end{array}$$

(56)

The low-level controller generates the necessary body-fixed torques to asymptotically track the desired attitude orientation $R_d$. These torques, along with the total thrust, are converted into individual rotor commands. Further details on the 6-DOF model and control architecture are provided in Appendix A.

A Monte Carlo analysis of 2000 simulations was performed to rigorously evaluate the performance of the proposed path planning methodology in the high-fidelity 6-DOF environment. For these simulations, the UAV inertia matrix was set to $J=\mathrm{diag}(0.003,0.003,0.006)$$\mathrm{k}\mathrm{g} {\mathrm{m}}^2$, the vehicle’s arm length to $L = 0.18 \mathrm{m}$, and the thrust-to-torque coefficient to $\gamma =0.012 \mathrm{N} \mathrm{m}$. To quantify the benefit of the feedback component, we compare (i) the Open-Loop (OL) case, where the UAV executes the pre-computed nominal trajectory and control sequence without feedback, and (ii) the closed-loop (CL) case, where the UAV utilizes the full feedback guidance law from Equation (10), actively correcting for any deviations from the nominal state in real-time.

Table 3. Case 1. Comparison between the open-loop and closed-loop Monte Carlo 6-DOF simulations, in terms of cost function and final state distribution.

Quantity	6-DOF OL	6-DOF CL	Units
J	100.44	101.75	N$\mathrm{s}$
$\Delta \overline{x}\left(t_f\right)$	−0.700 × 10−1	−7.232 × 10−4	$\mathrm{m}$
$\Delta \overline{y}\left(t_f\right)$	−0.728 × 10−1	−4.10 × 10−4	$\mathrm{m}$
$\Delta \overline{z}\left(t_f\right)$	0.972 × 10−1	5.50 × 10−3	$\mathrm{m}$
$\Delta {\overline{v}}_x\left(t_f\right)$	−0.114 × 10−1	−0.110 × 10−1	$\mathrm{m}/\mathrm{s}$
$\Delta {\overline{v}}_y\left(t_f\right)$	−0.114 × 10−1	−0.76 × 10−2	$\mathrm{m}/\mathrm{s}$
$\Delta {\overline{v}}_z\left(t_f\right)$	0.39 × 10−2	−0.86 × 10−2	$\mathrm{m}/\mathrm{s}$
${\sigma }_x^2\left(t_f\right)$	0.27 × 10−2	0.917 × 10−4	${\mathrm{m}}^2$
${\sigma }_y^2\left(t_f\right)$	0.26 × 10−2	0.9163 × 10−4	${\mathrm{m}}^2$
${\sigma }_z^2\left(t_f\right)$	0.39 × 10−2	1.023 × 10−4	${\mathrm{m}}^2$
${\sigma }_{v_x}^2\left(t_f\right)$	0.107 × 10−3	0.651 × 10−4	${\mathrm{m}}^2 {\mathrm{s}}^{-2}$
${\sigma }_{v_y}^2\left(t_f\right)$	0.117 × 10−3	0.628 × 10−4	${\mathrm{m}}^2 {\mathrm{s}}^{-2}$
${\sigma }_{v_z}^2\left(t_f\right)$	0.134 × 10−3	0.799 × 10−4	${\mathrm{m}}^2 {\mathrm{s}}^{-2}$

presents the p-quantile of the control effort, the error in the mean final state, and the final state variances for either case. The results demonstrate the advantages of the closed-loop policy. The feedback law reduces the error in the mean terminal position by two orders of magnitude, effectively eliminating the steady-state error induced by accumulated process noise. Furthermore, the closed-loop controller actively suppresses uncertainty, reducing the terminal position variances by a factor of about 30. This significant improvement in accuracy and precision is achieved with only a 1.3% increase in the mean control effort, confirming the efficiency of the synthesized guidance law.

A comparison of the 6-DOF results with the ideal 3-DOF Monte Carlo analysis presented in Table 2 reveals the practical validity of the proposed guidance law. Only a minor increase in terminal state variance is observed. In particular, the terminal variance in the x-position, ${\sigma }_x^2\left(t_f\right)$, increased from 0.888 × 10⁻⁴ ${\mathrm{m}}^2$ in the 3-DOF case to 0.917 × 10⁻⁴ ${\mathrm{m}}^2$ in the 6-DOF simulation. This modest degradation is an expected and acceptable consequence of the unmodeled nonlinear attitude dynamics and the finite response time of the low-level controller. This is achieved with only a modest increase in control effort (approximately 1%), thereby confirming that the 3-DOF covariance-control formulation here presented can be applied effectively to the synthesis of a guidance law for UAVs operating in realistic environments that include simple, static obstacles.

Figure 9 shows the position residuals, defined as the deviation of each 6-DOF Monte Carlo trajectory from the nominal 3-DOF path. The feedback controller constrains these deviations effectively, with the mean error remaining close to zero throughout the maneuver. This demonstrates that, on average, the simulated trajectories track the planned path with high accuracy.

Figure 10, instead, presents the simulated and desired attitude angles in the roll ($\varphi$, ${\varphi }_d$) and pitch ($\theta$, ${\theta }_d$) channels, respectively. The tracking errors remain generally small, with only occasional peaks of up to 1 or 2 degrees, indicating that the acceleration commands generated by the 3-DOF planner are feasible and well within the performance envelope of the vehicle’s attitude-control system.

4.2. Case Study 2: Cylindrical Obstacles

As a second case, we consider a scenario where the UAV moves in an environment with three vertical cylindrical obstacles. The height of the obstacles is large enough (e.g, a few meters) to discourage the UAV to attempt fly over them. The radii of the three cylinders are $r_1=0.7 \mathrm{m}$, $r_2=0.8 \mathrm{m}$, and $r_3=0.5 \mathrm{m}$. The obstacles’ shape and position are thus given as

$$\begin{array}{cccc}\hfill {\mathit{o}}_1& \hfill =(3.5,3,2)\mathrm{m}\hfill & \hfill & \hfill {\Omega }_1=\mathrm{diag}(r_1^2,r_1^2,0)\hfill \end{array}$$

(57)

$$\begin{array}{cccc}\hfill {\mathit{o}}_2& \hfill =(2,2.5,2)\mathrm{m}\hfill & \hfill & \hfill {\Omega }_2=\mathrm{diag}(r_2^2,r_2^2,0)\hfill \end{array}$$

(58)

$$\begin{array}{cccc}\hfill {\mathit{o}}_3& \hfill =(4.5,5,2)\mathrm{m}\hfill & \hfill & \hfill {\Omega }_3=\mathrm{diag}(r_3^2,r_3^2,0)\hfill \end{array}$$

(59)

The first two obstacles are positioned closer to each other, and the flight time is reduced to $t_f=5s$, creating a more challenging scenario.

Figure 11 shows the optimal trajectory (blue line) and 1000 Monte Carlo simulations (gray lines) in the presence of cylindrical obstacles. The 95%-confidence ellipsoids, computed from the state covariance matrices $P_{x,j}$, are shown as black contours at each time step (they are clearly distinguishable only in Figure 11b). As in the previous case, the initial guess is a straight-line interpolation between the initial and final states, which violates the collision avoidance constraints. Nevertheless, the solver successfully finds a feasible solution in just six iterations, with a total computation time of approximately 3 s.

In Figure 12, the x, y, and z components of the velocity along the optimal trajectory are shown with their corresponding 95%-confidence intervals. Compared to the previous case with ellipsoidal obstacles, the velocity components here exhibit less frequent oscillations, indicating a smoother trajectory, characterized by three main maneuvers to avoid the cylindrical obstacles. It is worth noting that the mean of the z component remains nearly zero throughout the trajectory, but, due to the initial dispersion and the omnidirectional perturbations, the z velocity exhibits non-null variance in the Monte Carlo simulations, as reflected in the confidence intervals. Nevertheless, the sampled trajectories remain well within the predicted confidence bounds and meet the final variance requirements.

Figure 13 shows the thrust norm over time. The thrust level is high, ranging from approximately 90% to 100% of $u_{\max }$, yet it never reaches the maximum. The red curve, representing the p-quantile of the control norm, remains above all Monte Carlo realizations, but the margin between the red curve and the Monte Carlo simulations is small, indicating that the sufficient condition in Equation (31) is again marginally conservative.

Figure 14 shows the distance to the nearest obstacle over time. Compared to the previous case with ellipsoidal obstacles, the UAV flies for longer periods close to the obstacles, indicating a more aggressive and efficient use of the available space. Nevertheless, as confirmed by the Monte Carlo simulations, the distance to the nearest obstacle remains above zero at all times, demonstrating the effectiveness and robustness of the chance-constrained formulation in ensuring collision avoidance under uncertainty.

Figure 15 shows the terminal state distribution in terms of pairwise projections of the final position (Figure 15a) and velocity (Figure 15b) components. The final position distribution closely matches the desired target distribution, while the final velocity confidence ellipsoid are slightly smaller than the target, indicating that smaller than required velocity variances were necessary to achieve the desired final position variance.

Further results comprising the validation within the 6-DOF UAV simulation model are here omitted for the sake of conciseness, as they closely match those discussed in Section 4.1.

5. Conclusions

This paper introduced a novel approach for chance-constrained path planning of UAVs operating under uncertainty. By integrating covariance control with chance-constrained optimization, the proposed method enables the synthesis of closed-loop guidance policies that steer both the mean and covariance of the UAV state toward a target distribution, while ensuring probabilistic collision avoidance with ellipsoidal obstacles. The underlying convexification framework transforms the non-convex covariance control problem into a tractable optimization problem that can be solved with little computational effort.

Numerical results confirm the effectiveness of the proposed approach across several performance metrics. The sequential optimization reliably converged to a feasible solution in minimal computational time, even when initialized with a coarse, infeasible guess. The near-zero relaxation and linearization errors at convergence further validate the proposed convexification strategy.

Importantly, Monte Carlo simulations in both three-degree-of-freedom (3-DOF) and six-degree-of-freedom (6-DoF) nonlinear UAV models further validated the framework in scenarios that are more representative of real system dynamics. These experiments demonstrated the robustness of the generated trajectories and control policies, showing that the chance constraints were consistently satisfied at the prescribed probability levels and that the final state distributions closely matched the desired targets. A comparison with the deterministic formulation underscored the superior safety guarantees offered by the probabilistic one, supporting its applicability to real-world UAV scenarios.

Given the preliminary nature of the present results, further external validation is required to assess the applicability of the proposed approach under more realistic operating conditions. A more comprehensive validation should be carried out in higher-fidelity simulation models that better capture real-world dynamics, including representative sensor and actuator behavior. In this context, the assumption of perfect state feedback should be replaced with feedback on the estimated state provided by an on-board filter (e.g., an Extended Kalman Filter). High-fidelity hardware-in-the-loop tests also need to be carried out before actual in-flight experimentation, taking into account the real onboard computing constraints of the flight hardware.

In addition, further extensions to the framework should enable the handling of dynamic obstacles with generic, non-ellipsoidal geometries. The use of signed distance functions appears particularly promising in this regard; however, their integration within convex-based covariance control remains an open challenge for future research.