Toward Asymptotically-Optimal Inspection Planning via Efficient Near-Optimal Graph Search

TL;DR

This paper introduces IRIS, a novel sampling-based method for inspection planning that guarantees asymptotic optimality and significantly improves computational efficiency over previous approaches.

Contribution

IRIS is a new incremental graph search algorithm that densifies a motion planning roadmap to achieve asymptotically optimal inspection plans efficiently.

Findings

IRIS outperforms prior methods in plan quality and speed.

IRIS achieves asymptotic convergence to optimal inspection plans.

Demonstrated effectiveness on robotic inspection tasks in simulation and medical applications.

Abstract

Inspection planning, the task of planning motions that allow a robot to inspect a set of points of interest, has applications in domains such as industrial, field, and medical robotics. Inspection planning can be computationally challenging, as the search space over motion plans that inspect the points of interest grows exponentially with the number of inspected points. We propose a novel method, Incremental Random Inspection-roadmap Search (IRIS), that computes inspection plans whose length and set of inspected points asymptotically converge to those of an optimal inspection plan. IRIS incrementally densifies a motion planning roadmap using sampling-based algorithms, and performs efficient near-optimal graph search over the resulting roadmap as it is generated. We demonstrate IRIS's efficacy on a simulated planar 5DOF manipulator inspection task and on a medical endoscopic inspection task for a continuum parallel surgical robot in anatomy segmented from patient CT data. We show that IRIS computes higher-quality inspection paths orders of magnitudes faster than a prior state-of-the-art method.

Figures

Equations

$${\text{PP}}_1\oplus {\text{PP}}_2:=(P_1,(\min \{\ell (\widetilde{P_1}),\ell (\widetilde{P_2})\},\mathcal{S}(\widetilde{P_1})\cup \mathcal{S}(\widetilde{P_2}))).$$

Full text

I Introduction

In this work we investigate the problem of inspection planning, or coverage planning [2, 20]. Here, we consider the specific setting where we are given a robot equipped with a sensor and a set of points of interest (POI) in the environment to be inspected by the sensor. The problem calls for computing a minimal-length motion plan for the robot that maximizes the number of POI inspected. This problem has a multitude of diverse applications, including industrial surface inspections in production lines [47], surveying the ocean floor by autonomous underwater vehicles [4, 21, 28, 57], structural inspection of bridges using aerial robots [5, 6], and medical applications such as inspecting patient anatomy during surgical procedures [34], which motivates this work.

Naïvely-computed inspection plans may enable inspection of only a subset of the POI and may require motion plans orders of magnitude longer than an optimal plan, and hence may be undesirable or infeasible due to battery or time constraints. In medical applications, physicians may want to maximize the number of POI inspected for diagnostic purposes. Additionally, the procedure should be completed as fast as is safely possible to reduce costs and improve patient outcomes, especially if the patient is under anesthesia during the procedure. For example, a robot assisting in the diagnosis of the cause of a pleural effusion (a serious medical condition which causes the collapse of a patient’s lung) will need to visually inspect the surface of the collapsed lung and chest wall inside the body in as short a time as possible (see Fig. 1). We note that it may not be possible to inspect some POI due to obstacles in the anatomy and the kinematic constraints of the robot. Our goal is to compute kinematically-feasible collision free inspection plans that maximizes the number of POI inspected, and of the motion plans that inspect those POI we compute a shortest plan.

Inspection planning is computationally challenging because the search space is embedded in a high-dimensional configuration space $\mathcal{X}$ (space of all parameters that determine the shape of the robot) [11, 35, 36]. Even finding the shortest plan between two points in $\mathcal{X}$ that avoid obstacles (without reasoning about inspection) is computationally hard.111Computing the shortest plan for a point robot moving amidst polyhedral obstacles in 3D is $\mathsf{NP}$-hard, and many variants of the general motion planning problem are $\mathsf{PSPACE}$-hard. For further details, see [23]. If we want a minimum-length motion plan that maximizes the number of POI inspected, our problem is accentuated as we have to simultaneously reason about the system’s constraints, motion plan length, and POI inspected.

There are multiple approaches to computing inspection plans. Optimization-based methods locally search over the space of all inspection plans [5, 8]. Decoupled approaches first independently select suitable viewpoints and then determine a visiting sequence, i.e., a motion plan for the robot that realizes this sequence [12, 18]. Finally, recent progress in motion planning [30] has enabled methods to exhaustively search over the space of all motion plans [7, 29, 45] thus guaranteeing asymptotic optimality, a key requirement in many applications, including medical ones. Roughly speaking, asymptotic optimality for inspection planning means these methods produce inspection plans whose length and number of points inspected will asymptotically converge to those of an optimal inspection plan, given enough planning time.

Of all the aforementioned methods, only algorithms in the latter group provide any formal guarantees on the quality of the solution. This guarantee is achieved by attempting to exhaustively compute the set of Pareto-optimal inspection plans embedded in $\mathcal{X}$. In our setting, the set of Pareto-optimal inspection plans is the minimal set of inspection plans such that each plan is either shorter or has better coverage of the POI than any other inspection plan222More formally, a plan $P$ connecting two configurations $\mathbf{q},\mathbf{q}^{\prime}\in\mathcal{X}$ is said to be Pareto optimal in our setting if any other plan connecting $\mathbf{q}$ to $\mathbf{q}^{\prime}$ is either longer or does not inspect a point visible to $P$.. Unfortunately, this comes at the price of very long computation times as the size of the search space is exponential in the number of POI.

