Planning and Navigation For Robots

6 - Planning and Navigation
Todays Outline
Path planning and navigation Visual SLAM
R. Siegwart, ETH Zurich - ASL
Autonomous Mobile Robots

Localization
Environment Model Local Map "Position" Global Map
Cognition
Path
Perception
Real World Environment
Motion Control
Planning and Navigation

Where am I? Where am I going? How do I get there?
Zrich
Autonomous Systems Lab

6
Competencies for Navigation I

is the ability to decide what actions are required to achieve a certain goal in a given situation (belief state). decisions ranging from what path to take to what information on the environment to use.
Cognition / Reasoning :
Todays industrial robots can operate without any cognition (reasoning) because their environment is static and very structured. In mobile robotics, cognition and reasoning is primarily of geometric nature, such as picking safe path or determining where to go next.
already been largely explored in literature for cases in which complete information about the current situation and the environment exists (e.g. sales man problem).

6
Competencies for Navigation II
However, in mobile robotics the knowledge of about the environment and situation is usually only partially known and is uncertain.
makes the task much more difficult requires multiple tasks running in parallel, some for planning (global), some to guarantee survival of the robot.
Robot control can usually be decomposed in various behaviors or functions

e.g. wall following, localization, path generation or obstacle avoidance.
In this chapter we are concerned with path planning and navigation, except
the low lever motion control and localization.
We can generally distinguish between (global) path planning and (local) obstacle avoidance.

6
Path Planning
The problem: find a path in the phisical space from the initial position to the goal position avoiding all collisions with the obstacles We can generally distinguish between
(global) path planning and (local) obstacle avoidance.

6
Global Path Planing
Assumption: there exists a good enough map of the environment for navigation.
Topological or metric or a mixture between both.
First step:
Representation of the environment by a road-map (graph), cells or a potential field. The resulting discrete locations or cells allow then to use standard planning algorithms.
Examples that we will see:

Visibility Graph Voronoi Diagram Cell Decomposition -> Connectivity Graph Potential Field

6
Path Planning: Configuration Space
State or configuration q can be described with k values qi
What is the configuration space of a mobile robot?

Work Space Configuration Space: the dimension of this space is equal to the Degrees of Freedom (DoF) of the robot

6
Configuration Space for a Mobile Robot
Mobile robots operating on a flat ground have 3 DoF: (x, y, ) For simplification, mobile roboticists assume that the robot is a point. In this way the configuration space is reduced to 2D (x,y) Because we have reduced each robot to a point, we have to inflate each obsttacle by the size of the robot radius to compensate.

6
Path Planning Overview

Identify a set of routes within the free space
1. Road Map, Graph construction 2. Cell decomposition

Discriminate between free and occupied cells
Where to put the nodes? Topology-based:

at distinctive locations
Where to put the cell boundaries? Topology- and metric-based:

where features disappear or get visible
Metric-based:
where features disappear or get visible
3. Potential Field
Imposing a mathematical function over the space

6
10
Road-Map Path Planning: Visibility Graph
Particularly suitable for polygon-like obstacles Shortest path length Grow obstacles to avoid collisions

6
11
Visibility Graph
It is easy to find the shortest path from the start to the goal positions Implementation simple when obstacles are polygons
Pros
Cons
Number of edges and nodes increases with the number of polygons Thus it can be inefficient in densely populated environments The solution path found by the visibility graph tend to take the robot asclose as possible to obstacles: the common solution is to grow obstacles by more than robots radius

6
12
Road-Map Path Planning: Voronoi Diagram
In contrast to the Visibility Graph approach tends to maximize the distance between robot and obstacles Easy executable: Maximize the sensor readings Works also for map-building: Move on the Voronoi edges Sysquake Demo

6
13
Voronoi Diagram
Using range sensors like laser or sonar, a robot can navigate along the Voronoi diagram using simple control rules
Pros
Cons
Because the Voronoi diagram tends to keep the robot as far as possible from obstacles, any short range sensor will be in danger of failing
Peculiarities
when obstacles are polygons, the Voronoi map consists of straight and parabolic segments

6
14
Road-Map Path Planning: Cell Decomposition
Divide space into simple, connected regions called cells Determine which open sells are adjacent and construct a connectivity graph Find cells in which the initial and goal configuration (state) lie and search for a path in the connectivity graph to join them. From the sequence of cells found with an appropriate search algorithm, compute a path within each cell. e.g. passing through the midpoints of cell boundaries or by sequence of wall following movements.
Possible cell decompositions:

Exact cell decomposition Approximate cell decomposition:
Fixed cell decomposition Adaptive cell decomposition

6
15
Road-Map Path Planning: Exact Cell Decomposition

