Online Trajectory Generation of a MAV for Chasing a Moving Target in 3D Dense Environments

Online Trajectory Generation of a MAV for Chasing a Moving Target in 3D Dense Environments

Online Trajectory Generation of a MAV for Chasing a Moving Target in 3D Dense Environments

I Introduction

Ii Related Works

Iii Problem Statement

Iv Viewpoints Generation

V Smooth Trajectory Generation

Vi Validation Result

Vii Conclusion

References

Authors

I Introduction

Ii Related Works

Iii Problem Statement

Iv Viewpoints Generation

V Smooth Trajectory Generation

Vi Validation Result

Vii Conclusion

References

Authors

Related Research

I Introduction

Ii Related Works

Iii Problem Statement

Iv Viewpoints Generation

V Smooth Trajectory Generation

Vi Validation Result

Vii Conclusion

References

Ii-a Chasing in obstacle environments

Ii-B Safe trajectory generation with pre-planning

Iii-a Problem settings

Iii-B Capabilities

Iii-C Nomenclature

Iv-a Visibility metric

Iv-B Preplanning with safety and visibility

Iv-C Chasing corridor generation

Integrated Motion Planner for Real-time Aerial Videography with a Drone in a Dense Environment

Search-based 3D Planning and Trajectory Optimization for Safe Micro Aerial Vehicle Flight Under Sensor Visibility Constraints

Visibility-aware Trajectory Optimization with Application to Aerial Tracking

FAR Planner: Fast, Attemptable Route Planner using Dynamic Visibility Update

Elastic Tracker: A Spatio-temporal Trajectory Planner Flexible Aerial Tracking

Perception-Aware Perching on Powerlines with Multirotors

Strategy Synthesis for Surveillance-Evasion Games with Learning-Enabled Visibility Optimization

Ii-a Chasing in obstacle environments

Ii-B Safe trajectory generation with pre-planning

Iii-a Problem settings

Iii-B Capabilities

Iii-C Nomenclature

Iv-a Visibility metric

Iv-B Preplanning with safety and visibility

Iv-C Chasing corridor generation

Integrated Motion Planner for Real-time Aerial Videography with a Drone in a Dense Environment

Search-based 3D Planning and Trajectory Optimization for Safe Micro Aerial Vehicle Flight Under Sensor Visibility Constraints

Visibility-aware Trajectory Optimization with Application to Aerial Tracking

FAR Planner: Fast, Attemptable Route Planner using Dynamic Visibility Update

Elastic Tracker: A Spatio-temporal Trajectory Planner Flexible Aerial Tracking

Perception-Aware Perching on Powerlines with Multirotors

Strategy Synthesis for Surveillance-Evasion Games with Learning-Enabled Visibility Optimization

Ii-a Chasing in obstacle environments

Ii-B Safe trajectory generation with pre-planning

Iii-a Problem settings

Iii-B Capabilities

Iii-C Nomenclature

Iv-a Visibility metric

Iv-B Preplanning with safety and visibility

Iv-C Chasing corridor generation

This week in AI

Due to the enhanced autonomy from localization [2, 3], tracking [4] and control [5], MAVs have been widely utilized for the vision-based tasks such as videography and surveillance. Especially, planners proposed in the previous research ([1, 8, 9]) improved the performance of autonomous target following with real-time applicability. Although those methods can be reasonably employed in obstacle-free or sparse situations, there remain issues for the general cluttered environments.

The presence of multiple obstacles poses two main difficulties in the chasing task: collision and occlusion which should be handled simultaneously. Aside from the safety consideration, occlusion handling especially becomes critical in a situation where a chaser does not have perfect information on the future movement of a target. In the case, the target visibility over the future time horizon is not guaranteed although it is visible from chaser at current time. Additionally, if the chaser spends too much resource (e.g. travel distance) in preparation for the visibility in the future horizon, inefficient motion will be caused.

To overcome the issues, a chasing planner needs to optimize visibility and travel efficiency with safety guarantees. Also, fast computation is essential to effectively respond to the uncertainty of target movement.

Fig. 1: Upper: chasing mission in complex city
example. The target moves on the ground following red-coloured line. Bearing vectors connecting the target and chaser are stamped with the visibility score in the jet colormap scale (Sec.
IV-A,VI). Lower: chasing trajectories obtained from different weights of visibility $w_{v}$ (Sec. IV-B). In (b), $w_{v}$ is set to a larger value than (a). The chasing corridor (cyan) can be adjusted based on the relative importance of travel efficiency and visibility while ensuring safety. As depicted in dashed boxes (A),(B), different chasing behaviors were obtained for different $w_{v}$ . In case of lower $w_{v}$ , the target was not visible in dashed box region (A) although the chaser travels with shorter distance. The strategy of higher $w_{v}$ efforts to achieve a more safe viewpoint against the uncertain target movements after $t_{4}$ (box region (B)).

