PANTHER: Perception-Aware Trajectory Planner in Dynamic Environments

This paper presents PANTHER, a real-time perception-aware (PA) trajectory planner for multirotor-UAVs (Unmanned Aerial Vehicles) in dynamic environments. PANTHER plans trajectories that avoid dynamic obstacles while also keeping them in the sensor field of view (FOV) and minimizing the blur to aid in object tracking. The rotation and translation of the UAV are jointly optimized, which allows PANTHER to fully exploit the differential flatness of multirotors to maximize the PA objective. Real-time performance is achieved by implicitly imposing the underactuated dynamics of the UAV through the Hopf fibration. PANTHER is able to keep the obstacles inside the FOV 7.9 and 1.5 times more than non-PA approaches and PA approaches that decouple translation and yaw, respectively. The projected velocity (and hence the blur) is reduced by 18% and 34%, respectively. This leads to average success rates three times larger than state-of-the-art approaches in multi-obstacle avoidance scenarios. The MINVO basis is used to impose low-conservative collision avoidance constraints in position and velocity space. Finally, extensive hardware experiments in unknown dynamic environments with all the computation running onboard are presented, with velocities of up to 5.8 m/s, and with relative velocities (with respect to the obstacles) of up to 6.3 m/s. The only sensors used are an IMU, a forward-facing depth camera, and a downward-facing monocular camera.


