ROBOTICS 01PEEQW Basilio Bona DAUIN Politecnico di Torino - - PowerPoint PPT Presentation

ROBOTICS 01PEEQW

Basilio Bona DAUIN – Politecnico di Torino

Trajectory Planning 1

Introduction

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

• 1. Objective: it defines the highest hierarchical level; typically due to the

goal of the overall process where the robot is present; for example, the assembly of an engine head in an assembly line

• 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 grasping and insertion of a piston in the cylinder

Basilio Bona - DAUIN - PoliTo 3 ROBOTICS 01PEEQW - 2016/2017

Introduction

• 4. Move: it defines a single motion that must be executed to perform an
• peration: for example, close the hand to grasp the piston, move the

piston in a predefined position, move the arm near the sample, attain the right pose.

• 5. Path/Trajectory: the elementary move is decomposed in one or more

geometrical paths (no time law is defined ) or trajectories (time law and kinematic constraints are defined).

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

path/trajectory; it is supplied to motors for their control: this represents the reference signal at the most basic level.

Basilio Bona - DAUIN - PoliTo 4 ROBOTICS 01PEEQW - 2016/2017

Decomposition of a planning problem

Basilio Bona - DAUIN - PoliTo 5 ROBOTICS 01PEEQW - 2016/2017

Objective … … ... Task Operation

Move Path Reference … … … … …

…

Planning and control

The control problem consists in designing a control algorithm for the robot motors, 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 point 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

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

Basilio Bona - DAUIN - PoliTo 6 ROBOTICS 01PEEQW - 2016/2017

Fixed vs mobile robots

This first part of the course will introduce the planning problems and algorithms related to fixed (industrial) robotic arms Mobile robots path planning will be treated later on The two problems are very similar The only difference is the kinematic model of the robot and the actuation controls that operate on it:

• n the revolute joints, for robotic arms
• n the wheel motors, for wheeled robots
• n the leg motors, for legged (humanoid and other types of

biomimetic robots) Etc.

Basilio Bona - DAUIN - PoliTo 7 ROBOTICS 01PEEQW - 2016/2017

Industrial Robots

Basilio Bona - DAUIN - PoliTo 8 ROBOTICS 01PEEQW - 2016/2017

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

Basilio Bona - DAUIN - PoliTo 9 ROBOTICS 01PEEQW - 2016/2017

1( )

q t

2( )

q t

3( )

q t

( ) ( )

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

4( )

q t

5( )

q t

6( )

q t

A B

An example

Basilio Bona - DAUIN - PoliTo 10 ROBOTICS 01PEEQW - 2016/2017

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

PATH TRAJECTORY

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

desired speed desired acceleration

The geometrical path is usually described by an implicit equation

A B A B constraints

Trajectory planning

Basilio Bona - DAUIN - PoliTo 11 ROBOTICS 01PEEQW - 2016/2017

TRAJECTORY PLANNER Desired path Desired kinematic constraints Robot dynamic constraint Joint reference samples

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

r

q

Basilio Bona - DAUIN - PoliTo 12 ROBOTICS 01PEEQW - 2016/2017

The control problem and the trajectory planner

Controller Actuator Gearbox Robot Transducer

r

q ( ) t q

TRAJECTORY PLANNER

Usually, in control design courses, the reference signal generation is not considered (since typical signals, as step functions or sinusoidal, 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

Task-space path

( )

( ) t π′ q

Joint-space path Inverse Kinematics

Basilio Bona - DAUIN - PoliTo 13 ROBOTICS 01PEEQW - 2016/2017

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

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 b) Initial and final accelerations c) Velocities on a given part of the path (e.g., constant velocity) d) Acceleration (e.g., centrifugal acceleration affecting curvature radius) e) Fly-by points

• 3. Technological constraints (joint space constraints)

a) Motor maximum velocities b) Motor maximum accelerations c) Motor temperature, etc.

Basilio Bona - DAUIN - PoliTo 14 ROBOTICS 01PEEQW - 2016/2017

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

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

( ) t q ( )

f

t q

Basilio Bona - DAUIN - PoliTo 15 ROBOTICS 01PEEQW - 2016/2017

Task Space

( ) t p ( )

f

t p

Joint Space

Joint space vs Task space

Basilio Bona - DAUIN - PoliTo 16 ROBOTICS 01PEEQW - 2016/2017