In this paper, we propose an online chasing planner which handles occlusion and collision in an integrated way against dense obstacles.

Our key contributions are twofold. Firstly, we introduce a visibility metric for a moving target which can be defined in general environments without an restriction on the shape of obstacles. With a simple computation, the metric encodes the sensitivity of visibility against uncertainty of future movement of the target. Secondly, a cascaded chasing strategy with real-time performance is presented. Rather than adopting non-convex optimization with multiple objectives, we seed sparse viewpoints which guarantee safety and visibility by means of an graph-search method. Based on the discrete viewpoints skeleton, piecewise polynomials are generated in quadratic programming (QP) formulation with additional safe consideration. The planner guarantees the safety of the entire chasing path while adjusting the importance between travel efficiency and visibility. We found that the cascaded planner ensures the chasing quality of trajectory without sacrificing the online computational speed.

The research [14], [15] dealt with the occlusion and collision in chasing task based on a formulation in image space. [14] took advantage of quadratic programming (QP) to compute a control input to avoid occlusion from a single polyhedral occluder. A switching controller was used in [15], which is composed of multiple objectives (a nominal tracking, collision avoidance and occlusion handling). Although the formulations in the mentioned works do not require the reconstruction of target in 3D space, the methods were validated either in a single obstacle or sparse environments.

In contrast to [14], [15], a group of research [6, 7, 16, 17] used Cartesian space formulation. Those works employed 3d reconstruction process to deal with more complex scenarios by the virtue of reliable mapping and localization. To account the occlusion into consideration, the authors in [17] introduced a risk function which is defined as the shortest distance for the target to escape the sensor range of chaser to evaluate the visibility. Although the algorithm was validated against multiple obstacles, it has to calculate the precise geometry of obstacles for occlusion culling. [16] employed safe corridor generation method [11] for collision avoidance for dynamic target chasing. They examined the trade-off between flight efficiency and collision avoidance. Despite of safe flight avoiding obstacles, visibility for a target was not considered, without guarantee for maintaining target in the sight.

More recently, [6, 7] proposed planning strategies which consider occlusion and collision simultaneously in obstacle environments. [6] defined the cost functions for visibility and safety consideration based on the assumption of the ellipsoidal shape of occluder. [7] formulated an optimization problem under octomap framework ([18]) accommodating more general shape of obstacles. In order to measure visibility of a target, the authors calculated the integral of signed distance field (SDF) over a 2D manifold.

The works [6, 7] both defined nonlinear cost function and optimized it with gradient descent methods. Although these approaches guarantee the dynamic feasibility of MAVs and compute the solution online, they are subject to the local optimal due to non-convexity of multi-objectives cost function to account for safety and visibility simultaneously. It is especially ineffective to entirely rely on the non-convex formulation in more dense environments as pointed out in [13].

In this subsection, we mention several recent research on safe planning to reach a static goal while avoiding obstacles. Although its mission is different from ours, it is worthy to note the approaches developed in [10, 11, 13] to deal with dense environments. The works adopted a cascaded planner where a sequence of sparse waypoints or together with safe corridor is seeded in the first phase, which is followed by continuous trajectory completion. Let us call the first process as preplanning. The discrete optimization such as search-based [19] or sampling based methods [20] were used for the preplanning phase. The output of it acts as a good initial guess or constraints for the continuous optimization circumventing the local solutions of poor quality. The applicability of the cascaded planners in the mentioned works were validated in complex environments enjoying safety, dynamic feasibility and stability in optimization. Inspired by the approaches, we develop a preplanner for the autonomous chasing which is effective for the more challenging cases.

Fig. 2: Overview of the planning pipeline for the proposed chasing strategy. This work focuses on the motion planner which consists of two phases (i.e. two boxes in the right side) in response to the update of information. The preplanning process (green dashed box) generates the sequence of viewpoints which are safe and visible. The lines connecting each point also guarantee the safety clearance from obstacles, from which a set of corridors are obtained. Based on the corridors and viewpoints, a smooth chasing path is computed with convex optimization (QP).

In our chasing setting, multiple obstacles of arbitrary shape are involved. The environment is assumed to be known as a priori. MAV (chaser) is assumed to be equipped with a vision sensor with finite FOV for the purpose of target tracking and state estimation. In this paper, these processes are assumed to be reliable (

[2, 4]) and we focus on the motion strategy for chasing. Regarding a moving target of interest, it is assumed to have bounded accelearation, and is located in the FOV of the MAV initially. The entire path of the target is not available a priori to the chaser. Instead, the chaser is provided with the future movement of the target over only short horizon, which is repeatedly updated in the air in a similar manner with [8, 16].

Based on these assumptions on the chaser and target, we aim to build a chasing planner which has the following capabilities.

Safety of entire chasing path is guaranteed.
Visibility of target is guaranteed at pre-defined time steps.
Visibility of target and total travel distance is optimized.
The integral of high order derivatives of chasing path is minimized for the smoothness and flight efficiency of the path.
Real-time performance of planner is achieved in response to the online information updated (i.e. the left side in Fig. 2).

Before preceding, we define several notations for the ease of discussion.

$x_{c} \in R^{3}$ : Position of a chaser.
$x_{p} \in R^{3}$ : Position of a target.
$L (x_{1}, x_{2}) = {x | t x_{1} + (1 - t) x_{2}, 0 \leq t \leq 1}$ : The line segment connecting $x_{1}, x_{2} \in R^{3}$ .
$B (p, l) = {x | - l \leq x - p \leq l} (p, l \in R^{3})$ : The set of points in the axis-parallel 3D box represented by scale $l$ and center position $p$ .
$χ \subset R^{3}$ : Configuration space.
$χ_{f r e e} = {x | P (x) < ϵ}$ : Free space in $χ$
, i.e. the set of points where the probability of occupancy
$P (x)$ obtained from octomap is very small.
$χ_{o b s} = χ ∖ χ_{f r e e}$ : Space occupied by obstacles.
: A set of visible vantage points for a target position $x_{p}$ .
$χ_{o c c} (x_{p}) = χ ∖ χ_{v i s}$ : A set of occluded points for a target position $x_{p}$ .

In this subsection, we explain the metric for visibility of $x_{p}$ to be maximized for the robustness of chasing. We assume that the chaser has map information $χ$ as stated in the Sec. III-A. For simplicity, we assume that the chaser takes $x_{p} - x_{c}$ as its bearing vector.

We first consider the level set function $ϕ (x)$ for $x \in R^{3}$ to represent the obstacle region $χ_{o b s}$ subject to $ϕ (x) \leq 0$ for $x \in χ_{o b s}$ and $ϕ (x) > 0$ for $x \in χ_{f r e e}$ . $ϕ (x)$ is chosen as Euclidean distance field EDF( $x; χ_{o b s}$ ) which can be computed efficiently by the methods such as [21]. Based on $ϕ (x)$ , we define the visibility of target $x_{p}$ when observed from the position $x$ with bearing vector $x_{p} - x$ as below.

ψ (x; x_{p}) = min L (x, x_{p}) ϕ (x)

(1)

Fig. 3: Grid field of $ϕ (x)$ (left) and $ψ (x; x_{p})$ (right) in a 2D case. In the figure, a target position $x_{p}$ is denoted as a blue circle with velocity(magenta quiver) and chaser $x_{c}$ as a camera icon. $ϕ (x)$ corresponds to the closest distance to the ambient obstacles while $ψ (x; x_{p})$ is the closest distance between bearing vector $x_{p} - x_{c}$ and obstacles as the red lines show in the right figure. Following the definition, chaser $x_{c, 1}$ secures higher visibility score than $x_{c, 2}$ indicating that $x_{c, 1}$ is more robust for observation against the future movement of $x_{p}$ . We will use $ϕ (x)$ as a measure for collision safety and $ψ (x; x_{p})$ as visibility of $x_{p}$ .

The interpretation of (1) is the closet distance between the line segment $L (x, x_{p})$ and elements in $χ_{o b s}$ because $ϕ (x)$ is equivalent to EDF( $x; χ_{o b s}$ ) as illustrated in Fig 3. Because (1) is a level set representation for the occlusion region and visible region with respect to $x_{p}$ , $χ_{v i s} (x_{p})$ can be expressed as a set ${x | ψ (x; x_{p}) > 0}$ and $χ_{o c c} (x_{p})$ with ${x | ψ (x; x_{p}) \leq 0}$ . The concept of the visibility level set (1) was used in several literature for occlusion rendering [23] to compute the visible volume of environment. As we focus on a chasing mission, we will leverage $ψ (x; x_{p})$ to gauge how reliably a chaser position $x$ can maintain visibility for the target $x_{p}$ .

Fig. 4: An illustrative preplanning process for chasing waypoints $σ_{[t_{0}, t_{4}]}$ in a 2D case: The thick red circle denotes $x_{c}$ and blue circle means $x_{p}$ . The set $Ω (s_{p, n})$ are depicted as a set of red points. To obtain $σ_{[t_{0}, t_{4}]}$ , The waypoint $x_{n}$ for $t_{n}$ is chosen as one of the elements of $Ω (s_{p, n})$ by means of graph search. For the graph construction, we have nodes $v_{n} \in Ω (s_{p, n})$ connected with edges $e = (v_{n}, v_{n} + 1)$ based following Alogrithm 1.