I. INTRODUCTION AND RELATED WORK
W HILE the last decade has seen an increase on the number of successful deployments of multirotor-UAVs in different real-world scenarios, their applicability is often limited by two common assumptions, namely the fact that the environment is static, and/or the omnidirectional coverage of the sensor(s) of the UAV. Indeed, many UAVs have a limited FOV, and many applications (delivery, aerial videography, emergency response, etc.) have non-static environments due to the presence of cars, people, and/or other UAVs. Hence, relaxing these assumptions is critical to fully exploit the potential of the UAVs and expand the range of their possible applications.
When a UAV equipped with a limited FOV sensor is flying in an unknown environment (e.g., Fig 1), it is crucial to plan both the position and orientation of the UAV to    [8]- [11] A.3 PA planning Decoupling [12]- [14] Joint opt.
B.1 Reduce state estimation uncertainty [13]- [16], [18]- [27] B.2 Record/chase a target [17], [24], [28]- [32] B.3 Avoidance of dynamic obstacles PANTHER maximize the detection and the tracking accuracy of the unknown obstacles while at the same time doing obstacle avoidance. This perception-aware (PA) component is especially important when flying in dynamic environments, because a consistent detection of the moving obstacles is necessary to obtain a good estimate of their locations and prediction of their future trajectories.
Perception-awareness for UAVs has been studied thoroughly in the literature, and, as shown in Table 1, the related work could be classified according to the formulation used and the goal itself. From the point of view of the formulation used, there are approaches that are not PA A.1 , which typically plan the translation and then have either a constant yaw or a yaw such that the FOV of the camera points in the direction of travel (e.g., see [1]- [7]). For instance, [5] used potential fields to avoid dynamic obstacles, but without taking into account perceptionawareness, which can degrade the detection and prediction of the trajectories of the obstacles.
Other approaches are PA by including additional hardware A.2 : For example, by gimbal-mounting the camera, some of its degrees of freedom can be controlled independently of the rotation of the UAV [8]- [10]. Another option is to mount omni-directional sensors [11]. However, these approaches usually require additional hardware and mechanical complexity, which is typically undesirable on small UAVs. PA planning A.3 has received increased attention over the last few years due to its inherent ability to leverage the trajectory planned to maximize the PA objective. The related works could be subclassified according to whether or not the translation and yaw of the UAV are jointly optimized. On one hand, there are approaches that decouple translation and yaw by optimizing them separately [12]- [14]. For instance, in [12], a yaw trajectory is obtained for a fixed translational path to gain information about unknown static obstacles. For features or landmarks whose locations are known a priori, [13] optimizes the time parametrization on a fixed spatial and yaw path to maximize their visibility. In [14], translation is optimized first, and then yaw is optimized to guarantee the co-visibility of the features. While this decoupling of translation and yaw has computational advantages, it can lead to conservative results, since the translational trajectory (and consequently two degrees of freedom of the rotation as well) is fixed in the yaw optimization. Other works assume a downward-facing camera, and hence only translation (not yaw) is planned to keep a specific target in the FOV of the camera [28].
Another approach taken is to jointly optimize translation and yaw, which enables the planner to fully exploit both the position trajectory and the yaw angle [15]- [17]. This joint optimization leads to less conservative results than the approaches that decouple translation and yaw, but it typically comes at the expense of much higher computation times, especially when done in combination with dynamic obstacle avoidance constraints. For example, [15] proposed an onmanifold trajectory optimization approach that couples together translation with the full rotation, but the computation times required (up to 30 s) are not real time. Ref. [16] successfully presented a real-time MPC formulation that keeps the centroid of the VIO features in the center of the image while minimizing its projected velocity. However, this formulation does not include collision avoidance of static (or dynamic) obstacles, which greatly simplifies the complexity of the optimization problem. In [17], translation and yaw are optimized jointly, but only static obstacle avoidance is performed. The technical gap then is how to jointly optimize the full pose of the UAV, satisfy its underactuated dynamics, and guarantee safety in dynamic environments while maintaining real-time computational tractability.
The underactuated dynamics of the UAV (caused by the total thrust of the UAV being fixed in the body frame) makes this joint optimization especially hard, since a given spatio-temporal path fixes two degrees of freedom of the rotation, leaving only one extra degree of freedom in the rotation 1 . A typical way to impose this constraint is via the dynamic equations of the UAV. However, this comes at the expense of having differential equations as constraints in the optimization.
An alternative is to leverage the differential flatness of the UAVs [33] and make use of the map (a ∈ R 3 \ (3) that maps ψ and the acceleration a to the rotation of the body. However, and due to the hedgehog theorem in S 2 [34], [35], there is no single continuous function that defines this map for all possible accelerations a. For the most common definitions of this map, the singularity appears for each ψ at two antipodal points in the unit sphere of possible normalized relative accelerations, which means that there is at least one singularity with a great-circle distance ≤ 90 • with respect to the hovering condition. This closeness between the hovering condition and the singularity can limit the set of possible accelerations in aggressive flights, since an optimal solution that passes through or close to this singularity can provoke numerical instabilities and/or lead to artificial large changes in orientation. Recently, the Hopf map was leveraged in [36] to place the singularity in the inverted ("upside-down") configuration, which is independent of ψ and has the farthest possible angle away from the hovering condition. Although flying highly aggressive trajectories is not the main goal of this work, we decide to use the Hopf map (as opposed to the commonly-used maps presented in [33], [37]) since it automatically maximizes the distance to the singularity by simply changing the definition of the map. In [36], however, the Hopf fibration was only used in the controller to track predefined trajectories. It was also leveraged in [38] to find the set of charts for a previously-optimized position trajectory, which are then used for the controller and to obtain the ψ trajectory. In this work, we propose instead to embed the Hopf fibration in the joint (translation and yaw) coupled planning optimization as a way to directly obtain trajectories in SE(3) that, by construction, satisfy the underactuated dynamics of the UAV.
From the point of view of the goal of the perception awareness, most of the related works focus on reducing the state estimation uncertainty B.1 , usually by keeping specific features/landmarks in the FOV, and/or choosing high-textured areas [13]- [16], [18]- [27]. These features are typically static in the world frame. Some of these approaches also leverage the Observability Gramian [25]- [27], especially when trying to ease the estimation of an unknown parameter of the dynamical system.
Further relevant work addresses the problem of having a UAV record or chase a target B.2 [17], [24], [28]- [32]. For example, [28] focused on tracking a moving target with a downward-facing camera, while [17] proposed a way to follow a moving target while avoiding other static obstacles in the environment. Most of these works focus therefore on chasing a static or dynamic target, not on avoiding it.
Our work differs from these two previous approaches because it proposes the use of PA planning to enhance the avoidance of dynamic obstacles B.3 . Compared to B.1 or B.2 , PA planning to avoid unknown dynamic obstacles comes with many additional challenges, such as the coupling of both the ego-motion and the motion of the obstacle in the visibility cost and blur of the image, the inclusion of dynamic obstacle avoidance constraints in the optimization, the need to predict the future trajectories of the obstacles, and the consideration of the uncertainty of these predicted trajectories, just to name a few.
In summary, the proposed contributions of this work are as follows: • Real-time PA planning formulation that jointly optimizes the translation and the full rotation to maximize the visibility of unknown dynamic obstacles, while simultaneously avoiding them. Compared to non-PA approaches and PA decoupled approaches, our proposed coupled solution leads to a presence of the obstacle in the FOV 7.9 and 1.5 times more frequent, Sigmoid function [39]. ez, 1 ez := 0 0 1 T , 1 := 1 1 1 T FOV, AABB Field of View, Axis-Aligned Bounding Box. SO(n), SE(n) Special orthogonal group, Special Euclidean group. S n n-sphere. wrap π −π (·) Wrapping of an angle in [−π, π) N (·) Normal distribution.
Set of clamped uniform splines with dimension d, degree p, and m + 1 knots.
n (np and nψ ) n := m − p − 1 n + 1 is the number of control points of the spline.
p, v, a, j Position, Velocity, Acceleration, and Jerk of the UAV, ∈ R 3 . All of them are of the body w.r.t. the world frame, and expressed in the world frame.
x State vector: p a Point expressed in the frame a. For the definitions of this table that include the sentence "expressed in the world frame", the notation of the frame is omitted.
Rotation matrix associated with the quaternion q.   The predicted trajectory of the obstacle i, in the world frame, is ∼ N (pi) w (t), (diag (σi(t))) 2 .
f Focal length of the camera in meters. θ Opening angle of the cone that approximates the FOV.
is the trajectory the UAV is currently executing. is the trajectory the UAV is currently optimizing, t ∈ [tin, tf] d ( ) is a point in , used as the initial position of M is a sphere of radius r around d. g ( ) is the projection of g term ( ) onto the sphere M. d, g, and g term are expressed in the world frame. VOLUME 4, 2016 respectively. The success rates achieved are on average 2.98 times larger than other state-of-the-art approaches when flying in multi-obstacle dynamic environments. • We show how the Hopf fibration can be embedded in the planning optimization to jointly optimize translation and yaw while implicitly imposing the underactuated dynamics that couples acceleration and orientation. This avoids the need to explicitly impose the dynamics of the UAV as differential constraints, while automatically guaranteeing the largest possible greatcircle distance between the hovering condition and the differential flatness singularity. Dynamic obstacle avoidance constraints are imposed by leveraging the MINVO basis to reduce conservatism. • Extensive set of hardware experiments in unknown dynamic environments, with everything (navigation, perception, planning, and control) executed onboard the UAV, and without any prior knowledge of the trajectories or specific shape/size of the obstacles. The UAV achieves velocities of up to 5.8 m/s and relative velocities (with respect to the obstacles) of up to 6.3 m/s. The replanning times achieved onboard are ≈ 53 ms. • The code has also been released open source for the community. This paper uses the notation shown in Table 2.

II. PANTHER
PANTHER comprises four modules: Tracker and predictor, selector of the obstacle in the PA term, planes and initial guess generator, and optimization (see Fig. 2A). A summary of how all these modules work together is as follows: First the incoming point clouds of the onboard depth sensor are clustered and tracked using the Hungarian algorithm [41] to obtain the trajectory, as a probability distribution, of each of the obstacles (section II-A). The obstacle i * that the UAV is most likely to collide with is then selected to be included in the PA term of the cost function (section II-B). Then, a kinodynamic search-based planner (Octopus Search Algorithm [42]) is run to find a initial guess of the translational trajectory p(t) that avoids the probabilistic trajectories of the obstacles found before (section II-C1). This translational guess and the obstacle i * selected are then used to run a graph search algorithm to find the ψ(t) guess (section II-C2). Finally, the p(t) and ψ(t) guesses are used for the nonconvex optimization to obtain the optimized trajectory, that is sent to the controller of the UAV (section II-D). In this framework, the coupling between rotation and acceleration is imposed implicitly using the Hopf fibration. All these modules are described in detail in the following subsections.

A. TRACKING AND PREDICTION
We create a k-d tree representation of the point clouds coming from the onboard depth sensor, and perform Euclidean clustering to group the points that are more likely to belong to the same obstacle (see Fig. 2A). For each cluster found, we compute the AABB (Axis-Aligned Bounding Box) centered on the centroid of that cluster 2 . Then, to assign each cluster to a specific track, we minimize the total assignment cost using the Hungarian algorithm [41], where the cost is the pairwise distance between the centroid of each cluster and the prediction of the tracks at the time the point cloud was produced. If this distance is above a specific threshold (usually ≈ 1-2 m), we create a new track for it. If a cluster is not assigned to any track (which can happen if there are more clusters than tracks), then a new track is created for it. Finally, given a sliding window history of all the observations associated with a track, we fit a polynomial for each coordinate {x, y, z}. To capture the stochasticity of the prediction problem, the predicted position at time t is then approximated by a 3D Gaussian distribution (mean from the value of the fitted polynomial and a diagonal covariance matrix obtained from the prediction intervals [44, section 5.7]).

B. SELECTION OF THE OBSTACLE IN THE PA TERM
When there are several predicted trajectories, and to maintain computational tractability, the agent needs to choose which one of them to include in the PA term of the cost function. It does so by choosing the most likely obstacle to collide with in the future, using a simple heuristic of the probability of collision based on Boole's inequality [45]: where U is the number of samples taken, is a point in a straight line from d to g term . Note that although only one obstacle is included in the PA objective function, all the predictions of the tracked obstacles are included in the collision avoidance constraints.
Additionally, and to address the trade-off between gathering information about the obstacle, and gathering information about the direction of travel, the UAV will include the obstacle i * in the PA term if the angle between (g term − d) and Otherwise the UAV will try to align the FOV of the camera with the direction of travel.

C. PLANES AND INITIAL GUESSES 1) Separability planes and initial guess for position
We use the Octopus Search Algorithm (OSA) [42], which is a search-based algorithm that operates directly on the control points of the position spline. It ensures collision-free constraints between the agent and the dynamic obstacles by finding the planes that separate the inflated MINVO polyhedral representation of each interval j of the trajectory of the obstacle i (denoted as C MV ij ) and the MINVO polyhedral Hopf fibration and its stereographic projection, partly inspired from [43]. Given a specific relative acceleration ξ (withξ = −e z ), the quaternion q w b = q ξ • q ψ is a fiber (specifically a circle) in S 3 parameterized by ψ. On the bottom right, the body frames for different values of ψ i for each ξ i are shown. VOLUME 4, 2016 representation of that interval j of the trajectory of the agent, denoted as Q MV j agent (see Fig. 2B). The outputs of this algorithm are both the position control points and the planes π ij (given by n T ij x + d ij = 0) ∀i, ∀j. The position control points are then used as initial guess in the optimization, while the planes π ij are held fixed in the optimization. The reader is referred to our previous work [42] for a more indepth explanation of the OSA.