Joint space Task space

Task space vs Joint space

Basilio Bona - DAUIN - PoliTo 17 ROBOTICS 01PEEQW - 2016/2017

Task space Joint space

Elbow up Elbow down

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

Basilio Bona - DAUIN - PoliTo 18 ROBOTICS 01PEEQW - 2016/2017

( ) ( )

( )

( ) 1 ( ) ( ) ( ) ( )

f f

t s t s t s t s t π′ = − + = + − = + q q q q q q q q ∆ ( ) ( ) ( ) 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)

Initial value Final value

Point-to-Point Trajectory – 3

Basilio Bona - DAUIN - PoliTo 19 ROBOTICS 01PEEQW - 2016/2017

PROFILE GENERATOR CONVEX COMBINATION

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

1( )

q t

2( )

q t

3( )

q t

4( )

q t

5( )

q t

6( )

q t

This approach allows a coordinated 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

Basilio Bona - DAUIN - PoliTo 20 ROBOTICS 01PEEQW - 2016/2017

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 The various kinematic and dynamic constraints are reflected in the constraints on the max velocity and acceleration of ( ) s t ( ) s t

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 t t

1

t

1

t

1

t

2

t

2

t

2

t

f

t

f

t

f

t

f

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

max

s ɺ

max

s + ɺɺ

max

s − ɺɺ

Acceleration is limited Trapezoidal velocity 2-1-2 profile

s

Area A +B −B

f

A s s = −

f

B B s + − = ɺ

Basilio Bona - DAUIN - PoliTo 21 ROBOTICS 01PEEQW - 2016/2017

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 max

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

f f f f

s t s t A s t s t s t s t s 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 overshoot the final target. The coordinate s(t) represents a sort of percentage of the path completed at time t ( ) s t ɺ

Basilio Bona - DAUIN - PoliTo 22 ROBOTICS 01PEEQW - 2016/2017

Continuous Profile

Basilio Bona - DAUIN - PoliTo 23 ROBOTICS 01PEEQW - 2016/2017

2-1-2 profile

Basilio Bona - DAUIN - PoliTo 24 ROBOTICS 01PEEQW - 2016/2017

2-1-2 profile

Basilio Bona - DAUIN - PoliTo 25 ROBOTICS 01PEEQW - 2016/2017

2-1-2 profile

Basilio Bona - DAUIN - PoliTo 26 ROBOTICS 01PEEQW - 2016/2017

2-1-2 profile

Basilio Bona - DAUIN - PoliTo 27 ROBOTICS 01PEEQW - 2016/2017

2-1-2 profile

Basilio Bona - DAUIN - PoliTo 28 ROBOTICS 01PEEQW - 2016/2017

2-1-2 profile – An example

Basilio Bona - DAUIN - PoliTo 29 ROBOTICS 01PEEQW - 2016/2017

0.2 0.4 0.6 0.8

• 0.5

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 1.4 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

Basilio Bona - DAUIN - PoliTo 30 ROBOTICS 01PEEQW - 2016/2017

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

Sampled Data Profile

Basilio Bona - DAUIN - PoliTo 31 ROBOTICS 01PEEQW - 2016/2017

Discrete Time (sampled data) profile

Since the manipulator controller is a discrete-time computer, it is necessary to sample the continuous variable s(t) → sk. 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 - DAUIN - PoliTo 32 ROBOTICS 01PEEQW - 2016/2017

{ }

1 1

( ) , , , , ,

k N

s t s s s s

−

→ … …

Discrete Time (sampled data) profile

Basilio Bona - DAUIN - PoliTo 33 ROBOTICS 01PEEQW - 2016/2017

Sampled profile

Basilio Bona - DAUIN - PoliTo 34 ROBOTICS 01PEEQW - 2016/2017

Sampled position profile (2-1-2)

Basilio Bona - DAUIN - PoliTo 35 ROBOTICS 01PEEQW - 2016/2017

k =

f

s s

1

13 k =

2

22 k = 43

f

k =

k

s k

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

Sampled velocity profile

Basilio Bona - DAUIN - PoliTo 36 ROBOTICS 01PEEQW - 2016/2017

max

s ɺ

k

s ɺ k

k =

1

13 k =

2

22 k = 43

f

k =

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

Sampled acceleration profile

Basilio Bona - DAUIN - PoliTo 37 ROBOTICS 01PEEQW - 2016/2017

