Trajectory planning Trajectory planning 1 1 Basilio Bona 1 - - PowerPoint PPT Presentation

Trajectory planning Trajectory planning – – 1 1

Basilio Bona ROBOTICA 03CFIOR 1

Introduction

The robot planning problem can be decomposed into a structured class of interconnected activities, at different hierarchical levels, usually referred to with different names:

• 1. Objective: it defines the highest activity level; typically shared by

the entire process or FMS where the robot is present; for example, the assembly of an engine head.

• 2. Task: it defines a subset of actions/operations to be

accomplished for the attainment of the objective: for example, the assembly of the engine pistons.

• 3. Operation: it defines one of the single activities in which the task

is decomposed: for example, the insertion of a piston in the cylinder.

Basilio Bona 2 ROBOTICA 03CFIOR

Introduction

• 4. Move: it defines a single motion that must be executed to

perform an operation: for example, close the hand to grasp the piston, move the piston in a predefined position.

• 5. Path/Trajectory: the elementary move is decomposed in one
• re more paths (no time law defined) or trajectories (time law
• re more paths (no time law defined) or trajectories (time law

and kinematic constraints are defined).

• 6. Reference: it consists of the vector of the data obtained

sampling the path/trajectory, supplied to the motors as references for their control control: this is represents the action performed at the most basic level.

Basilio Bona 3 ROBOTICA 03CFIOR

Decomposition of a planning problem

Objective … … ... Operation

Move Path Reference … …

Basilio Bona 4 ROBOTICA 03CFIOR

Task

… … … …

…

Planning and control

The control control problem consists in designing of control algorithms for the robot drives, such that the TCP motion follows a specified path in the cartesian space. Two types of tasks can be defined:

• 1. tasks that do not require an interaction with the environment (free

space motion); the manipulator moves its TCP following cartesian trajectories, with constraint on positions, velocities and accelerations. Sometimes it is sufficient to move the joints from a specified value to Sometimes it is sufficient to move the joints from a specified value to another without following a particular geometric path

• 2. tasks that require and interaction with the environment, i.e., where the

TCP shall move in some cartesian subspace while it applies (or is subject to) forces or torques to the environment

We will consider only the first type of tasks

The control may take place at joint level (joint space control) or at cartesian level (task space control)

Basilio Bona 5 ROBOTICA 03CFIOR

Path vs trajectory Path = is the geometrical description of the set of desired points in the task space. The control shall maintain the TCP on the commanded path Trajectory = is the path AND the time law required to follow the path, from the starting point to the endpoint follow the path, from the starting point to the endpoint

Basilio Bona 6 ROBOTICA 03CFIOR

1( )

q t

2( )

q t

3( )

q t

( ) ( )

( ) ( ) t t                  x q q ⋯ α

4( )

q t

5( )

q t

6( )

q t

An example

PATH TRAJECTORY

desired speed desired acceleration

Basilio Bona 7 ROBOTICA 03CFIOR

( , , , , , ) f x y z φ θ ψ = ( ( ), ( ), ( ), ( ), ( ), ( )) f x t y t z t t t t φ θ ψ =

The geometrical path is usually described by an implicit equation

Trajectory planning

TRAJECTORY PLANNER Desired path Desired kinematic constraints Joint reference samples

r

q

Basilio Bona 8 ROBOTICA 03CFIOR

Robot dynamic constraint

The trajectory planner is a software function that computes the joint reference values (for the control block) given the desired path, the kinematic constraints (max speed etc.) and the dynamic constraints (max accelerations, max torques, etc.)

r

q

The control problem and the trajectory planner

Controller Actuator Gearbox Robot

r

q ( ) t q

TRAJECT PLANN

Basilio Bona 9 ROBOTICA 03CFIOR

Controller Actuator Gearbox Robot Transducer JECTORY ANNER

Usually, in control design courses, the reference signal generation is not considered (typical signals are assumed), but here is very important

Trajectory Planning

Task Space Joint Space

( ) t p ( )

f

t p ( ) t q ( )

f

t q

( )

( ) t π p

( )

′

( )

( ) t π p

Task-space path

( )

( ) t π′ q

Joint-space path Inverse Kinematics

Basilio Bona 10 ROBOTICA 03CFIOR

Task-space and joint-space paths can be different, since the inverse kinematics function is highly nonlinear

Constraints of different type

• 1. Desired Path (task space constraints)

a) Initial and final positions b) Initial and final orientations