To this end, we introduce Incremental Random Inspection-roadmap Search (IRIS), a new asymptotically-optimal inspection-planning algorithm. IRIS incrementally constructs a sequence of increasingly dense roadmaps—graphs embedded in $\mathcal{X}$ wherein each vertex represents a collision-free configuration and each edge a collision-free transition between configurations—and computes an inspection plan on the roadmaps as they are constructed (see Fig. 2). Unfortunately, even the problem of computing an optimal inspection plan on a graph (and not in the continuous space) is computationally hard. To this end, our key insight is to compute a near-optimal inspection plan on each roadmap. Namely, we compute an inspection plan that is at most $1+\varepsilon$ the length of an optimal plan while covering at least $p$-percent of the number of POI (for any $\varepsilon\geq 0$ and $p\in(0,1]$). This additional flexibility allows us to improve the quality of our inspection plan in an anytime manner, i.e., the algorithm can be stopped at any time and return the best inspection plan found up until that point. We achieve this by incrementally densifying the roadmap and then searching over the densified roadmap for a near-optimal inspection plan—a process that is repeated as time allows. By reducing the approximation factor between iterations, we ensure that our method is asymptotically optimal.

The key contribution of our work is a computationally-efficient algorithm to compute provably near-optimal inspection plans on graphs. By pruning away large portions of the search space, this algorithmic building block enables us to dramatically outperform Rapidly-exploring Random Tree Of Trees (RRTOT) [7]—a state-of-the-art asymptotically-optimal inspection planner. Specifically, we demonstrate the efficacy of our approach in simulation for a continuum robot with complex dynamics—the needle-diameter Continuum Reconfigurable Incisionless Surgical Parallel (CRISP) robot [3, 40], working in a medically-inspired setting involving diagnosis of a pleural effusion (see Fig. 1).

II Related Work

II-A Sampling-based motion planning

Motion planning algorithms aim to compute a collision-free motion for a robot to accomplish a task in an environment cluttered with obstacles [23, 36, 39]. A common approach to motion planning is by sampling-based algorithms that construct a roadmap. Examples include the Probabilistic Roadmaps (PRMs) [31] (for solving multiple queries) and the Rapidly-exploring Random Trees (RRTs) [37] for solving single queries. These methods, and many variations thereof, are probabilistically complete—namely, the likelihood that they will find a solution, if one exists, approaches certainty as computation time increases.

Recent variations of these methods, such as PRM* and RRT* [30], improve upon this guarantee by exhibiting asymptotic optimality—namely that the quality of the solution obtained, given some cost function, approaches the global optimum as computation increases. Roughly speaking, this is achieved by increasing the (potential) edge set of roadmap vertices considered as its size increases [30, 55]. One such algorithm is the Rapidly-exploring Random Graphs (RRGs) [30] which will be used in our work. RRG combines the exploration strategy of RRT with an updated connection strategy that allows for cycles in the roadmap. It requires solving the two-point boundary value problem [36], which is only available for some robotic systems (including ours).

Guaranteeing asymptotic optimally can come with heavy computational cost. This inspired work on planners that trade asymptotic optimality guarantees with asymptotic near optimality (e.g., [38, 43, 52]). Asymptotic near optimality states that given an approximation factor $\varepsilon\geq 0$, the solution obtained converges to within a factor of $(1+\varepsilon)$ of the optimal solution with probability one, as the number of samples tends to infinity. Relaxing optimality to near optimality allows a method to improve the practical convergence rate while maintaining desired theoretic guarantees on the quality of the solution.

II-B Inspection planning

Many inspection-planning algorithms, or coverage planners, decompose the region containing the POI into multiple sub-regions, and then solve each sub-region separately [20]. These methods have limitations, however, such as when occlusions play a significant role in the inspection [17], or when kinematic constraints must be considered [15].

Other approaches simultaneously consider all POI. One approach decouples the problem into the coverage sampling problem (CSP) and the multi-goal planning problem (MPP), and solves each independently [5, 12, 15, 17, 18]. In CSP, a minimal set of viewpoints that provide full inspection coverage is computed. In MPP, a shortest tour that connects all the viewpoints is computed. These corresponds to solving the art gallery problem and the traveling salesman problem, respectively. Several of these variants have been shown to be probabilistically complete [17], but none provide guarantees on the quality of the final solution.

The set of viewpoints and the inspection plan itself can also be generated simultaneously. Papadopoulos et al. propose the Random Inspection Tree Algorithm (RITA) [45]. RITA takes into account differential constraints of the robot and computes both target points for inspection and the trajectory to visit the targets simultaneously. Bircher et al. propose Rapidly-exploring Random Tree Of Trees (RRTOT) which constructs a meta–tree structure consisting of multiple RRT* trees [7]. Both methods, which were shown to be asymptotically optimal, iteratively generate a tree, in which the inspection plan is enforced to be a plan from the root to a leaf node. However, each inspection plan does not consider configurations from other branches in the tree which may cause long planning times. This motivates our RRG-based approach.

II-C Path planning on graphs

Planning a minimal-cost path on a graph is a well studied problem. When the cost function has an optimal substructure (namely, when subpaths of an optimal path are also optimal), efficient algorithms such as Dijkstra [14], A* [24] and the many variants there-of can be used. However, in certain settings, including ours, this is not the case. For example Tsaggouris and Zaroliagis [58] consider the case where every edge has two attributes (e.g., cost and resource), and the cost function incorporates the attributes in a non-linear fashion.

Inspection planning also bears resemblance to multi-objective path planning. Here, we are given a set of cost functions and are required to find the set of Pareto-optimal paths [46]. Unfortunately, this set may be exponential in the problem size [16]. However it is possible to compute a fully polynomial-time approximation scheme (FPTAS) for many cases [59]. For additional results on path-planning with multiple-objectives or when the cost function does not have an optimal substructure, see e.g., [10, 50] and references within.