6
16
Road-Map Path Planning: Approximate Cell Decomposition

6
17
Road-Map Path Planning: Adaptive Cell Decomposition

6
18
Road-Map Path Planning: Path / Graph Search Strategies
Wavefront Expansion NF1 (see also later)
Breadth-First Search
Depth-First Search
Greedy search and A*

6
19
Breadth-First Search
A B C D E B A C D E B A C F D E B A C F D G E G B A C F H D G E G B A C F H D G F I D E G I B A C F H D G F I
A B E G D I C F H K D G F I D E G I B
A C F H K C D G F H I D E G I B
A C F H K C D G F H I L D A E G I B
A C F H K C D G F H I L D A E G I L B
A C F H K C D G F H I L D A E G I L B
A C F H K L C D G F H I L D A E G I L B
A C F H K L C A D G F H I L
First path found! = optimal

A B E G D A I L C F H K L C A D G F H K I L D A E G I L B A C F H K L C A L D G F H I L
B A=initial
E F G H K
C D
L=goal

6
20
Depth-First Search
A B C D E B A C D E B A C F D E G B A C F H D D E G I B A C F H D A D E G I B A C F H D A D E G I L B A C F H D
First path found! NOT optimal
A B E G D A I L C F H K D A D E G I L B
A C F H K L D A D E G I L B
A C F H K L D A D G E G I L B
A C F H K L D G F I D A E G I L B
A C F H K L C D G F H I D A E G I L B
A C F H K L C A D G F H I D A E G I L B
A C F H K L C A D G F H K I
A B E G D A I L C F H K L C A D G F H K L I D A E G I L B
A C F H K L C A D G F H K L I L
B A=initial
E F G H K
C D
L=goal

6
21
Greedy Search
h(n) = straight-line distance

cost estimate of the cheapest path from state at node n to the goal

6
22
A*
f(n) = g(n) + h(n)

heuristic cost function f(n) is equal to the sum of the path cost g(n) to get from the start to node n and the straight-line distance h(n) from node n to the goal.

6
23
Potential Field Path Planning

Robot is treated as a point under the influence of an artificial potential field.
Generated robot movement is similar to a ball rolling down the hill Goal generates attractive force Obstacle are repulsive forces

6
24
Potential Field Path Planning: Potential Field Generation

attracting (goal) and repulsing (obstacle) fields summing up the fields functions must be differentiable
Generation of potential field function U(q)
Generate artificial force field F(q)
U x F (q) = U (q) = Uatt (q) Urep (q) = U y
Set robot speed (vx, vy) proportional to the force F(q) generated by the field
the force field drives the robot to the goal if robot is assumed to be a point mass

6
25
Potential Field Path Planning: Attractive Potential Field
Parabolic function representing the Euclidean distance to the goal
goal = q q goal
U att (q ) = =
1 2 k att goal (q) 2 1 k att (q q goal ) 2 2
Attracting force converges linearly towards 0 (goal)
U att (q) Fatt (q ) = = k att (q q goal )

6
26
Potential Field Path Planning: Repulsing Potential Field
Should generate a barrier around all the obstacle

strong if close to the obstacle not influence if fare from the obstacle
: minimum distance to the object Field is positive or zero and tends to infinity as q gets closer to the object
obst.

6
27
Potential Field Path Planning:

Local minima problem exists problem is getting more complex if the robot is not considered as a point mass If objects are convex there exists situations where several minimal distances exist can result in oscillations
Notes:
Sysquake Demo The Demo can also be seen at: http://www.k-team.com/kteam/index.php?page=174&rub=&site=1


6
28 Potential Field Path Planning: Additionally a rotation potential field and a task potential field in introduced Rotation potential field
force is also a function of robots orientation relative to the obstacles. This is done using a gain factor that reduces the repulsive force when obstacles are parallel to robots direction of travel
Extended Potential Field Method
Task potential field

Filters out the obstacles that should not influence the robots movements, i.e. only the obstacles in the sector in front of the robot are considered
Khatib and Chatila


6
29 Potential Field Path Planning: Forces in the polar plane
Potential Field using a Dyn. Model

Monatana et at.
no time consuming transformations
Robot modeled thoroughly

potential field forces directly acting on the model filters the movement -> smooth
Local minima
set a new goal point

6
30
Potential Field Path Planning: Using Harmonic Potentials

robot is moving similar to a fluid particle following its stream
Hydrodynamics analogy Ensures that there are no local minima
Note:
Complicated, only simulation shown
Autonomous Mobile Robots
Real Time 3D SLAM with a Single Camera
This presentation is based on the following papers: Andrew J. Davison, Real-Time Simultaneous Localization and Mapping with a Single Camera, ICCV 2003 Nicholas Molton, Andrew J. Davison and Ian Reid, Locally Planar Patch Features for Real-Time Structure from Motion, BMVC 2004
Zrich
Autonomous Systems Lab

