Rapid Uncertainty Propagation and Chance-Constrained Path Planning for Small Unmanned Aerial Vehicles
[Summary]With the number of small Unmanned Aircraft Systems (sUAS) in the national airspace projected to increase in the next few years, there is growing interest in a traffic management system capable of handling the demands of this aviation sector. It is expected that such a system will involve trajectory prediction, uncertainty propagation, and path planning algorithms. In this work, we use linear covariance propagation in combination with a quadratic programming-based collision detection algorithm to rapidly validate declared flight plans. Additionally, these algorithms are combined with a Dynamic, Informed RRT* algorithm, resulting in a computationally efficient algorithm for chance-constrained path planning. Detailed numerical examples for both fixed-wing and quadrotor sUAS models are presented.
1]Andrew W. Berning Jr.*
4]Sarah N. D’Souza
Berning et al
There were 110,604 registered small Unmanned Aircraft Systems (sUAS) in the United States at the end of 2017, and that number is expected to quadruple by 2022 schaufele2018faa. There has been great interest, accordingly, in a UAS Traffic Management (UTM) system to handle the demands of this rapidly growing aviation sector kopardekar2014unmanned, kopardekar2016unmanned, dill2016safeguard. In at least one potential design of such a system, real-time communication channels exist between a central computation platform and the sUAS. This would provide the vehicles access to valuable computation resources and information about nearby vehicles, terrain, and atmospheric conditions, allowing for safe and efficient route planning.
One of the responsibilities of such a UTM system will likely be to apply a risk-based approach where geographical needs and use cases determine the airspace performance requirements dsou:17cp. Addressing this will require a sUAS trajectory prediction model that validates vehicle flight performance and allows for UTM to determine whether or not the vehicle can operate in the airspace given real-time information about wind, other vehicles, and/or obstacles. Given a large number of vehicles predicted to be in operation, this trajectory prediction model must accommodate multiple vehicle types and airspace environments (wind, terrain, etc.). Another challenge in this setting is that the trajectory could depend on proprietary information such as control systems, methods and gain tuning specific to a particular vehicle. Implementing a system that is reliant on such information could be prohibitive due to constant modifications that would be required to accommodate diverse and evolving control laws and due to the potential legal barriers to acquiring proprietary information from all operators.
In the context of UTM operations, where varied vehicle types and uncertainties are expected, there is a need for a model that represents expected vehicle performance while accounting for the uncertainty in the context of operational environments. The motivation for the work detailed in this paper is to develop computationally efficient trajectory validation and planning algorithms that utilize uncertainty quantification (UQ) and propagation techniques to provide an assessment of the vehicle performance and risk of violating constraints.
1.2 Problem Statement
The work described in this paper focuses on two separate but related problems: rapid uncertainty propagation for trajectory validation and chance-constrained path planning.
For the former, we seek an algorithm that takes the vehicle’s dynamics, initial state, parameters, and desired trajectory as inputs, and outputs some quantification of the uncertainty associated with the vehicle’s state trajectory over the specified flight horizon, as well as an assessment of whether the probability of trajectories violating constraints is sufficiently low at any given time instant. For the latter, we are interested in the ability to re-plan or repair the trajectory if it is found that the vehicle’s probabilistic trajectory tube intersects with a keep-out zone. Such a tube and constraint are illustrated in Figure 1.
1.3 Literature Review
The source of uncertainty considered in this work is atmospheric turbulence and stochastic wind gusts, which affect the response richardson2013envelopes and safety belcastro2010aircraft, FAA2004accidents of aircraft. These effects are particularly pronounced in relatively lightweight sUAS. Hoblit hoblit1988gust provides a detailed description of discrete and continuous gust models and Richardson richardson2013quantifying provides a thorough review of the modeling techniques involved. The Dryden and Von Kármán models are the two most common gust/turbulence models liepmann1952application, de1938statistical, von1948progress, von1951statistical, diederich1957effect, which are used in both Federal Aviation Administration (FAA) and military specifications. Both consider the linear and angular velocity components of the gusts to be varying stochastic processes and then specify those components’ Power Spectral Density (PSD). The Dryden model utilizes rational PSDs, while the Von Kármán model uses irrational PSDs. The latter matches experimental gust observations more closely than the former, but its use of irrational spectral densities prevents its spectral factorization from being exactly expressed.
We also consider the problem of propagating the vehicle’s state uncertainty subject to the aforementioned wind gust disturbances. Given an initial probability distribution of a state, the objective of a UQ algorithm is to obtain a characterization of the state’s probability distribution at a future instant in time. For this work, we require the algorithm to compute the uncertainty for all time instants between the initial and final times. The conceptually simple solution is through a Monte Carlo (MC) simulation, but the computational intensity of this method limits its usefulness for our application junkins1996non, sabol2013nonlinear. Linearization techniques suffer from diminished accuracy for highly nonlinear systems or for long time horizons, but their simplicity and high computational efficiency make them well-suited for real-time trajectory optimization or path planning geller2006linear, geller2009event. Other nonlinear UQ methods include unscented transformation (UT) julier1995new, julier2000new, polynomial chaos (PC) expansions wiener1938homogeneous, and Gaussian mixture models (GMM) terejanu2008uncertainty, demars2013entropy, vittaldev2016space. A thorough survey of many other UQ methods is provided by Luo luo2017review, though none of these were deemed appropriate for our use due to our models’ large number of states, the presence of stochastic inputs, and the need for rapid computations. For that reason, the linear covariance propagation method is an attractive choice for our problem. The ease of incorporating exogenous disturbances and computing a complete time history of the covariance further distinguish it from the other UQ methods.
There are a wide array of solution strategies for planning a vehicle’s path subject to uncertainty, including convex programming blackmore2011chance, mixed integer linear programming da2019collision, graph search bry2011rapidly, fast marching trees janson2018monte, and probabilistic roadmaps ding2013strategic. One of the more popular approaches luders2010chance, pairet2018uncertainty, axelrod2018provably, kothari2013probabilistically, borowski2012evaluation utilizes a class of stochastic search algorithms called Rapidly-exploring Random Trees (RRTs) lavalle1998rapidly. These algorithms are well suited for real-time implementation pairet2018uncertainty and are sampling-based, so they scale well with problem size, but only offer a probabilistic guarantee of completeness. A comparison of sequential quadratic programming, genetic algorithms, and RRT is provided in borowski2012evaluation. Extensions to RRT such as RRT*karaman2011sampling, Informed RRT*gammell2014informed, and Dynamic RRT*adiyatov2017novel, improve optimality of the solution, increase sampling efficiency, and allow for dynamically changing constraints, respectively. These three RRT extensions are utilized in this work.
1.4 Original Contributions
The main contribution of this paper is the amalgamation of existing Dynamic RRT* and Informed RRT* algorithms, and the addition of an obstacle buffer resizing technique, resulting in a single algorithm that allows computationally efficient, chance-constrained path planning for sUAS. In particular, this contribution addresses a dilemma in chance-constrained path planning under uncertainty: trajectory re-planning changes the outcome of the covariance propagation, which, when obstacles or exclusion zones are involved, may require further re-planning.
This paper builds upon previous work berning2018Rapid that considered rapid uncertainty propagation, collision detection, and trajectory optimization for fixed-wing, 2D longitudinal flight dynamics. The numerical examples presented here utilize both fixed-wing and quadrotor sUAS in full 3D flight in a non-static atmosphere with inner loop and outer loop controllers in closed form. Other contributions in support of the path planning algorithm are linear covariance propagation of the uncertainty in initial states and exogenous disturbances for three dimensional vehicle motion, and a quadratic programming-based approach to 3D collision detection.
The purpose of this paper is to present and illustrate with simulations a procedure for rapid uncertainty propagation and chance constrained path planning that is broadly applicable in the sUAS setting in terms of models and uncertainties assumed. This section introduces the two systems used for numerical examples in this work: a quadrotor sUAS model and a fixed-wing sUAS model.
The quadrotor sUAS is modeled as a double integrator with quadratic aerodynamic drag, constant drag coefficient, dynamic extension controller, and a Dryden-based wind disturbance. The full model can be found in Appendix A. The fixed-wing sUAS model is based on a 6-state model for longitudinal and lateral flight in a moving atmosphere, with inner and outer-loop controllers added and a Dryden wind gust model. The full model can be found in Appendix B.
where the state is for the quadrotor model or for the fixed-wing model, is the set of vehicle, environmental, and control parameters, and is the white noise input.
3 Technical Approach
3.1 Covariance Propagation
For the uncertainty propagation analysis, we linearize the sUAS model in Eq. (1). The linearized model has the following form:
Here, is the nominal trajectory obtained by propagating Eq. (1) subject to and specified time history of . Physically, this corresponds to the nominal trajectory that the sUAS would fly under no-wind conditions.
Under the assumption that is a standard white noise process, the state covariance matrix, , satisfies the Lyapunov differential equation kabamba2014fundamentals:
Note that (5) can be solved using any standard ODE solver. The initial condition for is the zero matrix if the vehicle’s initial state is known deterministically, or may be chosen as a diagonal matrix of state covariances, otherwise.
Now, we can use the nominal trajectory , covariance matrix , and a Gaussian distribution density function to estimate the probability that the vehicle is contained in a specified set at time . For the quadrotor model: given the mean and the block of the covariance matrix corresponding to these states, , we can build a set from the definition of a probability ellipsoid malyshev1992optimization:
where is a prescribed probability level and is solved for using the three degree-of-freedom chi-squared distributionlancaster1969chi. Thus the vehicle has a probability of being within the set (6) at time .
For verification purposes, Eq. (1) is also solved numerically in a series of MC simulations. For these computations, the white Gaussian noise is computed using MATLAB’s function, scaled by the inverse square root of the integration time step, as detailed in hanson2007applied. A comparison between the linear covariance propagation and the covariance matrix estimated from the MC runs is shown in Section 4.
3.2 Collision Detection
For trajectory validation purposes, it is desirable to have a collision detection algorithm that can quickly determine whether or not the trajectory tube formed by the set of probability ellipsoids defined in Eqn. (6) intersects with an obstacle, represented in this work as a cuboid, as illustrated in Figure 2.
The ellipsoid-cuboid intersection at time instant is formulated as a quadratic programming (QP) problem that can be solved by a standard QP solver:
Here, , the optimization parameter, is a concatenation of , and coordinates: and are the linear inequalities that represent the cuboid obstacle. Thus we are solving for the point that belongs to the smallest ellipsoid centered at that still touches the cuboid obstacle defined by . Let denote the solution to this QP and let . If , then there is no intersection between the ellipsoid and cuboid at time , and the chance constraint is not violated.
For the numerical implementation of this collision detection scheme, the frequency at which collisions are checked is an algorithm tuning parameter, and (7) is only solved after it has been determined that the overbounding spheres of the ellipsoid and cuboid intersect. The latter check is computationaly very simple.
3.3 Path Planning
In our prototypical UTM scenario mentioned in Section 1, the covariance propagation and tube generation algorithms discussed in Section 3.1 inform the functionality to validate aircraft trajectories. If a desired flight plan is found to be unsafe due to possible collisions with obstacles, the UTM system should return a safe flight path for the UAS to fly. Here, the UAS is operating in a densely populated urban environment with many no-fly zones or obstacles. For scenarios such as this, with a large number of nonconvex obstacle constraints, a Rapidly-expanding Random Tree (RRT) algorithm is useful.
This section lays out the modifications made to the standard 2D RRT algorithm lavalle1998rapidly for the purpose of generating a desired constant-altitude flight path . Note that the path planning is handled in two dimensions, under the assumption that the UAS operates in an urban environment at a constant desired altitude, but all tube generation and collision checks are three-dimensional.
3.3.1 Informed Subset
By their nature, RRT* algorithms find optimal paths between the initial state and every state in the search space, regardless of what final state the user is actually interested in. This is an inefficient use of computational resources that the Informed variant gammell2014informed attempts to address. By only sampling within an ellipsoidal subset and excluding regions of the sample space that cannot possibly improve the solution, the algorithm makes a better-informed decision about where to look for more optimal paths.
This informed subset is defined as follows:
where is the current best solution cost, is the global sample space, is the initial state and is the desired final state.
This Informed variant is outlined in Algorithm 1 from gammell2014informed, where is the current set of nodes, is the rewiring radius, and is the total number of iterations. Note that the subroutine returns a randomly sampled point that is contained within the ellipsoid defined in (8), and lines 7–18 in Algorithm 2 include the rewiring steps integral to the RRT algorithm variant.
3.3.2 Chance Constraints and Dynamic Extension
If the solution of Algorithm 1 was passed to the tube generation algorithm as , there would be no guarantee that the tube generated would not intersect the obstacles. The solution to a path planning problem with obstacles often passes very closely to the obstacle, where any nonzero uncertainty in the state would produce a tube that would intersect the obstacle. Thus a buffer region is added to the obstacles and adjusted periodically in a manner informed by full covariance propagation.
This buffer can be seen in Figure 3, which represents a single outer-loop iteration in which the buffer is held constant. Here, the green triangle is the starting point , the blue circle is the end point , the green line is the current best solution, the dashed red ellipse is the current informed subset as defined in Eqn. (8), and the black lines represent the total set of sampled nodes and connecting vertices. The solid red rectangle is the actual obstacle, while the dashed red rectangle is the buffered obstacle that is actually passed to Algorithm 1 and used in the subroutine . The bottom subfigure is the same as the top, but with the covariance tube as defined by (6) overlaid, illustrating how the buffer prevents the tube from intersecting the true obstacle.
The dynamic extension to this algorithm involves intermittently halting the standard RRT algorithm, propagating the nominal vehicle trajectory and covariance, and adjusting the obstacle buffer sizes accordingly. This requires an algorithm that informs the buffer size adjustments, answering the question: "By how much should we grow or shrink the buffer such that the covariance tube just touches the obstacle?". To answer this, we start with the easier question of "What size ellipsoid just touches the obstacle?" This is illustrated in Figure 4 and can be expressed as a QP:
We alter (10) to take into account the size of the buffer as follows:
where is a vector of ones and is a scalar representing the size of the buffer. We can then vary until , which solves for the buffer size such that the smallest ellipsoid that intersects the buffer is the same size as the actual covariance ellipsoid. This solution is used as the buffer size for the next iteration. Note that this new buffer size is only an approximation of the actual solution since the size of the covariance tube is dependent on the solution to the inner loop, , which changes every iteration.
Letting be a vector of all ones has the effect of buffering the obstacle by an equal amount in all directions, as opposed to extending the buffer only in certain directions. Equal buffering on all sides of the obstacle may result in less optimal trajectories in a situation where the vehicle’s trajectory interacts with multiple edges of an obstacle and this is viewed as the trade-off for not including the computationally nontrivial logic for a more discriminating buffer re-sizing. Additionally, expanding the buffer in all directions reduces the RRT search space more than merely expanding in one direction, which aids convergence.
Because the size of the probability ellipsoids vary spatially, the optimal buffer size is not necessarily the same for every obstacle. Thus, buffer changes happen independently for every obstacle. There are two cases to be considered: the buffer shrinks or stays the same, and the buffer grows. If a buffer is shrunken or left unchanged, no RRT nodes are changed, and the algorithm continues as normal. However, if a buffer grows, the nodes and connecting vertices that are now contained within the newly-sized obstacle are no longer valid. They are thus deleted, leaving only the parent trees and the now-stranded branches. Then, the Informed RRT* algorithm is used to grow branches from the parent tree until any part of the stranded branch is re-connected. At this point, any nodes left without a parent are removed from the set of nodes, and the Informed RRT* algorithm continues to run according to Algorithm 1 until the next covariance propagation and buffer resize. This is laid out in more detail in Algorithm 3.
4.1 Trajectory Validation
The first result of interest is a validation of the linear covariance (LC) propagation described in Section 3.1 versus the results of the Monte Carlo simulations. For both the fixed-wing and quadrotor models, a simple desired trajectory was prescribed, and the state covariance computed from a 10,000 run MC simulation is compared to the LC results computed from Eqn. (5). For the quadrotor model, the desired trajectory is a three phase ascent-cruise-descent mission profile. For the fixed-wing model, it is a constant-altitude trajectory with a sinusoid shape in the lateral direction.
The comparison results are shown in Figures 5 – 8. Here, , , , , , and represent either the mean position, or the nominal trajectory , from the MC and LC simulations, respectively. All plots show good agreement between MC and LC, with the largest deviations being in the channel in Figure 5. This justifies our use of LC propagation in path planning under uncertainty. Note that the deviations in appear large in the plot, but the vertical axis range is quite small. Also note that the nominal trajectory for the fixed-wing example does not track the desired trajectory exactly, but that is a function of the model’s controller, which is not the focus of this work, as explained in Section 2.
All computations are performed in the MATLAB environment running on a dual-core Intel Core i5 at 2.7 GHz, and all confidence ellipsoid tubes use . This value was chosen for illustration purposes, though some UTM applications may call for smaller or larger values. For the quadrotor model, the LC propagation took of computation time for of simulated flight time. For the fixed-wing model, it took of computation time for of simulated flight time.
4.2 Path Planning
Now that we have shown that the LC propagation scheme is sufficiently fast and accurate when compared to MC simulations of the nonlinear system, we can use the LC propagation algorithm to enable chance-constrained path planning as laid out in Section 3.3. For brevity, only the quadrotor model is utilized in this section.
Figures 9 and 10 show the results of Algorithm 3 for a three obstacle scenario. A solution is found that is very close to the minimum path length between and , while still ensuring that the confidence ellipsoid tube does not intersect any of the obstacles. Total computation time for this example was .
Note that the buffer re-sizing algorithm converges, but only to a region, the size of which is dictated by . There is no guarantee that the algorithm will exit without a constraint violation, but, based on extensive simulation studies, the magnitude of the violation will be sufficiently small for sufficiently large and small .
Future UTM systems will need to rely on rapid UQ for trajectory validation and path planning in order to account for the effects of wind turbulence/gusts and other uncertainties and disturbances. This work combines existing Dynamic RRT* and Informed RRT* algorithms, and adds an obstacle buffer resizing technique to solve a challenge of chance-constrained path planning: trajectory re-planning changes the outcome of the covariance propagation. The results presented here shows that this RRT*-based algorithm, in combination with QP-based collision checking for trajectory validation, successfully solves the aforementioned problem, resulting in a computationally efficient chance-constrained path planner. Trajectory validation examples were presented for both quadrotor and fixed-wing models in 3D flight in a non-static atmosphere. Path planning examples were presented for the quadrotor model, showing near-optimal navigation around 3 obstacles while enforcing chance state constraints.
This work was supported in part by NASA under Cooperative Agreement NNX16AH81A.
Conflict of interest
The authors declare no potential conflict of interests.
Appendix A Quadrotor Modeling
As a balance between model fidelity and computational efficiency, the quadrotor model used for uncertainty propagation is built upon double integrator dynamics augmented with aerodynamic drag of the following form:
where , , and are the vehicle’s coordinates in an inertial reference frame, is its velocity with respect to the atmosphere, is the velocity of the local atmosphere with respect to the ground, is the control input, is the vehicle mass, is the air density, is the reference area, and is the coefficient of drag, assumed to be constant. This model assumes that an inner-loop controller for vehicle attitude and thrust has been implemented that has a feed forward term to cancel out gravity and has sufficiently high bandwidth that we can control acceleration directly. It also assumes a non-static atmosphere and a drag coefficient that remains constant regardless of vehicle state or direction of the relative wind vector . Uncertainty in the drag coefficient can, however, be handled using our UQ and planning algorithms.
When using the above model for uncertainty propagation, it is important that a controller be added so that the uncertainty in vehicle states does not grow so large as to be useless for trajectory prediction purposes. The method presented here is agnostic to controller choice, but for modeling purposes, it is beneficial if the controller can be expressed in a closed form that is easily linearizable. This work utilizes a dynamic extension controller of the following form:
where and are controller gains that may be treated as tuning parameters to match the flight characteristics of our model to the flight characteristics of a real-world vehicle for which the UTM may be trying to predict its trajectory.
Finally, we consider the model for our wind disturbance. A Dryden wind model specifies the power spectral density (PSD) for the body-fixed longitudinal, lateral, and vertical directions of a fixed-wing aircraft. Other works waslander2009wind apply this model directly to quadrotors. Here, the longitudinal channel is replicated in the , , and directions of the inertial frame. The resulting Dryden-like wind model is summarized as
for . Here, is the transfer function of Dryden model coloring filter, , , and are its state space realization, is the gust intensity parameter, is the characteristic length, and is the Gaussian white noise input hoblit1988gust. The output of this model, , is the wind velocity vector in (13).
Appendix B Fixed-Wing Aircraft Modeling
To illustrate another typical setting which involves sUAS, the EOMs for a fixed-wing aircraft under closed-loop control in a non-static atmosphere were derived. Hull hull2007fundamentals provides EOM’s for longitudinal flight with moving atmosphere and EOM’s for full three-dimensional flight in a static atmosphere, but does not provide EOM’s for 3D flight in a moving atmosphere.
Figure 11 shows the unit vectors and rotations necessary to define flight in three dimensions. Here, frame is the inertial frame, while frames , , and are rotated by angles , , and , respectively, where is velocity yaw or heading angle, is velocity pitch, and is velocity roll.
Additionally, the thrust, drag, lift and gravity forces acting on the aircraft can be defined as follows:
and the velocity of the aircraft can be represented as , as seen in Figure 12.
Here, , , , and are the desired values of the vehicle’s yaw, pitch, thrust, and velocity, respectively. The feed forward terms and are the values of and , respectively, that are required to maintain steady flight:
The vehicle’s drag polar parameter is denoted . Similar to the quadrotor model, controller gains , , , and may be tuned to better replicate the behavior of a real-world UAS.
The outer-loop controller is separated into two components: longitudinal and lateral. Its design requires us to add two additional states ( and ) and to estimate via finite differences, which is undesirable from a computational efficiency perspective, but avoids wrap-around issues.
For the longitudinal controller, is assumed to be constant and only the dynamics are considered:
Note that because above controller is part of the outer-loop, the output is and not .
For the lateral controller, we need to control so a different approach is pursued. Here, is assumed to be constant and only the and kinematics are considered:
This control is invalid if is singular, corresponding to a pitch angle of or a desired velocity of , which are flight conditions not encountered in this work.
For the wind disturbance, the Dryden wind model defines wind in the longitudinal, lateral, and vertical body fixed directions, not in the directions. In the two dimensional case berning2018Rapid, we assumed that is small and thus we have wind in the and directions. In the three-dimensional case, however, we cannot necessarily make the same assumptions about . Thus, the following wind definitions are used:
We can now write the full equations of motion as follows:
If we assume that then we can express the wind accelerations as follows:
The power spectral densities (PSD) for the Dryden model are defined following hoblit1988gust.
For the longitudinal channel:
For the lateral channel:
For the vertical channel:
where . The corresponding coloring filters are as follows:
These have state space realizations of the form:
for , where is the Gaussian white noise input and the matrices are defined as follows: