Robot Motion Planning in Dynamic, Cluttered,
Uncertain Environments
Noel E. Du Toit and Joel W. Burdick
California Institute of Technology, Pasadena, CA 91125
Email: {ndutoit, jwb}@robotics.caltech.edu
I. I NTRODUCTION
We present a strategy for planning robot motions in dynamic, cluttered, and uncertain environments (DCUEs) (e.g.,
a service robot which must move through a swarm of moving
humans in a cafeteria). Successful and efficient robot operation
in DCUEs requires reasoning about the future evolution and
uncertainties of the system states (including moving agents).
While the DCUE planning problem can be posed as a Stochastic Dynamic Program (SDP), the solution is often computationally intractable [1]. Alternatively, SDPs can be approximately
solved using Stochastic Receding Horizon Control (SRHC).
Previous implementations of SRHC are conservative. Our proposed algorithm accounts for future measurements during the
planning phase to reduce conservatism, allowing application
to complex, dynamic scenarios (including multimodal agent
behaviors, interactions, and information gathering) [2]. The
underlying probabilistic collision avoidance problem is also
considered [3].
II. PARTIALLY C LOSED -L OOP R ECEDING H ORIZON
C ONTROL
In classical SRHC, an approximate problem is solved and a
part of the solution is executed, before new measurements are
obtained and the problem is resolved (denoted the outer-loop
feedback mechanism). Formulating SRHC in terms of belief
states allows us to reason about the anticipated set of measurements. Classical SRHC approaches (e.g., [4, 5, 6]) assume that
no future measurements occur during plan execution (OpenLoop RHC, or OLRHC). The future belief states becomes
deterministic in the chosen control sequence, but the resulting
solutions are conservative (open-loop predicted distributions
are used, see Section III).
To solve SRHC while incorporating the influence of future information, we introduce Partially Closed-Loop RHC
(PCLRHC) [2], which assumes that the most likely measurement will occur. This is the least informative assumption about
the measurement value (proved via relative information entropy). The effect of the anticipated measurement (uncertainty
reduction) is captured, but the value of the measurement is
ignored (without introducing bias). The future belief states are
known for the chosen control sequence, but now these belief
states more accurately model the actual (executed) states. The
reduction in uncertainty reduces the solution conservatism (see
Sec. III).
(a) Constraints at stage k
(b) OLRHC constraints at k + 1
(c) OLRHC constraints beyond k + 1
(d) PCLRHC constraints at k + 1
(e) PCLRHC constraints beyond k+1
Fig. 1. The robot (blue) moves along a static obstacle (red). The safety buffer
around the robot (due to chance constraints) is given in green. (a) Open-loop
prediction over tr = 2. (b) and (c) Beyond stage k, OLRHC ignores the
anticipated measurement and uncertainty grows. (d) and (e) For PCLRHC,
anticipated measurements are incorporated and uncertainty remains bounded.
III. P ROBABILISTIC C OLLISION AVOIDANCE
For systems with probabilistically distributed states, constraints are imposed as probabilistic limits on constraint violation, P (c(x)0) ≤ δ. x is the system state and δ is the level of
confidence. The original constraint is adjusted as a function of
the uncertainty and level confidence. These constraints grow
as the uncertainty increases (see Fig. 1a).
In DCUEs, it is unrealistic to assume that the robot can
instantaneously react to changes in the environment (due to
kinematic and dynamic constraints). Chance constraint conditioning is analyzed to (probabilistically) guarantee system
safety [2]. Over some reaction horizon, tr , open-loop predicted
distributions are used to guarantee probabilistic safety, but
beyond this horizon the anticipated measurements help to
bound the growth in uncertainty (see Fig. 1 for tr = 2).
IV. S IMULATION R ESULTS
Assume disk objects (linear models, Gaussian noise) in a
planar environment [2]. OLRHC and PCLRHC results are
compared when the robot must: (i) skirt a static obstacle
(Fig. 2), and (ii) move between two oncoming agents (Fig. 3).
For (i), PCLRHC is less conservative (uncertainty growth is
managed) and the planned and executed trajectories are more
alike (anticipated information is efficiently used). OLRHC
Fig. 2. Comparison of the planned (dashed) and executed (solid) paths
produced by the OLRHC (green) and PCLRHC (blue) approaches in the
presence of a static obstacle (red)
Fig. 4.
OLRHC’s initially planned trajectory and the predicted agent
trajectories towards the two possible destinations
Fig. 3. Planned OLRHC and PCLRHC trajectories with two oncoming agents
(predicted trajectories in red, dashed lines). The anticipated 1-σ positional
uncertainty ellipses are plotted along the planned and predicted trajectories.
plans away from the static obstacle and relies on the outerloop feedback mechanism to execute a reasonable trajectory.
For (ii), PCLRHC plans between the agents, while the OLRHC
plans around them. The benefit of PCLRHC over OLRHC is
significant in some scenarios (this is verified more generally
with a Monte-Carlo simulation [2]).
The PCLRHC approach manages the growth in uncertainty
and can be applied in more cluttered, complicated scenarios,
including multi-modal agent behaviors (Fig. 4 and 5). Complex behavior (e.g., agents with multiple possible destinations
to their current paths) increases the effective clutter, and
PCLRHC is better able to find solutions. For OLRHC, the
additional uncertainty (from multiple destinations) prevents
the planner from finding reasonable solutions until the true
destination is distinguished.
For interactive scenarios (friendly, neutral, and adversarial
agents), PCLRHC is less conservative (yet safe) and is better
able to outmaneuver the agents. Also, PCLRHC considers the
quality of anticipated information and addresses the information gathering problem [2].
V. C ONCLUSION
PCLRHC incorporates future measurements in a SRHC
framework to reduce solution conservatism. The benefit of
Fig. 5.
PCLRHC’s initially planned trajectory and the predicted agent
trajectories towards the two possible destinations
PCLRHC over OLRHC is demonstrated in simulation, particularly in complicated dynamic scenarios. Multimodal agent
behaviors (multiple possible destinations and models), robotagent interaction, and information gathering examples are
presented.
R EFERENCES
[1] D.P. Bertsekas, Dynamic Programming and Optimal Control, 3rd ed., vol.
1, Athena Scientific, 2005
[2] N.E. Du Toit and J.W. Burdick, Robot Motion Planning in Dynamic,
Cluttered, Uncertain Environments, submitted IEEE Trans Robotics, 2010
[3] N.E. Du Toit Robot Motion Planning in Dynamic, Cluttered, Uncertain
Environments: the Partially Closed-Loop Receding Horizon Control Approach, Ph.D. dissertation, California Institute of Technology, 2010
[4] J. Yan and R.R. Bitmead, Incorporating State Estimation into Model
Predictive Control and its Application to Network Traffic Control, Automatica, vol. 41, no. 4, pp. 595-604, 2005
[5] D. van Hessem and O. Bosgra, Closed-loop Stochastic Model Predictive
Control in a Receding Horizon Implementation on a Continuous Polymerization Reaction Example, Proc. American Control Conf, vol. 1, pp.
914-919, 2004
[6] L. Blackmore, Robust Path Planning and Feedback Design Under
Stochastic Uncertainty, Proc. AIAA Guidance, Navigation, and Control
Conf, 2008