优化采样算法在路径规划中的应用与分析

需积分: 9 159 浏览量更新于2024-07-09 收藏 9.72MB PDF 举报

"Sampling-based Algorithms for Optimal Motion Planning.pdf" 这篇论文深入探讨了基于采样的运动规划算法在机器人路径规划中的应用。采样算法，如概率道路地图（Probabilistic Road Maps, PRM）和快速探索随机树（Rapidly-exploring Random Trees, RRT），在过去的十年中已被证明在实践中效果良好，并且具有理论上的保证，如概率完备性。然而，这些算法提供的解决方案的质量，尤其是在样本数量增加时如何影响解决方案的成本，尚未得到充分的正式分析。作者Sertac Karaman和Emilio Frazzoli指出，尽管现有的采样算法在实际操作中表现出色，但它们返回的解质量可能随着样本数量的增加而趋于非最优。论文中提供了若干负面结果，揭示了广泛应用的采样算法在某些技术条件下，其解决方案的成本几乎必然趋向于非最优值。论文的主要贡献在于引入了两种新的算法，即PRM* 和RRT*。这两种算法被证明是渐近最优的，意味着当样本数量增加时，它们找到的路径成本将趋近于全局最优解。这标志着对采样基础路径规划算法的重大改进，解决了之前算法可能无法保证找到最短或最低成本路径的问题。 PRM* 和RRT* 的设计考虑了优化采样策略和连接策略，以确保随着样本数量的增加，找到的路径将越来越接近全局最优。这些算法通过更有效地探索配置空间，减少了在非最优路径上花费的时间，提高了规划效率。此外，论文还详细讨论了这些新算法的理论基础，包括概率分析和收敛性质，为理解和应用这些算法提供了坚实的数学框架。通过对采样方法的深入理解，这些算法不仅在理论上提供了最优解保证，而且在实际的机器人运动规划问题中也具有高度的实用价值。这篇论文对基于采样的运动规划算法进行了深入的分析，揭示了现有算法的局限性，并提出了改进的算法以解决这些问题。这对于机器人学、自动控制和计算几何等领域具有重要意义，为未来的研究提供了新的方向，并为实际的机器人系统设计提供了优化工具。

parameter. However, there are no clear indications in the literature on the appropriate func-

tional relationship between r and n.

Rapidly-exploring Random Trees (RRT): The Rapidly-exploring Random Tree algorithm

is primarily aimed at single-query applications. In its basic version, the algorithm incrementally

builds a tree of feasible trajectories, rooted at the initial condition. An outline of the algorithm is

given in Algorithm 3. The algorithm is initialized with a graph that includes the initial state as its

single vertex, and no edges. At each iteration, a point x

rand

∈ X

free

is sampled. An attempt is made

to connect the nearest vertex v ∈ V in the tree to the new sample. If such a connection is successful,

rand

is added to the vertex set, and (v, x

rand

) is added to the edge set. In the original version

of this algorithm, the iteration is stopped as soon as the tree contains a node in the goal region.

In this paper, for consistency with the other algorithms (e.g., PRM), the iteration is performed n

times. In the absence of obstacles, i.e., if X

free

= X, the tree constructed in this way is an online

nearest neighbor graph.

Algorithm 3: RRT

1 V ← {x

init

}; E ← ∅;

2 for i = 1, . . . , n do

3 x

rand

← SampleFree

;

4 x

nearest

← Nearest(G = (V, E), x

rand

);

5 x

new

← Steer(x

nearest

, x

rand

) ;

6 if ObtacleFree(x

nearest

, x

new

) then

7 V ← V ∪ {x

new

}; E ← E ∪ {(x

nearest

, x

new

)} ;

8 return G = (V, E);

A variant of RRT consists of growing two trees, respectively rooted at the initial state, and

at a state in the goal set. To highlight the fact that the sampling procedure must not necessarily

be stochastic, the algorithm is also referred to as Rapidly-exploring Dense Trees (RDT) (LaValle,

2006).

3.3 Proposed algorithms

In this section, the new algorithms considered in this paper are presented. These algorithms

are proposed as asymptotically optimal and computationally eﬃcient versions of their “standard”