• 2. Trajectory (time-dependent task space constraints)

a) Initial and final velocities a) Initial and final velocities b) Initial and final accelerations c) Velocities on a given part of the path, e.g., constant velocity or others d) Acceleration, e.g., centrifugal acceleration affecting curvature radius e) Fly points

• 3. Technological constraints (joint space constraints)

a) Motor maximum velocities b) Motor maximum accelerations

Basilio Bona 11 ROBOTICA 03CFIOR

Point-to-Point Trajectory – 1

When it is not important to follow a specific path, the trajectory is usually planned in the joint space, implementing a simple point-to- point (PTP) linear path, while the time law is constrained by the motor maximum velocity and maximum acceleration values

( ) t q

A simple joint space PTP path may generate a “strange” task space path

( )

f

t q

Basilio Bona 12 ROBOTICA 03CFIOR

Point-to-Point Trajectory – 2

Usually the PTP trajectory in the joint space is obtained implementing a linear (convex) combination of the initial and final values

( ) ( )

( )

( ) 1 ( ) ( ) ( ) ( )

f f

t s t s t s t s t π′ = − + = + − = + q q q q q q q q ∆

Initial value Final value

Basilio Bona 13 ROBOTICA 03CFIOR

( ) ( ) ( ) 1

f

s t s t s t = ≤ ≤ =

Convex combination

This is obtained using a unique scalar time-varying quantity called the curvilinear or profile abscissa ( )

s t

Point-to-Point Trajectory – 3

PROFILE GENERATOR CONVEX COMBINATION

( ) s t

1( )

q t

2( )

q t

3( )

q t

4( )

q t

Basilio Bona 14 ROBOTICA 03CFIOR

( ) s t ɺ ( ) s t ɺɺ ( ) s t

4( )

q t

5( )

q t

6( )

q t

This approach allows a coordinate motion coordinate motion, i.e., a motion of all joints that starts and ends at the same time instants, providing a smoother motion of the entire mechanical structure, avoiding unwanted jerks that can introduce undesirable vibrations

Simple Trajectory Planning

A seen in the previous formula, a PTP trajectory planning in the joint space requires only the design of the time law (i.e., the profile) for the scalar variable Assume that the various kinematic and dynamic constraints are reflected in the constraints on the max velocity and acceleration of ( ) s t ( ) s t

Basilio Bona 15 ROBOTICA 03CFIOR

max max max

( ) s s t s s − ≤ ≤ > ɺ ɺ ɺ ɺ

max max max max

( ) 0, s s t s s s

− + − +

− ≤ ≤ > > ɺɺ ɺɺ ɺɺ ɺɺ ɺɺ

Acceleration constraints Positive acceleration may be different from negative acceleration (deceleration) Velocity constraints

Simple profile

t

1

t

2

t

f

t

f

s ( ) s t ɺ

max

s ɺ

Trapezoidal velocity 2-1-2 profile

s

A

A s s = −

t t

1

t

1

t

2

t

2

t

f

t

f

t ( ) s t ɺɺ

max

s + ɺɺ

max

s + ɺɺ

Acceleration is limited Trapezoidal velocity

Area A +B −B

f

A s s = −

f

B B s + − = ɺ

Basilio Bona 16 ROBOTICA 03CFIOR

Simple profile

Since every trajectory is a mono-dimensional curve, it can be described by a single variable. In our case we use s(t) to parameterize the curve, after adding some minor constraints

Area

max

( ) ( ) 1 1 ( ) ( ) ( ) 0; ( )

f f

s t s t A s t s t s t s t s +

− + −

= = ⇒ = = = = = = = ɺ ɺ ɺɺ ɺɺ ɺɺ ɺɺ ɺɺ ɺɺ

max

( ) ; ( )

f f

s t s s t

− − +

= = ɺɺ ɺɺ ɺɺ

Another constraint is the continuity of the velocity This kind of trajectory is the most simple one, since it allows to fulfil the technological constraints on s(t) and its derivatives, and at the same time, provide a continuous curve, that does not overshoots the final target. The coordinate s(t) represents a sort of percentage of the path completed at time t ( ) s t ɺ

