Post on 04-Jun-2018
Lecture 8: Kinematics: Path and Trajectory Planning
• Concept of Configuration Space
c©Anton Shiriaev. 5EL158: Lecture 8 – p. 1/20
Lecture 8: Kinematics: Path and Trajectory Planning
• Concept of Configuration Space
• Path Planning
◦ Potential Field Approach
◦ Probabilistic Road Map Method
c©Anton Shiriaev. 5EL158: Lecture 8 – p. 1/20
Lecture 8: Kinematics: Path and Trajectory Planning
• Concept of Configuration Space
• Path Planning
◦ Potential Field Approach
◦ Probabilistic Road Map Method
• Trajectory Planning
c©Anton Shiriaev. 5EL158: Lecture 8 – p. 1/20
Concept of Configuration Space
Given a robot with n-links,• A complete specification of location of the robot is called its
configuration
c©Anton Shiriaev. 5EL158: Lecture 8 – p. 2/20
Concept of Configuration Space
Given a robot with n-links,• A complete specification of location of the robot is called its
configuration
• The set of all possible configurations is known as theconfiguration spaceQ =
{
q}
c©Anton Shiriaev. 5EL158: Lecture 8 – p. 2/20
Concept of Configuration Space
Given a robot with n-links,• A complete specification of location of the robot is called its
configuration
• The set of all possible configurations is known as theconfiguration spaceQ =
{
q}
• For example, for 1-link revolute armQ is the set of allpossible orientations of the link, i.e.
Q = S1 or Q = SO(2)
c©Anton Shiriaev. 5EL158: Lecture 8 – p. 2/20
Concept of Configuration Space
Given a robot with n-links,• A complete specification of location of the robot is called its
configuration
• The set of all possible configurations is known as theconfiguration spaceQ =
{
q}
• For example, for 1-link revolute armQ is the set of allpossible orientations of the link, i.e.
Q = S1 or Q = SO(2)
• For example, for 2-link planar arm with revolute joints
Q = S1 × S1 = T 2 ← torus
c©Anton Shiriaev. 5EL158: Lecture 8 – p. 2/20
Concept of Configuration Space
Given a robot with n-links,• A complete specification of location of the robot is called its
configuration
• The set of all possible configurations is known as theconfiguration spaceQ =
{
q}
• For example, for 1-link revolute armQ is the set of allpossible orientations of the link, i.e.
Q = S1 or Q = SO(2)
• For example, for 2-link planar arm with revolute joints
Q = S1 × S1 = T 2 ← torus
• For example, for a rigid object moving on a plane
Q ={
x, y, θ}
= R2 × S1
c©Anton Shiriaev. 5EL158: Lecture 8 – p. 2/20
Concept of Configuration Space
Given a robot with n-links and its configuration space,
• DenoteW the subset of R3 where the robot moves. It is
called workspace of the robot
c©Anton Shiriaev. 5EL158: Lecture 8 – p. 3/20
Concept of Configuration Space
Given a robot with n-links and its configuration space,
• DenoteW the subset of R3 where the robot moves. It is
called workspace of the robot
• The workspaceW might contains obstacles Oi
c©Anton Shiriaev. 5EL158: Lecture 8 – p. 3/20
Concept of Configuration Space
Given a robot with n-links and its configuration space,
• DenoteW the subset of R3 where the robot moves. It is
called workspace of the robot
• The workspaceW might contains obstacles Oi
• Denote A a subset of workspaceW , which is occupied bythe robot, A = A(q)
c©Anton Shiriaev. 5EL158: Lecture 8 – p. 3/20
Concept of Configuration Space
Given a robot with n-links and its configuration space,
• DenoteW the subset of R3 where the robot moves. It is
called workspace of the robot
• The workspaceW might contains obstacles Oi
• Denote A a subset of workspaceW , which is occupied bythe robot, A = A(q)
• Introduce a subset of configuration space that occupied byobstacles
QO :={
q ∈ Q : A(q) ∩ Oi 6= ∅, ∀ i}
c©Anton Shiriaev. 5EL158: Lecture 8 – p. 3/20
Concept of Configuration Space
Given a robot with n-links and its configuration space,
• DenoteW the subset of R3 where the robot moves. It is
called workspace of the robot
• The workspaceW might contains obstacles Oi
• Denote A a subset of workspaceW , which is occupied bythe robot, A = A(q)
• Introduce a subset of configuration space that occupied byobstacles
QO :={
q ∈ Q : A(q) ∩ Oi 6= ∅, ∀ i}
• Then collision-free configurations are defined by
Qfree := Q \ QO
c©Anton Shiriaev. 5EL158: Lecture 8 – p. 3/20
(a) The end-effector of the robot has a from of triangle. It movesin a plane. The plane contains a rectangular obstacle.
(b)QO is the set with the dashed boundaryc©Anton Shiriaev. 5EL158: Lecture 8 – p. 4/20
(a) Two-links planar arm robot. The workspace has a singlesquare obstacle.
(b) The configuration space and the setQO occupied by theobstacle is in gray.
c©Anton Shiriaev. 5EL158: Lecture 8 – p. 5/20
Lecture 8: Kinematics: Path and Trajectory Planning
• Concept of Configuration Space
• Path Planning
◦ Potential Field Approach
◦ Probabilistic Road Map Method
• Trajectory Planning
c©Anton Shiriaev. 5EL158: Lecture 8 – p. 6/20
Path Planning
Problem of Path Planning is the task to find a path in theconfiguration spaceQ• that connects an initial configuration qs to a final
configuration qf
• that does not collide any obstacle as the robot traverses thepath.
c©Anton Shiriaev. 5EL158: Lecture 8 – p. 7/20
Path Planning
Problem of Path Planning is the task to find a path in theconfiguration spaceQ• that connects an initial configuration qs to a final
configuration qf
• that does not collide any obstacle as the robot traverses thepath.
Formally, the task is to find a continuous function γ(·) such that
γ : [0, 1]→ Qfree with γ(0) = qs, and γ(1) = qf
c©Anton Shiriaev. 5EL158: Lecture 8 – p. 7/20
Path Planning
Problem of Path Planning is the task to find a path in theconfiguration spaceQ• that connects an initial configuration qs to a final
configuration qf
• that does not collide any obstacle as the robot traverses thepath.
Formally, the task is to find a continuous function γ(·) such that
γ : [0, 1]→ Qfree with γ(0) = qs, and γ(1) = qf
Common additional requirements:• Some intermediate points qi can be given• Smoothness of a path• Optimality (length, curvature, etc)
c©Anton Shiriaev. 5EL158: Lecture 8 – p. 7/20
Path Planning: Potential Field Approach
Basic idea:
• Treat a robot as a particle under an influence of an artificialpotential field U(·);
c©Anton Shiriaev. 5EL158: Lecture 8 – p. 8/20
Path Planning: Potential Field Approach
Basic idea:
• Treat a robot as a particle under an influence of an artificialpotential field U(·);
• Function U(·) should have
◦ global minimum at qf ⇒ this point is attractive
◦ maximum or to be +∞ in the points ofQO ⇒ thesepoints repel the robot
c©Anton Shiriaev. 5EL158: Lecture 8 – p. 8/20
Path Planning: Potential Field Approach
Basic idea:
• Treat a robot as a particle under an influence of an artificialpotential field U(·);
• Function U(·) should have
◦ global minimum at qf ⇒ this point is attractive
◦ maximum or to be +∞ in the points ofQO ⇒ thesepoints repel the robot
• Try to find such function U(·) constructed in a simple from,where we can easily add or remove an obstacle and changeqf . The common form for U(·) is
U(q) = Uatt(q)+(
U (1)rep(q) + U (2)
rep(q) + · · ·+ U (N)rep (q)
)
c©Anton Shiriaev. 5EL158: Lecture 8 – p. 8/20
Path Planning: Probabilistic Road Map Method
Basic idea:
• Sample randomly the configuration spaceQ;
• Those samples that belong toQO are disregarded;
c©Anton Shiriaev. 5EL158: Lecture 8 – p. 9/20
Path Planning: Probabilistic Road Map Method
Basic idea:
• Sample randomly the configuration spaceQ;
• Those samples that belong toQO are disregarded;
• Connecting Pairs of Configuration, e.g.◦ Choose the way measure the distance d(·) inQ◦ Choose ε > 0 and find k neighbors of distance no more
than ε that can be connected to the current oneThis step will result in fragmentation of the workspaceconsisting of several disjoint components
c©Anton Shiriaev. 5EL158: Lecture 8 – p. 9/20
Path Planning: Probabilistic Road Map Method
Basic idea:
• Sample randomly the configuration spaceQ;
• Those samples that belong toQO are disregarded;
• Connecting Pairs of Configuration, e.g.◦ Choose the way measure the distance d(·) inQ◦ Choose ε > 0 and find k neighbors of distance no more
than ε that can be connected to the current oneThis step will result in fragmentation of the workspaceconsisting of several disjoint components
• Make enhancement, that is, try to connect disjointcomponents
c©Anton Shiriaev. 5EL158: Lecture 8 – p. 9/20
Path Planning: Probabilistic Road Map Method
Basic idea:
• Sample randomly the configuration spaceQ;
• Those samples that belong toQO are disregarded;
• Connecting Pairs of Configuration, e.g.◦ Choose the way measure the distance d(·) inQ◦ Choose ε > 0 and find k neighbors of distance no more
than ε that can be connected to the current oneThis step will result in fragmentation of the workspaceconsisting of several disjoint components
• Make enhancement, that is, try to connect disjointcomponents
• Try to compute a smooth path from a family of points
c©Anton Shiriaev. 5EL158: Lecture 8 – p. 9/20
Steps in constructing probabilistic roadmap
c©Anton Shiriaev. 5EL158: Lecture 8 – p. 10/20
Lecture 8: Kinematics: Path and Trajectory Planning
• Concept of Configuration Space
• Path Planning
◦ Potential Field Approach
◦ Probabilistic Road Map Method
• Trajectory Planning
c©Anton Shiriaev. 5EL158: Lecture 8 – p. 11/20
Trajectory Planning
Trajectory is a path γ : [0, 1]→ Qfree with explicitparametrization of time
[Ts, Tf ] ∋ t τ ∈ [0, 1] : q(t) = γ(τ ) ∈ Qfree
c©Anton Shiriaev. 5EL158: Lecture 8 – p. 12/20
Trajectory Planning
Trajectory is a path γ : [0, 1]→ Qfree with explicitparametrization of time
[Ts, Tf ] ∋ t τ ∈ [0, 1] : q(t) = γ(τ ) ∈ Qfree
This means that we make specifications on
• velocity ddt
q(t) of a motion;
• acceleration d2
dt2q(t) of a motion;
• jerk d3
dt3q(t) of a motion;
• . . .
c©Anton Shiriaev. 5EL158: Lecture 8 – p. 12/20
Trajectory Planning
Trajectory is a path γ : [0, 1]→ Qfree with explicitparametrization of time
[Ts, Tf ] ∋ t τ ∈ [0, 1] : q(t) = γ(τ ) ∈ Qfree
This means that we make specifications on
• velocity ddt
q(t) of a motion;
• acceleration d2
dt2q(t) of a motion;
• jerk d3
dt3q(t) of a motion;
• . . .
In fact, it is common that the path is not given completely, but asa family of snap-shots
qs, q1, q2, q3, . . . , qf
So that we have substantial freedom in generating trajectories.c©Anton Shiriaev. 5EL158: Lecture 8 – p. 12/20
Decomposition of a path into segments with fast and slowvelocity profiles
c©Anton Shiriaev. 5EL158: Lecture 8 – p. 13/20
Trajectories for Point to Point Motion
Consider the ith joint of a robot and suppose that thespecification
at time t = t0 is : qi(t0) = q0, ddt
q(t0) = v0
c©Anton Shiriaev. 5EL158: Lecture 8 – p. 14/20
Trajectories for Point to Point Motion
Consider the ith joint of a robot and suppose that thespecification
at time t = t0 is : qi(t0) = q0, ddt
q(t0) = v0
at time t = t1 is : qi(t1) = q1, ddt
q(t1) = v1
c©Anton Shiriaev. 5EL158: Lecture 8 – p. 14/20
Trajectories for Point to Point Motion
Consider the ith joint of a robot and suppose that thespecification
at time t = t0 is : qi(t0) = q0, ddt
q(t0) = v0
at time t = tf is : qi(tf) = qf , ddt
q(tf) = vf
In addition, we might be given constraints of accelerations
d2
dt2q(t0) = α0
d2
dt2q(tf) = αf
c©Anton Shiriaev. 5EL158: Lecture 8 – p. 14/20
Trajectories for Point to Point Motion
Consider the ith joint of a robot and suppose that thespecification
at time t = t0 is : qi(t0) = q0, ddt
q(t0) = v0
at time t = tf is : qi(tf) = qf , ddt
q(tf) = vf
In addition, we might be given constraints of accelerations
d2
dt2q(t0) = α0
d2
dt2q(tf) = αf
If we choose to generate a polynomial
q(t) = a0 + a1t + a2t2 + · · ·+ amtm
that will satisfy the interpolation constraints,
what degree this polynomial should be chosen?c©Anton Shiriaev. 5EL158: Lecture 8 – p. 14/20
Trajectories for Point to Point Motion
The interpolation constraints
at time t = t0 is : qi(t0) = q0, ddt
q(t0) = v0
at time t = tf is : qi(tf) = qf , ddt
q(tf) = vf
for the 3rd-order polynomial
q(t) = a0 + a1t + a2t2 + a3t3, ddt
q(t) = a1 + 2a2t + 3a3t2
are
c©Anton Shiriaev. 5EL158: Lecture 8 – p. 15/20
Trajectories for Point to Point Motion
The interpolation constraints
at time t = t0 is : qi(t0) = q0, ddt
q(t0) = v0
at time t = tf is : qi(tf) = qf , ddt
q(tf) = vf
for the 3rd-order polynomial
q(t) = a0 + a1t + a2t2 + a3t3, ddt
q(t) = a1 + 2a2t + 3a3t2
areq0 = a0 + a1t0 + a2t20 + a3t30
v0 = a1 + 2a2t0 + 3a3t20
qf = a0 + a1tf + a2t2f + a3t3f
vf = a1 + 2a2tf + 3a3t2f
c©Anton Shiriaev. 5EL158: Lecture 8 – p. 15/20
Trajectories for Point to Point Motion
The equationsq0 = a0 + a1t0 + a2t20 + a3t30
v0 = a1 + 2a2t0 + 3a3t20
qf = a0 + a1tf + a2t2f + a3t3f
vf = a1 + 2a2tf + 3a3t2f
written in matrix form are
q0
v0
qf
vf
=
1 t0 t20 t300 1 2t0 3t201 tf t2f t3f0 1 2tf 3t2f
a0
a1
a2
a3
c©Anton Shiriaev. 5EL158: Lecture 8 – p. 16/20
Trajectories for Point to Point Motion
The equationsq0 = a0 + a1t0 + a2t20 + a3t30
v0 = a1 + 2a2t0 + 3a3t20
qf = a0 + a1tf + a2t2f + a3t3f
vf = a1 + 2a2tf + 3a3t2f
written in matrix form are
q0
v0
qf
vf
=
1 t0 t20 t300 1 2t0 3t201 tf t2f t3f0 1 2tf 3t2f
a0
a1
a2
a3
What is the determinant of this matrix?
c©Anton Shiriaev. 5EL158: Lecture 8 – p. 16/20
The parameters for interpolation
t = 0 and tf = 1, q0 = 10 and qf = −20, v0 = vf = 0
what is wrong with the trajectory?
c©Anton Shiriaev. 5EL158: Lecture 8 – p. 17/20
Trajectories for Point to Point Motion
Consider the ith joint of a robot and suppose that thespecification
at time t = t0 is : qi(t0) = q0, ddt
q(t0) = v0
at time t = tf is : qi(tf) = qf , ddt
q(tf) = vf
and additional constraints of accelerations
d2
dt2q(t0) = α0
d2
dt2q(tf) = αf
c©Anton Shiriaev. 5EL158: Lecture 8 – p. 18/20
Trajectories for Point to Point Motion
Consider the ith joint of a robot and suppose that thespecification
at time t = t0 is : qi(t0) = q0, ddt
q(t0) = v0
at time t = tf is : qi(tf) = qf , ddt
q(tf) = vf
and additional constraints of accelerations
d2
dt2q(t0) = α0
d2
dt2q(tf) = αf
To find interpolating polynomial we need to choose a polynomialof order ≥ 5
q(t) = a0 + a1t + a2t2 + a3t3 + a4t4 + a5t5
c©Anton Shiriaev. 5EL158: Lecture 8 – p. 18/20
The parameters for interpolation
t = 0 and tf = 2, q0 = 0 and qf = 20, v0 = vf = 0
c©Anton Shiriaev. 5EL158: Lecture 8 – p. 19/20
Interpolation by LSPB: Linear segments with parabolic blends
c©Anton Shiriaev. 5EL158: Lecture 8 – p. 20/20