2) Initial guess for ψ
To obtain the initial guess for ψ, we uniformly sample the position guess spline obtained through the OSA, and for each of these position samples, we uniformly sample several values of ψ ∈ [−π, π). Each one of these p-ψ samples will be a node, and all the nodes associated with the same position sample, but with different ψ, will constitute a layer (see Fig. 2A). Then, we create a graph connecting with directed edges all the nodes of one layer to the nodes of the next layer [12]. Each node has therefore a time, position, acceleration, and yaw associated with it, and all the nodes of the same layer have the same time, position, and acceleration. The cost of the edge between two nodes n 1 and n 2 of the graph is then given by Here, c ψ , c Ψmax , and c FOV are nonnegative weights, while ψ nu , t nu , and (T nu (t nu )) w c are the angle ψ, the time, and the transformation matrix associated with node n u . Note that the edge cost is guaranteed to be nonnegative at all times. The transformation matrix can be directly obtained from the position, acceleration, and yaw of the node. The first term in the cost penalizes the distance between two ψ angles, the second term penalizes edges that do not satisfy the limit Ψ max , and the last one rewards the visibility of the obstacle. The units of the weights above are such that the corresponding term is dimensionless (see section III). To choose these weight values, we first set c Ψmax to a large value to guarantee the Ψ max constraint. Then, c ψ and c FOV are selected as a trade-off between smoothness and inclusion of the obstacle i * in the FOV of the UAV. The root node of the graph corresponds to the state d (see last row of Table 2). We solve the search problem using Dijkstra's algorithm [46], with early termination when the search reaches a node of the last layer. Letting Λ denote the indexes of the nodes of the path found, we shift the angles ψ n λ ∀λ ∈ Λ (by adding or subtracting 2πr, r ∈ Z) such that the absolute difference between two consecutive angles is ≤ π. Usingψ n λ to denote these shifted angles, a spline is fitted to these angles by solving the following constrained least square problem: Note that, as this problem is a quadratic program with linear equality constraints, its solution can be easily found by simply solving the linear Karush-Kuhn-Tucker (KKT) conditions associated with it [47], [48] 3 . The control points of this fitted spline are then used as the initial guess for ψ(t) in the optimization.