counterparts, as will be made clear through the analysis in the next section. Input and output data

are the same as in the algorithms introduced in Section 3.2.

Optimal Probabilistic RoadMaps (PRM

∗

): In the standard PRM algorithm, as well as in its

simpliﬁed “batch” version considered in this paper, connections are attempted between roadmap

vertices that are within a ﬁxed radius r from one another. The constant r is thus a parameter of

PRM. The proposed algorithm—shown in Algorithm 4—is similar to sPRM, with the only diﬀerence

being that the connection radius r is chosen as a function of n, i.e., r = r(n) := γ

PRM

(log(n)/n)

1/d

where γ

PRM

> γ

∗

PRM

= 2(1 + 1/d)

1/d

(µ(X

free

)/ζ

)

1/d

, d is the dimension of the space X, µ(X

free

)

denotes the Lebesgue measure (i.e., volume) of the obstacle-free space, and ζ

is the volume of the

unit ball in the d-dimensional Euclidean space. Clearly, the connection radius decreases with the

number of samples. The rate of decay is such that the average number of connections attempted

from a roadmap vertex is proportional to log(n).

Note that in the discussion of variable-radius PRM in LaValle (2006), it is suggested that the

radius be chosen as a function of sample dispersion. (Recall that the dispersion of a point set

contained in a bounded set S ⊂ R

is the radius of the largest empty ball centered in S.) Indeed,

the dispersion of a set of n random points sampled uniformly and independently in a bounded set

is O((log(n)/n)

1/d

) (Niederreiter, 1992), which is precisely the rate at which the connection radius

is scaled in the PRM

∗

algorithm.

Algorithm 4: PRM

∗

1 V ← {x

init

} ∪ {SampleFree

}

i=1,...,n

; E ← ∅;

2 foreach v ∈ V do

3 U ← Near(G = (V, E), v, γ

PRM

(log(n)/n)

1/d

) \ {v};

4 foreach u ∈ U do

5 if CollisionFree(v, u) then E ← E ∪ {(v, u), (u, v)}

6 return G = (V, E);

Another version of the algorithm, called k-nearest PRM

∗

, can be considered, motivated by the

k-nearest PRM implementation previously mentioned, whereby the number k of nearest neighbors

to be considered is not a constant, but is chosen as a function of the cardinality of the roadmap n.

More precisely, k(n) := k

PRM

log(n), where k

PRM

> k

∗

PRM

= e (1 + 1/d), and U ← kNearest(G =

(V, E), v, k

PRM

log(n)) \ {v} in line 3 of Algorithm 4.

Note that k

∗

PRM

is a constant that only depends on d, and does not otherwise depend on the

problem instance, unlike γ

∗

PRM

. Moreover, k

PRM

= 2e is a valid choice for all problem instances.

Rapidly-exploring Random Graph (RRG): The Rapidly-exploring Random Graph algo-

rithm was introduced as an incremental (as opposed to batch) algorithm to build a connected

roadmap, possibly containing cycles. The RRG algorithm is similar to RRT in that it ﬁrst at-

tempts to connect the nearest node to the new sample. If the connection attempt is successful, the

new node is added to the vertex set. However, RRG has the following diﬀerence. Every time a new

point x

new

is added to the vertex set V , then connections are attempted from all other vertices in

V that are within a ball of radius r(card (V )) = min{γ

RRG

(log(card (V ))/ card (V ))

1/d

, η}, where

η is the constant appearing in the deﬁnition of the local steering function, and γ

RRG

> γ

∗

RRG

2 (1 + 1/d)

1/d

(µ(X

free

)/ζ

)

1/d

. For each successful connection, a new edge is added to the edge set

E. Hence, it is clear that, for the same sampling sequence, the RRT graph (a directed tree) is a

subgraph of the RRG graph (an undirected graph, possibly containing cycles). In particular, the

two graphs share the same vertex set, and the edge set of the RRT graph is a subset of that of the

RRG graph.

Another version of the algorithm, called k-nearest RRG, can be considered, in which connections

are sought to k nearest neighbors, with k = k(card (V )) := k

RRG

log(card (V )), where k