III Problem Definition

In this section we formally define the inspection planning problem. The robot operates in a workspace $\mathcal{W}\subset\mathbb{R}^{3}$ amidst a set of obstacles $\mathcal{W}_{\rm obs}\subset\mathcal{W}$. The robot’s configuration $\mathbf{q}$ is a $d$-dimensional vector that uniquely defines the shape of the robot (including, for example, rotation angles and translational extension of all joints). The set of all such configurations is the configuration space $\mathcal{X}$. The geometry of the robot is a configuration-dependent shape ${\rm Shape}(\mathbf{q})\subset\mathcal{W}$ and we say that $\mathbf{q}\in\mathcal{X}$ is collision free if ${\rm Shape}(\mathbf{q})\cap\mathcal{W}_{\rm obs}=\emptyset$. In this work we define a motion plan for the robot as a path $P$ in $\mathcal{X}$, which is represented as a sequence of $n$ configurations $\{\mathbf{q}_{0},\dots,\mathbf{q}_{n-1}\}$ (vertices) connected by straight-line segments (edges) in $\mathcal{X}$. And we say that $P$ is collision free if all configurations along $P$ (vertices and edges) are collision free. We assume that we have a distance function $\ell:\mathcal{X}\times\mathcal{X}\rightarrow\mathbb{R}$ and denote the length of a path $P$ as the sum of the distances between consecutive vertices, i.e., $\ell(P):=\sum_{i}\ell(\mathbf{q}_{i},\mathbf{q}_{i-1})$.

We assume that the robot is equipped with a sensor $\mathcal{S}$ and we are given a set of $k$ points of interest (POI) $\mathcal{I}=\{i_{1},\ldots,i_{k}\}$ in $\mathcal{W}$. We model the sensor as a mapping $\mathcal{S}:\mathcal{X}\rightarrow 2^{\mathcal{I}}$, where $2^{\mathcal{I}}$ is the power set of $\mathcal{I}$ and $S$ denotes the subset of $\mathcal{I}$ that can be inspected from each configuration. By a slight abuse of notation, given a path $P$ we set $\mathcal{S}(P):=\bigcup_{i=0}^{n-1}\mathcal{S}(\mathbf{q}_{i})$ and note that in our model, we only inspect $\mathcal{I}$ along the vertices of a path.

Definition 1

A point of interest $i\in\mathcal{I}$ is said to be covered by a configuration $\mathbf{q}\in\mathcal{X}$ or by a path $P$ if $i\in\mathcal{S}(\mathbf{q})$ or if $i\in\mathcal{S}(P)$, respectively. In such a setting, we say that $\mathbf{q}$ (or $P$) covers the point of interest $i$.

Given a start configuration $\mathbf{q}_{\rm s}\in\mathcal{X}$, POI $\mathcal{I}$, and a sensor model $\mathcal{S}$, the inspection planning problem calls for computing a collision-free path $P$ starting at $\mathbf{q}_{\rm s}$ which maximizes $|\mathcal{S}(P)|$ while minimizing $\ell(P)$. Note that this is not a bicriteria optimization problem—our primary optimization function is maximizing the coverage of our path. Out of all such paths we are interested in the shortest one.

IV Method Overview

In this section we provide an overview of IRIS—our algorithmic framework for computing asymptotically-optimal inspection plans. A key algorithmic tool in our approach is to cast the continuous inspection planning problem (Sec. III) to a discrete version of the problem where we only consider a finite number of configurations from which we inspect the POI, and a discrete set of feasible movements between those configurations. Thus, we start in Sec. IV-A by formally defining the graph inspection problem and then continue in Sec. IV-B to provide an overview of how IRIS builds and uses such graphs. We then describe the method in detail in Sec. V, and in Sec. VI show that IRIS’s solution converges to the length and coverage of an optimal inspection path.

IV-A Graph inspection problem

Similar to the (continuous) inspection problem, a graph inspection problem is a tuple $\left(\mathcal{G},\mathcal{I},\mathcal{S},\ell,v_{\rm s}\right)$ where $\mathcal{G}=(\mathcal{V},\mathcal{E})$ is a motion-planning roadmap (namely, a graph embedded in $\mathcal{X}$, in which every vertex $v\in\mathcal{V}$ is a configuration and every edge $(u,v)\in\mathcal{E}$ denotes the transition from configuration $u$ to $v$), $\mathcal{I}$ and $\mathcal{S}$ are defined as in Sec. III, $\ell:\mathcal{E}\rightarrow\mathbb{R}$ denotes the length of each edge in the roadmap, and $v_{\rm s}$ is the start vertex (corresponding to the start configuration $\mathbf{q}_{\rm s}$). A path $P$ on $\mathcal{G}$ is represented by a sequence of vertices $v_{i}\in\mathcal{V}$ such that $P=\{v_{0},\dots,v_{n-1}\}$, $v_{0}=v_{s}$ and $(v_{i},v_{i+1})\in\mathcal{E}$. It is important to note that there can be loops in a path, so it is possible that $v_{m}=v_{k}$ for $m\neq k$. The length and coverage of $P$ are defined as the total length of $P$’s edges and the set of all points inspected when traversing $P$, respectively. Namely, $\ell(P):=\sum_{i=0}^{n-2}\ell\left(v_{i},v_{i+1}\right)$ and $\mathcal{S}(P):=\bigcup_{v\in P}\mathcal{S}(v)$. The optimal graph inspection problem calls for a path $P^{*}$ that starts at $v_{\rm s}$ and maximizes the number of points inspected. Out of all such paths, $P^{*}$ has the minimal length. Finally, a path is said to be near-optimal for some $\varepsilon\geq 0$ and $p\in(0,1]$ if $|\mathcal{S}(P)|/|\mathcal{S}(P^{*})|\geq p$ and $\ell(P)\leq(1+\varepsilon)\cdot\ell(P^{*})$.