D. OPTIMIZATION 1) Coupling rotation and acceleration with the Hopf fibration
In a standard multirotor-UAV, the perpendicularity of the total thrust with respect to the plane spanned by b 1 and b 2 (see the coordinate frames shown in Fig. 2C) makes the UAV underactuated by imposing the following constraint [36]: whereξ is the normalized relative acceleration expressed in the world frame (see Table 2). In a planning optimization problem where rotation and translation are jointly optimized, (2) needs to be satisfied at all times. A very common way to guarantee (2) is via direct imposition of the dynamic equations of the UAV as explicit constraints. However, these differential equations in the optimization problem typically lead to computationally-expensive problems, due to the fine sampling needed in the discretization methods (shooting or collocation). The direct imposition of the dynamic equations can be avoided by leveraging the differential flatness map (a ∈ (3), which takes the acceleration a and ψ and maps them to the rotation of the body frame. Due to the hedgehog theorem 4 in S 2 [34], [35], this map is guaranteed to have at least one singularity when tried to be defined with a single continuous function. Several possible definitions of this differential flatness map are shown in Table 3, all of which satisfy (2) by construction. In the first two definitions, one body axis is obtained as the cross product of b 3 ≡ξ with a vector lying in the xy world plane, and the remaining body axis is such that the resulting body frame is right-handed. These two definitions present a singularity whenever the normalized relative acceleration ξ ∈ S 2 is parallel to a vector defined by ψ which lies in the xy world plane. This means that, for a given ψ, the singularity appears for twoξ that have a great-circle distance of 90 • with respect to the hovering condition. In aggressive flights, and due to numerical instabilities and artificial large changes of orientations near the singularity, this closeness between the hovering condition and the singularity can limit the set of possible accelerations for the planner.  3: Some commonly-used definitions for the differential flatness map (a ∈ R 3 \ [0 0 − g] T , ψ ∈ S 1 ) → R w b ∈ SO(3). The colormap represents the great-circle distance to the closest singularity (yellow is closer), (·) n denotes the normalization of a vector, andξ := ([a x a y a z + g] T ) n is the normalized relative acceleration, expressed in the world frame. See also [13], [38], [50]- [52] for more possible definitions, which are usually rotations of the first two definitions of this table. The third definition of Table 3 leverages the Hopf fibration h(·), which can be defined as a map S 3 → S 2 [36], [43] that takes a unit quaternion q and produces the resulting rotation of the vector e z := 0 0 1 T (see Fig. 2D):

Definition 1 Definition 2 Definition 3 (Hopf fibration)
Making use now of the inverse image of the Hopf fibration, we have that q w b will be a composition of two rotations 5 : q ξ , that aligns b 3 with ξ, followed by q ψ , which is a rotation around ξ by an angle ψ. Given a specific ξ (with ξ = −e z ), the quaternion q w b = q ξ • q ψ will then be a fiber (specifically a circle) in S 3 parametrized by ψ [43]. The main advantage of the Hopf fibration over the previous two definitions is that the singularity only occurs when the UAV is inverted (i.e., whenξ = −e z ), which is the orientation that has the largest possible great-circle distance from the hovering configuration, and hence much less likely to happen. Although the goal of this paper is not to plan highly aggressive trajectories, and hence any of the singularities shown in Table 3 are unlikely to be reached, we use the Hopf map to automatically ensure the maximum distance to the singularity. Note also that, with the Hopf fibration, a second chart could be used to cover the inversion pointξ = −e z , but the use of multiple charts, while computationally cheap in the controller level [36] or in an intermediate check step in a decoupled p-ψ optimization [38], would significantly 5 Note that q ψ , q ξ , and q w b are guaranteed to be unit quaternions by construction.
increase the computation time when embedded in the pψ joint planning optimization. This fact, together with the improbability of an upside-down configuration as being PA optimal, led us to the inclusion of only the first chart.
Our work differs from other works that have used the Hopf fibration for UAVs [36], [38], [53] as follows: Ref. [36] uses the Hopf fibration only in a controller to track predefined trajectories. In [38], the Hopf fibration is used in the planner to find the charts in a step after the p optimization and before the ψ optimization, and [53] does not optimize ψ. We instead propose to embed the Hopf fibration map directly on the p-ψ joint optimization, as a way to directly obtain trajectories in SE(3) that are dynamically feasible by construction, and with the crucial advantage of not needing to explicitly impose the dynamic equations as constraints in the optimization.

2) Cost function
A PA term in the objective function should maximize the presence in the FOV of the predicted position of the obstacle i * ∈ I. However, this alone is not enough to guarantee good PA trajectories, since a fast moving projected obstacle in the image plane may cause significant blur, which can lead to stereo matching failure and consequently tracking failure. To take into account both the presence in the FOV and the blur, we use inFOV(·) 1+ 2 ṡ 2 as the running reward, whereṡ is the projected velocity in the image plane, and where 1 and 2 are nonnegative parameters such that 1 + 2 > 0. Note how this reward is high if the predicted position of the obstacle is in the FOV with a small projected velocity, and it is VOLUME 4, 2016 approximately zero if the predicted position of the obstacle is not in the FOV, regardless of the value of the projected velocity. The position in the image plane of the projection of the obstacle can be obtained using the pinhole camera model as c ] x:y (where each component of s is expressed in meters, not in pixels), and As detailed in Table 2, the discontinuity of the function inFOV(·) is addressed by approximating it with a sigmoid function.
In addition to the PA term explained above, we also add two terms in the cost function to maximize the smoothness in position (by minimizing jerk) and ψ (by minimizingψ), and a terminal cost that penalizes the distance between p(t f ) and g.

