【免费】Sun等。-2015-High-FrequencyReplanningUnderUncertaintyUsin资源-CSDN文库

需积分: 0 31 浏览量 2022-08-04 16:16:19 上传评论收藏 734KB PDF 举报

资源详情

资源评论

资源推荐

104 IEEE TRANSACTIONS ON ROBOTICS, VOL. 31, NO. 1, FEBRUARY 2015

High-Frequency Replanning Under Uncertainty

Using Parallel Sampling-Based Motion Planning

Wen Sun, Student Member, IEEE, Sachin Patil, Member, IEEE, and Ron Alterovitz, Member, IEEE

Abstract—As sampling-based motion planners become faster,

they can be reexecuted more frequently by a robot during task

execution to react to uncertainty in robot motion, obstacle motion,

sensing noise, and uncertainty in the robot’s kinematic model. We

investigate and analyze high-frequency replanning (HFR) where,

during each period, fast sampling-based motion planners are ex-

ecuted in parallel as the robot simultaneously executes the ﬁrst

action of the best motion plan from the previous period. We con-

sider discrete-time systems with stochastic nonlinear (but lineariz-

able) dynamics and observation models with noise drawn from

zero mean Gaussian distributions. The objective is to maximize the

probability of success (i.e., avoid collision with obstacles and reach

the goal) or to minimize path length subject to a lower bound on

the probability of success. We show that, as parallel computation

power increases, HFR offers asymptotic optimality for these ob-

jectives during each period for goal-oriented problems. We then

demonstrate the effectiveness of HFR for holonomic and non-

holonomic robots including car-like vehicles and steerable medical

needles.

Index Terms—Motion and path planning, motion planning un-

der uncertainty, sampling-based methods.

I. INTRODUCTION

N many robotic tasks such as navigation and manipulation,

uncertainty may arise from a variety of real-world sources

such as unpredictable robot actuation, sensing errors, errors in

modeling robot motion, and unpredictable movement of ob-

stacles in the environment. The cumulative effect of all these

sources of uncertainty can be difﬁcult to model and account for

in the planning phase before task execution. We leverage the in-

creasingly fast performance of sampling-based motion planners

available for certain robots, combined with stochastic model-

ing, to enable these robots to quickly and effectively respond to

uncertainty during task execution.

In this paper, we consider tasks in which the objective is to

maximize the probability of success (i.e., avoid collision with

obstacles and reach the goal) or to minimize path length subject

to a lower bound on the probability of success. We consider

Manuscript received May 29, 2014; accepted December 6, 2014. Date of

publication January 29, 2015; date of current version February 4, 2015. This

paper was recommended for publication by Associate Editor D. Hsu and Editor

C. Torras upon evaluation of the reviewers’ comments. This work was supported

in part by the National Science Foundation under Awards IIS-1117127 and

IIS-1149965 and by the National Institutes of Health under Award R21EB-

017952.

W. Sun is with the Robotics Institute, Carnegie Mellon University, Pittsburgh,

PA 15213 USA (e-mail: [email protected]).

S. Patil is with University of California, Berkeley, CA 94709 USA (e-mail:

sachinpatil@berkeley.edu).

R. Alterovitz is with the Department of Computer Science, University of

North Carolina at Chapel Hill, NC 27599 USA (e-mail: [email protected]).

Color versions of one or more of the ﬁgures in this paper are available online

at http://ieeexplore.ieee.org.

Digital Object Identiﬁer 10.1109/TRO.2014.2380273

Fig. 1. We apply HFR to medical needle steering in the liver for a biopsy

procedure. The objective is to access the tumor (yellow), while avoiding the

hepatic arteries (red), hepatic veins (blue), portal veins (pink), and bile ducts

(green). To reach the tumor, the needle (white) must pass through several types

of tissue: muscle/fat tissue and then liver tissue. The maximum curvature of

the needle in the liver is not known aprioriand must be estimated during task

execution. (a) and (b) HFR in simulation successfully guided the needle to the

tumor by estimating online the needle’s maximum curvature and replanning with

high frequency. (c) Using an approach based on preplanning and LQG control

(LQG-MP), the needle veered away from the goal, and the LQG controller was

unable to correct the trajectory.

holonomic and nonholonomic robots with linear or nonlinear

(but linearizable) discrete-time dynamics and with motion and

sensing uncertainty that can be modeled as zero-mean Gaussian

distributions. We also assume that obstacle locations are known

or can be sensed. Our approach is applicable to robots for which

fast subsecond sampling-based motion planners are available,

including medical steerable needles [30], [31] (see Fig. 1).

We investigate and analyze the approach of high-frequency

