Information
AI Chat

This is a Premium Document. Some documents on Studocu are Premium. Upgrade to Premium to unlock it.

RMP Summary SS 2021 of all lectures and exercises

Summary of all lectures, including exercises with explanations from th...

Course

Bewegungsplanung in der Robotik (IN2138) (0821095077)

58 Documents

Students shared 58 documents in this course

University

Technische Universität München

Academic year: 2021/2022

Listed bookIntroduction to Robotics

Uploaded by:

leo nie

Technische Universität München

0followers

22Uploads

15upvotes

Recommended for you

Comments

Please sign in or register to post comments.

Related Studylists

17 RMP 2024

Preview text

1. Motivation
Content
2. Configuration Space
1. Degrees of freedom
1. From workspace to configuration space
1. Paths
1. Obstacles
3. Bug algorithms
1. Base Navigation
1. Bug algorithms
1. Application of Bug Strategies for Configuration Spaces Ch 3.2
4. Classical path planning approaches
1. Roadmaps Ch
- 4.1. Visibility graph method CH. 5
- 4.1. Voronoi Diagram CH. 5
1. Cell Decomposition Ch
- 4.2. Trapezoidal decomposition Ch 6
- 4.2. The Halting Problem
- 4.2. Implementation of Vertical Cell Decomposition in a higher dimension 6.3
- 4.2. k-d Trees
- 4.2. Approximate Cell decomposition
1. Potential field
- 4.3. Function of the potential field
- 4.3. Bushfire Algorithm Ch 4.3
- 4.3. Wave Front planner CH 4
5. Sampling-Based Algorithms Ch7
1. Characteristics of Sampling-Based Planners
1. Sampling Strategy: Create a uniform distribution
1. Classic multiple query PRM (Probabilistic Roadmap)
1. Single Query PRM Ch7
- 5.4. rapidly-exploring random trees (RRT, by LaValle & Kuffner) Ch 7.2
1. Analysis of PRM: Expansive Spaces Ch 7.4
1. PRM Conclusion
1. OBPRM: Obstacle-Based PRM
6. Kalman Filtering Ch
1. Data Fusion example
1. Observability matrix
1. Problems with the Kalman filter
1. Kalman examples
- 6.4. Example: Water level in tank
1. The Extended Kalman Filter (EKF) Ch 8
1. Linearized Motion Model for a Robot
1. Extended Kalman Filter for SLAM Ch 8
1. Summary
7. Unscented Kalman Filter
1. Filtering Problem: General Problem Statement
1. Unscented Transformation
1. Scaled Unscented Transformation (SUT)
1. Unscented Kalman Filter
8. Bayesian and Particle Filter Ch
1. Localization
1. Bayesian Filtering
1. Particle Filtering
9. Overview Filters

Semester Summary Burschka: Robot Motion Planning

Grübeler’s Formula:

N: 6 for 3D, 3 for 2D
k: # of links (including the ground link)
n: # of joints
f_i: DOF of the joint

Type of joints

2D: Revolute and Prismatic
3D: see picture

2. From workspace to configuration space

Configuration of a moving object:
- specification of the position of every point on the object —> vector of position & orientation
Configuration space / C-space:
- set of all possible configurations of the system —> configuration is simply a point in this abstract

2. Configuration Space

The number of degrees of freedom of a robot system is the dimension of the configuration space, or the minimum number of parameters needed to specify the configuration.
powerful abstraction for solving motion planning problems (finding feasible motions for robots from x_1 to x_G)
in C-space: searching for a path in the joint space of 2D position and rotation
topology of C: usually not Cartesian

Examples

screw in nut is 1D because height is coupled to rotation
(a) Workspace: A two-joint manipulator. (b) The configuration of the robot is represented as a point on the toral configuration space. (c) Configurations space: The torus can be cut and flattened onto the plane. This planar representation has “wraparound” features where the edge F R is connected to G P , etc.
many more on slides

Matrix representations in SO(3)

nine elements of rotation matrix are subject to six constraints —> 3 DOF
Expectation: SO(3) can be parametrized using three variables —> Euler angles are often used
Correspond to three successive rotations around z-axis, y-axis and lastly x-axis:
no global parametrization because there are some singularities —> define functions like atan, ...

DOF = N ( k 2 1) 2

3 i =

( N 2 fi )= N ( k 2 n 2 1) +

3 i =

q =( q 1 , q 2 , ..., qn )

( x , y )*= 2 » * S 1

R * SO (3)

Determining the DoF for General

Robots

2D chains c:::> Base link is 3D (lffi. 2 x S 1 ) c:::> If fixed, then often 1D c:::> Adding joints generally adds one more dimension 3D chains ¢Base link is 6D (lffi. 3 x S0(3)) ¢ If fixed, depending on the joint ¢Then add the DOF of each additional joint Closed chains n n ¢We have a formula! c:::> N: 6 for 3D, 3 for 2D

DOF=N(k-1)-L(N-J;)=N(k-n-1)+ Lli

¢ k: # of links (including the ground link) c:::>n: the number of joints ¢ [;_: DOF of the joint ¢Examples

