Lecture 8: Kinematics: Path and Trajectory Planning · Lecture 8: Kinematics: Path and Trajectory Planning •Concept of Conﬁguration Space •Path Planning Potential Field Approach

Lecture 8: Kinematics: Path and Trajectory Planning

• Concept of Configuration Space

c©Anton Shiriaev. 5EL158: Lecture 8 – p. 1/20



• Path Planning

◦ Potential Field Approach

◦ Probabilistic Road Map Method




• Path Planning



• Trajectory Planning


Concept of Configuration Space

Given a robot with n-links,• A complete specification of location of the robot is called its

configuration




configuration

• The set of all possible configurations is known as theconfiguration spaceQ =

{

q}




configuration


{

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)




configuration


{

q}


Q = S1 or Q = SO(2)

• For example, for 2-link planar arm with revolute joints

Q = S1 × S1 = T 2 ← torus




configuration


{

q}


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



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}








• 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


(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.




• Path Planning





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.


Path Planning


configuration qf


Formally, the task is to find a continuous function γ(·) such that

γ : [0, 1]→ Qfree with γ(0) = qs, and γ(1) = qf


Path Planning


configuration qf


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)


Path Planning: Potential Field Approach

Basic idea:

• Treat a robot as a particle under an influence of an artificialpotential field U(·);



Basic idea:


• Function U(·) should have

◦ global minimum at qf ⇒ this point is attractive

◦ maximum or to be +∞ in the points ofQO ⇒ thesepoints repel the robot



Basic idea:


• 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)

)


Path Planning: Probabilistic Road Map Method

Basic idea:

• Sample randomly the configuration spaceQ;

• Those samples that belong toQO are disregarded;



Basic idea:



• 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



Basic idea:





• Make enhancement, that is, try to connect disjointcomponents



Basic idea:





• Make enhancement, that is, try to connect disjointcomponents

• Try to compute a smooth path from a family of points


Steps in constructing probabilistic roadmap




• Path Planning





Trajectory Planning

Trajectory is a path γ : [0, 1]→ Qfree with explicitparametrization of time

[Ts, Tf ] ∋ t τ ∈ [0, 1] : q(t) = γ(τ ) ∈ Qfree


Trajectory Planning



This means that we make specifications on

• velocity ddt

q(t) of a motion;

• acceleration d2

dt2q(t) of a motion;

• jerk d3


• . . .


Trajectory Planning



This means that we make specifications on

• velocity ddt

q(t) of a motion;

• acceleration d2


• jerk d3


• . . .

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


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





q(t0) = v0


q(t1) = v1





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





q(t0) = v0


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


The interpolation constraints


q(t0) = v0


q(tf) = vf

for the 3rd-order polynomial

q(t) = a0 + a1t + a2t2 + a3t3, ddt

q(t) = a1 + 2a2t + 3a3t2

are



The interpolation constraints


q(t0) = v0


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



The equationsq0 = a0 + a1t0 + a2t20 + a3t30

v0 = a1 + 2a2t0 + 3a3t20


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



The equationsq0 = a0 + a1t0 + a2t20 + a3t30

v0 = a1 + 2a2t0 + 3a3t20


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?


The parameters for interpolation

t = 0 and tf = 1, q0 = 10 and qf = −20, v0 = vf = 0

what is wrong with the trajectory?





q(t0) = v0


q(tf) = vf

and additional constraints of accelerations

d2

dt2q(t0) = α0

d2

dt2q(tf) = αf





q(t0) = v0


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


The parameters for interpolation

t = 0 and tf = 2, q0 = 0 and qf = 20, v0 = vf = 0


Interpolation by LSPB: Linear segments with parabolic blends


Documents

Lecture 8: Kinematics: Path and Trajectory Planning · Lecture 8: Kinematics: Path and Trajectory Planning •Concept of Conﬁguration Space •Path Planning Potential Field Approach