1

EEE 187: RoboticsSummary 9

PATH PLANNING USING POLYNOMIAL TRAJECTORIES

It is possible to use polynomial trajectories to control thepoint-to-point motion of the joint variables. In this case, thejoints are controlled independently. The degree of the polyno-mial depends on the number of constraints or specified values.For example if we have a desired value for the configurationvariable and its speed, we will need to use a polynomial ofdegree 3 (with four terms).

I. TWO END POINTS

Assume that we have constraints on the initial and finalvalues of the configuration variable, as well as its initial andfinal speed. We want to plan the trajectory of the configurationvariable i.e., find a function q(t) that satisfies the following

• At time t0

q(t0) = q0 (1)q̇(t0) = v0 (2)

• At time tf

q(tf ) = qf (3)q̇f (t) = vf (4)

where vector [q0, qf , v0, vf ] is given. It is also possible tospecify constraints on the initial; and final values of theacceleration as follows

q̈(t0) = α0 (5)q̈f (t) = αf (6)

where vector [α0, αf ] is given. The goal is to compute thetrajectory using low order polynomial

q(t) = a0 + a1t+ ... (7)

The order depends on the number of constraints:• Cubic polynomial allows specifying the initial and final

position and velocity• Quintic polynomial allows specifying the initial and final

acceleration in addition to the initial and final positionand velocity.

Cubic polynomial

We have four constraints because we specify the initial andfinal position and speed. Because we have four constraints, weneed four parameters; therefore, we use polynomial of degree3 as follows

q(t) = a0 + a1t+ a2t2 + a3t

3 (8)q̇(t) = a1 + 2a2t+ 3a3t

2 (9)

Now, we have four constraints with four unknowns as follows

q0 = a0 + a1t0 + a2t20 + a3t

30 (10)

v0 = a1 + 2a2t0 + 3a3t20 (11)

qf = a0 + a1tf + a2t2f + a3t

3f (12)

v0 = a1 + 2a2tf + 3a3t2f (13)

We can write this under matrix form as follows:The solution for A is

A = T−1Q (14)

with

A =

a0

a1

a2

a3

(15)

and

Q =

q0

v0

qf

vf

(16)

T =

a0 a1t0 a2t

20 a3t

30

0 a1 2a2t0 3a3t20

a0 a1tf a2t2f a3t

3f

0 a1 2a2tf 3a3t2f

(17)

A solution exists only in the determinant is different fromzero, which gives in this case:

Det = (tf − t0)2 ̸= 0 (18)

Example

The desired initial and final values for the joint angle arev0 = 0; vf = 0, q0 = 0, qf = 90o. Find q(t) when

• Case 1: when q0 = 0, qf = 90o

• Case 2: when q0 = 0, qf = −90o

Solution

• Case 1: we have for parameters: a0 = 0, a1 = 0, a2 =0.1885, a3 = −0.0251

• Case 2: we have for parameters: a0 = 0, a1 = 0, a2 =−0.1885, a3 = 0.0251

The configuration variable and the velocity profiles areshown in figure 1 and 2

2

0 1 2 3 4 5−100

−80

−60

−40

−20

0

20

40

60

80

100

qf = −90o

qf = 90o

Fig. 1. Configuration variable profile

0 1 2 3 4 5−0.5

−0.4

−0.3

−0.2

−0.1

0

0.1

0.2

0.3

0.4

0.5

case 2: qf = −90o

Case 1: qf = 90o

Fig. 2. Velocity profile

Fig. 3. Trapezoidal velocity profile and corresponding acceleration

LINEAR SEGMENTS WITH PARABOLIC BLENDS (LSPB)

In this case, we have a specific velocity profile whichconsists of a trapezoidal velocity profile. The velocity isinitially increasing, then constant in the middle interval, thendecreasing. The velocity profile is shown in figure 3 where tbis the blend time. The desired trajectory can be divided intothree parts.

• Part 1: time interval [t0, tb], the time evolution of the jointvariable q(t) consists of a quadratic polynomial.

• Part 2: time interval [tb, tf −tb], the time evolution of thejoint variable q(t) is linear.

• Part 3: time interval [tf − tb, tf ], the joint variable q(t)switches back to the quadratic polynomial.

For simplicity, we assume that

t0 = 0 (19)q̇(tf ) = 0 (20)q̇(0) = 0 (21)

Our goal is determine the trajectory of the joint variable q(t).• Part 1: time interval [t0, tb] In this case, the joint

variable has a constant positive acceleration, the speed isincreasing, and the joint variable is a quadratic functionof time, that is