i=l i=l

¢ 2D, 3 links ¢ 2D, 4 links ¢ 2D, 6 links Image source: Principles of Robot Motion Darius Burschka, TU Munchen, mvp .tum/courses/motion 6

DoF and Types of Joints

Configuration: specification of where all points of a robot is

Degrees of freedom (dof): the smallest number of real-valued (i., continuous) coordinates to fully describe configurations of a robot ¢ More on th is later Types of joints ¢2D

¢3D

Rcvolutc

r--' I I I I : : : I J. ___ I _ A I I I I I - Prismatic

Revolutc Prismatic Screw Cylindrical Spherical Planar 1 Degree of Freedom 1 Degree of Freedom 1 Degree of Freedom 2 Degrees of Freedom 3 Degrees of Freedom 3 Degrees of Freedom

Darius Burschka, TU Munchen, mvp .tum/courses/motion 7

2 π

F G

R P

(b) (c)

GF PR

q 1

q 2 q 2

(a)

q 1

q 2

R = Rz ,φ Ry ,θ Rz ,ψ

=





c φ − s φ 0 s φ c φ 0 0 0 1









c θ 0 s θ 0 1 0 − s θ 0 c θ









c ψ − s ψ 0 s ψ c ψ 0 0 0 1





=





c φ c θ c ψ− s φ s ψ − c φ c θ s ψ− s φ c ψ c φ s θ s φ c θ c ψ+ c φ s ψ − s φ c θ s ψ+ c φ c ψ s φ s θ − s θ c ψ s θ s ψ c θ



.

x 0

y 1

xb x 1

zb , z 1

z 0 , za

ya , yb

Axis-angle parametrization: Rodrigues formula

efficient algorithm for rotating a vector in space, given an axis and angle of rotation
can be used to transform all three basis vectors to compute a rotation matrix in SO(3), the group of all rotation matrices, from an axis–angle representation
In other words, the Rodrigues' formula provides an algorithm to compute the exponential map from so(3), the Lie algebra of SO(3), to SO(3) without actually computing the full matrix exponential

is the rotation matrix through an angle » counterclockwise about the axis k. I the 3 × 3 identity matrix

Unit quaternion (p)

The axis-angle parameterization described above parameterizes a rotation matrix by three parameters. Quaternions, which are closely related to the axis-angle parameterization, can be used to define a rotation by four numbers.
Quaternions are a popular choice for the representation of rotations in three dimensions because compact, no singularity and naturally reflect the topology to the space of orientations

• # DOF = 6

Topology =

2. Paths

path: continuous curve connecting two configurations q and q’:
trajectory: path parametrized by time:
Constraints: Finite length, Bounded curvature, Smoothness, Minimum length, time, energy, ...

Free space topology

A free path lies entirely in the free space F —> no contact between robot and obstacle
The moving object and the obstacles are modeled as closed subsets, meaning that they contain their boundaries.
C-obstacles are closed subsets of the configuration space C
Free space F is an open subset of C. Hence, each free configuration is the center of a ball of non-zero radius entirely contained in F.

Semi-free space

semi-free path —> allows robot to contact the boundary of the obstacle
semi-free space is a closed subset of C. Its boundary is a superset of the boundary of F.

2. Obstacles

A configuration q is collision-free, or free, if a moving object placed at q does not intersect any obstacles in the workspace.
The free space F is the set of free configurations.
A configuration space obstacle (C-obstacle) is the set of configurations where the moving object collides with workspace obstacles.
Is the configuration in the free space? Compute the position of the robot and check for collision with any obstacle at that position:
- if colliding —> configuration within C-space obstacle

R = I + (sin » ) K + (1 2 cos » ) K 2

u =( u 1 , u 2 , u 3 , u 4 ) with u 12 + u 22 + u 32 + u 42 = 1

( u 1 , u 2 , u 3 , u 4 )=(cos » /2, n xsin » /2, n ysin » /2, n zsin » /2) with n

2 x+ n

2 y+ n

2 z = 1

= 3 × SO (3)

Ç : s [0,1]³ Ç ( s ) C such that Ç (0) = q and Ç (1) = q 2 Ç : t [0, T ]³ Ç ( t ) C

(b) (c)(a) Figure 3 (a) The circular mobile robot approaches the workspace obstacle. (b) By sliding the mobile robot around the obstacle and keeping track of the curve traced out by the reference point, we construct the configuration space obstacle. (c) Motion planning for the robot in the workspace representation in (a) has been transformed into motion planning for a point robot in the configuration space.

Topology of the Space in Path Search

C

~C.

------ • 0 W •

Points cannot be connected

Multiple possibilities because ofthe periodicity of the angle 3 ~/courses/motion 33

Semester Summary Burschka: Robot Motion Planning

3. Application of Bug Strategies for Configuration Spaces Ch 3.2

grid-based representation of the configuration space
grid on the torus: for each point on the grid, perform collision check
left: two-joint arm in a planar workspace, right: configuration space

Classical path planning approaches

