Trajectory Planning Under Stochastic and Bounded Sensing Uncertainties Using Reachability Analysis
TTrajectory Planning Under Stochastic and BoundedSensing Uncertainties Using Reachability Analysis
Akshay Shetty, University of Illinois Urbana-ChampaignGrace Xingxin Gao, Stanford University
Akshay Shetty is a Ph.D. candidate in the Department of Aerospace Engineering at University of Illinois at Urbana-Champaign,and a visiting scholar at Stanford University. He received his M.S. degree in Aerospace Engineering from University of Illinoisat Urbana-Champaign in 2017, and his B.Tech. degree in Aerospace Engineering from Indian Institute of Technology, Bombay,India in 2014. His research interests include safety, trajectory planning, and control for autonomous vehicles.
Grace Xingxin Gao
Grace Xingxin Gao is an assistant professor in the Department of Aeronautics and Astronautics at StanfordUniversity. Before joining Stanford University, she was an assistant professor at University of Illinois at Urbana-Champaign. Sheobtained her Ph.D. degree at Stanford University. Her research is on robust and secure positioning, navigation and timing withapplications to manned and unmanned aerial vehicles, robotics, and power systems.
Abstract
Trajectory planning under uncertainty is an active research topic. Previous works predict state and state estimation uncertaintiesalong trajectories to check for collision safety. They assume either stochastic or bounded sensing uncertainties. However, GNSSpseudoranges are typically modeled to contain stochastic uncertainties with additional biases in urban environments. Thus, givenbounds for the bias, the planner needs to account for both stochastic and bounded sensing uncertainties. In our prior work [1]we presented a reachability analysis to predict state and state estimation uncertainties under stochastic and bounded uncertainties.However, we ignored the correlation between these uncertainties, leading to an imperfect approximation of the state uncertainty.In this paper we improve our reachability analysis by predicting state uncertainty as a function of independent quantities. Wedesign a metric for the predicted uncertainty to compare candidate trajectories during planning. Finally, we validate the planner forGNSS-based urban navigation of fixed-wing UAS. KeywordsTrajectory Planning, Reachability Analysis, Probabilistic Zonotope, Global Navigation Satellite System (GNSS), Un-manned Aerial System (UAS)
I. I
NTRODUCTION
Recently there has been growing interest in autonomous applications of robotic systems for various purposes such as deliveringgoods, surveying areas of interest, and search and rescue [2]–[6]. In general the problem of planning a trajectory for such systemshas been extensively explored in literature. Traditional planning approaches proposed rapidly exploring random trees [7] andtheir variants [8]. However, these works do not account for uncertainty in the system state (position and orientation) that arisesdue to motion and sensing uncertainties.Recent works [9]–[12] extended the traditional planners by accounting for motion and sensing uncertainties. These workspredict the state uncertainty along candidate trajectories in order to find optimal, collision safe trajectories. In [9] the authorspredict the state uncertainty for a system with linear-quadratic-Gaussian (LQG) control, while accounting for sensing uncertainties.The work in [10] predicts a distribution over the system states by considering the state estimation error distribution as well asall possible state estimates that could be realized in the future. In [11] the authors plan trajectories using a velocity obstacleparadigm in combination with a convex approximation of the sensing uncertainties. The work in [12] predicts state uncertaintyby using reachability analysis to account for tracking errors of a high-fidelity model in a lower-dimensional planning subspace.However, these works assume either a stochastic [9], [10] or a bounded [11], [12] sensing uncertainty model, which may notalways be valid.For outdoor navigation, autonomous systems generally use Global Navigation Satellite System (GNSS) measurements for stateestimation. GNSS pseudorange measurements are typically modeled to contain stochastic uncertainties along with an additionalbias in urban environments due to signal reflections from nearby buildings. These effects are classified either as multipath,where both the direct and reflected signals from the same satellite are received; or as non-line-of-sight (NLOS), where only thereflected satellite signal is received [13]. Generally, NLOS effects result in large biases in pseudorange measurements. Variousoutlier rejection techniques and three-dimensional (3D) map-based techniques have been proposed to detect and exclude thecorresponding measurements [14]–[16]. On the other hand, biases due to multipath effects are relatively lower. Previous works[13], [17] have proposed methods to calculate the bounds for these multipath biases using a 3D map and the GNSS receiver a r X i v : . [ c s . R O ] D ec rchitecture. Thus, given bounds for the additional bias, the trajectory planner needs to account for the presence of both stochastic and bounded sensing uncertainties.In our prior work [1], we used reachability analysis to predict state uncertainty under motion uncertainty and both stochasticand bounded sensing uncertainties. We recursively predicted the state uncertainty and the state estimation uncertainty, which arefunctions of the motion and sensing uncertainties along the trajectory. However, we ignored the correlation between the stateand the state estimation uncertainties, which led to an imperfect approximation of the state uncertainty. Thus, in this paper weimprove our prior reachability analysis and use it within a trajectory planning framework. This paper is based on our work in[18]. The main contributions are listed as follows:1) We discuss the limitations in our prior reachability analysis [1] and present an improved analysis in order to predict stateuncertainty as a function of independent (uncorrelated) quantities. We account for the presence of both stochastic andbounded sensing uncertainties in our analysis.2) Next, we combine our method to predict state uncertainty with an available trajectory planning framework [19]. In orderto compare candidate trajectories within the planning framework, we design a metric for the size of the predicted stateuncertainty.3) Finally, via simulations, we demonstrate the applicability of the trajectory planner for GNSS-based navigation of fixed-wing unmanned aerial systems (UAS) in an urban environment. Planning results are presented for two scenarios: a singleUAS in a static environment, and multiple UAS in a shared airspace. We statistically validate the collision safety of theUAS by simulating multiple trajectories along the planned trajectories.The rest of the paper is organized as follows: we begin by formulating the problem in Section II; in Section III, we present theset representations and operations used in the paper; in Section IV, we mention the limitations of our prior work [1] and discussour improved reachability analysis used to predict state uncertainty; Section V provides an overview of the trajectory planningframework [19] along with our metric for the size of the predicted state uncertainty; and in Section VI, we demonstrate theplanning results for urban GNSS-based navigation of a fixed-wing UAS.II. P ROBLEM F ORMULATION
We consider a non-linear discrete-time system with the following motion model: x k = f ( x k − , u k − ) + w k , (1)where x k is the state vector, u k is the input vector, f is a non-linear function representing the kinematics, w k is the motionmodel error vector modeled as a zero-mean Gaussian distribution N (0 , Q ) , and k represents the time instant. For measurements,the following non-linear sensing model is considered: z k = h ( x k ) + ν k , (2)where z k is the measurement vector, h is a non-linear function representing the measurements, and ν k is the sensing model errorvector modeled as a Gaussian distribution with an uncertain mean, i.e., N ( b k , R k ) . Here the mean b k can take any value in abounded set B k . Thus, ν k captures the presence of both stochastic and bounded sensing uncertainties.Similar to prior trajectory planning work [10], we assume there exists a low-level CONNECT function that connects a trajectorybetween two states. Thus, given states x i and x j , the CONNECT function provides us the following: (cid:0) ˇ X i,j , ˇ U i,j , ˇ K i,j (cid:1) = CONNECT ( x i , x j ) , (3)where ˇ X i,j is the set of nominal states (ˇ x τ i , ˇ x τ i +1 , · · · , ˇ x τ j − ) and ˇ U i,j is the set of nominal inputs (ˇ u τ i , ˇ u τ i +1 , · · · , ˇ u τ j − ) .These nominal states and inputs satisfy the nominal motion model from Equation (1): ˇ x k = f (ˇ x k − , ˇ u k − ) ∀ k ∈ [ τ i + 1 , τ j − ,x i = ˇ x τ i , x j = f (ˇ x τ j − , ˇ u τ j − ) , (4)where ˇ K i,j is the set of stabilizing linear state feedback control gains ( ˇ K τ i , ˇ K τ i +1 , · · · , ˇ K τ j − ) . Given the feedback controlgain and an on-line state estimate ˆ x k , the total control input during execution is of the form: u k = ˇ u k − ˇ K k (ˆ x k − ˇ x k ) . (5)Let X obs denote the set of states representing obstacles in the environment. Given an initial state x init and a goal state x goal ,the problem for the trajectory planner is defined as: min ( ˇ X, ˇ U, ˇ K ) cost ( ˇ X, ˇ U ) , subject to: Pr( x k ∈ X obs ) < δ, (6)here δ is a specified threshold for the probability of collision, and ( ˇ X, ˇ U , ˇ K ) are concatenated sets representing the nominaltrajectory from x init to x goal as follows: ( ˇ X, ˇ U , ˇ K ) = (cid:0) CONNECT ( x init , x ) , CONNECT ( x , x ) , · · · , CONNECT ( x l , x goal ) (cid:1) . (7)For our trajectory planning problem, we set the cost function in Equation (6) to be the total length of the concatenated nominaltrajectory from x init to x goal . III. S ET R EPRESENTATIONS AND O PERATIONS
For our reachability analysis we use probabilistic zonotopes as the set representation since they are suitable to represent bothstochastic and bounded sensing uncertainties [20]. Probabilistic zonotopes have been shown to be computationally efficient andclosed under linear transform and Minkowski sum operations [21]. A zonotope P is defined as follows: P = (cid:40) x ∈ R n | x = c P + r (cid:88) i =1 β i · g { i }P , − ≤ β i ≤ (cid:41) , (8)where c P is the center of the zonotope, and g { i }P are n − dimensional column vectors referred to as generators. The generatorsof a zonotope determine its shape relative to its center. The zonotope can be concisely written as P = Z ( c P , G P ) , where G P = (cid:104) g { }P , . . . , g { r }P (cid:105) is the corresponding n × r generator matrix. Fig. 1(a) shows an example 2D zonotope along with itsgenerator matrix. Zonotopes have been commonly used in literature to represent bounded uncertainties. In [20], the authorsextended the representation to include stochastic uncertainties by defining a probabilistic zonotope as a Gaussian distributionwith an uncertain and bounded mean: P = Z ( c P , G P , Σ P ) , (9)where c P and G P represent the zonotope for the bounded uncertainty, and Σ P is the Gaussian covariance representing thestochastic uncertainty. Fig. 1(b) provides an example visualization of a 2D probabilistic zonotope. Note that probabilistic zonotopesdo not have a normalized distribution, and in fact can enclose multiple distributions.The Minkowski sum operation between two probabilistic zonotopes is defined as [20]: P ⊕ P = Z ( c P + c P , [ G P , G P ] , Σ P + Σ P ) , (10)whereas, the linear transform operation is defined as [20]: T · P = Z (cid:0) T c P , T G P , T Σ P T (cid:62) (cid:1) . (11)Note that the Minkowski sum operation in Equation (10) assumes P and P to be independent probabilistic zonotopes as itdoes not consider the correlation between the corresponding Gaussian covariances Σ P and Σ P . This property gives rise tothe limitation in our prior work and hence motivates our approach for computing stochastic reachable sets as discussed later inSection IV-C. IV. R EACHABILITY A NALYSIS
In this section, we present our reachability analysis used to predict the state uncertainty. We first briefly describe the stateestimation filter used on-board. Next, we address the limitation in our prior reachability analysis, and derive the equations for (a) (b)
Fig. 1: Example 2D visualizations of (a) a zonotope, and (b) a probabilistic zonotope.ig. 2: Visualization of over-bounding hypothesis for the state estimation filter used in the reachability analysis.computing the stochastic reachable sets as a function of independent quantities. Finally, we discuss the approximations we maketo predict the state uncertainty.
A. On-board State Estimation
During trajectory execution, the true state x k will not be available to compute the control input. Thus, we need to estimate thestate ˆ x k in order to compute the total input as shown in Equation (5). Given the non-linear motion and measurement models,we use an EKF as the state estimation filter. The prediction step of the EKF is performed as: ¯ x k = f (ˆ x k − , u k − ) , (12) ¯ P k = A k P k − A (cid:62) k + Q, (13)where A k = ∂f∂x (cid:12)(cid:12)(cid:12) x =ˇ x k and Q is the covariance of the motion model uncertainty defined in Equation (1). For the EKF correctionstep, similar to our prior work, we choose an over-bounding hypothesis ˆ R k as the measurement covariance matrix. The over-bounding is performed such that ˆ R k matches the tail of the distribution at a desired confidence level. Fig. 2 illustrates thehypothesis for a simple one-dimensional case. Here, without loss of generality, we assume the sensing model error vector ν k tobe centered at origin. Note that our reachability analysis does not necessarily require choosing an over-bounding hypothesis forthe EKF measurement covariance matrix. If desired, a different hypothesis can be chosen and used with the rest of the analysis.We choose the over-bounding hypothesis since it is equivalent to scaling or inflating the covariance matrix, which is a commonapproach for practical implementation of KF and its variants [22]–[24].Once ˆ R k has been computed, the EKF correction step is performed as: L k = ¯ P k C (cid:62) k ( C k ¯ P k C (cid:62) k + ˆ R k ) − , (14) ˆ x k = ¯ x k + L k ( z k − h (¯ x k )) , (15) P k = ¯ P k − L k C k ¯ P k , (16)where L is the Kalman gain and C k = ∂h∂x (cid:12)(cid:12)(cid:12) x =ˇ x k . B. Limitations in Our Prior Work
In our prior work we used the system equations (1)-(5) and state estimation equations (12)-(16) to derive the following recursiveequations for the state x k and state estimation error ˜ x k ( = ˆ x k − x k ): x k = ( A k − − B k − ˇ K k − )( x k − − ˇ x k − ) − B k − ˇ K k − ˜ x k − + ˇ x k + L f [ s, ˇ s ] k − + w k , (17) ˜ x k = ( I − L k C k ) A k − ˜ x k − + ( I − L k C k )( L f [ˆ s, ˇ s ] k − − L f [ s, ˇ s ] k − ) + L k ( L h [ x, ˇ x ] k − L h [¯ x, ˇ x ] k ) − ( I − L k C k ) w k + L k ν k , (18)where s (cid:62) = [ x (cid:62) , u (cid:62) ] and L are Lagrange remainders resulting from the linearization of the non-linear models. For instance theremainder L f [ s, ˇ s ] k − is expressed as: L f ( i ) [ s, ˇ s ] k − = 12 ( s k − − ˇ s k − ) (cid:62) J f ( i ) s ( ξ )( s k − − ˇ s k − ) ,ξ ∈ { ˇ s k − + η ( s k − − ˇ s ) k − | η ∈ [0 , } , (19)here subscript ( i ) represents the i th element of the remainder vector and J f ( i ) s ( ξ ) = ∂ f ( i ) ( ξ ) ∂s .We then extended Equations (17) and (18) to set notations in order to recursively compute the stochastic reachable set X k andthe state estimation error set ˜ X k : X k = ( A k − − B k − ˇ K k − )( X k − − ˇ x k − ) ⊕ B k − ˇ K k − ˜ X k − + ˇ x k ⊕ L f [ s, ˇ s ] k − ⊕ W k , (20) ˜ X k = ( I − L k C k ) A k − ˜ X k − ⊕ ( I − L k C k )( L f [ˆ s, ˇ s ] k − ⊕ L f [ s, ˇ s ] k − ) ⊕ L k ( L h [ x, ˇ x ] k ⊕ L h [¯ x, ˇ x ] k ) ⊕ ( I − L k C k ) W k ⊕ L k V k , (21)where W k and V k are the set representations for uncertainties in the motion and sensing models, respectively. However, forthe first Minkowski sum operation in Equation (20) the sets X k − and ˜ X k − are not independent. By rewriting Equations (20)and (21) for the previous time instant, we observe that both X k − and ˜ X k − depend on W k − . Thus, ignoring the correlationbetween X k − and ˜ X k − results in an imperfect approximation of the stochastic reachable set X k . C. Improved Computation of Stochastic Reachable Sets
In order to avoid the imperfect approximation in our prior work, we first derive equations for the state and the state estimationerror as a function of independent (and hence uncorrelated) quantities. This is in contrast to Equation (17) where x k − and ˜ x k − are correlated. We then extend the equations to set notations in order to compute the stochastic reachable set and the stateestimation error set. Since the corresponding sets represent independent quantities, the Minkowski sum operations do not resultin an imperfect approximation, in contrast to Equation (20).We prove using mathematical induction that the state x k and the state estimation error ˜ x k can be obtained as a function ofindependent quantities as follows: x k = ˇ x k + φ k ( x − ˇ x )+ φ k ˜ x + k (cid:88) n =1 3 φ nk w n + k (cid:88) n =1 4 φ nk ν n + k − (cid:88) n =0 5 φ nk L f [ s, ˇ s ] n + k − (cid:88) n =0 6 φ nk L f [ˆ s, ˇ s ] n + k − (cid:88) n =0 7 φ nk L h [ x, ˇ x ] n + k − (cid:88) n =0 8 φ nk L h [¯ x, ˇ x ] n , (22) ˜ x k = ˜ φ k ˜ x + k (cid:88) n =1 3 ˜ φ nk w n + k (cid:88) n =1 4 ˜ φ nk ν n + k − (cid:88) n =0 5 ˜ φ nk L f [ s, ˇ s ] n + k − (cid:88) n =0 6 ˜ φ nk L f [ˆ s, ˇ s ] n + k − (cid:88) n =0 7 ˜ φ nk L h [ x, ˇ x ] n + k − (cid:88) n =0 8 ˜ φ nk L h [¯ x, ˇ x ] n , (23)where all φ k and ˜ φ k are matrix coefficients derived from system and state estimation matrices. Here x is the initial state of thesystem, ˜ x is the initial state estimation error, w n and ν n are motion and sensing model errors independently sampled along thetrajectory, and L are the Lagrange remainders. Proof:
For the induction base case, the initial state and state estimation error can be written in the above form as: x = ˇ x + ( x − ˇ x ) , ˜ x = ˜ x , (24)which can be obtained by setting φ = I , i φ = O ∀ i = { , · · · , } , ˜ φ = I in Equation (22), and i ˜ φ = O ∀ i = { , · · · , } in Equation (23).For the induction step, we first assume that Equations (22) and (23) hold true for time instant k − , i.e.: x k − = ˇ x k − + φ k − ( x − ˇ x ) + φ k − ˜ x + k − (cid:88) n =1 3 φ nk − w n + k − (cid:88) n =1 4 φ nk − ν n + k − (cid:88) n =0 5 φ nk − L f [ s, ˇ s ] n + k − (cid:88) n =0 6 φ nk − L f [ˆ s, ˇ s ] n + k − (cid:88) n =0 7 φ nk − L h [ x, ˇ x ] n + k − (cid:88) n =0 8 φ nk − L h [¯ x, ˇ x ] n , (25) ˜ x k − = ˜ φ k − ˜ x + k − (cid:88) n =1 3 ˜ φ nk − w n + k − (cid:88) n =1 4 ˜ φ nk − ν n + k − (cid:88) n =0 5 ˜ φ nk − L f [ s, ˇ s ] n + k − (cid:88) n =0 6 ˜ φ nk − L f [ˆ s, ˇ s ] n + k − (cid:88) n =0 7 ˜ φ nk − L h [ x, ˇ x ] n + k − (cid:88) n =0 8 ˜ φ nk − L h [¯ x, ˇ x ] n . (26)Next, in order to show that Equations (22) and (23) hold true for time instant k we begin by replacing Equation (26) in Equation18). This gives us the following expression for the state estimation error ˜ x k : ˜ x k = ( I − L k C k ) A k − ˜ φ k − ˜ x + ( I − L k C k ) A k − k − (cid:88) n =1 3 ˜ φ nk − w n − ( I − L k C k ) w k + ( I − L k C k ) A k − k − (cid:88) n =1 4 ˜ φ nk − ν n + L k ν k + ( I − L k C k ) A k − k − (cid:88) n =0 5 ˜ φ nk − L f [ s, ˇ s ] n − ( I − L k C k ) L f [ s, ˇ s ] k − + ( I − L k C k ) A k − k − (cid:88) n =0 6 ˜ φ nk − L f [ˆ s, ˇ s ] n + ( I − L k C k ) L f [ˆ s, ˇ s ] k − + ( I − L k C k ) A k − k − (cid:88) n =0 7 ˜ φ nk − L h [ x, ˇ x ] n + L k L h [ x, ˇ x ] k + ( I − L k C k ) A k − k − (cid:88) n =0 8 ˜ φ nk − L h [¯ x, ˇ x ] n − L k L h [¯ x, ˇ x ] k . (27)The above equation can then be written in the form of Equation (23) where the ˜ φ k matrix coefficients can be obtained as: ˜ φ k = ( I − L k C k ) A k − ˜ φ k − , ˜ φ nk = ( I − L k C k ) A k − ˜ φ nk − ∀ n ∈ { , · · · , k − } , ˜ φ kk = − ( I − L k C k ) , ˜ φ nk = ( I − L k C k ) A k − ˜ φ nk − ∀ n ∈ { , · · · , k − } , ˜ φ kk = L k , ˜ φ nk = ( I − L k C k ) A k − ˜ φ nk − ∀ n ∈ { , · · · , k − } , ˜ φ k − k = − ( I − L k C k ) , ˜ φ nk = ( I − L k C k ) A k − ˜ φ nk − ∀ n ∈ { , · · · , k − } , ˜ φ k − k = ( I − L k C k ) , ˜ φ nk = ( I − L k C k ) A k − ˜ φ nk − ∀ n ∈ { , · · · , k − } , ˜ φ k − k = L k , ˜ φ nk = ( I − L k C k ) A k − ˜ φ nk − ∀ n ∈ { , · · · , k − } , ˜ φ k − k = − L k . (28)Similarly for state x k , replacing Equations (25) and (26) in Equation (17), we get the following expression: x k = ˇ x k +( A k − − B k − ˇ K k − ) φ k − ( x − ˇ x )+ (cid:16) ( A k − − B k − ˇ K k − ) φ k − − B k − ˇ K k − ˜ φ k − (cid:17) ˜ x + k − (cid:88) n =1 (cid:16) ( A k − − B k − ˇ K k − ) φ nk − − B k − ˇ K k − ˜ φ nk − (cid:17) w n + w k + k − (cid:88) n =1 (cid:16) ( A k − − B k − ˇ K k − ) φ nk − − B k − ˇ K k − ˜ φ nk − (cid:17) v n + k − (cid:88) n =0 (cid:16) ( A k − − B k − ˇ K k − ) φ nk − − B k − ˇ K k − ˜ φ nk − (cid:17) L f [ s, ˇ s ] n + L f [ s, ˇ s ] k − + k − (cid:88) n =0 (cid:16) ( A k − − B k − ˇ K k − ) φ nk − − B k − ˇ K k − ˜ φ nk − (cid:17) L f [ˆ s, ˇ s ] n + k − (cid:88) n =0 (cid:16) ( A k − − B k − ˇ K k − ) φ nk − − B k − ˇ K k − ˜ φ nk − (cid:17) L h [ x, ˇ x ] n + k − (cid:88) n =0 (cid:16) ( A k − − B k − ˇ K k − ) φ nk − − B k − ˇ K k − ˜ φ nk − (cid:17) L h [¯ x, ˇ x ] n . (29)The above equation can then be written in the form of Equation (22) where the φ k matrix coefficients can be obtained as: φ k = ( A k − − B k − ˇ K k − ) φ k − , φ k = ( A k − − B k − ˇ K k − ) φ k − − B k − ˇ K k − ˜ φ k − , φ nk = ( A k − − B k − ˇ K k − ) φ nk − − B k − ˇ K k − ˜ φ nk − ∀ n ∈ { , · · · , k − } , φ kk = I, φ nk = ( A k − − B k − ˇ K k − ) φ nk − − B k − ˇ K k − ˜ φ nk − ∀ n ∈ { , · · · , k − } , φ kk = O, φ nk = ( A k − − B k − ˇ K k − ) φ nk − − B k − ˇ K k − ˜ φ nk − ∀ n ∈ { , · · · , k − } , φ k − k = I, φ nk = ( A k − − B k − ˇ K k − ) φ nk − − B k − ˇ K k − ˜ φ nk − ∀ n ∈ { , · · · , k − } , φ k − k = O, φ nk = ( A k − − B k − ˇ K k − ) φ nk − − B k − ˇ K k − ˜ φ nk − ∀ n ∈ { , · · · , k − } , φ k − k = O, φ nk = ( A k − − B k − ˇ K k − ) φ nk − − B k − ˇ K k − ˜ φ nk − ∀ n ∈ { , · · · , k − } , φ k − k = O. (30)Thus, by the principle of induction, we show that Equations (22) and (23) hold true for all k ∈ { , , · · · } . The matrix coefficients φ k and ˜ φ k are obtained recursively using Equations (28) and (30).Once the above equations for the state and state estimation errors have been derived, we extend them to set notations in ordero compute the stochastic reachable set X k and the state estimation error set ˜ X k as follows: X k = ˇ x k ⊕ φ k ( X − ˇ x ) ⊕ φ k ˜ X ⊕ k (cid:88) n =1 3 φ nk W n ⊕ k (cid:88) n =1 4 φ nk V n ⊕ k − (cid:88) n =0 5 φ nk L f [ s, ˇ s ] n ⊕ k − (cid:88) n =0 6 φ nk L f [ˆ s, ˇ s ] n ⊕ k − (cid:88) n =0 7 φ nk L h [ x, ˇ x ] n ⊕ k − (cid:88) n =0 8 φ nk L h [¯ x, ˇ x ] n , (31) ˜ X k = ˜ φ k ˜ X + k (cid:88) n =1 3 ˜ φ nk W n + k (cid:88) n =1 4 ˜ φ nk V n + k − (cid:88) n =0 5 ˜ φ nk L f [ s, ˇ s ] n + k − (cid:88) n =0 6 ˜ φ nk L f [ˆ s, ˇ s ] n + k − (cid:88) n =0 7 ˜ φ nk L h [ x, ˇ x ] n + k − (cid:88) n =0 8 ˜ φ nk L h [¯ x, ˇ x ] n . (32)Here the sets X , ˜ X , W n , and V n represent independent quantities and hence the Minkowski sum operations do not result inan imperfect approximation of the stochastic reachable sets. D. Predicting State Uncertainty
Once we have an expression to compute the stochastic reachable sets, we perform the following two approximations in orderto predict the state uncertainty:1) We heuristically bound the number of Minkowski sum operations in Equation (31). From Equations (28) and (30) weobserve that the matrix coefficients contain contractive terms, which consequently results in some quantities having anegligible contribution in the computation of X k . Thus, we only consider quantities whose corresponding matrix coefficientshave a Frobenius norm (cid:13)(cid:13) i φ nk (cid:13)(cid:13) F higher than a specified threshold i ζ .2) We approximate the Lagrange remainders L with a Gaussian distribution (cid:98) L , as presented in our prior work [1].Thus, we obtain the following expression for the predicted state uncertainty X k : X k = ˇ x k ⊕ φ k ( X − ˇ x ) ⊕ φ k ˜ X ⊕ (cid:88) n ∈ N k φ nk W n ⊕ (cid:88) n ∈ N k φ nk V n ⊕ (cid:88) n ∈ N k φ nk (cid:98) L f ,n ⊕ (cid:88) n ∈ N k φ nk (cid:98) L f ,n ⊕ (cid:88) n ∈ N k φ nk (cid:98) L h ,n ⊕ (cid:88) n ∈ N k φ nk (cid:98) L h ,n , (33)where i N k contains elements { n } such that (cid:13)(cid:13) i φ nk (cid:13)(cid:13) F ≥ i ζ .V. T RAJECTORY P LANNING
A. Trajectory Planning Overview
In Section IV we presented our method to predict state uncertainty along a single nominal trajectory. The objective of thetrajectory planner is to use this method to explore the environment and find an optimal trajectory for the problem defined inEquation (6). In this paper we choose the planning framework presented in [19] which is desirable given its highly parallelizablestructure. However, note that our method to predict state uncertainty can be used with other planning frameworks that consideruncertainty [10], [25].For details of the trajectory planning framework, we refer the readers to [19]. Here we illustrate the framework in Fig. 3 andsummarize it as follows:1) The planner begins with an offline graph constructing phase. Multiple states are sampled in the environment and a graphof kinematically feasible and collision-free trajectories is constructed.2) Next, the planner explores the graph by predicting state uncertainty along candidate trajectories. Intersection betweenthe predicted state uncertainty and the obstacles is checked in order to maintain the desired level of collision safety asspecified in Equation (6).3) For computational tractability, the planner removes undesirable trajectories by comparing candidate trajectories that arriveat the same state. Here the comparison is done in terms of the trajectory lengths and the sizes of the predicted stateuncertainties.4) Once a collision-safe trajectory to the goal state is found, the planner stops the exploration phase.Thus, in order to use our method of predicting state uncertainty along with the above planning framework, we need to design ametric to represent the size of the predicted state uncertainty.
B. Metric for Predicted State Uncertainty
The state uncertainty along a candidate trajectory is predicted using Equation (33). Given that we use probabilistic zonotopesas our set representation, the state uncertainty can be expressed in the form of Equation (9) as: X k = Z ( c X k , G X k , Σ X k ) , (34)where c X k is the center of the probabilistic zonotope, G X k is the generator matrix for the bounded uncertainty, and Σ X k is thecovariance for the stochastic uncertainty. a) (b) (c) (d) Fig. 3: Overview of trajectory planning framework [19]. (a) The planner first builds a graph of feasible trajectories. Then itexplores the graph rejecting (b) collision unsafe and (c) undesirable trajectories. (d) The planner stops exploring once it finds atrajectory to the goal state.In order to obtain a metric for the size of a probabilistic zonotope, we perform the following steps:1) Generate a confidence zonotope for the state uncertainty as shown in Fig. 4. We represent the confidence zonotope as Z ( c conf , G conf ) , with center c conf = c X k and generator matrix: G conf = [ G X k , α (cid:112) λ v , · · · , α (cid:112) λ n v n ] , (35)where α is a scalar that follows a chi-square distribution [26], [27] based on the collision probability threshold fromEquation (6), λ and v represent the eigenvalues and eigenvectors of the covariance matrix Σ X k , and n here is thedimension of the system.2) Use the covariation of the confidence zonotope as the metric for the size of X k . The covariation of a zonotope is definedas trace ( G (cid:62) conf G conf ) [28]. VI. S IMULATIONS
In this section we demonstrate the applicability of the trajectory planner for GNSS-based navigation of fixed-wing UAS inurban environments. We first describe the motion and sensing models we use for the simulations, and then we discuss trajectoryplanning results for two scenarios: single UAS in a static environment and multiple UAS in a shared airspace.
A. Motion and Sensing Models
For simplicity we restrict the fixed-wing UAS motion to a horizontal plane and represent it by a 2D Dubins model as follows: (cid:34) x k x k θ k (cid:35)(cid:124) (cid:123)(cid:122) (cid:125) x k = (cid:34) x k − x k − θ k − (cid:35) + (cid:34) V k − cos( θ k − )∆ tV k − sin( θ k − )∆ tω k − ∆ t (cid:35)(cid:124) (cid:123)(cid:122) (cid:125) f ( x k − ,u k − ) + w k , (36)Fig. 4: Confidence zonotope of a probabilistic zonotope. a) (b) Fig. 5: (a) Simulated urban environment where we perform ray-tracing to compute the differential path lengths between thedirect signal (green) and the multipath signals (yellow). (b) Multipath noise envelope for a receiver with a quarter-chip early/latecorrelator spacing.where the state vector consists of the 2D position ( x , x ) and the heading angle θ , the inputs to the system are the forwardvelocity V and the angular velocity ω , and ∆ t is the time-step that we set as . . We set the covariance matrix for motionmodel error w k as Q = diag ([0 .
01 m .
01 m .
001 rad ]) .For the sensing model, we consider GNSS pseudorange measurements and heading measurements from an on-board compass.The GNSS pseudorange measurement from the i th satellite can be expressed as [13]: ρ ( i ) k = r ( x k , x s i k ) + cδt + ν ( i ) k , (37)where r represents the true range between the receiver position x k and satellite position x s i k (which we simulate from publiclyavailable almanac data), cδt is the clock bias error, and ν ( i ) k ∼ N ( b ( i ) k , R ( i ) k ) . Here ν ( i ) k contains both the bounded uncertaintydue to additional multipath bias b ( i ) k and the stochastic uncertainty with covariance R ( i ) k . We model the stochastic uncertaintywith an elevation-based factor [29], [30] as: R ( i ) k = Σ ρ / sin ( el ( i ) ) , where we set Σ ρ = 5 m . Since we are primarily concernedwith the UAS position states, we assume for simplicity that the receiver clock and the satellite clocks are perfectly synced, i.e.there is zero clock bias error. However, if desired, clock bias states can also be included in the state vector for the trajectoryplanner. Thus, given N GNSS satellites, the measurement model for the fixed-wing UAS looks as follows: z (1) k ... z ( N ) k z ( N +1) k (cid:124) (cid:123)(cid:122) (cid:125) z k = r ( x k , x s k ) ... r ( x k , x s N k ) θ k (cid:124) (cid:123)(cid:122) (cid:125) h ( x k ) + ν (1) k ... ν ( N ) k ν ( N +1) k (cid:124) (cid:123)(cid:122) (cid:125) ν k , (38)where z ( i ) k = ρ ( i ) k ∀ i = 1 to N , and z ( N +1) k represents the heading measurement with ν ( N +1) k ∼ N (0 , .
001 rad ) . For themultipath bias in the pseudorange measurements, we assume a quarter-chip spacing between the early and late correlators [13].We perform ray-tracing as shown in Fig. 5(a) and assume that the reflected signal strength can be as strong as the direct LOSsignal. Based on these characteristics, we get a multipath noise envelope as shown in Fig. 5(b). B. Trajectory Planning Results
Given the above motion and sensing models, we setup a × wide urban environment to evaluate our trajectoryplanner. In order to simulate multipath effects, we set the flight altitude to
65 m and include buildings up to
120 m tall. Wefirst consider trajectory planning for a single UAS and show the results in Fig. 6. Buildings taller than the flight altitude arecolored orange, whereas buildings shorter than the flight altitude are colored yellow. For constructing the planner graph ofkinematically feasible and collision-free trajectories, we use a grid of states with
100 m spacing as shown in Fig. 6(a). Once thegraph is constructed, the planner explores the graph to find an optimal trajectory between a given start and goal state. In orderto statistically validate the collision safety of the planned trajectory, we simulate trajectories along the planned trajectory.We observe that all simulated trajectories remain within the σ confidence zonotopes of the predicted state uncertainty a) (b) Fig. 6: Planning results for a single fixed-wing UAS. (a) Graph of feasible trajectories built by planner. (b) All simulatedtrajectories lie inside the predicted state uncertainty confidence sets, thus validating collision safety of the planned trajectory.as shown in Fig. 6(b). Thus, the trajectory planner safely leads the UAS to the goal state under stochastic and bounded GNSSuncertainties.Next, we evaluate the trajectory planner for multiple UAS in a shared airspace. Here we sequentially plan trajectories forfive UAS in the same urban environment. Since we assume all UAS to be fixed-wing with the same motion model in Equation(36), the planning framework allows us to reuse the same planner graph constructed in the above scenario. Fig. 7(a) showsthe planner graph along with the planned trajectories for the five UAS. The planner begins by finding an optimal trajectoryfor UAS-1 while maintaining collision safety with respect to the obstacles (orange-colored buildings). Next, the planner findsan optimal trajectory for UAS-2 while maintaining collision safety with respect to UAS-1 and the obstacles. Note that whilea shorter candidate trajectory existed for UAS-2, it was rejected by the planner as it could not maintain collision safety withrespect to UAS-1. Next, the planner finds an optimal trajectory for UAS-3 while maintaining collision safety with respect toUAS-1, UAS-2, and the obstacles. For UAS-4 the planner finds an optimal trajectory that seems to collide with the trajectoryfor UAS-2. However, the trajectories do not intersect temporally, thus maintaining collision safety. For UAS-5, we set the samestart state as for UAS-4 and set the trajectory start time a few seconds after that of UAS-4. Again the optimal trajectory foundby the planner does not intersect temporally with the trajectories for UAS-2 and UAS-4, thus maintaining collision safety. Inorder to statistically validate the collision safety of the planned trajectories for the five UAS, we simulate trajectories foreach UAS. Figs. 7(b)-(f) show snapshots of the UAS flights with the simulated trajectories and the σ confidence zonotopes forthe predicted state uncertainties. All simulated trajectories for all the five UAS remain within the σ confidence zonotopes,thus demonstrating the applicability of the planner to plan collision safe UAS trajectories in a shared airspace.VII. C ONCLUSIONS
We have presented a trajectory planner that maintains collision safety in the presence of stochastic and bounded sensinguncertainties. We first improved our prior reachability analysis by computing reachable sets as a function of independent quantities.The expression for computing the reachable set was then approximated to predict state uncertainty along the trajectory. Next, wecombined our method to predict state uncertainty with a highly parallel trajectory planning framework. We designed a metricfor the size of the state uncertainty by obtaining the covariation of its confidence zonotopes. Finally, we demonstrated theapplicability of the trajectory planner for GNSS-based navigation of fixed-wing UAS in urban environments. We consideredmultiple scenarios and statistically validated the collision safety of the planned trajectories.R
EFERENCES[1] A. Shetty and G. X. Gao, “Predicting State Uncertainty Bounds Using Non-linear Stochastic Reachability Analysis for Urban GNSS-based UAS Navigation,”
IEEE Transactions on Intelligent Transportation Systems , Accepted, 2020.[2] H. Zhou, H. Kong, L. Wei, D. Creighton, and S. Nahavandi, “On Detecting Road Regions in a Single UAV Image,”
IEEE transactions on intelligenttransportation systems , vol. 18, no. 7, pp. 1713–1722, 2016.[3] R. Ke, Z. Li, S. Kim, J. Ash, Z. Cui, and Y. Wang, “Real-time Bidirectional Traffic Flow Parameter Estimation from Aerial Videos,”
IEEE Transactionson Intelligent Transportation Systems , vol. 18, no. 4, pp. 890–901, 2016.a) (b) (c)(d) (e) (f)
Fig. 7: Planning results for multiple fixed-wing UAS. (a) Planned trajectories for all UAS along with the planner graph. (b)-(f)Snapshots during trajectory execution including simulated trajectories and predicted state uncertainty confidence sets. Thecomplete video can be found at https://youtu.be/eyA3vEojdnQ. [4] S. Jwa, ¨U. Ozguner, and Z. Tang, “Information-Theoretic Data Registration for UAV-based Sensing,”
IEEE Transactions on intelligent transportationsystems , vol. 9, no. 1, pp. 5–15, 2008.[5] H. Zhou, H. Kong, L. Wei, D. Creighton, and S. Nahavandi, “Efficient Road Detection and Tracking for Unmanned Aerial Vehicle,”
IEEE transactionson intelligent transportation systems , vol. 16, no. 1, pp. 297–309, 2014.[6] Y. Xu, G. Yu, X. Wu, Y. Wang, and Y. Ma, “An Enhanced Viola-Jones Vehicle Detection Method from Unmanned Aerial Vehicles Imagery,”
IEEETransactions on Intelligent Transportation Systems , vol. 18, no. 7, pp. 1845–1856, 2016.[7] S. M. LaValle,
Planning algorithms . Cambridge university press, 2006.[8] S. Karaman and E. Frazzoli, “Sampling-Based Algorithms for Optimal Motion Planning,”
The International Journal of Robotics Research , vol. 30, no. 7,pp. 846–894, 2011.[9] J. Van Den Berg, P. Abbeel, and K. Goldberg, “LQG-MP: Optimized Path Planning for Robots with Motion Uncertainty and Imperfect State Information,”
The International Journal of Robotics Research , vol. 30, no. 7, pp. 895–913, 2011.[10] A. Bry and N. Roy, “Rapidly-Exploring Random Belief Trees for Motion Planning Under Uncertainty,” in . IEEE, 2011, pp. 723–730.[11] D. Claes, D. Hennes, K. Tuyls, and W. Meeussen, “Collision Avoidance Under Bounded Localization Uncertainty,” in . IEEE, 2012, pp. 1192–1198.[12] S. Vaskov, U. Sharma, S. Kousik, M. Johnson-Roberson, and R. Vasudevan, “Guaranteed Safe Reachability-based Trajectory Design for a High-fidelityModel of an Autonomous Passenger Vehicle,” in . IEEE, 2019, pp. 705–710.[13] P. Misra and P. Enge, “Global Positioning System: Signals, Measurements and Performance Second Edition,”
Massachusetts: Ganga-Jamuna Press , 2006.[14] S. Peyraud, D. B´etaille, S. Renault, M. Ortiz, F. Mougel, D. Meizel, and F. Peyret, “About Non-Line-of-Sight Satellite Detection and Exclusion in a 3DMap-Aided Localization Algorithm,”
Sensors , vol. 13, no. 1, pp. 829–847, 2013.[15] W. Wen, G. Zhang, and L.-T. Hsu, “Exclusion of GNSS NLOS Receptions Caused by Dynamic Objects in Heavy Traffic Urban Scenarios Using Real-time3D Point Cloud: An Approach Without 3D Maps,” in . IEEE, 2018, pp. 158–165.[16] A. Shetty and G. X. Gao, “Covariance Estimation for GPS-lidar Sensor Fusion for UAVs,” in
Proceedings of the 30th International Technical Meeting ofThe Satellite Division of the Institute of Navigation (ION GNSS+ 2017) , 2017, pp. 2919–2923.[17] G. Zhang and L.-T. Hsu, “A New Path Planning Algorithm Using a GNSS Localization Error Map for UAVs in an Urban Area,”
Journal of Intelligent& Robotic Systems , vol. 94, no. 1, pp. 219–235, 2019.18] A. Shetty and G. X. Gao, “Trajectory Planning Under Stochastic and Bounded Sensing Uncertainties Using Stochastic Reachability,” in
Proceedings ofthe 33rd International Technical Meeting of The Satellite Division of the Institute of Navigation (ION GNSS+ 2020), St. Louis, MO, USA , 2020.[19] B. Ichter, E. Schmerling, A.-a. Agha-mohammadi, and M. Pavone, “Real-time Stochastic Kinodynamic Motion Planning via Multiobjective Search onGPUs,” in . IEEE, 2017, pp. 5019–5026.[20] M. Althoff, O. Stursberg, and M. Buss, “Safety assessment for stochastic linear systems using enclosing hulls of probability density functions,” in . IEEE, 2009, pp. 625–630.[21] M. Althoff, “Reachability Analysis and its Application to the Safety Assessment of Autonomous Cars,” Ph.D. dissertation, Technische Universit¨at M¨unchen,2010.[22] M. Susi, M. Andreotti, M. Aquino, and A. Dodson, “Tuning a Kalman Filter Carrier Tracking Algorithm in the Presence of Ionospheric Scintillation,”
GPS Solutions , vol. 21, no. 3, pp. 1149–1160, 2017.[23] P. N. Raanes, M. Bocquet, and A. Carrassi, “Adaptive Covariance Inflation in the Ensemble Kalman Filter by Gaussian Scale Mixtures,”
Quarterly Journalof the Royal Meteorological Society , vol. 145, no. 718, pp. 53–75, 2019.[24] Y. Yang and W. Gao, “An Optimal Adaptive Kalman Filter,”
Journal of Geodesy , vol. 80, no. 4, pp. 177–183, 2006.[25] G. Costante, C. Forster, J. Delmerico, P. Valigi, and D. Scaramuzza, “Perception-aware Path Planning,” arXiv preprint arXiv:1605.04151 , 2016.[26] B. Wang, W. Shi, and Z. Miao, “Confidence Analysis of Standard Deviational Ellipse and its Extension into Higher Dimensional Euclidean Space,”
PloSone , vol. 10, no. 3, p. e0118537, 2015.[27] W. E. Hoover, “Algorithms for Confidence Circles and Ellipses,”
NOAA Technical Report , 1984.[28] C. Combastel, “Zonotopes and Kalman Observers: Gain Optimality Under Distinct Uncertainty Paradigms and Robust Convergence,”
Automatica , vol. 55,pp. 265–273, 2015.[29] S. Tay and J. Marais, “Weighting Models for GPS Pseudorange Observations for Land Transportation in Urban Canyons,” 2013.[30] H. Hartinger and F. Brunner, “Variances of GPS Phase Observations: The SIGMA-E Model,”