Obstacle-aware Adaptive Informative Path Planning for UAV-based Target Search

TL;DR

This paper introduces OA-IPP, an obstacle-aware adaptive path planning algorithm for UAVs that improves target search efficiency in cluttered environments by balancing information gain, coverage, and collision avoidance.

Contribution

The paper presents a novel layered planning strategy with adaptive replanning for UAV target search, integrating Gaussian Process models and real-time obstacle awareness.

Findings

Outperforms existing planners in simulations

Effective in urban search and rescue scenarios

Balances multiple objectives for efficient search

Abstract

Target search with unmanned aerial vehicles (UAVs) is relevant problem to many scenarios, e.g., search and rescue (SaR). However, a key challenge is planning paths for maximal search efficiency given flight time constraints. To address this, we propose the Obstacle-aware Adaptive Informative Path Planning (OA-IPP) algorithm for target search in cluttered environments using UAVs. Our approach leverages a layered planning strategy using a Gaussian Process (GP)-based model of target occupancy to generate informative paths in continuous 3D space. Within this framework, we introduce an adaptive replanning scheme which allows us to trade off between information gain, field coverage, sensor performance, and collision avoidance for efficient target detection. Extensive simulations show that our OA-IPP method performs better than state-of-the-art planners, and we demonstrate its application in a realistic urban SaR scenario.

Equations

$${\psi }^{\ast }=\underset{\psi \in \Psi }{\mathrm{argmax}}\frac{k_1{O}_{info}(\psi )-k_2{C}_{coll}(\psi )}{t_{flight}(\psi )}\text{,}$$

