Informative Path Planning for Active Field Mapping under Localization Uncertainty

TL;DR

This paper presents an informative planning framework for active field mapping that explicitly incorporates robot pose uncertainty using Gaussian Processes, leading to more robust and accurate environmental maps.

Contribution

It introduces a novel utility function coupling localization and mapping objectives in GP-based planning, addressing pose uncertainty explicitly.

Findings

Reduces mean pose uncertainty in simulations

Decreases map error compared to existing strategies

Demonstrates effectiveness in indoor temperature mapping

Abstract

Information gathering algorithms play a key role in unlocking the potential of robots for efficient data collection in a wide range of applications. However, most existing strategies neglect the fundamental problem of the robot pose uncertainty, which is an implicit requirement for creating robust, high-quality maps. To address this issue, we introduce an informative planning framework for active mapping that explicitly accounts for the pose uncertainty in both the mapping and planning tasks. Our strategy exploits a Gaussian Process (GP) model to capture a target environmental field given the uncertainty on its inputs. For planning, we formulate a new utility function that couples the localization and field mapping objectives in GP-based mapping scenarios in a principled way, without relying on any manually tuned parameters. Extensive simulations show that our approach outperforms existing strategies, with reductions in mean pose uncertainty and map error. We also present a proof of concept in an indoor temperature mapping scenario.

Equations

$${\psi }^{\ast }=$$

$$\text{s.t. }\text{textsc}cost(\psi )\le B\text{.}$$

$$\text{upmu}=$$

$$(\mathbf{y}-m(\mathbf{X}))\text{,}$$

$$\mathbf{P}=$$

$$K({\mathbf{X}}_{\ast },\mathbf{X}{)}^{\top }\text{,}$$

$$\tilde{k}=E[k]={\int }_{X}kp(x)dx\text{.}$$

$$\tilde{k}=\frac{1}{(2\pi)^{\frac{3}{2}}}\sum^{{\color[rgb]{0,0,0}\definecolor[named]{pgfstrokecolor}{rgb}{0,0,0}\pgfsys@color@gray@stroke{0}\pgfsys@color@gray@fill{0}M}}_{i_{1}=1}\sum^{{\color[rgb]{0,0,0}\definecolor[named]{pgfstrokecolor}{rgb}{0,0,0}\pgfsys@color@gray@stroke{0}\pgfsys@color@gray@fill{0}M}}_{i_{2}=1}\sum^{{\color[rgb]{0,0,0}\definecolor[named]{pgfstrokecolor}{rgb}{0,0,0}\pgfsys@color@gray@stroke{0}\pgfsys@color@gray@fill{0}M}}_{i_{3}=1}\bar{w}k_{i_{1:3}}\,\textrm{,}$$

$${\mathrm{H}}_{\alpha }\left(\mathbf{X}\right)=\frac{1}{2}\log \left(\left|\left(2\pi {\alpha }^{\frac{1}{\alpha -1}}\right)\mathbf{P}\right|\right)\text{,}$$