Basilio Bona 17 ROBOTICA 03CFIOR

2-1-2 profile

Basilio Bona 18 ROBOTICA 03CFIOR

2-1-2 profile

Basilio Bona 19 ROBOTICA 03CFIOR

2-1-2 profile

Basilio Bona 20 ROBOTICA 03CFIOR

2-1-2 profile

Basilio Bona 21 ROBOTICA 03CFIOR

2-1-2 profile

Basilio Bona 22 ROBOTICA 03CFIOR

2-1-2 profile – An example

0.2 0.4 0.6 0.8

• 0.5

0.5 1 1.5 2 2.5 0.2 0.4 0.6 0.8 1 1.2 1.4

Basilio Bona 23 ROBOTICA 03CFIOR

0.2 0.4 0.6 0.8

• 0.5

tempo (s) 0.2 0.4 0.6 0.8 tempo (s) 0.2 0.4 0.6 0.8

• 6
• 4
• 2

2 4 6 8 10

max max max

2 8 5 s s s

+ −

= = = ɺ ɺɺ ɺɺ

Bang-bang profile – An example

0.2 0.4 0.6 0.8 0.5 1 1.5 2 2.5 tempo (s) 0.2 0.4 0.6 0.8 0.2 0.4 0.6 0.8 1 1.2 tempo (s)

Basilio Bona 24 ROBOTICA 03CFIOR

max max max

8 5 4 s s s

+ −

= = = ɺ ɺɺ ɺɺ

0.2 0.4 0.6 0.8

• 6
• 4
• 2

2 4 6 8 10 tempo (s)

Discrete Time (sampled data) profile

Since the manipulator controller is a discrete-time computer, it is necessary to sample the continuous variable s(t). The sampling interval T is fixed according to the control specifications, and in modern robots is approximately 1 ms A sequence of N samples is obtained as The samples are then rounded off to be stored in a fixed length internal register (it can be a fixed length word or exponent + mantissa)

Basilio Bona 25 ROBOTICA 03CFIOR

{ }

1 1

( ) , , , , ,

k N

s t s s s s

−

→ … …

Discrete Time (sampled data) profile

Basilio Bona 26 ROBOTICA 03CFIOR

Sampled profile

Basilio Bona 27 ROBOTICA 03CFIOR

Sampled position profile (2-1-2)

f

s

k

s

vmax=2 amaxp=8 2 2 1 Phase 1 Phase 2 Phase 3

Basilio Bona 28 ROBOTICA 03CFIOR

k =

s

1

13 k =

2

22 k = 43

f

k = k

amaxp=8 amaxm=5 alfa=1 deltat=0.02

Sampled velocity profile

max

s ɺ

k

s ɺ

vmax=2 amaxp=8 amaxm=5 alfa=1 deltat=0.02

Basilio Bona 29 ROBOTICA 03CFIOR

k

k =

1

13 k =

2

22 k = 43

f

k =

Sampled acceleration profile

max

s + ɺɺ

k

s ɺɺ

vmax=2 amaxp=8 amaxm=5 alfa=1 deltat=0.02

Basilio Bona 30 ROBOTICA 03CFIOR

k

k =

1

13 k =

2

22 k = 43

f

k =

max

s − ɺɺ

Practical problems

Basilio Bona 31 ROBOTICA 03CFIOR

Interpolation schemes

Basilio Bona 32 ROBOTICA 03CFIOR

Incremental Interpolation

Which one?

Basilio Bona 33 ROBOTICA 03CFIOR

Incremental Interpolation

This plot shows the difference between the exact computation and the incremental interpolation Notice that the final value of the profile is larger than 1, since no correction of the commuting instants was implemented

Basilio Bona 34 ROBOTICA 03CFIOR

This plot shows the error between the two values; as one can see, during the constant velocity phase, no error arises

Absolute Interpolation

Basilio Bona 35 ROBOTICA 03CFIOR

Absolute interpolation

This plot shows the difference between the exact computation and the absolute interpolation Large errors arise, mainly due to the errors accumulated in the first and third phase