IV-B *Overview of *IRIS

Our algorithmic framework, depicted in Fig. 2, interleaves sampling-based motion planning and graph search. Specifically, we incrementally construct an RRT $\mathcal{T}$ rooted at $\mathbf{q}_{\rm s}$ which implicitly defines a corresponding RRG $\mathcal{G}$. All edges in $\mathcal{T}$ are checked for collision with the environment during its construction (so the roadmap is guaranteed to be connected) while all the other edges of $\mathcal{G}$ are not explicitly checked for collision. Lazy edge evaluation, common in motion-planning algorithms [9, 25, 13, 51], allows us to defer collision detection until absolutely necessary and reduce computational effort. This is critical in our domain of interest where computing $\rm{Shape}()$ typically dominates algorithms’ running times [44].

The roadmap $\mathcal{G}=(\mathcal{V},\mathcal{E})$ induces the subset of the POI that can be inspected, denoted as $\mathcal{I}_{\mathcal{G}}:=\bigcup_{v\in\mathcal{V}}\mathcal{S}(v)$. Given two approximation parameters $\varepsilon\geq 0$ and $p\in(0,1]$, we compute a near-optimal inspection path for the graph-inspection problem $\left(\mathcal{G},\mathcal{I}_{\mathcal{G}},\mathcal{S},\ell,v_{\rm s}\right)$ by casting the problem as a graph-search problem on a different graph $\mathcal{G}_{\mathcal{S}}$ (to be defined shortly).

As we add vertices and edges to $\mathcal{T}$ incrementally, the roadmap $\mathcal{G}$ is incrementally densified. In addition, we tighten approximations by decreasing $\varepsilon$ and increasing $p$ between iterations. As we will see (Sec. VI), this will ensure that IRIS is asymptotically optimal.

V Method

In this section we detail the different components of IRIS. Sec. V-A and V-B describe how we construct a roadmap and then search it, respectively. After describing in Sec V-C how we modify the approximation parameters used by IRIS, we conclude in Sec. V-D with implementation details.

V-A Roadmap construction

We construct a sequence of graphs embedded in $\mathcal{X}$. Specifically, denote the RRT constructed at the $i$’th iteration as $\mathcal{T}_{i}$ defined over the set of vertices $\mathcal{V}_{i}$. We start with an empty tree rooted at $\mathbf{q}_{\rm s}$ and at the $i$’th iteration sample a random configuration, compute it’s nearest neighbor in $\mathcal{T}_{i}$, and extend that vertex toward the random configuration. If that extension is collision free we add it to the tree. If not, we repeat this process (see [33, 36] for additional details regarding RRT).

The tree $\mathcal{T}_{i}$ implicitly-defines an RRG $G_{i}=(\mathcal{V}_{i},\mathcal{E}_{i})$ defined over the same set of vertices where every vertex is connected to all other vertices within distance $r_{i}$. Here, we define $r_{i}$ as in [54, Thm. IV.5] which will allow us to prove that our approach is asymptotically optimal (see Sec. VI).

V-B Graph inspection planning

We use the RRG described in Sec. V-A to define a graph inspection problem, and then compute near-optimal inspection paths over this graph. Before describing how we compute near-optimal inspection paths, we first describe how we compute optimal paths given a graph inspection problem.

V-B1 Optimal planning

Given a graph inspection problem $\left(\mathcal{G},\mathcal{I}_{\mathcal{G}},\mathcal{S},\ell,v_{\rm s}\right)$, we compute optimal inspection paths by formulating our inspection problem as a graph-search problem on an inspection graph $\mathcal{G}_{\mathcal{S}}:=(\mathcal{V}_{\mathcal{S}},\mathcal{E}_{\mathcal{S}})$. Here, vertices are pairs comprised of a vertex $u\in\mathcal{V}$ in the original graph and subsets of $\mathcal{I}_{\mathcal{G}}$. Namely, $\mathcal{V}_{\mathcal{S}}=\mathcal{V}\times 2^{\mathcal{I}_{\mathcal{G}}}$, and note that $|\mathcal{V}_{\mathcal{S}}|=O\left(|\mathcal{V}|\cdot 2^{|\mathcal{I}_{\mathcal{G}}|}\right)$. An edge $e$ between vertices $(u,\mathcal{I}_{u})$ and $(v,\mathcal{I}_{v})$ exists if $(u,v)\in\mathcal{E}$ and $\mathcal{I}_{u}\cup\mathcal{S}(v)=\mathcal{I}_{v}$. If it exists, its cost is simply $\ell(u,v)$.