6
32
Outline
Visual SLAM versus Structure From Motion Extended Kalman Filter for First-Order Uncertainty Propagation Camera Motion Model Matching of Existing Features Initialization of new features in the map Improving feature matching

6
33
Structure From Motion (SFM)
Structure from Motion: Take some images of the object to reconstruct Features (points, lines, ) are extracted from all frames and matched among them All images are processed simultaneously Both camera motion and 3D structure can be recovered by optimally fusing the overall information, up to a scale factor
Robust solution but far from being Real-Time !

6
34
SLAM: Simultaneous Localization and Mapping
(From Davison03) Can be Real-Time !


6
35
SLAM versus SFM
Repeatable Localization
Ability to estimate the location of the camera after 10 minutes of motion with the same accuracy as was possible after 10 seconds. Features must be stable, long-term landmark, no transient (as in SFM)

6
36 State
of the System
The State vector and the first-order uncertainty are:
Where:
In the SLAM approach the Covariance matrix is not diagonal, that is, the uncertainty of any feature affects the position estimate of all other features and the camera

6
37 Extended
Kalman Filter (EKF)
The state vector X and the Covariance matrix P are updated during camera motion using an EKF:
Prediction: a motion model is used to predict where the camera will be in the next time step (P increases)
?
Observation: a new feature is measured (P decreases)

6
38

A Motion Model for Smoothly Moving Camera

Attempts to predict where the camera will be in the next time step In the case the camera is attached to a person, the unknown intentions of the person can be statistically modeled Most Structure from Motion approaches did not use any motion model!

6
39
Constant Velocity Model
The unknown intentions, and so unknown accelerations, are taken into account by modeling the acceleration as a process of zero mean and Gaussian distribution:
By setting the covariance matrix of n to small or large values, we define the smoothness or rapidity of the motion we expect. In practice these values were used:
4 m/s 6 rad / s

6
40
Selection of Features already in the Map
By predicting the next camera pose, we can predict where each features is going likely to appear:

6
41
Selection of Features already in the Map
At each frame, the features occurring at previous step are searched in the elliptic region where they are expected to be according to the motion model (Normalized Sum of Square Differences is used for matching) Large ellipses mean that the feature is difficult to predict, thus the feature inside will provide more information for camera motion estimation Once the features are matched, the entire state of the system is updated to EKF according R. Siegwart, ETH Zurich - ASL

6
42
Initialization of New Features in the Database
The Shi-Tomasi feature is firstly initialized as a 3D line Along this line, 100 possible feature positions are set uniformly in the range 0.5-5 m A each time, each hypothesis is tested by projecting it into the image Each matching produces a likelihood for each hypothesis and their probabilities are recomputed

6
43
Map Management
The number of features to be constantly visible in the image varies (in practice between 610) according to Localization accuracy Computing power available If a feature is required to be added, the detected feature is added only if it is not expected to disappear from the next image A feature is deleted if it produces 50% of mismatches when it should be visible

6
44 Improved

Feature Matching
Up to now, tracked features were treated as 2D templates in image space Long-term tracking can be improved by approximating the feature as a locally planar region on 3D world surfaces

6
45
Locally Planar 3D Patch Features
The transformation between two view of a planar surface is described by a Homography H The motion model is used to predict the appearance of a feature in the new frame: The features templates are pre-warped before matching Then, gradient based image alignment is used to estimate the new surface normal

6
46
Locally Planar 3D Patch Features

6
47

References
A. J. Davison, Real-Time Simultaneous Localization and Mapping with a Single Camera, ICCV 2003 N. Molton, A. J. Davison and I. Reid, Locally Planar Patch Features for Real-Time Structure from Motion, BMVC 2004 A. J. Davison, Y. Gonzalez Cid, N. Kita, Real-Time 3D SLAM with wide-angle vision, IFAC Symposium on Intelligent Autonomous Vehicles, 2004 J. Shi and C. Tomasi, Good features to track, CVPR, 1994.

Planning and Navigation For Robots

Transféré par

Informations du document

Titre original

Copyright

Formats disponibles

Partager ce document

Partager ou intégrer le document

Options de partage

Avez-vous trouvé ce document utile ?

Ce contenu est-il inapproprié ?

Droits d'auteur :

Formats disponibles

Planning and Navigation For Robots

Transféré par

Droits d'auteur :

Formats disponibles

6 - Planning and Navigation

Path planning and navigation Visual SLAM

R. Siegwart, ETH Zurich - ASL

Autonomous Mobile Robots

Real World Environment