Basilio Bona 36 ROBOTICA 03CFIOR

Approximation of commutation instants

Since the commutation times are rarely an exact multiple

• f the sampling period, it is necessary to compute the

profile so that the profile constraints are never violated We proceed as follows

We compute the new profile samples recursively The transition between the acceleration phase and the constant speed phase is computed so that the maximal constant speed phase is computed so that the maximal velocity is not exceeded The transition between constant speed phase and the deceleration phase is computed so that

a) The maximal deceleration is not exceeded b) There is sufficient time intervals to decelerate and reach the zero final speed without violating a) c) The final zero velocity must be reached “uniformly” from above

Basilio Bona 37 ROBOTICA 03CFIOR

Approximation of commutation instants

What happens if one does not take care of numerical problems (e.g., when using Matlab)?

Delta=0.005 Delta=0.05

Basilio Bona 38 ROBOTICA 03CFIOR

Transition from phase 1 to phase 2

Transition from phase 1 (max acceleration) to phase 2 (constant velocity):

• max

max max

IF THEN ELSE

1 1 1 k k k k

s s s s s s s T

+ + + +

> = = + ɺ ɺ ɺ ɺ ɺ ɺ ɺɺ

Condition TRUE Go to phase 2 Condition FALSE Remain in phase 1

Basilio Bona 39 ROBOTICA 03CFIOR

The transition acceleration is

k

s s s s T

+

− = < ɺ ɺ ɺɺ ɺɺ

max trans max

The max velocity should not be exceeded

max

s ɺ

Basilio Bona 40 ROBOTICA 03CFIOR

k

s ɺ k

Basilio Bona 41 ROBOTICA 03CFIOR

The max velocity should not be exceeded

max

s ɺ

Basilio Bona 42 ROBOTICA 03CFIOR

k

s ɺ k

Transition from phase 2 to phase 3

Transition from phase 2 (constant velocity) to phase 3 (max deceleration) :

• (

)

IF THEN < > ELSE

1

1 -

d k max k k max

s s T s s s

+

< + = ɺ ɺ ɺ START DECELERATION

Braking space

max 2

2

d k k

s s s − = ɺ ɺɺ

Basilio Bona 43 ROBOTICA 03CFIOR

Condition TRUE Go to phase 3 Condition FALSE Remain in phase 2 The transition deceleration is

( )

* 2 1 1

1 1 2

d k k k k D

s s s s T s T

+ +

= − = − + − ɺ ɺɺ

The max deceleration should not be exceeded

max

s ɺ

Max deceleration exceeded

Basilio Bona 44 ROBOTICA 03CFIOR

k

s ɺ k

The zero final velocity must be attained from above

max

s ɺ

Basilio Bona 45 ROBOTICA 03CFIOR

k

s ɺ k

Velocity becomes negative

An example – velocity profile

Basilio Bona 46 ROBOTICA 03CFIOR

0.26 0.25

Exact commutation time Approximate commutation time

An example – acceleration profile

The acceleration profiles approximately follows the standard profile

Basilio Bona 47 ROBOTICA 03CFIOR

Joint trajectory planning

Basilio Bona 48 ROBOTICA 03CFIOR

Joint point-to-point trajectory planning Point-to-point joint trajectory

Basilio Bona 49 ROBOTICA 03CFIOR

Point-to-point joint trajectory

Continuous time Discrete time

Joint point-to-point trajectory planning

Basilio Bona 50 ROBOTICA 03CFIOR

Example: point-to-point

q

This is also called a convex combination

Basilio Bona 51 ROBOTICA 03CFIOR

i

q

1 i−

q

1

1

k i k i

s s

−

= → = → q q

Technological constrains on actuators

Basilio Bona 52 ROBOTICA 03CFIOR

Technological constrains on actuators

Basilio Bona 53 ROBOTICA 03CFIOR

Conclusions

Path planning is a very important issue in industrial robotics The geometrical path (and its time law) is the reference signal necessary for any control implementation The real algorithms path planning must work in discrete The real algorithms path planning must work in discrete time, since robot acts on a sampled data control system Path planning may be defined in joint space or task space Task space planning requires the computation of inverse kinematic functions (beware of singularities)

Basilio Bona 54 ROBOTICA 03CFIOR