Any (possibly non-simple) path $P_{G}$ in the original graph $G$ from $v_{\rm s}$ to $u$ can be represented by a corresponding path $P_{\mathcal{G}_{\mathcal{S}}}$ in the inspection graph $\mathcal{G}_{\mathcal{S}}$, from $(v_{\rm s},\mathcal{S}(v_{\rm s}))\in\mathcal{V}_{\mathcal{S}}$ to $(u,\mathcal{S}(P_{G}))\in\mathcal{V}_{\mathcal{S}}$, and $\ell(P_{G})=\ell(P_{\mathcal{G}_{\mathcal{S}}})$. Thus, our algorithm will run an A*-like search from $(v_{\rm s},\mathcal{S}(v_{\rm s}))\in\mathcal{V}_{\mathcal{S}}$ to any vertex in the goal set $\mathcal{V}_{\rm goal}=\{(v,\mathcal{I}_{\mathcal{G}})|v\in\mathcal{V}\}$. An optimal inspection path is the shortest path between $(v_{\rm s},\mathcal{S}(v_{\rm s}))$ and any vertex in $\mathcal{V}_{\rm goal}$. For a visualization, see Fig. 3. Note that here we assume the graph $\mathcal{G}$ is connected and that the set of points to be inspected is $\mathcal{I}_{\mathcal{G}}$. This implies that an optimal inspection path always exists.

We can speed up this naïve algorithm using the notion of dominance, which is used in many shortest-path algorithms (see, e.g., [53]). In our context, given two paths $P,P^{\prime}$ in our original roadmap $\mathcal{G}$ that start and end at the same vertices, we say that $P$ dominates $P^{\prime}$ if $\ell(P)\leq\ell(P^{\prime})$ and $\mathcal{S}(P)\supseteq\mathcal{S}(P^{\prime})$. Clearly, $P$ is always preferred over $P^{\prime}$. Thus, when searching $\mathcal{G}_{\mathcal{S}}$, if we compute a shortest path to some node $(u,\mathcal{I}_{u})$ of length $\ell_{u}$, we do not need to consider any path of length larger than $\ell_{u}$ from all vertices $(u,\mathcal{I}_{u}^{\prime})$ such that $\mathcal{I}_{u}^{\prime}\subseteq\mathcal{I}_{u}$. For pseudo-code describing a general A*-like search algorithm to optimally solve the graph-inspection problem, see Alg. 1 without lines 17-27.

While path domination may prune away paths in the open list of the A*-like search, this algorithm is hardly practical due to the exponential size of the search space (recall that $|\mathcal{V}_{\mathcal{S}}|=O(|\mathcal{V}_{\mathcal{S}}|\cdot 2^{|\mathcal{I}_{\mathcal{G}}|})$). In the next sections, we show how to prune away large portions of the search space by extending the notion of dominance to approximate dominance.

V-B2 Near-optimal planning

Let $P,P^{\prime}$ be two paths in $\mathcal{G}$ that start and end at the same vertices and let $\varepsilon\geq 0$ and $p\in(0,1]$ be some approximation parameters.

Definition 2

We say that path $P$ $\varepsilon,p$-dominates path $P^{\prime}$ if $\ell(P)\leq(1+\varepsilon)\cdot\ell(P^{\prime})$ and $|\mathcal{S}(P)|\geq p\cdot|\mathcal{S}(P)\cup\mathcal{S}(P^{\prime})|$.

Note that it is possible that both $P$ $\varepsilon,p$-dominates $P^{\prime}$ and $P^{\prime}$ $\varepsilon,p$-dominates $P$. For a visualization of the notions of dominance and the approximate dominance, see Fig. 4.

Intuitively, approximate dominance allows to dramatically prune the search space by only considering paths that can significantly improve the quality (either in terms of length or the set of points inspected) of a given path. When pruning away (strongly-) dominated paths, it is clear that they cannot be useful in the future. However, if we prune away approximate-dominated paths, we need to efficiently account for all paths that were pruned away in order to bound the quality of the solution obtained. We do this using the notion of potentially-achievable paths or PAP’s.

Definition 3

A potentially-achievable path (PAP) $\tilde{P}$ to some vertex $u\in\mathcal{V}$ is a pair $(\tilde{\ell},\tilde{\mathcal{I}})$ such that $\tilde{\ell}\geq 0$ and $\mathcal{S}(u)\subseteq\tilde{\mathcal{I}}\subseteq\mathcal{I}_{\mathcal{G}}$. By a slight abuse of notation, we extend the definitions of $\ell(\cdot)$ and $\mathcal{S}(\cdot)$ such that $\ell(\tilde{P})=\tilde{\ell}$ and $\mathcal{S}(\tilde{P})=\tilde{\mathcal{I}}$.

It may seem that a PAP is simply a path but note (as the name PAP suggests) that we do not require that there exists any path $P$ from $v_{\rm{s}}$ to $u$ such that $\ell(P)=\ell(\tilde{P})$ and $\mathcal{S}(P)=\mathcal{S}(\tilde{P})$. It merely states that such a path may exist.

We now use PAP’s to define the notion of a path pair:

Definition 4

Let $P$ and $\tilde{P}$ be a path and a PAP from $v_{\rm{s}}$ to some $v\in\mathcal{V}$ such that $\ell(\tilde{P})\leq\ell(P)$ and $\mathcal{S}(\tilde{P})\supseteq\mathcal{S}(P)$. Their path pair is PP $:=(P,\tilde{P})$ and we call $P$ and $\tilde{P}$ the achievable and potentially-achievable paths of PP, respectively.