q(t) = a0 + a1t+ a2t2 (22)

q̇ = a1 + 2a2t (23)

Now, by considering the initial condition and the valuesat time tb, we have

q̇(0) = 0 =⇒ a1 = 0 (24)q(0) = a0 (25)

at time tb, q̇ = V = constant

q̇ = 2a2tb = V =⇒ a2 =V

2tb(26)

Therefore, in this time interval

q(t) = q0 +V

2tbt2 (27)

q̇(t) =V

tbt (28)

q̈(t) =V

tb(29)

• Part 2: time interval [tb, tf − tb], q(t) is linear withconstant speed V . We have

q̇ = V (30)∫ t

tb

q̇dt =

∫ t

tb

V dt (31)

Thereforeq(t)− q(tb) = V (t− tb) (32)

Thusq(t) = q(tb) + V (t− tb) (33)

Note that tb is not a free variable, it is constrained by

3

the initial and final configurations, and the speed V . Bysymmetry we have

q(tf2) =

q0 + qf2

(34)

Because q(tf2 ) happens during the second time interval,

equation (33) gives us

q(tf2) = q(tb) + V (

tf2

− tb) (35)

Combining equations (35) and (34), we get

q0 + qf2

= q(tb) + V (tf2

− tb) (36)

q(tb) =q0 + qf

2− V (

tf2

− tb) (37)

Now q(tb) can be expressed using the equation of q(t)in the first time interval given by equation (27)

q(tb) = q0 +V

2tb (38)

and thus by equating the equations for q(tb)

q0 +V

2tb =

q0 + qf2

− V (tf2

− tb) (39)

and finally

tb =q0 − qf + V tf

V(40)

so finally for the time interval [tb, tf − tb], we have

q(t) = q(tb) + V (t− tb) (41)

q(t) =qf + q0 − V tf

2+ V t (42)

q̇(t) = V (43)

• Part 3: time interval [tf − tb, tf ], q(t) switches back toquadratic, using similar logic, we have

q(t) = qf − V

2tbt2f +

V

tbtf t−

V

2tbt2 (44)

It is important to recall that in this case, we assumed thatmotion begins and ends at rest, we have specified values forthe initial and final values of the joint variable and its speedq0, qf , q̇0, q̇f as well as the final time tf . We did not specifythe blend time tb or q(tb).

SHORTEST TIME

In the previous case, we specified the final time, it ispossible to calculate the shortest time. This case correspondsto a triangular velocity profile as shown in figure 4. Thejoint variable moves with maximum acceleration (α) in thefirst time interval and switches at time ts (switching time) tothe minimum acceleration (−α). Let Vs be the speed at theswitching time, we have

Vs = αts (45)

and

tf = 2ts (46)

Fig. 4. Triangular velocity profile

−2 0 2 4 6 8 100

1

2

3

4

5

6

7

8

Fig. 5. Planar manipulator

The switching time is given by

ts =

√qf − q0

α(47)

Example

This example illustrates motion planning for the planarmanipulator using polynomial trajectories. The initial config-uration is q1(0) = 0, q̇1(0) = 0, q2(0) = 0, q̇2(0) = 0 andthe desired configuration is q1(0) = π/2, q̇1(0) = 0, q2(0) =π/4, q̇2(0) = 0. Figure 5 shows the manipulator and its path.Figures 6 and 7 show the time evolution for the joint anglesand velocities.

BUG ALGORITHM AND CELL DECOMPOSITION

The bug algorithm is one of the simplest and earliest pathplanning methods. The robot has a contact sensor and movesaccording to two behaviors only

4

0 1 2 3 4 5−1

−0.5

0

0.5

1

1.5

2

θ2(t)

θ1(t)

time

Fig. 6. Time evolution of the joint variables

0 1 2 3 4 5−0.3

−0.2

−0.1

0

0.1

0.2

0.3

0.4

0.5

θ̇2(t)

θ̇1(t)

time

Fig. 7. Time evolution of the velocity of the joint variables

• move on a straight line• Follow a boundary

The robot moves towards the goal unless there is an obsta-cle. When an obstacle is encountered the robot follows theboundary as shown in figure 8.

The cell decomposition method is another popular method.The principle is to divide the workplace into cells, someof these cells are occupied and others are free. The robotdetermines the free cells and search a path that connects themto reach its final goal.

The bug algorithm and cell decomposition and the cell de-composiiton method are shown in figures 8 and 9, respectively.

Fig. 8. Bug algorithm

Fig. 9. Cell decomposition method