没有合适的资源?快使用搜索试试~ 我知道了~
LaValle 和 Kuffner - 2001 - Randomized Kinodynamic Planning1
需积分: 0 1 下载量 84 浏览量
2022-08-04
15:16:21
上传
评论
收藏 1.8MB PDF 举报
温馨提示
试读
23页
LaValle 和 Kuffner - 2001 - Randomized Kinodynamic Planning1
资源详情
资源评论
Steven M. LaValle
Department of Computer Science
Iowa State University
Ames, IA 50011
lavalle@cs.iastate.edu
James J. Kuffner, Jr.
Department of Mechano-Informatics
University of Tokyo
Bunkyo-ku, Tokyo, Japan
kuffner@jsk.t.u-tokyo.ac.jp
Randomized
Kinodynamic
Planning
Abstract
This paper presents the first randomized approach to kinodynamic
planning (also known as trajectory planning or trajectory design).
The task is to determine control inputs to drive a robot from an initial
configuration and velocity to a goal configuration and velocity while
obeying physically based dynamical models and avoiding obstacles
in the robot’s environment. The authors consider generic systems
that express the nonlinear dynamics of a robot in terms of the robot’s
high-dimensional configuration space. Kinodynamic planning is
treated as a motion-planning problem in a higher dimensional state
space that has both first-order differential constraints and obstacle-
based global constraints. The state space serves the same role as the
configuration space for basic path planning; however, standard ran-
domized path-planning techniques do not directly apply to planning
trajectories in the state space. The authors have developed a ran-
domized planning approach that is particularly tailored to trajectory
planning problems in high-dimensional state spaces. The basis for
this approach is the construction of rapidly exploring random trees,
which offer benefits that are similar to those obtained by success-
ful randomized holonomic planning methods but apply to a much
broader class of problems. Theoretical analysis of the algorithm
is given. Experimental results are presented for an implementation
that computes trajectories for hovercrafts and satellites in cluttered
environments, resulting in state spaces of up to 12 dimensions.
KEY WORDS—motion planning, trajectory planning, non-
holonomic planning, algorithms, collision avoidance
1. Introduction
There is a strong need for a general purpose, efficient plan-
ning technique that determines control inputs to drive a robot
The International Journal of Robotics Research
Vol. 20, No. 5, May 2001, pp. 378-400,
©2001 Sage Publications
from an initial configuration and velocity to a goal configu-
ration and velocity while obeying physically based dynami-
cal models and avoiding obstacles in the robot’s environment
(see Fig. 1). In other words, a fundamental task is to design
a feasible open-loop trajectory that satisfies both global ob-
stacle constraints and local differential constraints. We use
the words kinodynamic planning, introduced in Donald et al.
(1993), to refer to such problems. (In nonlinear control liter-
ature, kinodynamic planning for underactuated systems is en-
compassed by the definition of nonholonomic planning. Us-
ing control-theoretic terminology, we characterize our work
as open-loop trajectory design for nonlinear systems with drift
and nonconvex state constraints. Other terms include trajec-
tory planning and trajectory design.) These solutions would
be valuable in a wide variety of applications. In robotics, a
nominal trajectory can be designed for systems such as mo-
bile robots, manipulators, space robots, underwater robots,
helicopters, and humanoids. This trajectory can be used to
evaluate a robot design in simulation, or as a reference tra-
jectory for designing a feedback control law. In virtual pro-
totyping, engineers can use these trajectories to evaluate the
design of many mechanical systems, possibly avoiding the
time and expense of building a physical prototype. For ex-
ample, in the automotive industry, the planning technique can
serve as a “virtual stunt driver” that determines whether a
proposed vehicle can make fast lane changes or can enter a
dangerous state such as toppling sideways. In the movie and
game industries, advanced animations can be constructed that
automate the motions of virtual characters and devices while
providing realism that obeys physical laws. In general, such
trajectories may be useful in any application area that can be
described using control theoretic models, from analog circuits
to economic systems.
The classic approach in robotics research has been to de-
couple the general robotics problem by solving basic path
378
LaValle and Kuffner / Randomized Kinodynamic Planning 379
Fig. 1. We consider planning problems with dynamic con-
straints induced by physical laws. The above image shows
the state exploration trees computed for a rigid rectangular
object (bottom left). The goal location is represented by a
sphere (upper right).
planning and then finding a trajectory and controller that
satisfies the dynamics and tracks the path (Bobrow,
Dubowsky, and Gibson 1985; Latombe 1991; Shiller and
Dubowsky 1991). The vast majority of basic path-planning
algorithms considers only kinematics while ignoring the sys-
tem dynamics entirely. In this paper, we consider kinody-
namic planning as a generalization of holonomic and nonholo-
nomic planning in configuration spaces by replacing popu-
lar configuration-space notions (Lozano-Perez 1983) by their
state-space (or phase-space) counterparts. A point in the state
space may include both configuration parameters and velocity
parameters (i.e., it is the tangent bundle of the configuration
space).
It may be the case that the result of a purely kinematic
planner will be unexecutable by the robot in the environment
because of limits on the actuator forces and torques. Impreci-
sion in control, which is always present in real-world robotic
systems, may require explicitly modeling system dynamics to
guarantee collision-free trajectories. Robots with significant
dynamics are those in which natural physical laws, along with
limits on the available controls, impose severe constraints on
the allowable velocities at each configuration. Examples of
such systems include helicopters, airplanes, certain classes
of wheeled vehicles, submarines, unanchored space robots,
and legged robots with fewer than four legs. In general, it
is preferable to look for solutions to these kinds of systems
that naturally flow from the physical models, as opposed to
viewing dynamics as an obstacle.
These concerns provide the general basis for kinodynamic
planning research. One of the earliest algorithms was pre-
sented in Sahar and Hollerbach (1986), in which minimum-
time trajectories were designed by tessellating the joint space
of a manipulator and performing a dynamic programming-
based search that uses time-scaling ideas to reduce the search
dimension. Algebraic approaches solve for the trajectory
exactly, although the only known solutions are for point
masses with velocity and acceleration bounds in one dimen-
sion (O’Dunlaing 1987) and two dimensions (Canny, Rege,
and Reif 1991). Dynamic programming-based algorithms
that provide approximately optimal solution trajectories were
introduced in Donald et al. (1993). Other papers have ex-
tended or modified this technique (Donald and Xavier 1995a,
1995b; Heinzinger et al. 1990; Reif and Wang 1997). A
dynamic programming-based approach to kinodynamic plan-
ning for all-terrain vehicles was presented in Cherif (1999).
Ferbach (1996) performed a state-space search using an incre-
mental, variational approach. An approach to kinodynamic
planning based on Hamiltonian mechanics was presented in
Connolly, Grupen, and Souccar (1995). An efficient approach
to kinodynamic planning was developed by adopting a sensor-
based philosophy that maintains an emergency stopping path
that accounts for robot inertia (Shkel and Lumelsky 1997).
Although several kinodynamic planning approaches exist,
they are limited to either low-degree-of-freedom problems
or particular systems that enable simplification. Randomized
techniques have led to efficient, incomplete planners for basic
holonomic path planning; however, there appears to be no
equivalent technique for the broader kinodynamic planning
problem (or even nonholonomic planning in the configuration
space). We present a randomized approach to kinodynamic
planning that quickly explores the state space and scales well
for problems with high degrees of freedom and complicated
system dynamics. Our ideas build on a large foundation of
related research, which is briefly presented in Section 2.
Section 3 defines the problem. Our proposed planning
approach is based on the concept of rapidly exploring random
trees (RRTs) (LaValle 1998b) and is presented in Section 4.
To demonstrate the utility of our approach, a series of planning
experiments for hovercrafts in
2
and spacecrafts in
3
are
presented in Section 5. Theoretical analysis of the planner’s
performance is given in Section 6. Finally, some conclusions
and directions for future research are provided in Section 7.
2. Other Related Research
2.1. Dynamic Programming
For problems that involve low degrees of freedom, classical
dynamic programming ideas can be employed to yield nu-
merical optimal control solutions (Bellman 1957; Bertsekas
1975; Larson and Casti 1982; LaValle 1998a). Because con-
trol theorists have traditionally preferred feedback solutions,
the representation often takes the form of a mesh over which
cost-to-go values are defined using interpolation, enabling in-
puts to be selected over any portion of the state space. If open-
loop solutions are the only requirement, then each cell in the
380 THE INTERNATIONAL JOURNAL OF ROBOTICS RESEARCH / May 2001
mesh could be replaced by a vertex that represents a single
state within the cell. In this case, the control-theoretic numer-
ical dynamic programming technique can often be reduced to
the construction of a tree grown from an initial state (Larson
1967). This idea has been proposed in path-planning liter-
ature for nonholonomic planning (Barraquand and Latombe
1991a; Lynch and Mason 1996) and kinodynamic planning
(Cherif 1999; Donald et al. 1993). Because these methods are
based on dynamic programming and systematic exploration
of a grid or mesh, their application is limited to problems with
low degrees of freedom.
2.2. Steering Methods
The steering problem has received considerable attention in
recent years. The task is to compute an open-loop trajectory
that brings a nonholonomic system from an initial state to a
goal state without the presence of obstacles. Given the gen-
eral difficulty of this problem, most methods apply to purely
kinematic models (i.e., systems without drift or momentum).
For a kinematic car that has a limited turning radius and moves
forward only, it was shown that the shortest path between any
two configurations belongs to one of a family of six kinds
of curves comprising straight lines and circular arcs (Dubins
1957). For a car that can move forward or backward, opti-
mal solutions comprising 48 curve types have been obtained
(Boissonnat, Cérézo, and Leblond 1994; Reeds and Shepp
1990; Sussman and Tang 1991). For more complicated kine-
matic models, nonoptimal steering techniques have been in-
troduced, which include for example a car pulling trailers
(Murray and Sastry 1993) and fire trucks (Bushnell, Tilbury,
and Sastry 1995). Techniques also exist for general system
classes, such as nilpotent (Laffierriere and Sussman 1991),
differentially flat (Murray, Rathinam, and Sluis 1995; Fliess
et al. 1993), and chained form (Bushnell, Tilbury, and Sastry
1995; Murray and Sastry 1993; Struemper 1997). For sys-
tems with drift and/or obstacles, the steering problem remains
a formidable challenge.
2.3. Nonholonomic Planning
The nonholonomic planning problem was introduced in Lau-
mond (1987) and has blossomed into a rich area of research
in recent years. Rather than surveying this large body of re-
search, we refer the reader to recent, detailed surveys (Duleba
1998; Laumond, Sekhavat, and Lamiraux 1998). Most cur-
rent approaches to nonholonomic planning rely on the exis-
tence of steering methods that can be used in combination with
holonomic motion-planning techniques. Other approaches
exploit particular properties or very special systems (espe-
cially kinematic car models). For most nonholonomic sys-
tems, it remains a great challenge to design efficient path-
planning methods.
2.4. Lower Bounds
Kinodynamic planning in general is at least as difficult as
the generalized mover’s problem, which has been proven to
be PSPACE-hard (Reif 1979). Hard bounds have also been
established for time-optimal trajectories. Finding an exact
time-optimal trajectory for a point mass with bounded ac-
celeration and velocity moving amid polyhedral obstacles in
three dimensions has been proven to be NP-hard (Donald et al.
1993). The need for simple, efficient algorithms for kinody-
namic planning, along with the discouraging lower bound
complexity results, has motivated us to explore the develop-
ment of randomized techniques for kinodynamic planning.
This parallels the reasoning that led to the success of random-
ized planning techniques for holonomic path planning.
2.5. Randomized Holonomic Planning
It would certainly be useful if ideas could be borrowed or
adapted from existing randomized path-planning techniques
that have been successful for basic, holonomic path planning.
For the purpose of discussion, we choose two different tech-
niques that have been successful in recent years: randomized
potential fields (e.g., Barraquand and Latombe 1991b; Chal-
lou et al. 1995) and probabilistic roadmaps (e.g., Amato and
Wu 1996; Kavraki et al. 1996). In the randomized potential
field approach, a heuristic function is defined on the config-
uration space that attempts to steer the robot toward the goal
through gradient descent. If the search becomes trapped in
a local minimum, random walks are used to assist in escape.
In the probabilistic roadmap approach, a graph is constructed
in the configuration space by generating random configura-
tions and attempting to connect pairs of nearby configurations
with a local planner. Once the graph has been constructed,
the planning problem becomes one of searching a graph for
a path between two nodes. If an efficient steering method
exists for a particular system, then it is sometimes possible
to extend randomized holonomic planning techniques to the
case of nonholonomic planning (Svestka and Overmars 1995;
Sekhavat et al. 1997; Sekhavat et al. 1998).
2.6. Drawing Inspiration from Previous Work
Inspired by the success of randomized path-planning tech-
niques and Monte Carlo techniques in general for addressing
high-dimensional problems, it is natural to consider adapt-
ing existing planning techniques to our problems of inter-
est. The primary difficulty with existing techniques is that
although they are powerful for standard path planning, they
do not naturally extend to general problems that involve dif-
ferential constraints. The randomized potential field method
(Barraquand, Langlois, and Latombe 1992), while efficient
for holonomic planning, depends heavily on the choice of
a good heuristic potential function, which could become a
daunting task when confronted with obstacles and differential
LaValle and Kuffner / Randomized Kinodynamic Planning 381
constraints. In the probabilistic roadmap approach (Amato
and Wu 1996; Kavraki et al. 1996), a graph is constructed in
the configuration space by generating random configurations
and attempting to connect pairs of nearby configurations with
a local planner that will connect pairs of configurations. For
planning of holonomic systems or steerable nonholonomic
systems (see Laumond, Sekhavat, and Lamiraux 1998), the
local planning step might be efficient; however, in general the
connection problem can be as difficult as designing a non-
linear controller, particularly for complicated nonholonomic
and dynamical systems. The probabilistic roadmap technique
might require the connections of thousands of configurations
or states to find a solution, and if each connection is akin to
a nonlinear control problem, it seems impractical for solving
problems with differential constraints. Furthermore, the prob-
abilistic roadmap is designed for multiple queries. The un-
derlying theme in that work is that it is worthwhile to perform
substantial precomputation on a given environment to enable
numerous path-planning queries to be solved efficiently.
In our approach, we are primarily interested in answering a
single query efficiently without any preprocessing of the envi-
ronment. In this case, the exploration and search are combined
in a single method without substantial precomputation that is
associated with a method such as the probabilistic roadmap.
This idea is similar to classical artificial intelligence search
techniques, the Ariadne’s clew algorithm for holonomic plan-
ning (Mazer, Ahuactzin, and Bessière 1998), and the related
holonomic planners in Hsu, Latombe, and Motwani (1999)
and Yu and Gupta (1998).
To directly handle differential constraints, we would like
to borrow some of the ideas from numerical optimal control
techniques while weakening the requirements enough to ob-
tain methods that can apply to problems with high degrees of
freedom. As is common in most of path-planning research,
we forgo an attempt to obtain optimal solutions and attempt to
find solutions that are “good enough” as long as they satisfy
all of the constraints. This avoids the use of dynamic pro-
gramming and systematic exploration of the space; however,
a method is needed to guide the search in place of dynamic
programming. These concerns have motivated our develop-
ment of RRTs (LaValle 1998b) and the proposed planning
algorithm.
2.7. Recent Advances
This paper is an expanded version of LaValle and Kuffner
(1999). Since that time, several interesting developments
have occurred. The ideas contained in Frazzoli, Dahleh, and
Feron’s (1999) paper were applied to the problem of designing
trajectories for a helicopter flying among polyhedral obsta-
cles. Substantial performance benefits were obtained by using
a metric based on the optimal cost-to-go for a hybrid nonlin-
ear system that ignores obstacles. In Toussaint, Ba¸sar, and
Bullo (2000), H
∞
techniques and curve fitting were applied
to yield an efficient planner that builds on ideas from LaValle
and Kuffner (1999). A randomized kinodynamic planning
algorithm was proposed recently for the case of time-varying
environments in Kindel et al. (2000). A deterministic kinody-
namic planning algorithm based on collocation was presented
recently in Faiz and Agrawal (2000).
3. Problem Formulation: Path Planning in the
State Space
We formulate the kinodynamic planning problem as path
planning in a state space that has first-order differential con-
straints. For kinodynamic planning, the state space will serve
the same purpose as the configuration space for the classi-
cal path-planning problem. Let C denote the configuration
space (C-space) that arises from a rigid or articulated body
that moves in a two- or three-dimensional world. Each con-
figuration q ∈ C represents a transformation that is applied to
a geometric model of the robot. Let X denote the state space
in which a state, x ∈ X, is defined as x = (q, ˙q). The state
could include higher order derivatives if necessary, but such
systems are not considered in this paper.
3.1. Differential Constraints
When planning in C, differential or nonholonomic constraints
often arise from the presence of one or more rolling contacts
between rigid bodies or from the set of controls that it is possi-
ble to apply to a system. When planning in X, nonholonomic
constraints also arise from conservation laws (e.g., angular
momentum conservation). Using Lagrangian mechanics, the
dynamics can be represented by a set of equations of the form
h
i
( ¨q, ˙q,q) = 0. Using the state-space representation, this
can be simply written as a set of m implicit equations of the
form g
i
(x, ˙x) = 0, for i = 1,... ,m and m<2n, in which
n is the dimension of C. It is well-known that under appro-
priate conditions, the implicit function theorem allows the
constraints to be expressed as
˙x = f (x, u), (1)
in which u ∈ U, and U represents a set of allowable controls
or inputs. Equation (1) effectively yields a convenient param-
eterization of the allowable state transitions via the controls
in U.
The proposed approach will require a numerical approxi-
mation to (1). Given the current state, x(t), and inputs applied
over a time interval, {u(t
) | t ≤ t
≤ t + t}, the task is to
compute x(t + t). This can be achieved using a variety
of numerical integration techniques. For example, assuming
constant input, u, over the interval [t, t+t ), a standard form
of fourth-order Runge-Kutta integration yields
x
= f
x(t) +
t
2
f (x(t), u), u
382 THE INTERNATIONAL JOURNAL OF ROBOTICS RESEARCH / May 2001
x
= f
x(t) +
t
2
x
,u
x
= f
x(t) +
t
2
x
,u
and
x(t + t) ≈ x(t) +
t
6
(f (x(t), u) + 2x
+ 2x
+ x
).
For many applications, (1) might not be available. For
example, motions might be generated from a complicated dy-
namical simulation package that considers collisions, flexible
parts, vehicle dynamics, finite element analysis, and so on.
For these cases, our techniques can be directly applied with-
out requiring (1). The only requirement is that an incremental
simulation of the system be generated for any current state and
input.
3.2. Obstacles in the State Space
Assume that the environment contains static obstacles and
that a collision detection algorithm can determine efficiently
whether a given configuration is in collision. It may even be
assumed that an entire neighborhood around a configuration is
collision free by employing distance computation algorithms
(Lin and Canny 1991; Mirtich 1997; Quinlan 1994). There are
interesting differences between finding collision-free paths in
C versus in the state space, X. When planning in C,itis
useful to characterize the set C
obst
of configurations at which
the robot is in collision with an obstacle (or itself) (Latombe
1991). The path-planning problem involves finding a contin-
uous path that maps into C
free
= C \ C
obst
. For planning in
X, this could lead to a straightforward definition of X
obst
by
declaring x ∈ X
obst
if and only if q ∈ C
obst
for x = (q, ˙q).
However, another interesting possibility exists: the region of
inevitable collision. Let X
ric
denote the set of states in which
the robot is either in collision or, because of momentum, can-
not do anything to avoid collision. More precisely, a state
lies in X
ric
if there exist no inputs that can be applied over
any time interval to avoid collision. Note that X
obst
⊆ X
ric
.
Thus, it might be preferable to define X
free
= X \ X
ric
as
opposed to X \ X
obst
.
The region of inevitable collision, X
ric
, provides some
intuition about the difficulty of kinodynamic planning over
holonomic and purely kinematic nonholonomic planning.
Figure 2 illustrates conservative approximations of X
ric
for
a point mass robot that obeys Newtonian mechanics without
gravity. The robot is assumed to have L
2
-bounded accelera-
tion and an initial velocity pointing along the positive x-axis.
As expected intuitively, if the speed increases, X
ric
grows.
Ultimately, the topology of X \ X
ric
may be quite distinct
from the topology of X \ X
obst
, which intuitively explains
part of the challenge of the kinodynamic planning problem.
Even though there might exist a kinematic collision-free path,
a kinodynamic trajectory might not exist. For the remainder
of the paper, we assume that X
free
= X \ X
obst
(one could
alternatively define X
free
= X \ X
ric
).
3.3. A Solution Trajectory
The kinodynamic planning problem is to find a trajectory
from an initial state x
init
∈ X to a goal state x
goal
∈ X
or goal region X
goal
⊂ X. A trajectory is defined as a time-
parameterized continuous path τ :[0,T]→X
free
that sat-
isfies the nonholonomic constraints. Recall from (1) that the
change in state is expressed in terms of an input, u. A more
convenient way to formulate the problem is to find an input
function u :[0,T]→U that results in a collision-free tra-
jectory that starts at x
init
and ends at x
goal
or X
goal
. The
trajectory, x(t) for t ∈[0,T], is determined through the inte-
gration of (1). It might also be appropriate to select a path that
optimizes some criterion, such as the time to reach x
goal
.Itis
assumed that optimal solutions are not required; however, in
this paper we assume that there is some loose preference for
better solutions. A similar situation exists in the vast majority
of holonomic planning methods.
3.4. Why Does the Problem Present Unique Challenges?
The difference between X and C is usually a factor of 2 in di-
mension. The curse of dimensionality has already contributed
to the success and popularity of randomized planning methods
for C-space; therefore, it seems that there would be an even
greater need to develop randomized algorithms for kinody-
namic planning. One reason that might account for the lack
of practical, efficient planners for problems in X-space is that
attention is usually focused on obtaining optimal solutions
with guaranteed deterministic convergence. The infeasibility
of this requirement for generic high-dimensional systems has
led many researchers to adopt a decoupled approach in which
classical motion planning is performed and trajectory design
is optimized around a particular motion-planning solution.
Another reason randomized kinodynamic planning ap-
proaches have not appeared is that kinodynamic planning is
considerably harder owing to momentum. Consider adapting
randomized holonomic planning techniques to the problem of
finding a path in X
free
that also satisfies (1) instead of find-
ing a holonomic path in C
free
. The potential field method
appears to be well suited to the problem because a discrete-
time control can repeatedly be selected that reduces the poten-
tial. The primary problem is that dynamical systems usually
have drift, which could easily cause the robot to overshoot the
goal, leading to oscillations. Without a cleverly constructed
potential function (which actually becomes a difficult nonlin-
ear control problem), the method cannot be expected to work
well. Imagine how often the system will be pulled into X
ric
.
The problem of designing a good heuristic function becomes
extremely complicated for the case of kinodynamic planning.
剩余22页未读,继续阅读
安全验证
文档复制为VIP权益,开通VIP直接复制
信息提交成功
评论0
最新资源