\chapter{Safe Exploration of Unknown Environments}
\label{ch:safe_exp}

\minitoc


\section{Introduction}
\label{sec:safe_exp_intro}

In this thesis, we consider planning problems for mobile robots deployed on long-term monitoring missions in unstructured environments.
%
So far, we have assumed access to a known topological map of the environment, providing a structured representation of the area of interest that is safely navigable by the robot.
%
However, in many of the real-world scenarios that motivate this work, such maps may not be available, and may be difficult or costly to obtain.
%
In this chapter, we present a novel algorithm which enables a robot to autonomously explore and map an unknown environment, incrementally constructing a topological map whilst ensuring safety with respect to an unknown hazard.
%
In doing so, we address \challengeref{challenge:priors}, reducing the reliance on prior knowledge of the environment topology, and thereby broadening the applicability of our planning to a wider range of real-world problems.


The ability to explore and map unknown environments is a fundamental capability for autonomous robots, enabling operations in a wide range of unstructured environments.
%
However, many of the environments that are of interest in this thesis have the potential to present safety hazards for robots.
%
Challenging terrain or strong underwater currents, for example, can present dangers to a robot attempting to navigate them, or create states from which the robot may not be able to return once entered.
%
To explore safely, the robot must avoid states which are explicitly unsafe, as well as those from which it would not be able to return safely.


In this chapter, we address the problem of safe exploration in the presence of a hazard represented as an unknown process $\processfunction$, which is observable locally by the robot via an on-board sensor.
%
We propose a planning algorithm named SafeEstMDP-Map, which enables the robot to incrementally build a topological map of an unknown environment, whilst remaining safe with respect to the unknown hazard.
%
The hazard is modelled based on local observations as a GP, which is used to reason about the safe reachability and returnability of unexplored states.
%
This algorithm is an extension to the SafeEstMDP-Process algorithm from \citep{stephens2024auro}, which safely explores a \emph{known} topological map with the objective of estimating the unknown process.
%
In contrast, our objective is to map the maximal safely reachable area of the environment.
%
SafeEstMDP-Map outputs a topological graph spanning this maximal safely reachable area, which can then be used as the basis for subsequent robot missions, such as the monitoring missions considered in previous chapters.


The work in this chapter is published in the journal Autonomous Robots as part of \citet{stephens2024auro}, which is joint work with M. Budd.
%
The contributions presented here are the work of the author of this thesis. % who developed the SafeEstMDP-Process algorithm


\section{Preliminaries}
\label{sec:safe_exp_preliminaries}

This section provides background on some of the key concepts and methods that underpin the work in this chapter, in addition to the general background on safe exploration and exploration of unknown environments provided in Section~\ref{sec:relwork_planning}.

\subsection{Constrained reachability}

To formulate our safe exploration objective and find safe policies, we are interested in \emph{cost-optimal reach-avoid} problems~\cite{summers2010verification}, which are a special case of the constrained reachability problem from control theory and RL.
%
The aim in such problems is to minimise the cost to reach a set of goal states, whilst avoiding a set of unsafe (or, \emph{forbidden}) states.

Consider an MDP $\mdp = \langle \mdpstatespace, \mdpinitstate, \mdpactionspace, \mdptransition, \mdpcostfunc\rangle$, noting that here we use the cost function formulation from Definition~\ref{def:MDP}.
%
We do not specify a planning horizon for $\mdp$, since we are interested in the cost associated with reaching a set of goal states rather than the cost accumulated within a given number of timesteps, as we shall explain further below.
%
The MDP costs could represent, for example, time taken to execute actions, or the energy consumed in doing so.
%
We assume this MDP to be \emph{time-invariant}---that is, the transition probabilities and costs have no dependence on time.
%
A \emph{path} through an MDP is a sequence $\mdppath=\mdpstate_0 \stackrel{\mdpaction_0}{\rightarrow}\mdpstate_1\stackrel{\mdpaction_1}{\rightarrow} ... $ where $\mdptransition(\mdpstate_i,\mdpaction_i,\mdpstate_{i+1})>0$ for all $i \in \mathbb{N}$.
%
We denote the set of all paths through $\mdp$ starting from state $\mdpstate$ by $\mdppathset_{\mdp,\mdpstate}$.
%
% We consider deterministic, stationary policies for $\mdp$ defined as $\mdppolicy: \mdpstatespace \rightarrow \mdpactionspace$, and denote the set of all such policies as $\mdppolicyset$.
%
Then, given an MDP $\mdp$ and a deterministic policy $\mdppolicy \in \mdppolicyset$, we can define a probability measure $\mdppathprob_{\mdp,\mdpstate}^\mdppolicy$ over the set of paths $\mdppathset_{\mdp,\mdpstate}$~\citep{kemeny1976denumerable}.
%
For a measurable function over paths, $\mdppathfunc:\mdppathset_{\mdp,\mdpstate}\rightarrow\mathbb{R}$, we let $\mdppathexp_{\mdp,\mdpstate}^{\mdppolicy}(\mdppathfunc)$ denote the expected value of $\mdppathfunc$ with respect to $\mdppathprob_{\mdp,\mdpstate}^{\mdppolicy}$.


\begin{definition}[Reach-avoid paths]
    Let $\mdpgoalstates \subset \mdpstatespace$ be a set of goal states and $\mdpforbiddenstates \subset \mdpstatespace$ be a set of forbidden states in an MDP $\mdp$.
    %
    We define the set of paths that reach $\mdpgoalstates$ whilst avoiding $\mdpforbiddenstates$ as:
    \begin{align}
        \begin{split}
            \creach{\mdpforbiddenstates}{\mdpgoalstates} = \{
             & (\mdpstate_0\stackrel{\mdpaction_0}{\rightarrow}\mdpstate_1\stackrel{\mdpaction_1}{\rightarrow} \hdots ) \in \mdppathset_{\mdp,\mdpinitstate} \mid \exists i \in \mathbb{N} \text{ s.t. } \\
             & \mdpstate_i \in \mdpgoalstates \text{ and } \mdpstate_j \not \in \mdpforbiddenstates \text{ for all } j \leq i\}.
        \end{split}
    \end{align}
\end{definition}

In addition to minimising the expected cost to reach the goal set, we consider that there may be cases where reaching the goal set is not possible.
%
In such cases, a cost-optimal solution minimises the expected cost to reach a state from which reaching the goal set is \emph{not} possible.

%
%To define the above more formally, we start by defining the extended set of forbidden states $\overline{F} = \{s \in S \ | \  \mathit{Pr}_{\mathcal{M}, s_k}^{\max}(\mathit{reach}_{F,G}) = 0\}$.
%