Construct data structure once and then use data structure to plan subsequent paths more quickly
Topological maps —> Roadmaps : Represent the connectivity of the free space by a network of 1-D curves (connected nodes) and edges represent an adjacency relationship between nodes
Geometric models —> Cell Decomposition : Decompose the free space into simple cells and represent the connectivity of the free space by the adjacency graph of these cells
Occupancy grids —> Potential field : Define a potential function over the free space that has a global minimum at the goal and follow the steepest descent of the potential function

4. Roadmaps Ch

A union of one-dimensional curves is a roadmap if for all qstart and qgoal in free space that can be connected by a path, the following properties hold: Accessibility, Departability, Connectivity

4.1. VISIBILITY GRAPH METHOD CH. 5.

Defining characteristics: nodes share an edge if they are within line of sight of each other and all points in the robot's free space are within line of sight of at least one node on the visibility map —> properties of accessibility and departability, but connectivity must be proven
defined in a 2-D polygonal configuration space
Nodes: v_i of the visibility graph include the start qinit and goal location qgoal and

Comp arison

Tange ntial Bug (Page 23)

improvement to Bug 2: shorter path using a range sensor with a 360º infinite orientation resolution
Moves on straight line towards the goal until obstacle is sensed
Moves along the obstacle in distance of sensing radius

Name Description Working Case Problem case

Bug I v. Bug 2:

Draw worlds where Bug 2 performs better than Bug I (and vice versa)

I Bug 2 beats Bug 1 I I Bug 1 beats Bug 2 I

I ....-

I I I I • •

I IM FFr.~ ~QR/i:;QR - ~11tnrnh •

Darius Burschka, TU MOnchen, mvp.in.tum/courses/motion 20

Coming closer to Obstacles makes

the entire Object visible

qgoal

I I I I

I / ,, ,, ,····..

WC?z

Darius Burschka, TU MOnchen, mvp.in.tum/courses/motion 25

What is a visibility graph?

A visibility graph is a graph such that Nodes: qinit, qgoa1> or an obstacle vertex. Edges: An edge exists between nodes ti and v if the line segment between u and v is an obstacle edge or it does not intersect the obstacles. Darius Burschka, TU MOnchen, mvp.in.tum/courses/motion 37

all the vertices of the configuration space obstacles (not part of the offline graph, only added online)

Graph edges: are straight lines that connect two line-of-sight nodes Algorithm Input: q_init, q_goal, polygonal obstacles Output: visibility graph G

for every pair of nodes u,v if segment (u,v) is an obstacle edge then insert edge (u,v) into G; else for every obstacle edge e if segment (u,v) intersects e go to (1); insert edge (uv) into G.

Computation time in O(n 2 ) space:
- Simple algrorithm: O(n 3 )
- More efficient: Rotational Sweep O(n 2 log(n)) , Optimal O(n 2 )

Simplified Visibility Graph

all edges that do not lie on a supporting or separating line are removed
It's beneficial because it has fewer edges making the search for the shortest path more efficient
A supporting line is tangent to two obstacles such that both obstacles lie on the same side of the line.
A separating line is tangent to two obstacles such that the obstacles lie on opposite sides of the line.

• 4.1. VORONOI DIAGRAM CH. 5.

Generate paths that maximizes clearance: set of points where the distance to the two closest obstacle is the same
applicable mostly to 2-D configuration spaces
Space O(n) —> Running time O(n * log(n))
Bushfire method can be used to construct the Voronoi Diagram
- Input: grid of zeros for free space, ones for obstacles
- Output: discrete map with each pixel has value equal to the distance to the closest point on the closest obstacle

4. Cell Decomposition Ch

Exact cell decomposition: The free space F is represent by a collection of non-overlapping simple cells whose union is exactly F
Examples of cells: trapezoids, triangles

4.2. TRAPEZOIDAL DECOMPOSITION CH 6.

vertical line sweeps through space: if hit corner —> separation line
connect cells by a straight line segment that does not intersect any obstacle —> connecting the midpoints of the vertical extensions to the centroids of each trapezoid
Running time: O(n log(n)) by planer sweep in space O(n )

Roadmap Visibility graph Shakey Project, SRI [Nilsson, 1969]

Voronoi diagram Introduced by computational geometry researchers. Generate paths that maximizes clearance. Applicable mostly to 2-D configuration spaces.

Darius Burschka, TU MOnchen, mvp.in.tum/courses/motion 40

Adjacency graph

Nodes: cells Edges: There is an edge between every pair of nodes whose corresponding cells are adjacent. Udl IU::> DUI ::>l.,;111'\d, I U IVIUI ll.,;l lt::I I, I lll!J::>,//I I IVIJ.111 11::/l.,;UUI ::>t::::>/1 I IULIUI I 4ts

Obstacle

Separating

Supporting

q start

q goal

Figure 5 Reduced visibility graph.

q start

q goal Figure 5 represented as filled polygons. The thick dotted line represents the shortest path between theThe thin solid lines delineate the edges of the visibility graph for the three obstacles start and goal.

Semester Summary Burschka: Robot Motion Planning