Let us define operations on PAP’s and on PP’s, visualized in Fig. 5. The first operation we consider is extending a PAP $\tilde{P_{u}}$ by an edge $e=(u,v)$, denoted as $\tilde{P_{u}}+e$. This can be thought of as appending $e$ to $\tilde{P_{u}}$, had it existed and thus accounting for the length $\ell(e)$ and additional coverage $\mathcal{S}(v)$. Formally, extending $\tilde{P_{u}}+e$ yields a PAP $\tilde{P_{v}}$ such that $\ell(\tilde{P_{u}})=\ell(\tilde{P_{v}})+\ell(e)$ and $\mathcal{S}(\tilde{P_{u}})=\mathcal{S}(\tilde{P_{v}})\cup\mathcal{S}(u)$. Extending the path pair $\textsf{PP}_{u}=(P_{u},\tilde{P_{u}})$ by the edge $e=(u,v)$ (denoted as $\textsf{PP}_{u}+e$) simply extends both $P_{u}$ and $\tilde{P_{u}}$ by $e$. This yields the path pair $\textsf{PP}_{v}=(P_{v},\tilde{P_{v}})$ where $\ell({P_{v}})=\ell({P_{u}})+\ell(e)$, $\mathcal{S}({P_{v}})=\mathcal{S}({P_{u}})\cup\mathcal{S}(v)$ and $\tilde{P_{v}}=\tilde{P_{u}}+e$.

The second operation is subsuming a path pair by another one which can be thought of as constructing a PAP that dominates the PAPs of both path pairs. Formally, Let $\textsf{PP}_{1}=(P_{1},\tilde{P_{1}})$ and $\textsf{PP}_{2}=(P_{2},\tilde{P_{2}})$ be two path pairs such that both connect the start vertex $v_{\rm s}$ to some vertex $u\in\mathcal{V}$. The path pair defined by $\textsf{PP}_{1}$ subsuming $\textsf{PP}_{2}$ is

[TABLE]

We now define the notion of bounding a path pair which will be crucial for ensuring near-optimal solutions:

Definition 5

A path pair $\textsf{PP}:=(P,\tilde{P})$ is said to be $\varepsilon,p$-bounded for some $\varepsilon\geq 0$ and $p\in(0,1]$ if $P$ $p,\varepsilon$-dominates $\tilde{P}$.

To compute a near-optimal inspection path (Alg. 1 and Fig. 6), we extend each path considered by our search algorithm to be a path pair and use this additional data to prune away approximately-dominated paths. Similar to A*, our algorithm uses two priority queues OPEN and CLOSED to track the nodes considered by the search. It starts by inserting the start vertex $(v_{\rm s},\mathcal{S}(v_{\rm s}))$ to the OPEN list together with the path pair ${\textsf{PP}}_{s}=(P_{\rm s},P_{\rm s})$ (here $P_{\rm s}$ is a path containing only start vertex $v_{\rm s}$) (line 2).

Our algorithm proceeds in a similar fashion to A*—we pop the most promising node $n=(u,\mathcal{I}_{u},\textsf{PP}_{u})$ from OPEN (line 4) and move it to CLOSED (line 5). If the PAP of this node is in the goal set $\mathcal{V}_{{\rm goal}}$ (line 6), we terminate the search and return the achievable path of $\textsf{PP}_{u}$ (line 7). If not, we consider all neighboring edges $e$ of $u$ in $\mathcal{G}$ and extend the node $n$ (line 9). This is akin to computing $n$’s neighbors in $\mathcal{G}_{\mathcal{S}}$.

At this point our algorithm deviates from the standard A* algorithm. For each newly-created node $(v,\mathcal{I}_{v},\textsf{PP}_{v})$ we check if there exists a node in CLOSED that dominates it. If so, this node is discarded (lines 11-14). If no such node exists in the CLOSED list, we check if there exists a node in OPEN that may subsume it. If so, that node is updated and this node is discarded (line 17-21). Finally, we check if this node can subsume nodes that are in OPEN. If so, such nodes are discarded and this node is updated. (line 24-27).

It is straightforward to see that (i) the first path pair is $\varepsilon,p$-bounded and hence by induction (ii) all path pairs in the search are $\varepsilon,p$-bounded. Furthermore, when the algorithm terminates, it has found a valid solution whose potentially-achievable path is in the goal set. This yields the following corollary:

Corollary 1

Alg. 1 computes a path $P$ that $\varepsilon,p$-dominates the optimal inspection path $P^{*}$. Namely, that $\ell(P)\leq(1+\varepsilon)\cdot\ell(P^{*})$ and $|\mathcal{S}(P)|\geq p\cdot|\mathcal{S}(P)\cup\mathcal{S}(P^{*})|$.

V-C Tightening approximation factors

Recall that our algorithm starts with approximation parameters $p_{0}$ and $\varepsilon_{0}$. We endow our algorithm with a tightening factor $f\in(0,1]$, and at the $i$’th iteration we set our approximation parameters as $p_{i}=p_{i-1}+f\cdot(1-p_{i-1})$ and  $\varepsilon_{i}=\varepsilon_{i-1}+f\cdot(0-\varepsilon_{i-1})$. As we will see (Sec. VI), the tightening allows our method to achieve asymptotic optimality.

V-D Implementation details

V-D1 Lazy computation in graph inspection planning

We run our search algorithm on $\mathcal{G}$ (Alg. 1) without checking if its edges are collision free or not (recall that only the edges of $\mathcal{T}$ were explicitly checked for collision). Once a solution is found, we start checking edges one by one until the entire path was found to be collision free or until one edge is found to be in collision, in which case we remove it from the edge set. This approach is common to speed up motion-planning algorithms when edges are expensive to evaluate [13, 22].

V-D2 Node extension in graph inspection planning

Any optimal inspection path can be decomposed into a sequence of vertices that improve the coverage of the path called milestones. It is straightforward to see that an optimal inspection path will (i) terminate at a milestone and (ii) connect a pair of milestones via a shortest path in $\mathcal{G}$. Following this observation, instead of extending each path $P$ from a vertex $u$ by all outgoing edges in $\mathcal{G}$ (Alg. 1, line 8), we run a Dijkstra-like search from $u$ and collect all first-met vertices that can be milestones.

