Transcript An Extension to the Dynamic Window Approach for arbitrarily shaped Robots

An Extension to the Dynamic Window Approach
for arbitrarily shaped Robots
Christian Mandel A1[RoboMap], 09/17/04
An Extension to the Dynamic Window Approach for arbitrarily shaped Robots.
Overview
• Rolland: new hardware platform
• Motivation
• Basic principles of the Dynamic Window Approach
• DWA & the problem with non circular shaped robots
• Implementation issues: Curve Segments Table, Collision Table
• Computing the Trajectory
• Computing the Velocity Profile
• What remains to do
• Preliminary Experiments
SFB/IQN-Kolloquium 09/17/04
-0-
A1[RoboMap]
An Extension to the Dynamic Window Approach for arbitrarily shaped Robots.
• Rolland: hardware platform
 Meyra Champ wheelchair
 omnidirectional camera system
 controlling laptop connected via
single usb cable
 emergency stop button
 2 laser range finder
 2 incremental encoders
SFB/IQN-Kolloquium 09/17/04
-1-
A1[RoboMap]
An Extension to the Dynamic Window Approach for arbitrarily shaped Robots.
A1[RoboMap]
Motivation
decision points
voronoi edges
Wheelchair in its environment
Metrical & topological representation.
How to navigate between decision points while taking
care of dynamic obstacles?
SFB/IQN-Kolloquium 09/17/04
-2-
An Extension to the Dynamic Window Approach for arbitrarily shaped Robots.
A1[RoboMap]
Basic principles of the Dynamic Window Approach1
• Local navigation combined with reactive collision avoidance.
• DWA assumes: Robot velocity is a piecewise constant function in time.
• DWA considers: Robot has initial velocity and limited accelerations.
• DWA computes optimal circular arc in every time step.
• DWA looks one curve ahead.
1
[Fox, Burgard, Thrun] „The Dynamic Window Approach To Collision Avoidance“
SFB/IQN-Kolloquium 09/17/04
-3-
An Extension to the Dynamic Window Approach for arbitrarily shaped Robots.
A1[RoboMap]
DWA & the problem with non circular shaped robots
current pose
computed arc
headed pose
current pose
collision
computed arc
current pose
computed arc2
computed arc1
SFB/IQN-Kolloquium 09/17/04
-4-
An Extension to the Dynamic Window Approach for arbitrarily shaped Robots.
A1[RoboMap]
Curve Segments Table
SFB/IQN-Kolloquium 09/17/04
-5-
start
curvature
curvature
prime
direction
poses
0
+ MAX
backwards
( x0 , y0 , 0 ),...
0
- MAX
forwards
( x0 , y0 , 0 ),...
0
+ MAX
forwards
( x0 , y0 , 0 ),...
0
- MAX
backwards
( x0 , y0 , 0 ),...
0
0
forwards
( x0 , y0 , 0 ),...
0
0
backwards
( x0 , y0 , 0 ),...
...
...
...
...
An Extension to the Dynamic Window Approach for arbitrarily shaped Robots.
A1[RoboMap]
Collision Table
arclength (t)
0
...
offset between
occupied
cells
occupied
cells ({(x
1,y1
1),...,(x
n,yn
n)})
1
n
({(x1,y1),...,(xn,yn)})
{(-1,1),(-1,-1),(1,1),(1,-1),(2,1),(2,-1)}
{(0,-1),(1,0),(0,1),(1,0),(0,-1),(1,-1),
{(-1,1),(-1,-1),(1,1),(1,-1),(2,1),(2,-1),
(0,1),(1,0),(0,-1)}
(3,-1),(3,-2),(4,-1),(4,-2)}
...
t=8
t=7
t=6
t=5
t=0
SFB/IQN-Kolloquium 09/17/04
t=1
t=2
t=3
t=4
-6-
An Extension to the Dynamic Window Approach for arbitrarily shaped Robots.
A1[RoboMap]
Algorithmic Refinements 1
Goal:
reduce Size of
Precompute and store only paths whose first pose
has zero heading.
Curve Segments Table
Test-for-Collision-Operation has to rotate CT-entries.
Collision Table
Precompute additional table which holds rotated
offsets between occupied cells.
SFB/IQN-Kolloquium 09/17/04
-7-
An Extension to the Dynamic Window Approach for arbitrarily shaped Robots.
Score Function
A1[RoboMap]
startPose
headedPose

d
goalPose
path


score ( path, startPose, headedPose)  d  c1 *  c2 * path.length
SFB/IQN-Kolloquium 09/17/04
-8-
An Extension to the Dynamic Window Approach for arbitrarily shaped Robots.
Computing the Optimal Path
Input
Algorithm (basic idea)
Local Evidence Grid,
Odometry Pose,
Goal Pose
Curve Segments Table
Collision Table
SFB/IQN-Kolloquium 09/17/04
-9-
A1[RoboMap]
An Extension to the Dynamic Window Approach for arbitrarily shaped Robots.
A1[RoboMap]
Algorithmic Refinements 2
Goal:
reduce complexity
of computation
Computing the
optimal path w.r.t.
objective function
SFB/IQN-Kolloquium 09/17/04
Algorithm (basic
(with constant
idea) arc2.length)
set arc1.curvature,
arc2.length = MAX_arc2.length
arc1.length, arc1.direction do

arc1.length, arc1.direction do
 arc1.curvature,
arc2.curvature,
arc2.length, arc2.direction do

arc2.direction do
 arc2.curvature,
• construct path
•
•
•
•
•
•
•
construct path
test for collision and prune if necessary
test for collision and prune if necessary
calc score
minimise path.score
if (path.score < bestPath.score)
if set
(path.score
bestPath<=bestPath.score)
path
set bestPath = path
- 10 -
An Extension to the Dynamic Window Approach for arbitrarily shaped Robots.
Algorithmic Refinements 3
Algorithm
(witch constant
arc2.length)
minimise
path.score
 pose
 arc2
potential
goalPose
do
• arc2
calcdo
score with
goalPose
• calc
score = pose
storegoalPose
goalPose
•• store
withminimal
minimalscore
score
with
SFB/IQN-Kolloquium 09/17/04
- 11 -
A1[RoboMap]
An Extension to the Dynamic Window Approach for arbitrarily shaped Robots.
A1[RoboMap]
Computing the Velocity Profile
Input
Algorithm
FOR EVERY posei
 path DO:
v
COMPUTE MAXIMUM velocity i
vi  min( vmax ,
FOR EVERY
Solution Path
path  { pose0 ,..., posen }
vi
amax

,
)
posei .c posei .c
DO:
INCORPORATE LONGITUDINAL ACCELERATION LIMIT
Velocities in Start & Goal
vstart , v goal
Lateral Acceleration Limit
amax
Longitudinal Acceleration Limit

vmax
Rotational Velocity Limit
max
Longitudinal Velocity Limit
vmax
SFB/IQN-Kolloquium 09/17/04
FOR WHICH HOLDS:
- 12 -
vi:
An Extension to the Dynamic Window Approach for arbitrarily shaped Robots.
What remains to do
•
current implementation considers only binary
information from the evidence grid while doing
the collision test
•
better: collision test should also return
minimal distance to obstacles for tested path
score ( path, startPose, headedPose) 

d  c1 *  c2 * path.length  c3 * distToObst acles
SFB/IQN-Kolloquium 09/17/04
- 13 -
A1[RoboMap]
An Extension to the Dynamic Window Approach for arbitrarily shaped Robots.
Preliminary Experiments
SFB/IQN-Kolloquium 09/17/04
- 14 -
A1[RoboMap]