volumes and leave boundaries at critical point 4. Repeat 2. to each of the (n-1) dimensional slices

The 2-cells in a slice are actually slices of 3-cells in the 3D decomposition. The only places in which these 3-cells can critically change is when the sweeping plane stops at some x value.
The roadmap is constructed by placing a sample point in the center of each 3-cell and 2-cell. The vertices are the sample points, and edges are added to the roadmap by connecting the sample points for each case in which a 3-cell is adjacent to a 2-cell.
This same principle can be extended to any dimension, but the applications to motion planning are limited because the method requires linear models (or at least it is very challenging to adapt to nonlinear models; in some special cases, this can be done).
Canny’s Algorithm in higher dimensions
basic idea: project directed from Rn to R 2 many times
drops computation time to singly exponential (an)
still not very practical but theoretically much better 4.2. K-D TREES
A data structure for storing points in k dimensions
Assume a tree like structure
Useful for finding points with certain properties
Can be used for solving 1-NN
Construction of a k-d tree for n points
Pick dimension i, pick a point with coordinates x = (x 1 , ... , xi, ... , xk)
Split the points based on xi (greater or less than)
Repeat the above two steps recursively: Increase i (modulo k) each time (I., pick a new dimension each time)
Depth: log(n) if balanced
Construction takes O(k n log(n)) time
Each dimension needs sorting ~O(n log(n))
Can speed up to O(n log(n))
Very efficient, but: relies on complete knowledge about environment, if higher level node is removed —> reconstruct the tree again, changes in spaces —> reorder everything

4.2. APPROXIMATE CELL DECOMPOSITION

F is represented by a collection of non overlapping cells whose union is contained in F
does not need to be balanced, adapts to the structure

Quadtree decomposition

first node: whole space
divide space into subspaces —> subsides
only mark if node is empty, mixed or full
graph only extends to lower entries if it has a mixed entry
if obstacle is removed from graph —> just remove entries in the tree up to the node where obstacle was removed
boundaries are fix (don’t adapt to the obstacles)

Octree decomposition

extend to higher dimensions: subdivide volume into 8 subvolumes
not as optimal, but don’t need to be rebalance if information is removed from system

4. Potential field

mostly used for mobile robots

k-d Tree

k-d tree stands fork-dimensional trees

¢A data structure for storing points ink dimensions ¢Assume a tree like structure

(0, .55)

(0,0. 4)

' (0. 72.0)

(0,42,42) (0,0) ¢Useful for finding points w/ certain properties ¢Can be used for solving 1-NN

(0,0) (0,0)

Construction of a k-d tree for n points ¢Pick dimension i, pick a point with coordinates x = (xi, ... , xi, ... , xk) ¢Split the points based on xi (greater or less than) ¢Repeat the above two steps recursively ¢ Increase i (modulo k) each time ¢ I., pick a new dimension each time ¢Depth: logn if balanced ¢Construction takes O(knlogn) time ¢ Each dimension needs sorting ~O(nlogn) ¢Can speed up to O(nlogn) ¢Balancing is important

0,0 0,0.

Darius Burschka, TU Munchen, mvp.in.tum/courses/motion

0,0.

k-d Tree

k-d tree stands fork-dimensional trees ¢A data structure for storing points ink dimensions ¢Assume a tree like structure

(0, .55)

(0,0. 4)

' (0. 72.0)

(0,42,42)

¢Useful for finding points w/ certain properties (0,0) ¢Can be used for solving 1-NN

(0,0) (0,0)

0,0 0,0.

Darius Burschka, TU Munchen, mvp.in.tum/courses/motion

0,0.

Quadtree decomposition

D empty D mixed full Udl IU~ DUI ~~l,;l 11\d, I U IVIUI ll,;I lt:I I, I lllJ~~.//I IIVJ.J.111 I I:/l,;UUI t:/11 IULIUI I 11

Octree decomposition

5 6 7 EMPTY cell - MIXED cell FULL cell , ,._

Define a potential function over the free space that has a global minimum at the goal and follow the steepest descent of the potential function

4.3. FUNCTION OF THE POTENTIAL FIELD

potential field: scalar function over the free space
Navigation: applies a force proportional to the negated gradient of the potential field —> combination of repulsive and attractive forces
Navigation Function is a ideal potential field that
- has a global minimum at the goal
- has no local minima
- grows to infinity near obstacles
- is smooth
simplest function would be cone
high gradient = high velocities, low gradient = low velocities

Designing the free space function

u(x) = k*|x| —> not a good function because velocity will not slow down at the goal and will overshoot
u(x) = x 2 —> better function because velocity is proportional to the gradient of the function, no constant

gradient, but system will slow down, while approaching the goal § robot velocity may (very far away from the goal) reach and exceed the maximum velocity of the function due to high slope

Switch from quadratic to line function once the maximum velocity is reached:
qgoal: position of the goal
dgoal: distance from goal
dgoal*: threshold distance from the goal where the planner switches between conic and quadratic potentials
if gradient is 0: minimum, saddle point or maximum —> calculate next derivative:
- < 0 —> minimum,
- > 0 —> maximum
- if saddle point: push of the robot would keep it rolling so that it doesn’t stop at a step (saddle point)

