Object Recognition: Fourier Descriptors and Minimum-Distance Classification
Robotics: Modelling, Planning and Control
1. Introduction
Planning Techniques
Application to Robot Manipulators
Robotics: Modelling, Planning and Control
Chapter 12 Summary
Cody A. Ray
February 6, 2011
1 / 66
2. Introduction
Planning Techniques
Application to Robot Manipulators
Introduction
Motion Planning
Canonical Problem
Configuration Space
Examples of Obstacles
Planning Techniques
Retraction
Cell Decomposition
Probabilistic Planning
Artificial Potential
Application to Robot Manipulators
2 / 66
3. Motion Planning
Introduction
Canonical Problem
Planning Techniques
Configuration Space
Application to Robot Manipulators
Examples of Obstacles
Building on Previous Work
Chaps. 4 and 7 present trajectory planning methods for
manipulators and mobile robots, respectively
Chap. 12 removes assumption of empty workspace
Goal: avoid collision with obstacles – both static structures
and other moving objects
3 / 66
4. Motion Planning
Introduction
Canonical Problem
Planning Techniques
Configuration Space
Application to Robot Manipulators
Examples of Obstacles
Planning Examples
Manipulator in robotized cell must avoid collision with static
objects and other manipulators
Mobile robot carrying baggage in airport navigates among
obstacles that may be fixed (fittings, conveyor belts) or mobile
(passengers, workers)
Motion planning amounts to deciding which path to follow to
execute transfer task from initial to final posture without
colliding with obstacles
4 / 66
5. Motion Planning
Introduction
Canonical Problem
Planning Techniques
Configuration Space
Application to Robot Manipulators
Examples of Obstacles
Autonomous Planning
High-level task description provided by the user and a
geometric characterization of the workspace
Workspace characterization
made available entirely in advance, off-line planning
gathered by the robot itself during the motion by means of
on-board sensors, on-line planning
Developing automatic methods for motion planning is very
difficult, and is still an active topic of research
5 / 66
6. Motion Planning
Introduction
Canonical Problem
Planning Techniques
Configuration Space
Application to Robot Manipulators
Examples of Obstacles
12.1 Canonical Problem
Consider a robot B, which may consist of a single rigid body
(mobile robot) or a kinematic chain whose base is either fixed
(standard manipulator) or mobile (mobile robot with trailers
or mobile manipulator).
Robot moves in Euclidean space W = RN , with N = 2 or 3,
called workspace.
Let O1 , . . . , Op be the obstacles, i.e., fixed rigid objects in W.
Assume that both the geometry of B, O1 , . . . , Op and the
pose of O1 , . . . , Op in W are known.
Suppose that B is free-flying, that is, the robot is not subject
to any kinematic constraint.
6 / 66
7. Motion Planning
Introduction
Canonical Problem
Planning Techniques
Configuration Space
Application to Robot Manipulators
Examples of Obstacles
Canonical Problem Statement
The motion planning problem is the following
given an initial and a final posture of B in W, find if exists a
path, i.e., a continuous sequence of postures, that drives the
robot between the two postures while avoiding collisions
(including contacts) between B and the obstacles O1 , . . . , Op ;
report a failure if such a path does not exist.
7 / 66
8. Motion Planning
Introduction
Canonical Problem
Planning Techniques
Configuration Space
Application to Robot Manipulators
Examples of Obstacles
Special Cases
Robot is a single body moving in R2 is also known as the
piano movers’ problem
The generalized movers’ problem is canonical for single-body
robot moving in R3
8 / 66
9. Motion Planning
Introduction
Canonical Problem
Planning Techniques
Configuration Space
Application to Robot Manipulators
Examples of Obstacles
12.2 Configuration Space
Representing the robot as a mobile point in an appropriate
space, where the images of the workspace obstacles are also
reported. To this end, it is natural to refer to the generalized
coordinates of the mechanical system, whose value identifies
the configuration of the robot. This associates to each
posture of the latter a point in the configuration space, i.e.,
the set of all the configurations that the robot can assume.
9 / 66
10. Motion Planning
Introduction
Canonical Problem
Planning Techniques
Configuration Space
Application to Robot Manipulators
Examples of Obstacles
Configuration
Consider a system Br of r rigid bodies and assume that all the
elements of Br can reach any position in space. In order to
find uniquely the position of all the points of the system, it is
necessary to assign a vector x = [x1 . . . xp ]T of 6r = p
parameters, termed configuration. These parameters are
termed Lagrange or generalized coordinates of the
unconstrained system Br , and p determines the number of
degrees of freedom (DOFs).
10 / 66
11. Motion Planning
Introduction
Canonical Problem
Planning Techniques
Configuration Space
Application to Robot Manipulators
Examples of Obstacles
Configuration Space Examples
The configuration of a polygonal mobile robot in W = R2 is
described by the position of a representative point on the
body (e.g., a vertex) and by the orientation of the polygon,
both expressed with respect to a fixed reference frame. The
configuration space C is R2 × SO(2), with dimension n = 3.
For a polyhedral mobile robot in W = R3 , the configuration
space C is R3 × SO(3), whose dimension is n = 6.
For fixed-base planar manipulator with n revolute joints, the
configuration space is subset of (R2 × SO(2))n . Dimension of
C equals the dimension of (R2 × SO(2))n minus the number
of constraints due to the presence of joints, i.e., 3n − 2n = n.
In fact, in a planar kinematic chain, each joint imposes two
holonomic constraints on the following body.
11 / 66
12. Motion Planning
Introduction
Canonical Problem
Planning Techniques
Configuration Space
Application to Robot Manipulators
Examples of Obstacles
Configuration Space Examples (cont’d)
For a fixed-base spatial manipulator with n revolute joints, the
configuration space is a subset of (R3 × SO(3))n . Since in
this case each joint imposes five constraints on the following
body, the dimension of C is 6n − 5n = n.
For a unicycle-like vehicle with a trailer in R2 , configuration
space is subset of (R2 × SO(2)) × (R2 × SO(2)). If the trailer
is connected to the unicycle by revolute joint, configuration of
the robot can be described by the position and orientation of
the unicycle and the orientation of the trailer. The dimension
of C is therefore n = 4.
12 / 66
13. Motion Planning
Introduction
Canonical Problem
Planning Techniques
Configuration Space
Application to Robot Manipulators
Examples of Obstacles
Example 12.1
Consider a 2R manipulator (planar with two revolute joints)
Figure: (2.14.) Two-link planar arm
13 / 66
14. Motion Planning
Introduction
Canonical Problem
Planning Techniques
Configuration Space
Application to Robot Manipulators
Examples of Obstacles
Example 12.1
The configuration space has dimension 2, and may be locally
represented by R2 , or more precisely by its subset
Q = {q = (q1 , q2 ) : q1 ∈ [0, 2π), q2 ∈ [0, 2π)}.
This guarantees that the representation is injective, i.e., that a
single value of q exists for each manipulator posture. However, this
representation is not topologically correct: for example, the
configurations denoted as qA and qB in Fig. 12.1, left, which
correspond to manipulator postures that are ‘close’ in the
workspace W, appear to be ‘far’ in Q.
14 / 66
15. Motion Planning
Introduction
Canonical Problem
Planning Techniques
Configuration Space
Application to Robot Manipulators
Examples of Obstacles
Example 12.1
Figure: (12.1.) The configuration space of a 2R manipulator; left: a
locally valid representation as a subset of R2 , right: a topologically
correct representation as a two-dimensional torus
15 / 66
16. Motion Planning
Introduction
Canonical Problem
Planning Techniques
Configuration Space
Application to Robot Manipulators
Examples of Obstacles
Example 12.1
To take this into account, one should ‘fold’ the square Q onto
itself (so as to make opposite sides meet) in sequence along
its two axes. This procedure generates a ring, properly called
torus, which can be visualized as a two-dimensional surface
immersed in R3 as in Fig. 12.1, right. The correct expression
of this space is SO(2) × SO(2).
As we can see, even in simple examples, the geometric
structure of the configuration space is in general more
complex than that of a Euclidean space.
16 / 66
17. Motion Planning
Introduction
Canonical Problem
Planning Techniques
Configuration Space
Application to Robot Manipulators
Examples of Obstacles
12.2.1 Distance in configuration space C
Given a configuration q , let B(q ) be the subset of the
q
workspace W occupied by the robot B, and p(q ) be the
q
position in W of a point p on B. Intuition suggests that the
distance between two configurations q 1 and q 2 should go to
zero when the two regions B(q 1 ) and B(q 2 ) tend to coincide.
q q
d1 (q 1 , q 2 ) = max ||p(q 1 ) − p(q 2 )||
q q q (12.1)
p∈B
In other words, the distance between two configurations in C
is the maximum displacement in they induce on a point, as
the point moves all over the robot.
17 / 66
18. Motion Planning
Introduction
Canonical Problem
Planning Techniques
Configuration Space
Application to Robot Manipulators
Examples of Obstacles
Distance in configuration space C (cont’d)
q
However, the use of d1 (q ) is cumbersome. We often choose
the simple Euclidean norm as the configuration space distance.
d2 (q 1 , q 2 ) = ||p(q 1 ) − p(q 2 )||
q q q (12.2)
This is only appropriate when C is a Euclidean space. From
Example 12.1, it’s easy to realize that, unlike d1 (q A , q B ), the
q
q
Euclidean norm d2 (q A , q B ) does not represent correctly the
distance on the torus.
18 / 66
19. Motion Planning
Introduction
Canonical Problem
Planning Techniques
Configuration Space
Application to Robot Manipulators
Examples of Obstacles
12.2.2 Build the ‘images’ of obstacles in C
Given an obstacle Oi (i = 1, . . . , p) in W, its image in
configuration space C is called C-obstacle and is defined as
COi = {q ∈ C : B(q ) ∩ Oi = ∅}.
q q (12.3)
In other words, COi is the subset of configurations that cause
a collision (including simple contacts) between the robot B
and the obstacle Oi in the workspace.
19 / 66
20. Motion Planning
Introduction
Canonical Problem
Planning Techniques
Configuration Space
Application to Robot Manipulators
Examples of Obstacles
Build the ‘images’ of obstacles in C (cont’d)
The union of all C-obstacles
p
CO = COi (12.4)
i=1
defines the C-obstacle region, while its complement
p
Cfree = C − CO = {q ∈ C : B(q ) ∩
q q Oi = ∅} (12.5)
i=1
is the free configuration space, that is, the subset of robot
configurations that do not cause collision with the obstacles.
A path in configuration space is called free if it is entirely
contained in Cfree .
20 / 66
21. Motion Planning
Introduction
Canonical Problem
Planning Techniques
Configuration Space
Application to Robot Manipulators
Examples of Obstacles
Build the ‘images’ of obstacles in C (cont’d)
Although C is a connected space – given two arbitrary
configuration there exists a path that joins them – the free
configuration space Cfree may not be connected because of
occlusions due to C-obstacles. Note also that the assumption
of free-flying robot in the canonical problem means that the
robot can follow any path in the free configuration space Cfree .
21 / 66
22. Motion Planning
Introduction
Canonical Problem
Planning Techniques
Configuration Space
Application to Robot Manipulators
Examples of Obstacles
Compact Canonical Motion Planning Problem
Assume the initial and final posture of the robot B in W are
mapped to corresponding configurations in C, respectively
called start configuration q s and goal configuration q g .
Planning a collision-free motion for the robot means then
generating a safe path between q s and q g if they belong to
the same connected component of Cfree , and reporting a
failure otherwise.
22 / 66
23. Motion Planning
Introduction
Canonical Problem
Planning Techniques
Configuration Space
Application to Robot Manipulators
Examples of Obstacles
Example 12.2
Consider the case of a point robot B. In this case, the
configuration of the robot is described by the coordinates of
point B in the workspace W = RN and the configuration
space C is a copy of W. Similarly, the C-obstacles are copies
of the obstacles in W.
23 / 66
24. Motion Planning
Introduction
Canonical Problem
Planning Techniques
Configuration Space
Application to Robot Manipulators
Examples of Obstacles
Example 12.3
Configuration of spherical robot in W = RN can be described by
its center point; orientation is irrelevant. C is copy of W, but
C-obstacles aren’t copies of obstacles in W; growing procedure:
Figure: (12.2.) C-obstacles for a circular robot in R2 ; left: the robot B,
an obstacle Oi and the growing procedure for building C-obstacles, right:
the configuration space C and the C-obstacle COi
24 / 66
25. Motion Planning
Introduction
Canonical Problem
Planning Techniques
Configuration Space
Application to Robot Manipulators
Examples of Obstacles
Example 12.4
Configuration of polyhedral robot that’s free to translate with fixed
orientation in RN described by representative point (e.g., a vertex).
C is copy of W, C-obstacles obtained through growing procedure.
Figure: (12.3.) C-obstacles for a polygonal robot translating in R2 ; left:
the robot B, an obstacle Oi and the growing procedure for building
C-obstacles, right: the configuration space C and the C-obstacle COi
25 / 66
26. Motion Planning
Introduction
Canonical Problem
Planning Techniques
Configuration Space
Application to Robot Manipulators
Examples of Obstacles
Example 12.5
Polyhedral robot that can translate and rotate in RN has
higher dimensional C.
Ex. if W = R2 , C is represented as point (e.g., a vertex) and
angular coordinate θ w.r.t fixed frame, or R2 × SO(2).
Repeat growing procedure for each possible θ. The C-obstacle
COi is the volume generated by ‘stacking’ (in the direction of
the θ axis) all the constant-orientation slices thus obtained.
26 / 66
27. Motion Planning
Introduction
Canonical Problem
Planning Techniques
Configuration Space
Application to Robot Manipulators
Examples of Obstacles
Example 12.6
Robot manipulator B with rigid links B1 , . . . , Bn connected by
joints, there are two kinds of C-obstacles:
collision between body Bi and obstacle Oj
self-collisions, i.e., interference between two links Bi and Bj of
the kinematic chain.
To obtain the boundary of the C-obstacle COi , we must
identify through appropriate inverse kinematics computations
all configurations that bring one or more links of the
manipulator B in contact with Oi .
Fig 12.4 shows C-obstacles for 2R manipulator in two cases
(no self-collisions). Simple-shaped obstacles in W, complex
C-obstacles profile. (C is a torus shown as R2 for simplicity.)
27 / 66
28. Motion Planning
Introduction
Canonical Problem
Planning Techniques
Configuration Space
Application to Robot Manipulators
Examples of Obstacles
Example 12.6 (cont’d)
Figure: (12.4.) C-obstacles for a wire-frame 2R manipulator in two
different cases; left: the robot and the obstacles in W = R2 , right: the
configuration space C and the C -obstacle region CO.
28 / 66
29. Retraction
Introduction
Cell Decomposition
Planning Techniques
Probabilistic Planning
Application to Robot Manipulators
Artificial Potential
12.3 Planning via Retraction
Represent free configuration space as a roadmap R ⊂ Cfree , i.e.,
network of paths describing the connectivity of Cfree . The solution
of a particular instance of a motion planning problem is then
obtained by connecting (retracting) to the roadmap the start
configuration q s and the goal configuration q g , and finding a path
on R between the two connection points.
29 / 66
30. Retraction
Introduction
Cell Decomposition
Planning Techniques
Probabilistic Planning
Application to Robot Manipulators
Artificial Potential
Planning via Retraction (cont’d)
Assume Cfree is subset of C = R2 and is polygonal, i.e., its
boundary is entirely made of line segments, implying that the
C-obstacle region is itself a polygonal subset of C.
The clearance of each configuration q in Cfree is
γ(q ) = min ||q − s ||,
q q (12.6)
s ∈∂Cfree
where ∂Cfree is the boundary of Cfree . The clearance γ(q )
q
represents the minimum Euclidean distance between the
configuration q and the C-obstacle region.
30 / 66
31. Retraction
Introduction
Cell Decomposition
Planning Techniques
Probabilistic Planning
Application to Robot Manipulators
Artificial Potential
Planning via Retraction (cont’d)
Set of points on boundary of Cfree that are neighbors of q :
N(q ) = {s ∈ ∂Cfree : ||q − s || = γ(q )},
q s q q (12.7)
i.e., points on ∂Cfree determine the value of clearance for q .
With these definitions, the Voronoi diagram of Cfree is the
locus of its configurations having more than one neighbour:
V(Cfree ) = {q ∈ Cfree : card(N(q )) > 1}.
q q (12.8)
31 / 66
32. Retraction
Introduction
Cell Decomposition
Planning Techniques
Probabilistic Planning
Application to Robot Manipulators
Artificial Potential
Voronoi Diagrams
Figure: (12.5.) An example of Voronoi diagram and the solution of a
particular instance of the planning problem, obtained by retracting q s and
q g on the diagram. The solution path from q s to q g consists of the two
dashed segments and the thick portion of the diagram joining them
32 / 66
33. Retraction
Introduction
Cell Decomposition
Planning Techniques
Probabilistic Planning
Application to Robot Manipulators
Artificial Potential
Voronoi Diagrams (cont’d)
V(Cfree ) can be considered a graph having elementary arcs of
the diagram as arcs and endpoints of arcs as nodes.
Voronoi diagrams locally maximize clearance, and are
therefore a natural choice as a roadmap of Cfree for planning
motions characterized by a healthy safety margin with respect
to the possibility of collisions.
33 / 66
34. Retraction
Introduction
Cell Decomposition
Planning Techniques
Probabilistic Planning
Application to Robot Manipulators
Artificial Potential
Figure 12.6
A retraction procedure must be defined to connect a configuration
in Cfree to the diagram. (Using V(Cfree ) as a roadmap.)
Figure: (12.6.) The retraction procedure for connecting a generic
configuration q in Cfree to V(Cfree )
34 / 66
35. Retraction
Introduction
Cell Decomposition
Planning Techniques
Probabilistic Planning
Application to Robot Manipulators
Artificial Potential
Steps for Motion Planning via Retraction
1. Build the generalized Voronoi diagram V(Cfree ).
2. Compute the retractions r (q s ) and r (q g ) on V(Cfree ).
q q
3. Search V(Cfree ) for a sequence of consecutive arcs such that
q q
r (q s ) belongs to the first and r (q g ) to the last.
4. If the search is successful, replace the first arc of the sequence
q
with its subarc originating in r (q s ) and the last arc of the
q
sequence with its subarc terminating in r (q g ), and provide as
output the path consisting of the line segment joining q s to
q
r (q s ), the modified arc sequence, and the line segment joining
q
q g to r (q g ); otherwise, report a failure.
35 / 66
36. Retraction
Introduction
Cell Decomposition
Planning Techniques
Probabilistic Planning
Application to Robot Manipulators
Artificial Potential
12.4 Cell Decomposition
Assume Cfree can be decomposed in simply-shaped regions,
called cells, with the following characteristics:
‘easy’ to compute collision-free path that joins two
configurations belonging to the same cell.
‘easy’ to generate collision-free path between adjacent cells.
Generate connectivity graph from the cell decomposition,
where nodes represent cells and arcs represent adjacency.
Different motion planning methods based on type of cell; we
explore exact and approximate cell decomposition.
36 / 66
37. Retraction
Introduction
Cell Decomposition
Planning Techniques
Probabilistic Planning
Application to Robot Manipulators
Artificial Potential
12.4.1 Exact Cell Decomposition
Cfree partitioned as set of cells whose union gives exactly Cfree
convex polygonal cells guarantee the line segments joining two
configurations in the same cell lies entirely in the cell itself,
and therefore in Cfree .
travel safely between adjacent cells by passing through the
midpoint of the segment constituting their common boundary
37 / 66
38. Retraction
Introduction
Cell Decomposition
Planning Techniques
Probabilistic Planning
Application to Robot Manipulators
Artificial Potential
Exact Cell Decomposition (cont’d)
Figure: (12.7.) An example of trapezoidal decomposition via the sweep
line algorithm (above) and the associated connectivity graph (below).
Also shown is the solution of a particular planning problem (cs = c3 and
cg = c20 ), both as a channel in Cfree and as a path on the graph
38 / 66
39. Retraction
Introduction
Cell Decomposition
Planning Techniques
Probabilistic Planning
Application to Robot Manipulators
Artificial Potential
Exact Cell Decomposition (cont’d)
1. Compute a convex polygonal (e.g., trapezoidal)
decomposition of Cfree .
2. Build the associated connectivity graph C .
3. Search C for a channel, i.e., a sequence of adjacent cells from
cs to cg .
4. If a channel has been found, extract and provide as output a
collision-free path from q s to q g ; otherwise, report a failure.
39 / 66
40. Retraction
Introduction
Cell Decomposition
Planning Techniques
Probabilistic Planning
Application to Robot Manipulators
Artificial Potential
12.4.2 Approximate Cell Decomposition
Disjoint cells of predefined shape (e.g., rectangle for C = R2 ).
The union of all cells approximates Cfree .
Trade-off between approximation accuracy and decomposition
efficiency: recursive algorithm starts with coarse grid whose
resolution is locally increased to adapt to Cfree ’s geometry.
The associated connectivity graph is searched for a channel,
from which a solution path can be extracted.
40 / 66
41. Retraction
Introduction
Cell Decomposition
Planning Techniques
Probabilistic Planning
Application to Robot Manipulators
Artificial Potential
Approximate Cell Decomposition (cont’d)
Figure: (12.8.) An example of motion planning via approximate cell
decomposition; left: the assigned problem, right: the solution as a free
channel (thick line)
41 / 66
42. Retraction
Introduction
Cell Decomposition
Planning Techniques
Probabilistic Planning
Application to Robot Manipulators
Artificial Potential
12.5 Probabilistic Planning
Determine a finite set of collision-free configurations that
adequately represent the connectivity of Cfree , and use these
configurations to build a roadmap.
Choose at each iteration a sample configuration and check if it
entails a collision between the robot and workspace obstacles.
Discard samples with collisions. Else, add to current roadmap
and connect if possible to other stored configurations.
Sample configurations chosen according to some probability
distribution. Two planners of this type are discussed.
42 / 66
43. Retraction
Introduction
Cell Decomposition
Planning Techniques
Probabilistic Planning
Application to Robot Manipulators
Artificial Potential
12.5.1 Probabilistic Roadmap (PRM)
First, generate a random sample q rand of the configuration
space using a uniform probability distribution in C.
Test q rand for collision; if safe, try to connect through local
paths to sufficiently ‘near’ configurations already in roadmap.
‘Nearness’ usually defined on basis of Euclidean distance in C
Local path between q rand and q near usually selected as
rectilinear in C, chosen by local planner procedure
Sample path at sufficient resolution; test samples for collision
Stop after a maximum number of iterations or the number of
connected components becomes smaller than threshold.
Connect q s and q g to the same connected component of the
PRM by free local paths to find solution.
43 / 66
44. Retraction
Introduction
Cell Decomposition
Planning Techniques
Probabilistic Planning
Application to Robot Manipulators
Artificial Potential
Probabilistic Roadmap (cont’d)
Figure: (12.9.) A PRM in a two-dimensional configuration space (left)
and its use for solving a particular planning problem (right)
44 / 66
45. Retraction
Introduction
Cell Decomposition
Planning Techniques
Probabilistic Planning
Application to Robot Manipulators
Artificial Potential
12.5.2 Bidirectional Rapidly-exploring Random Tree (RRT)
Single-query methods quickly solve specific problem instance
An exhaustive Cfree connectivity roadmap is not generated.
Incremental expansion of RRT (T ) using simple randomized
procedure at each iteration
Generate a random configuration q rand according to uniform
probability distribution in C
Configuration q near in T closest to q rand , and new candidate
q new produced on segment joining q near to q rand at a
predefined distance δ from q near .
Collision check on q new and segment from q near to q new ; if
both in Cfree , T is expanded by incorporating q new and the
segment connecting it to q near .
q rand is not added, only indicates direction of expansion
45 / 66
46. Retraction
Introduction
Cell Decomposition
Planning Techniques
Probabilistic Planning
Application to Robot Manipulators
Artificial Potential
Bidirectional RRT (cont’d)
Speedup: use two trees Ts , Tg , respectively rooted at q s , q g
At each iteration, both trees expanded as previously described
After set number iterations, try to connect the two trees
Generate q new as expansion of Ts , try to connect Tg to q new
q new acts as q rand for Tg with variable step-size–not constant δ
If collision-free segment, done! else, add free portion to Tg , Tg
and Ts exchange roles and connection attempt is repeated
If not successful after set number iterations, conclude trees are
still far apart and resume the expansion phase
46 / 66
47. Retraction
Introduction
Cell Decomposition
Planning Techniques
Probabilistic Planning
Application to Robot Manipulators
Artificial Potential
Bidirectional RRT (cont’d)
Figure: (12.10.) The bidirectional RRT method in a two-dimensional
configuration space, left: the randomized mechanism for expanding a
tree, right: the extension procedure for connecting the two trees
47 / 66
48. Retraction
Introduction
Cell Decomposition
Planning Techniques
Probabilistic Planning
Application to Robot Manipulators
Artificial Potential
Extension to nonholonomic robots
Adapt RRT to nonholonomic robots – not free-flying
Ex. robot with unicycle kinematics, rectilinear paths in
configuration space are not admissible in general.
General approach: use motion primitives, admissible local
paths in C, each produced by specific choice of velocity inputs
in kinematic model. Admissible paths are concatenation of
motion primitives. Velocity inputs follow for unicycle robot:
υ=υ
¯ ω = {−¯ , 0, ω }
ω ¯ t ∈ [0, ∆], (12.9)
resulting in three admissible local paths: first and third are
left/right turn while second is rectilinear
48 / 66
49. Retraction
Introduction
Cell Decomposition
Planning Techniques
Probabilistic Planning
Application to Robot Manipulators
Artificial Potential
Bidirectional RRT for a unicycle robot
Figure: (12.11.) RRT-based motion planning for a unicycle; left: a set of
motion primitives, right: an example of RRTs
49 / 66
50. Retraction
Introduction
Cell Decomposition
Planning Techniques
Probabilistic Planning
Application to Robot Manipulators
Artificial Potential
12.6 Artificial Potential
Previous methods are off-line, requiring a priori knowledge of
geometry and pose of obstacles in workspace
On-line planning uses partial information on the workspace
gathered during the motion
Robot (point in C) moves under influence of potential field U
U is superposition of attractive potential to the goal and
repulsive potential from C-obstacle region.
Artificial force generated by U at each configuration q is the
negative gradient − U(q ) of the potential, indicating the
q
most promising direction of local motion
50 / 66
51. Retraction
Introduction
Cell Decomposition
Planning Techniques
Probabilistic Planning
Application to Robot Manipulators
Artificial Potential
12.6.1 Attractive Potential
Guide robot to goal q g using a paraboloid with vertex in q g
1 1
Ua1 (q ) = kae T (q )e (q ) = ka ||e (q )||2 ,
q q e q e q (12.10)
2 2
with ka > 0 and e = q g − q is the ‘error’ vector w.r.t the goal.
(Always positive and global minimum at goal, where it is zero)
Resulting attractive force is
f a1 (q ) = − Ua1 (q ) = kae (q )
q q q (12.11)
Alternatively, conical attractive potential and resulting force
q
e (q )
Ua2 (q ) = ka ||e (q )||
q e q q
f a2 (q ) = ka (12.12; 12.13)
||e (q )||
e q
51 / 66
52. Retraction
Introduction
Cell Decomposition
Planning Techniques
Probabilistic Planning
Application to Robot Manipulators
Artificial Potential
Attractive Potential (cont’d)
Figure: (12.12.) The shape of the paraboloidic attractive potential Ua1
(left) and of the conical attractive potential Ua2 (right) in the case
C = R2 , for ka = 1
52 / 66
53. Retraction
Introduction
Cell Decomposition
Planning Techniques
Probabilistic Planning
Application to Robot Manipulators
Artificial Potential
12.6.2 Repulsive Potential
Prevents collisions with obstacles as robot moves under
influence of attractive force f a
Assume C-obstacle region partitioned as convex components
COi , i = 1, . . . , p.
kr ,i γ
1 1
γ q
ηi (q ) − η0,i if ηi (q ) ≤ η0,i
q
q
Ur ,i (q ) = (12.14)
0 q ) > η0,i
if ηi (q
where kr ,i > 0, ηi (q ) = minq ∈COi ||q − q || is the distance of
q q
q from COi , η0,i is range of influence of COi and γ = 2, 3, . . .
53 / 66
54. Retraction
Introduction
Cell Decomposition
Planning Techniques
Probabilistic Planning
Application to Robot Manipulators
Artificial Potential
Repulsive Potential (cont’d)
The repulsive force resulting from Ur ,i is
f r ,i (q ) − Ur ,i (q )
q = q
kr ,i γ−1
1 1
ηi2 (q ) q
ηi (q ) − η0,i
q
ηi (q ) if ηi (q ) ≤ η0,i
q
= q
0 q
if ηi (q ) > η0,i
(12.15)
The aggregate repulsive potential is
p
q
Ur (q ) = q
Ur ,i (q ). (12.16)
i=1
54 / 66
55. Retraction
Introduction
Cell Decomposition
Planning Techniques
Probabilistic Planning
Application to Robot Manipulators
Artificial Potential
Repulsive Potential (cont’d)
Figure: (12.13.) The equipotential contours of the repulsive potential Ur
in the range of influence of a polygonal C-obstacle in C = R2 , for kr = 1
and γ = 2
55 / 66
56. Retraction
Introduction
Cell Decomposition
Planning Techniques
Probabilistic Planning
Application to Robot Manipulators
Artificial Potential
12.6.3 Total Potential
Total potential Ut is superposition of attractive and aggregate
repulsive potentials:
q q q
Ut (q ) = Ua (q ) + Ur (q ). (12.17)
This results in the force field
p
f t (q ) = − Ut (q ) = f a (q ) +
q q q q
f r ,i (q ) (12.18)
i=1
56 / 66
57. Retraction
Introduction
Cell Decomposition
Planning Techniques
Probabilistic Planning
Application to Robot Manipulators
Artificial Potential
Total Potential (cont’d)
Figure: (12.14.) The total potential in C = R2 obtained by superposition
of a hyperboloidic attractive potential and a repulsive potential for a
rectangular C-obstacle: left: the equipotential contours, right: the
resulting force field
57 / 66
58. Retraction
Introduction
Cell Decomposition
Planning Techniques
Probabilistic Planning
Application to Robot Manipulators
Artificial Potential
12.6.4 Planning Techniques
Three collision-free motion planning approaches using Ut and f t :
q
1. Consider f t (q ) as a vector of generalized forces that induce a
motion of the robot in accordance with its dynamic model.
q
τ = f t (q ) (12.19)
2. The second method regards the robot as a unit point mass
q
moving under the influence of f t (q ), as in
¨ q
q = f t (q ) (12.20)
q
3. The third possibility is to interpret the force field f t (q ) as a
desired velocity for the robot, by letting
˙ q
q = f t (q ) (12.21)
58 / 66
59. Retraction
Introduction
Cell Decomposition
Planning Techniques
Probabilistic Planning
Application to Robot Manipulators
Artificial Potential
Planning Techniques (cont’d)
(12.19) directly represents control inputs for the robot. It
generates smoother paths because reactions to presence of
obstacles are naturally ‘filtered’ through the robot dynamics.
(12.20) requires the solution of the inverse dynamics problem
(to compute the generalized forces τ ).
(12.21) can be used on-line in a kinematic control scheme. It
executes the motion corrections faster, and may be considered
safer. Further, it guarantees the asymptotic stability of q g
(the robot reaches the goal with zero velocity).
59 / 66
60. Retraction
Introduction
Cell Decomposition
Planning Techniques
Probabilistic Planning
Application to Robot Manipulators
Artificial Potential
Planning Techniques (cont’d)
Commonly, numerical integration of (12.21) via Euler method:
f q
q k+1 = q k + Tf t (q k ) (12.22)
where q k and q k+1 represent the current and next robot
configuration, and T is integration step. (Gradient method for
q
minimizing Ut (q ), aka the method of steepest descent).
Local minima can ‘trap’ generated paths from reaching their
goal (methods are not complete in general).
Grid-discretized steepest descent using best-first search is
resolution complete but exponential in dimension of C
Improve: bound ‘basin filling’ iterations before random walk
60 / 66
61. Retraction
Introduction
Cell Decomposition
Planning Techniques
Probabilistic Planning
Application to Robot Manipulators
Artificial Potential
12.6.5 Navigation Functions
Best-first search (basic or randomized) solve the local minima
problem, but may generate inefficient paths
Navigation functions: artificial potentials with no local minima
This property already holds if the C-obstacles are spheres
Could approximate-by-excess all C-obstacles with spheres
Could use differentiable homeomorphism to map C-obstacles
to collection of spheres, generate classic total potential in
transformed space, and map back to original space.
Could use harmonic functions, solutions of particular
differential equation describing physical process of heat
transmission or fluid dynamics
61 / 66
62. Retraction
Introduction
Cell Decomposition
Planning Techniques
Probabilistic Planning
Application to Robot Manipulators
Artificial Potential
Navigation Functions (cont’d)
Figure: (12.15.) An example of numerical navigation function in a simple
two-dimensional gridmap using 1-adjacency. Cells in gray denote a
particular solution path obtained by following from the start (cell 12) the
steepest descent of the potential on the gridmap
62 / 66
63. Introduction
Planning Techniques
Application to Robot Manipulators
12.7 The Robot Manipulator Case
High computational complexity due to high dimension of C
(typically n ≥ 4) and rotational DOFs (revolute joints).
Reduce the dimension of C approximating by excess robot size
(e.g, replace wrist/end-effector in six-DOF anthropomorphic
manipulator with the volume their joint ‘sweeps’.)
Shape of C-obstacles is complex due to rotational DOFs and
nonlinearity of inverse kinematics (recall Fig. 12.4).
Non-polyhedral shape doesn’t allow application of methods
presented in Secs. 12.3 and 12.4.
63 / 66
64. Introduction
Planning Techniques
Application to Robot Manipulators
The Robot Manipulator Case (cont’d)
The most convenient choice for off-line planning is
probabilistic methods, due to high-dimensional configuration
spaces. (Computation of C-obstacles is not required).
For on-line planning, use method from artificial potentials.
To avoid computation of C-obstacles and plan in reduced
dimensional space, the potential is directly built in workspace
W rather than C, and acts on manipulator control points set.
A point representing end-effector (assigned goal of motion
planning problem) and at least one point for each link body.
Use attractive-repulsive field for end-effector, and repulsive
field for other control points distributed on manipulator links.
64 / 66
65. Introduction
Planning Techniques
Application to Robot Manipulators
The Robot Manipulator Case (cont’d)
Let p1 , . . . , pP−1 represent body control points and pP the
end-effector control point
1. impose the generalized forces from combined action of various
force fields on control points in W, according to
P−1
τ =− J T (q ) Ur (p i ) − J T (q ) Ut (p p )
i q p P q p (12.23)
i=1
q
where J i (q ), i = 1, . . . , P, denotes the Jacobian of the direct
kinematics function associated with the control point p (q ). q
˙
2. Alternatively, we could feed these as joint velocities q rather
than forces τ (12.24).
These options correspond to (12.19) and (12.21), inheriting
the same characteristics.
65 / 66
66. Introduction
Planning Techniques
Application to Robot Manipulators
Example of Artificial Potentials
Figure: (12.16.) Examples of motion planning via artificial potentials
acting on control points for a planar 3R manipulator; left: planning is
successful and leads to a collision-free motion between the start S and
the goal G, right: a failure is reported because the manipulator is stuck
at a force equilibrium
66 / 66