max

s + ɺɺ

k

s ɺɺ k

k =

1

13 k =

2

22 k = 43

f

k =

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

max

s − ɺɺ

Practical problems

Basilio Bona - DAUIN - PoliTo 38 ROBOTICS 01PEEQW - 2016/2017

Interpolation schemes

Basilio Bona - DAUIN - PoliTo 39 ROBOTICS 01PEEQW - 2016/2017

Incremental Interpolation

Which one?

Basilio Bona - DAUIN - PoliTo 40 ROBOTICS 01PEEQW - 2016/2017

Incremental Interpolation

Basilio Bona - DAUIN - PoliTo 41 ROBOTICS 01PEEQW - 2016/2017

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 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 - DAUIN - PoliTo 42 ROBOTICS 01PEEQW - 2016/2017

Absolute interpolation

Basilio Bona - DAUIN - PoliTo 43 ROBOTICS 01PEEQW - 2016/2017

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

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 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 - DAUIN - PoliTo 44 ROBOTICS 01PEEQW - 2016/2017

Approximation of commutation instants

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

Basilio Bona - DAUIN - PoliTo 45 ROBOTICS 01PEEQW - 2016/2017

Delta=0.005 Delta=0.05

Transition from phase 1 to phase 2

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

Basilio Bona - DAUIN - PoliTo 46 ROBOTICS 01PEEQW - 2016/2017

• 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 The transition acceleration is

k

s s s s T

+

− = < ɺ ɺ ɺɺ ɺɺ

max trans max

The max velocity should not be exceeded

Basilio Bona - DAUIN - PoliTo 47 ROBOTICS 01PEEQW - 2016/2017

max

s ɺ

k

s ɺ k

Basilio Bona - DAUIN - PoliTo 48 ROBOTICS 01PEEQW - 2016/2017

The max velocity should not be exceeded

Basilio Bona - DAUIN - PoliTo 49 ROBOTICS 01PEEQW - 2016/2017

max

s ɺ

k

s ɺ k

Transition from phase 2 to phase 3

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

Basilio Bona - DAUIN - PoliTo 50 ROBOTICS 01PEEQW - 2016/2017

• 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

+ +

= − = − + − ɺ ɺɺ

( )

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 − = ɺ ɺɺ

The max deceleration should not be exceeded

Basilio Bona - DAUIN - PoliTo 51 ROBOTICS 01PEEQW - 2016/2017

max

s ɺ

k

s ɺ k

Max deceleration exceeded

The zero final velocity must be attained from above

Basilio Bona - DAUIN - PoliTo 52 ROBOTICS 01PEEQW - 2016/2017

max

s ɺ

k

s ɺ k

Velocity becomes negative

An example – velocity profile

Basilio Bona - DAUIN - PoliTo 53 ROBOTICS 01PEEQW - 2016/2017

0.26 0.25

Exact commutation time Approximate commutation time

An example – acceleration profile

Basilio Bona - DAUIN - PoliTo 54 ROBOTICS 01PEEQW - 2016/2017

The acceleration profiles approximately follows the standard profile

Joint trajectory planning

Basilio Bona - DAUIN - PoliTo 55 ROBOTICS 01PEEQW - 2016/2017

Joint point-to-point trajectory planning

Basilio Bona - DAUIN - PoliTo 56 ROBOTICS 01PEEQW - 2016/2017

Point-to-point joint trajectory

Continuous time Discrete time

Joint point-to-point trajectory planning

Basilio Bona - DAUIN - PoliTo 57 ROBOTICS 01PEEQW - 2016/2017

Example: point-to-point

Basilio Bona - DAUIN - PoliTo 58 ROBOTICS 01PEEQW - 2016/2017

i

q

1 i−

q

1

1

k i k i

s s

−

= → = → q q

This is also called a convex combination

Technological constrains on actuators

Basilio Bona - DAUIN - PoliTo 59 ROBOTICS 01PEEQW - 2016/2017

Technological constrains on actuators

Basilio Bona - DAUIN - PoliTo 60 ROBOTICS 01PEEQW - 2016/2017

Conclusions

Path planning is a very important issue in robotics The geometrical path (and its time law) provides the reference data necessary for any control implementation A real path planning algorithm must work in discrete time, (often in real-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 - DAUIN - PoliTo 61 ROBOTICS 01PEEQW - 2016/2017