3) Collision avoidance and dynamic limits constraints
For the obstacle avoidance of dynamic obstacles, we first create a polyhedral outer representation of both the trajectory of the agent and of the obstacle (see Fig. 2B): For the agent, we make use of the MINVO basis [54] (a polynomial basis that finds the simplex with minimum volume enclosing a polynomial curve) to obtain the set of control points Q MV j agent whose convex hull encloses each segment j of the agent. Similarly, for each obstacle i, we first compute the MINVO control points of the segment j of the predicted mean (p i ) w (t), and then we inflate it with norminv(δ) · σ i (t end j ), half of the sides the AABB (axisaligned bounding box) of the obstacle i and half of the sides of the AABB of the agent. Here, δ ∈ [0, 1] is the percentile of the standard normal distribution, and hence it encodes the desired level of conservativeness in the inflation. The resulting polyhedron is denoted as C MV ij . To ensure safety between the agent and the obstacle i, we then impose linear separability constraints (via planes) between Q MV j agent and C MV ij . The separating planes are found during the initial guess search for the position spline (see section II-C1), and are held fixed in the optimization. The MINVO basis is used in a similar way to impose low-conservative constraints in the velocity space. In the acceleration and jerk spaces, the MINVO control points are the same as the B-Spline control points. These constraints on v, a, j, andψ serve as a conservative approximation of the real actuator constraints of the motors of the UAV, while allowing us to reduce the complexity of the optimization problem.

