Path Planning for Improved Target Visibility Maintaining Line of Sight in a Cluttered Environment by Matthew Alexander Baumann B.Sc., The University of British Columbia, 2006 A THESIS SUBMITTED IN PARTIAL FULFILMENT OF THE REQUIREMENTS FOR THE DEGREE OF Master of Science in The Faculty of Graduate Studies (Computer Science) The University Of British Columbia (Vancouver) February, 2009 c© Matthew Alexander Baumann 2009 Abstract The visibility-aware path planner addresses the problem of path planning for target visibility. It computes sequences of motions that afford a line of sight to a stationary visual target for sensors on a robotic platform. The visibility-aware planner uses a model of the visible region, namely, the region of the task space in which a line of sight exists to the target. The planner also takes the orientation of the sensor into account, utilizing a model of the field of view frustum. The planner applies a penalty to paths that cause the sensor to lose target visibility by exiting the visible region or rotating so the target is not in the field of view. The planner applies these penalties to the edges in a probabilistic roadmap, providing weights in the roadmap graph for graph-search based planning algorithms. This thesis presents two variants on the planner. The static multi-query planner precomputes penalties for all roadmap edges and performs a best-path search using Dijkstra’s algorithm. The dynamic single-query planner uses an iterative test-and-reject search to find paths of acceptable penalty without the benefit of precomputation. Four experiments are presented which validate the planners and present examples of the path planning for visibility on 6-DOF robot manipulators. The algorithms are statistically tested with multiple queries. Results show that the planner finds paths with significantly lower losses of target visibility than existing shortest-path planners. ii Table of Contents Abstract . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . ii Table of Contents . . . . . . . . . . . . . . . . . . . . . . . . . . . . iii List of Tables . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . vi List of Figures . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . vii List of Algorithms . . . . . . . . . . . . . . . . . . . . . . . . . . . viii Nomenclature . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . viii Glossary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . xi Acknowledgements . . . . . . . . . . . . . . . . . . . . . . . . . . . xix Dedication . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . xx 1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1 1.1 Motivation . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3 1.1.1 Articulated Robots . . . . . . . . . . . . . . . . . . . 3 1.1.2 Mobile Robots . . . . . . . . . . . . . . . . . . . . . . 5 1.2 Research Context . . . . . . . . . . . . . . . . . . . . . . . . 5 1.3 Problem Statement . . . . . . . . . . . . . . . . . . . . . . . 6 1.4 Research Objectives . . . . . . . . . . . . . . . . . . . . . . . 7 1.5 Thesis Outline . . . . . . . . . . . . . . . . . . . . . . . . . . 8 2 Literature Review . . . . . . . . . . . . . . . . . . . . . . . . . 10 2.1 Preamble . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10 2.2 Path Planning . . . . . . . . . . . . . . . . . . . . . . . . . . 11 2.2.1 Multi-Query Approaches . . . . . . . . . . . . . . . . 11 2.2.2 Single-Query Approaches . . . . . . . . . . . . . . . . 12 2.2.3 Hybrid Path Planners . . . . . . . . . . . . . . . . . . 13 iii Table of Contents 2.3 Sensor Planning . . . . . . . . . . . . . . . . . . . . . . . . . 13 2.4 Path Planning for Visibility . . . . . . . . . . . . . . . . . . . 14 2.5 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 16 3 Methods . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17 3.1 Detailed Problem Statement . . . . . . . . . . . . . . . . . . 17 3.2 Task Space . . . . . . . . . . . . . . . . . . . . . . . . . . . . 18 3.3 Configuration Space . . . . . . . . . . . . . . . . . . . . . . . 18 3.4 Sensor Limitations . . . . . . . . . . . . . . . . . . . . . . . . 19 3.4.1 Field-of-View . . . . . . . . . . . . . . . . . . . . . . . 20 3.4.2 Occlusion . . . . . . . . . . . . . . . . . . . . . . . . . 20 3.5 Occlusion Boundary Algorithm . . . . . . . . . . . . . . . . . 22 3.6 Probabilistic Roadmap . . . . . . . . . . . . . . . . . . . . . 27 3.7 Adaptive Subdivision . . . . . . . . . . . . . . . . . . . . . . 29 3.8 Field of View Checking . . . . . . . . . . . . . . . . . . . . . 30 3.9 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 31 4 Occlusion-Aware Path Planning . . . . . . . . . . . . . . . . 32 4.1 Overview . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 32 4.2 Occluded Space . . . . . . . . . . . . . . . . . . . . . . . . . 33 4.3 Trajectory Segmentation . . . . . . . . . . . . . . . . . . . . 34 4.4 Trajectory Segment Classification . . . . . . . . . . . . . . . 38 4.5 Occlusion Penalties . . . . . . . . . . . . . . . . . . . . . . . 40 4.6 Dijkstra’s Algorithm . . . . . . . . . . . . . . . . . . . . . . . 42 4.7 Integrating Field of View Awareness . . . . . . . . . . . . . . 42 4.8 Effects of Roadmap Connectivity . . . . . . . . . . . . . . . . 44 4.9 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 45 5 Dynamic Occlusion-Aware Path Planning . . . . . . . . . . 46 5.1 Overview . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 46 5.2 Visibility Computations . . . . . . . . . . . . . . . . . . . . . 47 5.3 Iterative Path Search . . . . . . . . . . . . . . . . . . . . . . 48 5.4 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 50 6 Experiments . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 52 6.1 Experiment 1 – An automobile assembly scenario simulation 52 6.1.1 Purpose . . . . . . . . . . . . . . . . . . . . . . . . . . 52 6.1.2 Hypothesis . . . . . . . . . . . . . . . . . . . . . . . . 53 6.1.3 Method . . . . . . . . . . . . . . . . . . . . . . . . . . 53 6.1.4 Results . . . . . . . . . . . . . . . . . . . . . . . . . . 55 iv Table of Contents 6.1.5 Discussion . . . . . . . . . . . . . . . . . . . . . . . . 56 6.2 Experiment 2 – Observing a target through gaps in a wall using a physical robot setup . . . . . . . . . . . . . . . . . . 58 6.2.1 Purpose . . . . . . . . . . . . . . . . . . . . . . . . . . 58 6.2.2 Hypothesis . . . . . . . . . . . . . . . . . . . . . . . . 58 6.2.3 Method . . . . . . . . . . . . . . . . . . . . . . . . . . 58 6.2.4 Results . . . . . . . . . . . . . . . . . . . . . . . . . . 61 6.2.5 Discussion . . . . . . . . . . . . . . . . . . . . . . . . 61 6.3 Experiment 3 – Observing a target through a narrow channel 62 6.3.1 Purpose . . . . . . . . . . . . . . . . . . . . . . . . . . 63 6.3.2 Hypothesis . . . . . . . . . . . . . . . . . . . . . . . . 63 6.3.3 Method . . . . . . . . . . . . . . . . . . . . . . . . . . 63 6.3.4 Results . . . . . . . . . . . . . . . . . . . . . . . . . . 65 6.3.5 Discussion . . . . . . . . . . . . . . . . . . . . . . . . 66 6.4 Experiment 4 – Dynamic planning in a changing environment 67 6.4.1 Purpose . . . . . . . . . . . . . . . . . . . . . . . . . . 67 6.4.2 Hypothesis . . . . . . . . . . . . . . . . . . . . . . . . 68 6.4.3 Method . . . . . . . . . . . . . . . . . . . . . . . . . . 68 6.4.4 Results . . . . . . . . . . . . . . . . . . . . . . . . . . 70 6.4.5 Discussion . . . . . . . . . . . . . . . . . . . . . . . . 73 6.5 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 74 7 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 75 7.1 Contributions . . . . . . . . . . . . . . . . . . . . . . . . . . 75 7.2 Future Work . . . . . . . . . . . . . . . . . . . . . . . . . . . 76 7.3 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . 77 Bibliography . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 78 Index . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 81 v List of Tables 6.1 Experiment 1 Parameters . . . . . . . . . . . . . . . . . . . . 53 6.2 Experiment 1 Visibility Penalty t-test . . . . . . . . . . . . . 56 6.3 Experiment 2 Parameters . . . . . . . . . . . . . . . . . . . . 60 6.4 Experiment 2 Visibility Penalty t-test . . . . . . . . . . . . . 61 6.5 Experiment 3 Parameters . . . . . . . . . . . . . . . . . . . . 65 6.6 Experiment 3 Visibility Penalty t-test . . . . . . . . . . . . . 66 6.7 Experiment 4 Trial 1 Visibility Penalty t-test . . . . . . . . . 71 6.8 Experiment 4 Trial 2 Visibility Penalty t-test . . . . . . . . . 72 6.9 Experiment 4 Trial 3 Visibility Penalty t-test . . . . . . . . . 73 vi List of Figures 1.1 A robot with an arm-mounted sensor whose target is occluded. 2 1.2 Visibility-based path planning example. . . . . . . . . . . . . 3 3.1 A sensor’s field of view. . . . . . . . . . . . . . . . . . . . . . 20 3.2 A sensor’s unoccluded space. . . . . . . . . . . . . . . . . . . 21 3.3 The environment front faces. . . . . . . . . . . . . . . . . . . 25 3.4 A separating face . . . . . . . . . . . . . . . . . . . . . . . . . 26 3.5 The occlusion boundaries of each set of front faces. . . . . . . 27 3.6 The set of occlusion boundaries . . . . . . . . . . . . . . . . . 28 4.1 A visualisation of the occluded space. . . . . . . . . . . . . . 34 4.2 Trajectory Segmentation . . . . . . . . . . . . . . . . . . . . . 35 4.3 A visualisation of the trajectory segmentation. . . . . . . . . 37 4.4 An example of trajectory segment classification . . . . . . . . 39 4.5 A visualisation of the trajectory segment classification. . . . . 40 6.1 Experiment 1: Setup. . . . . . . . . . . . . . . . . . . . . . . 54 6.2 Experiment 1: An example query. . . . . . . . . . . . . . . . . 56 6.3 Experiment 1: The shortest path image sequence. . . . . . . . 57 6.4 Experiment 1: The occlusion-avoiding path image sequence. . 57 6.5 Experiment 2: Setup . . . . . . . . . . . . . . . . . . . . . . . 59 6.6 Experiment 2: Setup photograph . . . . . . . . . . . . . . . . 60 6.7 Experiment 2: The occlusion-avoiding path image sequence. . 62 6.8 Experiment 3: Setup . . . . . . . . . . . . . . . . . . . . . . . 63 6.9 Experiment 3: Occluder . . . . . . . . . . . . . . . . . . . . . 64 6.10 Experiment 3: Image sequence . . . . . . . . . . . . . . . . . 67 6.11 Experiment 4: Setup . . . . . . . . . . . . . . . . . . . . . . . 69 6.12 Experiment 4: Path comparison . . . . . . . . . . . . . . . . . 74 vii List of Algorithms 1 The Occlusion Boundary Algorithm . . . . . . . . . . . . . . 23 2 The Front Faces Algorithm . . . . . . . . . . . . . . . . . . . 24 3 The Label Connected Algorithm . . . . . . . . . . . . . . . . 24 4 The Planner Algorithm . . . . . . . . . . . . . . . . . . . . . 33 5 The Trajectory Segmentation Algorithm . . . . . . . . . . . . 36 6 The Trajectory Segmentation Algorithm - Field of View Variant 43 7 The Dynamic Planner Algorithm . . . . . . . . . . . . . . . . 49 viii Nomenclature  The sampling parameter for intersection detection λ The penalty mixing parameter µ The merge threshold for collapsing multiples of the same intersection point ψ(ξ) The length of the longest contiguous loss of visibility in path ξ ψmax The contiguous visibility penalty threshold τ The interpolation parameter of a motion qj,k θ1−d The rotational displacement of arm joints 1− d ξ A path through the PRM ξ′ A candidate path used in iterative path search PRM Probabilistic Roadmap C The camera (sensor) reference frame CC Connected Component CCD Crystalline Capture Device d The dimensionality of the configuration space Q dC(pt,qj,k(τ)) The distance from point pt to the frustum boundary in con- figuration qj,k(τ) dist(p, Lv) The distance from p to the nearest part of Lv DT The set of Dijkstra trees whose roots are the nodes of a graph G dt(n) The Dijkstra Tree whose root is node n E The set of edges in a PRM graph G = (N,E) ix List of Algorithms e An edge in a PRM graph G = (N,E) ej,k The PRM edge that represents qj,k F The set of faces in a triangle mesh f A (triangular) face within a mesh G A (PRM) graph GASP The General Automatic Sensor Planning System GTS The GNU Triangulated Surface Library H The set of all exterior edges of a group of front faces h An exterior edge of a front face I The set of intersection points that mark changes in visibility along a motion k The (attempted) roadmap connectivity `(qi,i+1) The maximum displacement due to qi,i+1 Lv The occlusion boundary Lv1 The set of front faces (subset of the occlusion boundary Lv) Lv2 The set of separating faces (subset of the occlusion boundary Lv) M The mesh representing the environment obstacles MVP The Machine Vision Planning System N The set of nodes in a PRM graph G = (N,E) n A node in a PRM graph G = (N,E) p A point in the task space P The task space Pfree Free space (the region of the task space unoccupied by obstacles) pm The midpoint of a motion segment Pocc The occluded region of the task space x List of Algorithms ps The sensor position psafe The safe point (which is known to have a line of sight to the target) pt A target point Pvis The visible region of the task space Q The configuration space q A point in the configuration space qdest The goal configuration in a path planning query Qfree The set of collision-free configurations in the configuration space Q qi The robot configuration at the start of a motion segment qi,i+1 qi,i+1 The ith segment of a motion qj,k The interpolated motion between configurations qj and qk qj,k(τ) The linearly interpolated configuration between qj and qk with pa- rameter τ qi+1 The robot configuration at the end of a motion segment qi,i+1 qstart The start configuration in a path planning query RRT Rapidly-Exploring Random Tree S The set of segments of a motion qj,k T The set of target points TS The recursive Trajectory Segmentation subroutine V The set of classification values representing the visibility states of the segments of a motion v(p) The visibility state of a sensor at point p W The set of all weights on edges in a graph G wD The set of weights used by iterative path search wdist(e) The length penalty edge weight wocc(e) The occlusion penalty edge weight xi Glossary adaptive bisection The adaptive-bisection method described by Schwarzer et al. [1] eliminates unneces- sary samples during collision/occlusion detec- tion by comparing the distance to the near- est obstacle or boundary to the maximum robot/sensor displacement for a given seg- ment of a trajectory., 36 candidate path A path proposed by the candidate planner during iterative path search. The candidate will be accepted or rejected based on its visi- bility penalties., 50 candidate planner A path planner that quickly and efficiently finds the path of lowest weight within a PRM. It is used to propose candidate paths during iterative path search., 50 configuration A d-tuple of real parameters representing the arrangement and positioning of the robot’s body. Denoted q, a configuration resides in the d-dimensional configuration space Q. Nodes in the roadmap represent individual configurations., 19 configuration space A d-dimensional space whose axes represent the range of robot configuration parameters, such as joint angles or positions., 19 connected component A region of a triangle mesh whose triangles share edges. That is to say, a triangle is part of a connected component if it shares an edge with any other triangle in the component., 24 xii Glossary connectivity The number and nature of edges connected to nodes in a graph. In the case of the prob- abilistic roadmap, the connectivity refers to how many attempted connections there are to neighbour nodes, and the metric which is used to determine which nodes are neighbouring., 44 dynamic environment An environment that changes between queries to the path planner., 46 dynamic path planner A path planner that can compute paths in an environment that is subject to periodic change. (See: dynamic environment), 46 environment The objects and surfaces within the robot’s working space. Includes physical obstacles with which the robot must not collide, and visual obstacles through which the sensor can- not penetrate. Each set of obstacles must be represented by a triangle mesh. The set of visual obstacles is denoted M ., 32 eye-in-hand A robot with an “eye-in-hand” configuration has a visual sensor mounted on the end- effector (the distal link)., 1 field of view The region of space within which the sensor can gather data. Typically represented by a frustum (truncated pyramid), 42 free space The region, denoted Pfree, of the task space P that is not occupied by obstacles. The robot will collide with an obstacle if any part of the robot’s volume exits the free space., 18 front face A face in a triangle mesh that contains a ref- erence point (such as the target position) is its positive half-space., 22 xiii Glossary iterative path search A path planning strategy that generates and evaluates candidate paths until an acceptable path is found. It is used in the Dynamic Vis- ibility Aware path planner., 50 line of sight A line segment in the task space connecting a target point to a sensor. If the line of sight does not intersect with an occluder, then the target is visible to the sensor (in terms of oc- clusion)., 19 motion A parameterised interpolation between two configurations of the robot. The motion is not necessarily parameterised with respect to time, and does not explicitly incorporate the robot dynamics. The path planner strings motions together into paths. The motion is represented by a roadmap edge ej,k, connect- ing the two configurations qj and qk., 29 multi-query path planner A multi-query path planner uses a set of pre- computed data to answer multiple different path planning queries., 11 occluded region Denoted Pocc, the occluded region is the re- gion of the task space P from within which a sensor does not have an unbroken line of sight to a particular target. The occluded region is bounded by the occlusion boundary Lv., 18 occlusion boundary The boundary Lv between the occluded and visible regions of the task space. It is repre- sented by a triangle mesh., 21 xiv Glossary occlusion penalty A value attached to a robot motion represent- ing the “badness” of that motion with respect to losses of the line of sight to a target. While not strictly occlusions, losses of line of sight due to the sensor field of view may also be factored into the occlusion penalty, as the vi- olation of either the occlusion or field of view constraints results in a loss of data on the tar- get., 32 path A series of robot motions, connected at way- points, that connect a start configuration to a goal configuration., 28 path of best visibility The path involving a minimal distance trav- elled where the sensor does not have line of sight to the target, within the constraints of the environment and/or the roadmap repre- senting known safe motions., 32 precomputation Computations performed prior to any path planning queries. These are assumed to be “offline”, and thus their computational cost is less critical than if they needed to be com- puted at the time of the query. In the case of occlusion penalties, precomputation greatly reduces the cost of making queries in the case of the static planner. However, if the environ- ment changes, the precomputed information may be invalidated., 32 probabilistic roadmap A graph G = (N,E) whose nodes repre- sent configurations and whose edges represent motions between those configurations, collec- tively representing all known safe paths in the robot’s configuration space., 28 xv Glossary query A path planning query is a computation which finds a path between two configurations qstart,qdest in the configuration space. It is performed by a path planner such as a prob- abilistic roadmap. The result is a list of way- points (additional configurations qi) between which the robot can interpolate without col- lisions, collectively referred to as a “path”., 11 sampling parameter The sampling parameter  is the radius of a sphere covered by a sample in the trajectory segmentation algorithm. If any part of the boundary Lv lies within that sphere, then that sample is recorded as an intersection between the trajectory that the sample lies upon and the boundary. Decreasing the sampling pa- rameter increases the accuracy of the intersec- tion search, but at increased computational cost. In the current implementation, the in- tersection search recursion terminates when the samples are less than 2e apart, avoiding redundant (overlapping) samples., 35 segment classification The process of determining which segments of a trajectory afford target visibility. This re- quires a test for both occlusion (does the seg- ment midpoint configuration place the sensor in Pvis) and for the field of view (does the seg- ment midpoint configuration place the target in the sensor field of view). If both tests are satisfied then the segment can be labelled as “target visible”, and is exempt from penalty., 38 sensor trajectory A parameterised motion of the sensor, in the task space. This encompasses both the sensor position and orientation. It can be computed from the robot’s motion using a forward kine- matics routine that can determine the sensor’s pose given the robots configuration., 35 xvi Glossary separating face A planar trapezoid representing one part of the occlusion boundary. The trapezoid em- bodies the section of the boundary created by a single exterior edge of the occluder. To- gether, all the separating faces form Lv2., 24 single-query path planner A single-query path planner is designed to ef- ficiently find a single path through the con- figuration space without the benefit of pre- processing. Single-query planners may be re- quired in environments that change between queries., 12 trajectory segmentation The process of determining at what points on a trajectory the target visibility changes. This is due to the sensor entering or exiting the occluded region, or the target entering or ex- iting the sensor field of view. Each point of change marks the boundary of a trajectory segment, within which there are not changes in the target visibility status. The visibility state of each segment is later determined by the classification step., 36 valence test A geometric test that determines the number of times a line segment intersects a surface. This is used to determine if two points lie in the same partition of a space. If the boundary separating the partitions is closed and mani- fold, then an even valence on a line segment connecting the two points indicates that the points occupy the same partition, and an odd valence indicates that they do not., 40 visibility-aware path planner A path planner that takes into account knowl- edge of the lines of sight in an environment when planning paths of a sensor-equipped robot. The planner may attempt to maintain or avoid the lines of sight depending on the application., 32 xvii Glossary visible region Denoted Pvis, the visible region is the region of the task space from within which a sensor has an unobstructed line of sight to a partic- ular target. The visible region is bounded by the occlusion boundary Lv., 18 vision-guided bin-picking A vision-guided bin-picking task requires a robot manipulator to use vision to detect, identify, grasp and manipulate known objects that are randomly arranged in a bin., 4 xviii Acknowledgements I would like to thank my supervisors, Dr. Elizabeth A. Croft and Dr. James J. Little, whose guidance and support made this effort come to fruition. I would also like to thank Dr. Wolfgang Heidrich and Katrina Jessen for their edits, guidance and suggestions. To Simon Léonard, I couldn’t have made this work without the tools you wrote and the advice and assistance you provided. To my colleagues in the CARIS lab, I couldn’t ask for a better atmo- sphere in which to work, learn, study and laugh. To all my fellow students, grad and undergrad: thanks for the long study sessions, the late nights in the lab and the crazy antics that kept us all (somewhat) sane. To the faculty and staff of the Computer Science and Mechanical En- gineering Departments, your assistance was greatly appreciated. I would also like to acknowledge our industry partner, Braintech Inc. and the finan- cial support of the Natural Sciences and Engineering Research Council of Canada. xix Dedication To my parents, Elaine and Paul, and my sister Niki; “support” does not begin to describe the help you have provided me over the years. You made all this possible, and my greatest hope is to make you proud. To Katrina, you were with me through all the stress and the setbacks of my first year, and all the hard work of the second. Being in love with you keeps it all in perspective. xx Chapter 1 Introduction Robotic systems rely on accurate sensory data in order to react intelligently to changing environments. Sensors are subject to a wide variety of limita- tions on their operating conditions. If the robot is unaware of these limi- tations, it risks losing reliable sensor input. Imaging sensors in particular provide very rich sensory data in a compact package. However, such sensors are limited by the extent of their line of sight. Cameras which operate in the visible spectrum, for example, are unable to image beyond opaque ob- jects, and the line of sight is said to be “occluded” when it is blocked by an opaque obstacle. This fact is intuitive given the direct parallels between camera-equipped robots and the human vision system, but still poses very significant challenges for robots when faced with a cluttered environment. Consider a task in which a mobile robot is required to observe a station- ary target using a sensor that is limited by line of sight. If the environment contains opaque obstacles, a sensor such as a camera cannot image beyond those obstacles. The robot may be required to move to various goal posi- tions (either the target or another location entirely), all the while keeping the target in view. Some examples would include a sentry robot perform- ing surveillance, or an unmanned aircraft flying in partly cloudy skies that obscure its view of events below. Another example would be a stationary robot manipulator performing an assembly task in a factory. A robot with an “eye-in-hand” configuration has a visual sensor mounted on the end- effector (the distal link). Since the sensor’s pose depends on the robot’s configuration, the robot could lose sight of the objects it is working on if it moves the sensor behind any of the fixtures in the workcell. An example of such an occlusion of the target is shown in Figure 1.1. To human beings, the problem of maintaining line of sight is so com- monplace that it rarely requires their full concentration. Peering around a corner or standing on tiptoes to see over a crowd are common motions. These motions place their eyes in a position where their line of sight is unoc- cluded to facilitate sensing. The choice of sensor position is dictated by the placement of the visual obstacles and the limitations of possible movements. The robotic system must be able to emulate this ability to predict what re- 1 Chapter 1. Introduction target visible line of sight blocked target obstacle (bin) sensor A B Figure 1.1: A sensor mounted on a robot’s end-effector experiences occlusion of the target in position B. gions of space afford line of sight to the target. It must also plan a motion to bring the sensor into this “visible region” and plan any subsequent motions to prevent exiting this region. In addition to the line of sight, the sensor must be facing the target with a small enough angle of incidence that the target falls in the sensor’s field of view. In other words, the image of the target must actually fall onto the retina or CCD of the sensor. Once again, to a human this constraint is intuitive: the eyes must point at the target. A human can rotate their body, head and eyes to satisfy this constraint, and can even accurately track objects while the body is in motion. To a robot whose sensors may or may not be rigidly attached to its body, the problem of moving while maintaining the field of view constraint can be complex. To address these problems, a robot requires a path planner which can take both the occlusion and field of view constraints into account during the planning process. An example is shown in Figure 1.2. In many cases there will be no feasible path which allows perfect target visibility through an environment. In these cases the planner must find the best compromise, maintaining visibility as much as possible given the limitations of the robot’s mobility. 2 Chapter 1. Introduction target obstacle occlusion-free path start position destinationpartially occluded path Figure 1.2: The robot can take the shorter path which is partially occluded, or the unoccluded path which is longer but affords continuous visibility. 1.1 Motivation A robot path planner that can plan with respect to lines of sight and main- tain visual contact with a target has a wide variety of applications. Station- ary articulated robots are common in industrial settings, and are attaining greater autonomy. In manufacturing assembly, a change in design at the end of a product cycle will require extensive retooling and the implementation of new pre-programmed robot actions. The robotic system would be more flex- ible if it based its actions on sensor data rather than an a priori assumption of the part locations and shapes. Not only would it be able to accommodate small inconsistencies in the task at hand, but also changes at the end of a product cycle would require a change in the robot’s software model, rather than physical retooling. Mobile robots also require good sensory data, and are increasingly asked to autonomously navigate cluttered environments. 1.1.1 Articulated Robots Industrial manipulators perform a variety of tasks such as object manipula- tion, welding, painting, heavy lifting and inspection. Their reliance on sen- 3 Chapter 1. Introduction sors varies by the application. Some operate relatively blindly, but sensors can provide accurate positioning and localization, quality control or safety features. Typically, a robot’s workcell is kept completely clear of obstacles to avoid collisions by the robot. However, collision avoidance techniques (such as those discussed in Chapter 3) allow objects to be introduced into the workcell, and the robot will avoid hitting them. While this feature greatly increases the variety of tasks the robot can perform, it also introduces the possibility that objects will occlude the robot’s sensors. In the case of vision-guided bin-picking, a robot manipulator with an eye- in-hand camera must detect, identify, grasp and manipulate known objects that are randomly arranged in a bin. The key role of the sensor is to provide data (as a series of images) to a pose estimator to determine the exact position and orientation of a target part. If the robot can view the bin and its contents while moving, it can save time by eliminating the need to perform separate motions to observe and localize parts before approaching them. A visibility-aware path planner could increase the amount of data gathered on the bin contents during both the approach motion before the pick, and the subsequent withdrawal to the drop-off point. One can consider the case of a robotic manipulator performing an assem- bly task, such as a robotic system for installing sunroofs in an automobile plant. The robot brings the sunroof assembly in through the windshield opening in the partially-completed vehicle. A human worker inside the car guides the final placement and affixes the sunroof in place before the arm withdraws. Given the mass and strength of the arm, a safety system which prevents the arm from approaching too close to the worker is desirable. However, the worker’s position inside the vehicle means that sensors on the arm may not have a line of sight in many positions. A visibility-aware path planner could compute both approach and withdrawal motions for the arm that would maintain the best possible sensor coverage of the worker. Scene or object reconstruction tasks require multiple images of a sin- gle object. A sensor on a robotic arm or gantry gathers many images of the target object(s) for the reconstruction system to combine into the final model. Typically, the space around the target must be cleared of obstacles to prevent interference with the sensor. However, if the motion planner can compute trajectories for the sensor that avoid interference by the obstacles, the system could potentially operate in a cluttered environment. 4 Chapter 1. Introduction 1.1.2 Mobile Robots Given an inspection task, a mobile robot must gather sensor data on ob- jects in its environment. Given the difficulties involved in identifying 3D objects from a 2D image, the robot has an increased chance of success if it has multiple images of a given object taken from different viewpoints. An example would be a household robot tasked with retrieving a known object. Gathering multiple images of a candidate object would provide data for both recognition and localization, allowing the robot to be confident in both the identity and position of the object. To gather these images, the robot must move to various positions around the target while capturing images. The robot must avoid occlusions of the target by furniture and other obstacles, all the while maneuvering within the constraints of collision avoidance. A visibility-aware path planner could provide a route that would afford the best visibility of the target. An autonomous aircraft assigned to surveillance needs to maintain vi- sual contact with a target using a turreted onboard camera. The view of the target can be occluded by clouds, tall buildings, or the terrain below. A visibility-aware path planner can determine a path for the aircraft to maintain contact, if possible. The same planner can perform the opposite task: avoiding a line of sight to one or more observers. By avoiding the line of sight, the path planner can compute a path that exposes the robot as little as possible to detection by line of sight dependent sensors such as visual or radar systems. 1.2 Research Context Sensor planning problems involve determining the position, orientation, and other operating parameters of sensors that allow data collection, potentially under constraints. The problem may involve constraint satisfaction, opti- mization, or both. Static sensor planning determines fixed parameter values, but if the sensor is in motion, then the problem becomes one of determining the regions of the parameter space that satisfy the constraints, and the effect of motions on the quality of data collected by the sensor. Path planning for robotics is the problem of determining a sequence of motions that achieve a goal, possibly under constraints. A typical path planning constraint is collision avoidance: the path must not contain a robot pose that causes the robot’s body to touch an obstacle. Other constraints could include avoiding forbidden regions or avoiding actions the robot can- not perform. The planner may optimize the path with respect to its length, 5 Chapter 1. Introduction cost or other metrics. Path planning is distinct from motion planning in that it does not take the robot’s dynamics into account, and is not necessar- ily parameterised by time. Some robots may require an additional motion planner to determine the necessary time-parameterised motions required to execute the planned path. Path planning for visibility introduces the constraints (and possibly the optimization) from sensor planning into the path planning problem. A visibility-aware path planner has one or more constraints which are related to the quality of a sensor’s operating conditions. For example, the path planner may be constrained to keep a target within a certain distance due to a range limit on a sensor. The specific problem under consideration herein is path planning for tar- get observability with line of sight dependent sensors such as cameras. The planner’s constraints are to maintain an unoccluded line of sight between the robot’s sensor and the target, and to ensure that the sensor is oriented such that the target falls into the sensor’s field of view. Both of these con- straints are binary, though the necessary displacement required to reach the constraint boundary is measurable. In some cases, other constraints such as collision avoidance will have a higher priority than the visibility constraints. In this case, the planner may treat the visibility constraints as “soft”, using a loss-of-visibility cost metric, and attempting to reduce that cost. The probabilistic roadmap, or PRM, is a common technique in path planning. It uses a graph to represent safe robot configurations and motions compactly. It is well suited to planning movements in multidimensional con- figuration spaces under constraints and adapts readily to different metrics for desirable paths [2]. The aim of this research will be to develop techniques that adapt the probabilistic roadmap to perform path planning for target visibility. 1.3 Problem Statement Path planning for improved target observability is a constrained path plan- ning problem involving two constraints: the occlusion avoidance constraint and the field of view maintenance constraint. The path planner’s goal is to compute paths through the robot’s configuration space which optimize some factor (such as path length) while satisfying the two constraints. In order to avoid such occlusions and maintain the target within the field of view, the path planner must take four factors into account: 1. The available motions of the robot and the resultant motions of the 6 Chapter 1. Introduction onboard sensor. 2. The shape, extent and orientation of the sensor’s field of view w.r.t. the sensor body. 3. The position and shape of the target. 4. The obstacles in the environment Given all of this data, a visibility-aware path planner should compute a path to a goal configuration, reducing or eliminating occlusions of the line of sight and losses of the target from the field of view during the motion. This will ensure that the sensor can collect as much data as possible, even while moving. 1.4 Research Objectives • To create an algorithm which assesses a sensor motion, segmenting it into sections that have line of sight to a target, and those that do not. • To create a framework which applies the line of sight-based motion segmentation to motions (edges) within a well-known and accepted path planning scheme, namely the probabilistic roadmap. • To enhance the existing single- and multi-query PRM path planners to take the visibility information into account. The expected algorithmic contributions of this work are: 1. The motion segmentation algorithm that segments a sensor trajectory of arbitrary shape into regions which do or do not afford target visi- bility due to occlusion and the field of view. 2. The segment classification algorithm which labels each segment as af- fording or not affording target visibility. 3. A visibility penalty computation that assigns a trajectory a penalty depending on how much of its length affords visibility. 4. A static, multi-query, visibility-aware path planner, which computes a visibility penalty for all trajectories in a probabilistic roadmap and then uses Dijkstra’s algorithm to answer path planning queries. 7 Chapter 1. Introduction 5. A dynamic, single-query, visibility-aware path planner which uses an iterative search for high visibility paths within a PRM, evaluating visibility penalties only on the path under consideration. Since the penalties are computed at the time of the query, they can use different targets and updated information on the state of the environment. 1.5 Thesis Outline This thesis will describe the visibility-aware path planner, a system for com- puting robot motions that improve the visibility of a target to an onboard sensor. Chapter 2 contains the literature review, providing an overview of the existing work that influenced the contents of this thesis. Chapter 3 describes in detail the existing methods that form the basis for this thesis’ contribu- tion. Many of the techniques are derived from the works in the literature review, and any modifications or additions are documented at this point. Chapter 4 presents this work’s main contribution: the visibility aware path planner, in the form of a static solution for fixed environments. Of particular note are the four key stages of the algorithm: • The occlusion boundary generator. • The trajectory segmentation algorithm. • The segment classification algorithm. • The penalty computation These stages provide the information necessary for path planners of var- ious types to take occlusion into account. The initial example application is in a static probabilistic roadmap planner. Chapter 5 describes a different method of applying the occlusion data. Instead of a static planner, the occlusion penalties are computed on the fly by a dynamic planner that can use data collected at runtime. The underlying occlusion computations are the same, but by computing the penalties as- needed at the time of the query, the system can solve problems in a changing environment. The experiments chapter presents the results of several simulations and experiments which validate the performance of the visibility-aware path planner, as compared to a regular shortest-path planner. The experimen- tal results show that the visibility-aware path planner reduces the distance 8 Chapter 1. Introduction travelled by the sensor in configurations where it cannot see the target. Fi- nally, the thesis ends with a summary and a discussion of the planner’s performance with respect to the goals stated earlier. 9 Chapter 2 Literature Review 2.1 Preamble This chapter is a summary of work by other authors that form the foundation for this thesis. The visibility-aware path planner described herein combines several elements, each representing the work of many researchers. Each element is examined in turn, and relevant work is discussed. Literature relating to classical path planning is examined first: the main objective of that work is to avoid collisions between a robot and its envi- ronment. The results of this work are several methods for modelling the interactions of a robot with structures in 3D space. Although the intent was for those structures to represent the shapes of physical objects, such colli- sion avoidance systems can be used to predict interactions between robots and other, more abstract geometric forms. If a geometric representation of the sensor’s constraints is available, the collision-avoidance techniques can ensure that the constraints are satisfied. Work regarding sensor constraints is examined next. A wide variety of metrics are available to quantify the conditions under which sensors operate. The critical issue is to construct a simple representation of the geometric distribution of the metric values in the task space, relating the quality of sensor data to the pose of the sensor. Examples in the sensor planning literature apply optimization to the sensor metrics, determining the best sensor poses under various constraints. Methods of choosing good sensor poses are discussed and evaluated. Meth- ods that evaluate whole motions rather than individual poses are of partic- ular interest. Next, existing work regarding path planning for visibility is discussed. Of particular interest are methods that reason with regard to the known en- vironment: computing and maintaining line of sight. It is worth noting that work regarding path planning for stealth is in fact addressing the visibility problem, but with the visibility constraint inverted. Finally, observation planning considers the ability to choose good mo- tions with respect to the sensors. Work regarding the computation of good 10 Chapter 2. Literature Review data-gathering motions is examined. 2.2 Path Planning 2.2.1 Multi-Query Approaches A multi-query path planner uses a set of precomputed data to answer mul- tiple different path planning queries [3]. While the precomputation step is frequently expensive, the resulting ability to make low-cost searches through the data to answer queries is valuable in situations where the environment is static [4]. A pivotal work in this area is the development of the probabilistic roadmap [2]. The probabilistic roadmap is a sparse representation of the configuration space, using a graph whose nodes represent collision-free configurations and whose edges represent collision-free transitions between the configurations. Configurations are chosen at random, within the limits of the configuration space. They are tested using volume-intersection techniques to determine if the configuration produces a collision. Only collision-free configurations are added to the roadmap. To connect the nodes, a local planner is employed[2]. The local planner deterministically computes a transition between two nodes and checks the motion for collisions. Several methods have been proposed to improve the efficiency of the local planner’s collision detection [1]. Once a sufficiently dense roadmap is constructed, queries become simple graph- search problems, usually solved using Dijkstra’s algorithm [2]. The PRM will form the underlying structure of the static visibility-aware path plan- ner. The classical PRM assigns weights to each graph edge based on the joint-space distance between the nodes. By adding a weighted penalty to edges that induce an occlusion, we can insert visibility information into the existing PRM planner. Not all multi-query path planners are entirely precomputed. In the case of [5], the roadmap is built incrementally using data from an onboard rangefinder. The technique is an iterative modification of the original. Each time a new set of sensor data is collected, a new set of nodes and edges are added to the PRM. Once the new nodes and edges have been added, the planner can conduct path planning within the established roadmap, or compute a single motion to a new vantage point, collect a new set of data, and repeat the PRM expansion. Since the introduction of the PRM, several modifications have been pro- posed. Siméon et al [6] describe a “Visibility Roadmap”: a PRM that is represented by a tree of relatively few nodes. Each node in the PRM oc- 11 Chapter 2. Literature Review cupies a visibility domain, or region of the configuration space to which it can be connected by an unbroken line segment. In this application, the line segment represents an interpolation, wherein the robot can linearly interpo- late its joints from the node to any point in the node’s visibility domain[6] without collisions. This concept of “visibility” meaning “reachable in a sin- gle linear interpolation” is distinct from the concept of visibility in the task space with respect to sensor lines of sight. A key part of the collision-avoidance algorithm is the collision detector. The methods of determining whether two volumes intersect in 3-dimensional space are a well-developed field of research that is beyond the scope of this thesis. However, methods that efficiently apply the collision testing apparatus are of immediate interest, as the visibility tests are in some ways similar to collision tests. The GNU Triangulated Surface Library (GTS) [7] is a collection of geometric data structures and predicates that will form the framework for these computations in the visibility-aware path planners. One paper which stands out is the work of Schwarzer et al. [1], which describes an adaptive sampling method for applying collision tests. The key idea is that if an upper bound can be determined on the displacement of a robot arm due to a given joint angle change, then that upper bound can be compared to the minimum distance to any obstacle. If the displacement is insufficient to bring any part of the arm in contact with the nearest obstacle, then the trajectory created by the joint interpolation is guaranteed to be collision-free[1]. Thus, a collision detector which uses bisection to sample a trajectory for collisions can use this bounded-displacement estimate to eliminate “safe” segments of the trajectory. By avoiding these unnecessary collision tests, the performance is improved, sometimes quite dramatically. This adaptive-bisection method will be the foundation for the trajectory segmentation algorithm proposed in Section 4. 2.2.2 Single-Query Approaches A single-query path planner is designed to efficiently find a single path with- out the benefit of preprocessing [8]. Generally, this is desirable when the environment is subject to changes that would invalidate any precomputed information. A single-query planner typically employs a search in the config- uration space, which terminates when a suitable path is found. The search does not exhaustively compare all possible paths, and so has no basis for evaluating the quality of the path with respect to any alternatives. Addi- tionally, a search-based planner may have a high computational cost in the worst case, as the search may not easily find an acceptable path. 12 Chapter 2. Literature Review An example of a single-query path planner is the Rapidly-Exploring- Random-Tree-Connect (RRT-Connect) algorithm developed by Kuffner and LaValle [8]. This algorithm utilises two search trees originating from the start and goal points in the configuration space. The trees alternate adding new nodes. The nodes are chosen using a random distribution which is bi- ased to have a high likelihood in unexplored regions of the configuration space, and in regions that reduce the minimum distance between the two trees. Nodes are only permitted in the tree if they lie within the free con- figuration space (do not produce a collision) and can be connected to the exiting parent tree via a collision-free edge. The algorithm terminates when it is possible to connect the two trees by linking two leaf nodes. In effect, RRT-Connect creates a probabilistic roadmap that is tailored to a particular start/goal node pair. Sánchez and Latombe [9] use a similar approach, with two search trees that explore the configuration space, starting at the start and goal nodes and expanding towards one another. The algorithm utilises lazy collision check- ing, and does not perform collision tests until they are absolutely necessary to improve computation time. 2.2.3 Hybrid Path Planners In some scenarios a path planner may have to cope with an environment that is largely static, but has small dynamic elements. In this case it may be beneficial to have a hybrid single/multi-query planner that utilises pre- computed data to provide candidate solutions that are evaluated or refined by a single-query planner. Jaillet and Siméon [3] presented a PRM-based motion planner which uses the PRM graph to find a candidate path, and then uses the RRT-connect algorithm to attempt re-planning around any edges that are blocked by dynamic obstacles. 2.3 Sensor Planning Sensor planning is the problem of computing sensor placements and other parameters to improve the quality of captured data [10]. One of the most relevant contributions of sensor planning research to the problem at hand is the formulation of metrics for evaluating the quality of a given set of sensor parameters with respect to the sensor’s ability to operate. These metrics will need to be adapted to evaluate not only single sets of parameters, but entire parameterised motions. 13 Chapter 2. Literature Review Sensor planning can be formulated as an optimization problem, maxi- mizing a series of metrics describing the sensor’s performance in terms of its parameters [11]. The types of metrics to be optimized are highly het- erogeneous, resulting in a difficult constrained nonlinear optimization [11]. Evaluating visibility of a target with respect to occlusions by environmental obstacles requires computing the visibility volume, or the region of space from which there is a clear line of sight to the target [11]. Tarabanis et al. [12] present a boundary-based algorithm and a decomposition-based algo- rithm for computing the visibility volume of a planar polygonal target with respect to obstacles defined as a triangle mesh. The boundary-based algo- rithm will form the basis for computing the occlusion space in Section 4.2. In the Machine Vision Planning (MVP) system [13] all the constraints are integrated into a system that can plan intelligent sensor locations. The General Automatic Sensor Planning (GASP) system proposed by Trucco et al. [14] computes optimal sensor placements to observe feature points on the surface of a 3D object. The system takes into account oc- clusions of the features by the bulk of the object, so sensors do not try to observe an feature on the opposite side of the object. The computations are based on the target object’s aspect graph, and they produce a labelled tes- sellation of a sphere around the target. Each vertex in the sphere represents a potential sensor placement, and each vertex label represents the level of potential information gained there. While this system handles self-occlusion of features of the target by the target’s bulk, it does not take into account occlusions by any other objects, as the environment is assumed to be oth- erwise empty. Since the previous Boundary-Based algorithm can account for both self-occlusion and environmental occlusion, it is the superior choice when the additional computational cost is acceptable. 2.4 Path Planning for Visibility One notable type of path planning is centered on visibility. Path planners can take any factors into consideration if those factors can be expressed as a cost function in the search space. Path planners for visibility use cost functions that express the effects of motion through the environment on the ability of sensors to detect and examine their targets. Target visibility relies on the motion of the targets, the sensors and of other objects which influence the sensing process, such as obstacles. Murrieta et al. [15] propose a strategy for maintaining a line of sight between a mobile observer and a mobile target in a polygonal 2D world. The 14 Chapter 2. Literature Review strategy addresses the pursuit-evasion problem in an inescapable cell, where the observer moves in response to the target’s motion. Since the observer has no goal position, it moves according to rules that attempt to establish a “rod” (line segment) between it and the target without intersecting the walls of the enclosing polygon. While effective for a holonomic observer in a 2D polygonal world, it is unclear if this method will scale well to configuration spaces with more dimensions. Marzouqi et al. [16] describe a path planner for covert robot navigation. A robot must navigate through a 2D environment cluttered with obsta- cles, whilst using those obstacles as visual cover to avoid being detected by sentries. This problem is the same as the maximum-visibility path planner problem, but with penalties assigned to visibility, rather than occlusion. The planner uses a dense grid of cells, each with a penalty based on how many sentries have a line of sight to that cell. Using a distance transform based on the cumulative penalty from each cell to the goal position, the planner uses gradient descent to compute a maximum occlusion path. While effective in the 2D cell-based world, the planner’s distance transform is expensive to compute in high-density 2D maps, much less higher numbers of dimensions. Additionally, it would be prohibitively expensive to compute the visibil- ity status and penalty of every cell in a 6-dimensional configuration space. However, it shows the validity of the use of cumulative visibility/occlusion penalties in a path planner. Léonard et al. [17] address the problem of the field of view constraint. If a sensor mounted on a robot arm must observe a target at a known position, then the target must fall into the field of view frustum of the sensor. Paths provided by a probabilistic roadmap planner using linear joint interpolation may cause the sensor to point in a variety of different directions during the course of the path. In this case it is highly likely that the target will move out of the field of view. The paper proposes a dynamic check which determines if a given roadmap edge will cause the target to leave the field of view by performing a collision test between the target point and the walls of the sensor frustum. If the target collides with the frustum wall, then it is about to pass out of the field of view, and that particular roadmap edge cannot be used. By checking during the path planing query, the planner can accommodate targets at different positions for different path planning queries. The target-frustum collision test formulation will form the basis of field of view awareness in the visibility planner. 15 Chapter 2. Literature Review 2.5 Summary The literature provides all of the necessary components to construct an oc- clusion aware path planner in a high-dimensional configuration space. The probabilistic roadmap provides the planner framework, awaiting appropri- ate occlusion penalties. The boundary-based visibility region algorithm pro- vides the partitioning of the task space into the visible and occluded regions. Adaptive subdivision is an efficient strategy for detecting changes in geo- metric relationships for objects in motion, which will form the basis of the system to detect intersections between roadmap trajectories and the visi- bility boundary. A system which combines these can effectively determine when and where a given motion causes a sensor to gain or lose the line of sight, and use this combined approach to provide visibility penalties to the roadmap planner. 16 Chapter 3 Methods The existing literature provides a wealth of techniques for path planning. This section contains the techniques that were actively involved in the de- velopment of the occlusion aware path planners, as well as a breakdown of any modifications that were necessary to adapt them for use. Addition- ally, the wealth of different notations and terminology will be aligned and standardized for the purposes of this thesis. 3.1 Detailed Problem Statement As discussed in Chapter 1, line of sight dependent sensors cannot gather data on any target that is occluded by an object. If the sensor is mounted on a moving robotic platform, then it may be possible to move the sensor so that the target is no longer occluded. A visibility-aware path planner would compute motions of the robot, and thus the sensor, that afford the best target visibility. In order to do this, the planner would need models of any potentially occluding objects in the environment, the target position and shape, the range of motion of the robot, and the sensor itself. Given that data, the planner must reason about the lines of sight, and compute a path through the robot’s configuration space that reduces or eliminates any occlusions of the line of sight by objects in the task space environment. The probabilistic roadmap (PRM) planner described in Section 2.2 pro- vides an efficient, sparse representation of the collision-safe motions available to the robot. In particular, it provides the following capabilities: 1. The ability to sample corresponding points in the configuration space and task space to determine waypoints (nodes) which are safe in both spaces according to binary constraints such as collision avoidance . 2. The ability to find paths (edges) between the sampled waypoints that also satisfy the binary constraints. 3. The ability to plan paths within the roadmap graph, using numerical penalties on the roadmap nodes or edges as a cost function. The 17 Chapter 3. Methods planner can find paths of minimal total penalty. A visibility-aware probabilistic roadmap planner should be able to pro- vide the following capabilities: 1. The ability to partition the task space into the visible region, within which a sensor has line of sight to the target, and the occluded region, within which the sensor does not. 2. The ability to determine any and all configurations at which the robot’s motion causes the sensor to move in or out of the visible region. These configurations mark the points at which the visibility constraints be- come satisfied or violated. The sections of the motion between these configurations will be referred to as motion segments. 3. The ability to determine which segments of the motion do or do not afford target visibility. 4. The ability to assign each motion a penalty based on the length of any segments that do not afford visibility. By adding penalties that accurately represent the cost of a motion in terms of the distance the sensor must travel while occluded, the existing roadmap planner can find paths that minimize this penalty. 3.2 Task Space The robot, its environment, and the target objects that must be observed reside in the 3-dimensional task space P. The task space can present difficul- ties, particularly in representing what parts of the task space are occupied by the robot or observed by the robot’s sensors. Mapping a robot configuration to the task space requires a forward kinematics computation, which can be prohibitively expensive to exhaustively compute over a motion. The region of the task space that is unoccupied by obstacles is called Pfree. The robot is constrained by the roadmap to always lie within Pfree. Points within P are denoted p, and are in the world coordinate frame unless otherwise noted. 3.3 Configuration Space The configuration space Q (sometimes referred to as “joint space”) repre- sents the range of possible states of the robot. In the case of a d-degree- of-freedom manipulator, the configuration space is d-dimensional and is 18 Chapter 3. Methods bounded by the robot’s joint limits. Points in the configuration space are d-tuples denoted q. In the case of articulated manipulators, representing features of the task space such as Pfree may be difficult, as the mapping from the task space to the configuration space is often nonlinear and may be unavailable in a closed form. Probabilistic roadmaps overcome this problem through sampling, constructing a sparse representation of the configuration space with individual mappings to the task space for each sampled point [2]. 3.4 Sensor Limitations There are many types of sensors used to guide autonomous robots. This work focuses on imaging sensors such as cameras or laser rangefinders, but the principles are applicable to any sensors that rely on line of sight. All sensors rely on appropriate operating conditions to provide reliable data. While these conditions are wide and varied, only a few are directly influenced by the motions and configuration of the robot itself. These are: • Field of view: The sensor collects data from a limited conical or pyra- midal region in front of the aperture. • Range: The sensor cannot collect data on objects that are too close or too far away. • Resolution: Data gathered must have sufficient resolution on the ob- jects of interest. • Depth of field: Optical sensors have a limited field of focus, outside of which the image will be blurred. • Motion blur: Optical sensors will return blurred images if the velocity of an object’s projected image is too high. • Occlusion: The sensor cannot collect data if an object which is opaque to the sensor lies between it and the object of interest. These conditions form constraints on the robot’s motion if sensor data is required. The first four constraints are described in detail in the work of Tarabanis, Tsai and Abrams [11]. Satisfying all these constraints greatly improves the likelihood of collecting useful data [11]. The visibility-aware path planner will primarily address the occlusion constraint. The planner will also address the field-of view constraint, using techniques from [17]. For the purposes of this task, we assume that the other constraints are always 19 Chapter 3. Methods satisfied. Under well-lit conditions in a typical robot workcell utilizing a sensor with a large depth of field, the assumption is reasonable. 3.4.1 Field-of-View The sensor can only pick up signals in a limited angle in front of its aperture. This region is called the field of view (FOV) and may be conical or pyramidal (See Fig. 3.1). Satisfying this constraint involves keeping the sensor pointed towards the target, within the tolerance of the FOV cone. There are many ways to address this constraint, the simplest of which is to constrain the angle between the sensor’s axis and the ray pointing to the target. Such an approach may be difficult to formulate in the configuration space, as it is dependent on the position and orientation of the sensor. Field of View Sensor M frustum boundary Figure 3.1: The sensor field of view (FOV) is a conical or pyramidal region of space within which objects project onto the sensor’s receptor. The target must be in the field of view to be visible to the sensor. 3.4.2 Occlusion If objects that are opaque to the sensor lie between the sensor and the target, the target will not be visible to the sensor. In some cases, even a partial occlusion of a target can prevent accurate data gathering. The primary goal 20 Chapter 3. Methods of the visibility-aware path planner is to avoid approaching or achieving a configuration where such an occlusion occurs. The occlusion constraint partitions the task space P into the occluded region Pocc and unoccluded (visible) region Pvis with respect to the sensor position ps. The sensor can only collect data on targets that fall within the unoccluded region. Fig 3.2 shows the unoccluded region Pvis of a sensor at ps. The unoccluded region is analogous to the region of space that would be illuminated by a light source at the sensor’s location. Sensor Pvis Pocc Lv M Figure 3.2: Objects in the environment cause occlusions of the sensor’s line of sight. The target must be in the sensor’s unoccluded region to be visible. Given the reciprocal nature of line of sight, it is equivalent to test if the target is within the space visible to the sensor, or if the sensor is in space from which the target is visible. The latter approach is more efficient, as the planner must sample many sensor locations, typically far more than the highest likely number of target points. Thus, it is cheaper to compute the space from which the targets are visible, and compare the many sampled sensor positions to that space’s boundary. 21 Chapter 3. Methods 3.5 Occlusion Boundary Algorithm Tarabanis et al. proposed the boundary-based algorithm for computing the visibility region in a polyhedral world in [13]. Given a polyhedral representa- tion of the environment, the algorithm computes a polyhedral representation of the boundary between the unoccluded and occluded regions of the task space with respect to a target. The target is represented by a planar poly- gon, and the boundary represents the region from which the entire target polygon is visible. A valence test can determine if any point in the task space lies inside or outside the boundary [18]. As noted in [13], the boundary Lv between the occluded space and the unoccluded space can be divided into two types of surfaces: • Regions of the surfaces of the environment objects that have line of sight to the target (and thus occlude anything behind them), called front faces and collectively denoted Lv1 . • The (imaginary) surfaces that separate the occluded regions in the “shadow” of elements of Lv1 from the visible regions. These are col- lectively denoted Lv2 and called “separating faces” in [13] The visibility-aware path planner is not tightly coupled with the boundary- based algorithm; it only requires a metric which can provide the task-space distance from any point to the nearest occlusion boundary, and a predicate that can determine whether a given task-space point is in the occluded or visible region. The algorithm in [13] is efficient and robust, but by no means the only algorithm available. The occlusion boundary algorithm used in the visibility-aware path plan- ner is a modified version of the original presented in [13]. The primary difference is in the output. Unlike the original, the occlusion boundary is represented by a set of boundaries, each representing the region of space occluded by a single occluder with respect to one target point. An occluder is a region of the surface of an obstacle that is visible to the sensor, and thus obscures the region behind it. The boundary of the occluded region in- cludes both the occluding surface itself, and a series of separating faces that radiate from the occluder’s silhouette (see Fig 3.4). The occluded region is the union of the volumes enclosed by each of the individual boundaries. Formally: 22 Chapter 3. Methods Algorithm 1 : The Occlusion Boundary Algorithm 1: for all target point pt ∈ T do 2: Compute front faces Lv1(pt) of M w.r.t pt 3: Compute the separating faces Lv2(pt) from Lv1(pt) 4: Lv ← Lv1 ∩ Lv2 5: end for 6: Lv ← the set of all Lvt∀pt ∈ T 7: return Lv Inputs The algorithm’s inputs are a representation M of the environment and a set T of target points, both in task space. The environment M is represented as a triangle mesh. The algorithm uses the triangle orientations to determine what parts of the surface are facing the target, so the mesh must be oriented, and it is highly recommended that the mesh be closed and manifold. The system is robust to self-intersections in the mesh. The M should represent any objects that are opaque to the sensor. The set T of target points is not constrained to define a planar polygon as in [13]. In that case the occluded space was computed with respect to the area enclosed by the polygon. In the algorithm herein the occluded space is computed with respect to the points pt ∈ T only, and the points have no relationships that must be maintained. The algorithm produces a set of occlusion boundaries for each target point, so the computation time and boundary polygon count will scale linearly with the number of targets. Front Faces As in [13], the boundary algorithm computes the set of front faces Lv1 using backface culling. Lv1 is the set of all faces in M that contain the target point in their positive half-space (this is why M must be closed and oriented). Any back-faces must lie within the occluded space, not on its boundary, so long as M is closed. Unlike the approach in [13], at this point the planner subdivides Lv1 into its connected components. Each connected component (CC) can be treated as an individual occluder with its own occluded space (as shown in Figures 3.3 and 3.5). The total occluded space is the union of the volumes occluded by all components of Lv1 . It is possible for two connected components to arise from the same object, so the number of connected components relies upon the shape of the occluders and the position of the target points. This 23 Chapter 3. Methods modification to the algorithm reduces the complexity of the final mesh- generation operation, and allows for separate parts of the boundary mesh to be updated individually. The final output of the front faces computation is a set of boundary components collectively representing Lv1 , and possibly some redundant (oc- cluded) regions of the surface M . Algorithm 2 details the process of deter- mining the set Lv1 of all front faces of M . Algorithm 2 : The Front Faces Algorithm 1: for all target points pt ∈ T do 2: for all faces f ∈M do 3: if pt lies above the plane of f then 4: add f to Lv1 if not already a member 5: end if 6: end for 7: label← 0 8: for all unlabelled face f ∈ Lv1 do 9: labelConnected(f, label) 10: end for 11: end for 12: return Lv1 Algorithm 3 : The labelConnected(root, label) Algorithm 1: for all neighbour face fn of f do 2: if root is not labelled then 3: set root label as label 4: labelConnected(fn, label) 5: end if 6: end for Separating Faces The separating faces are planar polygons that radiate from the exterior edges of Lv1 . Collectively they form Lv2 , which together with Lv1 comprises the visibility boundary. Each connected component in Lv1 has a set of exterior edges H. Exterior edges are part of only a single front face (see Fig. 3.4). Interior edges are part of two adjoining faces. Each edge h ∈ H generates one separating face, 24 Chapter 3. Methods target Lv1 M Figure 3.3: The front faces of the environment objects (with respect to the spherical target) are marked in red. an unbounded trapezoid emanating from h. The two sides of the trapezoid are rays starting at the endpoints of h and emanating away from the target point t (see Fig. 3.4). Crossing the separating face causes t to be occluded by h and its adjoining face. The separating faces are unbounded, and extend to infinity. However, it is typically computationally convenient to constrain them to a finite space. The two rays that define the sides of the trapezoid can be given a fixed length that is much larger than the size of the workspace, or the entire boundary can be clipped at a certain radius from the centroid of the target point set. If it is computationally convenient for Lv to be a closed mesh, then a duplicate of Lv1 can be scaled up to form a “cap” on the outer limits of the boundary mesh. The Occlusion Boundaries The occlusion boundaries Lv collectively represent the occluded space. In the original implementation in [13] the final step was to combine the multiple boundaries into a single collective boundary using mesh boolean operations. However, the boolean operation is computationally expensive, having a com- 25 Chapter 3. Methods t Back Faces Separating Face Q q Lv1 Lv2 Figure 3.4: The front faces of the objects are denoted Lv1 , and are sur- rounded by the boundary edges H. The target point t and the edge h ∈ H define a triangle, which in turn defines the trapezoidal separating face. Lv2 is the collection of all separating faces from all target point-edge pairs. plexity of O(n2) in the worst case [19]. The often unmentioned side effect of this process is that meshes produced by boolean intersections often have many more polygons than the source meshes, and that some of the resul- tant polygons will be of poor quality1. One solution is to re-triangulate the resultant mesh to improve the quality of the triangulation, but this has an additional computational cost. For the purposes of the visibility-aware path planner, there is another op- tion. Instead of combining the boundaries into a single mesh, the trajectory can be compared to each separate boundary in turn, and the intersections detected this way. This can produce superfluous intersections, as the trajec- tory may pass through boundaries that lie within other cells. However, as will be discussed in the description of the segment classification algorithm in Section 4.4, the additional intersections do not affect the result of the algo- rithm. Since only one occluder is necessary to block the line of sight, it does not matter if trajectory lies within one, two, or several occluded volumes, the planner only measures the occluded length of the trajectory. 1Mesh polygons are considered to have the highest quality when they are equilateral (or nearly so). 26 Chapter 3. Methods (a) 1 (b) 2 (c) 3 Figure 3.5: Each connected component of front faces produces a separate occlusion boundary (blue). By keeping the occlusion boundaries as separate components, the planner avoids the cost of the boolean operations, and any subsequent operations necessary to “clean up” the mesh. The number of polygons in the boundary mesh will remain fixed, whereas the result of a boolean intersection will have a higher polygon count, and the number of polygons will vary depending on the arrangement of the components. Additionally, if some of the objects in the environment move or change shape, it it not necessary to recompute the entire boundary, only the boundaries produced by components that have changed. In the case of the dynamic planner discussed in Section 5.3 this could result in a major improvement in efficiency if only a few objects in the environment are moving. Keeping the components of the boundary separate does incur an addi- tional cost. If one component is completely contained inside another, then the interior boundary is “eclipsed”and is not conveying any additional infor- mation, and thus any intersection tests made on it are wasted. The number of volumes is the number of connected components in Lv1 multiplied by the number of target points in T . In the worst case, where one boundary eclipses all the others, there will be many times more intersection compu- tations than are necessary. If the particular application frequently leads to eclipsing, then a geometric test such as spherical projection [14] that can detect and ignore eclipsed volumes would improve performance. 3.6 Probabilistic Roadmap As introduced in Section 2.2, the probabilistic roadmap is a static multi- query path planning structure first proposed by Kavraki et al [2]. The 27 Chapter 3. Methods target Lv1 MLv2 Figure 3.6: The occlusion boundaries of all the front face components col- lectively enclose the occluded space. Note that the individual boundaries may overlap. PRM consists of a graph G = (N,E) whose nodes N represent collision-free configurations, and whose edges E represent collision-free motions between configurations. A sufficiently dense roadmap can be used for multiple path planning queries, so long as a collision-free path can be established from the initial and goal nodes to any nodes in the roadmap. Once those connections exist the problem becomes a path search in a weighted, undirected graph. Standard techniques such as Dijkstra’s algorithm can provide paths through the roadmap. The PRM generation process samples configurations q ∈ Q at random and tests them for collisions. Each collision-free configuration : q ∈ Qfree is added to the roadmap graph as a new node n ∈ N . The terms “con- figuration” (q) and “node” are generally interchangeable, but specifically, each node n contains a d-tuple q. This process continues until the desired number of collision-free configurations are created, represented as nodes in the graph. If specific configurations such as the robot’s home position or the drop-off position must be present in the roadmap, those configurations can be added manually to N at this point. 28 Chapter 3. Methods The algorithm then attempts to connect each node to its k-nearest neigh- bours, using a “local planner”. The local planner is a single-query path planner that is designed to work well over small distances in the configu- ration space. The local planner rejects a candidate motion if the motion causes a collision; otherwise it accepts the motion, adding a new edge e to the set of edges E. The actual motion is assumed to be deterministic and is not explicitly stored. A given edge ej,k provides access to its two endpoint nodes, nj and nk, which in turn provide their configurations qj and qk. The most common type of motion is a linear interpolation in the configuration space, as seen in [2]. Any type of interpolation may be used so long as it is consistently parameterised in the roadmap construction step and the occlusion penalty computation step. It is desirable for the local planner to be simple and fast. It can er- roneously reject motions without compromising the planner, but must not erroneously accept any. The aim is to achieve sufficient density so that paths exist in the PRM to connect all reachable regions of the configuration space. If the planner has a very low acceptance rate, increasing the number of attempted connections k may compensate. In [2], the k-th nearest neighbours of each node are selected for attempted connection in the roadmap using the configuration space distance between them. “Distance” is conventionally defined using the 2-norm (Euclidean distance in Q) or the infinity norm (maximal joint displacement). These metrics reflect the interpolation time, which is restricted by the rotational speed of the robot’s joints. However, in the case of visibility-aware planning, the joint displacements are less important than the sensor’s displacement. In this case, a metric such as the sensor positional displacement in P (using a 2- norm) will make connections with short sensor displacements between them more likely to exist within the roadmap. Using a metric in Q may lead to connections which are proximal in Q, but lead to large sensor displacements in P due to the radii of the robot links. This is a liability for a planner that is searching for paths that finely control the sensor position. Further details are discussed in Chapter 4. 3.7 Adaptive Subdivision The probabilistic roadmap local planner usually relies on exhaustive sam- pling to determine if a continuous path between two nodes is free of colli- sions. Adaptive dynamic collision checking, proposed in [1], reduces many unnecessary samples by only sampling when there is a risk of collision. 29 Chapter 3. Methods The key idea in adaptive sampling is that sampling can be done via bi- section, and at each bisection step it is possible to determine if the segment to be bisected has the potential to contain a collision. If it does not, it will not require any further subdivision. To determine if a segment requires subdivision, the minimum distance between the moving object and the ob- stacles is compared to an upper bound on the displacement of the moving object due to the motion segment in question [1]. Let qi and qi+1 repre- sent the robot configurations at the endpoints of the motion segment qi,i+1. Let d(qi) and d(qi+1) be the minimum distances from the robot in the two configurations to the nearest obstacle. Let `(qi,i+1) be a computed upper bound on the maximum possible displacement of any point on the robot due to the motion segment qj−k. If min(d(qi), d(qi+1)) > `(qi,i+1) then even if the robot moves as much as possible directly towards the obstacle, it will not be able to traverse the distance. Thus, no collision is possible in the motion segment qi,i+1 [1]. The inequality can function as a termi- nation condition on the iterative or recursive bisection of the motion, thus preventing unnecessary samples when collisions are impossible. Not only does this technique improve the efficiency of the roadmap con- struction, it can also be adapted for the sampling algorithms that detect losses of the line of sight, and losses of the visual target from the field of view. These topics are addressed further in Sections 4.3 and 4.7, respectively. 3.8 Field of View Checking The work of Léonard et al. [17] describes a dynamic field of view test using adaptive subdivision. The field of view constraint is reformulated as a collision test between the target point(s) T in the task space and the field of view frustum of the sensor. Assuming that the target points are initially in the frustum, then any movement of the sensor that causes the target to collide with the planar walls of the frustum will suffer a loss of the target from the field of view. Specifically, the position of the target is represented in the sensor’s coordinate frame. In this frame the motion segments represent motions of the target point, rather than the robot body. The maximum possible target point displacement is compared to the minimum distance from the target point to the frustum boundary in the same manner as in [1]. The proposed algorithm rejects edges that cause a loss of target visibility due to the field of view constraint. However, the same algorithm could instead detect the points along the motion at which the collisions occur, a property that will be used in Section 4.7. 30 Chapter 3. Methods 3.9 Summary All of these techniques will be required to create the visibility-aware path planner. The probabilistic roadmap will provide a collision-free set of candi- date motions. The visibility boundary algorithm will provide a partitioning of the task space into the occluded and unoccluded regions. The adaptive subdivision technique will provide a sampling strategy to detect when the motions pass through the boundary. The visibility-aware path planner de- scribed in the following chapter will combine all of these techniques in a coherent system. 31 Chapter 4 Occlusion-Aware Path Planning 4.1 Overview The goal of the visibility-aware path planner is to find paths within the roadmap that afford the best visibility of the target region. In the context of this algorithm, the definition for “path of best visibility” is the path that minimizes the “occluded distance travelled”. The “occluded distance travelled” is the distance moved by the sensor while it does not have line of sight to the target. This distance will be reduced by assigning an “occlusion penalty” to each edge in the roadmap based on the task-space length of the occluded segments of the sensor trajectory qj, k associated with the edge ej,k, and then searching for the path of minimum penalty within the graph. To compute the occlusion penalty, the planner must have a model of the environment, represented as a triangle mesh M . Additionally, the target region must be represented as a planar polygon, denoting the region that the planner must attempt to keep unoccluded. Finally, the algorithm requires a forward-kinematics routine, able to determine the sensor position and orientation in task space corresponding to a given robot configuration. All of these computations will take place in the precomputation stage, so the high cost of determining the penalties will not impact the performance of the planner during queries. The target position and environment objects are known at the time of the precomputation, and must remain static, otherwise the penalties will become invalid. Algorithm 4 shows the high-level steps of the precomputation stage. The individual steps are detailed in Sections 4.3–4.6. The computation iterates through each edge, performing a series of steps to compute the edge penalty. When all the penalties are complete, Dijkstra’s algorithm computes the shortest paths from each node to each other node, the results of which are referred to herein as a “Dijkstra Tree”, and are stored in preparation for path planning queries. The output is a set of occlusion penalties: one per PRM edge. The 32 Chapter 4. Occlusion-Aware Path Planning penalties are non-negative. Dijkstra’s algorithm requires strictly positive edge weights, so the occlusion penalties cannot be used in their raw form as edge weights. However, simple measures such as adding a very small value to all occlusion penalties would suffice to meet the positive weight requirement. The value can be a constant or proportional to the interpolation distance of the motion represented by the edge. Algorithm 4 : The Occlusion Aware Path Planner Algorithm 1: G = (N,E)← CreatePRM(M) {See Section 3.6} 2: Lv ← OcclusionBoundary(M,T ) {See Section 3.5} 3: for all edge e ∈ E do 4: I ← SegmentTrajectory(e, Lv) {See Section 4.3} 5: V ← ClassifySegments(I, Lv, t) {See Section 4.4} 6: wocc(e)← OcclusionPenalty(e, I, V ) {See Section 4.5} 7: wdist(e)← Length(e) 8: w(e)←MixPenalties(wocc(e), wdist(e)) {See Equation 4.3} 9: end for 10: W ← the set of all weights w(e)∀e ∈ E 11: for all node n ∈ N do 12: dt(n)← ComputeDijkstraTree(N,E,W ) {See Section 4.6} 13: end for 14: DT ← the set of all dt(n)∀n ∈ N 15: return DT The visibility-aware path planner must also take the sensor field of view into account when determining the effects of motions on target visibility. This capability is described in detail in Section 4.7. The term “occlusion- aware planner” refers to system that avoids occlusions only, whereas the “visibility-aware planner” takes both constraints into account. 4.2 Occluded Space As described in Section 3.5, the occluded region Pocc is a subset of the task space P. It is defined as the region of the task space from which a clear line of sight does not exist to the target T . It is the complement of Pvis, the visible region. Pocc and Pvis are separated by Lv, the occlusion boundary. The objective of the visibility-aware path planner is to plan paths that minimize intersection with Pocc. The shape of Pocc and thus Lv are determined by the target points T and the environment objects M (see Fig. 4.1). The occlusion boundary Lv is represented by a triangle mesh which is provided 33 Chapter 4. Occlusion-Aware Path Planning by the occlusion boundary algorithm described in Section 3.5. The two functions that utilise this boundary are a point-boundary distance metric dist(p, Lv) and a predicate v(p) to determine if a given task-space point resides in Pvis or Pocc. target environment objects occluded region Figure 4.1: The shaded area represents the region occluded by the environ- mental obstacles. 4.3 Trajectory Segmentation Each edge in the roadmap graph represents a trajectory for the robot arm. The endpoint configurations of the trajectory, q1 and q2 define the endpoints of a line segment in a d-dimensional configuration space. That line segment represents the linear interpolation of the joint angles. The interpolation along the trajectory qj,k is parameterised by a parameter τ ∈ [0, 1] using the linear interpolation: qj,k(τ) = (1− τ)qj + τqk (4.1) The task-space trajectory of the arm-mounted sensor can be computed from the arm motion as a function of the parameter τ . The occlusion com- putations do not consider the field of view of the sensor, so only the sensor’s 34 Chapter 4. Occlusion-Aware Path Planning position, not its orientation is considered. The sensor trajectory is a curve in the task-space, tracing out the path of the sensor’s aperture during the motion. The orientation of the sensor will be addressed in Section 4.7. occluder targetcamera trajectory intersection occluded region Lv Figure 4.2: The segmentation algorithm finds the intersections of the sensor trajectory with the visibility boundary. Algorithm 5 details the trajectory segmentation process, which performs a binary search along the trajectory. The search interpolates τ and uses the forward kinematics routine to compute the position of the arm-mounted sen- sor. At each sampled point the algorithm computes the minimum distance dist(p, Lv) from the sensor position to the boundary mesh. If the distance is less than a threshold , an intersection is reported. The parameter  sets the maximum positional error of the computed intersection points. It is important to note that if the trajectory passes through two triangles that are less than  apart, this event may be detected as a single intersection. As a result, the algorithm will ignore the small distance between the two intersections, regardless of whether it is occluded or non-occluded. The parameter  determines the spatial resolution of the sampling process. The value of  should reflect the magnitude of acceptable error in the detection of intersections. A convenient value of  is one hundredth or thousandth of the length of the shortest trajectory in task space. Since two samples could conceivably report the same intersection, any intersections i, i + 1 for which |τt − τi+1| < µ should be merged, where 35 Chapter 4. Occlusion-Aware Path Planning µ is a user-defined merge threshold. A convenient value of µ is 0.01 or 0.001, enforcing the requirement that consecutive intersections are more than 1/100th or 1/1000th of the trajectory length apart. Once the algorithm finishes sampling, it sorts the list I of intersections by their τ value and merges any consecutive entries that differ by less than the threshold. Algorithm 5 : The Trajectory Segmentation Algorithm. SegmentTrajectory(ej,k, Lv) uses a recursive subroutine TS to per- form adaptive subdivision, detecting intersections between a segment of the trajectory qj,k between τ1 and τ2 and the occlusion boundary Lv 1: TS(qj,k, τ1, τmid, d1, dmid) 2: 3: isect← ∅ 4: if depth > maxdepth then 5: return isect 6: end if 7: τmid ← (τ1 + τ2)/2 {segment midpoint parameter} 8: pmid ← FK(qj,k(τmid)) {camera position} 9: dmid ← dist(pmid, Lv) {dist. to boundary} 10: if dmid <  then 11: isect← isect ∩ [τmid,pmid] 12: end if 13: if min(d1, d2) > `(qj,k(t1)− qj,k(t2)) then 14: return isect 15: end if 16: isect← isect ∩ TS(qj,k, τ1, τmid, d1, dmid) 17: isect← isect ∩ TS(qj,k, τmid, τ2, dmid, d2) 18: return isect The adaptive-bisection method described by Schwarzer et al. [1] elimi- nates unnecessary samples during collision detection by comparing the dis- tance to the nearest obstacle to the maximum displacement for a given segment of a trajectory. The same technique can be used to eliminate un- necessary samples when computing the intersections of the trajectory with the occlusion boundary. This technique requires an upper bound for the arc length of the sensor’s motion for each trajectory segment, `. If ` is less than the distance from the sensor position at either end of a trajectory segment to the nearest triangle in the occlusion boundary, then that segment cannot contain an intersection. Thus no further subdivisions of that segment are necessary (see Fig. 4.3). 36 Chapter 4. Occlusion-Aware Path Planning The basis for computing ` is as follows. If the arm has d joints, then there will be d angular displacements (one per joint), θ1−d. The radii of the individual arm links in the plane of these rotations are termed r1−d. The maximum arc length due to a given joint in the arm is the angular displacement θ of the joint multiplied by the maximum possible length of the arm distal to that joint. Since this is an upper bound, the worst-case scenario is considered where all the distal links are positioned so that they form the largest possible radius. Thus, the worst-case radius for a given joint is the sum of the radii of all links distal to that joint. The maximum displacement for the whole arm is the sum of all joint angles multiplied by their respective worst-case radii, yielding the maximum possible sensor trajectory arc length, `. Formally: `(θ) = d∑ i=1 (θi × d∑ j=i rj). (4.2) boundary A BC boundary A BCD E boundary A BCD EF G H boundary intersection Initial Subdivision Second Subdivision Third Subdivision Fourth Subdivision Figure 4.3: The black dots mark the intersections of the trajectory with the visibility space boundary. Segments marked with checks are guaranteed to be intersection free, and require no subdivision. Those with ’X’ marks may contain intersections, and require further subdivision. 37 Chapter 4. Occlusion-Aware Path Planning Accurately bounding the running time of the mesh-trajectory intersec- tion algorithm is difficult due to its adaptive nature. An absolute worst-case scenario would involve a trajectory whose entire length lies at a distance minutely greater than  from the mesh surface. The algorithm would thus exhaustively subdivide the trajectory until the ` value for every segment becomes less than the distance to the mesh. Thus, the algorithm will ter- minate when ` <  for all segments. The displacement ` monotonically decreases as the trajectory segments are subdivided, and  is a constant, so we can be certain that the algorithm will terminate in a finite number of subdivisions for any  > 0. By adding the simple expedient that no two samples should be less than  apart, the subdivision will cease at a depth where length/2depth < , where length is the task-space arc length of the trajectory. Thus we can compute that there are 2× (length/)− 1 nodes in the tree, which means the complexity is linear with respect to the ratio of the trajectory length and  in the worst case. Fortunately, for articulated robots, the curved task space sensor trajec- tories are unlikely to lie so precisely along the visibility boundary. Actual running times will depend heavily on the geometry of the robot and visibility boundary. The intersection detection process returns the list I of intersection points in task space and the associated parameter value τ for each intersection. 4.4 Trajectory Segment Classification Trajectory segmentation is the process of dividing up the sensor trajectory into the segments separated by the occlusion boundary. The intersection points mark the boundaries of the segments of the trajectory. Each segment must then be tested to determine if it lies inside or outside the occluded space, classifying it as occluded or unoccluded. Figure 4.4 shows an example trajectory in an automobile manufacturing setting. The curve is a visualisation of the trajectory of a sensor mounted on a industrial robot located beside a vehicle under construction. The labels indicate which segments of the trajectory violate either the occlusion or field of view constraints. The short lines extending from the trajectory indicate the camera’s optical axis. The intersection points I divide the trajectory into a set of segments S (See Fig. 4.5). Each segment must be tested to determine whether it lies inside or outside the visible space Pvis. The intersection points are represented by their parameter values τ . 38 Chapter 4. Occlusion-Aware Path Planning visible out of FOV occluded both target occluder b o v f b v f b f v f o b Figure 4.4: The classification step determines which constraints are satisfied in each trajectory segment. As described in Section 3.5, the boundary is represented by the triangle mesh Lv which partitions P into two regions, Pocc and Pvis. Since the boundary itself has no explicit inside or outside, a point psafe in P must be supplied to act as a reference, determining which half of P is Pvis. This point is referred to as the “safe point” and corresponds to a point known to have a clear line of sight to the target. The target point can serve as a safe point provided it does not lie on Lv. Otherwise, the sensor position in which the target was first detected can serve as psafe. The classification algorithm divides the trajectory at each intersection point in I, producing |I| + 1 trajectory segments. For each segment it computes a midpoint parameter τm by linear interpolation of the parameter τ at each endpoint of the segment. Next it computes the task-space position pm of the midpoint corresponding to τm. The algorithm then tests whether pm is inside or outside Pvis by computing the valence of a line segment connecting pm to the safe point (see Fig. 4.5). The valence is the number of intersections between the line segment and the boundary. An even valence indicates that pm is inside Pvis, as the line segment has re-entered the 39 Chapter 4. Occlusion-Aware Path Planning volume as many times as it has exited it [18]. The classification outputs a set V of boolean classification values: one for each segment, indicating if the segment is occluded or not. P Pvis occboundary safepoint A B im mAi iB intersection Figure 4.5: Each trajectory segment is tested to see if it is inside or outside the occluded space. A linear search implementation of the valence test has a complexity of O(|F |), where |F | is the number of triangular faces in the boundary mesh Lv. Thus, for a trajectory of s segments, the overall complexity is O(sF ). This can be improved by using an acceleration structure such as those described in [20] to reduce the number of faces that must be checked. 4.5 Occlusion Penalties Once the trajectory’s segments have been labelled, the algorithm must de- termine a penalty for the trajectory based on the lengths of the occluded segments. The occlusion penalty (weight) on an edge e in the roadmap is denoted w(e) and represents the suitability of the sensor trajectory associ- ated with that edge for data gathering. A value of zero would indicate that the entire trajectory affords line of sight. The value of the penalty should increase as the sensor encounters occlusions of the target of greater distances along the trajectory. Ideally the value of the penalty should reflect spatial information: how far the sensor moves without line of sight, or temporal 40 Chapter 4. Occlusion-Aware Path Planning information: how much time will elapse while the target is occluded. The spatial computation of occlusion penalties is relatively simple. The task-space penalty is proportional to the task-space length of the occluded trajectory segments. The actual value of the segment lengths can be com- puted using the forward kinematics. If a closed-form solution for a segment length is difficult or impossible to compute, then a simple piecewise-linear approximation using points sampled along the trajectory can determine the task-space length to an arbitrary level of accuracy. The temporal computation requires knowledge of the sensor’s velocity at any point on the trajectory. Using the velocity data it is then possible to compute how much time the sensor will remain in an occluded state, or length of the longest interval. The choice between spatial or temporal penalties depends on the ap- plication. Regardless of which is chosen, let wocc(e) denote the occlusion penalty for the edge e. Let wdist(e) denote an additional penalty repre- senting the overall length of the trajectory. Assuming that e connects two different nodes, wdist(e) will be greater than zero. The choice of exactly how to compute wdist(e) is also application-specific, but should reflect a metric on the type of motion the planner should minimize. A simple Euclidean distance in the joint space will serve in many cases. The total weight on an edge is denoted w(e) and is a linear combination of wdist(e) and wocc(e) (See Eqn. 4.3). A parameter λ ∈ [0, 1) controls the relative strengths of the two penalties. w(e) = wdist(e)× (λ− 1) + wocc(e)× λ (4.3) If the total path length is not a significant concern, then a value of λ close to (but not equal to) 1 will give the highest level of target visibility. Since the occlusion penalties can be zero (indicating a path with 100% visibility) a value of zero for λ is inadvisable, as it could result in paths with weights of zero, which is not an acceptable input to Dijkstra’s algorithm. In the case where avoiding violation of the visibility constraint is ex- tremely important, instead of simply applying a penalty to occluded edges, the edges could be removed from candidacy in the roadmap entirely. In this case, another metric should be used to determine edge penalties, such as the edge length in Q. Then, edges which cause occlusions should have their penalties raised by a large value (possibly an effectively “infinite” value). This way, the path planner will avoid these edges entirely. This is similar to the method in [17]. However, it is possible that no occlusion-free path exists between the initial and destination configurations. In that case, the 41 Chapter 4. Occlusion-Aware Path Planning path planner would be unable to answer the query. 4.6 Dijkstra’s Algorithm Dijkstra’s algorithm computes the minimum-weight paths from a single source node to all other nodes in a weighted graph [21]. The result is repre- sented as a Dijkstra Tree: a vector of length |N |, where |N | is the number of nodes in the graph. Each entry in the tree is the index of the previous node in the path to the start node. Starting at the desired destination, the query algorithm backtracks through the Dijkstra tree, following the “previous” nodes back to the start. The path planner uses Dijkstra’s algorithm to compute minimum-penalty paths in the probabilistic roadmap. If the paths all begin at the same posi- tion, then a single Dijkstra tree will suffice for all queries. If the paths can start at arbitrary nodes, then |N | Dijkstra Trees will be required, using |N |2 unsigned integers’ worth of storage. The computational complexity of Dijkstra’s algorithm on a graph G = (N,E), with |N | nodes and |E| edges is O(|E|+|N |log|V |) for a single-source solution using the most efficient Fibonacci Heap implementation. If an all- pairs solution is necessary, then the complexity is multiplied by |N |. This is expensive, but can all be done in the precomputation stage. The queries will take O(|N |) time to compute in the worst case, when every node is included in the path (which is extremely unlikely in a well-connected roadmap). 4.7 Integrating Field of View Awareness Avoiding occlusions is a necessary but not sufficient condition for target vis- ibility. The trajectory segmentation algorithm lends itself well to managing both constraints simultaneously. The same adaptive-bisection algorithm can discover all points on the trajectory where the target passes in or out of the field of view. This, along with the occlusion boundary crossings, provides series of potential changes in the target visibility. As in [17] the algorithm determines the minimum distance from the target point pt to the frustum boundary dC(pt,qj,k(τ)) for the two segment endpoint configurations qj,k(τ1), qj,k(τ2) in the camera reference frame C. It compares the minimum distance to the maximum target point displacement `C(qj,k(τ1),qj,k(τ2)), also expressed in the camera coordinate frame [17]. The FOV-based segmentation returns another set of intersection points. By merging these intersections with the occlusion boundary intersections 42 Chapter 4. Occlusion-Aware Path Planning and sorting them, the algorithm generates a list of all points at which either one or the other constraint changes status along the motion. In the subsequent classification step, two tests are necessary per trajec- tory segment. The two tests classify the segment as occluded or unoccluded using the mesh predicate v(p), and in-FOV or outside-FOV using the sign of the frustum-distance dC(pt). Only segments that are both in-FOV and unoccluded afford target visibility, therefore the penalty is proportional to the length of all other segments. The final classification appears as in Figure 4.4, with various segments satisfying none, one or both of the constraints. Algorithm 6 : The Trajectory Segmentation Algorithm - Field of View Variant. As in SegmentTrajectory, this algorithm recursively subdivides a trajectory, but in this case it returns the points at which the target crosses the field of view frustum boundary. 1: TS(qj,k, τ1, τmid, d1, dmid) 2: 3: isect← ∅ 4: if depth > maxdepth then 5: return isect 6: end if 7: τmid ← (τ1 + τ2)/2 {segment midpoint parameter} 8: dmid ← dC(pt,qj,k(τmid)) {dist to boundary} 9: if dmid <  then 10: isect← isect ∩ [τmid,pmid] 11: end if 12: if min(d1, d2) > `(qj,k(τ1)− qj,k(τ2)) then 13: return isect 14: end if 15: isect← isect ∩ TS(qj,k, τ1, τmid, d1, dmid) 16: isect← isect ∩ TS(qj,kτmid, τ2, dmid, d2) 17: return isect By taking both constraints into account, the planner is able to avoid paths that lose visibility for either reason. This is beneficial, given that line of sight is unlikely to be useful if FOV is not satisfied. However, FOV aware- ness may increase the penalties on many edges, given that the likelihood of a randomly-selected path in a roadmap will point the sensor at the target is low. This may effect a dilution of the occlusion awareness, as the penalties may simply be high across the board. For the doubly-constrained planner to function effectively, there must be suitable motions within the roadmap. 43 Chapter 4. Occlusion-Aware Path Planning Adding FOV-awareness makes the definition of “suitable” much more strin- gent, the roadmap must be made more dense in order to compensate. 4.8 Effects of Roadmap Connectivity The visibility-aware planner will operate within the existing roadmap. It will not add any nodes or edges during its operation. Thus, the range of potential paths available to the planner depends on the level of connectivity of the roadmap. Larger numbers of potential paths increase the likelihood of a high-quality result. However, the computational cost of the path search algorithm will increase accordingly. Some modified versions of the probabilistic roadmap use a sparsely- connected graph or spanning tree as an alternative to the dense roadmap. In [6] the proposed “visibility roadmap” uses only a few edges and ver- tices to represent the collision-free space (in this case “visibility” refers to a collision-free linear interpolation in the configuration space, and does not refer to sensors at all). The visibility-aware path planner would be unable to operate on such a roadmap, as there exists only one path through the roadmap between any two points. In [3] the hybrid planner overcomes the sparse nature of the underlying roadmap by creating new nodes and edges at the time of the query using the RRT-connect algorithm. Since the visibility- aware planner does not add new nodes or edges, a high initial connectivity is desirable. A high connectivity has an additional desirable effect. In a classical prob- abilistic roadmap for an articulated manipulator the new edges are added starting with the nearest neighbours to each node in the configuration space. As a result the edges tend to represent short interpolations. However, all di- mensions of the configuration space are weighted equally in the computation of C-space “distance”. In the case of a typical industrial (anthropomorphic) manipulator with rotational joints, this means that a rotation of the base joint, is considered to be the same “distance travelled” as a rotation of the same angle in the last wrist joint. Two nodes may be considered “near neigh- bours” if they differ only by the value of the proximal joints. Rotations of the proximal joints tend to result in large displacements of the end effector due to the radius of the arm. Thus, many of the edges in the roadmap repre- sent large, sweeping motions of the end-effector in task space, which in turn are more likely to receive large occlusion penalties. In practice, this may mean that two roadmap nodes that have similar end-effector positions and orientations may not be connected if their configurations are not considered 44 Chapter 4. Occlusion-Aware Path Planning “near neighbours”. As a result, many paths through the roadmap, while relatively straightforward in the configuration space, send the end-effector (and by extension, the sensor) zig-zagging around the task space. Thus, the resultant sensor motions associated with each roadmap edge can be quite long, and thus more prone to high occlusion penalties. Another way of providing more useful edges to the planner is to perform the roadmap construction using a task-space metric of “nearest neighbours”. If the candidates for connection are sorted based on the difference in sensor position between them and the connecting node, then many connections with small sensor displacements will exist within the roadmap. These short sensor motions are less likely to be rejected by the path planner, as they are less likely to wander through large areas of the task space, incurring penalties as they go. Instead, the paths will comprise of a greater number of waypoints, with each edge comprising a smaller displacement. This allows for much finer control, and is the preferred method of roadmap construction for the visibility-aware planner. A roadmap with motions that result in smaller displacements of the sensor is desirable. The roadmap construction process should connect each node to its k-nearest neighbours by sensor position rather than configuration distance. This is a relatively minor adjustment with large benefits in the quality of detected paths. 4.9 Summary The static path planner computes paths with a soft occlusion-avoidance con- straint. If the environment if known and static, the precomputed penalties will offer fast online path planning performance. The system is not able to account for changes in the environment without a full recomputation of all penalties. 45 Chapter 5 Dynamic Occlusion-Aware Path Planning 5.1 Overview While the multi-query static visibility roadmap can efficiently compute paths in a static environment, any change in the target position or visual obstacles potentially invalidates all the precomputed visibility penalties. In a dynamic environment, the planner must not rely on precomputed data. For the purposes of this algorithm, a dynamic environment will be de- fined as an environment that changes between queries to the path planner. The environment is assumed to remain static while the path planner is run- ning. For example, in the case of robotic assembly or bin picking, the path planner would rely on data captured on the current state of the workcell, which will remain undisturbed until the robot performs its next action. We assume that a valid collision-avoidance probabilistic roadmap exists at all times. Such a dynamically updated PRM is described in [3], but the details are beyond the scope of this thesis. The static planner is a global approach using Dijkstra’s algorithm, find- ing the path with the lowest visibility penalty between the start and desti- nation nodes. The dynamic planner cannot afford to evaluate the visibility penalty on every edge in the roadmap, so it must instead evaluate candidate paths produced by a much faster (but visibility-unaware) distance-based shortest path planner (which could use Dijkstra’s algorithm or A*, for ex- ample). The dynamic planner can evaluate a visibility penalty on each edge in the candidate path, and use that information to accept or reject the path. If the path is rejected due to an overly high visibility penalty, then one or more of the edges in the path must be marked as unusable, and a new candidate path computed which does not use the marked edge. By succes- sively evaluating candidate paths, the planner will either find a satisfactory path, or it will mark enough edges as impassable that it can no longer find any candidates, in which case it would report failure. This is a constraint- 46 Chapter 5. Dynamic Occlusion-Aware Path Planning satisfaction approach, in contrast to the static planner, which will always return the best path, regardless of how bad that path actually is. The algorithm recomputes the visibility information for each query. It is a single-query system. However, since it must only compute a single path, it is not required to examine every roadmap edge, but rather a subset examined during the search. Though the algorithm may iterate over many candidate paths during a single query, it can cache the visibility penalty information for each roadmap edge, as subsequent candidate paths may contain some of the same edges. The cached information needs to be purged at the end of the query, at which point it is possible that the environment may have changed. Additionally, this approach requires that the visibility boundary be recomputed for each query. The computational cost of generating a new boundary mesh is low compared to the trajectory segmentation tests. The dynamic planner has two major advantages over the static planner: • It can handle changes to the environmental obstacles between queries. • It can handle changes in the target shape or position. The major disadvantage is the computational cost of the query in time and memory, as the search algorithm must run at the time of the query. Its completion time depends heavily on the obstacles and environment config- uration, and thus is not easily predicted. Dynamic obstacles refer to obstacles that change between path planner queries. This could include mobile objects in the workspace, changes in configuration of obstacles such as doors, or the effects of the last robot action. The input to the planner is a mesh representing a snapshot of the obstacles in the environment. This mesh can be based on a priori knowledge, or gathered using sensors; the planner will function in the same manner, assuming that the information embodied by the mesh is accurate. The mesh can contain an arbitrary configuration of triangles, and need not be closed or manifold. However, due to the backface-culling stage of the boundary mesh construction, the mesh must be oriented and all triangles are considered invisible when viewed from the back. Thus, a closed, manifold mesh is desirable for the most predictable results. 5.2 Visibility Computations The occlusion boundary computation, trajectory segmentation, classifica- tion and penalty computations are identical to those found in the static 47 Chapter 5. Dynamic Occlusion-Aware Path Planning planner in Sections 3.5, 4.3, 4.4 and 4.5 respectively. The difference be- tween the dynamic planner and its static counterpart is that the dynamic planner performs all of these computations on an as-needed basis. The planner performs the occlusion boundary computation once per query, to account for changes in the boundary due to changes in the en- vironment and visual target. The triangle mesh representing Lv contains multiple boundaries, each representing the boundary of a volume occluded by one connected component of occluding faces (see Section 3.5). If some environment objects have remained static, then it is possible to re-use the corresponding boundaries from the previous query. If a large portion of the environment does not change between queries, this can improve efficiency greatly. The segmentation, classification and penalty computations require no modifications other than to ensure they receive the up-to-date target and boundary information. The results of those computations are the list of segments, their classifications and their penalties for each examined trajec- tory. This data should be cached during each query, as there is no point in re-computing this information for a given roadmap edge until the start of a new query. It should be discarded if changes occur in the target or environment. 5.3 Iterative Path Search The dynamic planner uses an iterative path search. Its goal is to find a path through the probabilistic roadmap from the start node qstart to the desti- nation node qdest that affords acceptable target visibility. In the case of the dynamic planner, acceptable visibility is defined as never having an contigu- ous occluded section of the entire path that exceeds a pre-set maximum value ψmax. A path ξ is a list of nodes connected by edges in the roadmap that connect the start node to the destination node. The full planner algorithm is given in Algorithm 7. The planner requires a probabilistic roadmap representing feasible collision- safe paths through the workspace. It also needs a lowest-weight path search algorithm such as Dijkstra’s algorithm or the A* algorithm [21] that can pro- duce candidate paths ξ′ between the start and destination nodes provided by the query. This algorithm will be referred to as the candidate planner. The dynamic planner iterates, using the candidate planner to get the path of lowest weight using a set of weights wD(e)∀e ∈ E on the roadmap edges. These weights, marked “D” to denote their use in the dynamic planner are 48 Chapter 5. Dynamic Occlusion-Aware Path Planning Algorithm 7 : The Iterative Dynamic Planner Algorithm 1: wD ← {Length(e)∀e ∈ E} {Initialize weights} 2: bestpenalty ←∞ 3: bestpath← ∅ 4: iterations← 0 5: while iterations < limit do 6: ξ′ ← ShortestPath(qstart,qdest, wD) {get candidate path} 7: if ξ′ = ∅ then 8: return bestpath as FAILURE {all candidates exhausted} 9: end if 10: if ψ(ξ′) < bestpenalty then 11: bestpenalty ← ψ(ξ′) 12: bestpath← ξ′ 13: end if 14: if ψ(ξ′) > ψmax then 15: disabled← 0 16: for all e ∈ ξ′ do 17: if ψ({e}) > ψmax then 18: wD(e)←∞ {disable edges with long occlusions} 19: disabled← disabled+ 1 20: end if 21: end for 22: if disabled = 0 then 23: e∗ ← arg maxe∈ξ′ OcclusionPenalty(e) 24: wD(e∗)←∞ {disable edge with highest penalty} 25: end if 26: else 27: return ξ′ as SUCCESS 28: end if 29: end while 30: return bestpath as FAILURE {too many iterations} 49 Chapter 5. Dynamic Occlusion-Aware Path Planning specific to a particular query, and are initialized at the beginning of each query with the lengths of the corresponding roadmap edges. Length can be defined in the task space or configuration space, depending on which is the preferred metric of distance. The dynamic planner manipulates the weights at each iteration. With each new query the weights need to be reset to their basic length values. For each candidate path produced by the candidate planner, the dy- namic planner determines the edges along the path and performs trajectory segmentation and classification. A practical criterion for acceptance of the path is the length of the longest loss of target visibility. This value, denoted ψ(ξ), is the arc length in the task space of the longest contiguous section of a path which does not afford visibility of the target. This section may span multiple edges in the path. If the longest loss of visibility ψ(ξ′) on a candidate path ξ′ is greater than the user-defined maximum ψmax, then ξ′ not an acceptable path. If the path is rejected, then at least one of the edges in the path must be disabled, removing it from consideration by the candidate planner. If one or more of the edges have contiguous losses of visibility longer than ψmax, then they should be disabled. The planner sets their weight wD(e) to a very high value to prevent the candidate planner from using them. If none of the edges are excluded this way, then the edge with the largest penalty should be disabled. The planner must change the weight on at least one edge, in order to make the candidate planner return a new path during the next iteration. The dynamic planner iterates until it finds an acceptable path, or until so many edges have been disabled that the candidate planner can no longer provide a new candidate path. In a large roadmap the planner may run through many iterations before it determines success or failure. If desired, a limit on the number of iterations can prevent the planner from attempting an excessively long search. The planner keeps track of the path with the lowest maximum visibility loss ψ(ξ) and the associated path. In a failure case, the planner will report the failure and provide the best path encountered up to that point. 5.4 Summary The dynamic occlusion aware planner represents a different method of for- mulating the path penalty reduction process. For a single query, it represents a great improvement in computational cost vs. the static planner. As such 50 Chapter 5. Dynamic Occlusion-Aware Path Planning it will have different applications than the multi-query static planner, pro- viding a single-query complement. However, it has the possibility of failure, and is not guaranteed to find the best path through the roadmap, just an acceptable one. If optimality is not necessary, the dynamic planner repre- sents a low computational cost solution to the occlusion and field of view aware path planning problem. 51 Chapter 6 Experiments The following experiments demonstrate the capabilities of the visibility- aware path planners. In each case, an obstacle occludes the view of a stationary target point from some parts of the robot workcell. The path planner computes a path between two selected nodes in the roadmap. The experiments take into account the camera parameters and offset from the tool center point. Experiment 1 tests the static visibility-aware path planner in simula- tion. Experiment 2 tests the static visibility-aware planner on an actual robot manipulator, collecting video confirming that the path maintains the line of sight. Experiment 3 introduces the field of view constraint in addi- tion to the occlusion avoidance, demonstrating the complete visibility-aware path planner system. Experiment 4 tests the dynamic visibility-aware path planner (with both occlusion and field of view constraints) in simulation. 6.1 Experiment 1 – An automobile assembly scenario simulation Experiment 1 is a basic proof-of concept simulation. The experiment sets up a simulated industrial robot with a realistic problem: observing an object inside a partially-assembled car body. This application was chosen because of the high level of interest in vision guided robotics for assembly operations. The planner demonstrated its ability to plan occlusion-avoiding paths which are both visually and numerically an improvement over paths from a non- visibility-aware roadmap. 6.1.1 Purpose Experiment 1 aims to demonstrate the basic functionality of the static oc- clusion aware path planner by computing paths of low occlusion in a static workcell. This experiment only takes occlusion into account: it does not consider the sensor field of view. Success will be evaluated by comparing 52 Chapter 6. Experiments the occlusion penalty (as computed by the path planner) of the occlusion- avoiding paths vs. the shortest-distance paths for sample path planning queries. The aim is to demonstrate that the occlusion-avoiding paths have a significantly decreased occlusion penalty, indicating that the sensor expe- riences a reduced distance of travel while occluded. 6.1.2 Hypothesis • H0 The visibility-aware paths will not differ in visibility penalty from the shortest path through the roadmap for each query. • H1 The visibility-aware paths will have lower visibility penalties than the shortest path for each query. 6.1.3 Method The test environment was a simulated automotive manufacturing scenario. The target was a single point located inside a car chassis. The window and door holes in the chassis provide visibility of the target for a sensor mounted on an IRB6600 6-DOF industrial manipulator beside the car (see Fig. 6.1). The simulated sensor is located at the robot’s tool center point, and is aligned with the z-axis of the sixth link. This scenario reflects poten- tial real-world manufacturing tasks where an assembly robot must maintain visibility of objects inside the car under assembly. The objects could be visual registration targets, potential construction faults, or safety-critical areas such as human workspaces. Table 6.1: Experiment 1 Parameters |N | 1000 |E| 113,000 k 100 Lv polygon count 302  0.01m merge threshold 0.01 First, the collision-avoidance planner computes a probabilistic roadmap (PRM) of its configuration space. Once finished, the PRM contains only collision-free edges and nodes. Next, the static planner computes occlusion penalties for all edges (motions) in the PRM. This is the longest stage of the precomputation. 53 Chapter 6. Experiments The path-planning queries consist of pairs of nodes selected from within the roadmap. The planner responds to the query by computing a path through the graph from the start node to the destination node, reporting back the list of waypoint nodes that comprise the path as well as the path’s total occlusion penalty. The paths are assessed using two metrics: the oc- clusion penalties wocc, and configuration space lengths wdist. The occlusion penalty indicates how far the sensor travels without target visibility; this will be the primary method of comparison. The configuration-space length is a secondary concern, as it indicates the path’s length and therefore time of interpolation. The planner returns both the visibility-aware path and the shortest path for comparison. robot sensor target environment object Figure 6.1: The robot must maintain visibility of a target inside the car body. In this simulation, the probabilistic roadmap has 1,000 nodes and 113,000 edges. The sampled connectivity of a given node is k =100, meaning that the edges to the 100 nearest neighbours of each node are considered as potential roadmap edges. Since some of the edges may be rejected by the collision detector, the actual connectivity of the roadmap nodes is likely to be less than 100. The precomputation determines the occlusion penalty on each edge. The penalty mixing step combines the occlusion penalties using 54 Chapter 6. Experiments Equation 4.3 with a λ value of 0.99. This ensures that in the case of a zero occlusion penalty, the edge penalty will never be zero because of the small contribution of the path length penalty. Dijkstra’s algorithm computes the minimum-penalty path between all node pairs. This query process returns the actual planned paths, and com- piles timing statistics to determine the query return time. The precomputation yields a set of visibility penalties for the roadmap. Subsequently, the planner responds to queries for paths between selected start and goal nodes. The planner computes the shortest path and the occlusion-avoiding path between the two nodes. In some cases, no zero- occlusion path exists between the start and goal nodes, in which case the occlusion avoiding path still contains some occluded segments. 6.1.4 Results The simulation queried the planner 100 times, which found both the shortest path between the query nodes and the occlusion-avoiding path. A paired t-test determined that the occlusion penalties of the paths computed by the occlusion aware planner were significantly less than those computed by the shortest-path planner. The mean penalty difference (mean = 1.134, sd = 1.406, n = 100) was significantly greater than zero; lower penalties were from the visibility-aware paths. The T statistic was t(99) = 8.071, greater than the two-tail Tcrit = 1.660, indicating significance. The probability that the samples came from the same distribution (H0) is p = 8.44×10−13. This evidence indicates that the visibility-aware planner is effective in reducing the overall distance travelled by the sensor while the target is occluded. The 95% confidence interval was 0.279, indicating that the occlusion-avoiding path has a penalty that is between 0.855 and 1.413 lower than the shortest path’s penalty with 95% probability. As expected, the occlusion-avoiding paths were more convoluted than the shortest paths (See Fig. 6.2). The edges present in the roadmap constrain the actual shape of the path. Since the roadmap exists in the configuration space, two nodes that appear to be neighbouring in the task space may not be connected by an edge, as they may be quite distant in configuration space. A rendering of the view from the robot’s end effector illustrates the difference between the two paths (See Figs. 6.3,6.4). It is important to note that these images represent the view from a camera at the end-effector, but oriented to point towards the target regardless of the end-effector’s actual orientation. This is due to the fact that field of view is not considered in this simulation. This is a constraint that will be addressed in later experiments. 55 Chapter 6. Experiments Table 6.2: Experiment 1 Visibility Penalty t-test n = 100 Occlusion Avoiding Shortest Path Length Mean 8.282 4.397 Length S.D. 3.816 1.555 Penalty Mean 0.842 1.976 Penalty S.D. 0.740 1.547 Difference Mean 1.134 Difference S.D. 1.406 T (99) 8.071 Tcrit 1.660 p 8.44× 10−13 95% C.I. 0.279 (a) Shortest Path (b) Occlusion-Avoiding Path target roadmap nodes occlusion 1 8 1 8 4 3 6 obstacle Figure 6.2: The path computed from start point S to destination point D. (a) the shortest path. (b) the occlusion-avoiding path. 6.1.5 Discussion The initial results are promising. The t-test shows that the planner reduces the overall occluded distance travelled by the sensor. However, the occlu- sions are not eliminated entirely in many cases. This is entirely dependent on the shape of the occluders and the position of the targets. In the case of the targets inside car body, the window and door openings create “cells” of unoccluded space, which are only connected inside the car body. Thus, unless the robot travels inside the body, it is forced to pass through and occluded region at some point if it moves from viewing the target through one window to another. Additionally, the maximum-visibility paths are constrained by the avail- 56 Chapter 6. Experiments Figure 6.3: A sequence of views from the shortest path. Note that in frames 3-6 the target (black ball) is not visible due to occlusion by the car roof. Figure 6.4: A sequence of views from the occlusion-avoiding path. Note that the target (black ball) is always visible. able paths within the roadmap. This is a subtle but very significant con- straint. The initial roadmap construction process attempts to connect each node to its k-nearest neighbours in configuration space. However, nearness in configuration space does not in any way guarantee nearness of the sen- sor position and pose in the task space. Thus, any given node may not be connected to the intuitively next best node in the task space. Ideally, the roadmap should have a very high connectivity, and/or the roadmap should be connected using a task-space pose similarity metric for selecting nearest neighbours. This would increase the likelihood of existence of suitable paths within the roadmap, giving the planner more options from which to choose. Otherwise, the system performs as expected. The quality of the path 57 Chapter 6. Experiments depends on a high-quality path existing both in the environment and the roadmap. Best performance will depend on constructing the roadmap in such a way that it contains good paths, and then applying the planner to find those paths. Experiments in the following sections feature improved roadmaps to test the effects of roadmap improvements. 6.2 Experiment 2 – Observing a target through gaps in a wall using a physical robot setup 6.2.1 Purpose The following experiment tested the planner on a 6-DOF CRS A465 robot manipulator located in the Collaborative Advanced Robotics and Intelligent Systems Laboratory a the University of British Columbia. The aim, as in the previous simulation, is to verify that the visibility-aware planner can determine occlusion-free paths within a probabilistic roadmap. Performing experiments on real robots also serves to determine the influence of practical concerns on the performance of the planner. 6.2.2 Hypothesis • H0 The distance travelled by the camera in which the target is occluded will not differ significantly between the shortest and visibility-aware paths for a given query. • H1 The distance travelled by the camera in which the target is occluded will be lower on visibility-aware paths than on the shortest paths for a given query. 6.2.3 Method The experiment tested the ability of the path planner to create occlusion- free paths for a CRS A465 6-DOF robot manipulator. The robot is equipped with a video camera mounted on the end-effector, which it uses to observe a stationary target. Figure 6.5 shows a schematic of the experimental setup. A wall with a cut-out region partially occludes the robot’s view of the target. Figure 6.6 is a photograph of the physical experimental setup. The planner first computes a static probabilistic roadmap of the workspace, using an a priori model of the obstacles. The roadmap nodes are constrained to configurations that place the target in the sensor’s field of view. Note 58 Chapter 6. Experiments CRS A465 occluder target camera Figure 6.5: The robot must maintain visibility of a target behind the cut-out wall. that this constraint only affects the nodes, and not the edges of the roadmap. Due to the multiple revolute joints in the manipulator, even while travelling between two nodes that satisfy the field of view constraint, an interpolated motion may allow the target out of the field of view. The planner then computes static occlusion-only penalties for all edges in the roadmap with respect to the target and the occluding wall. Since the sensor does not al- ways point at the target, success is determined by observing the robot’s end effector (and the sensor) from the target position with a manually-oriented camera. The planner answered queries using Dijkstra’s algorithm, providing two paths between the start and goal nodes. The first path was the maximum visibility path, the result of the occlusion-avoiding planner. The second path was the shortest path, using the standard probabilistic roadmap approach, of weighting each edge by its length in the configuration space. The purpose of the planner is to compute paths that avoid occlusions to the line of sight. However, the field-of-view constraint must also be satisfied in order to collect video under real-world conditions. In a typical probabilis- tic roadmap, only a small fraction of the nodes correspond to configurations that actually point the camera at the target within the tolerance of the cam- 59 Chapter 6. Experiments target occluder CRS A465 camera Figure 6.6: Experiment 2 apparatus. Table 6.3: Experiment 2 Parameters |N | 1000 |E| 590,000 k 800 Lv polygon count 120  0.01m merge threshold 0.01 era’s field of view. Even fewer of the edges keep the target in the field of view for their entire length. To mitigate this problem, all the nodes in the roadmap are pre-screened to ensure that only nodes that satisfy the field-of- view constraint are accepted. While this greatly reduces the FOV problems, there are still many interpolations (edges) that violate the FOV constraint. The previous experiment ignored this problem. In order to verify the line of sight, a second camera was placed at the target position. Due to the reciprocal nature of the line of sight, the path planner can be verified by checking if the robot’s sensor is always unoccluded in the target’s view. 60 Chapter 6. Experiments 6.2.4 Results The experiments displayed the desired behaviour: the robot moved while maintaining the line of sight. Figure 6.7 shows a series of views of the robot from the target position. Note that the robot’s sensor lens was always visible to the target. The robot avoided collisions and occlusions by the wall. The camera at the target position was hand-oriented to keep the robot’s end-effector in-frame. The camera on board the robot, however, lost sight the target due to the boundaries of its field of view at several points along the motion. Regardless of the camera orientation, the line of sight was maintained, so the experiment confirmed the effectiveness of the visibility-aware planner. A paired t-test determined that the mean difference in the occlusion penalty between the shortest path and the path computed by the occlusion aware planner. The mean penalty difference (mean = 1.134, sd = 1.406, n = 100) was significantly greater than zero, the lower penalties being from the visibility-aware paths. The T statistic was t(99) = 8.0713, greater than the two-tail Tcrit = 1.984, two-tail p = 1.69x10−12. This evidence indicates that the visibility-aware planner is effective in reducing the overall distance trav- elled by the sensor while the target is occluded. A 95% confidence interval about the penalty difference is 0.279, which indicates that the visibility- aware planner reduces the occluded distance travelled by between 43 and 72% with 95% confidence. Table 6.4: Experiment 2 Visibility Penalty t-test n = 100 Occlusion Avoiding Shortest Path Difference Mean 1.134 T (99) 8.0713 Tcrit 1.984 p 1.69× 10−12 95% C.I. 0.279 6.2.5 Discussion The experiment, while successful, highlights the limiting factor of the sys- tem: the roadmap density. A human observer can intuitively generate an appropriate path, but the 1000-node, 590000-edge roadmap frequently did not contain the necessary motions or connections to execute that motion. As 61 Chapter 6. Experiments 1 5 2 6 3 7 4 8 Figure 6.7: A sequence of views from the target position. Note that the sensor on the robot is always visible. a result, the planner frequently could not find an occlusion free path within the roadmap, despite the existence of such paths within the configuration space. The importance of a well-connected roadmap is very clear. Future experiments should use roadmaps that prioritize connections between nodes that are neighbouring in task space, not joint space, and are neighbouring in both sensor position and orientation, to avoid excessive rotations between waypoints, which frequently cause the sensor to radically change orientation. Despite these issues, the experiment demonstrated occlusion avoidance. The planner would work well in the case of a robot with an omnidirectional sensor or a sensor whose orientation is decoupled from the robot’s configura- tion, such as a pan-tilt-unit equipped sensor. The FOV-aware option in the planner could mitigate the losses of visibility due to the sensor orientation if the roadmap is sufficiently dense. If the roadmap is not dense enough, then the two constraints (occlusion and field of view) may penalize so many of the edges that even the lowest-penalty path does not afford decent visibility. 6.3 Experiment 3 – Observing a target through a narrow channel Experiment 3 is a follow-up to Experiment 2, incorporating a more difficult task with a more complex occluder, but also using a roadmap with much better range of possible motions for the robot. Again a physical robot setup is used. 62 Chapter 6. Experiments 6.3.1 Purpose This experiment continues the testing of the visibility-aware path planner, demonstrating fully integrated occlusion and field of view constraints under adverse conditions. The visible region of the task space is small and is comprised of narrow channels. The aim is to determine if the path planner can accommodate such tight constraints. 6.3.2 Hypothesis • H0 The occlusion aware paths will not differ in occlusion penalty from the shortest path through the roadmap for each query. • H1 The visibility-aware paths will have lower occlusion penalties that the shortest path for each query. 6.3.3 Method The experimental setup is similar to the previous experiment. In this case, the occluder is a 70x70cm planar panel with a cut-out section. The cut-out is in the shape of a squared-off “S” figure, with channels 15cm wide in the outer areas and 10cm wide in the central channel. Figure 6.8 shows the robot, occluder, target and obstacles. target occluder 1 lines of sight CRS A465 obstacle 23 4 5 6 7 8 CRS A465 occluder camera obstacle z y x x z y (a) Experiment 3 Setup (b) Experiment 3 Physical Apparatus Figure 6.8: Experiment 3 setup. 63 Chapter 6. Experiments The occluder is mounted on a frame, suspending it above the robot. Figure 6.9 shows the occluder and target in detail as seen from below. The target is mounted above the frame, 1.8 m directly above the robot’s origin, so that robot’s sensor must look up through the occluder to see the target. This configuration was chosen because the robot has the greatest range of motion when the end-effector (and by extension the sensor) is pointing upwards. This configuration reduces the likelihood of reaching the robot’s joint limits when traversing the workspace. The frame is included in the probabilistic roadmap construction step for collision avoidance, but is not included in the occlusion avoidance step, and is considered transparent to the sensor to maintain the simplicity of the demonstration. Figure 6.9: Experiment 3 occluding panel and target (as seen from below). On the +Y side of the workspace there is an obstacle: a fixed camera mounting bracket, visible in Figure 6.8 (marked “obstacle”). It is repre- sented by a vertical rectangular pillar in the collision model, and thus the robot cannot extend completely in the +Y direction. The workspace and the robot’s range of motion are otherwise symmetric across the XZ plane. The probabilistic roadmap itself contains collision-free configurations 64 Chapter 6. Experiments Table 6.5: Experiment 3 Parameters |N | 5000 |E| 208176 k 20 Lv polygon count 144  0.01m merge threshold 0.01 total penalty computation time 26.1h mean penalty computation time 0.45s mean query time 0.6s and motions, and only nodes that satisfy the field of view constraint are present. The edges may violate the field of view constraint, and the planner takes this into account, computing the visibility penalties with respect to both occlusion and loss of the target from the field of view. Additionally, in this experiment the roadmap is connected using a task- space nearest-neighbour metric. This ensures that nodes with high sensor proximity will have the highest priority for connection. A high node count (5000) provides the planner with a wealth of different sensor positions, and the task-space nearest neighbour metric increases the likelihood that nodes will be connected to other nodes that place the sensor nearby. Such a setup does not require as high a connectivity as a configuration-space based con- nection scheme, and so the connectivity parameter k is only 20, meaning that the PRM generator only attempts to connect each node to its 20 nearest neighbours. A preference for quality over quantity in roadmap connectivity reduces computation time and increases the quality of the final paths. 6.3.4 Results The experiment displayed visibility-aware paths, moving the robot’s end- effector in an “S” pattern to keep the line of sight in the cut-away sections of the occluder. A sequence of views from the onboard camera is shown in Figure 6.10. The planner is not perfect: occlusions of the target occur at point 4 and just after point 8. At point 4, the robot was unable to extend far enough in the +Y direction to maintain the line of sight due to a fixture in the workspace. The fixture, marked “obstacle” in Figure 6.8 was a fixed camera mounting bracket mounted vertically beside the robot. The probabilistic roadmap did not contain any nodes or edges that brought the arm into contact with the fixture, preventing motions that brought the 65 Chapter 6. Experiments sensor into the far +Y extents of its reach. As a comparison, at points 6-7, the robot negotiated a corner in the occluder of the exact same dimensions. However, since there was no fixture on the -Y side, the robot was able to reach its full extent, allowing the camera to maintain line of sight with the target. The experiment also tested the planner using 100 randomly-generated queries. In each case the planner computed the shortest path and the path of best visibility between the selected start and destination nodes. The mean penalty difference for the 100 queries was 3.387 m less for the occlusion and field of view aware paths. This was a significant reduction, with a T-statistic of t(99) = 14.267, greater than the critical value tcrit = 1.984. The likelihood that the penalties form the shortest and visibility-aware paths came from the same distribution was p = 9.356× 10−26. Table 6.6: Experiment 3 Visibility Penalty t-test Visibility Penalty t-test n = 100 Occlusion Avoiding Shortest Path Penalty Mean 0.713 4.100 Penalty S.D. 1.024 2.562 Difference Mean 3.387 T (99) 14.267 Tcrit 1.984 p 9.356× 10−26 95% C.I. 0.471 6.3.5 Discussion The experiment successfully showed a fully integrated test of the system, computing a path that satisfies the two constraints over most of its length. The visibility-aware displays a significant improvement over shortest-path planning in terms of target visibility. The precomputation has a high com- putational cost, with 26.1 hours of computation. However, a great deal of the experimental software is unoptimised. The actual planning queries are relatively fast, with sub-second computation time. In a static environment this system presents an effective solution for maintaining target visibility. On a more subjective level, the combination of occlusion avoidance and keeping the sensor pointed at the target causes the robot to display “lifelike” behaviour. The location of the target is apparent to an outside observer 66 Chapter 6. Experiments 1 5 2 6 3 7 4 8 Figure 6.10: A sequence of frames from the arm-mounted camera. The occluder has been darkened (in red) for clarity. even if the target is not actually visible, as the robot’s sensor is moving while pointing at a fixed location in space. Being able to follow the robot’s gaze gives the impression of attention in the robot’s behaviour. 6.4 Experiment 4 – Dynamic planning in a changing environment Experiment 4 is a simulation designed to test the dynamic visibility-aware path planner. The dynamic planner can accommodate changes in the envi- ronment between queries, so it is tested using an environment model that is supplied to the planner at the time of the query. The planner uses an iterative search, and unlike the static planner, is attempting a constraint- satisfaction problem, accepting the first path through the PRM that satisfies a visibility constraint. It is possible for the dynamic planner to fail to find a path that satisfies the chosen constraint. In contrast, the static planner is attempting to optimize the visibility penalty within the PRM, and will always return the lowest-penalty path if any path exists. 6.4.1 Purpose This experiment will determine the ability of the dynamic visibility-aware path planner to find paths that are free of long occluded sections in a PRM without any precomputation. Success is defined as the ability to plan paths quickly and with a high rate of success when success is possible. If success 67 Chapter 6. Experiments is not possible, the planner should fail gracefully. 6.4.2 Hypothesis • H0 When the planner is successful the visibility-aware path will not differ in visibility penalty from the shortest path through the roadmap. • H1 When the planner is successful the visibility-aware path will have a lower visibility penalty than the shortest path through the roadmap. 6.4.3 Method The experimental setup is similar to Experiment 3: the robot is situated underneath a frame holding the occluding objects. The occluders are planar rectangular panels of varying sizes affixed to the frame rails above the robot. The panels are parallel to the floor and are 20 cm beyond the reach of the robot. The target is located above the robot and occluders, and again the robot must look up past the occluders to see the target. The target is a single point. The path planner is queried twice, once with the occluders in their initial positions, and then again after two of the occluders have moved (See Fig. 6.11). Since the dynamic planner does not rely on precomputed data, the change in occluder position will not invalidate the planner. So long as the planner receives an updated model of the occluders’ positions, it will be able to function under the new conditions. This demonstrates the planner’s robustness to known changes that occur between queries. The frame rails are considered to be transparent to the sensor for sim- plicity. This setup was chosen for ease of reconfiguration to test the ability of the planner to accommodate periodic updates in the occluder positions. Once again, there is an obstacle on the +Y side of the workspace, and the robot must avoid collisions with it. The probabilistic roadmap used in this experiment is exactly identical to the one used in Experiment 3. Once again it prevents the robot from colliding with the floor, frame rails and posts, and the obstacle fixture. It has 5000 nodes, all of which satisfy the field of view constraint. The roadmap connectivity parameter k is 20. The roadmap is connected using a task-space sensor position based metric for nearest neighbours. The dynamic planner performs an iterative test-and-reject search for suitable paths in the roadmap. The distance-based path search that deter- mines the candidates functions within the PRM so all candidate paths that the planner evaluates are collision-free and feasible. The planner rejects a 68 Chapter 6. Experiments target occluder CRS A645 sensor x z y x z y a b Figure 6.11: The setup of Experiment 4. path if it contains a contiguous loss of visibility that is longer than a pre- set threshold. In this experiment, that threshold is 5 cm, representing the maximum acceptable loss of visibility, from which a vision system observing the target can be expected to recover. Three trials were conducted, the first with 50 randomly-selected queries and the second and third with 100. A query called upon the path planner to find a path through the PRM between two nodes. The nodes were selected at random from the PRM nodes that satisfied both the field of view constraint (which they all do) and the occlusion constraint, due to the fact that the path planner in increasingly likely to fail outright if either the start or destination node are occluded, as this creates an unavoidable visibility penalty for any path. A query to the planner can result in two outcomes: first, the planner may find a suitable path (possibly after many iterations) and will report it and its penalty. Secondly, the planner may fail if no path exists without a contiguous loss of visibility less than the threshold. In the second case the planner reports the best (lowest penalty) path encountered during the iterative search, which is considered a failure. Since the computation time is directly related to the number of itera- tions, for practical reasons there is an upper limit on the number of allowed iterations before the planner must cease iterations. In that case the planner returns the path of least penalty encountered during the iterations, which may or may not satisfy the constraints. The first set of trials had an it- 69 Chapter 6. Experiments eration limit of 25. The second and third trials had an iteration limit of 50. Trials 2 and 3 test the planner operating on the same roadmap but with different occluder configurations (shown in Figure 6.11). The three rectan- gular panels are moved to different positions in the two trials, demonstrating the ability of the planner to function even if the visibility conditions change. The planner can handle changes at any time between queries. The two trials perform 100 queries each for the purposes of creating a sufficient statistical sample size. 6.4.4 Results In each trial, the planner attempted to find paths in respond to queries. Each query can resulted in one of three outcomes: either the planner was successful and returned a path (success), it reached the iteration limit and returned the best path it found up to that point (failure), or the candidate planner was unable to find any more candidates, and the planner returned the shortest path (failure). Different trials had different proportions of each outcome. Trial 1 Trial 1 consisted of 50 randomly selected queries. Out of those 50, 13 success- fully found a path with a maximum occlusion length less than 0.05 m. Table 6.7 shows the results of a t-test on the visibility penalties of the successful queries. Of the failed queries, 21 failed due to reaching the 25-iteration limit, returning the best path encountered during those 25 iterations. The remaining 16 failed when no available path satisfied the constraints. In this case the planner returned the shortest path. Trial 2 extends the iteration limit to 50 to gain a better understanding of the need for very large numbers of iterations. The successful queries had an mean computation time of 41.96 s and the mean number of iterations was 7.92. The longest successful computation required 22 iterations in 115.75 s, and the shortest was 1 iteration in 5.83 s. In the latter case the shortest path (iteration 1) satisfied the constraints. Unsuccessful computations which reached the 25 iteration limit did so in approximately 130 s, the longest taking 133.73 s. The actual time to evaluate a single iteration depends on the number of edges in the candidate path, which in turn depends on the size and connectivity of the roadmap. The maximum number of edges in a path in trial 1 was 4. 70 Chapter 6. Experiments The successful queries all resulted in lower visibility penalties on the dynamic planner’s path than the shortest path through the roadmap except in one case where the paths were the same and thus both penalties were zero. The algorithm guarantees that the penalty will not increase, as it evaluates the shortest path as its first candidate, so any subsequent candidates must have better penalties than the shortest path in order to be returned. The reduction in penalty was significant according to the t-test on the penalties in the successful queries. In 9 out of 13 cases the dynamic planner found a path with 0 visibility penalty. The T-statistic was 4.891, exceeding the Tcrit of 1.782. Additionally, the successful queries by definition produce paths wherein all contiguous losses of visibility are less than the present threshold of 0.05 m. The dynamic planner recomputes the occlusion boundary at each query. The mean computation time to compute the boundary was 0.0007 s. The boundary had 36 polygons, and the mesh representing the occluders had 72 polygons. Table 6.7: Experiment 4 Trial 1 Visibility Penalty t-test Visibility Penalty t-test n = 13 Dynamic Planner Shortest Path Penalty Mean 0.0178 0.213 Penalty S.D. 0.001 0.022 Difference Mean 0.195 Difference S.D. 0.144 T (12) 4.891 Tcrit 1.782 p 1.857× 10−4 95% C.I. 0.087 Trial 2 Trial 2 repeated the process in Trial 1, but with an increased iteration limit of 50. Out of 100 queries, 46 were successful, 37 failed due to reaching the iteration limit, and the remaining 17 failed due to the absence of a suitable path. Table 6.8 shows the results of a t-test comparing the visibility-aware planner’s paths to the shortest paths in the 46 successful queries. The mean computation time for successful queries was 23.1 s with a mean 3.7 iterations. The longest successful query took 224.8 s for 37 iterations. 71 Chapter 6. Experiments The queries that hit the 50-iteration limit did so in approximately 300 s. Table 6.8: Experiment 4 Trial 2 Visibility Penalty t-test Visibility Penalty t-test n = 46 Dynamic Planner Shortest Path Penalty Mean 0.014 0.134 Penalty S.D. 0.028 0.122 Difference Mean 0.119 Difference S.D. 0.122 T (46) 6.584 Tcrit 1.679 p 2.11× 10−8 95% C.I. 0.036 Trial 3 Trial 3 performed an additional 100 queries with a 50-iteration limit, with a different configuration of the occluding panels. Since the planner does not rely on any visibility data stored between queries, the updates to the occluder model are taken into account in all subsequent queries. The oc- cluders’ new positions place them further from the center of the workspace, so they occlude the target’s view in far fewer of the robot configurations. Thus, the path planner was successful in all 100 queries, frequently finding that the shortest path had a low or zero visibility penalty. Table 6.9 shows the results of a t-test comparing the visibility-aware planner’s paths to the shortest paths in the 100 (all successful) queries. The shortest paths had zero visibility penalties in all but 14 of the 100 queries. The dynamic planner found paths with zero visibility penalties in all but 1 of the 100 queries. The mean query computation time was 9.54 s, and the mean number of iterations was 1.75. Figure 6.12 shows two paths between the same start and destination points. The images represent a top-down view of the workspace. The view- point is above the apparatus in the target location, facing the in -Z direction (compare Figs. 6.11 and 6.12). The robot’s range of motion (ghosted circle) is represented by the PRM nodes (grey dots). The rectangular occluders are show in red, numbered 1-3 and are above the robot. The sensor path is shown as a black curve in each case. Image A shows the path computed by the dynamic planner with the occluders in their original configuration 72 Chapter 6. Experiments Table 6.9: Experiment 4 Trial 3 Visibility Penalty t-test Visibility Penalty t-test n = 100 Dynamic Planner Shortest Path Penalty Mean 4.7× 10−4 0.041 Penalty S.D. 4.7× 10−3 0.014 Difference Mean 0.041 Difference S.D. 0.116 T (100) 3.514 Tcrit 1.660 p 3.33× 10−4 95% C.I. 0.023 (used in trials 1 and 2) it has a small occlusion behind panel 1, but not enough to put it over the rejection threshold. Image B shows the path com- puted by the dynamic planner after the occluders have moved. The first path avoids the center of the robot’s range of motion, as the occluders block visibility from that area. Once the occluders move towards the outsides of the workspace, the center area affords better visibility, so the path in image B passes through the center of the workspace. Since the FOV constraint requires the end-effector to be pointing upwards at all times, the paths may require considerable rotations of the robot’s lower joints to maintain that orientation, particularly if the end-effector is directly above the robot’s base. This produces the pronounced curves seen the paths, particularly in Figure 6.12b. 6.4.5 Discussion The experiments successfully demonstrated the efficacy of the dynamic plan- ner. While lacking the static planner’s assurance of finding a path, the dynamic planner can nonetheless find paths in most instances without the associated precomputation. Its applicability to changing environments will also increase the number of problems it can solve. Many factors affect the planner’s ability to successfully find paths. Obvi- ously, a path that satisfies all the constraints must exist within the roadmap, so a dense, highly-connected roadmap will yield better results. The more stringent the penalty limit, the less likely a path will be found. Finally, the lower the iteration limit, the fewer attempts the planner has to find a suit- able path. All of these factors can be alleviated with a very dense roadmap 73 Chapter 6. Experiments 1 2 3 1 2 3 x y x ystart dest. start dest. a b Figure 6.12: Image (a) shows the path found by the planner in the original environment configuration. The workspace is shown in a top-down perspec- tive, as though from the target’s point of view. Image (b) shows the path found by the planner between the same start and end points after occluders 1 and 3 have moved. and a very high iteration limit, but this will have a higher computational cost. The penalty limit has the most practical interpretation, and is deter- mined by the tolerances of the sensor. The iteration limit is dictated by the time available to the planner before a response is demanded. 6.5 Summary The experiments demonstrated the ability of the proposed path planners to solve the path planning for visibility problem as stated in Section 1.3. The overall effectiveness of the planner will depend on the existence of high- visibility paths in the workspace. The experiments show that the planner can find those paths when they exist, even in a configuration space that has a complex relationship with the sensor position in the task space. The planner can simultaneously accommodate both the field of view and occlu- sion constraints, as well as maintaining the high priority of the collision avoidance constraint. 74 Chapter 7 Summary The experiments demonstrate the effectiveness of the path planner, find- ing paths of improved target visibility within the robot’s workspace. The improvements in target visibility were verified statistically. 7.1 Contributions The research objectives stated in Section 1.4 were fulfilled. Specifically, the first objective was to create an algorithm which assesses a sensor motion, segmenting it into sections that have line of sight to a target, and those that do not. The segmentation and classification algorithms (Sections 4.3 & 4.4) fulfill the first requirement, dividing a parameterised motion into those segments that afford target visibility and those that do not. The second research objective was to create a framework which applies the line of sight-based motion segmentation to motions (edges) within a well-known and accepted path planning scheme, namely the probabilistic roadmap. Using the motion segmentation from the first requirement, the penalty computation detailed in Section 4.5 provides numerical values that can be used to weight the edges of a probabilistic roadmap graph. With weights on the roadmap edges, path search algorithms such as Dijkstra’s algorithm provide a weighted path search within the graph. The third research objective was to enhance the existing single- and multi-query PRM path planners to take the visibility information into ac- count. The static visibility-aware path planner provides a multi-query so- lution for static environments. At the expense of a long precomputation, the static planner can answer multiple path planning queries in an unchang- ing environment. The planner can find the path through the PRM graph with the lowest visibility penalty, indicating that the sensor spends moves the shortest possible distance while unable to see the target. The dynamic visibility-aware planner performs single-query path search for a path with contiguous losses of visibility that are less than a specified length. The dy- namic planner does not provide the same guarantees of minimal penalty, nor is it guaranteed to find a path that meets the criteria, but it does not 75 Chapter 7. Summary require the lengthy precomputation of the static planner, and thus is suit- able to changing environments and target positions. Together, these two planners fulfill the third research objective. 7.2 Future Work The visibility-aware planners would benefit from performance enhancements, particularly in the trajectory segmentation step. The limiting factor is the speed of the mesh-point distance computation, which occurs at each step in the adaptive bisection process (Section 4.3). One well-suited technique is the Adaptively Sampled Distance Field (ASDF) technique [22]. The distance field would provide an alternative representation of the visibility boundary; a representation that is well-suited to efficiently responding to distance-to- surface queries. Not only would an acceleration structure improve perfor- mance, but successive samples are in similar regions of the task space. This property could be harnessed to further improve performance, as successive queries to an acceleration structure can be bootstrapped if they are highly similar. Another area of potential performance improvement is the displacement estimate for adaptive bisection. In the current implementation, the estimate uses a worst-case scenario where the robot arm is fully extended. If the esti- mate were to consider the actual robot pose in the current motion segment, it could give a more accurate bound on the possible displacement. However, this estimate would have to be computed at each sampled segment, rather than once for the entire trajectory, so the performance tradeoff may or may not prove effective. The system does not currently take into account occlusions of the line of sight by the robot’s body. This would require adding the model of the robot’s body to the environment model, taking into account the robot’s configuration at each point in the motion. This is possible in both the static and dynamic cases, as the robot’s motion due to the roadmap edge is known in advance. This would eliminate the possibility of self-occlusion of the line of sight by the robot itself. The tasks displayed in the experiments are centered on stationary multi- link manipulators, but the probabilistic roadmap can be applied to many types of robot path planning problems [2]. Since it is based on the PRM, the visibility-aware planner generalises in the same way, provided it is possible to segment each motion with respect to the visibility boundary and field of view. 76 Chapter 7. Summary At its core, the visibility-aware path planner is a system that evaluates how a single point on a robot travels with respect to a well-known bound- ary in the task space. The actual meaning of the boundary is application- specific: here it refers to visibility, but it could be a safety boundary, a region of interest, a danger zone, or a region of aesthetic preference. This work has developed a method of partitioning space into desirable and unde- sirable regions, and created a method of planning paths of high desirability. Using these techniques, an assistive robot could avoid approaching too close to a user; a stealthy robot could avoid the fields of view of observers, and a tele-operated robot could try to remain within the view of its operator. All of these applications use the same underlying logic: finding the best path through a partitioned and labelled task space. 7.3 Conclusion This thesis demonstrates the visibility-aware probabilistic roadmap path planner. This path planner allows a robot to compute the motions it must make to reach a goal while observing a target. The planner has two vari- ants: a static planner for multiple queries in an unchanging environment, and a dynamic variant for single queries in a changing environment. The planners were validated in a series of four experiments. The experimen- tal results showed that the visibility-aware planners produced statistically significant reductions in the distance travelled by a robot’s onboard sensor without target visibility as opposed to typical shortest-path planners. This path planner is one more step towards robots that can autonomously and intelligently plan actions to sense and interact with their environment. 77 Bibliography [1] Fabian Schwarzer, Mitul Saha, and Jean-Claude Latombe. Adaptive dynamic collision checking for single and multiple articulated robots in complex environments. IEEE Transaction on Robotics and Automation, 21(3):338–353, 2005. [2] Lydia E. Kavraki, P. Svestka, J. Latombe, and M. H. Overmars. Prob- abilistic roadmaps for path planning in high–dimensional configuration spaces. IEEE Transactions on Robotics and Automation, 12:566–580, 1996. [3] L. Jaillet and T. Simeon. A prm-based motion planner for dynam- ically changing environments. Intelligent Robots and Systems, 2004. (IROS 2004). Proceedings. 2004 IEEE/RSJ International Conference on, 2:1606–1611 vol.2, 28 Sept.-2 Oct. 2004. [4] Fredo Durand. A multidisciplinary survey of visibility. In ACM SIG- GRAPH Course Notes. Visibility, problems, techniques and applica- tions., pages 1–146, 2000. [5] Yu Yong and K. Gupta. Sensor-based roadmaps for motion planning for articulated robots in unknown environments: some experiments with an eye-in-hand system. In IEEE/RSJ Int’l Conference on Intelligent Robots and Systems, volume 3, pages 1707–1714, 1996. [6] T. Siméon, J. P. Laumond, and C. Nissoux. Visibility-based proba- bilistic roadmaps for motion planning. Advanced Robotics, 14:477–493, 2000. [7] S. Popinet. The GNU Triangulated Surface Library. Available at http://gts.sourceforge.net. [8] J. J. Kuffner Jr. and S. M. LaValle. Rrt-connect: An efficient approach to single-query path planning. pages 995–1002, 2000. 78 Bibliography [9] Gildardo Sánchez and Jean-Claude Latombe. A Single-Query Bi- Directional Probabilistic Roadmap Planner with Lazy Collision Check- ing. Robotics Research: The Tenth Int’l Symposium, page 408, 2003. [10] Konstantinos Tarabanis, Roger Y. Tsai, and P. K. Allen. A survey of sensor planning in computer vision. IEEE Transactions on Robotics and Automation, 11:86–104, 1995. [11] Konstantinos Tarabanis, Roger Y. Tsai, and S. Abrams. Planning viewpoints that simultaneously satisfy several feature detectability con- straints for robotic vision. In Proc. ICAR, Fifth Int’l Conference on Advanced Robotics, volume 2, pages 1410–1415, 1991. [12] Konstantinos Tarabanis, Roger Y. Tsai, and Anil Kaul. Computing Occlusion–Free Viewpoints. IEEE Transactions on Pattern Analysis and Machine Intelligence, 18:279–292, 1996. [13] Konstantinos Tarabanis, Roger Y. Tsai, and P. K. Allen. The mvp sensor planning system for robotic vision tasks. IEEE Transactions on Robotics and Automation, 11:72–85, 1995. [14] E. Trucco, M. Umasuthan, A. M. Wallace, and V. Roberto. Model- based planning of optimal sensor placements for inspection. IEEE Transactions on RObotics and Automation, 13:182–194, 1997. [15] R. Murrieta, A. Sarmiento, S. Bhattacharya, and S.A. Hutchinson. Maintaining visibility of a moving target at a fixed distance: the case of observer bounded speed. Robotics and Automation, 2004. Proceedings. ICRA ’04. 2004 IEEE International Conference on, 1:479–484 Vol.1, April-1 May 2004. [16] Mohamed S. Marzouqi and Ray A. Jarvis. New visiblity-based path- planning approach for covert robotic navigation. Robotica, 24:759–773, 2006. [17] S. Léonard, E.A. Croft, and J.J. Little. Dynamic visibility checking for vision-based motion planning. Robotics and Automation, 2008. ICRA 2008. IEEE International Conference on, pages 2283–2288, May 2008. [18] Eric Haines. Point in Polygon Strategies: Graphics Gems IV, pages 24–46. Academic Press, 1994. [19] Sang C. Park. Triangular mesh intersection. Vis. Comput., 20(7):448– 456, 2004. 79 Bibliography [20] Tamas Juhasz. Graphics acceleration techniques for a mobile robot simulator. In Proceedings of CESCG03: Central European Seminar on Computer Graphics, volume 5, pages 22–24, 2003. [21] Thomas H. Cormen, Charles E. Leiserson, Ronald L. Rivest, and Clif- ford Stein. Introduction to Algorithms 1st ed. MIT Press, 1997. [22] Sarah F. Frisken, Ronald N. Perry, Alyn P. Rockwood, and Thouis R. Jones. Adaptively sampled distance fields: a general representa- tion of shape for computer graphics. In SIGGRAPH ’00: Proceed- ings of the 27th annual conference on Computer graphics and inter- active techniques, pages 249–254, New York, NY, USA, 2000. ACM Press/Addison-Wesley Publishing Co. 80 Index ψ(ξ), 50 ψmax, 48 ξ, 48 ξ′, 48 2-norm, 29 adaptive bisection, 29, 36, 42 adaptive subdivision, 12, 29, 30 algorithm boundary-based occlusion, 14 classification, 8 complexity, 37 Dijkstra, 7, 28, 33, 42, 46, 55 frontfaces, 24 occlusion boundary, 22 segmentation, 7, 8 trajectory segmentation, 36 trajectory segmentation (field of view variant), 43 visibility-aware path planner, 33 algorithm complexity, 44 automated assembly, 52 back faces, 23 backface culling, 23 bin-picking, 4 binary search, 35 bisection, 29 boundary occlusion, 8 Boundary-Based Algorithm, 14 candidate planner, 48 classification, 7, 8 trajectory segment, 38 collision avoidance, 17 collision detection, 12, 29 lazy, 13 complexity, 37, 40, 44 boundary algorithm, 26 Dijkstra’s Algorithm, 42 computation occlusion boundary, 47 penalty, 8, 32 precomputation, 7, 11, 32, 46, 53 query-time, 8, 46 recomputation, 47 computational cost, 47 conclusion, 77 configuration, 28 configuration space, 18 connected components, 23, 24 connectivity, 29, 44, 54 constraint binary, 17 field of view, 15, 19, 43 occlusion, 19, 21, 43 soft, 45 constraint satisfaction, 43 constraints sensor, 19 contiguous occlusion, 48 contributions, 7, 75 coordinate frame 81 Index camera, 42 world, 18 covert navigation, 15 d, 19 dC(pt), 43 dC(pt,qj,k(τ)), 42 Dijkstra Tree, 32 Dijkstra’s algorithm, 7, 33, 42, 46, 55 complexity, 42 displacement, 12, 29 dist(p, Lv), 34, 35 distance to occlusion boundary, 22 distance metric, 29 distance transform, 15 distance travelled, 32 d(q), 30 dynamic obstacles, 47 E, 28 , see sampling parameter, see sam- pling parameter e, 40 ej,k, 29, 32 eclipsing, 27 efficiency, 27, 30, 48 environment changes, 8 dynamic, 46, 47 model, 17 obstacle, 1 obstacles, 7 representation, 22, 23 state, 8 environment model, 32 experiment 1, 52 eye in hand, 1 field of view, 15, 19, 20, 30, 42, 52 field of view constraint, 43 forward kinematics, 18, 32, 35 front faces, 22 frustum, 15, 30, 42 future work, 76 G, 28 GASP, 14 General Automatic Sensor Plan- ning, 14 GNU Triangulated Surface Library, 12 graph roadmap, 11, 28 GTS, 12 H, 25 holonomic, 15 hypothesis, 53 I, 36, 38 interpolation, 34 interpolation time, 54 IRB6600, 53 k, 29 k-nearest neighbours, 29 `, 36–38 λ, see penalty mixing parameter line of sight, 1, 7, 15, 17, 19, 21, 40 linear interpolation, 15 link radii, 36 literature review, 10 local planner, 29 `(q), 30 Lv, 22, 34 M , 23, 32, 34 µ, see merge threshold 82 Index Machine Vision Planning system, 14 merge threshold, 36 mesh, 47 metric, 29 minimum-weight path, 42 model environment, 17, 32 target, 17 motion planning, 6 motion segmentation algorithm seetrajectory segmentation al- gorithm, 36 motivation, 3 MVP, 14 N , 28 nearest neighbours, 29 configuration space, 45 task space, 45 norm, 29 objectives, 7 obstacle, 1 occluded region, 18, 21, 33 occluded space, 33 occlusion, 1–3, 19, 20 occlusion boundary, 8, 14, 22, 25, 26 components, 27 occlusion constraint, 43 occlusion penalty, 32, 40, 53, 54 outline, 8 P, 18, 21, 34, 39 Pfree, 18 Pocc, 21, 34, 39 Pvis, 21, 34, 38, 39 pt, 23 pm, 40 psafe, 39 parameter interpolation, 34 sampling, 35 parameter optimization, 13 path, 17 minimum-weight, 42 of best visibility, 32 quality, 45 path planner, 2 algorithm, 33 candidate, 48 dynamic, 15, 27, 46, 47 field of view-aware, 15, 42 hybrid, 13 local, 29 multi-query, 11 query, 11, 32 single-query, 12, 46 static, 32 static vs.. dynamic, 47 visibility, 14 visibility-aware, 7, 8, 17, 32 path planning, 5 for visibility, 6 multi-query, 27 quality, 45 path search iterative, 48 penalties occlusion, 45 penalty, 8, 17 cumulative, 15 edge, 55 minimization, 18 occlusion, 32, 40, 53–55 spatial, 41 strictly positive, 41 task space, 41 temporal, 41 visibility, 7, 15 83 Index penalty mixing parameter, 41 planner dynamic, 8 static, 7, 8 visibility-aware, 7 planning motion, 6 sensor, 5, 13 precomputation, 11, 53 PRM, 11 probabilistic roadmap, 6, 7, 11, 27, 46, 53 connectivity, 29, 44 construction, 29, 45 edge, 33, 34 incremental construction, 11 sampling, 29 sparse, 12, 44 visibility, 12, 44 problem statement, 6, 17 pursuit-evasion, 15 Q, 19, 28 q, 19, 28 q1, 34 q2, 34 qdest, 48 qj,k, 32, 34 qstart, 48 quality metric, 13 query, 11, 46, 54 radii link, 36 reachable, 29 research context, 5 rod, 15 RRT-Connect, 12, 44 S, 38 sampling, 17, 29 configurations, 28 sampling parameter, 35 search, 12 binary, 35 local, 46 tree, 12 search breadth, 44 segment occluded, 41 segment classification, see trajec- tory segment classification segmentation, 7, 8 trajectory, 34 sensor constraints, 19 field of view, 30 frustum, 30 limitations, 19 location, 32 motion, 7 orientation, 32 trajectory, 35 sensor placement, 13, 14 sensor planning, 5, 13 separating faces, 22, 24, 25 spherical projection, 14, 27 stealth, 15 subdivision, 29, 30 adaptive, 12 T , 23, 34 τ , 34, 38 target, 7, 17 model, 17 representation, 22, 23 task space, 18 θ, 37 trajectory sensor, 35 trajectory classification, 50 84 Index trajectory segment classification, 38 trajectory segmentation, 34, 50 trajectory segmentation algorithm, 36 trajectory segmentation algorithm (field of view variant), 43 upper bound, 29 v(p), 34, 43 valence, 22, 40 visibility target, 17 visibility boundary, 24 visibility checking, 15 visibility loss, 50 visible region, 2, 18, 21 w(e), 40 wD, 48 wdist, 54 wdist(e), 41 wocc, 54 wocc(e), 41 waypoint, 17, 54 weights edge, 11 worst case, 38 85