\begin{definition}[Cost accumulation over paths]
    \label{def:path_cost_timestep}
    Let  $\mdppath= \mdpstate_0\stackrel{\mdpaction_0}{\rightarrow}\mdpstate_1\stackrel{\mdpaction_1}{\rightarrow} ... \in \mdppathset_{\mdp, \mdpstate_0}$.
    %
    We define $\mdppathtimestep_\mdppath$ as the timestep until which cost will be accumulated for path $\mdppath$:
    %
    \begin{equation}
        \mdppathtimestep_\mdppath = \left\{
        \begin{array}{ll}
            \min_\mdppathtimestep \text{ s.t. } \mdpstate_\mdppathtimestep \in \mdpgoalstates                                                            & \textrm{if } \mdppath \in \creach{\mdpforbiddenstates}{\mdpgoalstates} \\
            \min_\mdppathtimestep \text{ s.t. } \mdppathprob_{\mdp, \mdpstate_\mdppathtimestep}^{\max}(\creach{\mdpforbiddenstates}{\mdpgoalstates}) = 0 & \textrm{otherwise},
        \end{array}
        \right.
    \end{equation}
    %
    where $\mathit{Pr}_{\mdp, s_l}^{\max}(\creach{F}{G}) $ denotes the maximum over all policies $\mdppolicy \in \mdppolicyset$ of the reach-avoid probability  $\mathit{Pr}_{\mdp, s_l}^{\mdppolicy}(\creach{F}{G})$.
\end{definition}
% (is is known that there exists a deterministic and stationary policy that attains this supremum).
%

Note that the second condition in Definition~\ref{def:path_cost_timestep} encompasses the case where a forbidden state is visited before a goal state, as well as the case where no goal state can be reached.
%
We can now define the cost-optimal reach-avoid problem.

\begin{definition}[Cost-optimal reach-avoid]
    Let $\cumulreach{\mdpforbiddenstates}{\mdpgoalstates}:\mdppathset_{\mdp, \mdpstate} \rightarrow \mathbb{R}_{\geq 0}$ be a function that maps a path $\mdppath=\mdpstate_0\stackrel{\mdpaction_0}{\rightarrow}\mdpstate_1\stackrel{\mdpaction_1}{\rightarrow}\dots$ to the cost accumulated up to timestep $\mdppathtimestep_\mdppath$:
    %
    \begin{equation}
        \cumulreach{\mdpforbiddenstates}{\mdpgoalstates}(\mdppath)  = \sum_{i=0}^{\mdppathtimestep_\mdppath-1} \mdpcostfunc(\mdpstate_i,\mdpaction_i),
    \end{equation}

    \noindent and $ \mdppolicyset^*= \{\mdppolicy \in \mdppolicyset \mid \mdppolicy = \argmax{\mdppolicy'} \mathit{Pr}_{\mathcal{M}, \overline{s}}^{\mdppolicy'}(\creach{F}{G}) \}$ be the set of  policies that maximise the probability of reaching $\mdpgoalstates$ whilst avoiding $\mdpforbiddenstates$.
    %
    The cost-optimal reach-avoid problem is defined as finding the policy $\pireach{\mdpforbiddenstates}{\mdpgoalstates} \in \mdppolicyset^*$ that has minimal expected cumulative cost:
    %
    \begin{equation}
        \pireach{\mdpforbiddenstates}{\mdpgoalstates} = \argmin{\mdppolicy \in \mdppolicyset^*} \mdppathexp_{\mdp,\overline{\mdpstate}}^\mdppolicy(\cumulreach{\mdpforbiddenstates}{\mdpgoalstates}).
    \end{equation}

\end{definition}

This is a variant of the stochastic safest and shortest path problem from ~\citet{teichteil2012aaai}, for which the minimal expected cost is known to converge to a finite value.
%
We will write $\mdppathexp_{\mdp,\overline{\mdpstate}}^*(\cumulreach{\mdpforbiddenstates}{\mdpgoalstates})$ to denote  the (minimal) expected value corresponding to $\pireach{\mdpforbiddenstates}{\mdpgoalstates}$. % except when $ \mathit{Pr}_{\mathcal{M}, \overline{s}}^{\max}(\creach{F}{G})=0$.
%
To find $\pireach{\mdpforbiddenstates}{\mdpgoalstates}$, we encode the constrained reachability problem in  \emph{co-safe linear temporal logic} and use the approach presented in~\citet{lacerda2019probabilistic}.
%
This allows us to formulate a scoring function to evaluate candidate goal states in terms of their expected cost to reach safely, which is the subject of Section~\ref{ssec:goal_scoring}.

% We are also interested in the probability of reaching a set of states within a bound on the number of allowed timesteps.
% %
% For notational consistency, we denote as $ \mathit{Pr}_{\M, s}^{\mdppolicy}\left(\bcreach{\emptyset}{G}{\leq n}\right)$ the probability,  starting in state $s$ and under policy $\mdppolicy$, of reaching a state in $G$ \emph{within $n$ steps}.
% %
% We can calculate this probability by building the set $S^{\leq n}$ of states reachable by applying $\mdppolicy$ from $s$ for $n$ timesteps, and determining the probability of reaching  $G$ in the sub-model of $\M$ with state-space equal to $S^{\leq n}$.


\subsection{Estimated MDPs (EstMDPs)}

The SafeEstMDP-Process algorithm addresses the problem of planning for safe exploration of an unknown, GP-modelled continuous environment feature.
%
Planning in this setting is achieved using a novel MDP formalism known as an \emph{Estimated MDP}~(EstMDP).
%
The key idea of the EstMDP is to partition the codomain of the unknown process into a finite set of intervals, abstracting away the continuous aspect of the planning problem and allowing it to be solved using standard MDP techniques.

First, consider the unknown process $\processfunction: \environment \rightarrow \processobsspace$, where $\processobsspace$ is partitioned according to the set of intervals $\intervals$.
%
Given an element $\processobselement \in \processobsspace$ of the observation space, $\interval(\processobselement)$ denotes the interval $\interval \in \intervals$ that contains $\processobselement$.
%
The GP posterior $\gpposterior$ over an interval $\interval$ is defined as below.


\begin{definition}[GP posterior over an interval]
    Let $\gpdataset$ be a dataset of observations of the unknown process $\processfunction$, $\tmnode \in \tmnodes$ be a waypoint, and $\interval \in \intervals$ be an interval.
    %
    The probability that the value of $\processfunction$ at  $\tmnode$ is within interval $\interval$ is defined as:
    %
    \begin{equation}
        \gpposterior(\processfunction(\tmnode) \in \interval \mid \gpdataset) = \int_{\processobs \in \interval} \gpposterior(\processfunction(\tmnode)= \processobs \mid \gpdataset)\dd{\processobs}.
    \end{equation}
\end{definition}

It is worth noting here that in this chapter, we consider environment processes (and thus, planning problems) which are static in time, and in which navigation has deterministic outcomes.
%
As such, for a PKSMDP $\pksmdp$ used to describe planning over an environment represented by a topological map $G = \langle \tmnodes, \tmedges \rangle$, the known state space $\pksmdpstatespaceknown$ is precisely the set of waypoints $\tmnodes$.
%
To aid with intuition for the remainder of this chapter, we shall describe the planning models in terms of waypoints of the topological map.
%
We can also describe actions in this manner, since each navigation action $\pksmdpaction \in \pksmdpactionspace$ has a direct correspondence with an edge $\tmedge \in \tmedges$---we can therefore write $(\tmnode, tmnode')$ to denote the action $\pksmdpaction$ that moves the robot from waypoint $\tmnode$ to waypoint $\tmnode'$.

This leads us to the \emph{Estimated MDP} (EstMDP), which encodes both the probabilistic transition function of the PKSMDP and the GP model, given the observed dataset $\gpdataset$.
%
The EstMDP acts as the basis for planning in the SafeEstMDP-Process algorithm, which we describe in the following section.

\begin{definition}[Estimated MDP (EstMDP)]
    \label{def:estmdp}
    Let $\pksmdp=\langle \pksmdpstatespace, \pksmdpinitstate, \pksmdpactionspace, \pksmdptransition, \pksmdpcostfunc \rangle$ be a PKSMDP, $\gpdataset$ be a dataset of observations of the unknown process $\processfunction$, and $\mathcal{I}$ be a set of intervals partitioning the codomain of $\processfunction$.
    %
    The estimated MDP is a tuple $\estmdp=\langle \estmdpstatespace, \estmdpinitstate, \estmdpactionspace, \estmdptransition, \estmdpcostfunc \rangle$, where:

    \begin{itemize}
        \item $\estmdpstatespace = \tmnodes \times \intervals$, i.e. the state space consists of pairs of known PKSMDP states and intervals over the unknown process value at those waypoints;
        \item $\estmdpinitstate=(\pksmdpinitwaypoint, \interval(\processobs(\pksmdpinitwaypoint)))$ is the initial state, where $\pksmdpinitwaypoint \in \tmnodes$ is the initial location of the robot;
        \item The transition function $\estmdptransition: (\tmnodes \times \intervals) \times \estmdpactionspace \times (\tmnodes \times \intervals)  \rightarrow [0,1]$ is defined as
              \begin{equation}
                  \estmdptransition ((\tmnode, \interval),  \pksmdpaction, (\tmnode',  \interval')) =
                  \pksmdptransition((\tmnode,\interval),  \pksmdpaction, \tmnode') \gpposterior (\processfunction(\tmnode') \in \interval' \mid \gpdataset),
              \end{equation}
              where $\pksmdptransition((\tmnode,\interval),  \pksmdpaction, \tmnode')$ is the average of $\pksmdptransition((\tmnode,\processobselement),  \pksmdpaction, \tmnode')$ along interval $\interval=[l,u]$:
              \begin{equation}
                  \pksmdptransition((\tmnode,\interval),  \pksmdpaction, \tmnode')=
                  \frac{1}{u-l} \int_{l}^u \pksmdptransition((\tmnode,\processobselement),  \pksmdpaction, \tmnode') \dd{\processobselement}.
              \end{equation} \label{eq:t0_transition_function}
    \end{itemize}
\end{definition}


The EstMDP formalism manages the continuous codomain of $\processfunction$ in a different way to the MCTS-based planning methods used in the previous chapters, by discretising it \emph{a priori} into intervals.
%
As we shall see in the following section, this approach is particularly well-suited to reasoning about the safety of policies to reach goal states, compared with the progressive widening approach used in MCTS.

% The  EstMDP defines a transition function for which we can plan for using standard techniques. %
% It does so by (i) defining the transition function over intervals of $O$ rather than $O$ itself, by averaging it along each interval; and (ii) completing the transition definition, by weighting the transition probabilities in $T^O$ the GP estimate  with the GP posterior over each possible interval, given the set of observed data $\mathcal{D}$.
% %
% Figure~\ref{fig:transition_function} depicts an example $T^\mathcal{D}$.



\subsection{SafeEstMDP-Process}
\label{ssec:prelims_safeestmdp}

The primary contribution of this chapter is the extension of the
SafeEstMDP-Process algorithm from \citet{stephens2024auro} to completely unknown environments.
%
We summarise the key features of the SafeEstMDP-Process here---see the original paper for the complete algorithm description.


SafeEstMDP-Process considers the problem of a robot exploring an environment which is represented as a PKSMDP $\pksmdp = \langle \pksmdpstatespace, \pksmdpinitstate, \pksmdpactionspace, \pksmdptransition, \pksmdpcostfunc \rangle$ (Definition~\ref{def:PKSMDP}), with the objective of estimating an unknown process $\processfunction$.
%
At each step of its mission, the robot takes a noisy observation of the unknown process, over which it maintains a GP model.
%
%
During exploration, the robot must avoid states that are considered \emph{unsafe}, a notion which is formalised by way of a safety function over the PKSMDP state space.

%
\begin{definition}[Safety function]
    A \emph{safety function} is a mapping $\safefunc: \pksmdpstatespace \rightarrow \{0,1\}$ where $\safefunc(\pksmdpstate) =1$ when $\pksmdpstate$ is considered safe and $0$ otherwise.
    %
    The sets of safe and unsafe states for a state space $\pksmdpstatespace$ can then be defined as $\safe_{\safefunc} = \{\pksmdpstate \in \pksmdpstatespace \mid \safefunc(\pksmdpstate) = 1 \}$ and  $\unsafe_{\safefunc} = \pksmdpstatespace \setminus \safe_{\safefunc}$.
\end{definition}


\begin{figure}[]
    \centering
    \includegraphics[width=0.9\linewidth]{chapters/chSafeExp/figures/safe-est-mdp-alg.pdf}
    \caption[SafeEstMDP-Process and SafeEstMDP-Map flow diagram]{Flow diagram of the SafeEstMDP-Process and SafeEstMDP-Map algorithms. Components in blue are specific to SafeEstMDP-Map, or---in the case of the \textsc{ChooseGoal} method in particular---modified from the SafeEstMDP-Process equivalent. Solid lines indicate the main control flow of the algorithm, dashed lines indicate structures that manage data and perform model updates online.}
    \label{fig:safe-est-mdp-alg}
\end{figure}

Figure~\ref{fig:safe-est-mdp-alg} presents a flow diagram of the SafeEstMDP-Process algorithm.
%
The main control loop of the algorithm involves repeatedly selecting goal states, generating policies to reach them, and executing those policies.
%
Over the course of the mission, the robot improves its knowledge of the PKSMDP through the observations it makes of the unknown process, and uses the EstMDP formalism to represent this knowledge for planning.
%
Goal states are selected on the basis of a scoring function, which considers how beneficial the state would be to visit from the current state, as well as the cost to safely reach the state, and its safe reachability/returnability properties.
%
When a goal state is selected by the \textsc{ChooseGoal} method, a policy to safely reach it is also returned---this policy having been generated during the reachability check for that goal state.
%
After each action in the policy is executed, the robot performs a safety check of the current policy, re-evaluating the reachability criterion from the current state given the latest data.
%
If the safety probability of the current policy falls below some threshold, the policy is aborted and a new goal state is selected.
%
Otherwise, the robot continues executing the policy until the goal state is reached.
%
Once no more valid goal states can be found, the algorithm terminates.


In previous chapters, we used MCTS to plan online over PKSMDPs, discretising the continuous GP codomain online using progressive widening.
%
In contrast, the EstMDP formalism incorporates this discretisation directly into the planning model, and distinguishes explicitly between safe and unsafe states according to the current GP model of $\processfunction$.
%
This enables efficient offline reasoning about the safety of entire policies to reach particular goal states, making it a more suitable method for goal selection in the safety-critical setting.

\subsection{Occupancy map models of unknown environments}
\label{ssec:prelims_mapping}

In order to extend SafeEstMDP-Process to unknown environments, we require an environment representation that can be used as the basis for planning.
%
Our chosen representation for this purpose is the \emph{occupancy map}~\cite{elfes1989using}, a widely-used and versatile framework for representing unstructured environments observed through noisy sensor measurements in robotics.
%
Consider a bounded environment $\mapspace \subset \mathbb{R}^n$, partitioned into  free space $\mapspace_\free$ and occupied space $\mapspace_\occ$.
%
In this thesis, we consider the case where $n=2$, i.e. we assume the robot is deployed in an environment which can be navigated in a horizontal 2D plane.
%
However, the methods we propose can generalise to 3D environments.
%
Thus, we represent the environment as a grid map $\occupancymap=\{\mapcell_1, \hdots \mapcell_\mapsize\}$ of $\mapsize$ cells, which each represent a region in 2D space, i.e. $\mapcell_i \subset \mathbb{R}^2$.
%
We keep a classification of cells $\mapcell \in \occupancymap$ as representing either free space, occupied space, or unknown space.
%
There are several ways to maintain such a classification.
%
In  our implementation, we use the OctoMap framework~\cite{hornung2013octomap}, an octree-based library that provides a probabilistic occupancy estimate of each 3D voxel in the environment, projecting it to 2D by taking a slice of the OctoMap at the height of the robot's lidar sensor.

As a component of our planning problem in this chapter, we require a method to evaluate prospective goal states in terms of how much they are likely to contribute to the goal of mapping the environment.
%
To quantify this objective, we use the notion of \emph{volumetric gain} from~\citet{dang2020graph}, which estimates the expected unmapped volume that would be perceived by an on-board sensor from a given location $\maploc \in \mapspace$.
%
% This measure can be used as a proxy for the information gain that would be obtained by visiting $\maploc$. % Hmm, bit of a useless sentence

% \citet{dang2020graph} present a graph-based exploration algorithm in which expected information gain can be evaluated at each vertex, using ray casting in a 3D world representation to estimate the unknown volume that could be observed from its location.


%
\begin{definition}[Volumetric gain]
    \label{def:vol_gain}
    Let $\numcells^{\maploc}$ be the number of cells observed by the robot from location $\maploc \in \mapspace$.
    %
    We define volumetric gain at $\maploc$  as a weighted sum over the cells in each class observable from $\maploc$, by casting uniformly-spaced rays outwards in all directions:
    %
    \begin{equation}
        \begin{split}
            \gain(\maploc) = \gaincoeff_\unknown & \numcells^\maploc_\unknown + \gaincoeff_\free \numcells^\maploc_\free + \gaincoeff_\occ \numcells^\maploc_\occ,
        \end{split}
        \label{eq:volumetric_gain}
    \end{equation}

    \noindent where $\numcells_\mapclass^\maploc$ is the number of cells observable from $\maploc$ of class $\mapclass$, so $\numcells^\maploc_\unknown + \numcells^\maploc_\free + \numcells^\maploc_\occ = \numcells^\maploc$  and  $\gaincoeff_\unknown > \gaincoeff_\free > \gaincoeff_\occ \geq 0$.
    %
\end{definition}

The $\gaincoeff$ coefficients are normalised such that a gain value of $1$ represents every ray cast seeing purely unknown space, and a value of $0$ represents the robot being completely enclosed by occupied space.
%
The specific values of the $\gaincoeff$ parameters that produce the best performance will depend on the structure and connectivity of the environment: for example, whether it is made up of constrained corridors or wide-open spaces.

It is worth noting that in this chapter, we make use of an OctoMap-based environment representation obtained by projecting a 3D environment representation to 2D.
%
This assumes that robot navigation occurs in a 2D plane, which is the case for all environments considered here, and many real-world scenarios.
%
However, the exploration algorithms developed here also generalise naturally to 3D, so it would be straightforward to extend our proposed framework to more complex 3D exploration problems.





\section{Problem formulation}
\label{sec:safe_exp_problem_formulation}


% We now pose the two problems addressed in this paper.
% %
% First, we assume that the area has been previously spatially mapped and there  is a  known PKSMDP defined over the mapped area. The goal is to accurately estimate $\processfunction$ whilst remaining safe.
% %

% \begin{problem}\label{prob:estf}
% Let $\M^O=\langle S^O,\overline{s^O}, A^O,T^O, C^O\rangle$ be a PKSMDP and  $\safefunc:S^O \rightarrow \{0,1\}$ a safety function. Estimate the unknown process $f: X \rightarrow O$ whilst avoiding states $s^o = (v,o)$ where $s^o \in \unsafe_{\safefunc}$.
% \end{problem}

% In Problem~\ref{prob:estf}, we know the underlying spatial structure of the environment, and assume it has been discretised into a set of relevant waypoints.
% %
% This discretisation can be achieved manually by a designer, or automatically, e.g. by employing Voronoi diagrams or gridding the space, removing edges and cells that intersect with the obstacles.
% %
% Quantifying the quality of our estimate depends on the robot's objective and the approach used for estimation, thus we deliberately leave it undefined in the problem definition above.
% %
% In our proposed approach, we  use the variance of the GP predictions of $\processfunction$ on the waypoints, as will be explained in Section~\ref{sec:goal_choice}.


% Second, we remove the assumption of a known map  -- and hence of a known PKSMDP -- and consider the problem of safely exploring and mapping the environment.




We now pose the problem of safely exploring and mapping a completely unknown  environment, with no assumed prior knowledge of the environment layout.
%
We assume the robot is equipped with a sensor (e.g. lidar) and software which together enable it to build an occupancy map representation of its environment.
% %
% The occupancy map can then be used to classify voxels as unknown, occupied or free as described in Section~\ref{ssec:prelims_mapping}.
%
This occupancy map is then used to incrementally extend a topological map, which forms the basis of the PKSMDP and EstMDP that are used for planning.

The formal definition of our safe exploration and mapping problem is presented below.

\begin{definition}[Safe exploration objective]
    \label{def:safe_exp_problem}
    Let $\mapspace$ be a bounded environment, $\occupancymap$ a grid map representation of $\mapspace$, and  $\safefunc: \pksmdpstatespace \rightarrow \{0,1\}$ a safety function.
    %
    Classify as much of $\occupancymap$ as possible as free or occupied space, whilst avoiding states $\pksmdpstate = (\tmnode, \processfunction(\tmnode))$ where $\pksmdpstate \in \unsafe_{\safefunc}$.
    %
    It is assumed that the robot is always initialised in a safe state, i.e., $\safefunc(\pksmdpinitstate) = 1$.
\end{definition}


% We consider a PKSMDP $\pksmdp = \langle \pksmdpstatespace, \pksmdpinitstate, \pksmdpaction, \pksmdptransition, \pksmdpcostfunc\rangle$ which is grown \emph{incrementally} over the course of the mission.


To solve this problem, we adapt the SafeEstMDP-Process algorithm from \citet{stephens2024auro} (see Section~\ref{ssec:prelims_safeestmdp}) to incrementally grow a PKSMDP through exploration, rather than assuming that a fixed PKSMDP is provided as input.
%
We denote our algorithm as SafeEstMDP-Map, reflecting that the goal is now to safely map the unknown physical environment $\mapspace$, rather than to explore unknown process $\processfunction$ as in the case of SafeEstMDP-Process.



\section{Algorithm}
\label{sec:safe_exp_algorithm}


\subsection{Algorithm description}


In this section, we present the SafeEstMDP-Map algorithm, which extends the SafeEstMDP-Process algorithm from \citet{stephens2024auro} to address the problem described in Definition~\ref{def:safe_exp_problem}.
%
The objective is to safely explore a bounded environment $\mapspace$  represented by a grid map $\occupancymap$, and classify---as free or occupied---as many cells in $\occupancymap$ as possible.
%
For this problem, we will incrementally build a PKSMDP $\pksmdp$ over a topological map $\tm = \langle \tmnodes, \tmedges \rangle$ for navigation, halting exploration when the volumetric gain associated to all safely reachable waypoints in $\tmnodes$ is less than a specified threshold.
%




Our algorithm introduces two key changes with respect to the SafeEstMDP-Process algorithm.
%
First, it adds a new process running in parallel with the main algorithm control loop, which incrementally grows the topological map and PKSMDP as the robot explores the environment.
%
Each time the robot reaches a new state, it runs the \textsc{ExtendGraph} operation described in Section~\ref{ssec:extend_graph}, which uses the latest occupancy map data to extend the navigation graph into any new free space.
%
This process produces updates to the environment PKSMDP, which are in turn used (in addition to the latest GP model) to construct the EstMDP for planning.
%
Second, we modify the candidate waypoint selection, scoring function, and stopping condition used by the \textsc{ChooseGoal} process, to reflect the new exploration objective of safely mapping the environment.
%
These differences are highlighted in Figure~\ref{fig:safe-est-mdp-alg}, and described in detail in the following sections.


%We assume access to a map representation $\mathbb{M}$ which can be queried to calculate the volumetric gain at specific locations, as described in Definition~\ref{def:vol_gain}.


%The exploration algorithm for environments that are completely unknown (summarised in Figure~\ref{fig:flow_diagram_safeestexp}) has several key differences from that for known environments, following from the fact that the navigation graph is not available on initialisation and must be grown incrementally.
%%
%A new EstMDP is constructed each time t  incorporating the most up-to-date navigation graph and GP model.
%%
%The robot then executes this plan,  taking a new observation at each intermediate state as well as attempting to extend the map.
%%
%Once the goal is reached,  the GP model is updated and a new EstMDP model is constructed,  incorporating all available states and measurements.
%%
%We also introduce a new scoring function for goal candidates to emphasise exploration gain,  as described in Section~\ref{sec:scoring_safeestexp}.

%\begin{figure}
%   \begin{center}
%      \includegraphics[width=\columnwidth]{fig/flow_safeestexp_draft}
%      \caption{Flow diagram of the exploration method.}
%      \label{fig:flow_diagram_safeestexp}
%   \end{center}
%\end{figure}
%
%


% This is mostly just copied from the \safeestmdp{} bit,  needs rewriting
%\begin{algorithm}[t!]
%\footnotesize
%\caption{{\sc Safe Exploration (\emph{SafeEstMDP-Map})}}
%\label{alg:safe_est_explorer}
%\Input PKSMDP $\pksmdp$, safety function $\safefunc$, kernel $k(s,s')$, start observations $\overline{o}(S^{E}_{k,0})$,  map data stream $\mathcal{W}$ \\
%\Output Maximal and fully explored PKSMDP
%\begin{algorithmic}[1]
%\State $s=(s_{k}, \overline{o}(s_{k})) \gets \overline{s}$; \ $ s_{g} \gets s_k$; \ $t \gets 0$
%\While{$s_g \neq nil$}
%\If{$s_{k} = s_{g}$ \AlgOr $ \mathit{Pr}_{\M^e_t, s}^{\pi}\left(\bcreach{\emptyset}{\unsafe}{\leq n}\right)  > (1-p_{\text{min}})$}
%\State $t \gets t+1$
%\State $S^{E}_{k,t} \gets S^{E}_{k,t-1} \cup \{s_{k}\}$
%\State $\mathcal{GP}_{t} \gets $ update and optimise $\mathcal{GP}_{t-1}$ with $\overline{o}(s_{k})$
%\State $s_g, \ \pi \gets$ \textsc{ChooseGoal}  (Algorithm~\ref{alg:next_goal}, Section~\ref{sec:goal_choice})
%\EndIf
%\State  $s'=(s'_{k}, \overline{o}(s'_{k}))  \gets $ execute $\mdppolicy(s)$ and observe outcome
%\State $s \gets s'$
%\EndWhile
%\State No new goal state identified, end exploration
%\end{algorithmic}
%\end{algorithm}


\subsection{Incremental navigation graph expansion}
\label{ssec:extend_graph}

Our exploration algorithm requires a mechanism for incrementally growing $\pksmdp$ based on local sensor measurements.
%
For this,  we assume that the robot navigation is~\emph{deterministic}, i.e., the transition function is such that $\pksmdptransition((\tmnode,\processobselement), (\tmnode,\tmnode'), \tmnode'')=
    1$ if $ \tmnode''=\tmnode'$, and $0$ otherwise.
%
Furthermore, we consider the PKSMDP cost to be based on the Euclidean distance, i.e., $\pksmdpcostfunc((\tmnode,\processobselement), (\tmnode,\tmnode'))=\norm{\tmnode,\tmnode'} _2$.
%
The deterministic navigation assumption is used so we do not need to  estimate the outcome distribution of traversing a specific edge from sensor data, which is outside the scope of this work.
%
However, if we have a mechanism that can do so, then our approach can deal with stochastic navigation, as there is nothing in the exploration algorithm that assumes deterministic dynamics.
%
A PKSMDP with a deterministic transition function and Euclidean distance cost can be fully defined by its underlying graph $\tm=\langle \tmnodes, \tmedges\rangle$.
%
In the remainder of this section, we describe how to incrementally build $\tm$.
%
Note that any other cost function  that maps pairs of waypoints to a positive number can be considered; we use Euclidean distance for ease of presentation.
%
\correction{Furthermore, whilst uses volumetric gain derived from lidar scans to drive exploration, any other reward metric could be used as suited to the particular application---for example, information gain}~\cite{stachniss2005information} \correction{or reduction of model uncertainty}~\cite{pathak2017curiosity}.
%The method we propose uses ray casting in a 3-D voxel-based map representation which is derived from lidar scans.
%%
%However, in principle this also could be achieved through sampling-based approaches using either 2-D or 3-D environment representations.
%%
The graph-extending process is described by the \textsc{ExtendGraph} process in Algorithm~\ref{alg:extend_graph}, which can be seen in the context of the full SafeEstMDP-Map algorithm in Figure~\ref{fig:safe-est-mdp-alg}.

\begin{algorithm}[]
    \footnotesize
    \caption{{\sc ExtendGraph}}
    \label{alg:extend_graph}
    \Input Existing graph $\tm = \langle \tmnodes, \tmedges \rangle$,  current waypoint $\tmnode$,  angle increment $\extendgraphinc$,  range $\extendgraphrange$,  lengthscale $\extendgraphlenscale$,  proximity factor $\extendgraphproxfac$. \\
    \Output Updated graph $\tm$.
    \begin{algorithmic}[1]
        \For{$\extendgraphangle \in [ 0, 2\pi ]$ in increments of $\extendgraphinc$}
        \State{$\tmnode_{prev} \gets \tmnode$}
        \For{$d \in [\extendgraphlenscale,  \extendgraphrange]$ in increments of $\extendgraphlenscale$}
        \State{$\tmnode_q \gets \tmnode_\prev + d u_\extendgraphangle$} \Comment{$u_\extendgraphangle$ is the unit vector in direction $\extendgraphangle$.} \label{alg_safeestexp:line:query_node}
        \State{$\tmnode_c \gets \textsc{ClosestVertex}(\tmnodes, \tmnode_q)$}
        \If{$\norm{\tmnode_q, \tmnode_c}_2 \leq \extendgraphproxfac\extendgraphlenscale$ }
        \State{$\tmnode_q \gets \tmnode_c$ \Comment{Use existing vertex.}} \label{alg_safeestexp:line:use_existing}
        \EndIf
        \If{$\textsc{PathBetween}(\tmnode_\prev,\tmnode_q)$} \label{alg_safeestexp:line:path_between}
        \State{$\tmnodes \gets \tmnodes \cup \{\tmnode_q\}$}
        \State{$\tmedges \gets \tmedges \cup \{(\tmnode_\prev, \tmnode_q),(\tmnode_q, \tmnode_\prev)\}$}
        \Else
        \State{\textbf{break}}
        \EndIf
        \State{$\tmnode_\prev \gets \tmnode_q$}
        \EndFor
        \EndFor
        \State {\Return  $\tm = \langle \tmnodes, \tmedges \rangle$}
    \end{algorithmic}
\end{algorithm}


The algorithm casts rays outwards from the robot's current location $\tmnode$ in all directions within a horizontal plane, in angular increments of $\extendgraphinc$.
%
Along each ray, stepping outwards based on a lengthscale~$\extendgraphlenscale$, a new query waypoint~$\tmnode_q$ is created (Line \ref{alg_safeestexp:line:query_node}).
%
If~$\tmnode_q$ is too close to the nearest pre-existing waypoint~$\tmnode_c$, then the nearby waypoint is used as the query waypoint instead (Line~\ref{alg_safeestexp:line:use_existing}).
%
Then, the occupancy map is queried to check if there is a path through free space between the previous query waypoint $\tmnode_\prev$ and~$\tmnode_q$ (Line~\ref{alg_safeestexp:line:path_between}).
%
If such a path exists, $\tmnode_q$ is added to the vertices in the navigation graph, and a bidirectional edge is added linking it to the previous vertex.
%
Otherwise, the current chain of waypoints ends, as it is presumed blocked by an obstacle.
%
In this way, the algorithm constructs chains of waypoints outwards from the current waypoint $\tmnode$ through free space, connecting them to existing nearby waypoints whenever possible.

Figure~\ref{fig:graph_extension} shows the navigation graph construction algorithm in action, depicting the graph itself as well as the 3D occupancy map obtained from a Clearpath Jackal robot.
%
The initial navigation graph (Figure~\ref{fig:nav_graph_initial}) has edges radiating a few metres outwards from the robot's initial location, in all directions except where the lidar's line of sight is obstructed.
%
Figure~\ref{fig:nav_graph_progressed} shows a more developed navigation graph after a few actions have been taken, where the graph both extends further throughout the map and has more edges linking the initial set of waypoints.

% \begin{figure}
%    \begin{center}
%       \includegraphics[width=\columnwidth]{fig/extension_rviz.png}
%       \caption{Visualisation of the navigation graph built from an initial state in a Gazebo simulation.}
%       \label{fig:extension_illustration}
%    \end{center}
% \end{figure}

\begin{figure}
    \centering
    \begin{subfigure}[b]{0.7\linewidth}
        \centering
        \includegraphics[width=\textwidth]{chapters/chSafeExp/figures/nav_graph_rev_1.png}
        \caption{Initial navigation graph.}
        \label{fig:nav_graph_initial}
    \end{subfigure}
    \figvspace
    \begin{subfigure}[b]{0.6\linewidth}
        \centering
        \includegraphics[width=\textwidth]{chapters/chSafeExp/figures/nav_graph_rev_2.png}
        \caption{Navigation graph after a few actions have been taken.}
        \label{fig:nav_graph_progressed}
    \end{subfigure}
    \caption[Navigation graph incremental construction]{Navigation graph construction on a Clearpath Jackal robot simulated in Gazebo. Red spheres represent waypoints, yellow lines traversal actions.}
    \label{fig:graph_extension}
\end{figure}


\subsection{Choice of goal waypoint}
\label{ssec:choose_goal}

The \textsc{ChooseGoal} method of SafeEstMDP-Process consists of two main components.
%
The first is the candidate selection process \textsc{GetCandidates}, which returns a list of candidate goal states, in descending order of GP posterior variance evaluated at their respective locations.
%
This step requires that any candidate $\tmnode \in \tmnodes$ must have GP posterior variance above a given threshold $\gpvarthreshold$, i.e. $\variance\left(\gpposterior\left(\processfunction(\tmnode) \mid \gpdataset\right)\right) \geq \gpvarthreshold$.
%
The second is the goal selection process itself, which checks the candidates in batches, filtering out states which are not safely reachable and returnable with some required probability $\safetyprob$.
%
When a batch has at least one candidate remaining after the filtering step, all remaining candidates are scored based on a scoring function $\score$, and the highest-scoring candidate (along with a policy to reach it) is returned.

To adapt this for SafeEstMDP-Map, we introduce a modified \textsc{GetCandidates} routine, which is described in Algorithm~\ref{alg:get_candidates_explorer}.
%
Here, the GP predictive variance threshold is replaced by a volumetric gain threshold $\gainthreshold$, to reflect the mapping objective---that is, for a waypoint $\tmnode \in \tmnodes$ to be considered as a candidate goal, we now require $\gain(\tmnode) \geq \gainthreshold$.
%
The \emph{primary} candidate set of states~(Line~\ref{alg_get_candidates_explorer:line:primary_candidates}) can therefore be considered as equivalent to the candidate set returned by the original algorithm.


\begin{algorithm}
    \footnotesize
    \caption{{\sc GetCandidates} (SafeEstMDP-Map)}
    \label{alg:get_candidates_explorer}
    \Input Visited waypoints $\visited$, minimum safe probability $p_{\min}$, \correction{volumetric gain} threshold $\eta_{\gain}$, current waypoint $v$ \\
    \Output Candidate goal waypoint ordered sequence $V'$
    \begin{algorithmic}[1]
        %
        \State{$V^{\safe} \gets \{ v' \in  V \setminus \visited \mid P(I[f(v')] \in \mathcal{I}^{\safe}(v')  \mid \mathcal{D}) > p_{\min} \}$}
        %
        \State{$V^{\primary} \gets \{v' \in V^{safe} \mid \gain(v') \geq \eta_{\gain}\}$} \label{alg_get_candidates_explorer:line:primary_candidates}
        %
        \State{$V^{\progression} \gets \{v' \in V^{safe} \setminus V^{\primary}  \mid \min_{v'' \in V^{\primary}} {\Vert v', v'' \Vert_2} < \min_{\tilde{v} \in visited, v'' \in V^{\primary}} {\Vert \tilde{v}, v'' \Vert_2} \}$} \label{alg_get_candidates_explorer:line:progression_candidates}
        %
        \State {\Return  $V' = V^{\primary} \cup V^{\progression}$ as an ordered set, with elements of $V^{\primary}$ first, ordered by decreasing $\gain(v')$, and elements of $V^{\progression}$ after, ordered by  increasing $\Vert v, v' \Vert_2$}

    \end{algorithmic}
\end{algorithm}

However, we also introduce a new notion of \emph{progression candidates}, which are returned alongside the primary candidates (Line~\ref{alg_get_candidates_explorer:line:progression_candidates}).
%
Progression candidates are waypoints which have \correction{volumetric gain} below the threshold $\eta_{\gain}$, but which would decrease the minimum distance between the visited set of states and any primary candidate state if visited.
%
In other words, they are waypoints that progress the robot towards states with high \correction{volumetric gain}.
%
For most GP kernels that would be useful in this setting, taking measurements increasingly closer towards a primary candidate state will provide more information about the safety of that state.
%
The addition of progression candidates therefore provides an additional mechanism for the robot to select goal states that might benefit the mapping process, by improving its coverage and providing additional information about the safety of primary candidates.


\begin{figure}
    \centering
    \begin{subfigure}[b]{0.6\linewidth}
        \centering
        \includegraphics[width=\linewidth]{chapters/chSafeExp/figures/progression_candidacy_1.pdf}
        \caption{Timestep 1.}
        \label{fig:progression_candidacy:1}
    \end{subfigure}
    \figvspace
    \begin{subfigure}[b]{0.6\linewidth}
        \centering
        \includegraphics[width=\linewidth]{chapters/chSafeExp/figures/progression_candidacy_2.pdf}
        \caption{Timestep 2.}
        \label{fig:progression_candidacy:2}
    \end{subfigure}
    \caption[Progression candidates illustration]{Robot progress towards high-information-gain states using progression candidates.}
    \label{fig:progression_candidacy}
\end{figure}

Figure~\ref{fig:progression_candidacy} shows an example of a scenario in which the progression candidates are helpful.
%
The robot is at the start of a row of (unexplored) waypoints along a corridor, ending in a \qty{90}{\degree} corner.
%
At the first timestep (Figure~\ref{fig:progression_candidacy:1}), the robot is at $\tmnode_i$.
%
For this example, $\safetyprob = 0.99$.
%
The waypoint $\tmnode_c$ provides a view around the corner, so has a high \correction{volumetric gain} and is a primary candidate.
%
However, as it is several navigation edges away from the robot, $\reachprob(\tmnode_i,\tmnode_c) < \safetyprob$: the robot will not choose $\tmnode_c$ as a goal state.
%
The waypoint $\tmnode_a$ is safely reachable, but has \correction{volumetric gain} below the minimum threshold so cannot be a primary candidate.
%
However, $\tmnode_a$ is a progression candidate, because visiting it would decrease the minimum distance between the visited set of states and $\tmnode_c$.
%
At timestep 2, the robot has chosen $\tmnode_a$ as its new goal and transitioned to it.
%
Having taken a measurement at $v_a$, it is now more certain that $\tmnode_b$ and $\tmnode_c$ are safely reachable.
%
It can then continue to choose $\tmnode_b$, which is now safely reachable with probability greater than $\safetyprob$, as a new goal state and progress further towards $\tmnode_c$.
%
Without the addition of progression candidates, the robot would be unable to move along the corridor in this manner, since at first none of the states would meet both the safe reachability criterion and the \correction{volumetric gain} threshold.

% Note that in {\sc ChooseGoal} (Algorithm~\ref{alg:next_goal}), primary candidates will be evaluated as goal waypoints before progression candidates because $\gain(v) \geq \eta_{\gain}$ for all primary candidates and $\gain(v) < \eta_{\gain}$ for all progression candidates.

% \begin{attribution}
%     The progression candidate component of the algorithm is partially the work of M. Budd. Figure~\ref{fig:progression_candidacy} is the work of M. Budd, and is included here for context.
% \end{attribution}



\subsection{Goal scoring function}
\label{ssec:goal_scoring}

We now introduce our modified scoring function, which is used in the final step of the \textsc{ChooseGoal} process to select the candidate goal state to return.
%
In SafeEstMDP-Process, the scoring function $\score: \estmdpstatespace \times \tmnodes \rightarrow \mathbb{R}$ accounts for three factors to assess how beneficial a state $\tmnode$ would be to visit: the GP predictive variance at $\tmnode$, the expected cost to safely reach $\tmnode$ from the current state $\pksmdpstate$, and the safe reachability/returnability from $\tmnode$:
%
\begin{equation}
    \label{eq:score_safeestmdp_process}
    \begin{split}
        \score(\pksmdpstate, \tmnode) =
        \  & \variance \left( \gpposterior(\processfunction(\tmnode) \mid \gpdataset) \right)                                                                                                                       \\
           & \times \mdppathexp_{\estmdp,\pksmdpstate}^*\left(\cumulreach{\pksmdpstatespace^{\unsafe}}{\{(\tmnode, \interval) \mid (\tmnode, \interval) \in \pksmdpstatespace^{\safe}\}}\right)^{-\expscorecoeff_1} \\
           & \times (\reachprob(\pksmdpstate,\tmnode)\returnprob(\tmnode) - \safetyprob^2)^{\expscorecoeff_2},
    \end{split}
\end{equation}
%
where $\expscorecoeff_1$ and $\expscorecoeff_2$ provide the relative weightings of the components.

For SafeEstMDP-Map, we design a scoring function to encourage efficient exploration of the unknown space---a waypoint $\tmnode$ is  beneficial to observe if its volumetric gain is high,  as this indicates it would likely be helpful in growing the robot's map of the environment.
%
Thus, we replace the GP variance component in Equation~\ref{eq:score_safeestmdp_process} with the \correction{volumetric gain} term $\gain(\tmnode)$ defined in Equation~\ref{eq:volumetric_gain}.
%
This mirrors the functionality of the original scoring function, except with the exploration term now driven by increase of \correction{volumetric gain}, rather than minimisation of GP variance:
%
\begin{equation}
    \label{eq:score_safeestmdp_map}
    \begin{split}
        \score(\pksmdpstate, \tmnode) =
        \  & \gain(\tmnode)                                                                                                                                                                                         \\
           & \times \mdppathexp_{\estmdp,\pksmdpstate}^*\left(\cumulreach{\pksmdpstatespace^{\unsafe}}{\{(\tmnode, \interval) \mid (\tmnode, \interval) \in \pksmdpstatespace^{\safe}\}}\right)^{-\expscorecoeff_1} \\
           & \times (\reachprob(\pksmdpstate,\tmnode)\returnprob(\tmnode) - \safetyprob^2)^{\expscorecoeff_2}.
    \end{split}
\end{equation}


\section{Experiments}
\label{sec:safe_exp_experiments}

We present a series of experiments to demonstrate the performance of SafeEstMDP-Map.
%
We first test the algorithm in a physics-based simulation, evaluating its performance against a set of baselines in a variety of environment configurations.
%
We then also present results from an on-robot deployment in a physical environment, the Corsham Research Mine, integrating SafeEstMDP-Map with a complete robotic hardware and software stack.
%
For all experiments, we solve the MDP via PRISM~\citep{kwiatkowska2011prism} using nested value iteration~\citep{lacerda2019probabilistic}, and perform GP inference using GPy~\citep{gpy2014}.
%
SafeEstMDP-Map is implemented in Python and \Cpp{}, and executed using the robot operating system (ROS)~\citep{quigley2009ros}.


\subsection{Safety hazard model}
\label{ssec:safety_hazard}

In the experiments in this chapter, we consider the safety hazard of gamma radiation exposure, which can be harmful at high intensity to both robots and humans.
%
The unknown process $\processfunction : \environment \rightarrow \mathbb{R}$ represents the radiation level at each location in the environment.
%
Standard gamma radiation sensors only provide noisy, local measurements, which matches our locally observable safety formulation.
%
We use the radiation layouts from \citet{stephens2024auro}, which uses a combination of randomly-distributed point sources and Gaussian random fields to generate realistic scenarios.


Throughout this chapter, we consider a simple safety function which places an upper bound $\safebound\in\mathbb{R}$ on the value of the unknown process:
%
\begin{equation}
    \safefunc(\pksmdpstate) = \left\{
    \begin{array}{ll}
        1 & \textrm{if } \processfunction(\tmnode) \leq \safebound \\
        0 & \textrm{otherwise}.
    \end{array}
    \right.
\end{equation}
%
In general, however, the SafeEstMDP formulation allows $\safefunc$ to be an arbitrary Boolean function over the state space of the PKSMDP.
%
\citet{stephens2024auro} demonstrates SafeEstMDP-Process on a (simulated) AUV exploring a pre-mapped ocean environment, where $\processfunction$ represents unknown ocean currents, and the safety function considers the strength and direction of those currents.
%
In this case, unsafe states are those where the depth of the water is too shallow, \emph{or} states where the currents are strong enough in a direction that might push the UAV into neighbouring unsafe states.
%
% However, in this chapter we restrict our experiments to scenarios in which transition dynamics are unaffected by the unknown process, and the safety function is based solely on the value of the unknown process at each state.

Following \citet{snelson2003warped}, we use a log-warped GP regression, which is better suited to modelling the measurement noise of real-world radiation sensors, as well as capturing variation in the radiation field across multiple orders of magnitude~\cite{stephens2024auro}.
%
As the radiation level does not affect transition dynamics in this setting, we need only define two intervals $\intervals = \{\interval_\safe, \interval_\unsafe \}$ where $\interval_\safe = [0, \log(\safebound)]$ and $\interval_\unsafe = [\log(\safebound), \infty)$.
%
If a non-log GP were used, the EstMDP structure would be identical, with equivalent intervals $\interval_\safe = [0, \safebound]$ and $\interval_\unsafe = [\safebound, \infty)$.
%
The exploration GP models the 1D radiation level at each state: $\gpposterior: \mathbb{R}^2 \rightarrow \text{Dist}(\mathbb{R})$.
%
The GP input uses spatial coordinates $\{x, y\}$ in units of metres in a local coordinate frame.
%

\subsection{Simulated domains}
\label{ssec:evaluation_domains}


We evaluate the SafeEstMDP-Map algorithm in two simulated environments, each with three different radiation layouts.
%
These experiments are run as full physics simulations in Gazebo~\citep{koenig2004design}, using ROS as the primary interface between modules.
%
The two domains are shown in Figure~\ref{fig:domains}, and are as follows:
\begin{enumerate}
    \item \emph{Reactor room} (Figure~\ref{fig:domain_reactor_room}): a simulated world representing a \qtyproduct{20 x 20}{\metre} nuclear reactor room, from \citet{wright2021simulating}.
    \item \emph{Research mine} (Figure~\ref{fig:domain_corsham}): A \qtyproduct{35 x 28}{\metre} map built from 3D SLAM in a real underground mine section at Corsham, UK.
\end{enumerate}
%

\begin{figure}
    \centering
    \begin{subfigure}[b]{0.5\linewidth}
        \centering
        \includegraphics[width=\textwidth]{chapters/chSafeExp/figures/reactor_room_topological_map_clean_arrows.png}
        \caption{Reactor room, \qtyproduct{20 x 20}{\metre}.}
        \label{fig:domain_reactor_room}
    \end{subfigure}
    \figvspace
    \begin{subfigure}[b]{0.65\linewidth}
        \centering
        \includegraphics[width=\textwidth]{chapters/chSafeExp/figures/corsham_topological_map_clean_arrows.png}
        \caption{Research mine, \qtyproduct{35 x 28}{\metre}.}
        \label{fig:domain_corsham}
    \end{subfigure}
    \caption[Safe exploration simulated domains]{2D occupancy maps of both simulated experiment domains. Note that the depicted topological maps are not used by SafeEstMDP-Map during exploration, and are shown only to indicate the approximate area of the map that is accessible to the robot.}
    \label{fig:domains}
\end{figure}


\subsection{Exploration of unknown environments}
\label{ssec:results_exploration}



%
%
%Radiation sources are positioned in the \emph{ground truth} coordinate frame in the simulation, not the SLAM system frame, thus providing a more accurate simulation of the complete exploration system.

For this set of experiments, and those in the following section, we use $\safetyprob = 0.99$, goal selection weights $\expscorecoeff_1 = 1.5$ and $\expscorecoeff_2 = 0.8$,  batch evaluation size $\safeexpbatchsize = 30$,  gain threshold $\gainthreshold = 0.1$, and volumetric gain parameters $\gaincoeff_{\unknown} = 20.0$, $\gaincoeff_{\free}=1.0$, $\gaincoeff_{\occ}=0.0$.
%
The GP kernel is an RBF with lengthscale $\qty{2.0}{\metre}$ and variance $1.0$.
%
The GP observation noise $\gpvar^2 = 0.0009$, corresponding to a percentage observation noise $\gpvar_\% \approx 3\%$~(see \citet{stephens2024auro}, Equation~17).
%
The robot's initial position is randomly sampled from a set of 4 waypoints.

We evaluate our algorithm against two baselines.
%
The first is a threshold-based algorithm which we denote ThresholdBasedExplorer~(TB), which selects exploration goals using the same scoring function as SafeEstMDP-Map, but does so without an explicit hazard model.
%
Instead, the TB baseline uses a threshold fraction $\baselinethreshold \in (0,1)$ of the safety limit $\safebound$ to prevent it from entering unsafe states.
%
All states are initially presumed safe, but if the robot reaches a state at which the radiation level is more than ${\baselinethreshold}\safebound$ on the way to a goal, it marks all adjacent \emph{unvisited} states as unsafe.
%
It then selects a new goal from the remaining safely reachable states, thereby turning back from the path to the previous goal.
%
Since the reachability and returnability calculations (Equation~\ref{eq:score_safeestmdp_map}) are now performed based on this deterministic categorisation of states as safe or unsafe, $\reachprob$ and $\returnprob$ are now either $0$ or $1$ for all states.
%
% Thus, the purpose of this baseline is to demonstrate the importance of the probabilistic model of $\processfunction$ in the exploration process. 
%
The second baseline, MeanPredictionExplorer~(MP), again uses the same goal selection process as SafeEstMDP-Map, except with a limited version of the full EstMDP.
%
Rather than weighting the EstMDP intervals based on the GP posterior at each state, this baseline instead assigns all probability mass to the interval containing the posterior mean value at each state.
%
This baseline therefore classifies states as safe or unsafe based purely on the GP predictive mean, with no consideration of the uncertainty of these predictions.
%
We evaluate SafeEstMDP-Map against MeanPredictionExplorer and ThresholdBasedExplorer with $\baselinethreshold=0.3$ and $\baselinethreshold=0.8$, simulating each configuration for 30 exploration runs.

\begin{figure}
    \centering
    \centering
    \includegraphics[width=1.0\linewidth]{chapters/chSafeExp/figures/expts_gazebo_reactor_room.pdf}
    \caption[Simulated experiment results (reactor room domain)]{Experiments for SafeEstMDP-Map in the reactor room domain. Each algorithm is tested on three randomly generated radiation layouts, with each configuration repeated 30 times. Left: number of runs (out of 30) terminated successfully; middle: violin plots summarising the statistics over area explored; right: violin plots summarising the statistics over distance travelled.}
    \label{fig:expts_gazebo_reactor}
\end{figure}

\begin{figure}
    \centering
    \includegraphics[width=1.0\linewidth]{chapters/chSafeExp/figures/expts_gazebo_corsham.pdf}
    \caption[Simulated experiment results (research mine domain)]{Experiments for SafeEstMDP-Map in the research mine domain for different radiation layouts. Each algorithm is tested on three randomly generated radiation layouts, with each configuration repeated 30 times. Left: number of runs (out of 30) terminated successfully; middle: violin plots summarising the statistics over area explored; right: violin plots summarising the statistics over distance travelled.}
    \label{fig:expts_gazebo_corsham}
\end{figure}

The results in Figures~\ref{fig:expts_gazebo_reactor} and~\ref{fig:expts_gazebo_corsham} show that SafeEstMDP-Map is able to explore more effectively than the baselines across the variety of simulated configurations.
%
The mean-prediction approach frequently results in unsafe termination, since without the full EstMDP model the safe reachability check is much less accurate, incorrectly classifying many unsafe states as safe.
%
Of the threshold-based baselines, the more conservative one ($\baselinethreshold = 0.3$) is able to remain safe on almost every run.
%
However, its exploration coverage on many of them is poorer than SafeEstMDP-Map's, as it will tend to turn back as soon as its radiation reading begins to increase, whereas SafeEstMDP-Map's radiation model allows it in some cases to continue forwards cautiously.
%
On the other hand, the more aggressive baseline ($\baselinethreshold = 0.8$) explores more area, at the cost of far more runs terminating unsafely due to violating the safety specification.

SafeEstMDP-Map is able to use its GP model to explore more effectively across varied environments, pushing forwards when an unexplored area is likely to be safe, and remaining cautious when it is believed unsafe.
%
It also scales well with map size, as the EstMDP model is usually not much larger than the underlying navigation graph---even when the map is large, most waypoints will have been visited or be safe with probablility close to $1$, so only the outer areas of the map will have significant uncertainty over radiation level.
%
Consequently, the robot can evaluate dozens of goal choice waypoints per second during exploration, allowing for online planning with minimal delay, even in the research mine environment with maps of up to 150 waypoints.
%
In some radiation layouts, SafeEstMDP-Map does still violate the safety constraint, although this is likely due to the GP model failing to accurately model the radiation field.
%
This issue could be mitigated through further tuning or, more generally, applying domain knowledge in the design of the GP kernel.

\subsection{Hardware and system configuration}
\label{ssec:hardware}

For the real-world validation of our system,  we conducted experiments on a Boston Dynamics Spot robot, shown with its sensor payload in Figure~\ref{fig:spot}.
%
The robot's on-board computer was an Intel NUC with an Intel i7-8559U CPU, a \qty{256}{\giga\byte} SSD and \qty{16}{\giga\byte} of RAM.
%
For perception, the robot was equipped with an Ouster OS0-64 3D lidar with a full 360\textdegree\ field of view around the robot.
%
Real-time localisation and mapping of the environment were provided by the VILENS SLAM system~\cite{wisth2022vilens}, and---as in the simulated experiments---interfaces between modules were through ROS.


In these experiments, we use simulated radiation sources due to the health and safety  difficulties of working with real radiation.
%
Simulating radiation in real-world experiments presents the additional challenge that the radiation simulator must know the ground truth position of the robot in order to accurately simulate its radiation readings.
%
In fully simulated experiments, the ground truth robot position is readily available from the simulator.
%
However, there was no external ground truth source (e.g. a motion-capture system) available in the Corsham Research Mine.
%
To provide ground truth robot positioning, we therefore ran a separate instance of the VILENS SLAM system in localisation mode using a pre-built full coverage 3D map of the environment.
% %
Independently from the ground truth localisation system, an \emph{online} VILENS instance provided SLAM for each experiment run.
%
The navigation graph construction component (Algorithm \ref{alg:extend_graph}) used the coordinate system and OctoMap established by the online SLAM system, and had no access to the pre-built environment map.




\subsection{Deployment in a physical environment}
\label{ssec:deployment}
%
% NOTE: trials start date is 04/05
%

In order to illustrate the behaviour that can be obtained from the SafeEstMDP-Map in the real world, this section first steps through a successful run of the algorithm in the Corsham Research Mine.
%
The output from this particular run is shown in Figure~\ref{fig:corsham_deployment}.
%
We then also comment on the exploration performance of the system across further deployments.

\begin{figure}
    \centering
    \begin{subfigure}[b]{0.45\textwidth}
        \centering
        \includegraphics[width=\textwidth]{chapters/chSafeExp/figures/spot_2.jpg}
        \caption{Our Boston Dynamics Spot robot exploring the Corsham Research Mine in Wiltshire, UK.}
        \label{fig:spot}
    \end{subfigure}
    % \hfill
    % \hspace{1em}
    \fighspace
    \begin{subfigure}[b]{0.45\textwidth}
        \centering
        \includegraphics[width=\textwidth]{chapters/chSafeExp/figures/corsham_topdown_1.png}
        \caption{The map and topological graph constructed by SafeEstMDP-Map upon initialisation.}
        \label{fig:example_run:initial_topdown}
    \end{subfigure}
    \figvspace
    \begin{subfigure}[b]{0.45\textwidth}
        \centering
        \includegraphics[width=\textwidth]{chapters/chSafeExp/figures/corsham_topdown_3.png}
        \caption{The map and topological graph mid-way through the run.}
        \label{fig:example_run:intermediate_topdown}
    \end{subfigure}
    % \hfill
    % \hspace{1em}
    \fighspace
    \begin{subfigure}[b]{0.45\textwidth}
        \centering
        \includegraphics[width=\textwidth]{chapters/chSafeExp/figures/corsham_topdown_4_labelled.png}
        \caption{The final configuration and path followed by the robot during the run.}
        \label{fig:example_run:final_topdown}
    \end{subfigure}
    \caption[SafeEstMDP-Map real-world deployment example run]{Progression of a single run of SafeEstMDP-Map in the Corsham Research Mine. (b-d) show a 3D occupancy map representing the robot's knowledge of the world around it (filtered for visual clarity), as well as a topological graph constructed online for planning and navigation. The grayscale background represents (ground truth) log radiation levels as a heatmap, with darker regions corresponding to higher levels.}
    \label{fig:corsham_deployment}
\end{figure}



The robot started at the location indicated in Figure~\ref{fig:example_run:initial_topdown}, from which SafeEstMDP-Map constructed its initial topological map.
%
The map at this point extended at most only a few metres from the robot's position, in directions in which line of sight was unobstructed.
%
This area of the map had relatively low and uniform radiation levels, so the variation in the SafeEstMDP-Map scoring function (Equation~\ref{eq:score_safeestmdp_map}) was dominated by the volumetric gain term.
%
The robot's first plan was therefore to move up towards Location~2 in Figure~\ref{fig:example_run:final_topdown}, since the nodes in this direction were slightly more open than those to the right, and were therefore expected to provide more information for growing the map.

Upon reaching Location~2, the robot found the corridor blocked.
%
Additionally, now that its map of this area was more complete, there was very little unobserved space nearby, so all nearby nodes had volumetric gain values below the required threshold to act as exploration candidates.
%
Therefore, SafeEstMDP-Map turned back towards its other unexplored frontier at Location~3, where the nodes still had high exploration scores and were  considered safe.

From Location~3, the robot was able to map the entire ``room'' located below it (Figure~\ref{fig:example_run:intermediate_topdown}), thus there was no more unexplored space there and no need to explore further downwards.
%
Instead, the robot moved to the right, incrementally extending its map towards Location~4 as shown in the figure.
%
It is worth noting here that Figure~\ref{fig:example_run:final_topdown} does not show \emph{every} goal selected by SafeEstMDP-Map during the run, but rather a simplified intuitive representation of the path followed---in reality, when moving into unexplored territory, the algorithm tends to plan only a few nodes ahead at a time, since far-away nodes cannot be known with high confidence to be safe.

As the robot approached Location~4, the radiation levels it measured began to increase, as indicated by the darker patch in  Figure~\ref{fig:example_run:final_topdown}.
%
As this happened, SafeEstMDP-Map became more cautious, tending to generate plans that only moved the robot forwards one node at a time, or had it visit additional nodes near the frontier rather than pushing aggressively into the unknown region.
%
Ultimately, when the robot did reach Location~4, the unvisited nodes ahead of it were deemed unsafe based on the GP model.
%
The unvisited nodes in the previously explored areas of the map, on the other hand, were still safe to visit, but their volumetric gain values were below the required threshold.
%
There were therefore no more nodes that could serve as candidates for SafeEstMDP-Map, so the run terminated at Location~4.

This is overall a successful run of SafeEstMDP-Map, exhibiting the desired behaviours of the algorithm.
%
It balanced exploitation of volumetric gain information with safety, exploring more aggressively when confident about safety, and becoming more cautious as safety became less certain.

In total, three full runs of SafeEstMDP-Map were conducted in the Corsham Research Mine using the radiation layout shown in Figure~\ref{fig:corsham_deployment}.
%
In all runs, the system was able to plan online and successfully explore the majority of the safely accessible area.
%
With a total accessible area of approximately \qty{400}{\metre\squared}, the system achieved consistent results of  \qtylist{348.31;321.93;345.07}{\metre\squared} of the environment explored, without violating the safety specification.





\section{Discussion}
\label{sec:safe_exp_discussion}

SafeEstMDP-Map offers an efficient approach to safe exploration of completely unknown environments.
%
The goal selection process balances safety and cost-to-reach with \correction{exploration of the environment}, and combining this with a GP model of the safety hazard allows it to do this in a sensible way.
%
In Section~\ref{ssec:results_exploration}, we showed that rule-based approaches struggle to achieve this balance across different environments.
%
Exploring too cautiously can be highly inefficient, resulting in inefficient exploration or failure to explore the full safely reachable area altogether.
%
On the other hand, exploring too aggressively will often result in violations of the safety constraint, which could be dangerous or costly in real-world applications.

The robustness of our algorithm could be bolstered significantly by introducing better mechanisms for hyperparameter tuning.

Our experiments used manual tuning of the scoring function parameters (Equation~\ref{eq:score_safeestmdp_map}) in order to achieve effective exploration behaviour.
%
However, with a small amount of data from previous runs, it would be possible to select these parameters automatically, or even to adapt them online as the robot gains a better understanding of its environment.
%
Similarly, the use of more specialised GP kernels could also improve modelling accuracy, and therefore overall exploration performance.
%
This could be done either by directly incorporating domain knowledge into the kernel design, or by introducing techniques such as deep kernel learning~\cite{wilson2016deep} to learn more expressive kernel functions from data.


The incremental graph construction algorithm from Section~\ref{ssec:extend_graph} implicitly assumes a planar environment, in which the robot can navigate purely in the horizontal plane.
%
This is, of course, a highly limiting assumption which would preclude the use of the algorithm in many real-world environments where true 3D navigation is required.
%
However, the SafeEstMDP-Map algorithm itself is modular, and compatible with any approach for incremental graph construction.
%
Our approach could be extended to a broader set of challenging environments by integrating it with a more sophisticated, terrain-aware graph-based exploration algorithm~(e.g. \citep{dang2020graph}).


% Within the broader context of this thesis, this chapter addresses \challengeref{challenge:priors} by extending deployment capability to completely unknown environments.
% %
% Missions in highly uncertain or safety-critical environments, such as deep ocean monitoring or nuclear inspection, could use SafeEstMDP-Map to discover the safely reachable workspace, which could then form the basis for further monitoring.
% %
% In more dynamic environments, the monitoring and safe exploration components could also be fused more tightly to ensure ongoing safety and adaptability to changes.

\section{Summary}
\label{sec:safe_exp_summary}

We have presented the SafeEstMDP-Map algorithm for safe exploration and mapping of unknown environments in the presence of a GP-modelled hazard.
%
We have addressed \challengeref{challenge:priors}, extending previous work on safe exploration to remove reliance on a pre-existing topological map of the environment.
%
This paves the way for monitoring missions in completely unknown environments, enabling a robot to autonomously map the safely reachable area without putting itself at risk.
%
The work in this chapter also contributes towards addressing \challengeref{challenge:modelling}, reinforcing the effectiveness of GPs for modelling continuous environment processes in robot missions.
%
Our experiments validated the exploration approach in simulation, and demonstrated a successful deployment on a real robot.
