Embed Size (px)
Citation preview
RoboticsRobot Navigation
Tullio Facchinetti<[email protected]>
Wednesday 16th October, 2019
http://robot.unipv.it/toolleeo
Maps
a map is a data structure that represents theenvironment where the robot (or a generic point)
can move
• it represents an important asset for path planning andlocalization
• it is useful for planning more than one trajectory in the sameenvironment
• the mapping is the incremental process that builds a mapusing the information gathered from sensors
Different types of mapping
topological mapping
• the representation is based on graphs
• nodes represent relevant points in the environment (e.g.,crossroads)
• edges determine the adjacency between nodes
• scale may be ignored, paths rectified
Different types of mapping
topological mapping
• the representation is based on graphs
• nodes represent relevant points in the environment (e.g.,crossroads)
• edges determine the adjacency between nodes
• scale may be ignored, paths are rectified
Different types of mapping
geometrical mapping
• the representation uses geometrical primitives
• the environment is modeled as a set of lines or, in3-dimensional spaces, as a set of triangles
Different types of mapping
occupancy grids
• grids are made by adjacent cells having adequate shapes
• for each cell, a flag (boolean value 0/1) indicates whether anobstacle occupies the cell
Different types of mapping
occupancy grids
• grids are made by adjacent cells having adequate shapes
• for each cell, a flag (boolean value 0/1) indicates whether anobstacle occupies the cell
Different types of mapping
occupancy grids
• grids are made by adjacent cells having adequate shapes
• for each cell, a flag (boolean value 0/1) indicates whether anobstacle occupies the cell
Graphs
Basics about graphs
• made by n nodes V1, . . . ,Vn ((V)ertex)
• nodes are connected by m edgesE1, . . . ,Em
some notions
• path: succession of nodes connected by edges
• oriented graph: edges have an orientation (arrow)
• acyclic graph: no closed circuits are present in the graph,i.e., there are no paths starting from and getting back to thesame node going through distinct nodes
• connected graph: for each pair of nodes, there is a pathconnecting it
The tree
a tree is a non-oriented connected acyclic graph
The visit of a tree
the visit consists in examining (visiting) thenodes of a graph to search a node
associated with the desired information
• the application of graphs to the robot navigation, thus to themotion from a starting point to the goal, uses a visit togenerate the path to follow
• the searched node is the goal
Depth-first search (post-order)
all childs are visited with a depth-first search,before visiting the parent node
Depth-first search (pre-order)
the parent node is visited first, then childs arevisited in depth-first order
Visibility maps
the map is based on a visibility graph
nodes
• the start location and the goal
• all the vertices of obstacles
edges
• there is an edge from node v to node w i.i.f.
∀λ ∈ [0, 1] : λv + (1− λ)w ∈ Qfree
Visibility graph: example
Visibility graph: example
Reduced visibility graph
a visibility graph may include many redundant,non necessary edges
non necessary edges can be eliminated considering some peculiarfeatures:
1 segments of support
2 segments of separation
Reduced visibility graph
all the edges that are not supportnor separation segments areeliminated
actually, all segments that would intersect anobstacle are eliminated
Example of reduced visibility graph
Representation of a visibility graph
Representation of a visibility graph
Visibility graph: construction
• V = {v1, . . . , vn} is the set of vertices
• for each vi ∈ V , the segment vivj must be checked forintersections with obstacles ∀vj 6= vi• the number of segments vivj to check for intersections isO(n2)
there are n verticeseach vertex can be connected to the remaining n − 1 vertices
• for each segment vivj the intersection must be checkedagainst the edges of all obstacles, that are O(n)
the overall complexity is O(n3)
Grid-based maps
• the space is divided in adjacent cells
shape and size can change depending on the problem to solve
• the space is mapped such that a cell containing a piece ofobstacle is marked as occupied; it is free otherwise
• the resolution of the map is determined by the size of cells
Effect of the resolution on the navigation
Effect of the resolution on the navigation
Effect of the resolution on the navigation
Effect of the resolution on the navigation
Effect of the resolution on the navigation
Effect of the resolution on the navigation
Effect of the resolution on the navigation
Resolution completeness
• the success of the trajectory planning depends on theresolution
• an higher resolution increases the chance to find a path
• however, it requires more computing time and more memoryspace to store the map
trade-offs must be found between completenessand time/space requirements
The concept of “adjacent cell”
for square cells, the adjacency of two cells can be of two types:
d = 1d = 2
d = 1 d = 1
d = 1d = 2 d = 2
d = 2
d = 0
4 points connectivity
d = 1
d = 1 d = 1
d = 1d = 1
d = 1
d = 1
d = 1
d = 0
8 points connectivity
Wave-front algorithm
1 mark the cell containing the goal with i = 1
2 mark with i + 1 every adjacent cell to the one marked with i
3 repeat step 2 until the cell containing the starting point ismarked or all cells have been marked
4 use the gradient descent to go from the starting cell to thegoal cell
Wave-front algorithm: example
12
2
2
2
3
3
3
3
3
3
3
3
4
4
4
4
4 4
4
4
4
5
4 5
5
5
5
5
5
5
5
6
6
6
6
6
7
7
7
7
7
8
8 8
8
8
89
9
9
9
9
9
9
10
10
10
10
10
10
10
10
11
11
11
11
11
11
11
1112
12 12
12
12
12
12
13
13
13
13
1314
14
14
14
1415
15
15
15
15
15
16
1616
16
16
16
16
17
17
17
1717
17
5
Wave-front algorithm: example
12
2
2
2
3
3
3
3
3
3
3
3
4
4
4
4
4 4
4
4
4
5
4 5
5
5
5
5
5
5
5
6
6
6
6
6
7
7
7
7
7
8
8 8
8
8
89
9
9
9
9
9
9
10
10
10
10
10
10
10
10
11
11
11
11
11
11
11
1112
12 12
12
12
12
12
13
13
13
13
1314
14
14
14
1415
15
15
15
15
15
16
1616
16
16
16
16
17
17
17
1717
17
5
Wave-front algorithm and graph search
the assignment of values to the cells is similar to a breadth-firstsearch
12
2
2
2
3
3
3
3
3
3
3
3
4
4
4
4
4 4
4
4
4
5
4 5
5
5
5
5
5
5
5
6
6
6
6
6
7
7
7
7
7
8
8 8
8
8
89
9
9
9
9
9
9
10
10
10
10
10
10
10
10
11
11
11
11
11
11
11
1112
12 12
12
12
12
12
13
13
13
13
1314
14
14
14
1415
15
15
15
15
15
16
1616
16
16
16
16
17
17
17
1717
17
5
1 2 3 4 5 6 7 8 9 10 11 12
1
2
3
4
5
6
7
8
9
10
11
12
Wave-front algorithm: associated graph (partial representation)
12
2
2
2
3
3
3
3
3
3
3
3
4
4
4
4
4 4
4
4
4
5
4 5
5
5
5
5
5
5
5
6
6
6
6
6
7
7
7
7
7
8
8 8
8
8
89
9
9
9
9
9
9
10
10
10
10
10
10
10
10
11
11
11
11
11
11
11
1112
12 12
12
12
12
12
13
13
13
13
1314
14
14
14
1415
15
15
15
15
15
16
1616
16
16
16
16
17
17
17
1717
17
5
1 2 3 4 5 6 7 8 9 10 11 12
1
2
3
4
5
6
7
8
9
10
11
12
the breadth-first search may explore (assign labels)to a large, uninteresting area
Wave-front algorithm: characteristics
• complete: if a path exists (at a given resolution), it is found
• low efficiency: a large amount of “non necessary” cells canbe visited to assign the numbers to the cells
• optimal: it finds the shortest path (measured in number ofcells)
these features arise from the breadth-first searchperformed on the grid
The A∗ algorithm
• developed to find a path in a graph
• based on the knowledge of the goal location
• uses an heuristic search
• the heuristic is used to select the direction of movement
• it takes into account the distance between the currentlocation, the starting point and the goal
The A∗ algorithm: some notation
• O: set of “open” nodes (open set -priority queue)
• C : set of visited nodes (closed set)
• Neigh(V ): set of nodes adjacent to V
• c(V1,V2): length (cost) of the edgeconnecting V1 to V2
• g(V ): length (cost) of the backward path from V to pstart
• h(V ): heuristic function; estimates the cost from V to pgoal
• f (V ) = g(V ) + h(V ) estimated the total cost of the pathfrom pstart to pgoal passing by V
The A∗ algorithm: pseudo-code
input: the graph to analyze
output: the backward path from pgoal to pstart
repeatselect Vbest ∈ O : f (Vbest) ≤ f (V ) ∀V ∈ Oremove Vbest from O and add it to Cif Vbest = pgoal then
path foundend iffor all V ∈ Neigh(Vbest) : V /∈ C do
if V /∈ O thenadd V to O
elseif g(Vbest) + c(Vbest ,V ) < g(V ) then
connect V to Vbest
end ifend if
end foruntil O is emptyno path esists
Example of application of A∗
• nodes contain thevalue of the heuristics
• edges are labelledwith edge’s costs
Example of application of A∗
Example of application of A∗
• at each step the node in the priority queue having the lowercost is expanded
• once the goal is found, all the nodes in the priority queuehaving cost higher than the cost of the path are removed
• all the remaining nodes may bring to a lower cost path, thusthey are examined
Features of A∗
completeness
• A∗ generates a tree, which has no cycles by definition
• in a finite tree there is a finite number of distinct paths
• at most, every path is examined
• eventually, A∗ terminates by finding a path if it exists
however...completeness does not necessarily mean
that A∗ is efficient
Features of A∗
efficiency
• A∗ does not necessarily examine all the possible paths
• it explores in decreasing order all the paths that have the bestchances (heuristic function) to lead to the goal
• it terminates when no nodes provide better chances than thecurrent path
• this is the actual definition of “efficiency”
• if all paths are explored without finding a solution, then novalid path exists (completeness!)
however...efficiency does not necessarily mean that A∗ is optimal
Features of A∗
optimality• once a path to the goal is found (assuming it has cost c):
every node in the priority queue having cost less than c areexploredsuch paths are explored until their cost remains less than c
• A∗ explores new paths until the priority queue becomes empty
• it concludes the search by finding the path having the lowestcost path
a condition must hold to find an optimal path:
the heuristic function must be optimistic to guaranteethat the optimal path is found
Optimistic heuristic function
an heuristic function is optimistic if it returns an estimateof the distance from the goal that is less or equal to the
real distance
let’s consider:
• a grid of square cells
• 4 points connectivity
• the distance between two cells is computed using theManhattan distance
Optimistic heuristic function
Manhattan distance:
dist(V , pgoal) = ‖V .x − pgoal .x‖+ ‖V .y − pgoal .y‖
Euclidean distance:
dist2(V , pgoal) =√
(V .x − pgoal .x)2 + (V .y − pgoal .y)2
• the Euclidean distance (heuristic)is always less or equal to the realdistance
• is optimistic
Example of non optimistic heuristic
• the heuristic is not optimistic: node V 2 estimates its distancefrom the goal equal to 11, while it is 2
• the resulting path passes through V 1 (cost 8) instead ofpassing through V 2 (cost 4)
Example of application of A∗
assumptions:
• grid composed by square cells
• 8 points connectivity
heuristic:
• horizontal and vertical distance between cells = 1
• diagonal distance = 1.4 (approximating√
2)
ATTENTION: we are not using an approximatedEuclidean distance: the distance from a cell to the onelocated 2 cells on the right and 1 above is 2.4, not
√5
Example of application of A∗
Example of application of A∗
Example of application of A∗
Example of application of A∗
Example of application of A∗
Example of application of A∗
Example of application of A∗
Example of application of A∗
Simple variants of A∗
Greedy search
• assumes f (V ) = h(V ): only considers the estimated best pathfrom the current node
Dijkstra algorithm
• assumes f (V ) = g(V ): does not use any heuristic
• grows the current shortest path from the starting node
Roadmap
a roadmap M is a sequence of unidimensional curvesconnecting a starting point pstart and a goal pgoal
properties1 accessibility: a path exists from
pstart to some point p′s ∈ M
2 departability: a path exists from apoint p′g ∈ M to the goal pointpgoal
3 connectivity: a path exists betweenp′s and p′g made of nodes belongingto M
Probabilistic methods
when the dimension of the configuration space becomeshigh, some approaches are needed to reduce thecomplexity of the search: this is the objective of
probabilistic methods
• we will consider the so-called “sampling-based methods”
• the father of all sampling-based methods is the ProbabilisticRoadMap (PRM) trajectory planner
PRM: Probabilistic RoadMap
it leverages the idea that it is effortless to decide whethera point belongs to the free configuration space or not
• first, a map of the free space is built
• the planner randomly generates points in the configurationspace
• it keeps only those point that belong to the free space
• rough generation of point
• accurate planning of trajectories between collected points
Probabilistic methods: example
Operations of a probabilistic planner
learning phase
• a map of the configuration space is built
• a graph is generated to connect points in the free space
query phase
• check whether pstart and pgoal can be connected to the map,to nodes p′s and p′g respectively
• generate the shortest path between p′s and p′g (e.g. using A∗)
Learning phase
1 a set of V points in the configuration space is generated
2 for each point q ∈ V , the k closest points are considered,composing the Vk set
3 for each point p ∈ Vk a path is found to connect p and q, if pand q are not yet connected
key issues
• how the points in V are generated (randomly, using somemodels of obstacles, addressing the narrow passage problem,etc.)
• considering the distance between points in Vk (thecomputation time increases with such a distance, since morepoints are involved)
• how to connect pairs of points (“local planner”: straight lines,splines, etc.)
Query phase
for both the starting point pstart and the goal pgoal:
1 the k closest points of the map to pstart and pgoal areconsidered
2 the possiblity to connect pstart to p′s and pgoal to p′g is checked
3 the shortest path between p′s and p′g is searched
key issues
• how many nodes k shall be considered
• how to connect pairs of points (“local planner”: straight lines,splines, etc.)
Voronoi diagrams
also known as:
• Voronoi tessellation
• Voronoidecomposition
• Voronoi partition
• Dirichlet tessellation
Example and notation
• pi : sites
• q : free points
• e : (Voronoi) edge
• v : (Voronoi) vertex
• Fi : region or cell
Voronoi region
set of sites:
P = {pi}, 1 ≤ i ≤ n
Voronoi diagram of Vor(P):
• division of the plane into n regions
• there is a region for each site
definition of the i -th region:
Fi = {q ∈ Qfree : d(pi , q) ≤ d(pj , q)∀i 6= j}
Voronoi regions are bounded by line segments
Features of vertices
• if all the sites arecollinear, then Vor(P)consist of n-1 parallellines and n cells
• the edge e(pi , pj) isperpendicular to thesegment pipj
Features of vertices
• if sites are notcollinear, Vor(P) is aconnected graph andits edges are eitherline segments orhalf-lines
• if pi , pj are notcollinear with pk , thene(pi , pj) and h(pj , pk)can not be parallel
Features of vertices
• given a circle passingthrough 3 site points,which does notcontain any other sitepoint, its centerdefines a vertex
• each vertex hasdegree 3
Features of vertices
1 the locus of thecenter of a largestempty circles passingthrough only a pair ofpoints pi , pj ∈ Pdefines an edge
2 the locus of thecenter of largestempty circles passingthrough only onepoints in P defines aregion
Complexity of a Voronoi diagram
Theorem
the number of vertices in the Voronoi diagram of a set of n pointsin the plane is at most 2n-5 and the number of edges is at most3n-6
Construction of a Voronoi map
sensor-based
• uses the robot onboard sensor to build the map
• the map can be built online
polygonal spaces
• a full representation of the environment is needed
• obstacles must be polygons
grid map
• a full representation of the environment is needed
• easy and fast to compute
Delaunay triangulation
• Delaunay triangulation iscomplementary to thevoronoi tessellation
• each triangle’s edge isorthogonal to a Voronoiedge
• it can be leveraged toconstruct the Voronoipartition
an algorithm is applied tobuild the Delauneytriangulationthe Voronoi graph isderived from thetriangulation
Sensor-based construction
• the robot reaches the Vor with gradient ascent
• motion along the direction that maximise ∇d(qi , q) from theclosest obstacle
Sensor-based construction
• the robot moves along a line that keep d(qi , q) = d(qj , q)
• qi and qj are the closest points belonging to the closestobstacles
Sensor-based construction
• meet points are detected from changes in the closest obstacle
Grid-based construction
• the space is represented by means of a grid
Grid-based construction
• a wave-front grow is started from each obstacle
Grid-based construction
• a collision between two waves detects the presence of a Voredge
Grid-based construction
• where there is a collision “on the boundary”, the selection ofthe grid cell to mark is up to the algorithm
Grid-based construction
• the algorithm continues until...
Grid-based construction
• ...the grid is fully divided into Voronoi regions
Cell decomposition
exact cell decomposition
the free space is represented by the union of adjacent cells(regions)
• cell boundaries are often associatedto physical features of the freespace
change in the line of sightchange in the closest obstacleintersection with vertices
• adjacent cells share a commonboundary
Cell decomposition
adjacency graph
it encapsulates the relationships between adjacent cells
• a node is associated to a cell or an edge (boundary)
• graph edges connect adjacent cells
path planning
• the planner finds the cell containing the start and goal points
• the adjacency graph is used as a roadmap
Trapezoidal decomposition
• made by two-dimensional cells having trapezoidal shape
• triangular cells are possible
they can be seen as degenerated trapezoidone of the parallel edges is collapsed into a 0-length edge
assumptions
• planar configuration space
• obstacles are polygonal
• the free space is bounded by a polygon
Build the trapezoidal decomposition
• for each vertex vi , perform the following procedure
• draw two segments originating from vi
• the extension of each segment stops when it first intersects anedge
• segments can be drawn in any direction, but they must beparallel
note:
• some vertices may generate only one or even no segment
Build the trapezoidal decomposition: example
Build the trapezoidal decomposition: example
Build the trapezoidal decomposition: example
Build the trapezoidal decomposition: example
Build the trapezoidal decomposition: example
Build the trapezoidal decomposition: example
Build the trapezoidal decomposition: example
Path planning
• the cells containing the start ang goal points are identified
• the adjacency graph is searched for a path connecting the twocells
• the sequence of nodes in the graph must be translated intowaypoints in the free space
Path planning
trapezoids are convextherefore the centroid can be connected with every
boundary segment by a straight line
1 beginning from the start point...
2 each point is connected with the mid-point of the segmentassociated with the next graph node
3 the mid-point is connected to the centroid of the trapezoid
4 repeat from step 2 until the cell containing the goal point isreached
5 connect the last mid-point with the goal point
Properties of trapezoid
• many boundary segments are related with cell c10• the approach to reach the centroid allows to easily reach every
boundary segment
Coverage based on cell decomposition
coverage
the ability to pass a sensor or effector over all points composingthe free space
applications:
• exploration
• survellaince
• manufacturing (e.g., painting, polishing, smoothing)
Coverage based on cell decomposition
• in general, the structure of a cell is simple
• after the free space is divided into cells, simple algorithms canbe used to cover each cell
• once all cells are visited, coverage is obtained
back-and-forth coverage of a cell