$$\mathrm{H}_{\alpha}\!\left(\mathbf{X}\right)\mathrel{\vbox{ \offinterlineskip\halign{\hfil$#$\cr\propto\cr\kern 2.0pt\cr\sim\cr\kern-2.0pt\cr}}}\hat{\mathrm{H}}_{\alpha}\!\left(\mathbf{X}\right)=\log{\left(\operatorname{Tr}{\left(\mathbf{P}\right)}\,\alpha^{\frac{1}{\alpha-1}}\right)}\,\textrm{,}$$

$$\alpha \left(\mathbf{\Sigma }\right)=1+\frac{1}{\mathrm{Tr}\left(\mathbf{\Sigma }\right)},$$

$${\mathrm{I}}_{\alpha (\mathbf{\Sigma })}(\mathbf{p})=\hat{\mathrm{H}}({\mathbf{P}}^{-})-{\hat{\mathrm{H}}}_{\alpha (\mathbf{\Sigma })}({\mathbf{P}}^{+})\text{,}$$

Full text

I INTRODUCTION

Rapid technological advancements are inciting the use of autonomous mobile robots for exploration and data acquisition. In many marine [1, 2], terrestrial [3, 4], and airborne [5, 6] applications, these systems have the ability to bridge the spatiotemporal divides limiting traditional measurement methods in a safer and more cost-effective manner [7]. However, to fully exploit their potential, algorithms are required for planning efficient informative paths in complex environments under platform-specific constraints.

This paper examines the problem of active mapping using a robot, where the aim is to recover a continuous 2-D or 3-D field, e.g., of temperature, humidity, etc., using measurements collected by an on-board sensor. In similar setups, most existing strategies [5, 1, 8] incorrectly assume perfect pose information, which is an implicit requirement for building high-quality maps in initially unknown environments. Our motivation is to improve upon the robustness and accuracy of field reconstructions by allowing the robot to adaptively trade-off between gathering new information (exploration) and maintaining good localization (exploitation).

Despite recent efforts [2, 6, 9, 10], propagating both the localization and field map uncertainties into the planning framework in a principled manner remains an open challenge. A major issue arises due to the different ways in which the target field and robot pose are modeled. In particular, our work considers the task of mapping a field using a Gaussian Process (GP) with the robot pose represented as a multivariate Gaussian distribution. In this setup, measures of uncertainty in the field and robot pose, e.g., entropy-based criteria [4], are not directly comparable since they are obtained from their respective model variances which have different units and scales, and are therefore difficult to couple in a single utility function for multi-objective planning. Heuristic methods, e.g., based on a linear weighting of uncertainties [11], are commonly applied; however, they require careful manual tuning and are often scenario-specific.

To address this, we present an approach that accounts for the pose estimation uncertainty in two places. First, we consider the additional noise the pose uncertainty induces in the environmental field model; second, we include it as a shaping factor in the utility function that defines our informative planning task. Our mapping strategy uses GPs with uncertain inputs [2] to propagate the pose uncertainty into the field model, as visualized in Figure 1. During a mission, the map built online is used to plan informative trajectories in continuous space by optimizing initial solutions obtained by a coarse grid search. We develop a new utility formulation for GP-based mapping scenarios that jointly considers the uncertainty of the robot and the field models. This enables us to capture the desired exploitation-exploration trade-off in a mathematically sound manner, without relying on any manually-tuned, environment-dependent parameters. In summary, the contributions of this work are:

A utility function based on GP field models that tightly couples the objectives of robot localization and field mapping in active mapping problems. 2. 2.

An informative planning framework that accounts for the pose uncertainty in both mapping and the information objective for planning. 3. 3.

Evaluations of the approach in a 3-D graph Simultaneous Localization and Mapping (SLAM) setup and a proof of concept in a temperature mapping scenario.

We note that our framework can be used in any scalar field mapping scenario, e.g., spatial occupancy [6, 4, 12], signal strength [2, 8], aerial surveillance [7, 5], etc., and with any SLAM or localization-only algorithm supplying the pose uncertainty. Specifically, in this work, we focus on a general case where the spatial field information being collected is decoupled from the localization routine. More generally, the applicability of our utility function extends to other areas of robotics, such as reinforcement learning [13].

II RELATED WORK

Significant recent work has been done on autonomous data gathering strategies in the context of robotics and related fields. The discussion in this section focuses on two main research streams: (1) methods for probabilistic environmental mapping [9, 2, 6, 14], and (2) algorithms for informative planning [15, 16, 5, 4, 12, 1].

GPs are a popular non-parameteric Bayesian technique for modeling spatio-temporal phenomena [14]. They have been applied in various active sensing scenarios [1, 5, 16, 3] to gather data based on correlations and uncertainty in continuous maps. However, most of these works assume that the training data for prediction is inherently noise-free, which may lead to inaccuracies if measurements are incorporated at wrong locations and mislead predictive planning algorithms.

Propagating the input uncertainty through dense GP models is a computationally challenging task. To address this, analytical [9] and heteroscedastic approximation methods [10, 2, 3] have been proposed. Our work leverages the expected kernel technique of Jadidi et al. [2] by integrating over UIs with deterministic query points. Specifically, we apply their approach to more complex planning problems in 3-D scenarios, and integrate it with a new uncertainty-aware utility function. This coupling enables robust, tractable mapping under robot pose uncertainty for online sensing applications.

An active sensing task can be expressed in a Partially Observable Markov Decision Process (POMDP) [17] as one of decision-making under uncertainty. In practice, informative planning algorithms are typically used to render this problem computationally tractable with dense belief representations. We broadly distinguish between planning strategies operating in (a) discrete [12, 4] and (b) continuous space. This study focuses on the latter class of methods, which leverage incremental sampling [18, 6, 8, 19] or splines [5, 1] to offer greater scalability compared to discrete approaches. As in our prior work [15, 16, 5], we define smooth polynomial robot trajectories [20] and optimize them globally for an information objective in a finite-horizon manner.

Relatively limited research has been invested in active sensing scenarios where robot localization is uncertain. This setup has been tackled in the contexts of belief-space planning [19, 6, 18] and active SLAM [12, 11], where the aim is to maintain good localization as an unknown environment is explored. In contrast, and similarly to Papachristos et al. [6] and Costante et al. [18], our paper considers a setting where the map building and robot localization problems are decoupled. An important distinction is that we aim to reconstruct a continuous field that is independent of the features used for localization.

An open challenge in this setup is formulating utility functions to trade off between robot localization and field mapping in a principled manner. To address this, previous approaches have examined heuristic parameter tuning [12], e.g., using a weighted linear combination of the map and pose uncertanties [11], and multi-layer [18, 6] planning strategies. In contrast to these methods, we follow Carrillo et al. [4] in using the concept of Rényi’s entropy to discount information gain based on predicted localization uncertainty. Thereby, our utility function shares the benefit of coupling the two objectives in a mathematically sound way, without manual tuning requirements. The core difference is that our formulation is developed for a continuous mapping scenario based on a GP field model, instead of an occupancy grid. Moreover, by mapping with UIs, we present a unified framework where the robot localization uncertainty is jointly accounted for in both mapping and planning to achieve more robust data acquisition.

III PROBLEM STATEMENT

The general active mapping problem is formulated as follows. We seek an optimal trajectory $\psi^{*}$ in the space of all continuous trajectories $\Psi$ to maximize an information-theoretic measure:

[TABLE]

The function measure(·) obtains a finite set of measurements along trajectory $\psi$, and cost(·) provides its associated cost, which cannot exceed a predefined budget $B$. The operator $\mathrm{I}($·$)$ defines the information objective quantifying the utility of the acquired sensor measurements. In Section V-C, we propose a utility function for active mapping in GP-based scenarios that incorporates both the robot localization and field mapping objectives without any manual parameter tuning requirements.

IV MAPPING APPROACH

This section presents our mapping approach as the basis of our framework. We first describe our method for environmental field modeling using a GP, then present a strategy which folds the robot pose uncertainty into the map inference.

IV-A Gaussian Processes

We use a GP to model spatial correlations of a field in a probabilistic and non-parametric manner [14]. The target field variable for mapping is assumed to be a continuous function: $f:\mathcal{E}\rightarrow\mathbb{R}$. Essentially, a GP is a generalization of the Gaussian probability distribution, where the stochastic process governs the properties of functions, as opposed to vectors or scalars in the case of the probability distribution. A GP is fully characterized by the mean function $m(\mathbf{x})\triangleq\mathop{{}\mathbb{E}}[f(\mathbf{x})]$ and covariance function $k(\mathbf{x},\mathbf{x}^{\prime})\triangleq\mathop{{}\mathbb{E}}[(f(\mathbf{x})-m(\mathbf{x}))(f(\mathbf{x^{\prime}})-m(\mathbf{x^{\prime}})]$ as $f(\mathbf{x})\sim\mathcal{GP}(m(\mathbf{x}),\,k(\mathbf{x},\mathbf{x}^{\prime}))$, where $\mathop{{}\mathbb{E}}[$·$]$ is the expectation operator and $\mathbf{x}$ and $\mathbf{x}^{\prime}$ are input vectors composed of spatial coordinates in the environment.

Let $\mathbf{X}\subset\mathcal{E}$ be a set of $n$ observed robot locations $\mathbf{x}_{i}$ in the fixed-size environment with associated target field values $\mathbf{y}$. For a new set of $n^{*}$ query locations ${\mathbf{X}_{*}}\subset\mathcal{E}$ we can infer the field Gaussian distribution by conditioning in the observed values $\mathbf{f}_{*}\,|\,\mathbf{X},\,\mathbf{y},\,\mathbf{X}_{*}\sim\mathcal{N}(\boldsymbol{\upmu},\,\mathbf{P})$ as follows [14]:

[TABLE]

where $\sigma_{n}^{2}$ is a hyperparameter representing the observation noise variance, $K(\mathbf{X}_{*},\mathbf{X})$ denotes the cross-correlation terms between the query and observed points, and $K(\mathbf{X},\mathbf{X})$ and $K(\mathbf{X}_{*},\mathbf{X}_{*})$ are the joint covariance matrices for the observed and query points, respectively. Note that $K($·$,\,$·$)$ corresponds to the covariance function $k($·$,\,$·$)$ for only one element.

Note that $k$ encodes our assumptions about the field we are interested in modeling. In this work, we assume that the mapped environmental phenomena is smooth and isotropic; thus common choices of covariance functions are the squared exponential (SE) and Matérn family [14].

IV-B Mapping Under Pose Uncertainty

To propagate the robot pose uncertainty into our mapping framework, we apply the expected kernel technique of Jadidi et al. [2] to planning problems in 3-D setups. The key idea lies in taking the expectation of $k$ over UIs. Instead of $X$ being a deterministic location as in Section Section IV-A, let $X$ now be a random variable distributed according to a probability distribution $p(x)$. The expected covariance function $\tilde{k}$ can be computed as:

[TABLE]

Assuming a Gaussian distribution for the robot position $\mathcal{N}(\mathbf{p},\,\mathbf{\Sigma})$, we apply Gauss-Hermite quadrature to efficiently approximate the integral in Equation 4 in three dimensions. Specifically, in this case, through a change of variable such that $\mathbf{L}\mathbf{L}^{\top}=\mathbf{\Sigma}$ and $\mathbf{u}=\mathbf{L}^{-1}(\mathbf{x}-\mathbf{p})$, where $\mathbf{L}$ is a lower triangular matrix computed via the Cholesky decomposition:

[TABLE]

where {\color[rgb]{0,0,0}\definecolor[named]{pgfstrokecolor}{rgb}{0,0,0}\pgfsys@color@gray@stroke{0}\pgfsys@color@gray@fill{0}M} is the number of sample points used in the approximation, $\bar{w}\triangleq\prod^{3}_{j=1}w_{i_{j}}$, $u_{i_{j}}$ corresponds to the roots of the Hermite polynomial $H_{n}$, $\mathbf{u}_{i_{1:3}}\triangleq[u_{i_{1}},u_{i_{2}},\,u_{i_{3}}]^{\top}$, and $k_{i_{1:3}}$ is the covariance function evaluated at $\mathbf{x}_{i_{1:3}}=\mathbf{L}\mathbf{u}_{i_{1:3}}+\mathbf{p}$.

V PLANNING APPROACH

This section overviews our informative planning scheme, which generates fixed-horizon plans through a combination of a 3-D grid search and evolutionary optimization. We summarize the key steps of the algorithm, focusing on our uncertainty-aware utility function for GP-based active mapping scenarios as the main contribution of this paper. For further details concerning the planning approach itself, the reader is referred to our previous publications [15, 16, 5].

V-A Trajectories

A polynomial trajectory $\psi$ is represented by $N$ ordered control waypoints to visit $\mathcal{C}=[\mathbf{c}_{1},\ldots,\mathbf{c}_{N}]$ connected using $N-1$ $k$-order spline segments. Given a reference velocity and acceleration, we optimize the trajectory for smooth minimum-snap dynamics [20], clamping $\mathbf{c}_{1}$ as the initial robot position. In Equation 1, measure(·) is defined by computing the spacing of measurement sites given a constant sensor frequency and the traveling speed of the robot.

V-B Algorithm

We plan using a fixed-horizon approach, alternating between replanning and execution until the elapsed time $t$ exceeds the budget $B$. Our replanning strategy (Algorithm 1) consists of two steps. First, an initial trajectory is obtained through a grid search (Lines 3-7) based on a coarse set of points $\mathcal{L}$ in the 3-D robot workspace. In this step, we conduct a sequential greedy search for $N$ control waypoints in $\mathcal{C}$; selecting the next-best point $\mathbf{c}^{*}$ (Line 4) by evaluating Equation 1 over $\mathcal{L}$. As described in Section V-C, our new utility function (Equation 9) defines the objective $\mathrm{I}($·$)$ in Equation 1 for planning under pose uncertainty. During this procedure, for each candidate goal, the evolution of the robot pose uncertainty $\mathbf{\Sigma}$ along a trajectory connecting it to the current pose is predicted using SLAM (Line 5), as detailed in Section V-D. The result is used to update the covariance matrix of the GP field model $\mathbf{P}$ (Line 6) via Equation 3 based on the locations at which new measurements are taken. It is then added to the points $\mathcal{C}$ representing the initial trajectory solution (Line 7).

The second replanning step (Line 8) refines the coarse grid search output for $\mathcal{C}$ using Equation 1. This is done by computing $\mathrm{I}($·$)$ for a sequence of measurements taken along the corresponding trajectory. For prediction, we apply the same principles as described above in Lines 5 and 6. An important benefit of our two-step approach is that the informed initialization procedure exploiting the grid-based solution effectively speeds up the convergence of the optimizer, making it suitable for quickly finding solutions on computationally limited systems.

Note that the optimization step is agnostic to the actual method considered; specifically, in this work, we employ the Covariance Matrix Adaptation Evolution Strategy (CMA-ES) [21]. The CMA-ES is a generic global derivative-free optimizer based on evolutionary algorithms that has been successfully applied to solve high-dimensional, nonlinear, non-convex problems in a continuous domain [15, 16, 1, 21]. Essentially, it operates by iteratively evolving candidate solutions drawn from a search distribution whose parameters are adapted over time. Our choice of this routine is motivated by the nonlinearity of the objective space in Equation 1 as well as by the above mentioned results. For further details, the reader is referred to the in-depth review of Hansen [21].

V-C Utility Definition

We introduce a new utility, or information gain, function $\mathrm{I}($·$)$ in Equation 1 which directly relates the uncertainty of both the robot pose and the field map without the need for additional hand-tuned scaling parameters. Our utility function is derived from the Rényi entropy, which for a multivariate Gaussian distribution is given by [22]:

[TABLE]

where $\alpha\in[0,1)\cup(1,\infty)$ is a free parameter. Note that as $\alpha\rightarrow 1$, the Rényi entropy converges to the Shannon entropy in the limiting case.

To improve computational tractability, we approximate the determinant in Equation 6 using the trace of the covariance matrix [18, 1, 16]. Dropping the constant terms then gives:

[TABLE]

where $\operatorname{Tr}($·$)$ denotes the matrix trace, and the covariance matrix $\mathbf{P}$ is obtained using the expected kernel in Equation 4 for mapping under uncertainty.

Our key insight is to relate the free parameter $\alpha$ to the predicted pose uncertainty $\mathbf{\Sigma}$ along a candidate trajectory such that the expected information gain reduces when localization uncertainty is high. Noting that $\mathrm{H}_{\alpha}$ is strictly non-increasing in $\alpha$, this intuition can be captured by setting $\alpha$ to:

[TABLE]

and computing information gain as:

[TABLE]

where the superscripts $-$ and $+$ denote the prior and posterior covariance matrix $\mathbf{P}$ of the GP model, respectively. Similar to our approximation in Equation 7, the first term on the right hand side in Equation 9 is computed using the trace of the GP covariance. This approximates the Shannon entropy of the model before executing the candidate trajectory. The second term approximates the Rényi entropy of the model given the predicted observations, which is shaped according to the associated pose uncertainties by Equation 8. Thus, Equation 9 bears similarities to the mutual information, with the key difference that we are able to explicitly discount the information gain according to the predicted localization uncertainty along the candidate path.

It is worth noting that other functions may be used to relate $\alpha$ to the localization uncertainty. For example, Carrillo et al. [4] also suggest using the determinant or the maximum eigenvalue of $\mathbf{\Sigma}$ in place of the trace in Equation 8. Nevertheless, modulating the Rényi entropy provides a formal way in which to incorporate the localization uncertainty while maintaining a utility function that is primarily computed over the GP field map uncertainty.

V-D Uncertainty Prediction

A key requirement for predictive planning is propagating the robot localization uncertainty for a candidate action (Line 5 of Algorithm 1). We consider this problem for active sensing in initially unknown and known environments.

V-D1 Unknown Environments

In unknown environments, we consider a solution to this task assuming graph SLAM using odometry and point landmark observations as constraints [23]. Figure 2 schematizes a 2-D example. To predict the localization uncertainty along a possible path (dashed line), the graph is simply extended from the current pose $\mathcal{N}(\mathbf{p}_{1},\,\mathbf{\Sigma}_{1})$. The trajectory is interpolated at a fixed frequency to add $K-1$ odometry constraints (hollow circles) in the extended graph, giving rise to the sequence $\{\mathcal{N}(\mathbf{p}_{1},\,\mathbf{\Sigma}_{1}),\ldots,\mathcal{N}(\mathbf{p}_{K},\,\mathbf{\Sigma}_{K})\}$. For each consecutive node pair, we apply control noise drawn from a Gaussian distribution whose variance is proportional to the control input magnitude. This reflects the fact that longer motion steps are likely to be associated with higher actuation errors.

To address potential loop closures, we simulate re-observations to the known landmarks (green triangles) maintained in the current graph given the predicted robot pose and sensor field of view (FoV). In unknown space, we assume that no new landmarks will be detected, and thus, the robot pose uncertainty will grow as a result of the control noise. The resulting graph is then solved using QR factorization to obtain $\mathbf{\Sigma}_{K}$. Future work will consider extending these ideas to other SLAM systems.

V-D2 Known Environments

Similarly, in a known environment, the uncertainty can be predicted assuming a Monte Carlo Localization (MCL) approach. This method uses a motion model and the expected sensor measurements obtained to estimate the robot state by propagating the distribution of particles in a particle filter. In Section VI-C, this method is applied to localize a robot with a laser scanner in an indoor area.

Putting together our ideas in mapping and planning in a single framework, the GP field model with UIs accounts for the pose uncertainty in the received observations, while the proposed utility function accounts for the pose uncertainty in potential future measurements. This coupling allows us to achieve robust performance in GP-based mapping scenarios.

VI EXPERIMENTAL RESULTS

In this section, our approach is evaluated in simulation by comparing it to different strategies for planning and mapping. We then show proof of concept by using it to map temperature using a ground robot in an indoor environment.

VI-A Comparison of Planning Methods

First, the aim is to evaluate our new utility function by comparing it against existing strategies. To focus on examining our utility function and planning performance in particular, all methods in this sub-section use the GP-based approach with UIs for mapping under pose uncertainty (Section IV-B). The experiments are executed in MATLAB running on an Intel $1.8$ GHz computer with $16$ GB of RAM.

Our experimental setup considers 5 different $5$ m$\times$ $5$ m$\times$ $4$ m Gaussian Random Field environments for mapping, assumed to be initially unknown. We use a $0.25$ m$\times$ $0.25$ m$\times$ $1$ m resolution grid and apply the isotropic SE kernel with hyperparameters trained by minimizing log marginal likelihood in each environment [14]. For mapping with UIs, the modified kernel in Equation 4 is estimated using Gauss-Hermite quadrature with 5 points. During a mission, field measurements are taken at $0.25$ Hz using a point-based sensor centered on a 3 degrees of freedom robot moving as a point mass along the $(x,y,z)$ axes. These new measurements are added to our GP model as observed input training points to achieve uncertainty reduction as exploration takes place.

To emulate an aerial robot setup, 10 visual 3-D point landmarks are placed on the ground $1$ m below the target field and are distributed over one side of the space, as shown in Figures 1(a) and 4(a). For SLAM, the robot is equipped with a downward-facing camera with $(47.9\degree,\,36.9\degree)$ FoV. Landmark detection is done based on a pinhole projection camera model with standard deviations of $1.0$ px and $0.1$ m for measurement errors in pixel and depth. Our framework uses the graph SLAM algorithm [23] implemented in the SLAM Toolbox111github.com/joansola/slamtb, with the approach described in Section V-D1 for predicting the robot localization uncertainty in unknown environments. We sample trajectories at $0.5$ Hz to simulate control actions using an odometry motion model, applying a coefficient of $0.01$ in all three co-ordinate dimensions to scale the control noise variance. For planning, covariance matrices from the graph are extracted through the computations developed by Kaess and Dellaert [24].

Our two-step CMA-ES-based replanning framework (Section V-B) with the Rényi-based utility function (Section V-C) is compared against itself using different objectives: (a) field map uncertainty reduction only (Equation 9 using standard Shannon’s entropy in the map posterior); (b) field map uncertainty reduction over time (rate), as in our previous works [15, 16, 5]; and (c) a linear composite of the map and pose uncertanties weighting both objectives equally based on their upper bounds, tuned according to the approach of Bourgault et al. [11]. As planning benchmarks, we also study: the rapidly exploring information gathering tree (RIG-tree) using objectives of (d) uncertainty reduction only and (e) our Rényi-based utility, as well as (f) random waypoint selection. The former is a state-of-the-art sampling-based planning algorithm introduced by Hollinger and Sukhatme [8] that incrementally extends a geometric tree to find the best information gathering trajectory. We consider this benchmark to assess our two-step replanning approach and, in (f), show the applicability of our generic concepts in uncertainty-aware active sensing with different planners. A $150$ s budget $B$ is specified for all methods. To evaluate mapping performance, we quantify uncertainty with the GP covariance trace $\operatorname{Tr}(\mathbf{P})$ and accuracy with Root Mean Squared Error (RMSE) with respect to the ground truth map. Similarly, for planning, our measures are the robot covariance trace $\operatorname{Tr}(\mathbf{\Sigma})$ and the pose RMSE with respect to the ground truth trajectory. Intuitively, lower values using all metrics signify better performance.

In all environments, the starting robot position is ($2$ m, $2$ m, $1$ m) with no prior localization nor field map information. For trajectory optimization, the reference velocity and acceleration are $1.5$ m$/$s and $3$ m$/$s2 using polynomials of order $k=12$. In our planner, we define polynomials with $N=4$ waypoints and use a uniformly spaced 27-point lattice for the 3-D grid search. In RIG-tree, we associate control waypoints with vertices, and form polynomials by tracing the parents of leaf vertices to the root. The finite-horizon replanning procedure follows the approach of Popović et al. [5] with the uncertainty-only objective and a branch expansion step-size of $5$ m empirically set for best performance based on a search over a discrete range of values. We set $60$ tree-sampling iterations to obtain the same $\sim 76$ s average replanning time as required by our replanning routine for a single trajectory. In the random planner, random destinations are sampled in the workspace and a trajectory is generated by connecting them sequentially to the current robot position. We consider $4$ waypoints per plan to ensure that the trajectory lengths are fairly comparable to our method.

We conduct $50$ simulations in each environment, giving a total of $250$ trials. Figure 3 shows how the metrics evolve for each planning method. As expected, in Figure 3(b), informed strategies perform better than the random benchmark (blue) as they are guided by planning objectives. In Figure 3(a), the uncertainty-based (black, green) and weighted (cyan) utility functions yield map uncertainty and error reduction rates similar to our Rényi-based objective (red). However, our method significantly improves upon the robot pose estimation in the same scenario; confirming that it effectively trades off between gathering information and maintaining good localization given the known landmarks. This cannot be done using manually tuned parameters (cyan) as the variability of the pose uncertainty is much lower compared to that of the map. In terms of localization, the uncertainty-only function (black) performs worst as it does not exploit any knowledge of the trajectory dynamics.

Similarly, in Figure 3(a), the RIG-tree variant using our Rényi-based (purple) objective provides better localization when compared against planning for map uncertainty reduction only (yellow). This further validates the expected behaviour of our utility function, as described above, using a different replanning routine. Interestingly, compared to our replanning approach (red), the RIG-tree scores comparatively well on the localization metrics whereas the rate at which the map quality improves is limited. This likely relates to the step-size parameter required by the algorithm, which sets the range of navigation achievable during the mission. By traveling trajectories with shorter steps, the robot re-observes landmarks more frequently at the cost of restricted exploration; leading to a poorer final field reconstruction.

An example result using our CMA-ES-based framework and our proposed utility function is shown in Figure 4. Figure 4(a) confirms that the robot successfully explores the environment while re-visiting known landmarks to stay well-localized. With a total RMSE of 1.11, the map output in this instance (Figure 4(b)) is 1.86 times more accurate than the one produced by the naïve spiral path in Figure 1, thus justifying our informative planning strategy.

VI-B Evaluation of Field Mapping Under Uncertainty

Next, the aim is to assess the benefits of mapping under the robot pose uncertainty by evaluating the effects of incorporating UIs in the GP field model. We consider the same simulation setup as described above using our CMA-ES-based replanning framework and proposed Rényi-based utility function as the information objective. We conduct $50$ trials in each of the 5 environments (a) with and (b) without applying the modified kernel presented in Section IV-B. Note that (b) corresponds to a standard GP mapping approach without UIs as a benchmark.

Our results are depicted in Figure 5. Note that we omit the map uncertainty metric as the variance scales using the two approaches are not comparable. The plots confirm that our approach with UIs (red) presents more conservative exploratory behavior than the benchmark (blue) while also yielding more accurate field reconstructions. This is because our modified kernel can handle localization errors to build maps with better consistency and quality for reliable planning. Figure 1 shows a visualization that supports this result.

In summary, Figures 1 and 5 demonstrate that accounting for UIs in the field model improves mapping performance, whereas Figure 3 shows that incorporating the robot pose uncertainty in our utility function yields much better localization. By propagating the pose uncertainty into both the mapping and planning tasks, our proposed active sensing framework offers brings these benefits together to achieve robust mapping behaviour.

VI-C Proof of Concept

We show our active sensing framework running in real-time on a TurtleBot3 Waffle with an Intel Joule 570x running Ubuntu Linux 16.04 and the Robot Operating System. The experiments are conducted in a known indoor environment using Adaptive Monte Carlo Localization (AMCL)222wiki.ros.org/amcl receiving data from a LDS-01 laser distance scanner. As shown in Figure 6(b), for field mapping, a temperature distribution in an empty $2.8$ m $\times$ $2.8$ m area within the environment is generated using a $2400$ W radiant heater placed at one corner. Measurements are taken using a LM35 linear temperature sensor with a sensitivity of $10$ mV/$\degree$C.

A $0.4$ m resolution grid is set for mapping with UIs. To train the GP, we follow the method in Section VI-B using manually gathered data within the target area to obtain the hyperparametersfor the SE kernel. As before, the integral in Equation 4 is estimated using 5 Gauss-Hermite points.

Our uncertainty prediction method is based on localization in a known environment using AMCL (Section V-D). We sub-sample each candidate plan at $2$ Hz and estimate the robot pose using a differential drive odometry model and laser scans simulated in the known occupancy map. For the odometry model, variance parameters of $0.2$ m2 are used for Gaussian noise in both rotational and translational motion.

The aim is to show that our framework can map a realistic continuous field using a practical localization system. The initial measurement point is ($0.2$ m, $0.2$ m) within the corner opposite the radiator. We allocate a planning budget $B$ of $600$ s. Following the differential drive model, each plan is piecewise linear as defined by $N=3$ control waypoints with a constant velocity of $0.26$ m$/$s and temperature measurements sampled at $0.25$ Hz. The planning objective is our proposed Rényi-based utility function in Equation 9. Note that field map updates are triggered upon allowing the sensor readings to stabilize between successive measurement points.

Figure 7 summarizes our experiments. As expected, the field map becomes more complete over time and uncertainty decreases as the yellower heated region is discovered in a successful proof of concept implementation. Note that the values of the measurements obtained are higher than that at which the field is initialized ($23.64\degree$), i.e., the GP mean. This is due to the effects of heating and diffusion over time, after the training data was collected. Future work will address these issues by investigating map representations that capture temporal, as well as spatial, dynamics.

VII CONCLUSIONS AND FUTURE WORK

This work introduced an informative planning framework for active mapping that accounts for the robot pose uncertainty in both the mapping and planning stages. Our method uses GPs with UIs to propagate the robot pose uncertainty into the model of a target environmental field. For planning, we introduced a new utility function that tightly couples the uncertainties in the robot pose and field map by applying the concept of Rényi’s entropy in GP-based mapping scenarios. Our formulation enables the robot to trade off between exploration and exploitation in a principled way, without relying on any manually tuned parameters.

Our framework was evaluated extensively in simulation. We showed that it achieves more conservative exploratory behavior compared to different planning and mapping strategies, while producing more accurate maps. Experimental validation was performed through a proof of concept deployment, revealing its promise in future applications.

Future work will examine field models with temporal dynamics and efficiency improvements to handle more complex environments. Other interesting research directions involve refining the uncertainty prediction method and its relationship to the Rényi parameter $\alpha$ for more reliable planning.