In this paper, we operate on a 3D grid map $D$ for the compatibility with octomap. The definition of visibility (1) is suitable for our scenario for the following reasons: First, (1) can be computed from $D$ that represents a more general $χ$ than the previous research on chasing where particular shapes of obstacles are assumed (e.g. ellipsoids in [6] and polygons in [17]). Secondly, the computation of (1) is tractable for online applications. In our problem setting, (1) can be calculated within order of milliseconds once $ϕ (x)$ is obtained. Thirdly, (1) offers a measure of the robustness of a viewpoint for $x_{p}$ , because $ψ (x; x_{p})$ is level set function encoding the sensitivity for occlusion with respect to the perturbation of $x$ or $x_{p}$ as discussed in [22] (please refer Fig. 3). This helps us reliably evaluate the quality of viewpoint against unexpected future movements of target.

Utilizing $ϕ (x)$ and $ψ (x; x_{p})$ as measures for safety and visibility of target respectively, a preplanner is introduced to seed a sequence of waypoints in response to the available information of the target’s future path for a short horizon $H$ . As mentioned in Sec. III-A, the chaser at the current position $x_{c} (t)$ is provided with $x_{p} (τ)$ $(t \leq τ \leq t + H)$ . Let us divide the time horizon with time step $Δ t = \frac{H}{N}$ , and let $σ_{[t_{0}, t_{N}]} = (x_{0}, x_{1}, . . ., x_{N})$ denote a sequence of $N + 1$ waypoints at where $x_{n} = x_{c} (t_{n})$ and $t_{n} = t + n Δ t$ ( $n = 0, 1, . ., N$ ).

For $x_{p, n} = x_{p} (t_{n})$ , we now define the transitional visibility cost $c_{v} (x_{n - 1}, x_{n})$ between the two wapoints $x_{n - 1}$ and $x_{n}$ as below.

		$c_{v} (x_{n - 1}, x_{n}) =$		(2)
		$(\sqrt{\int_{L (x_{n - 1}, x_{n})} ψ (x; x_{p, n - 1}) d x \int_{L (x_{n - 1}, x_{n})} ψ (x; x_{p, n}) d x})^{- 1}$		(2)

Eqn. (2

) represents inverse of geometric mean of the line integrals of the two successive visibility fields corresponding to

x_{p, n - 1}

and

x_{p, n}

and

c_{v} (x_{n - 1}, x_{n})

becomes

+ \infty

if all the points along

L (x_{n - 1}, x_{n})

belong to

χ_{o c c} (x_{p, n})

χ_{o c c} (x_{p, n - 1})

. Our goal is to find a preplanning path

σ_{[t_{0}, t_{N}]}

which is the solution of the following optimization problem.

$argmin σ_{[t_{0}, t_{N}]}$	$N \sum n = 1 c (x_{n - 1}, x_{n})$	(3)
$w h e r e$	$c (x_{n - 1}, x_{n}) = ∥ x_{n - 1} - x_{n} ∥^{2}      i n t e r v a l d i s t a n c e + w_{v} c_{v} (x_{n - 1}, x_{n})      v i s i b i l i t y$
$+ w_{d} (∥ x_{p, n} - x_{n} ∥ - d_{d e s})^{2}      t r a c k i n g d i s t a n c e$
subject to	$x_{0} = x_{c} (t_{0})$
$x_{n} \in χ_{v i s} (x_{p, n}) \cap Ω (x_{p, n})$
$min x \in L (x_{n - 1}, x_{n}) ϕ (x) \geq r_{s a f e}$
$∥ x_{h - 1} - x_{h} ∥ \leq d_{m a x}$

Here, $r_{s a f e}$ is the safety clearance from obstacles and $d_{d e s}$ is the desired relative distance between the target and tracker. $d_{m a x}$ is the maximally allowable inter-distance between waypoints. For the tracking distance limits $d_{u p p e r}, d_{l o w e r}$ subject to $d_{l o w e r} \leq ∥ x_{n} - x_{p} ∥ \leq d_{u p p e r}$ , $Ω (x_{p, n})$ is a set of discretized points selected in a box region $B (x_{p, n}, 2 d_{u p p e r}) ∖ B (x_{p, n}, 2 d_{l o w e r})$ where a viewpoint at the corresponding time step $t_{n}$ will be chosen (see the set of red dots in Fig. 4). By adjusting the weights $w_{v}$ and $w_{d}$ , a different behavior of chaser is achieved while satisfying the constraints in (3). In this work, we are interested in examining the trade-off between the total travel distance and visibility, so will analyze the effect of $w_{v}$ in more detail (see the lower one in Fig. 1 and Fig. 8).

In order to solve (3), we now construct a directed graph $G (V, E)$ where each node corresponds to a candidate waypoint for $σ_{[t_{0}, t_{N}]}$ . For a vertex set $V = N + 1 ⋃ n = 0 V_{n}$ where

V_{n} = ⎧ ⎪ ⎨ ⎪ ⎩ \begin{matrix} {x_{c} (t_{0})} & n = 0 Ω (x_{p, n}) & 1 \leq n \leq N {x_{d u m m y}} & n = N + 1 \end{matrix}

(4)