RRG

∗

RRG

= e (1 + 1/d), and X

near

← kNearest(G = (V, E), x

new

, k

RRG

log(card (V ))), in line 7 of

Algorithm 5.

Note that k

∗

RRG

is a constant that depends only on d, and does not depend otherwise on the

problem instance, unlike γ

∗

RRG

. Moreover, k

RRG

= 2e is a valid choice for all problem instances.

Optimal RRT (RRT

∗

): Maintaining a tree structure rather than a graph is not only economical

in terms of memory requirements, but may also be advantageous in some applications, due to, for

instance, relatively easy extensions to motion planning problems with diﬀerential constraints, or

Algorithm 5: RRG

1 V ← {x

init

}; E ← ∅;

2 for i = 1, . . . , n do

3 x

rand

← SampleFree

;

4 x

nearest

← Nearest(G = (V, E), x

rand

);

5 x

new

← Steer(x

nearest

, x

rand

) ;

6 if ObtacleFree(x

nearest

, x

new

) then

7 X

near

← Near(G = (V, E), x

new

, min{γ

RRG

(log(card (V ))/ card (V ))

1/d

, η}) ;

8 V ← V ∪ {x

new

}; E ← E ∪ {(x

nearest

, x

new

), (x

new

, x

nearest

)} ;

9 foreach x

near

∈ X

near

10 if CollisionFree(x

near

, x

new

) then E ← E ∪ {(x

near

, x

new

), (x

new

, x

near

)}

11 return G = (V, E);

to cope with modeling errors. The RRT

∗

algorithm is obtained by modifying RRG in such a way

that formation of cycles is avoided, by removing “redundant” edges, i.e., edges that are not part

of a shortest path from the root of the tree (i.e., the initial state) to a vertex. Since the RRT and

RRT

∗

graphs are directed trees with the same root and vertex set, and edge sets that are subsets

of that of RRG, this amounts to a “rewiring” of the RRT tree, ensuring that vertices are reached

through a minimum-cost path.

Before discussing the algorithm, it is necessary to introduce a few new functions. Given two

points x

, x

∈ R

, let Line(x

, x

) : [0, s] → X denote the straight-line path from x

to x

. Given

a tree G = (V, E), let Parent : V → V be a function that maps a vertex v ∈ V to the unique vertex

u ∈ V such that (u, v) ∈ E. By convention, if v

∈ V is the root vertex of G, Parent(v

) = v

Finally, let Cost : V → R

≥0

be a function that maps a vertex v ∈ V to the cost of the unique

path from the root of the tree to v. For simplicity, in stating the algorithm we will assume an

additive cost function, so that Cost(v) = Cost(Parent(v)) + c(Line(Parent(v), v)), although this

is not necessary for the analysis in the next section. By convention, if v

∈ V is the root vertex of

G, then Cost(v

) = 0.

The RRT

∗

algorithm, shown in Algorithm 6, adds points to the vertex set V in the same way

as RRT and RRG. It also considers connections from the new vertex x

new

to vertices in X

near

, i.e.,

other vertices that are within distance r(card (V )) = min{γ

RRT

∗

(log(card (V ))/ card (V ))

1/d

, η}

from x

new

. However, not all feasible connections result in new edges being inserted in the edge set

E. In particular, (i) an edge is created from the vertex in X

near

that can be connected to x

new

along a path with minimum cost, and (ii) new edges are created from x

new

to vertices in X

near

, if

the path through x

new

has lower cost than the path through the current parent; in this case, the

edge linking the vertex to its current parent is deleted, to maintain the tree structure.

Another version of the algorithm, called k-nearest RRT

∗

, can be considered, in which con-

nections are sought to k nearest neighbors, with k(card (V )) = k

RRG

log(card (V )), and X

near

←

kNearest(G = (V, E), x

new

, k

RRG

log(i)), in line 7 of Algorithm 6.

4 Analysis

In this section, a number of results concerning the probabilistic completeness, asymptotic optimal-

ity, and complexity of the algorithms in Section 3 are presented.

The return value of Algorithms 1-6 is a graph. Since the sampling procedure SampleFree is