replanning (HFR), where the robot executes a motion plan that

is updated periodically with high frequency by executing fast

sampling-based motion planners in parallel during each period.

The replanning computation occurs as the robot is executing

the current best plan. If any of the computed plans is better

See http://www.ieee.org/publications

standards/publications/rights/index.html for more information.

SUN et al.: HIGH-FREQUENCY REPLANNING UNDER UNCERTAINTY USING PARALLEL SAMPLING-BASED MOTION PLANNING 105

Fig. 2. Consider two paths from conﬁguration q



to a conﬁguration q



, with

ellipsoids representing uncertainty at stages along the paths. RRT* and related

asymptotically optimal motion planners assume that the optimal substructure

property holds, i.e., if q



is along the optimal path from q



to q



, the subpath

from q



to q



is itself an optimal path from q



to q



. While this is true for shortest

path problems, optimal substructure does not hold for some uncertainty-aware

cost metrics such as maximizing probability of success. Between q



and q



the probability of collision for the purple path is larger than the probability of

collision for the blue path due to lesser clearance from the obstacles, but the

purple plan has lesser estimated uncertainty in state estimation at q



, which

will lead to a smaller probability of collision when the robot passes through

the narrow passage between q



and q



. The lack of optimal substructure when

planning in conﬁguration space makes achieving asymptotic optimality more

difﬁcult.

than the plan currently being executed, then the current plan

is updated. To select the best plan, we focus on maximizing

probability of success or minimizing path length subject to a

lower bound on the probability of success. We note that for

these cost metrics, previous optimal motion planners such as

RRT* and related variants [17] cannot be used, since the op-

timal substructure property does not hold. This is because, as

shown in Fig. 2, the optimal plan from any conﬁguration to a

goal is dependent on the plan chosen to reach that conﬁguration

[42]. This substantially complicates the motion planning chal-

lenge for the uncertainty-aware cost metrics considered in this

paper.

HFR combines the beneﬁts of global motion planning with the

responsiveness of a controller. By executing multiple sampling-

based rapidly exploring random tree (RRT) [21] motion plan-

ners in parallel, under certain reasonable assumptions, HFR

will compute at each period a motion plan that asymptotically

approaches the globally optimal plan as computation power (i.e.,

processor speed and number of cores) increases. For practical

problems with ﬁnite computational resources, we add a heuris-

tic to HFR to consider newly generated plans as well as the

best plan from the prior period adjusted based on the latest sen-

sor measurements and a linear feedback controller. Asymptotic

optimality in each period of HFR comes not from generating

more conﬁguration samples in a single RRT but rather from

generating many independent RRTs (where each RRT termi-

nates when it ﬁnds a plan and launches a new RRT to begin

construction). Another beneﬁt of HFR is that it automatically

handles control input bounds, since the sampling-based motion

planner enforces kinematic and dynamic constraints during plan

generation. HFR only applies to goal-oriented problems that do

not require returning to previously explored regions of the state

space for information gathering and, hence, does not address the

general partially observable Markov decision process (POMDP)

problem [20].

This paper makes three main contributions. First, we show

that generating multiple RRTs and selecting the one that opti-

mizes the chosen metric is asymptotically optimal at each period

under reasonable assumptions. Asymptotic optimality is impor-

tant for many applications that require high-quality solutions

and gives the user conﬁdence that increasing the computational

hardware applied to the problem will result in better and better

solutions that improve toward optimality. Second, we present

an approach for handling uncertainty by using HFR that con-

siders the impact of future uncertainty and uses a heuristic to

efﬁciently adjust the prior best plan based on the latest sensor

measurements. Third, we experimentally show that HFR is ef-

fective for many problems with uncertainty in motion, sensing,

and kinematic model parameters. Evaluation scenarios include

a holonomic disc robot navigating in a dynamic environment

with moving obstacles whose motion are not known apriori,

a nonholonomic car-like robot maneuvering with uncertainty

in motion and sensing, and a nonholonomic steerable medical

needle [39] maneuvering through tissues around anatomical ob-

stacles to clinical targets under uncertainty in motion, sensing,

and kinematic modeling.

II. R

ELATED WORK

Motion planning under uncertainty has received consider-

able attention in recent years. Approaches that blend planning

and control by deﬁning a global control policy over the entire

environment have been developed using Markov decision pro-

cesses (e.g., [2]) and POMDPs (e.g., [20]). These approaches

are difﬁcult to scale, and computational costs may prohibit their

application to robots navigating using nondiscrete controls in

uncertain dynamic environments. Sampling-based approaches

that consider uncertainty [1], [4], [13], [26], [37] or approaches