V-D3 Heuristic computation in graph inspection planning

Recall that A* orders nodes in the OPEN list according to their computed cost-to-come added to a heuristic estimate of their cost to reach the goal. The heuristic function that we use for vertex $(u,\mathcal{I}_{u})$ is computed as follows: we run a Dijkstra search on $\mathcal{G}$ from $u$ and consider the vertices $\mathcal{V}_{u}$ encountered during the search. We terminate when $\left(\bigcup_{v\in\mathcal{V}_{u}}\mathcal{S}(v)\right)\cup\mathcal{I}_{u}=\mathcal{I}_{G}$ and use the maximal distance between $u$ to any node in $\mathcal{V}_{u}$ as our admissible [24] heuristic function.

VI Theoretical Guarantees

We provide a proof sketch showing that, asymptotically, the length and coverage of the path produced by IRIS will converge to the length and coverage of an optimal inspection path. For ease of exposition, we state our results for the following simplified variant of IRIS where we start with an empty roadmap $G_{0}$ and some initial approximation factors $\varepsilon_{0}$ and $p_{0}$. At each iteration $i$ we (i) sample a collision-free configuration $q_{i}$ uniformly at random from $\mathcal{X}$, (ii) add $q_{i}$ to roadmap $G_{i}$ and connect it to all samples within radius $r_{i}$, and (iii) compute a near-optimal inspection path on this roadmap with parameters $\varepsilon_{i}$ and $p_{i}$. (Namely, instead of implicitly constructing an RRG, we implicitly construct a PRM. While not identical, both roadmaps exhibit similar properties which are typically easier to show for PRMs.)

Roughly speaking, the connection radius $r_{i}$ was chosen to ensure that as $i\rightarrow\infty$ an optimal inspection path may be traced arbitrarily well by the roadmap $\{G_{i}\}_{i=1}^{\infty}$. The approximation factors were chosen such that $\varepsilon_{i}>\varepsilon_{i+1}$, $p_{i}<p_{i+1}$, \raisebox{2.15277pt}{\scalebox{0.8}{\displaystyle\lim_{i\rightarrow\infty};}}\varepsilon_{i}=0 and \raisebox{2.15277pt}{\scalebox{0.8}{\displaystyle\lim_{i\rightarrow\infty};}}p_{i}=1. This will ensure that as $i\rightarrow\infty$ the inspection path found will converge to an optimal inspection path in $G_{i}$.

A key result that we rely on is probabilistic exhaustivity (see, [54, Thm. IV.5] and [26, 56]). Roughly speaking, it is the notion that given a sufficiently large set of uniformly sampled configurations, any path can be traced arbitrarily well.

Finally, we assume that for every configuration along an optimal inspection path, there exists a neighborhood of configurations that share the same visibility. This is critical as we will not be able to exactly trace an optimal inspection path but only to iteratively approximate it. When this assumption holds we say that our inspection problem is well behaved.

The combination of (i) probabilistic exhaustivity, (ii) the $\varepsilon,p$-dominance of our graph inspection algorithm (Cor. 1) (iii) that \raisebox{2.15277pt}{\scalebox{0.8}{\displaystyle\lim_{i\rightarrow\infty};}}\varepsilon_{i}=0 and \raisebox{2.15277pt}{\scalebox{0.8}{\displaystyle\lim_{i\rightarrow\infty};}}p_{i}=1, and (iv) that our inspection problem is well behaved ensures that asymptotically, our algorithm will converge to an optimal inspection path.

VII Results

We evaluated IRIS on two simulated scenarios: (1) a planar manipulator inspecting the boundary of a square region (Fig. 7(a)) and (2) a CRISP robot inspecting the inner surface of a pleural cavity (Fig. 7(b)). All tests were run on a 3.4GHz 8-core Intel Xeon E5-1680 CPU with 64GB of RAM.

VII-A Planar manipulator scenario

In this scenario, depicted in Fig. 7(a), we have a 5-link planar manipulator fixed at its base that is tasked with inspecting the boundary of a rectangular 2D workspace. We start by evaluating IRIS for fixed $p$ and $\varepsilon$ and then compare it with RRTOT using our approach for dynamically reducing the approximation factors. For every set of parameters we ran ten experiments for 1000 seconds and report the average value together with the standard deviation.

When $p=1$ and we vary $\varepsilon$ (Fig. 8(a)), we can see that even small approximation factors (e.g., $\varepsilon=0.5$) allow to dramatically increase the coverage obtained as each search episode takes less time and more configurations can be added to the RRT tree. While using $\varepsilon=0$ did not result in $80\%$ coverage even after 1000 seconds, this was achieved within one second for $\varepsilon\geq 1.0$. This comes at the price of slightly longer inspection paths. When $\varepsilon=0$ and we vary $p$ (Fig. 8(b)), we get roughly the same coverage per time but at the price of much longer paths for higher values of $p$.

Following the above discussion, when reaching high coverage is the sole objective, one should use large initial values of $p_{0}$ and $\varepsilon_{0}$. When we want initial solutions to also be short, one should start with smaller approximation factors. We compared IRIS with different initial approximation factors to RRTOT [7] (Fig. 8(c)). We can see that our approach allows to produce higher-quality paths than RRTOT. For example, IRIS obtains more than a $2450\times$ speedup when compared to RRTOT when producing the same quality of inspection planning for the case of roughly $83\%$ coverage and path length of $53$ units. Final inspection paths obtained by IRIS are both shorter and inspect larger portions of $\mathcal{I}$.

VII-B Pleural effusion scenario for the CRISP robot