$x_{d u m m y}$ is introduced to wrap the graph as a virtual goal point as the specific goal point is not pre-defined in our case. We construct $E$ by following process in Algorithm 1(the notation in Algorithm 1 is based on the operators used in C++). The solution of (3) is computed by means of the shortest path search methods on $G$ from $x_{c} (t_{0}) \in V_{0}$ to $x_{d u m m y} \in V_{N + 1}$ .

Based on the solution $σ_{[t_{0}, t_{N}]}$

obtained from the previous section, we generate a set of chasing corridors to be used for computing a continuous optimal trajectory which is described in the next section. For the moment, let us consider the following linear interpolation

γ (τ; σ_{[t_{0}, t_{N}]})

for

t_{0} \leq τ \leq t_{N}

γ (τ; σ_{[t_{0}, t_{N}]}) = \frac{τ - t_{n}}{Δ t} x_{n - 1} + \frac{τ - t_{n - 1}}{Δ t} x_{n} (t_{n - 1} \leq τ \leq t_{n})

(5)

Owing to our formulation (3), $σ_{[t_{0}, t_{N}]}$ satisfies $γ (τ; σ_{[t_{0}, t_{N}]}) \in χ_{f r e e}$ for $t_{0} \leq τ \leq t_{N}$ and $γ (t_{n}; σ_{[t_{0}, t_{N}]}) \in χ_{v i s} (x_{p, n})$ for $n = {0, 1, . ., N}$ as safety and visibility constraint as stated in the III-B-1) and 2). Also, in the case of large enough weight $w_{v}$ and small enough $Δ t$ , we observed that the condition $γ (τ; σ_{[t_{0}, t_{N}]}) \in χ_{v i s} (x_{p} (t))$ for $t_{0} \leq τ \leq t_{N}$ is satisfied instead of longer travel distance (see the right in Fig 1).

We now define the chasing corridor at $τ$ as below:

Q (τ; σ_{[t_{0}, t_{N}]}) = B (γ (τ; σ_{[t_{0}, t_{N}]}), l_{c o r r} (τ)),

(6)