Algorithm 6: RRT

∗

1 V ← {x

init

}; E ← ∅;

2 for i = 1, . . . , n do

3 x

rand

← SampleFree

;

4 x

nearest

← Nearest(G = (V, E), x

rand

);

5 x

new

← Steer(x

nearest

, x

rand

) ;

6 if ObtacleFree(x

nearest

, x

new

) then

7 X

near

← Near(G = (V, E), x

new

, min{γ

RRT

∗

(log(card (V ))/ card (V ))

1/d

, η}) ;

8 V ← V ∪ {x

new

};

9 x

min

← x

nearest

; c

min

← Cost(x

nearest

) + c(Line(x

nearest

, x

new

));

10 foreach x

near

∈ X

near

do // Connect along a minimum-cost path

11 if CollisionFree(x

near

, x

new

) ∧ Cost(x

near

) + c(Line(x

near

, x

new

)) < c

min

then

12 x

min

← x

near

; c

min

← Cost(x

near

) + c(Line(x

near

, x

new

))

13 E ← E ∪ {(x

min

, x

new

)};

14 foreach x

near

∈ X

near

do // Rewire the tree

15 if CollisionFree(x

new

, x

near

) ∧ Cost(x

new

) + c(Line(x

new

, x

near

)) < Cost(x

near

)

then x

parent

← Parent(x

near

);

16 E ← (E \ {(x

parent

, x

near

)}) ∪ {(x

new

, x

near

)}

17 return G = (V, E);

stochastic, the returned graph is in fact a random variable.

Since the sampling procedure is

modeled as a map from the sample space Ω to inﬁnite sequences in X, sets of vertices and edges

of the graphs maintained by the algorithms can be deﬁned as functions from the sample space Ω

to appropriate sets. More precisely, let ALG be a label indicating one of the algorithms in Section

3, and let {V

ALG

(ω)}

i∈N

and {E

ALG

(ω)}

i∈N

be, respectively, the sets of vertices and edges in the

graph returned by algorithm ALG, indexed by the number of samples, for a particular realization

of the sample sequence. (In other words, these are sequences of functions deﬁned from Ω into ﬁnite

subsets of X

free

or X

free

× X

free

.) Similarly, let G

ALG

= (V

ALG

, E

ALG

). (The label ALG will be at

times omitted when the algorithm being used is clear from the context.)

All algorithms considered in the paper are sound, in the sense that they only return graphs

with vertices and edges representing points and paths in X

free

.This statement can be easily veriﬁed

by inspection of the algorithms in Section 3.

4.1 Probabilistic Completeness

In this section, the feasibility problem is considered, and the (probabilistic) completeness properties

of the algorithms in Section 3 are analyzed. First, some preliminary deﬁnitions are given, followed

by a deﬁnition of probabilistic completeness. Then, completeness properties of various sampling-

based motion planning algorithms are stated.

Let δ > 0 be a real number. A state x ∈ X

free

is said to be a δ-interior state of X

free

, if the closed

ball of radius δ centered at x lies entirely inside X

free

. The δ-interior of X

free

, denoted as int

free

is deﬁned as the collection of all δ-interior states, i.e., int

free

) := {x ∈ X

free

x,δ

⊆ X

free

}. In

We will not address the case in which the sampling procedure is deterministic, but refer the reader to LaValle et al.

(2004), which contains an in-depth discussion of the relative merits of randomness and determinism in sampling-based

motion planning algorithms.

剩余75页未读，继续阅读

alexzcp

粉丝: 3
资源: 9

优化采样算法在路径规划中的应用与分析

Incremental Sampling-based Algorithms for Optimal Motion Planning, Robotics

Optimal, Sampling-Based Manipulation Planning

介绍机器人路径规划的外文文献

Statistical knowledge outline

基于采样的优化路径规划算法（Sampling-Based Optimization），主要是基于RRT（Rapidly-exploring Random Tree）和其变种算法进行的

Python RRT 算法

python按指定概率抽样_概率加权的随机抽样 (Weighted Random Sampling) – A-Res 蓄水池算法...

FIR 带通滤波器的C语言实现

Informed RRT*算法书籍

最新资源