that compute a locally optimal trajectory and an associated con-

trol policy (in some cases in belief space) [32], [36], [41], [43],

[46] are effective for a variety of scenarios. However, these

methods are currently not suitable for real-time planning in un-

certain, dynamic, and changing environments where during task

execution, the path may need to change substantially potentially

across different homotopic classes. Approaches have also been

developed to efﬁciently estimate the probability of collision of

plans (and associated control policies) under Gaussian models

of uncertainty [8], [33], [42].

An alternative to precomputing a control policy is to contin-

uously replan to account for changes in the robot state or the

environment. Several approaches have been suggested for plan-

ning in dynamic environments [14], [19], [34], [45], [48], [49],

but they do not explicitly account for robot motion and sensing

uncertainty. Majumdar and Tedrake [25] use a predeﬁned library

of motion primitives, which depending on the application can

be effective or restrictive. Our work is also related to model-

predictive control (MPC) approaches [38], which account for

state and control input constraints in an optimal control formu-

lation. HFR can be viewed as a type of MPC where the goal

is always within the horizon and global optimality is achieved

(in an asymptotic sense) at each time step using a sampling-

based motion planner. In this regard, Du Toit [8] introduced a

106 IEEE TRANSACTIONS ON ROBOTICS, VOL. 31, NO. 1, FEBRUARY 2015

new method for planning under uncertainty in dynamic environ-

ments by solving a nonconvex optimization problem, although

this method for some problems is computationally expensive

and suffers from local minima. In contrast, HFR is based on

an efﬁcient global motion planner and takes uncertainty into

account, resulting in higher probabilities of successful plan ex-

ecution.

Asymptotically optimal motion planners such as RRT* and

related variants [11], [16], [17], [23] return plans that converge

toward optimality for cost functions that satisfy certain criteria.

In this paper, we focus on cost metrics based on probability of

success, for which the optimal substructure property does not

hold, e.g., the optimal solution starting from any robot conﬁg-

uration is not independent of the prior history of the plan (see

Fig. 2). Consequently, RRT* and related variants cannot guaran-

tee asymptotic optimality for our problem without modiﬁcation.

CC-RRT* [24] utilizes RRT* to generate asymptotically opti-

mal motion plans that satisfy user-deﬁned chance constraints.

Each node of the CC-RRT* tree explicitly stores a sequence of

states and uncertainty distributions of the prior path that leads

to the node, enabling optimal substructure. However, unlike our

approach, CC-RRT* considers different optimization objectives

and propagates the robot’s state distribution forward in time only

using the stochastic system dynamics. In HFR, we explicitly

consider probability of success and grow the uncertainty distri-

butions in a closed-loop manner using linear quadratic Gaussian

(LQG) control and an extended Kalman ﬁlter (EKF).

HFR takes advantage of the fact that the speed and effec-

tiveness of sampling-based planning algorithms [6], [22] have

improved substantially over the past decade due to algorith-

mic improvements and innovations in hardware. One source

of speedup has been parallelism, including the use of multi-

ple cores or processors (see, e.g., [5], [7], [10], [15], [28], and

[35]) and GPUs (see, e.g., [3] and [29]) to achieve speedups for

single-query sampling-based motion planners (e.g., RRT and

RRT*). Wedge and Branicky showed that periodically restart-

ing sampling-based tree construction can improve the mean and

the variability of the runtime of RRT [47]. Our method extends

the OR RRT approach [5], [7] to solve a class of problems with

asymptotic optimality rather than solely feasibility. In addition,

unlike prior approaches that leverage parallelism, we explicitly

focus on motion planning problems that involve uncertainty in

robot motion and sensing and moving obstacles.

III. P

ROBLEM STATEMENT

Let q ∈Q⊂R

denote the robot state which consists of pa-

rameters over which the robot has control, such as the robot’s

position, orientation, and velocity. Let x ∈X⊂R

,n≥ l, de-

note the system state which includes the robot state and also

relevant parameters that can be sensed, such as obstacle posi-

tions and velocities, as well as robot-speciﬁc parameters over

which the robot does not have direct control, such as robot kine-

matic parameters (e.g., a steerable needle’s maximum curvature

in a particular tissue). Note that the robot’s state q is a subset

of the whole system’s state x. We deﬁne the initial state of the

system as x

We assume that continuous time τ is discretized into periods

of equal duration Δ such that the tth period begins at time τ =

tΔ. A motion plan π is then deﬁned by a sequence of discrete

control inputs π = {u

: t =0,...,T}, where u

∈U⊂R

is a control drawn from the space of permissible control inputs.

Starting from x

