机器人技术 第5讲 路径规划和避障

⎧ ⎛ 1 1 ⎞ 1 ∂ρ ⎪Kr ⎜ − ⎟ 2 Frep (x) = −∇U rep (x) = ⎨ ⎝ ρ ρ0 ⎠ ρ ∂x ⎪ 0 ⎩
∂ρ ⎛ ∂ρ =⎜ ∂x ⎝ ∂x x0
T
ρ ≤ ρ0 ρ > ρ0
x − x0 ∂ρ ⎞ ⎟ = ∂y ⎠ ρ
•
/
F (x) = −∇U (x) = −∇U att (x) − ∇U rep (x) = Fatt (x) + Frep (x)
! ! !
Fr
1
2
Ft F
•
/
/ !
(Ar8ficial!Poten8al!Field)!
– tential Fields
⎧ K a | x − xd |2 | x − xd |≤ d a U att (x) = ⎨ 2 K (2 d | x − x | − d a d a ) | x − x d |> d a ⎩ a
( 1) , ,pj
.
(p 1 , p 2 ,
•
PSO . p :p Optimization ! .
j j
, pm )
Particle Swarm
,
vi = w * vi
+ c1 ∗ rand () *( pibestgB −est. pi ) + c2 ∗ rand () *( g best − pi )
!
24!!!!70 24!!60 24!! 50 28!!!40
38!!30
• •
2Knorm
!
ManhaZan!distance,!Block !distance,!1Knorm L1!distance ! Chebychev!distance,!infinity ! Minkowski
• r !norm •
qnew qnew
/ – – Fail0
qnew
4 1
0
qnew
qgoal
3
Success0
– 0
Success
qinit
qgoal
n times. In the absence of obstacles, i.e. if Xfree = X , the tree constructed in this way is an online nearest neighbor graph. Algorithm 3: RRT.
t+ 1 t
m
,
v i, j = Xv i, j + c1 c2 r 2
t+ 1 t t (p g ,j
$
x i, j = x i, j + v i,
t+
: v ti, j , x ti, j
PRM(Probabilis8c!Roadmap)
! • • • • • • / roadmap q (q1,q2) 1 !d(q1,q2)! ! G(V,E) ! ! ! Roadmap,!V E
g s
19
PRM(Probabilis8c!Roadmap) Probabilistic Roadmap (PRM)
在 RM中搜索一条从起始点到终止点的路径 The PRM is searched for a path from s to g
g s
20
PRM(Probabilis8c!Roadmap)
/!
1 3 0!
Voronoi!diagram
• • / / ! ) )
•
– 1 –
!
1 1 0 0 – )
•
– –
• !
•
–
/
1 –
•
–
/
•
– 2 6 !
!
•
–
!
3 !
•
–
/!
/
Ft 5-4 5-5
•
/!
5-6 Fr1 Fr2
– – –
无碰撞a态成为图节点 The collision-free configurations are retained as milestones
15
PRM(Probabilis8c!Roadmap) Probabilistic Roadmap (PRM)
每个图节点和其最近相邻的 k个节点直P连接 Each milestone is linked by straight paths to its nearest neighbors
13
PRM(Probabilis8c!Roadmap) Probabilistic Roadmap (PRM)
对采样的a态进行碰撞检测 Sampled configurations are tested for collision
14
PRM(Probabilis8c!Roadmap) Probabilistic Roadmap (PRM)
!
! —
• Roland!Siegwart!and!Illah!R.!Nourbakhsh.!! Introduc8on!to!Autonomous!Mobile!Robots.! The!MIT!Press,!2004.!
2
• Where!am!I!?!!( • Where!am!I!going!?!!( • How!do!I!get!there!?!( ?)! ?)! ?)
16
PRM(Probabilis8c!Roadmap) Probabilistic Roadmap (PRM)
保留无碰路径为图的边 Each milestone is linked by straight paths to its nearest neighbors
17
PRM(Probabilis8c!Roadmap) Probabilistic Roadmap (PRM)
RRT(RapidKExploring!Random!Tree)
•
– –
• • u 41 3
/!
qinit
4
1
/
qnear
qrand qnew0
x
qnear
qnear
qrand
qrand
0
= f ( x, u) x
RRT(RapidKExploring!Random!Tree)
• •
Ka x xd da
Figure 1: Attractive linear apf
•
– –
/
/
Figure 1: / Attractive linear apf
(Ar8ficial!Poten8al!Field)!
⎧ 1 ⎛ 1 1 ⎞2 ⎪ Kr ⎜ − ⎟ U rep (x) = ⎨ 2 ⎝ ρ ρ0 ⎠ ⎪ 0 ⎩
h(n)f(n)
Y
44 7
2 tg<
openlist g(n)
N
Y 8 N
Y
44 74tg4>=4 tg=
closelist g(n) g(n)4+4
openlist closelist7
94!
!
84!
!
74!
!
68!
!
24!!!!70 24!!60 24!! 50 28!!!40 74!
!
60!
在a态M间坐标系中随机取点 Configurations are sampled by picking coordinates at random
12
PRM(Probabilis8c!Roadmap) Probabilistic Roadmap (PRM)
Configurations are sampled by picking coordinates at random 在a态M间坐标系中随机取点
2
d h(d)=4.5 3 h(e)=2 a e f(a)=5.5 b b h(b)=2 2 f(b)=5.5 c f(c)=10.5 4 f=7 d f(d)=6.5 e f(e)=7
1.5 a h(a)=4 2
3 c h(c)=4
4 2 openlist7 7 8 g(n)=tg,
N
Openlist g(n),h(n),f(n) Openlist7 f(n)
• 2Knorm
!
x−y 2 =
2 ( x − y ) ∑ i i i
x = ( x1 , x2 ,…, xn ) y = ( y1 , y2 ,…, yn )
• ManhaZan!distance!
x − y 1 = ∑ xi − yi
i
6
8 8 8
r
• !Chebychev!distance! x − y ∞ = max xi − yi
ρ ≤ ρ0 ρ > ρ0
ρ ρ0
Figure 2: Repulsive linear apf
•
–
/
⎧ −2 K a (x − x d ) | x − x d |≤ d a ⎪ Fatt (x) = −∇U att (x) = ⎨ x − xd − 2 K d | x − xd |> d a a a ⎪ | x − xd | ⎩
!
54!
!
4!!!!60 10!!50 60!
! !
4!!!!40 40! 0!!!!30 62!
!
56!
!
10!!50 74!
!
52!!10 56!! 62!
!
0
60!
!
54!
!
62!
!
4!!!!60 10!!50 94!
!
4!!!!40 74!
!
42!!20 52!!!!10 68!
!
84!
!
68!
1 2 3 4 5 6 7 8
RRT(RapidKExploring!Random!Tree) 1 V
2 3 4 5
A
f
V ← {xinit }; E ← ∅; for i = 1, . . . , n do xrand ← SampleFreei ; xnearest ← Nearest( G =( V , E) , xrand ); xnew ← Steer( xnearest , xrand ) ; if ObtacleFree( xnearest , xnew ) then V ← V ∪ {xnew }; E ← E ∪ {( xnearest , xnew ) } ; return G =( V , E); A variant of RRT consists of growing two trees, respec-
i
(Minkowski)
•
Minkowski (x, y ) = k
k
k
k | x − y | ∑ i i i
k
k = 1, k = 2, k → ∞,
(
O2 XY
,S .
,G
.
3
3. 1 PSO PSO ( v 2x )
PSO
P = {S , p 1 , p 2 ,
, p m , G },
•
/ 1 1 1 / !
•
Visibility graph
Voronoi diagram
•
– –
!
•
– –
/!
! !
•
– –
/!
2 ! 3 ! !
Voronoi!diagram
• / !
Voronoi!diagram
•
– – 0! – ! ! Voronoi!diagram! !
PRM(Probabilis8c!Roadmap) Probabilistic Roadmap (PRM)
Space ℜn forbidden space Free/feasible space
11
PRM(Probabilis8c!Roadmap) Probabilistic Roadmap (PRM)
构成R由a态M间中的Roadmap The collision-free links are retained as local paths to form the PRM
18
PRM(Probabilis8c!Roadmap) Probabilistic Roadmap (PRM)
加入起始点和终止点 The start and goal configurations are included as milestones
?
• • • •
/
1
!
/
• 1 •
/ 4 /
1
9
•
• •
– – – –
• 路径规划
1
• •
/ /
1
• • •
(Configura8on!space)
• •
– –
! /!
6 ! !
• 1 •
–
! /
7 3
!
!
•
–
/
7
!
/ 1
•
•
1 !
•
Baidu Nhomakorabea
1 !
Fr 2 Ft F 2 Fr 1
•
/
42
• NPKcomplete •
–
1 ! !
8Dijkstra !
/ !
–
• A*,!D*,!Focused!D*
•
!
!
–
•
!
!
A*
• !
– 3 4 41 41
f (n) = g (n) + h(n)
n g(n) h(n)
–
/ / /
–
• •
resolution completeness/
–
Probabilistic completeness:
•
– – – / / /
/!
•
– PRM – RRT
/!
Probabilis8c!RoadMaps ! RapidKExploring!Radom!Trees! !