4) Optimization problem
Including the initial state and the final hovering condition, the optimization problem is 6 : nonnegative weights, and the decision variables are the control points of the splines p(t) and ψ(t). The degrees chosen for the splines p(t) and ψ(t) are, respectively, 3 and 2, which are a good trade-off between computation time and dynamic feasibility for a UAV [33]. The units of the weights are such that the corresponding term is dimensionless (see section III). An empirical method to select these weight values is as follows: First set α g to a large value to ensure that the final location is near g. Then, α FOV , together with 1 and 2 , are tuned to obtain a good presence of the obstacle i * in the FOV. Finally, α j and α ψ are progressively increased to improve the smoothness of p(t) and ψ(t), without significantly deteriorating the FOV cost.
To solve this optimization problem, we utilize the Interior Point Optimizer Ipopt [55] 7 interfaced through CasADi [57] with MA27 and MA57 [58] as the linear solvers of Ipopt. All these optimization tools were installed and run onboard the UAV in the real-world experiments (section III-B). We approximate the PA term of the cost function using the composite Simpson's rule for numerical integration [59].

A. SIMULATION EXPERIMENTS
All the simulation experiments are run in an AlienWare Aurora r8 desktop running Ubuntu 20.04 and equipped with an Intel ® Core TM i9-9900K CPU, 3.60GHz×16 and 62.6 GiB. Moreover, and to focus the comparisons on the properties of the trajectories obtained by the planner, we assume, for all the algorithms benchmarked in simulation, 6 Time dependence of the variables in the cost function has been omitted for simplicity. 7 We classify an Ipopt solution as successful when Ipopt returns Solve_Succeeded (locally optimal solution) or Solved_To_Acceptable_Level (solution satisfying the acceptable tolerance level). For more details, see [56]. In the left plot of both subfigures, represents the number of infeasible stops of algorithm [6]. The other algorithms have zero infeasible stops. VOLUME 4, 2016 that the UAV can perfectly track the trajectories obtained by the planner.

1) Single obstacle
We first test PANTHER in an environment with a boxshaped obstacle of size 0.2 × 0.2 × 0.2 m 3 that follows a trefoil-knot [60] trajectory. During 60 s, the UAV is commanded to continuously fly between two different locations whose centroid is the area where the obstacle is moving. The camera has an image size of 120 × 120 px 2 , a limited FOV of 60 • × 60 • , and runs at a rate of 60 Hz. The weights used for this simulation are c Ψmax = 10 6 , c FOV = 1, c ψ = 0 rad −2 , α j = 10 −6 s 5 /m 2 , α ψ = 0 s 3 /rad 2 , α FOV = 20, α g = 70 m −2 , 1 = 0.3, and 2 = 0.45 s 2 /m 2 . To focus this comparison on the capabilities of the planner, we let the agent perfectly know the trajectory of the obstacle in these simulations. We compare the following three approaches: 1) No PA: ψ is held constant and only the smoothness in position and terminal goal costs are optimized. Works that do not plan ψ include, e.g., [1], [5], [7]. 2) PA with position and ψ decoupled: Translation p is optimized first (as in the method no PA) and then it is held fixed while ψ is optimized with the PA term. We will refer to this algorithm as PA dec. This decoupling is done in, e.g., [12]- [14]. 3) PANTHER (ours): Joint optimization of p and ψ.
As will be explained in section II-D2, two important metrics that characterize a good PA trajectory are the presence of the obstacle in the FOV and the norm of the projected velocity, which quantifies the blur. The percentage of time the obstacle was in the FOV of the camera is shown in Fig. 3B. PANTHER is able to keep the obstacle inside the FOV 7.9 and 1.5 times more than the algorithms no PA and PA dec, respectively. As PA dec decouples position and ψ in the optimization, the UAV lacks the ability to modify the spatial path (only ψ) to generate a better overall trajectory.
To qualitatively show the area of the projection, we apply a Gaussian filter to the histogram of the projection of the centroid of the obstacle onto each 10 × 10 px 2 cell of the image plane. The results are shown in Fig. 3A, where we can see that PANTHER is able to keep the obstacle inside the FOV limits much better, and more frequently, than methods no PA and PA dec.
The velocity of the projection of the centroid of the obstacle onto the image plane is shown in Fig. 3C, which highlights that PANTHER is able to obtain a 18% and 34% decrease in the mean of the norm of the projected velocity with respect to no PA and PA dec, respectively, achieving, therefore, a much less blurred projection of the obstacle than those two methods.
Finally, and as a continuous detection of the dynamic obstacle is crucial to achieve a good tracking and prediction, we show in Fig. 3D the boxplot of the number of frames of each continuous detection for the different algorithms. A continuous detection is defined as a set of consecutive frames for which the obstacle stayed in the FOV of the camera. On average, PANTHER is able to achieve continuous detections of 155 frames, while the mean number of frames per continuous detection for methods no PA and PA dec are 39 and 46 frames, respectively.