$$C_{hard}(\text{x})=\{\begin{array}{ll}0& \text{if }\text{ESDF}(\text{x})\ge \frac{r_{UAV}}{2}\text{,}\\ 1& \text{otherwise}\text{,}\end{array}$$

$$C_{coll}(\psi )=\sum _{\text{x}\in \psi }{C}_{hard}(\text{x})\text{.}$$

$$k_{Mat3}(x,x^{\prime})=\sigma_{f}^{2}\bigg{(}1+\frac{\sqrt{3}d}{l}\bigg{)}\exp\bigg{(}-\frac{\sqrt{3}d}{l}\bigg{)}\textit{,}$$

$$P=$$

$$K(X^{\prime },X{)}^T\text{,}$$

$$O_{info}(\psi )=Tr(P^{-})-Tr(P^{+}),$$

$$UCB(x)=\mu (x)+\kappa \sigma (x),$$

$$AV=\sum _{x\in FoV}UCB(x).$$

$$O_{info}=\begin{cases}AV\times\Bigg{[}\frac{1}{\sigma_{1}\sqrt{2\pi}}e^{-\frac{1}{2}\big{(}\frac{h-h_{opt}}{\sigma_{1}}\big{)}^{2}}\Bigg{]}&h<h_{sat},\\ 0&\text{otherwise}.\end{cases}$$

Full text

I INTRODUCTION

Autonomous target search in cluttered environments is a challenging problem relevant for a wide range of applications, e.g., finding victims in SaR operations [1, 2], monitoring vegetation in precision agriculture [3, 4], patrolling military borders [5], and tracking endangered species [6]. With recent technological advances, UAVs are rapidly gaining popularity as a aerial data acquisition tool for this task. Compared with traditional approaches, such as manned aircraft and ground-based search, they offer high maneuverability, adaptability, and can provide data with high spatial and temporal resolution at a lower cost [4].

However, deploying a UAV to search for targets presents several challenges. A fundamental task is to plan the robot’s motion to maximize the information collected about a terrain given its resource constraints, such as finite battery life. For detection with an on-board camera, a key consideration is that area coverage can be achieved at different resolutions depending on the flying altitude. Finally, in cluttered environments, e.g., urban scenarios, it must be ensured that the generated paths are obstacle-free with minimal obstructions to the camera field of view (FoV).

In this work we propose an algorithm that tackles these issues simultaneously. Our problem formulation considers a cluttered, known environment in which the UAV navigates. In this set-up, the objective is to quickly find targets on a ground field using an on-board camera. Our framework consists of three major components: (1) modelling of the 3D environment; (2) mapping of the target 2D terrain using a probabilistic sensor model; and (3) planning. The main idea is to treat target search as an optimization process that solves an IPP problem in continuous 3D space and couples the aspects presented above. This allows us to generate smooth, obstacle-free paths for efficient data acquisition, which abide by a limited time budget. Moreover, within our approach, we introduce an adaptive planning strategy to quickly focus on areas where targets are likely to be found.

The core contributions of this work are:

an algorithm for UAV-based target search which couples informative planning with obstacle awareness, 2. 2.

an adaptive replanning scheme based on Bayesian Optimization (BO) which trades off between exploration and exploitation for efficient target detection, and 3. 3.

the extensive evaluation of our framework in simulation and its validation in a realistic urban SaR scenario.

Figure 1 demonstrates the success of our algorithm in detecting all human targets on the field for an SaR problem.

II RELATED WORK

The task of searching for targets is relevant to many real-world scenarios [2, 7, 4, 6]. Most generally, this problem can be expressed as a Partially Observable Markov Decision Process (POMDP), which models planning under uncertainty. However, despite recent advances, current POMDP solvers [1, 8] still scale poorly for practical applications due to high-dimensional belief space.

In robotics, target search is often more efficiently formulated as an IPP problem, where information about targets is maximized subject to a budget constraint. IPP literature can broadly be classified based on three aspects: (i) adaptivity; (ii) myopicity; and (iii) continuity. Adaptive IPP [9] differs from non-adaptive IPP [10] in terms of how new measurements influence the planning routine. The former performs online replanning while the latter executes an a  priori path. Myopic methods [11] differ from non-myopic ones [3] in terms of their planning look-ahead. The former navigates greedily by selecting next-best views, while the latter plans with a finite lookahead to escape local minima. Finally, whereas discrete strategies [10, 12] perform combinatorial optimization on pre-defined grids, continuous solvers [3, 13, 14] operate directly in the robot workspace to achieve better scalability.

Our algorithm is adaptive, non-myopic, and continuous. It falls into a recently emerged category of methods which optimize a smooth continuous trajectory for maximal information gain [3, 9, 15, 14]. Specifically, we build upon the work of Popović et al. [3] by introducing a new adaptive planning scheme based on BO [15] to perform exploration while focusing on potential targets as they are detected.

Accounting for sensor uncertainty and facilitating target re-observation is crucial in procuring an error-free target map. Despite abundant prior research, accounting for both of these factors in cluttered environments is an understudied topic. Although Dang et al. [16] and Marchant and Ramos [14] plan for target re-observation, they do not consider variable sensor performance. Whereas the methods of Popović et al. [3] cater for sensor uncertainties, they do not address obstacles. By unifying these concepts, our work aims to bridge the gap towards practical applications, i.e., urban [7, 17] and natural [1] settings.

III PROBLEM STATEMENT

We formulate the target search task as an IPP problem as follows. Our aim is to find an optimal continuous trajectory $\psi$ in the space of all trajectories $\Psi$ for the robot:

[TABLE]

where $O_{info}(\psi)$, $C_{coll}(\psi)$, and $t_{flight}(\psi)$ are functions quantifying the information quality, collision cost, and flight time along a trajectory $\psi$, and $k_{1}$ and $k_{2}$ are non-negative constants trading off between useful data acquisition and obstacle avoidance.

IV PRELIMINARIES

To lay the foundation for our IPP framework, this section describes our methods of modeling the key elements of the target search problem. We first detail our method of representing cluttered flying environments before describing our field mapping strategy.

IV-A Environment modeling

Our set-up considers a known cluttered 3D environment above the monitored terrain, in which the UAV flies. We treat obstacles as standard geometric shapes and use the Voxblox system [18], based on the Euclidean Signed Distance Function (ESDF), to enable computationally inexpensive collision checks. A hard penalty for colliding with obstacles is placed based on an artificial potential field, defined as:

[TABLE]

where ESDF(·) is the signed distance to the closest obstacle from a UAV configuration x, and $r_{UAV}$ is the radius of the smallest sphere that can contain the UAV.

Based on Equation 2, we define the collision cost $C_{coll}(\psi)$ in Equation 1 as:

[TABLE]

Note that $C_{coll}(\psi)$ is non-zero if any point $\textbf{x}\in\psi$ is inside the obstacle, thereby incurring a penalty to the objective function. $C_{coll}(\psi)$ is evaluated by sub-sampling $\psi$ at a high sampling frequency for flight safety.

IV-B Field modeling and mapping

Our field mapping strategy is based on the approach of Popović et al. [3], allowing us to incorporate probabilistic sensor models for data fusion with constant-time measurement updates. We represent the monitored ground field of target occupancy using a GP model, which captures spatial correlations in a probabilistic and non-parametric way [19]. The field is assumed to be a continuous function in 2D space $\zeta:\varepsilon\rightarrow\mathbb{R}$, where $\varepsilon\subset\mathbb{R}^{2}$ is a location on the ground field. The GP is fully characterized by the mean $\mu=E[\zeta]$ and the covariance $P=E[(\zeta-\mu)(\zeta^{T}-\mu^{T})]$ as $\zeta\sim GP(\mu,P)$, where $E[$·$]$ is the expectation operator.

The field is discretized at a given resolution to obtain $n$ training locations $X\in\epsilon$. A set of $n^{\prime}$ prediction points $X^{\prime}\in\epsilon$ are specified at which prior map is to be inferred. To describe the field, we propose using the isotropic Matérn 3/2 kernel function [19] common in geostatistical analysis, due to its capability to capture discrete targets. It is defined as:

[TABLE]

where $l$ and $\sigma_{f}^{2}$ are the hyperparameters representing the lengthscale and signal variance, respectively, and $d$ is the Euclidean distance between input locations $x$ and $x^{\prime}$.

The covariance is evaluated using:

[TABLE]

where $K(X,X^{\prime})$ denotes the $n\times n^{\prime}$ matrix of the covariances evaluated at all pairs of training and test points, $P$ is the posterior covariance, and $\sigma_{n}^{2}$ is a hyperparameter representing the noise variance. These hyperparameters $\{l,\sigma_{f}^{2},\sigma_{n}^{2}\}$ can be learned by maximizing the log marginal likelihood of a training dataset [19].

Sensor measurements are fused sequentially into the GP field map in two steps: (1) FoV estimation and (2) data fusion. First, the camera FoV at the measurement pose is projected to index the cells in $X$ to be updated. In this step, we account for occlusions arising from obstacles in the 3D ESDF environment (Section IV-A). Then, the field map is updated using a recursive technique based on the Kalman Filter (KF), presented by Popović et al. [3]. Within our framework, this method enables capturing the altitude-dependant performance of practical target detectors for probabilistic mapping, as discussed in the following section.

IV-C Sensor modeling

In this section, we develop a sensor model for planning to exemplify the integration of our framework with a real target detector. Specifically, we consider You Only Look Once (YOLO) [20], a typical neural network-based detector using images, for the application of SaR.

To investigate how detection accuracy varies with FoV, the performance of YOLO is evaluated empirically using multiple images simulated at different altitudes, each containing one human target. We quantify accuracy using F1 score by counting successful target detections. Figure 2 summarizes our results for an altitude range of $0-25$ m. This plot suggests that YOLO performs best at an intermediate altitude of $\sim 10$ m, with an altitude of $\sim 20$ m beyond which accuracy deteriorates significantly due to coarse image resolution, larger FoV and higher clutter in image. This analysis motivates our planner in Section V-D which caters for altitude dependency.

IV-D Global Optimization

We use the state-of-the-art evolutionary optimizer called CMA-ES [21] for global optimization of Equation 1. The successful application of CMA-ES in gradient-free, high-dimensional and non-linear optimization problems [3, 22], along with its quasi-parameter free nature, motivate its selection when compared to other global optimizers [21].

V PLANNING

In this section, we present our planning framework, named the Obstacle-aware Adaptive Informative Path Planner (OA-IPP) algorithm. By coupling information gathering with obstacle avoidance, our method achieves efficient target search in cluttered environments. The planner is an extension of Popović et al. [3], enabling it with obstacle-aware features and introducing a layered optimization for exploration-exploitation trade-off.

V-A Trajectory parametrization

A polynomial trajectory $\psi$ is parameterized by a sequence of $N$ control waypoints to be visited by the UAV, defined as $C=[c_{1},...,c_{N}]$, where the first waypoint $c_{1}$ represents the current UAV location. The polynomial trajectory connects these control points using $N-1$ $k$-order spline segments for minimum-snap dynamics as given by Richter et al. [17]. Along $\psi$, we consider a constant frequency for the sensor, computing the spacing between measurement poses with respect to the UAV dynamics.

V-B Algorithm

Algorithm 1 calculates the initial greedy solution $C_{init}$ comprising of waypoints $nbvp$, based on the current UAV pose $R_{0}$. $nbvp$ is the next best viewpoint for the UAV which is estimated by randomly sampling points from the environment at different altitudes and greedily maximizing the IPP objective given in Equation 1 (Line 6). Additionally, each waypoint should be visible from the previous viewpoint for a collision free initial solution. The next waypoint is evaluated by assuming a measurement update at the previous waypoint (Line 8).

Algorithm 2 summarizes our approach. The UAV evaluates and initial path $C_{init}$ (Line 5) and refines it using global optimization (CMAES) to generate path $C$ (Line 6), which is checked for collisions (Line 7). The path $C$ is used for data acquisition and mapping (Line 8-11) at a constant measurement frequency. A new path is planned once the stipulated path flight time is complete. This is repeated until the budget flight time is exhausted.

V-C Uncertainty reduction as the objective

Many IPP approaches use uncertainty reduction as the objective for the IPP problem without using the target detections for planning [9, 3]. The information gain can be represented as:

[TABLE]

where $Tr(.)$ denotes the trace of the matrix, $P^{-}$ and $P^{+}$ are the prior and posterior covariances, respectively. The latter is evaluated by fusing all measurements along $\psi$. However, the planner is explorative and does not take target detection into account while planning.

V-D Layered optimization for adaptive planning

In this section we propose a layered optimization approach that facilitates target re-observation, rendering the planner with exploration-exploitation trade-off capability, which is crucial for robustness against wrong detections. We define an information-theoretic objective function based on the acquisition function that is used in the BO framework. We use the Upper Confidence Bound (UCB) [23] defined by Equation 7 as the acquisition function.

[TABLE]

Here $\kappa$ is the exploration-exploitation tuning parameter, and $\mu$ and $\sigma^{2}$ are the mean and variance of the GP. We define Acquisition View (AV) as the sum total of acquisition function values within the FoV, given by

[TABLE]

The sensor performance curve given in Figure 2 is modelled as a normal distribution $N(h_{opt},\sigma_{1})$, and is used as a reward function for the objective function. The information gain is then defined as the combination of sensor performance and AV as

[TABLE]

The AV increases with altitude, while the sensor performance follows the pattern in Figure 2. Therefore, this layer of initial optimization based on BO given by Equation 9 encodes the trade-off between sensor performance and FoV by facilitating target re-observation through a balanced exploration-exploitation strategy that is embedded within the UCB. This objective contributes to the next optimization layer to evaluate the optimal 3D path.

VI EXPERIMENTAL RESULTS

This section presents our experimental results. We validate our framework in simulation by comparing it to state-of-the-art methods and study the effects of our adaptive replanning scheme. We then show its application in a realistic urban SaR scenario.

VI-A Benchmarking

We evaluate our algorithm on a $30\times 30\times 26$ m target search scenario in the RotorS-based simulation environment [24]. The set-up, shown in Figure 3(a), features a $4\times 10\times 26$ m obstacle and 7 human targets. Targets are placed on lower half of the field, necessitating target re-observation in that region. We use a uniform resolution of $0.75$ m for the GP model, and initialize the mean with a constant low prior of 0.1, assuming a sparse target distribution. The hyperparameters of the Matérn 3/2 kernel function are similar to the training results given in [3]: $\{\sigma_{n}^{2},\sigma_{f}^{2},l\}=\{1.42,1.82,3.67\}$.

We use YOLO Tiny 2.0 for human detection with a threshold value of 0.05 and measurement frequency of $0.15$ Hz. The detector receives images from a downward facing camera on the UAV with a FoV of $(45,60)\degree$. For planning, the constants in Equation 9 were experimentally computed as: $h_{opt}=10$ m, $h_{sat}=26$ m and $\sigma_{1}=7$. The parameters of the altitude dependent uncertainty model given in [3] are $(A,B)=(1,0.05)$.

We compare our approach against (1) ”lawnmower” coverage planning [25] and (2) random waypoint selection, running 25 trials for each method with a flight budget of $150$ s. To evaluate performance, we quantify accuracy using the Root Square Error (RSE) with respect to the ground truth map of human occupancy with the same resolution as the GP. We opt for this metric as it weighs false positive and negative misclassifications equally, reflecting the practical aims of a SaR mission. During a mission, higher rates of RSE reduction indicate more efficient search performance.

For the coverage planner, a fixed flight altitude of $10$ m was set by considering various “lawnmower” patterns for complete coverage within the time budget and selecting the best one. For the random planner, we uniformly sample a destination measurement pose above the field and generate a trajectory by connecting it to the current UAV position. For our planner, we set a reference speed and acceleration of $5$ m/s and $3$ m/s2 for trajectory optimization [17] with the adaptive planning strategy from Section V-D.

Figure 4 shows the evolution of RSE for each method during a mission. Our OA-IPP method (pink) outperforms the state-of-the-art algorithms by reducing error at the fastest rate, as it trades off between sensor performance and coverage at different altitudes, and actively focuses on targets as they are found. This strategy permits target re-observation, which decreases false positive detections, and variable resolution mapping, which refines accuracy over time. In contrast, the coverage benchmark (red) is limited in accuracy by the fixed altitude, while the random planner performs worst as it is often limited in coverage by the low altitude of sampled measurement sites.

VI-B Adaptive replanning evaluation

Next, the effects of our adaptive replanning scheme are evaluated in terms of target search efficiency. In the same simulation set-up as above, we compare the performance of two variants of our algorithm using the non-adaptive (Section V-C) and adaptive (Section V-D) planning objectives. As before, we run 25 trials and examine the evolution of RSE over time.

Figure 5 demonstrates that our OA-IPP algorithm outperforms non-adaptive IPP planner. Initially, both the non-adaptive (blue) and adaptive (red) planner perform similarly while the UAV performs field coverage (exploration). At later stages of the mission ($<75$ s), the adaptive variant achieves improved accuracy, as the acquisition function permits the UAV to focus on the half of the field where targets are likely to be found. This behaviour leads to repeated target observations, which renders the search robust to sensing uncertainty and procures a higher-quality end map. This aspect illustrated in Figure 7, which shows the GP mean after measurement updates for the environment in Figure 3(a). Despite the false positive detection after the first update, repeated measurements enable the recovery of a final map which resembles the ground truth in Figure 3(a).

VI-C Environment complexity

Next, the obstacle avoidance capabilities of our OA-IPP algorithm are assessed by examining its performance in different environments. We study two types of environment within a $30\times 30$ m area, which contain (1) low-rising ($4\times 4\times 13$ m) and (2) high-rising ($4\times 4\times 26$ m) obstacles, to portray SaR scenarios in different urban landscapes. Case (2) is designed such that the UAV must fly around, rather than over, obstacles due to the saturation height $h_{sat}$ of the sensor. To demonstrate the applicability of our approach, we conduct $100$ trials in both scenarios, considering variable obstacle densities of $(5,10,15)$ units with randomly initialized positions. Since exploratory capabilities are crucial for very occluded, complex environments, we use uncertainty as the evaluation metric, quantified by the covariance trace of the GP field model ($Tr(P)$).

Our experimental results are depicted in Figure 6. Figure 6(a) confirms that our planner achieves consistent uncertainty reduction with low-rising obstacles, regardless of the environment density. However, with an increased number of high-rising obstacles (Figure 6(b)), variance reduction is limited due to increased occlusions of the camera FoV during mapping. As an example, Figure 8(b) shows the trajectory planned in the narrow environment in Figure 8(a) for a $150$ s mission. The flight path between the buildings validates that our approach is capable of generating complex collision-free search trajectories in challenging scenarios. Therefore, OA-IPP algorithm generalizes for a wide range of environment complexities and obstacle densities.

VI-D Realistic simulation

Finally, we validate our algorithm in a realistic RotorS-based simulation of an urban SaR scenario. Figures 1(a) and 1(d) show our experimental set-up with 7 human targets for mapping. Our aim is to demonstrate a single flight experiment in a more realistic urban environment, and quantify the accuracy of the target map produced. In this experiment, we consider a $150$ s mission and apply the same mapping and planning parameters as in the previous sections.

Figure 1(f) depicts the final map result produced. A visual comparison with the ground truth Figure 1(e) confirms that our planner successfully finds the 7 human victims. Quantitatively, Figure 9 establishes that map uncertainty and error reduce during the mission, which implies that our method delivers an end result with increasing quality and confidence. The map provides crucial information for practical applications, making the map quality and confidence highly important.

VII CONCLUSIONS AND FUTURE WORK

The paper introduced a novel obstacle-aware IPP algorithm that is applicable for target search problems using a UAV. The planner simultaneously trades off between coverage, obstacle avoidance, target re-observation, altitude dependent sensor performance, flight time and FoV to generate the optimal, finite-horizon 3D polynomial path in an obstacle filled environment. The proposed layered optimization approach facilitates a balanced exploration-exploitation strategy which makes it robust against false detections. Extensive simulations show that our planner outperforms non-adaptive IPP planner, coverage planner and random sampling planner in terms of search efficiency. The algorithm was generalized for a wide range of environment complexities and obstacle densities. It successfully found all the humans on the ground in a realistic SaR simulation, despite multiple false human detections by the sensor, demonstrating the robustness provided by the layered optimization approach.

The main drawback with the planner is the underlying assumption of a known and static environment. Moreover, a non-temporal field was assumed for target occupancy. Future research will investigate planning with dynamic obstacles and fields. Multi-UAV collaboration for large area search is also a promising direction for future work.

ACKNOWLEDGMENT

This project has received funding from the European Union’s Horizon 2020 research and innovation programme under grant agreement No 644227 and from the Swiss State Secretariat for Education, Research and Innovation (SERI) under contract number 15.0029. It was also partly funded by the National Center of Competence in Research (NCCR) Robotics through the Swiss National Science Foundation.