A Perception-Guided Approach to Motion and Manipulation Planning Yuan Wei

advertisement
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
Download