2) Several obstacles
We now test PANTHER in a simulation with several obstacles. The environment consists of a corridor of length of 39 m along the x direction with five dynamic obstacles that move following random trefoil-knot trajectories [60], see For the benchmark, we use the algorithms explained before (no PA, PA dec, and PANTHER), and the two additional algorithms: • Algorithm [6], proposed by Wang et al. This approach is not perception aware, but ψ tries to make the FOV of the camera point to the direction of travel. We will refer to this algorithm as Wang. Note also that this algorithm does not have constraints on j max and that it has a different ψ convention (it uses definition 1 of Table 3). • ψ sweep: ψ follows a sinusoidal trajectory that varies in [−90 • , 90 • ] as follows: We test two scenarios with different maximum velocities of the obstacles. In the slow scenario, the obstacles move with velocities up to 2.12 m/s, while in the fast scenario, the obstacles move with velocities up to 4.07 m/s. In the results, we compare the number of collisions, infeasible stops, success rate, flight time, and flight distance. An infeasible stop happens when the drone passes instantly from a nonstop condition (v = 0 or a = 0) to a stop condition (v = 0 and a = 0). A run is considered successful if the UAV is able to reach the end of the corridor while not colliding with any of the obstacles. To make these simulations closer to real-world applications, where no prior information about the trajectories of the obstacles may be available, a simple constant velocity model is used in the predictor. The obstacles themselves are moving along trefoil-knot trajectories [60].
The results, for 30 different runs per algorithm, are shown in Figs. 3F and 3G for the slow and fast environments, respectively. In the slow scenario, PANTHER is able to succeed 87% of the runs, while the other algorithms have a success rate below 47%. None of the algorithms present infeasible stops except Wang, that has a mean of 0.2 infeasible stops per run (light purple in Fig. 3F). In the fast scenario, PANTHER succeeds 70% of the runs, while all the other algorithms have a success rate below 27%. In terms of flight times and flight distances, most of the algorithms achieve very similar results in both scenarios, with a total flight time of approximately 20 s, and an approximate total flight distance of 41 m. The total flight distance for PANTHER is approximately 3 m more than the rest of the algorithms. This is expected, because PANTHER has the ability to modify the spatial path to maximize the visibility of the obstacles. Even with this longer flight distance, the flight time of PANTHER is very similar (and sometimes even shorter) than the rest of the algorithms.

3) Computational analysis of the replanning step as a function of the number of obstacles
We now compare the computational cost of different parts of the replanning step of PANTHER. As the computational cost of each part highly depends on the specific position of the obstacles relative to the UAV, we perform a Monte Carlo analysis by randomly deploying obstacles (which follow trefoil-knot trajectories) in the spherical shell To obtain the ψ initial guess (section II-C2), the average runtime of the Dijkstra's algorithm on the ψ graph is 0.137 ms, and the average runtime to fit a spline to the ψ samples (Eq. 1) is 0.048 ms.
These results above show the computational analysis for the different parts of the replanning step of PANTHER (convex hull computation, generation of the p(t) and ψ(t) initial guesses, and nonconvex optimization). For the computational cost of the tracker and predictor using real point clouds, see section III-B. The well-known results regarding the complexity analysis of the Hungarian algorithm are given in [41], [62].

