A Perception-Guided Approach to Motion and Manipulation Planning by Yuan Wei B.S., Massachusetts Institute of Technology (2008) Submitted to the Department of Electrical Engineering and Computer Science in partial fulfillment of the requirements for the degree of Master of Engineering in Electrical Engineering and Computer Science at the MASSACHUSETTS INSTITUTE OF TECHNOLOGY June 2009 © Yuan Wei, MMIX. All rights reserved. The author hereby grants to MIT permission to reproduce and distribute publicly paper and electronic copies of this thesis document in whole or in part. Author . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Department of Electrical Engineering and Computer Science May 26, 2009 Certified by . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Nicholas Roy Associate Professor Thesis Supervisor Accepted by . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Arthur C. Smith Chairman, Department Committee on Graduate Theses 2 A Perception-Guided Approach to Motion and Manipulation Planning by Yuan Wei Submitted to the Department of Electrical Engineering and Computer Science on May 26, 2009, in partial fulfillment of the requirements for the degree of Master of Engineering in Electrical Engineering and Computer Science Abstract Rapidly-Exploring Random Trees (RRT) have been successfully applied to many different robotics systems for motion and manipulation planning under non-holonomic constraints. However, the conventional RRT algorithm may perform poorly in the presence of noise and uncertainty. This thesis proposes a modified form of the algorithm that seeks to reduce the robot’s uncertainty in its estimate of the target by choosing solutions that maximize the opportunities for the robot’s sensors to perceive the target. This new perception-guided technique will be tested in simulation and compared to the conventional RRT as well as other approaches taken from the literature. The ultimate goal is to integrate this method with a semi-autonomous robotic forklift charged with the task of approaching and picking up a loaded wooden pallet over rough terrain. Thesis Supervisor: Nicholas Roy Title: Associate Professor 3 4 Acknowledgments First, I would like to thank my advisor Professor Nicholas Roy, who provided much advice, guidance and insight throughout my research, and who gave me this opportunity to learn and work with everyone this past year. I would also like to acknowledge everyone on the Agile Robotics project, Andrew, Brandon, Jeon-Hwang, Luke, Matt A., Mike, Rob, Seth, Sertac, Steve, Tara, Tom and anyone else I may have forgotten, for all their help and assistance. Special thanks here goes to Matthew Walter, who has been my co-supervisor during my time on this project, and who has always been there to answer my questions or work with me to fix problems that arise. I would like to thank my labmates, Abe, Alborz, Dimitar, Emma, Garrett, Javier, Josh, Larry, R.J., Sam and Tom, for all their help and inspiration, and for showing me all the various projects and robots that they have been working on. Special thanks goes to Emma Brunskill and Tom Kollar, for first introducing me to the Robust Robotics Group, and for all their work on HMM inference direction following and the successful ICRA paper. And last but not least, to those closest to me, my family and friends, I would not be who I am or where I am today without your love and support. Thank you. This work was made possible by the generous support of AFOSR under the Agile Robotics for Logistics project, contract number 7000038334. 5 6 Contents Contents 7 List of Figures 9 List of Tables 11 1 Introduction 15 1.1 Manipulation Planning . . . . . . . . . . . . . . . . . . . . . . . . . . 15 1.2 Related Work . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 16 1.3 Approach . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17 1.4 Thesis Statement . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 18 1.5 Contributions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 18 1.6 Outline . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 18 2 Background 2.1 21 Rapidly-Exploring Random Trees (RRT) . . . . . . . . . . . . . . . . 21 2.1.1 Basic Algorithm . . . . . . . . . . . . . . . . . . . . . . . . . . 22 2.1.2 Sampling Strategies . . . . . . . . . . . . . . . . . . . . . . . . 24 2.1.3 Nearest Neighbor Metrics . . . . . . . . . . . . . . . . . . . . 25 2.2 BI-RRT . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 26 2.3 Connect-RRT . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 28 2.4 Handling Uncertainty . . . . . . . . . . . . . . . . . . . . . . . . . . . 29 3 Perception-Guided Sampling 31 7 3.1 Field-Of-View Sampling . . . . . . . . . . . . . . . . . . . . . . . . . 31 3.1.1 Sensor Model . . . . . . . . . . . . . . . . . . . . . . . . . . . 32 3.1.2 Target Model . . . . . . . . . . . . . . . . . . . . . . . . . . . 32 3.1.3 Complete Model . . . . . . . . . . . . . . . . . . . . . . . . . 33 3.2 FOV RRT . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 33 3.3 FOV Connect-RRT . . . . . . . . . . . . . . . . . . . . . . . . . . . . 36 4 Entropy-based Sampling 37 4.1 Kalman Filter . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 37 4.2 Entropy-based Sampling . . . . . . . . . . . . . . . . . . . . . . . . . 40 4.3 Trajectory Evaluation 42 . . . . . . . . . . . . . . . . . . . . . . . . . . 5 Evaluation 43 5.1 Experimental Setup . . . . . . . . . . . . . . . . . . . . . . . . . . . . 43 5.2 Results and Analysis . . . . . . . . . . . . . . . . . . . . . . . . . . . 44 5.2.1 Sampling Strategies . . . . . . . . . . . . . . . . . . . . . . . . 44 5.2.2 Trajectories . . . . . . . . . . . . . . . . . . . . . . . . . . . . 47 5.2.3 Filter Results . . . . . . . . . . . . . . . . . . . . . . . . . . . 49 5.2.4 Running Time . . . . . . . . . . . . . . . . . . . . . . . . . . . 52 6 Conclusion 53 6.1 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 53 6.2 Further Work . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 54 6.2.1 Improvements to Manipulation Planner . . . . . . . . . . . . . 54 6.2.2 Adapting for the Forklift . . . . . . . . . . . . . . . . . . . . . 55 Bibliography 57 8 List of Figures 2-1 An example RRT . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 25 2-2 The Bugtrap example . . . . . . . . . . . . . . . . . . . . . . . . . . . 28 3-1 The LIDAR sensing region . . . . . . . . . . . . . . . . . . . . . . . . 32 3-2 The pallet insertion face . . . . . . . . . . . . . . . . . . . . . . . . . 33 3-3 Three examples of FOV sampling . . . . . . . . . . . . . . . . . . . . 34 5-1 Coverage for random and FOV sampling . . . . . . . . . . . . . . . . 45 5-2 Coverage for entropy sampling . . . . . . . . . . . . . . . . . . . . . . 45 5-3 Discrepancy between FOV and Entropy model . . . . . . . . . . . . . 46 5-4 Two example trajectories . . . . . . . . . . . . . . . . . . . . . . . . . 47 5-5 Covariances of the target estimate along two trajectories . . . . . . . 48 5-6 Comparing reg. RRT, FOV RRT and FOV Connect-RRT . . . . . . . 49 5-7 Comparing reg. RRT, Entropy RRT and FOV Connect-RRT . . . . . 50 5-8 Comparing reg. RRT and FOV Connect-RRT w/ process noise . . . . 51 9 10 List of Tables 5.1 Average running time for each algorithm. . . . . . . . . . . . . . . . . 11 52 12 List of Algorithms 2.1 RRT PLANNER . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 23 2.2 BI-RRT PLANNER . . . . . . . . . . . . . . . . . . . . . . . . . . . . 27 2.3 EXTEND . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 27 2.4 CONNECT-RRT PLANNER . . . . . . . . . . . . . . . . . . . . . . . 29 3.1 RANDOM FOV STATE . . . . . . . . . . . . . . . . . . . . . . . . . . 35 3.2 FOV CONNECT-RRT PLANNER . . . . . . . . . . . . . . . . . . . . 36 4.1 The Kalman Filter algorithm . . . . . . . . . . . . . . . . . . . . . . . 38 4.2 Pallet insert slot detection algorithm . . . . . . . . . . . . . . . . . . . 39 4.3 ENTROPY-RRT PLANNER . . . . . . . . . . . . . . . . . . . . . . . 41 4.4 RANDOM ENTROPY STATE . . . . . . . . . . . . . . . . . . . . . . 41 13 14 Chapter 1 Introduction 1.1 Manipulation Planning The problem of finding the optimal path through a given environment is of much interest to researchers in many fields, and has wide-ranging applications to robotics, supply-chain management and manufacturing. These problems deal with complex systems with many degrees of freedom, often with various non-holonomic and differential constraints. In robotics, there is research on motion planning for many different kinds of vehicles traveling through diverse environments, from legged land robots to underwater submersibles to quad-rotor copters. Getting from point A to point B is only part of the challenge. As robots become more capable and sophisticated, there arises the need for robots to be able to perceive and manipulate their environment in order to further their objectives. For example, a household helper robot needs to open multiple doors and drawers while navigating a multi-room house. Similarly, a forklift robot needs to identify and pick-up pallets in a crowded warehouse or outdoor supply lot. The problem becomes even more complicated when we factor in the many possible sources of uncertainty about the environment and the state of the vehicle. This can include everything from bumps and indentations in the terrain to the substantial wheel slippage on a car to the unavoidable imprecision of GPS data. There has been a recent introduction of a number of methods in the field of motion 15 planning that employ randomized sampling-based strategies. One such method which has enjoyed success is the Rapidly-Exploring Random Tree (RRT) algorithm [19]. While the conventional RRT has been shown to perform well given accurate knowledge of the robot’s state and its surroundings, the algorithm’s performance suffers in the presence of uncertainty. This ability to plan under uncertainty is a major stepping stone towards smarter, more reliable robots. For example, a forklift robot needs to insert 5-inch wide tines into slots less than twice that width. To be able to accomplish this feat multiple times over uneven terrain, all the while dealing with noisy sensor readings, the robot needs to take advantage of opportunities to obtain accurate measurements of the pallet while picking the most easily traversible approach route. Speaking more generally, a superior system should be able to cope with significant uncertainty and adapt in a way that will maximize its chances of success. 1.2 Related Work There has been much interest in adapting RRTs to problems in which the state and environment variables are uncertain. Most methods choose one of two ways to handle uncertainty. One approach is to try to model and account for all possible states which may arise out of any uncertainty. This is the approach put forth by Melchoir and Simmons [23], who utilize ideas from particle filtering in a modified RRT. Each new extension of the tree is simulated multiple times given different values of the uncertain parameters, and the resulting set of “particles” are grouped together into a new node. The path most likely to succeed in reaching the goal is returned. Other techniques seek to build a series of sub-trees or compute a map of differing levels of risk due to uncertainty [12, 9]. These methods all suffer from slow running times and inefficiency due to the need to perform repeated calculations of control dynamics. In the case of Probabilistic Roadmap (PRM) based algorithms, costly preprocessing of the environment is also required [31]. The second popular approach is to modify the RRT algorithm such that solutions 16 are chosen which serve to introduce the least uncertainty into the system. In the case of an autonomous manipulator, this may mean finding the series of configurations that affords the robot’s sensors the best view of the target and surrounding obstacles. Work has been done with robotic manipulator arms that implement this strategy with success [2, 22, 8, 4]. However, the methods used to model the sensors and discover the current “view” of the robot are inefficient and complex. For example, [24] describes a technique to calculate the percentage of occlusion of the target object as seen by the robot’s camera at any point in time. Although this method was shown to be successful in reducing occlusion, the heavy computations needed to model the camera viewpoint caused the algorithm to run 10-20 times slower than the conventional planner. 1.3 Approach In the examples given, whether it is a household robot bending down to grasp an object on a table, or a forklift maneuvering to pickup a pallet, the ultimate goal is to successfully manipulate a target object. Keeping this in mind, we wish to compute a series of planned motions that will maximize the chances of achieving this goal. Our approach is to incorporate perception information in our planning algorithm, which is roughly broken up into three challenges. How can we collect/model perception information? As we mentioned in the previous section, much of the existing work for perception-coupled planning relies on complex models of the robot. We wish to formulate a simple model of the robot’s sensors that nevertheless captures the essence of the robot’s perceptive capabilities. How can we utilize this information to guide our planning algorithm? Our approach here is to include our perception model in the sampling strategy. The sampling strategy will contain a bias towards robot configurations that allow high-quality measurements of the target’s manipulation surfaces. The hypothesis is that this will influence the planner to find trajectories that reduce the uncertainty of the target. How can we ensure that all this will end up reducing uncertainty and improving the chances of success? In addition to our perception-guided planner, we will implement 17 an entropy sampling planner similar to a few of the examples in the literature. We plan to evaluate the different trajectories by running them through a Kalman filter and comparing the prior and posterior covariances for the target state. Our hypothesis is that our perception-guided planner will perform better than conventional RRTs and as well as the entropy-based planner, but with faster running times. 1.4 Thesis Statement Using perception information in our manipulation planning will help reduce the robot’s uncertainty in its environment and improve our confidence in acquiring and engaging the target object. 1.5 Contributions In this thesis, we provide several novel contributions to the area of motion planning. 1. We present a new Field-Of-View sampling strategy based on a simple model of the perceptive capabilities of our robot. 2. We modify the conventional RRT planner in several ways to use this FOV sampling distribution. 3. We conduct a comparison between our method and several variants of the RRT as well as other approaches to perception-guided planning. 1.6 Outline The rest of this thesis is divided as follows. In Chapter 2, we introduce the RapidlyExploring Random Tree (RRT) family of planning algorithms. In Chapter 3, we describe our perception-guided Field-Of-View sampling strategy and how we incorporate it into a RRT-based planner. In Chapter 4, we provide the background in optimal state estimation necessary for the formulation of an entropy-based sampling 18 strategy and planner. We then explain briefly our experimental setup in Chapter 5 and present the results. Finally, we conclude with a summary of our research, as well as suggestions for further work. 19 20 Chapter 2 Background This section introduces the main planning algorithm used in our work, and attempts to provide a better perspective on the contributions of the research described in Chapter 3. 2.1 Rapidly-Exploring Random Trees (RRT) First introduced in 1998 by LaValle, the Rapidly-Exploring Random Tree (RRT) [19] is a randomized motion planning algorithm. A major advantage of the RRT over other randomized planning algorithms such as the randomized potential field [1] and probabilistic roadmap [15] algorithms is the speed and efficiency with which it can fully explore an unknown space as well as the ease with which it can be applied to systems with non-holonomic and differential constraints [27, 5]. RRTs have been used to solve a variety of problems involving high DOF vehicles and robotic manipulators [11, 6, 18]. The RRT is a “single-query” planner, meaning it is designed to handle a separate set of inputs each time it is run. Unlike the PRM and other “multiple-query” planners which require preprocessing of specialized data structures, the “single-query” approach assumes no prior computation of any data structures. Because all necessary information is computed at runtime, the speed and simplicity of any perception-guided technique is crucial if we wish to apply it to a RRT. 21 2.1.1 Basic Algorithm We wish to clarify some conventions and definitions that we will be using in the following explanation of the RRT algorithm, as well as in subsequent sections unless otherwise noted. Robot State - For simplicity, we will take the state of our four-wheeled car to be a 4x1 vector q of its (x,y) position, orientation θ (in radians) and velocity v. q = [x, y, θ, v]T Configuration space - By configuration space, we mean the complete set of values which the robot state can take on. For a Nx1 state vector, the configuration space will be an N-dimensional space. For our car example, the configuration includes the real x-y plane along with a value between 0 and 2π for the orientation, and a real value for the velocity. Control action - Our control input is a 2x1 vector u of the incremental velocity and orientation. u = [dv, dθ]T The configuration space is denoted by C, and the set of all valid, obstacle-free states is denoted by Cf ree , such that Cf ree ⊂ C We are given an initial state and a goal state qinit , qgoal ⊂ Cf ree We are also given a fixed obstacle region Cobs , which is the complement of Cf ree . The objective is to find a continuous path from qinit to qgoal such that all points in the path lie in Cf ree . 22 We begin by initializing a tree structure, T , at qinit . Nodes in this tree are unique states in the configuration space, and two nodes may be connected with a directed edge. An edge in this tree represents a specific control action that when applied to qa , results in the robot state transitioning to qb . Each iteration of the algorithm attempts to extend the tree such that all nodes and edges lie entirely within Cf ree . When there exists a complete path in the tree from qinit to qgoal , the solution is returned and the algorithm ends. Algorithm 2.1: RRT PLANNER Data: qinit ,qgoal ,obstacles Result: T 1 T .init; 2 for i = 1 to M AX N ODES do 3 qrand ← RAN DOM ST AT E(); 4 qnear ← N EAREST N EIGHBOR(qrand , T ); 5 u ← SELECT IN P U T (qrand , qnear ); 6 qnew ← N EW ST AT E(qnear , u); 7 if qnew = qgoal then 8 return T 9 end 10 end 11 return T rapped For each iteration of the algorithm, a random state qrand is first selected from the configuration space by the sampling strategy encapsulated in RANDOM STATE. Then, we find the existing node nearest to qrand in the tree, qnear , according to some distance metric ρ defined in NEAREST NEIGHBOR. In Steps 5 and 6, the best input u is selected that will take qnear closest to qrand subject to the dynamics of our system. These dynamics equations are contained in NEW STATE, which also conducts checks to ensure that the new state qnew and its accompanying edge does not intersect with Cobs . The new node qnew and the new edge connecting qnear and qnew are added to the tree. Finally, a solution is declared when qnew is found to be within a certain tolerance, ², of the goal, and the complete tree is returned. If after many iterations, the number of nodes in the tree is greater than some threshold MAX NODES, and still no solution has been found, then the algorithm returns T rapped and exits. The 23 full outline of the standard RRT algorithm is shown in Algorithm 2.1. While the RRT algorithm as described above is fairly straightforward, there are a number of decisions and practical considerations one needs to make during implementation. We shall briefly address each of these below. 2.1.2 Sampling Strategies While the algorithm is named Rapidly-Exploring “Random” Tree, there is no requirement that new nodes in the tree be randomly selected. In fact, choosing the sampling strategy is a powerful way to affect the output of the algorithm and to adapt it to the specific problem at hand. Besides uniform random sampling, there exist numerous other sampling strategies, each of which seeks to handle a particular problem or improve the performance of the planner in a specific situation. For example, a common shortcoming of randomized planners is the failure to efficiently navigate tight spaces or narrow passageways [20]. Thus, a number of sampling strategies have been proposed which seek to increase sampling at bottlenecks in the free configuration space, Cf ree , and near obstacles [13, 30]. A partial list of examples include Gaussian sampling, Bridge sampling, and Medial Axis sampling [10]. The task of navigating constrained spaces has some similarities to our problem of approaching an object for manipulation. In both cases, we wish to plan a path that may lie very close to obstacles without colliding with any. The basic strategy is to sample more states which lie within the confined space or close to the obstacle. Of course, an important consideration is how to achieve good performance in bottlenecks without sacrificing the rapid exploration of configuration space that is one of the advantages and hallmarks of RRTs. Our version of the conventional RRT will utilize random sampling during most of its exploration and planning phase. When the resulting tree approaches the goal however, the sampling strategy will begin to bias states close to the goal in order to facilitate finding a solution [26]. The trajectory from a RRT planner and the associated tree is shown in Figure 2-1. 24 Figure 2-1: The tree generated by our conventional RRT planner. The pallet is the red square and the forklift (white rectangle) is shown in its initial pose. The final trajectory is highlighted by the dashed black line. 2.1.3 Nearest Neighbor Metrics There are numerous distance metrics that can be used in computing the nearest neighboring node of a RRT to a given state. A common metric is Euclidean distance, often used in 2- or 3-dimensional configuration spaces. This metric makes sense for many mobile robots for which physical distance is a reasonable measure of the costto-go between two states. Equation 2.1 expresses the Euclidean distance of two robot state vectors in 2-dimensional space q1 = [x1 , y1 ] and q2 = [x2 , y2 ]. ρeuclidean = p (q1 − q2 )(q1 − q2 )T (2.1) When we take into account orientation as well, one possible metric is that shown in Equation 2.2, for two state vectors q1 = (x1 , y1 , θ) and q2 = (x1 , y1 , θ). ρweighted = p (q1 − q2 )Wt (q1 − q2 )T 25 (2.2) Wt here is the weight vector (wx , wy , wθ ) that represents the relative importance of each member of the state vector. One way to think about this is to consider the case when wθ is very high relative to wx and wy . This means even a small difference in orientation between two states will translate into a large value for the distance. By using this metric, we will very likely be biasing our solution to include many straight paths and gentle turns. On the other hand, if wx and wy is set very high relative to wθ , then the algorithm may try to extend one node in the tree toward another state that is close by but oriented differently. This presents a problem for vehicles with limited maneuverability and can lead to less than desirable trajectories. Experiments were done in our case to find the appropriate values of Wt that yielded the shortest paths and highest success rate. We found that an equal weighting scheme of Wt = [0.33 0.33 0.33] seemed to produce the best results. This is a reasonable finding because the forklift has a fairly tight, but still non-zero turning radius. Another popular metric often used for four-wheeled robots is Dubin’s distance based on the shortest Dubin’s path for a car-like robot with a minimum turning radius [7]. While a full explanation of Dubin paths is outside the scope of this thesis, we note here that it is a perfectly valid distance metric for this problem. We chose the weighted Euclidean distance metric mainly because of its simplicity. 2.2 BI-RRT For our method, we are mainly concerned with the sampling strategy, but there is a rich body of literature on the family of planning algorithms derived from RRTs. One of these variants which is relevant to our research is the Bi-directional RRT (BI-RRT) [21]. The Bi-directional RRT attempts to grow two tree structures, one from the initial state and one from the goal state. The two trees, Ti and Tg , are kept balanced by being alternately extended, then swapped with each other. When a common vertex is found, the two trees are joined at that node and the complete solution is returned. Additionally, as shown in Algorithm 2.2 and 2.3, Steps 5 through 9 of the original 26 RRT algorithm are encapsulated in the EXTEND subroutine. Otherwise, the rest of the algorithm is the same as for the original RRT. Algorithm 2.2: BI-RRT PLANNER Data: qinit ,qgoal ,obstacles Result: T 1 Ti .init(qinit ); 2 Tg .init(qgoal ); 3 for i = 1 to M AX N ODES do 4 qrand ← RAN DOM ST AT E(); 5 qnear ← N EAREST N EIGHBOR(qrand , T ); 6 if EXT EN D(Ti , qrand , qnear ) = Reached then 7 T ← COM BIN E(Ti , Tg ); 8 return T 9 end 10 SW AP (Ti , Tg ); 11 end 12 return T rapped Algorithm 2.3: EXTEND Data: T ,qrand ,qnear Result: Result 1 u ← SELECT IN P U T (qrand , qnear ); 2 qnew ← N EW ST AT E(qnear , u); 3 if qnew = qgoal then 4 return Reached 5 else 6 return Advanced 7 end The bi-directional approach has several advantages compared to the conventional RRT. One, it can handle bug-trap configurations such as the one shown in Figure 2-2, which would be very difficult for a single-RRT to navigate [20]. Two, it has been shown that utilizing two trees becomes more efficient in higher dimensional configuration spaces [21]. However, there is a tradeoff between the exploration properties of multiple trees against the cost of finding a common node to connect them. The extra cost of connecting two RRTs can be reduced significantly if a bias is introduced into the algorithm to improve the probability that the two trees will meet. 27 Figure 2-2: This slightly contrived ”bugtrap” example shows one of the advantages of growing two RRTs instead of one. This is the approach of the RRT variant described in the next section. 2.3 Connect-RRT The Connect-RRT [16] is a type of RRT planner that is designed to improve the performance of the BI-RRT, especially in high-dimensional configuration spaces. It employs a greedy heuristic called CONNECT that is an alternative to EXTEND from Step 6 in Algorithm 2.2. The full algorithm is shown in Algorithm 2.4. Remember from the previous section that EXTEND attempts to solve the dynamics equations and extend the RRT for a single time step. CONNECT, on the other hand, tries to extend the RRT until either an obstacle or some state q is reached. No sampling or nearest neighbor calculation is needed for each iteration. Instead the output of EXTEND from the previous step, qnew , becomes the new input, qnear , allowing for a long path to be added to the tree relatively cheaply. This is one of the advantages of the Connect-RRT. The Connect-RRT also achieves a balance between random exploration and directed growth. In Step 5, the algorithm selects a new sample and attempts to grow one tree towards that sample, increasing the coverage of the RRT. In Step 6, the algorithm attempts to connect the second tree to the most recently added node on the first tree, which if successful will yield a full solution. 28 Due to these advantages, the Connect-RRT will serve as the underlying model for our perception-guided planner. It has been demonstrated experimentally that the Connect-RRT performs better than both the conventional RRT and Bi-directional RRT in terms of running time and the ability to navigate cluttered environments [16, 17]. Algorithm 2.4: CONNECT-RRT PLANNER Data: qinit ,qgoal ,obstacles Result: T 1 Ti .init(qinit ); 2 Tg .init(qgoal ); 3 for i = 1 to M AX N ODES do 4 qrand ← RAN DOM ST AT E(); 5 qnear ← N EAREST N EIGHBOR(qrand , T ); 6 if CON N ECT (Ti , qrand , qnear ) 6= T rapped then 7 if CON N ECT (Tg , qnew , qnear ) = Reached then 8 T ← COM BIN E(Ti , Tg ); 9 return T 10 end 11 end 12 SW AP (Ti , Tg ); 13 end 14 return T 2.4 Handling Uncertainty One of the major shortcomings of the RRT algorithms described above is their inability to handle uncertainty. For example, incorrect knowledge about the environment affects the calculations of the obstacle-free configuration space and can result in invalid paths. Uncertainty in the robot position and dynamics, such as wheel slip, affects the computation of control inputs at each step of the algorithm. And although RRTs have been adapted to perform well in cluttered spaces, it is a problem if the positions of the obstacles are uncertain. Of course, there is a simple way to reduce the robot’s uncertainty in the world – take measurements of the environment. Currently, the RRT algorithm takes into 29 account a robot’s valid configuration space and its motion dynamics when building the RRT. However, it does not take into account the robot’s sensors, which are directly related to the robot’s ability to obtain measurements of the environment. In the next chapter, we will attempt to model a robot’s perceptive capabilities which will then be incorporated into the planning process. 30 Chapter 3 Perception-Guided Sampling The main focus of our research is on how we can incorporate perception information in the planning process. As mentioned in the previous chapter, one component of RRT-based planning techniques that is readily modified is the sampling distribution used at each iteration of the algorithm. If we can define an appropriate sampling strategy that emphasizes positive perceptive gains, then the resulting trajectory from the planner should include those gains. In this chapter, we introduce the Field-OfView (FOV) sampling strategy and describe how it is utilized by our planner to produce desirable trajectories. 3.1 Field-Of-View Sampling In the ideal case, we would like a sampling strategy that will only pick those points in the configuration space that lie on the optimal path. By optimal path, we mean the series of states from start to goal that minimize the posterior covariance of the target state and thus maximize the chances of successful manipulation. Of course, this is an unreasonable assumption since if we knew the optimal path, then we would not need a sampling strategy to plan that path in the first place. However, we are given that the uncertainty associated with the target pose is decreased by additional measurements of the target. If we can bias our sampling to favor those robot poses which are more likely to yield acceptable measurements of 31 Figure 3-1: The LIDAR sensing region, with a range of D and sensing angle of 2α target pose, then our sampling strategy should lead to trajectories that lie close to the optimal path. Not all robot poses yield measurements of the target, so accurate models of the robot’s sensors and of the target are required. 3.1.1 Sensor Model The primary sensors on the forklift robot that we will use in both real-world testing and simulation are laser range-finders mounted on the fork tines, facing forwards. They provide planar range data up to a certain distance D, and have a limited angular field-of-view, α. Figure 3-1 below shows a representation of the laser range-finder’s sensing region. For simplicity, we will model this region as a circular sector with its origin at the sensor’s (x,y) location, and its angle as 2α. Technically, the complete laser-range data is made up of individual returns from ‘rays’ typically spaced 0.25 to 0.5 degrees apart, but we shall ignore this for now, since even at the maximum distance, the gaps between successive rays are no more than a few millimeters. 3.1.2 Target Model Since we are interested in manipulating the pallet with our forklift robot, the target will be the front insertion face of the pallet, shown below in Figure 3-2. For simplicity, we represent the insert slot, position, and direction as a vector centered on the midpoint of the insertion face and pointing away and normal to the insertion face. 32 Figure 3-2: The pose of the pallet insertion face is represented by a vector centered on the face and pointing outward and normal to the face. 3.1.3 Complete Model Finally, we are ready to define our complete Field-Of-View model. Remember that we wish to find the set of robot poses for which a measurement of the target is able to be obtained. First, we require that the insertion face of the pallet be within the sensing region of the laser range-finder. Second, we require the position of the sensor to be within some angular distance from the orthogonal with respect to the insertion face. In Figure 3-3(a) the pallet is outside the robot’s sensing region, and the robot’s pose would not be considered appropriate for recording a measurement of the pallet. Similarly in Figure 3-3(b), while the pallet is technically within the robot’s sensing region, the pose of the robot is such that it is behind the front face of the pallet . This pose will not result in a measurement of the insertion face, so this particular robot pose will also be disqualified. Finally in Figure 3-3(c), the pallet is within the sensing region of the robot and the robot is positioned roughly orthogonal to the insertion face, so this robot pose will be included in our FOV sampling distribution. 3.2 FOV RRT We wish to incorporate our new Field-Of-View sampling strategy into the RRT algorithm to generate a perception-guided planner. The most straightforward approach is to include the FOV sampling strategy in a routine similar to RANDOM STATE from Step 2 of Algorithm 2.1. We shall name this routine RANDOM FOV STATE. 33 (a) (b) (c) Figure 3-3: Three different positions of the robot (blue rectangle) w.r.t. the pallet (yellow square) insertion face. In (a) the pallet is outside the sensing region, and in (b) the incident angle from the sensor is too steep, unlike in (c) where the pallet is well-situated within the range of the sensors. 34 We define a function IN F OV (qrand ) that returns true if and only if qrand is found to be within Cf ov . We also introduce a parameter γ, the FOV sampling ratio, that takes a real value between [0, 1] and determines what percentage of returned samples are guaranteed to be in Cf ov . A value of 1.0 means all sampled states returned by RANDOM FOV STATE will lie in Cf ov , while a value of 0.0 means RANDOM FOV STATE will sample from the full configuration space (see Algorithm 3.1). Algorithm 3.1: RANDOM FOV STATE Data: none Result: qrand 1 p ← rand[0, 1]; 2 if p < γ then 3 while 1 do 4 qrand ← RAN DOM CON F IG; 5 if IN F OV (qrand ) then 6 return qrand 7 end 8 end 9 else 10 qrand ← RAN DOM CON F IG; 11 return qrand 12 end The repeated sampling in the while loop in Algorithm 3.1 is cheap, even when Cf ov is relatively small compared to Cf ree . But while this FOV-RRT planner is simple and easy to implement, there are several problems with this approach. One, it is unclear what the value of γ should be. It could very well be that the best value of the ratio is different for each specific planning query. Some preliminary tests, not shown here, were conducted, but proved inconclusive. A related problem with this approach is the fact that sampling robot poses which obtain good measurements of the target does not necessarily imply those same poses will help build the tree toward the goal. Thus, large values of γ may risk not reaching the goal, while low values may produce less than desirable trajectories. 35 3.3 FOV Connect-RRT The Connect-RRT first introduced in chapter 2 is useful here. First, growing two trees, one from the start and one from the goal, reduces the worry that an overly selective sampling strategy will fail to reach the goal. Instead, the alternating use of the CONNECT function to grow towards either a newly sampled pose or the other tree, strikes a good balance between exploration and solution-finding. Additionally, it makes more intuitive sense to utilize the FOV sampling strategy when growing a tree backwards from the goal. In the goal pose, the robot should already be in a good position to obtain measurements of the target. Then, as we grow the tree backwards away from the goal pose, we expect the FOV sampling to have the effect of ‘encouraging’ the robot to maintain the target within sensor range. Algorithm 3.2 showing the Connect-RRT is the same as Algorithm 2.4 from the previous chapter, with the addition of the modified RANDOM FOV STATE described above. Algorithm 3.2: FOV CONNECT-RRT PLANNER Data: qinit ,qgoal ,obstacles Result: T 1 Ti .init(qinit ); 2 Tg .init(qgoal ); 3 for i = 1 to M AX N ODES do 4 qrand ← RAN DOM F OV ST AT E(); 5 qnear ← N EAREST N EIGHBOR(qrand , T ); 6 if CON N ECT (Ti , qrand , qnear ) 6= T rapped then 7 if CON N ECT (Tg , qnew , qnear ) = Reached then 8 T ← COM BIN E(Ti , Tg ); 9 return T 10 end 11 end 12 SW AP (Ti , Tg ); 13 end 14 return T rapped 36 Chapter 4 Entropy-based Sampling In the last chapter, we formulated a novel sampling strategy and demonstrated how it can be incorporated into a perception-guided planner. We used a simple, geometric model of the robot’s sensors, but other models are also available. One approach is to simulate a full set sensor readings, then use the output of some kind of filter to calculate the amount of information gained. This information could then be used in a perception-guided planner. In this chapter, we formulate a sampling strategy that fully simulates all sensor data and directly calculates the entropy-loss (or information-gain) for each sampled robot pose. We first provide some background on Kalman filters, and describe the use of the filter in implementing a entropy-based sampling strategy and an associated perception-guided planner. Then, we explain how we use filtering to keep a constantly updated estimate of the pallet’s state, which will become useful later on in evaluating the performance of the algorithms. 4.1 Kalman Filter The Kalman filter [14] is a very popular method for optimal state estimation in the presence of noise. We wish to use the Kalman filter to evaluate the performance of different planned trajectories by calculating the posterior covariance of the target state as well as the error in the final estimate. We briefly present the basic Kalman 37 filter equations below. The equations for a single update step of the Kalman filter are shown in Algorithm 4.1. We assume that the current state xk and observation zk are linear functions given by xk = Fk xk−1 + wk wk ∼ N (0, Qk ) z k = H k xk + v k vk ∼ N (0, Rk ) where Fk is the state transition matrix that updates the current state one timestep, and Hk is the measurement matrix that transforms state space to observation space. The process noise wk , and measurement noise vk , are Normal distributions centered around zero with a covariance of Qk and Rk respectively. The Kalman Filter requires only the state estimate from the previous timestep and the current observation to calculate the new state estimate. In Steps 1 and 2 of Algorithm 4.1, the state estimate and covariance are propagated to the current step. In Step 3, the optimal Kalman gain Kk is calculated such that the minimum mean square-error (MMSE) solution is achieved. Finally, in Steps 4 and 5, the state estimate and covariance are updated with the new measurement and the associated measurement noise. Algorithm 4.1: The Kalman Filter algorithm Data: previous state xk−1 ,Pk−1 ,action uk , observation zk Result: updated state xk ,Pk − 1 x̂k = Fk−1 x̂k−1 ; − T 2 Pk = Fk−1 Pk−1 Fk−1 + Qk−1 ; − T − T −1 3 Kk = Pk Hk (Hk Pk Hk + Rk ) ; − − 4 x̂k = xk + Kk (zk − Hk x̂k ); − 5 Pk = (I − Kk Hk )Pk ; We wish to use the Kalman filter to calculate the entropy-loss of a particular LIDAR scan. We shall apply the equations in Algorithm 4.1 for a single timestep, with a few slight modifications. For practical reasons as well as for simplicity, our filter will only keep track of three variables: the x-positions of the two pallet inserts 38 and the width of the pallet inserts. Although we do not filter over the pallet pose explicitly, we can obtain the pose indirectly from the estimate of the insert face, assuming the pallet has known, standard dimensions. The state vector is x̂ = [insertlef tx , insertrightx , w]T The position and dimensions of the pallet stay constant, so there is no change in the state for each update step and Fk = I. We make this assumption because for our problem, the pallets to be picked up are standard issue pallets and remain stationary on the ground. From the LIDAR scans, we can directly measure all three variables, with some measurement noise. We obtain the measurements by running a insert slot detection algorithm which operates as follows. We are given the current estimate of the pallet insertion face. First, the raw LIDAR returns are transformed into a local insertion face coordinate frame, and points that do not lie on the plane of the insertion face are culled. Then, the algorithm finds ‘gaps’ in the remaining points that may correspond to the insert holes. For each ‘gap’, we run a simple data association check using the Mahalanobis distance metric [25] to find the pair of ‘gaps’ which most closely match our current estimate of the insert slot position and width. This pair of ‘gaps’ is our measurement for this set of LIDAR scans. Pseudocode is shown in Algorithm 4.2. Algorithm 4.2: Pallet insert slot detection algorithm Data: current estimate xk ,Pk , raw LIDAR scans Result: new measurement zk 1 Transform LIDAR points to local coord. frame; 2 Cull points not on insertion face; 3 Find gaps in the scan; 4 Perform data association on all possible pairs of gaps; 5 Return most likely pair of gaps as zk ; 39 4.2 Entropy-based Sampling Our FOV sampling strategy sought to capture perception information using a somewhat simplified model of the robot’s sensors. But there is a more direct way to measure perception information. We can calculate the entropy-loss (or information gain) of a particular set of sensor readings using the Kalman filter. Because we obtain the state vector from the slot detector directly, we have an identity measurement model, Hk = I. If we further assume constant measurement noise, our Kalman filter update equations become − x̂k = x− k + Kk (zk − x̂k ) (4.1) Pk = (I − Kk )Pk− (4.2) Kk = Pk− (Pk− + Rk )−1 (4.3) From these equations, we proceed to compute the information gain. Notice in equation 4.2 above that the posterior covariance Pk is equal to the prior covariance Pk− plus some term involving Kk . This term is proportional to the information gain of the current measurement. The larger the optimal Kalman gain Kk is, the larger the information gain, or the smaller the entropy-loss. Rewriting equation 4.2 yields Pk− − Pk = Kk Pk− (4.4) the difference between the prior and posterior covariances, also known as the information gain. Using this result, we can calculate the information gain of each simulated LIDAR scan and formulate our “entropy” sampling strategy. Namely, those robot configurations which yield positive information gain according to equation 4.4 are biased in the sampling, similar to the sensing region in the Field-Of-View sampling method. By incoporating “entropy” sampling into the Connect-RRT, we obtain the Entropy RRT (see Algorithm 4.3). The Entropy RRT and FOV Connect-RRT from chapter 3 are identical except 40 for the sampling subroutines. Similar to the RANDOM FOV STATE subroutine, RANDOM ENTROPY STATE implements a bias toward choosing robot poses which offer positive infomation gain (see Algorithm 4.4). We also introduce a helper function named GET INFO GAIN which computes the information gain for a particular robot pose. Algorithm 4.3: ENTROPY-RRT PLANNER Data: qinit ,qgoal ,obstacles Result: T 1 Ti .init(qinit ); 2 Tg .init(qgoal ); 3 for i = 1 to M AX N ODES do 4 qrand ← RAN DOM EN T ROP Y ST AT E(); 5 qnear ← N EAREST N EIGHBOR(qrand , T ); 6 if CON N ECT (Ti , qrand , qnear ) 6= T rapped then 7 if CON N ECT (Tg , qnew , qnear ) = Reached then 8 T ← COM BIN E(Ti , Tg ); 9 return T 10 end 11 end 12 SW AP (Ti , Tg ); 13 end 14 return T rapped Algorithm 4.4: RANDOM ENTROPY STATE Data: none Result: qrand 1 p ← rand[0, 1] if p < γ then 2 while 1 do 3 qrand ← RAN DOM CON F IG; 4 if GET IN F O GAIN (qrand ) > 0 then 5 return qrand 6 end 7 end 8 else 9 qrand ← RAN DOM CON F IG; 10 return qrand 11 end 41 4.3 Trajectory Evaluation For our experiments, we wish to track the robot’s estimate of the state of the target pallet as the robot travels along the trajectory. In the previous section, we computed the entropy-loss due to a measurement by running the Kalman filter for a single timestep. To find the final state estimate and total entropy-loss for all measurements, we will run the filter over the entire trajectory. x̂− k = x̂k−1 (4.5) Pk− = Pk−1 + Qk−1 (4.6) − x̂k = x− k + Kk (zk − x̂k ) (4.7) Pk = (I − Kk )Pk− (4.8) Kk = Pk− (Pk− + Rk )−1 (4.9) The filter equations remain the same except in 4.6 we include the process noise Qk−1 when updating the covariance. This process noise represents the noise from wheel slippage on the forklift, instability from traveling on uneven terrain, and other sources of uncertainty that arise when the robot is in motion. 42 Chapter 5 Evaluation In this chapter, we describe our method for evaluating the performance of our novel planner. First, we explain the overall experimental setup. Then, we present a comparison of the sampling strategies and analysis of the output trajectories for the FOVplanner. We also compare the performance, as measured by the posterior covariance of the pallet inserts estimation, of our FOV-planner, the entropy-based planner as well as the conventional RRT planner. Finally, we briefly discuss the relative running times of the three algorithms. 5.1 Experimental Setup In the previous chapter we introduced the Entropy RRT, which employed a different perception-guided sampling strategy than the FOV Connect-RRT. Our motivation for this new algorithm is to provide a benchmark for the evaluation of our FOV planner. We present some experimental results below. All algorithms were implemented in MATLAB and tested on a Dell Precision M4400 laptop running a Intel Core 2 Duo @ 3.06GHz with 4 GB of RAM. The experiments were conducted as follows. The environment was initialized to have a single pallet at position (0,0), with its main insertion face oriented in the −x direction, within a larger area of 20m x 20m. Each algorithm was run once from each of 256 starting poses arranged in a grid pattern around the pallet. This grid 43 pattern included starting poses with various orientations and with different distances from the pallet. Therefore, even though the pallet position remains the same during the entire experiment, due to the symmetry of he obstacle-free environment, it would be as if we had tested many different starting and goal positions. The resulting 256 trajectories were then run in simulation using a specialized toolchain, and the LIDAR scans were saved. The pallet inserts were then estimated using a Kalman filter and the posterior covariances resulting from each trajectory were found. 5.2 5.2.1 Results and Analysis Sampling Strategies We begin with an examination of the sampling strategies introduced in this thesis. We sample N = 200 points each with the random and Field-Of-View sampling methods, and plot the results in Figure 5-1. As expected, random sampling spreads the samples uniformly over the entire space. FOV sampling (with γ = 1.0) performs as expected as well, only choosing poses in a circular arc directly in front of the pallet insertion face. The sampling coverage for the Entropy-based sampling strategy is shown in Figure 5-2a. Initially, it looks the same as the FOV sampling coverage. On closer inspection, however, we notice a few differences. By taking 1000 random points and evaluating each using both the FOV and Entropy models, we can obtain a differential comparison of the two sampling methods (Figure 5-2b). Data points with triangular markers are valid by the FOV model, but not by the Entropy model, and vice versa for points with circular markers. In effect, we are plotting the symmetric difference of the two sampling spaces Cf ov 4 Centropy = (Cf ov ∪ Centropy ) − (Cf ov ∩ Centropy ) (5.1) Upon initial examination, we notice that there are many more triangular points 44 (a) Random Sampling (b) FOV Sampling Figure 5-1: The sampling coverage for the random and FOV sampling strategies. The pallet (red square) has insert slots facing left in both figures. The line connected to each sample shows the orientation of that sample. (a) Entropy Sampling (b) FOV/Entropy Comparison Figure 5-2: The sampling coverage for the entropy sampling strategy and a differential comparison between the FOV and entropy samplers. 45 (a) (b) Figure 5-3: The FOV model will declare both examples to be valid poses, but the Entropy model requires a full view of the pallet insert face which is only satisfied by (a). than there are circular ones. This means the Field-Of-View model is less discriminating than the Entropy model about which robot poses are sampled. From investigating the reasons the two models do not agree on specific points, we discover the main discrepancy is due to our simplification of the pallet insert face. Recall from Chapter 3 that we chose to represent the pose of the pallet insert face as a single vector centered on the face and pointed outward and normal to the face. A particular pose is considered valid by the FOV model as long as this vector lies within its ‘sensing region’ (see Figure 5-3a). The Entropy model, on the other hand, runs a slot detector on simulated LIDAR returns of the pallet. This requires a full view of the pallet. In Figure 5-3b, we see very clearly that although this particular robot pose is valid by the FOV model, it does not have a full view of the pallet insert face. Although this discrepancy between the two sampling strategies is due to a simplification, we do not want to overreact and begin complicating our FOV model. If we try modeling the insert face as two vectors on each corner of the pallet, there will still be a discrepancy because the vertical supports on a pallet have some width to them. One of the advantages of its FOV sampling strategy is its simplicity, and as we will show later in this chapter, the FOV RRT performs as well as the Entropy RRT, and 46 (a) (b) Figure 5-4: Two typical trajectories returned by the (a) conventional RRT and (b) FOV Connect-RRT. The red box is the pallet, and the white rectangle represents the forklift. The arrows indicate the orientation of the forklift at that point in the trajectory. runs significantly faster. 5.2.2 Trajectories We continue with a comparison and qualitative analysis of the trajectories returned by the various planning methods being evaluated. In Figure 5-4a, we see an example of a trajectory returned by the regular RRT. The forklift approaches the pallet from the side before making a sharp right turn at the very end to face the pallet. Figure 5-4b shows the trajectory output by the FOV Connect-RRT given similar initial conditions. This time, the forklift first backs out away from the pallet a little bit before making a wide turn and approaching the pallet at rougly 45°. In this example, the wide turn and gentle approach of the second trajectory allows the forklift to take more and better LIDAR scans of the pallet inserts. In Figure 5-5, we plot the same two trajectories as before, but attempt to show how the uncertainty of the robot’s target estimate changes along each trajectory. Observe that since we are filtering over the pallet inserts pose, the covariances for the estimates will remain centered over the pallet. But we wish to show the changes 47 (a) (b) Figure 5-5: The covariances of the robot’s estimate of the target along the trajectories for the (a) conventional RRT and (b) FOV Connect-RRT. The radius of the circles indicates the relative uncertainty of the robot’s target estimate at that point in the trajectory. The larger the circle, the greater the uncertainty. to the covariance along the trajectory. Therefore, although the circles depicted in Figures 5-5a and 5-5b are centered over the robot, they do not represent the robot pose covariances. Instead, they represent the covariance of the robot’s estimate of the target at that point in the trajectory. It is also important to note that the size of the circles are not to scale. They are simply drawn to convey the relative differences in the target uncertainty at various points along the same trajectory, and between the two trajectories. Notice in the conventional RRT path (Figure 5-5a), that the target covariance decreases initially. But as the robot approaches the pallet from a steep angle and makes a sharp right into the goal pose, the robot loses sight of the pallet inserts, and the target covariance increases. Compare this to the path returned by the FOV Connect-RRT (Figure 5-5b). Again, the target covariance decreases initially to reach a steady-state value. However, the gentle angle of approach taken by the robot in this example allows the robot to keep the pallet inserts within its field-of-view and the final covariance is significantly lower than for the other trajectory. 48 0.01 regular RRT FOV Connect−RRT FOV RRT 0.009 0.008 Covariance Trace 0.007 0.006 0.005 0.004 0.003 0.002 0.001 0 200 300 400 500 Trajectory Length (# of nodes) 600 700 Figure 5-6: Comparison of the FOV Connect-RRT with the conventional RRT and FOV RRT. Shown is the average posterior covariance for trajectories of roughly the same length. 5.2.3 Filter Results If we ignore the process noise in the Kalman filter for now, we can find the posterior covariances of the pallet estimation output by the filter for each algorithm. Figure 5-6 shows the average posterior covariances for paths of roughly the same length, for the conventional RRT, FOV RRT and FOV Connect-RRT. As expected, the conventional RRT performs the worst out of the three. The FOV RRT performs better, but it is unclear whether the difference is significant, as evidenced by size of the error bars. The FOV Connect-RRT clearly outperforms both alternatives by having the lowest posterior covariance, and therefore the least uncertainty about the pallet inserts. In Figure 5-7, we compare the FOV Connect-RRT with the conventional RRT as well as the Entropy RRT. As expected, both the FOV Connect-RRT and the Entropy RRT far outperform the conventional RRT. The important result here is the 49 0.01 regular RRT FOV Connect−RRT Entropy RRT 0.009 0.008 Covariance Trace 0.007 0.006 0.005 0.004 0.003 0.002 0.001 0 200 300 400 500 Trajectory Length (# of nodes) 600 700 Figure 5-7: Comparison of the FOV Connect-RRT with the conventional RRT and Entropy RRT. Shown is the average posterior covariance for trajectories of roughly the same length. fact that our FOV Connect-RRT performs as well as the Entropy RRT. This shows our simplified Field-Of-View model of the LIDAR sensors successfully captures the perceptive capabilities of the robot. We also notice that the FOV Connect-RRT produces much longer trajectories than the other two methods. This is because our planner makes no guarantee about the optimality of the generated path. Desirable trajectories are not expected to be the shortest path, simply the ones which maintain the target in the field-of-view the longest. In fact, it may be the case that the long trajectories returned by the FOV Connect-RRT naturally provide more measurements of the pallet and thus yield better results. To control for this possible effect, we introduce a constant process noise into the filter. Figure 5-8 compares the posterior covariances for the conventional RRT vs. the 50 0.25 regular RRT FOV Connect−RRT Covariance Trace 0.2 0.15 0.1 0.05 0 300 350 400 450 500 550 Trajectory Length (# of nodes) 600 650 700 Figure 5-8: Output of the Kalman filter with constant process noise Qk = 0.0001 for the FOV Connect-RRT and conventional RRT. Shown is the average posterior covariance for trajectories of roughly the same length. 51 Avg. runtime (sec) RRT FOV Connect-RRT 37.5 42.9 Entropy RRT 389.2 Table 5.1: Average running time for each algorithm. FOV Connect-RRT when a constant process noise wk ∼ N (0, Qk ) is included in the Kalman filter. For shorter trajectories, there is not much difference in performance between the two algorithms. This may be because many short paths solve initial conditions in which the robot is close to the pallet and already oriented correctly for an approach. In these cases, even the conventional RRT will end up finding a trajectory that results in good measurements of the pallet. As the length of the trajectories increase, however, the FOV Connect-RRT performs significantly better than its conventional counterpart. 5.2.4 Running Time Finally, we compare running times for these same three algorithms in Table 5.1. While the regular RRT and the FOV Connect-RRT have similar runtimes, the Entropy RRT is significantly slower, due to the cost of simulating the sensor readings for each sample. This problem can be partially avoided by computing a lookup table of sensor readings and the associated information gains offline. However, that would require costly preprocessing and lead to a loss in resolution, because the configuration space will need to be discretized. Overall, the time needed to generate a trajectory is rather slow, regardless of the planner used. The main reason for this is that currently, the algorithm is implemented completely in MATLAB. In the future, porting the implementation to a faster language like C, or even rewriting some frequently used MATLAB subroutines in the precompiled .mex format should help improve running times. 52 Chapter 6 Conclusion 6.1 Summary Our motivation for this thesis was to improve the successful manipulation of a target object by reducing its associated uncertainty. We approached this problem by introducing perception information into the planning process in the hope of obtaining plentiful measurements of the target as we prepared to engage it, thus reducing the uncertainty of the target state. In Chapter 2, we provided a brief introduction to Rapidly-Exploring Random Trees, as well as one popular variant, the Connect-RRT, which served as the foundation for our perception-guided planner. In Chapter 3, we proposed our Field-Of-View sampling strategy and the associated sensor models and assumptions. We also described how the FOV sampling strategy was incorporated into the FOV Connect-RRT. In Chapter 4, we provided a brief primer on the Kalman Filter and optimal state estimation. We then formulated an alternate perception-guided sampling strategy that fully simulated all sensor readings and calculated the information gain from each reading. Finally, we discussed filtering over an entire trajectory in order to obtain the final state estimate for when the robot reaches the target. In Chapter 5, we described our experimental setup and presented our results and analysis. We showed that the FOV Connect-RRT performed as well as the alternate Entropy RRT in finding trajectories that reduced the posterior covariance of the target 53 state. We also showed that the FOV Connect-RRT enjoyed a significant advantage in running time. 6.2 Further Work The research described above has suggested a number of ways to improve the performance and robustness of our method. There is also significant work to be done to adapt our planner for use on an actual mobile robot, such as the semi-autonomous robotic forklift that was the original inspiration for this project. 6.2.1 Improvements to Manipulation Planner 1. Additional Sensor Models Our current sampling strategy only takes into account the perceptive properties of limited-range LIDAR sensors. We would like to extend our technique to model other types of sensors, especially since the robotic forklift has multiple video cameras mounted to the roof. 2. Uncertain Target and Obstacle Positions Currently, we are assuming perfect prior knowledge of the environment, including the positions of the taget and any obstacles. In the future, we would like to handle situations where the environment is not completely known. We would like to plan in such a way as to balance exploration and target aquisition, analogous to SLAM or the Next-Best-View problem. 3. Online, Adaptive Replanning Right now, our method assumes a static environment with stationary obstacles, and simply returns the first complete path that it finds. If the planning module can be speeded up enough to allow replanning on-the-fly, then it might be possible to adapt the initial trajectory to a changing environment and dynamic obstacles such as pedestrians or other robots, all in real time. 54 6.2.2 Adapting for the Forklift 1. Measure More Variables Currently, the Kalman filter only tracks the horizontal positions and widths of the insert slots. It would be useful to track more variables, such as the vertical position and heights of the insert slots. This is tricky with a planar LIDAR scan however, so different sensors may have to be used. 2. Trajectory Smoothing Before being handed to the controller on the actual robotic forklift, the trajectories output by our RRT planner need to be smoothed. The inherent randomness of the RRT will sometimes produce wrinkled paths with jerky controls. The control inputs have to be made to satisfy the physical constraints of brake, gas pedal and steering mechanisms on the robot. 3. Feasibility/Safety Evaluation Related to the problem of trajectory smoothing, we would also like to be able to evaluate trajectories on their drivability and feasibility, i.e. would this be the path a human operator might reasonably choose to follow? Safety is also a top concern, as we want to avoid trajectories that feature sharp turns, sudden stops and close brushes with nearby obstacles. 55 56 Bibliography [1] J. Barraquand, B. Langlois, and J.-C. Latombe. Numerical potential field techniques for robot path planning. Systems, Man and Cybernetics, IEEE Transactions on, 22(2):224–241, 1992. [2] M. A. Baumann, D. C. Dupuis, S. Leonard, E. A. Croft, and J. J. Little. Occlusion-free path planning with a probabilistic roadmap. In Proc. IEEE/RSJ Int. Conf. Intelligent Robots and Systems, pages 2151–2156, 2008. [3] Eli Brookner. Tracking and Kalman Filtering Made Easy. John Wiley and Sons, 1998. [4] B. Burns and O. Brock. Sampling-based motion planning with sensing uncertainty. In Proc. IEEE Int. Conf. on Robotics and Automation, pages 3313–3318, 2007. [5] P. Cheng. Sampling-based motion planning with differential constraints. PhD thesis, University of Illinois at Urbana-Champaign, 2005. [6] Juan Cortes and Thierry Simeon. Sampling-based motion planning under kinematic loop-closure constraints. In 6th Int. Workshop on Algorithmic Foundations of Robotics, pages 59–74, 2004. [7] L. E. Dubins. On curves of minimal length with a contraint on average curvature, and with prescribed initial and terminal positions and tangents. American Journal of Mathematics, 79:497–516, 1957. [8] G. Morel F. Schramm, F. Geffard and A. Micaelli. Calibration free image point path planning simultaneously ensuring visibility and controlling camera path. In Proc. IEEE Int. Conf. on Robotics and Automation, pages 2074–2079, 2007. [9] L.J. Guibas, D. Hsu, H. Kurniawati, and E. Rehman. Bounded uncertainty roadmaps for path planning. In Proc. Int. Workshop on the Algorithmic Foundations of Robotics (WAFR), 2008. [10] R. J. He. Planning in information space for a quadrotor helicopter in a gps-denied environment. Master’s thesis, Massachusetts Institute of Technology, 2008. [11] Jr. J. J. Kuffner. Autonomous Agents for Real-time Animation. PhD thesis, Stanford University, 1999. 57 [12] J. Kim J. M. Esposito and V. Kumar. Adaptive rrts for validating hybrid robotic control systems. In International Workshop on the Algorithmic Foundations of Robotics, pages 107–132, 2004. [13] M. Kalisiak and M. van de Panne. Rrt-blossom: Rrt with a local flood-fill behavior. In Proc. IEEE Int. Conf. on Robotics and Automation, pages 1237– 1242, 2006. [14] R.E. Kalman. A new approach to linear filtering and prediction problems. Journal of Basic Engineering, 82(1:):35–45, 1960. [15] L. Kavraki, P. Svestka, J-C. Latombe, and M. H. Overmars. Probabilistic roadmaps for path planning in high dimensioanl configuration space. IEEE Journal of Robotics and Automation, 12(4):566–580, 1996. [16] J. J. Kuffner and S. M. LaValle. Rrt-connect: An efficient approach to singlequery path planning. In Proc. IEEE Int. Conf. on Robotics and Automation, pages 995–1001, 2000. [17] James Kuffner, Koichi Nishiwaki, Satoshi Kagami, Masayuki Inaba, and Hirochika Inoue. Robotics Research, volume 15 of Springer Tracts in Advanced Robotics, chapter Motion Planning for Humanoid Robots, pages 365–374. Springer, 2005. [18] Yoshiaki Kuwata, Gaston A. Fiore, Justin Teo, Emilio Frazzoli, and Jonathan P. How. Motion planning for urban driving using rrt. In Proc. IEEE/RSJ Int. Conf. on Intelligent Robots and Systems, pages 1681–1686, 2008. [19] S. M. LaValle. Rapidly-exploring random trees: A new tool for path planning. Technical report, Iowa State University, Computer Science Dept., 1998. [20] S. M. LaValle. Planning Algorithms. Cambridge University Press, 2006. [21] S. M. LaValle and J. J. Kuffner. Randomized kinodynamic planning. In Proc. IEEE Int. Conf. on Robotics and Automation, pages 473–479, 1999. [22] S. Leonard, E. A. Croft, and J. J. Little. Dynamic visibility checking for visionbased motion planning. In Proc. IEEE Int. Conf. on Robotics and Automation, pages 2283–2288, 2008. [23] N. A. Melchior, J. Kwak, and R. Simmons. Particle rrt for path planning with uncertainty. In Proc. IEEE Int. Conf. on Robotics and Automation, pages 1617– 1624, 2007. [24] P. Michel, C. Scheurer, J. J. Kuffner, N. Vahrenkamp, and R. Dillmann. Planning for robust execution of humanoid motions using future perceptive capability. In Proc. IEEE/RSJ Int. Conf. on Intelligent Robots and Systems, pages 3223–3228, 2007. 58 [25] Jose Neira and Juan D. Tards. Data association in stochastic mapping using the joint compatibility test. Robotics and Automation, IEEE Transactions on, 17(6):890–897, 2001. [26] J. Qu. Non-holonomic mobile robot mobile planning. http://msl.cs.uiuc. edu/~lavalle/cs576_1999/projects/junqu/index.html, 1999. [27] A. Shkolnik, M. Walter, and R. Tedrake. Reachability-guided sampling for planning under differential constraints. In Proc. IEEE Int. Conf. on Robotics and Automation, 2009. To appear. [28] Dan Simon. Optimal state estimation :Kalman, H-infinity and nonlinear approaches. Wiley-Interscience, 2006. [29] G. Welch and G. Bishop. An introduction to the kalman filter. Technical report, University of North Caroline at Chapel Hill, 2006. [30] A. Yershova and S. M. Lavalle. Motion planning for highly constrained spaces. Technical report, University of Illinois, Dept. of Computer Science, 2008. [31] Y. Yu and K. Gupta. An information theoretic approach to viewpoint planning for motion planning of eye-in-hand systems. In Proc. of Int. Symp. on Industrial Robotics, pages 306–311, 2000. 59