In this work, we investigate the problem of inspection planning, or coverage planning (Almadhoun et al. 2016;Galceran and Carreras 2013). 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 surface inspections in industrial production lines (Raffaeli et al. 2013), surveying the ocean floor by autonomous underwater vehicles (Bingham et al. 2010;Gracias et al. 2013;Johnson-Roberson et al. 2010;Tivey et al. 1997), structural inspection of bridges using aerial robots (Bircher et al. 2015(Bircher et al. , 2016)), and medical applications such as inspecting patient anatomy during surgical procedures (Kuntz et al. 2018).
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 a robot's battery capacity 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 that can cause 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. 1a). In structural inspecting applications, Unmanned Aerial Vehicles (UAVs), or drones, can be used to efficiently inspect the complex geometry of built structures. High-quality inspection plans could reduce inspection time and reduce costs. Bridge inspection (see Fig. 1b), for example, is critical to ensuring bridge safety since almost 40% of the bridges in the United States exceed their 50-year design life (ASCE 2016). We note that it may not be possible to inspect some POI due to obstacles in the environment and the kinematic constraints of the robot. Our goal is to compute kinematically feasible collision-free inspection plans that maximize 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 X (the space of all parameters that determine the shape of the robot) (Choset et al. 2005;Latombe 1991;LaValle 2006). Even finding the shortest plan between two points in X that avoid obstacles (without (Anderson et al. 2017;Mahoney et al. 2016) is composed of needle-diameter tubes assembled into a parallel structure inside the patient's body (in which a tube uses a snare system to grip a tube with a camera affixed to its tip) and then robotically manipulated outside the body, allowing for smaller incisions and faster recovery times compared to traditional endoscopic tools (which have larger diameters). Middle: The CRISP robot in simulation inspecting a collapsed lung, a scenario segmented from a CT scan of a real patient with this condition. The visualization shows the robot (orange), the lungs (pink), and the pleural surface visible (green) and not visible (blue) by the robot's camera sensor in its current configuration. Right: Two example configurations with inspected POI. The CRISP robot (orange) inspects POI (blue) on the organ surface, and visible points are covered by the cone shape (yellow). (b) Inspection planning for bridge structures. A UAV, shown as a blue sphere, inspects points on the surface of a bridge structure. At a configuration, some points are visible (shown in green) while other points are not visible (shown in orange).
reasoning about inspection) is computationally hard. * If we want to compute a minimum-length motion plan that maximizes the number of POI inspected, the complexity of our problem is increased 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 (Bircher et al. 2015;Bogaerts et al. 2018). 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 (Danner and Kavraki 2000;Englot and Hover 2011). Finally, recent progress in motion planning (Karaman and Frazzoli 2011) has enabled methods to exhaustively search over the space of all motion plans (Bircher et al. 2017;Kafka et al. 2016;Papadopoulos et al. 2013) thus guaranteeing asymptotic optimality, an important feature in many applications, including medical ones. Roughly speaking, asymptotic optimality for inspection planning means these methods produce inspection plans whose length and the 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 X for which full coverage has not been obtained. Namely, for every configuration x 2 X , they (asymptotically) compute the set of paths ⇧ x starting from a given start configuration x s such that 8⇡ 1 , ⇡ 2 2 ⇧ x , either ⇡ 1 is shorter than ⇡ 2 and ⇡ 2 covers POI not covered by ⇡ 1 or vice versa. Once ⇧ x contains a path ⇡ ⇤
x that covers all POI, this path is considered as a candidate solution. 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 plan. † 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 Inspectionroadmap Search (IRIS), a new asymptotically optimal inspection-planning algorithm. IRIS incrementally constructs a sequence of increasingly dense roadmaps-graphs embedded in X wherein each vertex represents a collisionfree 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 + " the length of an optimal plan while covering at least ppercent of the number of POI (for any " 0 and p 2 (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 Fu et al.
Initialize RRT.
, .
= 0 p = p 0 Extend RRT.
Define RRG implicitly using RRT.
Construct inspection graph from RRG.
Compute near-optimal inspection plan P.
Update and . 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. Coupled with our method for generating this graph, this algorithmic building block enables us to dramatically outperform Rapidly-exploring Random Tree Of Trees (RRTOT) (Bircher et al. 2017)-a state-of-theart asymptotically optimal inspection planner. Specifically, we demonstrate the efficacy of our approach in simulation for several complex robotic systems (Fig. 1) , including a continuum robot with complex kinematics-the needlediameter Continuum Reconfigurable Incisionless Surgical Parallel (CRISP) robot (Anderson et al. 2017;Mahoney et al. 2016), working in a medically inspired setting involving the diagnosis of a pleural effusion.
In this paper, we are providing a refined version of our results in Fu et al. (2019), and two important extensions. First, we show results for a simulated bridge inspection task with a UAV, where the inspection target has a complex structure and the underlying roadmap to compute a nearoptimal inspection plan on is much larger (e.g. having more nodes and edges). Second, we prove that IRIS is asymptotically optimal under very general system and environment assumptions. These extensions show that IRIS can be used for general complex-structure inspection while providing provable guarantees.
Motion planning algorithms aim to compute a collisionfree motion for a robot to accomplish a task in an environment cluttered with obstacles (Halperin et al. 2018;LaValle 2006;Lynch and Park 2017). A common approach to motion planning is by sampling-based algorithms that construct a roadmap. Examples include the Probabilistic Roadmaps (PRMs) (Kavraki et al. 1996) (often for solving multiple queries) and the Rapidly-exploring Random Trees (RRTs) (LaValle and Kuffner 2001) 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* (Karaman and Frazzoli 2011), 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 (Karaman and Frazzoli 2011;Solovey et al. 2018). One such algorithm is the Rapidly-exploring Random Graphs (RRGs) (Karaman and Frazzoli 2011) 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 twopoint boundary value problem (LaValle 2006), which is only efficiently solvable for some robotic systems (including ours).
Guaranteeing asymptotic optimality can come with a heavy computational cost. This inspired work on planners that trade asymptotic optimality guarantees with asymptotic near optimality (e.g., Li et al. (2016); Marble and Bekris (2011); Salzman and Halperin (2016)). Asymptotic near optimality states that given an approximation factor " 0, the solution obtained converges to within a factor of (1 + ") 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.
Many inspection-planning algorithms, or coverage planners, decompose the region containing the POI into multiple subregions and then solve each sub-region separately (Galceran and Carreras 2013). These methods have limitations, however, such as when occlusions play a significant role in the inspection (Englot and Hover 2012), or when kinematic constraints must be considered (Edelkamp et al. 2017).
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 (Bircher et al. 2015;Danner and Kavraki 2000;Edelkamp et al. 2017;Englot andHover 2012, 2011). 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 correspond to solving the art gallery problem and the traveling salesman problem, respectively. Several of these variants have been shown to be probabilistically complete (Englot and Hover 2012), 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. (2013) propose the Random Inspection Tree Algorithm (RITA). RITA takes into account the differential constraints of the robot and computes both target points for inspection and the trajectory to visit the targets simultaneously. Bircher et al. (2017) propose Rapidly-exploring Random Tree Of Trees (RRTOT) which constructs a meta-tree structure consisting of multiple RRT* trees. 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 RRGbased approach.
Planning a minimal-cost path on a graph is a wellstudied problem. When the cost function has an optimal substructure (namely, when subpaths of an optimal path are also optimal), efficient algorithms such as Dijkstra (Dijkstra 1959), A* (Hart et al. 1968), and the many variants thereof can be used. However, in certain settings, including ours, this is not the case. For example, Tsaggouris and Zaroliagis (2004) 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.
It is worth mentioning the idea of progressively tightening the approximation factor was also adopted in some anytime A* algorithms based on weighted A* (Pohl 1970), including Anytime Repairing A* (ARA*) by Likhachev et al. (2003) and Restarting Weighted A* (RWA*) by Richter et al. (2010). Anytime Nonparametric A* (ANA*) by van den Berg et al. (2011) furthermore gets rid of the explicit approximation parameter and performs solution improvement adaptively. Nevertheless, as these methods are based on weighted A*, a prerequisite for good performance is a high-quality heuristic, which is not easy to obtain in the case of inspection planning due to the lack of optimal substructure. Furthermore, these methods focus on static graphs (while in our case the graph is incrementally updated) and consider only a single objective (while in our case we have two objectives, inspection and path length). When looking at multiple objectives (though still considering static graphs), recent work by Zhang et al. (2022) extends the approach presented in this paper to suggest an anytime approximate bi-criteria search algorithm.
Inspection planning also bears resemblance to multiobjective path planning. Here, we are given a set of cost functions and are required to find the set of Paretooptimal paths (Pardalos et al. 2008). Unfortunately, this set may be exponential in the problem size (Ehrgott and Gandibleux 2000). However, it is possible to compute a fully polynomial-time approximation scheme (FPTAS) for many cases (Tsaggouris and Zaroliagis 2009). For additional results on path planning with multiple objectives or when the cost function does not have an optimal substructure, see e.g., (Chen and Nie 2013;Reinhardt and Pisinger 2011;Hernández et al. 2023) and references within.
In this section, we formally define the inspection planning problem. The robot operates in a workspace W ⇢ R 3 amidst a set of obstacles W obs ⇢ W. The robot's configuration 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 X . The geometry of the robot is a configuration-dependent shape Shape(q) ⇢ W, here, Shape(•) is a mapping from configuration space X to workspace W, determining the subset of the workspace that the robot occupies for a given configuration. We say that q 2 X is in collision if Shape(q) \ W obs 6 = ;. Let X obs ✓ X be the obstacle space, such that X \ X obs is an open set. Then the collision free space is defined as X free = cl(X \ X obs ), where cl(•) is closure of a set. In this work we define a motion plan for the robot as a path P in X , which is represented as a sequence of n configurations {q 0 , . . . , q n 1 } (vertices) connected by straight-line segments (edges) in 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 `: X ⇥ X ! R and denote the length of a path P as the sum of the distances between consecutive vertices, i.e., `(P ) := P i `(q i , q i 1 ). We assume that the robot is equipped with a sensor S and we are given a set of k points of interest (POI) I = {i 1 , . . . , i k } in W. We model the sensor as a mapping S : X ! 2 I , where 2 I is the power set of I and S denotes the subset of I that can be inspected from each configuration. By a slight abuse of notation, given a path P we set S(P ) := S n 1 i=0 S(q i ) and note that in our model, we only inspect I along the vertices of a path. Definition 1. Inspection coverage. A point of interest i 2 I is said to be covered by a configuration q 2 X or by a path P if i 2 S(q) or if i 2 S(P ), respectively. In such a setting, we say that q (or P ) covers the point of interest i.
Given a start configuration q s 2 X , POI I, and a sensor model S, the inspection planning problem calls for computing a collision-free path P starting at q s which maximizes |S(P )| while minimizing `(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.
In this section, we provide an overview of IRISour algorithmic framework for computing asymptotically optimal inspection plans. A key algorithmic tool in our approach is to cast the continuous inspection planning problem (Sec. 3) 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. The assumption that inspection of POI only happens at vertices is not significantly limiting since a robot's motion can be approximated by multiple vertices, and many inspection applications require non-zero time to complete a high-quality sensor measurement (e.g., to obtain non-blurry images, highaccuracy lidar scans, etc.), which can be effectively encoded at vertices. Thus, we start in Sec. 4.1 by formally defining the graph inspection problem and then continue in Sec. 4.2 to provide an overview of how IRIS builds and uses such graphs. We then describe the method in detail in Sec. 5, and in Sec. 6 show that IRIS's solution converges to the length and coverage of an optimal inspection path.
Similar to the (continuous) inspection problem, a graph inspection problem is a tuple (G, I, S, `, v s ) where G = (V, E) is a motion-planning roadmap (namely, a graph embedded in X , in which every vertex v 2 V is a configuration and every edge (u, v) 2 E denotes the transition from configuration u to v), I and S are defined as in Sec. 3, `: E ! R denotes the length of each edge in the roadmap, and v s is the start vertex (corresponding to the start configuration q s ). A path P on G is represented by a sequence of vertices v i 2 V such that P = {v 0 , . . . , v n 1 },
It is important to note that there can be loops in a path, so it is possible that v m = v k for m 6 = 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, `(P ) := P n 2 i=0 `(v i , v i+1 ) and S(P ) := S v2P S(v). The optimal graph inspection problem calls for a path P ⇤ that starts at v 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 " 0 and p 2 (0, 1] if
)| p and `(P ) (1 + ") • `(P ⇤ ).
Our algorithmic framework, depicted in Fig. 2, interleaves sampling-based motion planning and graph search. Specifically, we incrementally construct an RRT T rooted at q s which implicitly defines a corresponding RRG G. All edges in 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 G are not explicitly checked for collision. Lazy edge evaluation, common in motion-planning algorithms (Bohlin and Kavraki 2000;Hauser 2015;Dellin and Srinivasa 2016;Salzman and Halperin 2015), allows us to defer collision detection until absolutely necessary and reduce computational effort. This is critical in our domain of interest where computing Shape(•), the geometry of the robot as is defined in Sec. 3, typically dominates algorithms' running times (Niyaz et al. 2018).
The roadmap G = (V, E) induces the subset of the POI that can be inspected, denoted as I G := S v2V S(v). Given two approximation parameters " 0 and p 2 (0, 1], we compute a near-optimal inspection path for the graphinspection problem (G, I G , S, `, v s ) by casting the problem as a graph-search problem on a different graph G S (to be defined shortly).
As we add vertices and edges to T incrementally, the roadmap G is incrementally densified. In addition, we tighten approximations by decreasing " and increasing p between iterations. As we will see (Sec. 6), this will ensure that IRIS is asymptotically optimal.
In this section, we detail the different components of IRIS. Sec. 5.1 and 5.2 describe how we construct a roadmap and then search it, respectively. After describing in Sec 5.3 how we modify the approximation parameters used by IRIS, we conclude in Sec. 5.4 with implementation details.
We construct a sequence of graphs embedded in X . Specifically, denote the RRT constructed at the i'th iteration as T i defined over the set of vertices V i . We start with an empty tree rooted at q s and at the i'th iteration sample a random configuration, compute its nearest neighbor in 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 Kuffner and LaValle (2000); LaValle (2006) for additional details regarding RRT).
Note that it is not necessary to add only one collisionfree configuration in each roadmap update. Adding multiple configurations in one iteration does not hurt the theoretical guarantees while providing us more room to improve an inspection plan in terms of both inspection and path length. There are different possible strategies to balance between roadmap construction and graph search on the roadmap. One example is Fu et al. (2021) where a condition on additional inspection coverage is used to trigger graph searches on the updated roadmap. As the major focus of this paper is to provide a theoretical foundation for the proposed algorithm framework, we only discuss the variant where configurations are added one at a time.
The tree T i implicitly defines an RRG G i = (V i , 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 Lemma 3 which will allow us to prove that our approach is asymptotically optimal (see Sec. 6).
We use the RRG described in Sec. 5.1 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.
Given a graph inspection problem (G, I G , S, `, v s ), we compute optimal inspection paths by formulating our inspection problem as a graph-search problem on an inspection graph G S := (V S , E S ). Here, vertices are pairs comprised of a vertex u 2 V in the original graph and subsets of
Any (possibly non-simple) path P G in the original graph G from v s to u can be represented by a corresponding path P G S in the inspection graph G S , from (v s , S(v s )) 2 V S to (u, S(P G )) 2 V S , and `(P G ) = `(P G S ). Thus, our algorithm will run an A*-like search from (v s , S(v s )) 2
An optimal inspection path is the shortest path between (v s , S(v s )) and any vertex in V goal . For a visualization, see Fig. 3. Note that here we assume the graph G is connected and that the set of points to be inspected is I 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., Salzman et al. (2017)). In our context, given two paths P, P 0 in our original roadmap G that start and end at the same vertices, we say that P dominates P 0 if `(P ) `(P 0
) and S(P ) ◆ S(P 0 ). Clearly, P is always preferred over P 0 . Thus, when searching G S , if we compute a shortest path to some node (u, I u ) of length `u, we do not need to consider any path of length larger than `u from all vertices (u, I 0 u ) such that I 0 u ✓ 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
)). 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.
To perform near-optimal planning, we introduce the idea of approximate dominance. Approximate dominance is a relaxed version of the (strong) dominance mentioned above, characterized by approximation parameters.
Let P, P 0 be two paths in G that start and end at the same vertices and let " 0 and p 2 (0, 1] be some approximation parameters.
Definition 2. ", p-domination. We say that path P ", p-
Note that it is possible that both P ", p-dominates P 0 and P 0 ", p-dominates P . For a visualization of the notions of dominance and the approximate dominance, see Fig. 4.
Intuitively, approximate dominance allows us 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 POI 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. If we prune away approximate-dominated paths without any special consideration, the "errors" introduced by each domination accumulate. To bound the accumulated "error", during the search, we need to consistently keep track of "what is the best inspection path if we do not perform approximate dominations that happened so far"? Getting the best inspection paths precisely is equivalent to optimal planning. Thus, we use estimations and name such estimations potentially achievable paths or PAP s.
Definition 3. Potentially achievable path. A potentially achievable path (PAP) P to some vertex u 2 V is a pair ( ˜, Ĩ) such that ˜ 0 and S(u) ✓ Ĩ ✓ I G . By a slight abuse of notation, we extend the definitions of `(•) and S(•) such that `( P ) = ˜and S( P ) = Ĩ.
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 s to u such that `(P ) = `( P ) and S(P ) = S( 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. Path pair. Let P and P be a path and a PAP from v s to some v 2 V such that `( P ) `(P ) and S( P ) ◆ S(P ). Their path pair is PP := (P, P ) and we call P and 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 Pu by an edge e = (u, v), denoted as Pu + e. This can be thought of as appending e to Pu , had it existed and thus accounting for the length `(e) and additional coverage S(v). Formally, extending Pu + e yields a PAP Pv such that `( Pu ) = `( Pv ) + `(e) and S( Pu ) = S( Pv ) [ S(u). Extending the path pair PP u = (P u , Pu ) by the edge e = (u, v) (denoted as PP u + e) simply extends both P u and Pu by e. This yields the path pair PP v = (P v , Pv ) where `(P v ) = `(P u ) + `(e), S(P v ) = S(P u ) [ S(v) and Pv = Pu + 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 PP 1 = (P 1 , P1 ) and PP 2 = (P 2 , P2 ) be two path pairs such that both connect the start vertex v s to some vertex u 2 V. The path pair defined by PP 1 subsuming PP 2 is
We now define the notion of bounding a path pair which will be crucial for ensuring near-optimal solutions: Definition 5. ", p-bounded. A path pair PP := (P, P ) is said to be ", p-bounded for some " 0 and p 2 (0, 1] if P ", p-dominates 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 s , S(v s )) to the OPEN list together with the path pair PP s = (P s , P s ) (here P s is a path containing only start vertex v s ) (line 2). Inspected points Our algorithm proceeds in a similar fashion to A*-we pop the most promising node n = (u, I u , PP u ) from OPEN (line 4) and move it to CLOSED (line 5). If the PAP of this node is in the goal set V goal (line 6), we terminate the search and return the achievable path of PP u (line 7). If not, we consider all neighboring edges e of u in G and extend the node n (line 9). This is akin to computing n's neighbors in G S .
At this point, our algorithm deviates from the standard A* algorithm. For each newly created node (v, I v , PP v ) we check if there exists a node in CLOSED whose PAP dominates Pv . 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).
We prove that, Alg. 1 returns a path that ", p-dominates an optimal inspection path on the roadmap (see Sec. 6).
Recall that our algorithm starts with approximation parameters p 0 and " 0 . We endow our algorithm with a tightening factor f 2 (0, 1], and at the i'th iteration we set our approximation parameters as
. As we will see (Sec. 6), since lim i!1 p i = 1, lim i!1 " i = 0, the tightening allows our method to achieve asymptotic optimality.
Algorithm 1 Near-optimal inspection graph search Input: (G S , v s , V goal , ", p)
29: return NULL 5.4 Implementation details 5.4.1 Lazy computation in graph inspection planning We run our search algorithm on G (Alg. 1) without checking if its edges are collision-free or not (recall that only the edges of 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
The International Journal of Robotics Research 42(4-5) set. This approach is common to speed up motion-planning algorithms when edges are expensive to evaluate (Dellin and Srinivasa 2016;Haghtalab et al. 2018).
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 G. Following this observation, instead of extending each path P from a vertex u by all outgoing edges in G (Alg. 1, line 8), we run a Dijkstra-like search from u and collect all first-met vertices that can be milestones.
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, I u ) is computed as follows: we run a Dijkstra search on G from u and consider the vertices V u encountered during the search. We terminate when S v2Vu S(v) [ I u = I G and use the maximal distance between u to any node in V u as our admissible (Hart et al. 1968) heuristic function. We now show why the heuristic is admissible. When we terminate with S v2Vu S(v) [ I u = I G , denote the last vertex added to V u as v last . Note that v last is also the most distant (via roadmap edges) vertex from u due to the nature of Dijkstra's algorithm. According to the termination condition, there exists a nonempty set of POI I last that is covered by v last but not covered by any other vertices in V u . Namely, I last ⇢ S(v last ) and
. To achieve an inspection coverage of I G , the minimum distance to travel is equal to or greater than the distance between u and v last because with any shorter path, I last is not covered.
In this section, we provide theoretical properties showing that IRIS is guaranteed to be asymptotically optimal, given that the system and the environment satisfy several general assumptions. For brevity, we only state the main definitions, lemmas, and theorems and defer all proofs to Appendix A and Appendix B. Recall that IRIS iteratively densifies the roadmap and runs a near-optimal inspection graph search on the latest roadmap. Thus, we begin (Thm. 1) by proving that our graph-inspection search algorithm (Alg. 1) returns a near-optimal solution when compared to the optimal solution (available for that specific roadmap). We then continue (Thm. 2) to show that IRIS is asymptotically optimal when the approximation parameters satisfy that lim N !1 " N = 0 and
where N is the number of vertices in the roadmap. This is done by using the notion of probablistic exhaustivity (Schmerling et al. 2015) coupled together with the assumptions that (i) an optimal inspection trajectory is well behaved (Def. 8) (ii) an optimal inspection trajectory weak cl -clearance and that (iii) the inspection problem is regular (Def. 10). It is worth noting that all proofs are general enough to account for complex robotic systems, including those having differential constraints. Having said that, as we use a graph-based search method, we require a method is required to compute valid motions (if such motions exist) between two close-by configurations.
We start by proving that Alg. 1 is near-optimal. Definition 6. Optimal inspection path on a roadmap. Let (G, I G , S, `, v s ) be a graph inspection problem. An optimal inspection path P ⇤ is a path on roadmap G, starting at v s , and satisfies
) and S ⇤ := S(P ⇤ ) = I G , denote the length and coverage of an optimal path, respectively. Lemma 1. In Alg. 1 (near-optimal inspection graph search), all path pairs in OPEN and CLOSED during the search are ", p-bounded. Lemma 2. Let P ⇤ = {v 0 , v 1 , . . . , v n ⇤ } be an optimal inspection path and denote P ⇤
[i] := {v 0 , . . . , v i }, for i 2 [0, n ⇤ ] as the path composed of the first i waypoints of P ⇤ . During every iteration of Alg. 1, there exists a path pair PP v = (P v , Pv ) in OPEN and an index i such that v = v i and Pv strictly dominates P ⇤
[i]. Namely, `( P ) `(P ⇤ [i]) and S(P ⇤
[i]) ✓ S( P ).
With Lemma 1 and 2, we can show that Alg. 1 returns a near-optimal result. Theorem 1. Near-optimal inspection graph search. Nearoptimal inspection graph search (Alg. 1) computes a path P that ", p-dominates an optimal inspection path P ⇤ . Namely,
) and |S(P )| p • |S(P ⇤ )|.
We now continue to prove that IRIS is asymptotically optimal. To prove this, we show that an optimal inspection path x ⇤ can be approximated by a sequence of configurations sampled by IRIS, given certain conditions. Specifically, we will need to show that as the number of iterations approaches infinity, the following requirements hold: (i) the length of the path induced by this sequence of samples converges asymptotically to the length of x ⇤ , (ii) the coverage obtained by this sequence of samples converges asymptotically to the coverage of x ⇤ and that (iii) our inspection graph search algorithm finds such a sequence of samples. To do so, we rely on the notion of probabilistic exhaustivity (Schmerling et al. 2015). Roughly speaking, it is the notion that given a sufficiently large set of uniformly sampled configurations, any path can be traced arbitrarily well in the configuration space by a path defined as a sequence of configurations.
Lemma 3. Probabilistic exhausitivity. Let x : [0, T ] ! X free be a dynamically feasible trajectory. Let Q N be a set of N points sampled independently and identically from the uniform distribution on the collision-free space X free and set
Here, D is a constant capturing the dimension of the system and is a commutable constant depending on the system dynamics, N , D, and some tuning parameter ⌘ 0. Let ÃN be the event that there exists a discrete sequence of configurations P = {q i } n i=1 ✓ V N that ( , r)-traces x for any 2 (0, 1) and r = r N . The probability that event ÃN doesn't happen, denoted by P( Ãc N ) is asymptotically bounded by
As is defined above, ⌘ 0 is some tuning parameter.
In addition, we assume that x ⇤ is well-behaved. Roughly speaking, this ensures that there are no singular points along the trajectory where a POI can only be inspected from. This, in turn, will allow us to ensure that trajectories that trace an optimal inspection path cover the same set of POI.
Definition 7. Inspecting configuration region. Let i 2 I be a point of interest (POI), the inspecting configuration region of i, denoted as X insp (i), is defined to be the union of all configurations from which the POI can be inspected. Namely,
Similarly, the inspecting configuration region of I 0 ✓ I is defined as
Definition 8. Well-behaving of an inspection trajectory. Let
x : [0, T ] ! X free be a feasible inspection trajectory.
x is said to be strongly ⇠-well behaved if 8i 2 S(x), there exist at least one point along x whose ⇠-neighborhood is completely within the inspecting configuration region of i. Namely,
Similarly, x is said to be weakly ⇠-well behaved if 8i 2 S(x)/ (S(x(0)) [ S(x(T ))), there exists at least one point along x whose ⇠-neighborhood is completely within the inspecting configuration region of i. Namely,
It is not hard to see, that any strongly well-behaved trajectory can be shortened to a weakly well-behaved trajectory without loosing coverage. Thus an optimal inspection trajectory can only be weakly well-behaved.
We further require the inspection-planning problem to be regular. The notion of regularity is required because for a weakly well-behaved optimal inspection trajectory x ⇤ : [0, T ] ! X free we need to take special care to cover the POI inspected at x ⇤ (T ). The notion of regularity will ensure that there always exists a region near x ⇤ (T ) that IRIS can sample inside.
Definition 9. Regular boundary. A set X 0 ✓ X free is said to have a regular boundary if there exists > 0 such that 8q 2 @X , there exists q 0 2 X with B e (q 0 , ) ✓ X 0 and q 2 @B e (q 0 , ). Definition 10. Regularity of an inspection-planning problem. Let P = (X , I, S, `, q s ) be an inspection-planning problem and X free be the set of collision-free configurations. P is said to be regular if 8q 2 X free , X insp (S(q)) has a regular boundary.
Similar to many other analyses of sampling-based planning algorithms (see, e.g., Kavraki et al. (1996); Karaman and Frazzoli (2011); Solovey et al. ( 2018)), we also assume that an optimal trajectory to trace has clearance from X obs .
Definition 11. Strong/weak cl -clearance. Let x : [0, T ] ! X free be a feasible trajectory. x has strong cl -clearance if 8t 2 [0, T ], x(t) is in cl -interior of X free (namely, x(t) is at least cl away from any point in X obs using the Euclidean distance). Furthermore, x has weak cl -clearance if there exists a sequence of homotopic paths {x k } k2N that satisfies:
(iii) 8k 2 [0, 1), x k is dynamically feasible and has strong k -clearance for some k > 0, and lim k!1 k = 0. (iv) lim k!1 `(x k ) = `(x). We are finally ready to state our final theorem.
Theorem 2. IRIS asymptotic optimality.
Let P = (X , I, S, `, q s ) be a regular inspection-planning problem and X free be the collision-free space. Assume that the robot system satisfies the assumptions mentioned in Schmerling et al. (2015) and Let x ⇤ : [0, T ] ! X free be an optimal feasible inspection trajectory such that (i) x ⇤ (0) = q s , (ii) x ⇤ has weak cl -clearance for some cl > 0, (iii) x ⇤ is weakly ⇠-well behaved for some ⇠ > 0.
Furthermore, let `i and S i denote the arc length, and the inspection coverage, respectively of the trajectory returned by IRIS at the N 'th iteration using approximation parameters " N and p N , and a lower-bound connecting radius r N (as defined in Lemma 4). If lim N !1 " N = 0 and
for any > 0 and that
We evaluated IRIS on three simulated scenarios: (1) a planar manipulator inspecting the boundary of a square region (Fig. 7a), (2) an unmanned aerial vehicle (UAV) inspecting the outer surface of a bridge (Fig. 7b), and (3) a CRISP robot inspecting the inner surface of a pleural cavity (Fig. 7c).
For all experiments, we order path pairs in OPEN (Alg. 1 line 4) according to the path pair with the minimal potentially achievable path cost. All tests were run on a 3.4GHz 8-core Intel Xeon E5-1680 CPU with 64GB of RAM.
In this scenario, depicted in Fig. 7a, we have a 5-link planar manipulator fixed at its base that is tasked with inspecting the boundary of a rectangular 2D workspace, which is discretized into 400 POI. The sensor is a camera attached to the tip of the manipulator, aligning with the direction of the robot's final segment. When modeling the camera for inspection, we consider a field of view (FOV) of 45 degrees and an unbounded effective inspecting distance. We start by evaluating IRIS for fixed p and " 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. This scenario serves as a simple example where we compare the approximation algorithm for inspection planning with optimal inspection planning (i.e., p = 1, " = 0). When p = 1, indicating we do not allow any approximation on inspection coverage, and we vary " (Fig. 8a), we can see that even small approximation factors (e.g., " = 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 optimal inspection planning (using " = 0) did not result in 80% coverage even after 1000 seconds, this was achieved within one second for " 1.0. This comes at the price of slightly longer inspection paths. When " = 0, indicating we do not allow any approximation on path length, and we vary p (Fig. 8b), 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 " 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 (Bircher et al. 2017), see Fig. 8c. We can see that our approach allows producing higher-quality paths than RRTOT. For example, IRIS obtains more than a 264⇥ speedup when compared to RRTOT when producing the same quality of inspection planning for the case of roughly 85% coverage and path length of 85.8 units. Final inspection paths obtained by IRIS are both shorter and inspect larger portions of I.
In this scenario, depicted in Fig. 7b, a UAV equipped with a camera inspects the surface of a bridge structure. The bridge structure is obtained from a 3D mesh (Elkassar 2008) and discretized into 3817 POI. The bridge structure serves as both an inspection target and an obstacle that may block movements and occlude sensing. And we do not consider other environmental obstacles except for the ground, which means the UAV can only fly above the ground.
The UAV has a configuration space of SO(2) ⇥ R 3 . It first can translate in 3D space, then can rotate around its vertical axis, and finally, the camera can rotate around the pitch axis. When modeling the camera for inspection, we consider a FOV of 90 degrees and an effective inspecting distance of 10 meters.
We ran IRIS and RRTOT for this scenario ten different times for 10,000 seconds (Fig. 9). IRIS obtains more than a 8⇥ speedup when compared to RRTOT when producing a better quality of inspection planning for the case of roughly 57% coverage and path length of 230 units, which is only 68% of that of RRTOT.
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 containing 49506 points. 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 SO(3) 2 ⇥ 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 model the camera with a 60-degree FOV and an unbounded effective inspecting distance.
We ran IRIS and RRTOT for this scenario ten different times for 10,000 seconds (Fig. 10). Similar to the planar manipulator scenario, IRIS allows producing higher-quality paths than RRTOT. For example, IRIS obtains more than a 79⇥ speedup when compared to RRTOT when producing the same quality of inspection planning for the case of roughly 33% coverage and path length of 0.3 units.
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 in the intersection between single and bi-criteria shortest path problems. Clearly, we are computing a shortest path on the inspection graph G S . However, en route, we compute an approximation of the set of Pareto optimal paths to every node in the original graph 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.
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 (Frigioni et al. 2000;Ramalingam and Reps 1996). Efficient algorithms exist even when running an A*-like search (Koenig et al. 2004). 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.
Recall that we employ a lazy search paradigm when computing near-optimal inspection plans on the inspection graph (Sec. 5.4). This was done because edge evaluation is computationally complex. However, as the number of iterations increases, the search starts to dominate the overall running time of our algorithm and not edge evaluation (see Fig. 11). Recently Mandalika et al. (2018Mandalika et al. ( , 2019) ) 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 using their method dynamically varying the so-called lazy look-ahead-in the initial stages of the algorithm, when the search is not a bottleneck, employ a large look-ahead (which corresponds to 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.
Recall that in our RRT constructions we sample configurations uniformly at random from X . Common implementations of RRT typically employ a goal bias where configurations from the goal are sampled with some probability LaValle ( 2006). Similarly, we suggest biasing sampling towards configurations that increase coverage. Namely, to configurations q such that S(q) [ I G 6 = ;. We suspect that the goal bias should be dynamically changed-when the inspection graph G S has low coverage the bias should be high. As the overall coverage of G S increases, the goal bias should be reduced to allow for shorter inspection plans.
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 employing recent developments from the search community to speed up this part of our framework. One such development is using multiple heuristics to guide the search in a systematic way (Aine et al. 2016) that has shown to be an effective tool in robot planning algorithms (Islam et al. 2018;Ranganeni et al. 2018). Roughly speaking, using multiple heuristics allows encoding 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* (Aine et al. 2016) to combine the efforts of these heuristics.
Prepared using sagej.cls
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 further inspecting how to update these parameters, possibly doing this in a dynamic fashion according to information obtained from previous search episodes.
", p-bounded path pair extended by edge e = (u, v) to obtain the path pair PP v (line 9). Let us consider both the length and coverage of PP v . By definition of the extending operation we have that (i) `(P v ) = `(P u ) + `(e) and (ii) `( Pv ) = `( Pu ) + `(e). Thus:
Here, the first and last equalities follow from the definition of the extend operator, the first inequality follows from the fact that PP u is ", p-bounded.
Moving to the coverage of the resultant path pair, by definition of the extend operator we have that (i) S(P v ) = S(P u ) [ S(v) and (ii) S( Pv ) = S( Pu ) [ S(v). Thus:
Here S(P ) := I G \ S(P ), the first and last equalities follow from the definition of the extend operation, the second inequality follows from the fact that PP u is ", p-bounded. Thus, an ", p-bounded PP will remain ", p-bounded after the extending operation. ⌅ Lemma 2. Let P ⇤ = {v 0 , v 1 , . . . , v n ⇤ } be an optimal inspection path.
] as the path composed of the first i waypoints of P ⇤ . During every iteration of Alg 1, there exists a path pair PP v = (P v , Pv ) in the open list and an index i such that v = v i and Pv strictly dominates P ⇤
[i]. Namely, `( P ) `(P ⇤ [i]) and S(P ⇤
[i]) ✓ S( P ). Proof. We will prove a slightly stronger claim showing that at every iteration of Alg 1 there exists a path pair PP v = (P v , Pv ) in the open list such that the following two properties hold: P1 There exists an index i such that v = v i and Pv strictly dominates P ⇤ [i]. P2 There is no index j > i such that there is another path pair PP u = (P u , Pu ) in either the open or the closed list where u = v j and Pu strictly dominates
Property P1 is exactly what we need to prove for Lemma 2 while property P2 simplifies the proof. The proof will be by induction on the iterations of Alg. 1. Induction base: In line 2, the open set is initialized with the path pair PP s = (P s , P s ). P s trivially dominates P ⇤
[0] (property P1). Furthermore, there is no path in the closed list so property P2 holds trivially. Induction step: Assume that at iteration i, there exists some index n i 2 [0, n ⇤ ] and some path pair
Note that following property P2, there is no other path pair PP u = (P u , Pu ) in either the open or the closed list and an index n j > n i such that u = v nj and Pu strictly dominates P ⇤
[n j ]. There are two cases to consider-either PP ni was popped from the open list or not.
C1 First, we consider the case where PP ni was popped from the open list. Consider the set V nbr of neighbors ‡ There may be more than one optimal path and it is possible that n ⇤ |V| since P ⇤ may revisit vertices on G.
The International Journal of Robotics Research 42(4-5) of v ni that lie on P ⇤ . Namely,
Note that V nbr is not empty as e i+1 = (v ni , v ni+1 ) 2 E. For each vertex v ni+k 2 V nbr , denote PP ni+k = (P ni+k , Pni+k ) the path pair that will be generated after extending the edge (v ni , v ni+k ) (line 9). Let k ⇤ be the maximal index such that (i) v ni+k ⇤ 2 V nbr and (ii) Pni+k ⇤ strictly dominates P ⇤
[n i + k ⇤ ]. Note that k ⇤ exists since Pni+1 strictly dominates P ⇤
[n i + 1] (this is easily verified using the induction hypothesis). Assume that PP ni+k ⇤ was the first path pair that was generated § (line 8) and that the induction hypothesis holds prior to this event. It is straightforward to see that both properties hold for PP ni+k ⇤ . Now, we need to consider the following sub-cases:
C1.1 P ni+k ⇤ is strictly dominated by some path
] and recall that Pni+k ⇤ strictly dominates
] in contradiction to property P2 meaning that this cannot occur. ]. This, in turn, implies that both properties hold.
C1.3 PP ni+k ⇤ is subsuming some other path in the open list (lines 24-27). Similar to C1.2, the resulting path pair from subsuming operation has potentially achievable path that strictly dominates Pni+k ⇤ , thus strictly dominates
Again, this implies that both properties hold.
being subsumed or subsuming any other path pair. Here the induction hypothesis trivially holds.
We now need to consider all other path pairs generated throughout. However, non can result in a path pair that subsumes PP ni+k ⇤ or is subsumed by PP ni+k ⇤ meaning that the induction hypothesis still holds.
C2 Now consider the case that PP ni was not popped from the priority queue. Some other path pair was popped from the priority and extended and pushed into the closed list. Out of all newly created path pairs (line 9) let PP nj be the one for which property P1 holds and for which the index n j is maximal. If no such path pair exists then the induction hypothesis continues to hold. Again, we will consider several sub-cases.
C2.1 If n j < n i , then both property P1 and P2 still hold for PP ni .
C2.2 If n j = n i ¶ , then either (i) PP ni was subsumed by PP nj , (ii) PP nj was subsumed by PP ni or (iii) no path pair was subsumed. It is straightforward to see that in all cases both property P1 and P2 still hold for the newly created path pair in cases (i) and (ii) or for both path pairs PP ni and PP nj on case (iii).
C2.3 If n j > n i , then both property P1 and P2 hold for PP nj .
Similar to C1, we need to consider all other path pairs generated throughout. However, non can result in a path pair that subsumes PP nj or is subsumed by PP nj meaning that the induction hypothesis still holds.
⌅ Note: Lemma 2 and its proof were stated for the optimal inspection path P ⇤ . However, the proof holds for any path P .
We can order path pairs in the open list (line 4) either according to (i) the path pair with the minimal achievable path cost or (ii) the path pair with the minimal potentially achievable path cost. We now show that if either method is used, the path returned by the algorithm (line 7) ", pdominates an optimal inspection path P ⇤ .
Theorem 1. Near-optimal inspection graph search. Nearoptimal inspection graph search (Alg. 1) computes a path P that ", p-dominates an optimal inspection path P ⇤ . Namely,
Proof. When Alg. 1 terminates (line 6-7), we have that
According to Lemma 2, for an optimal inspection path P ⇤ there always exists a path pair in the open set and an index i such that P dominates P ⇤
[i]. If the terminating PP u = (P u , Pu ) (on line 6-7) happens to be such a path pair, it is straightforward that
Otherwise, such a path pair PP 0 = (P 0 , P 0 ) is still in the open set. When using the achievable cost to order the open list, we have that
The assumption that PP n i +k ⇤ was the first path pair that was generated is not required for Lemma 2 to hold but it simplifies the proof. ¶ Strictly speaking, when n j = n i , the notation PPn i and PPn j is ambiguous. However, to simplify notation, we continue to refer to PPn i as the path pair for which the induction hypothesis holds and to PPn j as the newly created path pair.
When using potentially achievable costs to order the open list, we have
To prove that IRIS is asymptotically optimal, we will show that the optimal inspection path x ⇤ can be approximated by a sequence of configurations sampled by our algorithm.
Here, we will need to show that as the number of iterations approaches infinity, the following requirements hold: (i) the length of the path induced by this sequence of samples converges asymptotically to the length of x ⇤ , (ii) the coverage obtained by this sequence of samples converges asymptotically to the coverage of x ⇤ and that (iii) our graphinspection algorithm finds such a sequence of samples.
To do so, we rely on the notion of probabilistic exhaustivity (Schmerling et al. 2015). Roughly speaking, it is the notion that given a sufficiently large set of uniformly sampled configurations, any path can be traced arbitrarily well in the configuration space by a path defined as a sequence of configurations. The original definition of tracing (to be formalized shortly) was used in the context of path cost and, as we will see, is insufficient for our purposes. Thus, we start in Sec. A in extending the definition of tracing and showing that probabilistic exhaustivity still holds for the extended version. This lays the groundwork to show that (i) and (ii) hold.
We then continue in Sec. B formally define the wellbehaving of an inspection trajectory. Together with the notion of tracing, well-behaving lays the groundwork to show that (ii) holds. Roughly speaking, this will ensure that there are no singular points along a trajectory where a POI can only be inspected from. This, in turn, will allow us to ensure that trajectories that trace an optimal inspection path cover the same set of POI.
Finally, in Sec. C, we come to the conclusion that IRIS is asymptotically optimal.
In order for our proof to be applicable to general systems with differential constraints, we need to introduce several notations. These will be used to prove the notion of probabilistic exhaustivity (Schmerling et al. 2015) for our setting. We start by defining (following (Schmerling et al. 2015, Sec. IV)) the arc length of a path and sub-Riemmanian distances. Definition 2. Trajectory arc length. Let x : [0, T ] ! X be a continuous trajectory, the arc length of x is defined as
Here, k ẋ(t)k = p h ẋ(t), ẋ(t)i is the squared root of the standard Euclidean inner product. See Fig. 1. Definition 3. Sub-Riemmanian distance. Let q 1 , q 2 2 X be two configurations, the sub-Riemmanian distance between them is defined as d sr (q 1 , q 2 ) := inf
x `(x).
Namely, it is the length of the shortest dynamically feasible trajectory || x(t) connecting q 1 and q 2 . See Fig. 2. Given the above definitions, we define the Euclidean ball on X as B e (x, ") = {y 2 X : kx yk "}, and the sub-Riemannian ball on X as
Note that by definition it holds that 8x, y kx yk d sr (x, y) and 8x, " B(x, ") ✓ B e (x, "). See Fig. 3. The result of our algorithm, as is common in samplingbased methods, is a trajectory that is implicitly defined by a discrete sequence of configurations (a.k.a. waypoints). The following definition formalizes the continuous path associated with a discrete sequence of waypoints.
Definition 4. Associated optimal trajectory. Let P = {q i } n i=1 ✓ X be a discrete sequence of configurations. The k Roughly speaking, a trajectory is said to be dynamically feasible if there is a control function that satisfies the kinematic constraints of the system. For a precise definition, see (Schmerling et al. 2015, Sec. II). associated optimal trajectory of P , denoted by ⇡ P : [0, S] ! X , is (i) dynamically feasible and (ii) sequentially connects the waypoints q 1 , . . . , q n such that each connection is locally optimal. Namely, it provides the shortest connection length `(⇡ P (q i , q i+1 )) = d sr (q i , q i+1 ). See Fig. 4.
A key notion used in proving probabilistic exhaustivity is tracing (to be formally defined shortly). Roughly speaking, a given path x is said to be traced by a sequence of points P if they are close to x and the length of ⇡ P , the path associated with P , is close to the length of the given path. This corresponds to ensuring a one-way Hausdorff distance between ⇡ P and x ((iii) in Definition 5). In the original definition of ( , r)-tracing, (see (Schmerling et al. 2015, Section IV)), requiring only Hausdorff was sufficient as the proof was only concerned with path length (and not coverage). This is because if ⇡ P "shortcuts" the original path, the path length is only reduced. However, in our setting, this may result in points covered by x not being covered by ⇡ P . Thus, we add an additional requirement that all points along the path x are close to a point in P which, roughly speaking, corresponds to requiring the twoway Hausdorff distance ((iii) and (iv) in Definition 5).
Definition 5. ( , r)-tracing. Let x : [0, T ] ! X be a dynamically feasible trajectory, P = {q i } n i=1 ✓ X be a discrete sequence of configurations and ⇡ P be the associated optimal trajectory of P . P is said to ( , r) trace x if:
(i) d sr (q i , q i+1 ) i=1,...,n 1 r, (iv) any point along the path x is at most rdistance away from some point in P . Namely, To use the extended (and slightly more restrictive) definition of tracing, we introduce the notion of an (r, k)bounded trajectory. This notion formally defines to what extent a path can change (by limiting the curvature of a path) in a local neighborhood. This, in turn, will be used to inform us how many samples are required to trace a given path in order to prove probabilistic exhaustivity under our new definition of tracing.
Here, x(t 1 , t 2 ) is the curve segment on x between x(t 1 ) and x(t 2 ). See Fig. 6.
We now prove that when the target trajectory is not pathological (k ẋ(t)k < 1), then for any constant k > 1, we can always find a value r such that the trajectory is (r, k)-bounded. Intuitively, this will allow us to argue that Prepared using sagej.cls when we sample along a target trajectory at small-enough intervals (defined by r), the length of the curve connecting two nearby sampled points is not too long compared to the sub-Riemmanian distance between them. Lemma 3. Let x : [0, T ] ! X be a dynamically feasible trajectory and ẋ(t) is bounded, namely, 8t 2
For every constant k > 1, there exists some radius r = r curv (k, x) > 0 such that x is (r, k)bounded. See Fig. 7.
Proof. According to Definition 2, we have that
which can be written in another form as
According to the definition of limit, for some > 0, there always exists ⇠ > 0 such that when t 0 < ⇠, `(x(t,t+ t)) dsr(x(t),x(t+ t)) k 0 (t) < . Then we could take = k 1, then with corresponding ⇠, take r = min{d sr (x(t), x(t + ⇠)), t 2 [0, T ⇠]} would guarantee that x is (r, k)bounded.
⌅
We now use Lemma 3 to prove that for a target trajectory
x, given a large-enough number of samples, there exist a sequence of configurations that traces x, under the new definition of ( , r)-tracing (Definition 5). To do so, we use several results that were used in the original proof of probabilistic exhaustivity. Lemma 4. Probabilistic exhaustivity. Let x : [0, T ] ! X free be a dynamically feasible trajectory. Let Q N be a set of N points sampled independently and identically from the uniform distribution on the collision-free space X free and set
Here, D is a constant capturing the dimension of the system and is a commutable constant depending on the system dynamics, N , D, and some tuning parameter ⌘ 0. ** Let ÃN be the event that there exists a discrete sequence of configurations P = {q i } n i=1 ✓ V N that ( , r)-traces x for any 2 (0, 1) and r = r N . The probability that event ÃN doesn't happen, denoted by P( Ãc N ) is asymptotically bounded by
As is defined above, ⌘ 0 is some tuning parameter.
Proof. Define T N = {⌧ N,1 , ⌧ N,M N } as a sequence of points such that (i) ⌧ N,1 = x(0), (ii) x(⌧ N,i ) and
x(⌧ N,i+1 ) are at sub-Riemannian distances r N /2, where ⌧ N,i+1 > ⌧ N,i is the smallest timestamp that satisfies d sr (x(⌧ N,i ), x(⌧ N,i+1 )) = r N /2 (see Fig. 6) and (iii) ⌧ N,M N = x(T ). Furthermore, define B N = {B N,1 , B N,M N } a sequence of sub-Riemannian balls such that B N,i is centered at x(⌧ N,i ) and has radius r N /4. Define A N to be the event that each ball in B N contains at least one point in V N (see Fig. 8).
In (Schmerling et al. 2015, Thm. IV.5) it was shown that with probability at least
⌘ it holds that there exists a discrete sequence of configurations P = {q i } n i=1 ✓ V N such that A N exists and that requirements (i) (iii) in Definition 5 hold for and r = r N . Thus, we only need to show that requirement (iv) in Definition 5 holds for and r = r N . Namely, that sup t2[0,T ] inf i2{1,...n} d sr (q i , x(t)) r.
Thus, we now show that 8t 2 [0, T ], 9i s.t. (q i , x(t)) r which will conclude the proof. We consider two cases:
In this section, we are finally ready to prove the asymptotic optimality of IRIS. To simplify the proof, we do so for a variant of IRIS where at each iteration we construct a PRM and not an RRG. While not identical, both roadmaps exhibit similar properties which are typically easier to show for PRMs (see, Karaman and Frazzoli (2011); Solovey et al. (2018)).
We start (Thm. 2) by showing that w.h.p. given a weakly ⇠well behaved trajectory with strong cl -clearance and optimal inspection coverage, as the number of samples tends to infinity, IRIS returns an inspection trajectory whose length and coverage are bounded away from this trajectory. We then continue to introduce the notion of a regular inspection problem which, roughly speaking, states that the boundary of inspecting configuration regions has some structure. This will allow us (Thm. 3) to show that given a regular inspection planning problem, and an optimal inspection trajectory (that may be weakly ⇠-well behaved, with weak cl -clearance) then w.h.p. IRIS returns an inspection trajectory whose length and coverage are bounded away from this trajectory.
Theorem 2. IRIS comparison. Let P = (X , I, S, `, q s ) be an inspection-planning problem and X free be the set of collision-free configurations. Assume that the robot system satisfies the assumptions mentioned in Schmerling et al. (2015) and let x : [0, T ] ! X free be a feasible inspection trajectory such that (i) x(0) = q s , (ii) x has strong cl -clearance for some cl > 0, (iii) x is weakly ⇠-well behaved for some ⇠ > 0, (iv) x has optimal coverage, namely S(x) = I ⇤ , and (v) 9t 2 [0, T ] such that B e (x(t), ) ✓ X insp (S(x(T c ))) for > 0 where T c is the inspection completing point of x.
Furthermore, let `N and S N denote the arc length and inspection coverage, respectively, of the trajectory returned by IRIS at the N 'th iteration using approximation parameters " N and p N , and a lower-bound connecting radius r N (as defined in Lemma 4). Then for any fixed 2 (0, 1),
Proof. Let G N = (V N , E N ) denote the roadmap constructed by IRIS at the N 'th iteration and notice that N = |V|. Assume N to be sufficiently large so that r N min{⇠, , cl /2} and
v2V N S(v) = I ⇤ . Following Lemma 4, we have that with a probability of at least 1 O
, there exists a discrete sequence of configurations P = {q m } n m=1 ✓ V that ( , r N )traces x. We now show that in the event that such P exists, then IRIS will return a trajectory such that `N
Assume that such a sequence P exists. We can bound the minimal distance of any point in P to an obstacle in the configuration space as follows: Thus, B e (q m , r N ) ✓ X free . As P = {q m } n m=1 ( , r N )traces x, for any point q along the locally optimal edge between q m and q m+1 , we have kq m qk d sr (q m , q) d sr (q m , q m+1 ) r N .
Thus q 2 B e (q m , r N ) ✓ X free . Namely, all the vertices of P as well as locally optimal paths connecting subsequent vertices in P are collision-free. This, together with the connection radius used ensures that all edges connecting vertices along P are collision-free and are added to E N .
By construction, we have that q 1 = q s = x(0). In addition, following condition (v) and Definition 5 we have that 9m 2 [n] such that S(q m ) = S(x(T c )). Together with the fact that x is weakly ⇠-well behaved, P ( , r N )-traces x, and r N ⇠ then by Lemma 5 it holds that S(x) ✓ S(P ). As x has optimal coverage (condition (iv)), we have that
Let `⇤ N and S ⇤ N denote the arc length and inspection coverage of optimal inspection trajectory on roadmap G N , respectively. We have that `N (1 + " N )`⇤ N (1 + " N )`(⇡ P ) (1 + " N )(1 + )`(x).
Here, the first inequality follows from Thm. 1, the second inequality follows from the fact that `⇤ N is the infimum taken over the arc length of all trajectories that has optimal coverage on the graph, and the last inequality follows from Definition 5.
Similarly, we have that
Here, the first inequality follows from Thm. 1 and the first equality follows from the fact that S ⇤ N = I ⇤ . ⌅ Before stating our final result, we introduce the notion of a regular boundary and regular inspection planning problem. This is required because an optimal inspection trajectory will never be strongly well-behaved but only weakly wellbehaved and special care needs to be taken in order for IRIS to cover the POI covered by x ⇤ (T ). The notion of regularity will ensure that there always exists a region near x ⇤ (T ) that IRIS can sample inside. See also Fig. 12. Definition 10. Regular boundary. A set X 0 ✓ X free is said to have a regular boundary if there exists > 0 such that 8q 2 @X , there exists q 0 2 X with B e (q 0 , ) ✓ X 0 and q 2 @B e (q 0 , ). Definition 11. Regularity of an inspection-planning problem. Let P = (X , I, S, `, q s ) be an inspection-planning problem and X free be the set of collision-free configurations. P is said to be regular if 8q 2 X free , X insp (S(q)) has a regular boundary. (iii) 8k 2 [0, 1), x k is dynamically feasible and has strong k -clearance for some k > 0, and lim k!1 k = 0. (iv) lim k!1 `(x k ) = `(x). We are now ready to state our final result.
Theorem 3. IRIS asymptotic optimality.
Let P = (X , I, S, `, q s ) be a regular inspection-planning problem and X free be the collision-free space. Assume that the robot system satisfies the assumptions mentioned in Schmerling et al. (2015) and let x ⇤ : [0, T ] ! X free be an optimal feasible inspection trajectory such that (i) x ⇤ (0) = q s , (ii) x ⇤ has weak cl -clearance for some cl > 0, (iii) x ⇤ is weakly ⇠-well behaved for some ⇠ > 0.
Furthermore, let `N and S N denote the arc length, and the inspection coverage, respectively of the trajectory returned by IRIS at the N 'th iteration using approximation parameters " N and p N , and a lower-bound connecting radius r N (as defined in Lemma 4). If lim N !1 " N = 0 and Proof. Assume w.l.o.g. that `(x ⇤ ) > 0. Following (Karaman and Frazzoli 2011, Lemma 50) and the fact that x ⇤ has weak cl -clearance, there exists a sequence {x k } k2N of paths such that lim k!1 x k = x ⇤ and x k has strong k -clearance where { k } k2N is a sequence of real numbers such that lim k!1 k = 0. Choose k 0 2 N such that 8k k 0 we have that
and that
with ⇠ k 2 (0, ⇠). Furthermore, we assume that k 0 is large enough such that 8k k 0 , the inspection completing point of x k equals its completion time (namely, T c (x k ) = T k where T k is the completion time of x k ). Notice that as x ⇤ and x k are homotopic, we have that
x k (0) = x ⇤ (0) and x k (T k ) = x ⇤ (T ). Thus, using Lemma 6 we have that S(x k ) = I ⇤ and that x k is weakly ⇠ k -well behaved.
To use Thm. 2, we show how to construct a new trajectory
x 0 k that extends x k into the interior of X insp (S(x k (T c ))). This is similar to the construction used in (Schmerling et al. 2015, Thm. VI.2). As x k (T k ) 2 @X insp (x k (T k )) and since the inspection planning problem P is regular, then there exists some configuration z such that z 2 X insp (x k (T k )), B e (z, ) ✓ X insp (x k (T k )) and x ⇤ (T ) 2 @B e (z, ) for some > 0 (see also Fig. 13). Set z 0 to be the configuration on the straight-line segment connecting x k (T k ) and z such that d sr (x k (T k ), z 0
) ( /4)`(x ⇤
). Finally, set x 0 k to be the extension of x k constructed by concatenating x k with the shortest sub-Riemannian path between x k (T k ) and z 0 . We now bound the arc length of x 0 k by
).
(5)
Clearly, for 0 = kz 0 x k (T )k it holds that B e (z 0 , 0 ) ✓ B e (z, ) ✓ X insp (x k (T k )). In addition, for any point p 2 x 0 k along the path between x k (T k ) and z 0 we have that )) P(`N > (1 + " N )(1 + ) • `(x ⇤
)).
Here, the first equality follows from Thm. 2, the first inequality follows from equation ( 5), and the last inequality follows from the fact that 2 (0, 1). Similarly, Thm. 2 implies that
Using the fact that lim N !1 " N = 0 and that lim N !1 p N = 1, we have that
= lim N !1 P(`N > (1 + 0)(1 + )`(x ⇤ ))
= lim N !1 P(`N > (1 + )`(x ⇤ )).
Similarly,
Prepared using sagej.cls [Version: 2017/01/17 v1.20]
Prepared using sagej.cls