B. REAL-WORLD EXPERIMENTS
We run an extensive set of hardware experiments, where a UAV needs to go from a starting point to a goal location while avoiding unknown dynamic obstacles. The UAV used is equipped with a Qualcomm ® SnapDragon Flight, an Intel ® NUC i7DNK, and an Intel ® RealSense Depth camera D435i. The tracker, planner, and the camera run on the Intel ® NUC, while the control and state estimation run on the Qualcomm ® SnapDragon Flight. Note that the main  Figure 6: Snapshots of the onboard camera in experiments 3 (A), 6 (B), and 9 (C). (D) Computation times for each part of a replanning step, measured on the onboard Intel ® NUC i7DNK. The tracker, predictor, and the depth camera were also running on this computer at the same time these times were measured. The notation used is: CHs (convex hull computation for the polyhedral outer representations), Gp (generation of the planes and the guess for the position p(t)), Gψ (generation of the guess for ψ(t)) and Opt (Optimization time). VOLUME 4, 2016 onboard computer (Intel ® NUC) has similar computational power to the onboard hardware used in the recent literature (e.g., [1], [6], [7], [12]). For the controller, we run the approach presented in [36], [63] at 100 Hz to generate the desired orientation and angular rates from p(t) and ψ(t). The commanded thrusts for the motors are then found from these attitude commands using a geometric controller [64], which is run at IMU rate (500 Hz). For state estimation, we use a visual inertial odometry (VIO) package [65] running at 30 Hz that leverages an extended Kalman filter to fuse the IMU measurements of the SnapDragon and the images of its downward-facing camera. To obtain a high-rate state estimate, we then integrate forward the IMU (which runs at 500 Hz) between consecutive VIO estimates.
To generate the dynamic obstacles, we use three other UAVs with a Qualcomm ® SnapDragon Flight, and equip them with a box-shaped frame of ≈ 0.6 × 0.6 × 0.3 m 3 . The obstacles are following trefoil-knot trajectories [60].
A total of 9 experiments were performed (see attached video). The composite images of the trajectories flown by the agent and by the obstacles, together with the number of obstacles, distance flown, maximum velocity, maximum relative velocity with respect to the obstacles, and total flight time of each one of the experiments are shown in Fig. 5A. Experiments 1 and 2 were done with one obstacle, experiment 3 with two obstacles, and experiments 4-9 with three obstacles. The maximum velocity achieved by the agent, 5.77 m/s, happened in experiment 7. In that same experiment, the maximum relative velocity (6.28 m/s) with respect to the obstacles is also achieved. The relative distances between the UAV and the obstacles are shown in Fig. 5B. Any relative distance above the dashed horizontal line guarantees safety between the agent and the corresponding obstacle. For experiments 3, 6, and 9, different snapshots of the onboard camera are shown in Figs. 6A, 6B, and 6C, respectively. Note how the planned trajectories try to keep an obstacle in the FOV at all times to aid in obstacle tracking and prediction.
The computation times are shown in Fig. 6D. All these computation times were measured onboard, with the UAV flying, and with the depth camera node and the tracker running on the same computer (Intel ® NUC i7DNK). The mean total replanning times are 48.70, 51.66, and 58.59 ms for the experiments with 1, 2, and 3 obstacles respectively. The point cloud of the camera is generated at 90 Hz, and the tracker (clustering, assignment, and prediction) is able to process each point cloud in ≈ 8.6 ms.

IV. CONCLUSION
This work derived PANTHER, a perception-aware (PA) trajectory planner in dynamic environments. PANTHER is able to couple together the translation and the full rotation in the optimization, leading to PA trajectories computed in real time that maximize the presence of the obstacles in the FOV while minimizing their projected velocity. Extensive hardware experiments in unknown dynamic environments, with all the computation running onboard, and with relative velocities of up to 6.3 m/s have shown its effectiveness.
Our approach has also some limitations. Specifically, in the hardware experiments we observed the importance of the choice of the obstacle to include in the optimization (i.e., the choice of i * , see Table 2 and section II-B): when should the UAV include a specific (already tracked) obstacle in the PA term of the optimization, in order to predict its trajectory more accurately to be able to avoid it, and when should the UAV turn around to explore unknown space? This highlights the trade-off between exploration and exploitation: too much focus on exploitation may lead to collision with obstacles that were never detected, and too much focus on exploration may lead to a very poor trajectory prediction, and hence to a collision as well. Optimally solving this trade-off is a promising direction for future work.
Another possible direction of future work is to solve the trade-off between visibility and time optimality. This would entail adding the time minimization in the optimization problem of section II-D4, and would also allow to highlight the advantages of the Hopf fibration when flying aggressive trajectories that pass close to the singularity produced by the commonly-used maps presented in [33], [37] (first two definitions of Table 3).
Finally, another interesting research direction is how to incorporate disturbances in the planning problem, while still guaranteeing that the tracking error of the UAV remains bounded [66]. The incorporation of such disturbance information is especially important when flying outdoors under windy conditions, since a large deviation between the planned trajectory and the actual trajectory can provoke a collision with the obstacles.