Designing the obstacle function

Obstacle = repulsive force: keeps robot away from obstacle, the closer the robot is to an obstacle, the stronger the repulsive force should be
Q*: distance from the obstacle that define how large the influence of the obstacle on the robot ist
eta:
D(q): distance to the obstacle
goes to infinity close to the obstacle

U att( q ) =

1

2

·d 2 ( q , q goal), d ( q , q goal)≤ d goal*

d goal* ·d ( q , q goal)−

1

2

· ( d *goal)

2 , d ( q , q goal)> d goal*

∇ U att( q ) =

· ( q − q goal), d ( q , q goal)≤ d *goal

d goal* · , d ( q , q goal)> d *goal

U rep( q ) =

1 2

̧ (

1 D ( q )

−

1 Q *)

2 , D ( q )≤ Q *

0, D ( q ) > Q *

∇ U rep( q ) =

̧ (

1 Q *

−

1 D ( q ))

1 D 2 ( q )

∇ D ( q ), D ( q )≤ Q *

0, D ( q ) > Q *

Repulsive Field Component

whose gradient is

V U,ep(q) = { rJ ( J. - otq)) o21(q) V D(q ), 0,

Darius Burschka, TU Munchen, mvp.in.tum/courses/motion

D(q) Q*, D(q) > Q*,

Repulsive Field Component

whose gradient is V U,ep(q) = { rJ ( J. - otq)) o21(q) V D(q ), 0,

Darius Burschka, TU Munchen, mvp.in.tum/courses/motion

D(q) Q*, D(q) > Q*,

Algorithm in Pictures

+

The robot follows the gradient decent in a field constructed as a combination of the attractive and repulsive field

Darius Burschka, TU Munchen, mvp.in.tum/courses/motion 18

Sampling-Based Algorithms Ch

Difficulty with classical approaches:

Running time increases exponentially with the dimension of the configuration space: For a d-dimension grid with 10 grid points on each dimension, there 10d grid cells
Several variants of the path planning problem have been proven to be PSPACE-hard
not feasible to have complete knowledge about all obstacles in the environment
Classical approaches not possible in > 3D

—> solution: sampling based methods — instead of representing C_free explicitly and globally, we instead “probe” the space locally, as necessary

Sampling-based methods employ a variety of strategies for generating samples (collision-free configurations of the robot) and for connecting the samples with paths to obtain solutions to path-panning problems
Not sample entire space, but rely on very few samples in the free space

5. Characteristics of Sampling-Based Planners

No attempt to explicitly construct the boundaries of the configuration space obstacles or represent cells of the free space, but rely on whether a configuration is in collision with obstacle or not
Heuristic algorithm: Unreliable (Example: potential field)
Give up completeness:
- Completeness requires that the planner always answers a path-panning query correctly. A complete algorithm finds a path if one exists and reports no otherwise (Example: Canny’s roadmap method)
- Complete planners cannot be implemented for robots with more than 3 degrees of freedom due to their high combinatorial complexity
- Weaker form of completeness: probabilistic completeness —> If there is a solution path, the algorithm will find it with high probability

5. Sampling Strategy: Create a uniform distribution

Straight interval
- Pick a random interval on the unit interval [0, 1] —> create unit square/cubes
- Scale on unit interval:
Circle:
1. Method: Pick x uniform at random from [-1,1] , Set —> Intervals of same widths are sampled with equal probabilities
1. Method: Pick » uniformly at random from [0, 2 pi], Set —> Circular arcs of same angles are sampled with equal probabilities.
Both are uniform in some sense. For sampling orientations in 2-D, the second method is usually more appropriate.
Sphere:
Spherical patches of same areas are sampled with equal probabilities.
Suppose U_1 and U_2 are chosen uniformly at random from [0,1].
Unit quaternion:

5. Classic multiple query PRM (Probabilistic Roadmap)

Probabilistic Roadmaps for Path Planning in High- Dimensional Configuration Spaces, L. Kavraki et al., 1996.
Assumption: Obstacles are static —> Roadmaps don’t need to be modified
Multiple query: No knowledge a priori about start and end point, can be anywhere —> Many queries to be processed in the same environment
Example: Navigation in a static virtual environment, Robot manipulator arms in a work cell

x 1 = ( a − b ) x + a

y = 1 − x 2

x = cos » and y = sin »

(cos ¿ /2, n xsin ¿ /2, n ysin ¿ /2, nz sin ¿ /2) with n

2 x+ n

2 y+ n

2 z= 1

Semester Summary Burschka: Robot Motion Planning

Planning divided into two phases:
1. learning phase, during which a roadmap in free space is built and is stored as a graph
2. query phase, during which user-defined query configurations are connected with the precomputed roadmap

Implementation of Learning Phase