where the parameter $l_{c o r r} (τ) \in R^{3}$ representing the size of a corridor at $τ$ is selected to satisfy the following necessary condition for the safety: $(l_{c o r r} (τ))_{i} \leq \frac{ϕ (γ (τ; σ_{[t_{0}, t_{N}]})}{\sqrt{3}}$ for $i = 1, 2, 3$ . As a side note, although relaxing $l_{c o r r}$ may be advantageous for the flexibility of smooth path generation, it can reduce the visibility performance between the two points ( $x_{n - 1}, x_{n}$ ). Examples of the chasing corridor are shown in the lower part of Fig. 1 for the two different values of $w_{v}$ .

Given :

ϕ (\cdot)

c (\cdot, \cdot)

V_{n}

x_{p, n}

Initialize :

V = N + 1 ⋃ n = 0 V_{n}, E = \emptyset

1 for $n = 1$ to $N + 1$ do

2 forall $v_{n - 1} \in V_{n - 1}$ do

3 forall $v_{n} \in V_{n}$ do

x_{n - 1} = v_{n - 1} \to x

x_{n} = v_{n} \to x

5 if $n <= N$ then

6 isLineSafe

= min x \in L (x_{n - 1}, x_{n}) ϕ (x) \geq r_{s a f e}

7 isVisible1

= ψ (x_{n - 1}; x_{p}) > 0

8 isVisible2

= ψ (x_{n}; x_{p}) > 0

9 isClose

= ∥ x_{n - 1} - x_{n} ∥ < d_{m a x}

10 isConnectable = isLineSafe & isVisible1 & isVisible2 & isClose

12 else

13 // connect all

v_{N}

with

x_{d u m m y}

14 isConnectable = true

15 end if

16 if isConnectable then

e={Edge(}\emph{}{)}

e \to x_{1}

x_{n - 1}

e \to x_{2}

x_{n}

18 if $n <= N$ then

e \to w = c (x_{n - 1}, x_{n})

21 else

22 // arbitrary positive number

e \to w = p o s i t i v e c o n s t

25 end if

E→{append(}\emph{e}{)}

27 end if

29 end forall

31 end forall

33 end for

return G(V,E)

Algorithm 1 BuildGraph

The previous section explained how the preplanner yields a sequence of viewpoints and chasing corridors for the consideration of safety and visibility. By the virtue of the differential flatness of a quadrotor MAV [12] together with $σ_{[t_{0}, t_{N}]}$ and $Q (τ; σ_{[t_{0}, t_{N}]})$ from the preplanner, we describe how to generate a dynamically feasible path for an aerial chaser $x_{c} (τ) (t_{0} \leq τ \leq t_{N})$ .

The yaw angle of the chaser is determined so that the x-axis of body frame of MAV heads to the target. The chaser path for position is represented as the following piecewise polynomials.

x_{c} (τ) = ⎧ ⎪ ⎪ ⎪ ⎪ ⎨ ⎪ ⎪ ⎪ ⎪ ⎩ \begin{matrix} x_{c, 1} (τ) = \sum_{k = 0}^{K} p_{1, k} τ^{k} & (t_{0} \leq τ < t_{1}) x_{c, 2} (τ) = \sum_{k = 0}^{K} p_{2, k} τ^{k} & (t_{1} \leq τ < t_{2}) . . . x_{c, N} (τ) = \sum_{k = 0}^{K} p_{N, k} τ^{k} & (t_{N - 1} \leq τ < t_{N}) \end{matrix}

(7)

Here, $p_{n, k} \in R^{3}$ are coefficients for the polynomial of order $K$ . They are obtained from the following optimization.

Here, $x_{0}, {˙ x}_{0}, {¨ x}_{0}$ are the initial state of the chaser when replanning is triggered at $τ = t_{0}$ . The fourth constraint accounts for the continuity condition up to the second order derivatives. The last box constraint is the corridor condition at the subsampled time step $τ = τ_{n, i}$ between every two knots ( $x_{n - 1}, x_{n}$ ), i.e. $t_{n - 1} \leq τ_{n, 1} \leq τ_{n, 2} \leq . . . \leq τ_{n, M} \leq t_{n}$ where $M$ should be adjusted depending on $d_{m a x}$ , the prediction horizon $H$ and the polynomial order $K$ .

The objective of (8) is the weighted sum of integral of the jerk squared and squared error of waypoints. (8) can be transformed into quadratic programming following a similar manner to [12, 13]. The convex optimization can be solved efficiently with algorithms such as interior-point or active-set methods. In this way, the consideration for safety, visibility and travel distance is implicitly included as a box constraint $Q (τ; σ_{[t_{0}, t_{N}]})$ rather than including them in a non-convex objective function as [6, 7] did.

In order to implement the chasing planner in C++, BOOST graph library (BGL) was used to obtain $σ_{[t, t + H]}$ (as a reminder, $t$ and $t + H$ are equivalent to $t_{0}$ and $t_{N}$ respectively.) in (3) by the shortest path search (Dijsktra algorithm). qpOASES [28] was employed as a QP solver for the smooth path completion in (8) and dynamicEDT3D library for computing $ϕ (x)$ [18]. All computation was performed on a 2.7Ghz intel i7 CPU labtop.

Fig. 5: Left: image view of the target. Right: replanning result (blue line) based on the partial future information of the target $x_{p} (τ)$ for $t \leq τ \leq t + H$ (solid red-colored line). The elevation of bearing vector $θ_{r}$ is depicted.

We tested the proposed chasing planner for two examples: mini garden and complex city with respect to the two different weights on the visibility. Their 3D models can be found in the first row of Fig. 8. The common parameters for the both cases are summarized in Table I. We plotted the overall chasing result in Fig. 8 while the quantitative analysis is described in Table II and Fig. 7. For the realistic simulation, Rotors simulator was used [27] to operate a MAV equipped with fixedly attached vi-sensor ( $20^{\circ}$ downward and $150^{\circ}$ FOV). In both cases, the target is ground vehicle. To maintain the ground target within the view of the fixedly mounted vision-sensor, the elevation angle $θ_{r}$ of the vector $x_{c} - x_{p}$ is limited between $20^{\circ}$ and $70^{\circ}$ from the consideration of mounting angle of camera and FOV. This naturally imposes the maximally allowable attitude of the chaser MAV. We assume that the chaser executes the replanned path for $t < τ < t + H - ϵ$ where $ϵ > 0$ after which another replanning session will be triggered. The invoked times for replanning are denoted as vertical lines in the background of Fig. 6, 7.

Fig. 6: The validation result on the visibility performance in the two different environments. The y-axis of the figure denotes $ϕ (x_{p})$ for target (green) and $ψ (x_{c}; x_{p})$ for the chaser(blue and red for different $w_{v}$ ). The cyan dashed boxes represent the duration of occlusion. The small value of $ϕ (x_{p})$ accounts for the difficulty of maintaining sight for $x_{p}$ .

Fig. 7: The validation result regarding the flight safety of the chaser. For the entire horizon of all the simulations, the safety tolerance $r_{s a f e} = 0.3$ was satisfied. The average of $ϕ (x_{c})$ was reduced with smaller $w_{v}$ as an effort for the shorter travel distance.

Parameters
Type	Name	Value
Resoluion	Octomap[m]	res = 0.4
Resoluion	$Ω (x_{p})$ [m]	res = 0.8
Optimization	tracking distance weight	$w_{d} = 3.4$
	waypoint weight	$λ = 2$
	maximum connection[m]	$d_{m a x} = 2.0$
	polynomial order	$K = 6$
	horizon[s]	$H = 5$
	# of corridors	$M = 2$
Tracking spec.	desired tracking dist.[m]	$d_{d e s} = 2.5$
	bound of tracking dist. [m]	$1.0 \leq ∥ x_{c} - x_{p} ∥ \leq 4.0$
	safe tol.[m]	$r_{s a f e} = 0.3$
	tracking elev. $θ_{r}$	$20^{\circ} \leq θ_{r} \leq 70^{\circ}$

TABLE I: Common parameters for simulations

In the simulation results in Table I, ”# of corridors” means the number of imposed corridor constraints between two knots for a replanning session. The function $ϕ (x_{p})$ is used to encode the chasing difficulty as it measures density of obstacles along the target path during mission. As can be seen in the table, the total travel distance and the average velocity increased for the higher value of visibility weight $w_{v}$ while the performance of visibility (i.e. the average of $ψ (x_{c}; x_{p})$ ) was increased and duration of occlusion was reduced by fraction of five.

The results for the average computation is also included in Table II. To cope with more dense environment as in complex city, we imposed the increased number of polynomial segments which leads to the increased computing time for QP. For the graph solving process, in contrary, the computing time was reduced for more dense situation because of the reduced number of feasible edges and nodes in the graph construction. For the both cases, the total planning pipeline runs approximately 10Hz to yield replanning path for the horizon $H = 5$ [sec] which is enough for online operation. The safety constraint was satisfied during the entire path with the designed safety clearance as shown in Fig 7.

Data

Mini garden

complex city

Planning param.

N

Target

avg.

ϕ (x_{p})

[m]

1.22

0.92

avg.

∥ {˙ x}_{p} ∥

[m/s]

0.61

0.58

Chaser

w_{v}

1.0

7.5

1.0

7.5

travel dist[m]

22.5

25.5

36.53

40.1

avg.

∥ {˙ x}_{c} ∥

[m/s]

0.75

0.82

0.73

0.80

avg.

ψ (x_{c}; x_{p})

[m]

0.68

0.99

0.59

0.7

occ. duration[s]

2.5

3.0

0.6

Comp.

time

ϕ (x)

[s]

0.057

0.071

ψ (x; x_{p})

[s]

0.001

0.0009

Dijkstra[s]

0.055

0.0064

QP[s]

0.0017

0.0037

total

0.11

0.08

TABLE II: Simulation results

Fig. 8: The simulation results: the first column of the figure is the result of a mini garden environment and the second one is a complex city with two different values of the weight for visibility $w_{v}$ . The cyan boxes on figures denote the obstacles which caused the occlusion during missions. The relative vector $x_{p} - x_{c}$ is plotted with color representing the scale of $ϕ (x_{c}, x_{p})$ (red corresponds to the high visibility and blue to low visibility).

In this paper, we proposed the cascaded planner for chaser operating in dense environments. By means of the level set-based metric for visibility of target, the preplnnaer yields a set of chasing corridors which consider the safety and visibility together with the travel distance. As the following step, dynamically feasible path was generated using convex optimization with numerical stability enjoying the real-time performance. The detailed implementation results were described where the aimed capabilities were satisfied. For the future works, we will improve the proposed pipeline with consideration of the velocity of the moving target. Also, we plan to extend the proposed method to actual hardware platform.

[1] Penin, Bryan, et al. ”Vision-based minimum-time trajectory generation for a quadrotor UAV.” Intelligent Robots and Systems (IROS), 2017 IEEE/RSJ International Conference on. IEEE, 2017.
[2] S. Shen, Y. Mulgaonkar, N. Michael, and V. Kumar, “Vision-based state estimation and trajectory control towards high-speed flight with a quadrotor,” in Proc. of Robot.: Sci. and Syst., Berlin, Germany, June 2013.
[3] Christian Forster, Zichao Zhang, Michael Gassner, Manuel Werlberger, Davide Scaramuzza SVO: Semi-Direct Visual Odometry for Monocular and Multi-Camera Systems IEEE Transactions on Robotics, Vol. 33, Issue 2, pages 249-265, Apr. 2017.
[4] Ma, Chao, et al. ”Robust visual tracking via hierarchical convolutional features.” IEEE transactions on pattern analysis and machine intelligence (2018).
[5] G. Loianno, C. Brunner, G. McGrath, and V. Kumar, “Estimation, Control, and Planning for Aggressive Flight With a Small Quadrotor With a Single Camera and IMU,” IEEE Robotics and Automation Letters, vol. 2, iss. 2, pp. 404-411, 2017.
[6] N ̵̈ageli, Tobias, et al. ”Real-time motion planning for aerial videog-raphy with dynamic obstacle avoidance and viewpoint optimization.”IEEE Robotics and Automation Letters 2.3 (2017): 1696-1703
[7] Bonatti, Rogerio, et al. ”Autonomous drone cinematographer: Using artistic principles to create smooth, safe, occlusion-free trajectories for aerial filming.” arXiv preprint arXiv:1808.09563 (2018).
[8] JThomas, Justin, et al. ”Autonomous flight for detection, localization, and tracking of moving targets with a small quadrotor.” IEEE Robotics and Automation Letters 2.3 (2017): 1762-1769.
[9] Oh, Yoonseon, Sungjoon Choi, and Songhwai Oh. ”Chance-constrained target tracking using sensors with bounded fan-shaped sensing regions.” Autonomous Robots 42.2 (2018): 307-327.
[10] Liu, Sikang, et al. ”Planning dynamically feasible trajectories for quadrotors using safe flight corridors in 3-d complex environments.” IEEE Robotics and Automation Letters 2.3 (2017): 1688-1695.
[11] Chen, Jing, Tianbo Liu, and Shaojie Shen. ”Online generation of collision-free trajectories for quadrotor flight in unknown cluttered environments.” Robotics and Automation (ICRA), 2016 IEEE International Conference on. IEEE, 2016.
[12] Mellinger, Daniel, and Vijay Kumar. ”Minimum snap trajectory generation and control for quadrotors.” 2011 IEEE International Conference on Robotics and Automation. IEEE, 2011.
[13] Richter, Charles, Adam Bry, and Nicholas Roy. ”Polynomial trajectory planning for aggressive quadrotor flight in dense indoor environments.” Robotics Research. Springer, Cham, 2016. 649-666.
[14] Nicolis, Davide, et al. ”Occlusion-Free Visual Servoing for the Shared Autonomy Teleoperation of Dual-Arm Robots.” IEEE Robotics and Automation Letters 3.2 (2018): 796-803.
[15] Cadenat, Viviane, David Folio, and Adrien Durand Petiteville. ”Comparison of Two Sequencing Techniques to Perform a Vision-Based Navigation Task in a Cluttered Environment.” Advanced Robotics 26.5-6 (2012): 487-514.
[16] Chen, Jing, Tianbo Liu, and Shaojie Shen. ”Tracking a moving target in cluttered environments using a quadrotor.” Intelligent Robots and Systems (IROS), 2016 IEEE/RSJ International Conference on. IEEE, 2016.
[17] Bandyopadhyay, Tirthankar, et al. ”Motion planning for people tracking in uncertain and dynamic environments.” Workshop on People Detection and Tracking, IEEE International Conference on Robotics and Automation. 2009.
[18] Hornung, Armin, et al. ”OctoMap: An efficient probabilistic 3D mapping framework based on octrees.” Autonomous Robots 34.3 (2013): 189-206.
[19] D. D. Harabor et al., “Online graph pruning for pathfinding on grid maps,” in Proc. 25th AAAI Conf. Artif. Intell., 2011.
[20] Karaman, Sertac, and Emilio Frazzoli. ”Sampling-based algorithms for optimal motion planning.” The international journal of robotics research 30.7 (2011): 846-894.
[21] Maurer, Calvin R., Rensheng Qi, and Vijay Raghavan. ”A linear time algorithm for computing exact Euclidean distance transforms of binary images in arbitrary dimensions.” IEEE Transactions on Pattern Analysis and Machine Intelligence 25.2 (2003): 265-270.
[22] Kao, Chiu-Yen, and Richard Tsai. ”Properties of a level set algorithm for the visibility problems.” Journal of Scientific Computing 35.2-3 (2008): 170-191.
[23] Tsai, Y-HR, et al. ”Visibility and its dynamics in a PDE based implicit framework.” Journal of Computational Physics 199.1 (2004): 260-290.
[24] Oskam, Thomas, et al. ”Visibility transition planning for dynamic camera control.” Proceedings of the 2009 ACM SIGGRAPH/Eurographics Symposium on Computer Animation. ACM, 2009.
[25] Baumann, Matthew A., et al. ”Occlusion-free path planning with a probabilistic roadmap.” 2008 IEEE/RSJ International Conference on Intelligent Robots and Systems. IEEE, 2008.
[26] Baumann, Matthew, et al. ”Path planning for improved visibility using a probabilistic road map.” IEEE Transactions on Robotics 26.1 (2010): 195-200.
[27] Fadri Furrer, Michael Burri, Markus Achtelik, and Roland Siegwart. Robot operating system (ROS). Studies in Computational Intelligence, 2016.
[28] Ferreau, Hans Joachim, et al. ”qpOASES: A parametric active-set algorithm for quadratic programming.” Mathematical Programming Computation 6.4 (2014): 327-363.

$min p_{n, k}$

Subject to

$x_{c} (t_{0}) = x_{0}$

${˙ x}_{c} (t_{0}) = {˙ x}_{0}$

${¨ x}_{c} (t_{0}) = {¨ x}_{0}$

$x_{c, n}^{(k)} (t_{n}) = x_{c, n + 1}^{(k)} (t_{n}) (k = 0, 1, 2)$

$x_{c} (τ) \in Q (τ; σ_{[t_{0}, t_{N}]})$