and applying a control sequence π with number

of steps T , the state of the robot at time τ, where 0 ≤ τ ≤ T Δ,

is expressed as x(τ,π). For simplicity, we deﬁne x

= x(τ,π),

when τ = tΔ.

The whole system evolves according to a stochastic dynamics

model

= f(x

t−1

, u

t−1

, m

) (1)

where m

models the cumulative uncertainty, including uncer-

tainty in robot and obstacle motion. We assume that m

is from

a Gaussian distribution m

∼ N(0, M

) with variance M

During task execution, the robot obtains sensor measurements

according to the stochastic observation model

= h(x

, n

) (2)

where z

is the vector of measurements, and n

is the noise which

is from a Gaussian distribution n

∼ N(0, N

) with variance

In addition, our method also requires as input an estimate of

the initial system state

with corresponding variance

cost function c(π, x) that deﬁnes the expected cost of a motion

plan π from an initial system state x, the period duration Δ,a

set of obstacles O that may move over time, and a goal region

G such that x

∈Gsigniﬁes success at time step t. We consider

two optimization objectives.

1) Maximize the probability of success, i.e., the probability

of avoiding collision with obstacles and reaching the goal.

We deﬁne the cost function c(π, x) as the negative of the

probability of success estimated for plan π.

2) Minimize path length subject to a lower bound on the

probability of success. We deﬁne c(π, x) as the length of

the nominal plan resulting from executing π if the path

satisﬁes the chance constraint (i.e., a lower bound on the

estimated probability of success) and as ∞ if the plan

violates the chance constraint.

Our approach aims to compute and execute a control input u

at each time step based on the chosen optimization objective.

IV. A

PPROACH

We outline HFR in Algorithm 1. The outer loop of the HFR

algorithm (lines 3–17) runs at a high frequency with a period

of Δ to address uncertainty in robot motion, obstacle motion,

and the robot’s kinematic model. In each period, the bulk of

the computation is computing a motion plan. In each period, the

robot estimates the system state using a ﬁlter, updates the current

motion plan with a better motion plan if a better plan is found

(as determined by the optimization objective), and then executes

the ﬁrst control input of the current best plan. We assume that

剩余12页未读，继续阅读

评论收藏

内容反馈

史努比狗狗

粉丝: 27
资源: 318

Sun 等。 - 2015 - High-Frequency Replanning Under Uncertainty Usin

评论0

最新资源

Sun 等。 - 2015 - High-Frequency Replanning Under Uncertainty Usin

评论0

Decision Making Under Uncertainty

微观经济英文课件 chapter_5 Choice Under Uncertainty.ppt

Reasoning under Uncertainty for Overlay Fault Diagnosis

Economics003 Choice Under Uncertainty.pdf

PITFALLS OF IN-DOMAIN UNCERTAINTY ESTIMATION AND ENSEMBLING IN DEEP LEARNING

Portfolio selection with coherent Investor’s expectations under uncertainty.pdf

Design, Analysis and Test of Logic Circuits Under Uncertainty, 2013.pdf

Uncertainty-Aware Cascaded Dilation Filtering for High-Efficienc

High-Precision, Consistent EKF-based Visual-Inertial Odometry.pdf

ISO IEC GUIDE 98-3Suppl.2 2011 Uncertainty of measurement - Part 3 Guide to the expression of uncertainty in measurement (GUM).pdf

uncertainty in deep learning

IE598NH-lecture-19-Decision_Rule_Approach_for_Optimization_under_Uncertainty.pdf

不确定性量化的时空降阶建模_Space-time reduced-order modeling for uncertainty

2021.7.20_NeurIPS-2020-introducing-routing-uncertainty

论文研究-A Novel Mechanism in MPLS Network under Adversarial Uncertainty.pdf

Uncertainty in Economic Analysis and the Economic Analysis of Uncertainty

ISO IEC GUIDE 98-3 SP 1-2008 Uncertainty of measurement - Part 3 Guide to the expression of uncertainty in measurement (GUM1995) - Propagation of distributions using a Monte Carlo method.pdf

Uncertainty-Driven 6D Pose Estimation

BurpLoaderKeygen.jar.zip

最新版ISO/IEC 27001:2022、ISO 27002:2022中英文合集

Goby红队版-win-x64-2.4.7版本

Chrome Header Editor 插件

ISO SAE 21434-2021 中文版.pdf

OpenVAS GVM 中文翻译补丁

安全认证cisp教材全套

STM32F103C8T6核心板-电路原理图1.PDF

软件工程导论(第六版)课后习题答案1

goby红队&社区版-win-64-2.4.7

最新资源