Connection Step
- Set milestones : Create n uniform distribute milestones: If milestone is in free space —> keep it, if outside of workspace or in the obstacle —> try again
- Find local paths : If there is no collision along the local path, connect two milestones with straight line doing dense sampling (make sure there is no collision with many tests). This leads to a significant reduction of collision tests to build a roadmap, finding a solution from start to goal via green line (in the image above).
- Smoothing the resulting paths : milestone with the maximum distance from line is the new milestone and delete the others if path is collision free.
- If collision: repeat and use milestone with highest distance to the line
- —> fewer velocity changes —> more efficient BUT homotopic paths needs to be preserved
Expansion Step (resampling)
easy scenes give large set of milestones gives well connected roadmap § more constrained situations: roadmap consists of several small connected components
expand milestones in those regions: If you hit obstacle, continue in random direction (random walk) with max. distance L and then create a new milestone —> still keeps samples low but helps us to get samples in the problematic areas
Could solve problem to get through narrow passages

Uniform sampling Input : geometry of the moving object & obstacles Output : roadmap G = (V, E)

V <— Ø and E <— Ø, repeat ( q < a configuration sampled uniformly at random from C. if CLEAR(q)then Add q to V.( N, <a set of nodes in V that are close to q. for each q’e N,, in order of increasing d(q,q’) if LINK (q’,q) then Add an edge between q and q’ to E.

Homotopic paths

two paths 𝛕 and 𝛕’ with the same endpoints are homotopic if one

can be continuously deformed into the other:

A homotopic class of paths contains all paths that are homotopic to one another
During the deformation, path should not intersect with any obstacle
Homotopy ensures that after the path correction, the space topology gets visited from the same side as usually planned. While deforming, no other object may be intersected. It is important for a surveillance robot, which needs to observe e., specific walls to check for paintings.

Query processing in an online step

h : [0,1]×[0,1]→ F with h ( s ,0) = Ç ( s ) and h ( s ,1) = Ç

Voronoi regions belong to the states on the frontier of the search, this means that the tree preferentially expands towards large unsearched areas

Extend the tree from existing nodes in random directions with decreasing distances. Stop edge close to the obstacle
If obstacle: Stop at the border of the obstacle and move the node to the boundary —> nodes very close to obstacles, good for narrow passages
Growth limit q: limiting the length of the edge —> growth limited to this parameter, if no growth limit: explore whole space
generate nodes close to the goal —> preference of growing to the goal

Kinodynamic RRT

Besides solving single-query problems faster, Kinodynamic RRT is suitable for solving problems for systems with diûerential constraints (e. non-holonomic systems such as cars) § Standard PRM and RRT cannot be applied
respect the differential constraints —> Need to compute path more carefully: needs to solve a boundary value problem (differential equations)
—> Paths are not lines anymore, but curves (in germany: clothoids (curvature is getting narrower))

When does PRM and RRT work well? —> Re-Wiring

PRM: good for reasonably high-dimensional problems because problems are „easy“ / ·, ³, ́ - expansive
but its also possible to construct instance to make PRM/RRT produce long paths —> need to keep re-wiring the graph structure: RRT* (Karaman & Frazzoli)
- for each new sample x_n, check its log(n) neighborhood
- if there are better paths from x_1 to x_n, pick that path
- RRT* is an asymptotically optimal sampling based algorithm —> As the number of samples goes to infinity, an optimal path from x_I to x_G
Problem: Robot traveling between two spaces —> Many samples wasted with uniform samples because many samples are not part of the free space —> Extensions: Three possibilities
- 1. Divide space into subspaces (expansive components)
- 1. Use geometric transformations to increase the expansiveness of a free space, e. , smaller variance of uniform samples in narrow passages
- 1. Integrate the new planner with other planner for multiple-query path planning problems.

5. Analysis of PRM: Expansive Spaces Ch 7.4

Path Planning in Expansive Configuration Spaces, D. Hsu, J. Latombe, & R. Motwani, 1999.
How to determine the number of configurations that should be generated to ensure that, with probability exceeding a given constant, each roadmap is connected
Issues of PRM: Coverage and Connectivity —> find a method to find properties and derive the correct number of milestones
Make sure that
- coverage is adequate: It means that milestones are distributed such that almost any point of the configuration space can be connected by a straight line segment to one milestone. —> uniform distribution
- milestones are connected : There should be a one-to-one correspondence between the connected components of the roadmap and those of F —> not two separated graphs
  - Connectivity is difficult to capture when there are narrow passages § narrow passages are difficult to define
- Characterize coverage & connectivity? —> ³, ́, · - Expansiveness in order to capture the complexity of a configuration space due to narrow passages

difficult easy

Figure 7 with the smallestAn example of an (＀are located in the narrow passage between square A and square B. Each＀,,)-expansiveQfreewith＀,,≈ w / W. The points such point sees only a subset ofpoint near the top right corner of square A sees the entire square; but only a subset of A, ofQfreeof volume approximately 3 wW. Hence＀≈ w / W. A approximate volumeand≈ w / W. (From Hsu [192].) wW , contains points that each see a set of volume 2 wW ; hence≈ w / W

RRT after 500, 1000, 1582 nodes

Problem Optimal solution

"! "" "! "" "! ""

Likely RRT solution

RRT edge "# RRT* edge

Definition: Visibility set S of q
All configurations that can be connected to q by a straight line path in F
All configurations seen by q
Definition: T -good
is ·-good if given any point (¿ measures a volume)
Every free configuration sees at least T fraction of the free space, T in (0,1]
Definition: lookout of a subset S
- Subset of points in S that can see at least ́ fraction of F \ S , ́ is in (0,1].

•

A and B as big as possible: they will reduce the number of try outs to get a fully connected graph
Definition (·, {, ́)-expansive: The free space F is (·, {, ́)-expansive if
- 1. free space F is ·-good
- 1. for each subset S of F, its ́-lookout is at least { fraction of S
- The first condition of definition ensures that a certain fraction of F is visible from any configuration in F. The second condition ensures that each subset S ¦ F has a large lookout set. It is reasonable to think of S as the union of the reachability sets of a set V of points. Large values of ³ and ́ indicate that it is easy to choose random points from S such that adding them to V results in significant expansion of S. This is desirable since it allows for a quick exploration of the entire space.
- bigger ·, { and ́ —> lower cost of constructing a roadmap with good connectivity and coverage
Theorem: A roadmap of uniformly-sampled milestones has the correct connectivity

with probability at least 1 - gamma (gamma: failure rate —> exponential dependence of success) (each subgraph is a connected graph)

Definition: Linking sequence

reach( S ) ={ x ∈ F ∣ ∃ y ∈ S such that x y ⊂ F }

÷ f

S F F = C free q * F , ¿ ( V ( q ))g ·¿ ( F )

³ =

F / S ³ =

A S

n =

8 ln(8/ ·³³ )

·³

0-good 1-good

F is 0-good

S F \ S

0-lookout of S

This area is about 40% of F \ S

B

S F \S F is ·-goodà·=0. ³-lookout à³=0. Volume(³-lookout) Volume(S) àa=0. F is(·, ³,³)-expansive, where ·=0, a=0,³=0.

p 3 pn Pn+

p 2 p

p 1

Visibility of p

Lookout of V(p)

, TU München, mvp.in.tum/cou

! &*$()& !(

& '(&)

2 a)

Single-query. roadmapscheckthevisibility ofasystem Multipleqiery:

Example %" - growtrees simultaneously

³expansion

UniformSampling &connect Oncenodesaredose³connection

Start

Node distribution weighkdsampling aroundnodesinprecious layer ofthefree Uniform sampling

Application area feasibility test:Furniturepiece outofthe room/remove partof air Robotin aworkalllpredefined)³don'tknow startand end point. prepare for without collision ³single time all possible start/end point±dosedistance - Connecting nodes ..----

' dense sampling forcollisions makeare thereis no collision with many tests along theline Fully connected graph indilticult configurations

addmoresamples addmore samples in hopethattheywillSee eachother j 2 ³ no straight line,but randomdirection

t

3 .pathcorrection pushpath outsideofthe obstade find boundary of obstack with .....-

_-"" binary Search i i i obsTtade? yes³Searchhere 3. 2 b) search -no 1 here

, ggf× (A:Sinlecture) E:minimum visibility ³pickpoint thatcan See thesmallest area of the free spac }

%

reaA thepointcanSee {= ¥= ¥2. 1 = ÷ =%

6 T 7 :regionin confinedSpace which allows me to took inthefreespace

lookoutregion 7 = B-S = ÷ "A

most be seen from any point inB gg .. „„„„gun „„„„ „„„ „ „„„ gpa,„„„ „„ „„„„g ß= ÷= 1%= ¥

2 c) Draw a.ß,E in the given drawing

F=3. 6 - 0. 5 - 6 = 11. 5 {= ¥= ¥ 5 B: 1 { C 7 = §=

Is=

2 6

° ß= Es = 1 ¥, =¥

maximise 7 ,ß !Exam (usuallyhorizontal line)

Note: we can use more sophisticated heuristics to try to cover C-obstacle
Not for dynamic scenes

Kalman Filtering Ch 8

In this chapter, we begin to consider cases for which the robot’s knowledge of the world derives from measurements provided by imperfect, noisy sensors. Improve the knowledge about the world by moving the robot in the environment. Based on G. Welch & G. Bishop, “An Introduction to the Kalman Filter”

Why Kalman Filtering? Kalman Filter is a tool that predicts values using a bunch of mathematical equations under the assumptions that input data is in the form of Gaussian distribution and then followed by the application of linear equations to that Gaussian distribution. The filter is very powerful in several aspects: it supports estimations of past, present, and even future states, and it can do so even when the precise nature of the modeled system is unknown. Kalman filter (KF) techniques have been successfully used to predict and estimate errors in position of linear systems like ships.

What should we do if the sensors do not agree on the measurement? The mean of the Gaussian with smaller sigma should be trusted more. Weight the mean according to sigma and combine the measurements.

Idea of the Kalman Filter:

The main idea behind the Kalman filter is that you do not just have an estimate for a parameter x but also have some estimate for the uncertainty in your value for x
This is represented by the variance/covariance of the estimate Px
There are many advantages to this, as it allows you a means for estimating the confidence in your robot’s ability to execute a task (e. navigating through a tight doorway)
In the case of the KF, it also provides a nice mechanism for optimally combining data over time
This optimality condition assumes we have linear models , and the error characteristics of our sensors can be modeled as zero-mean, Gaussian noise

6. Data Fusion example

Use code from Team 1 and Team 2 to obtain two different

measurements Z = [z 1 , z 2 ]T for the range r to a beacon

Variance in each of these sensor measurements is R 1 and R 2 , respectively
Q: How should we fuse these measurements in order to obtain the “best” possible resulting estimate for r?
We’ll define “best” from a lest-squares perspective...
We have 2 measurements that are equal to r plus some additive zero-mean Gaussian noise v 1 and v 2
We want to fuse these measurements to obtain a new estimate for the range r hat
Using a weighted least-squares approach, the resulting sum

of squares error will be

Minimizing the error:

z 1 = r + N (0, R 1 )= r + v 1 z 1 = r + N (0, R 1 )= r + v 1

e =

3 i =

wi ( r 2 zi )

" e

" r

3 i =

wi ( r 2 zi )

2 = 2

3 i =

wi ( r 2 zi )= 0ó r =

n i =1 wizi 3

n i =1 wi

y ( k + 1) Hx ( k + 1| k ) = y ( k + 1)

0 –2 –1 0 1 2 3 4 5 6 7 8

Velocity measurement (y)

p(y)

Finding Most Likely Ouput

PDF from CombinedPDF measurement PDF fromprojected prediction

Figure 8 Measurements and predictions are then merged. The PDF plotted with the dot- dashed line results from the combination of the measurement PDF and the PDF of the predictionprojected into output space, where the combination is computed using theorem 8.2. The most likely output y ∗is the value at which this combined distribution reaches its peak.

We obtain: with

Rewrite as Kalman equation : with the Kalman gain K = [0,1]
R 1 —> infinity: rely on second estimate
first set r_hat to z 1 and the shift to z 2 according to the R values or if we think of this as adding a new measurement (z, P) to our current estimate (r, R) of the state we would get (—> prediction) :
For merging Gaussian distributions, the update rule is:
which if we write in our measurement update equation form we get:

Time Update

Step 1 of the time update phase is only our prediction based upon the linear state update equation that we have
A: state transition matrix, xk: current state (either position, position + velocity or position + velocity + acceleration) —> estimate v and a
B: process parameter uk from robot controller —> know v and a —> better prediction if we have access to the control values
Step 2 of the time update phase comes from projecting our covariance matrix forward where we only add the process noise variance Q due to the normal sum distribution property where Ã 32 = Ã 12 + Ã 22

Covariance matrix:

Prediction of next covariance (model error Q):

No control value (uk =0), replace xk+1:
Q will flatten the current prediction (flatter gaussian)
1. H: measurement matrix (e. x is a position (x, y) and z is rotated version (x’, y’) —> constant linear error corrected by measurement error, here: rotation matrix)
1. add measurement z_k, otherwise the distribution will flatten

6. Observability matrix

A system with state vector x of dimension n is observable if the observability matrix has row rank n (i. n linearly independent rows)
Example:

r =

z 1 R 1 +

z 2 R 2 1 R 1 +

1 R 2

R 2

R 1 + R 2

z 1 +

R 1

R 1 + R 2

z 2 wi =

Ãi 2

r = z 1 +

R 1

R 1 + R 2

( z 22 z 1 )

rk +1= r 2 k +1+

Pk 2 +

Pk 2 +1+ R

( zk +1 2 r

2 k +1)= r

2 k +1+ Kk +1( zk +1 2 r

2 k +1)

Ã 32

Ã 12

Ã 22

Ã 12 + Ã 22

Ã 12 Ã 22

ó Ã 32 =

Ã 12 Ã 22

Ã 12 + Ã 22

Pk +1=

Pk 2 +1 Rk +

Pk 2 +1+ Rk +

c Pk 2 +1 2 Kk +1 Pk 2 +

x ÷ 2 k +1= Axk ÷ + Buk ÷

Pk =

3 i =

( x ÷ i 2 ¿k ÷) ( xi ÷ 2 ¿k ÷)

Pk +1=

3 i =

( xi ÷, k +1 2 ¿k ÷+1) ( xi ÷, k +1 2 ¿k ÷+ i )

T + Q

Pk +1=

3 i =

[ A ( x ÷ i , k 2 ¿k ÷)] [ A ( x ÷ i , k 2 ¿k ÷)]

T + Q = A PkAT + Q

÷÷ø

ö ççè =æ 10 A 11 x =ççèæ vp ÷÷øö Measurement: position p Measurement: speed v H =( ) 01 H =( ) 10

÷÷ø

ö ççè

01 O ÷÷ ø

ö çç è

10 O

Multiple Choice

Multiple Choice

Was this document helpful?

Premium

This is a Premium Document. Some documents on Studocu are Premium. Upgrade to Premium to unlock it.