The anatomical pleural effusion environment for this simulation scenario was obtained from a Computed Tomography (CT) scan of a real patient suffering from this condition, and a fine discretization of the internal surface of the pleural cavity is used as the set of POI. We also use the internal surface of the cavity as obstacles, and prohibit the robot from colliding with the pleural surface, lung, and chest wall (except at tube entry points). Pleural effusion volumes can be geometrically complex, as the way in which the lung separates from the chest wall can be inconsistent. This results in unseparated regions of the lung’s surface that can inhibit movement and occlude the sensor from visualizing areas further in the volume.

Here we consider a CRISP robot with two tubes, where a snare tube is grasping a camera tube in order to create a parallel structure made of thin, flexible tubes. Each tube can be independently rotated in three dimensions about its entry point into the body, and independently translated into and out of the cavity. The system has 8 degrees of freedom with a configuration space of $\mathcal{SO}(3)^{2}\times\mathbb{R}^{2}$, which enables the parallel structure to move in a manner that enables obstacle avoidance as well as precise control of the camera’s pose.

We ran IRIS and RRTOT for this scenario ten different times for 10,000 seconds (Fig. 9). Similar to the planar manipulator scenario, IRIS allows to produce higher-quality paths than RRTOT. For example, IRIS obtains more than a $25\times$ speedup when compared to RRTOT when producing the same quality of inspection planning for the case of roughly $32\%$ coverage and path length of $1.2$ units.

VIII Conclusion and Future Work

In this work we presented IRIS, an algorithmic framework for computing asymptotically-optimal inspection plans. Our key contribution is an algorithm to efficiently compute near-optimal inspection plans on graphs. Interestingly, our problem of graph-inspection planning lies on the intersection between single and bi-criteria shortest path problems. Clearly, we are computing a shortest path on the inspection graph $\mathcal{G}_{\mathcal{S}}$. However, en route we compute an approximation of the set of Pareto optimal paths to every node in the original graph $\mathcal{G}$. Thus, we believe that our approach may be useful for the general problem of bicriteria optimization.

We showed IRIS outperforms the prior state-of-the-art, including in a medical application in which a surgical robot inspects a tissue surface inside the body as part of a diagnostic procedure. However, the efficiency of IRIS can be further improved. We now highlight several avenues where such improvement could be obtained.

VIII-A Dynamic updates in graph inspection planning

IRIS reruns Alg. 1 every iteration which may be highly inefficient as we would like to reuse information constructed from previous search episodes. Indeed, the general case where a graph undergoes a series of edge insertions and edge deletions and we wish to update a shortest-path algorithm is a well- studied problem referred to as the fully dynamic single source shortest-path problem [19, 48]. Efficient algorithms exist even when running an A*-like search [32]. Thus, an immediate next step to improve the efficiency of our algorithm is to adapt the aforementioned algorithms to the case of near-optimal graph inspection planning.

VIII-B Balancing graph search and lazy computation

Recall that we employ a lazy search paradigm when computing near-optimal inspection plans on the inspection graph (Sec. V-D). This was done because edge evaluation is computationally complex. However, as the number of iterations increases, search starts to dominate the overall running time of our algorithm and not edge evaluation (see Fig. 10). Recently Mandalika et al. [41, 42] presented an algorithm that balances edge evaluation and graph search when edges are expensive to evaluate using the notion of lazy look-ahead. Thus, we suggest to use their method dynamically varying the so-called lazy look-ahead—In the initial stages of the algorithm, when search is not a bottleneck, employ a large look-ahead (which corresponds to a performing more search). As the algorithm progress, reduce the look-ahead to account for the fact that edge-evaluation is relatively cheaper than graph search.

VIII-C Efficient sampling of configurations in RRT construction

Recall that in our RRT constructions we sample configurations uniformly at random from $\mathcal{X}$. Common implementations of RRT typically employ a goal bias where configurations from the goal are sampled with some probability [36]. Similarly, we suggest to bias sampling towards configurations that increase coverage. Namely, to configurations $\mathbf{q}$ such that $\mathcal{S}(\mathbf{q})\cup\mathcal{I}_{G}\neq\emptyset$. We suspect that the goal bias should be dynamically changed—when the inspection graph $\mathcal{G}_{\mathcal{S}}$ has low coverage the bias should be high. As the overall coverage of $\mathcal{G}_{\mathcal{S}}$ increase, the goal bias should be reduced to allow for shorter inspection plans.

VIII-D Employing multiple heuristics in graph inspection planning

As the number of iterations increases, graph search dominates the running time of our algorithm. Heuristics have been shown to be an effective tool in speeding up search algorithms and we suggest to employ recent developments from the search community to speed this part of our framework. One such development is using multiple heuristics to guide the search in a systematic way [1] that has shown to be an effective tool in robot planning algorithms [27, 49].

Roughly speaking, using multiple heuristics allows to encode domain knowledge without having to worry about the heuristic functions being admissible. In our setting, we are simultaneously reasoning about inspection coverage and plan length in our graph inspection planning. Thus, it may be beneficial to design one (or more) heuristics that account for path length and one (or more) heuristics that account for path coverage. Then we could apply a method similar to MHA* [1] to combine the efforts of these heuristics.

VIII-E Adaptively updating approximation parameters

In our work we used a simplistic approach to update the approximation parameters. These may have a dramatic effect on the quality of plans produced. We suggest to further inspect how to update these parameters, possibly doing this in a dynamic fashion according to information obtained from previous search episodes.

Acknowledgment

This research was supported in part by the National Institutes of Health under award R01EB024864 and the National Science Foundation under award IIS-1149965.