Robotics 2017 06 Trajectory Planning 1 - polito.it · Trajectory planning Basilio Bona -DAUIN...

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 3ROBOTICS 01PEEQW - 2016/2017

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

Decomposition of a planning problem

Objective

… … ...

Operation

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)

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:

� on the revolute joints, for robotic arms

� on the wheel motors, for wheeled robots

� on the leg motors, for legged (humanoid and other types of

biomimetic robots)

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

Industrial Robots

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

1( )q t

2( )q t

3( )q t

4( )q t

5( )q t

6( )q t

An example

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

PATH TRAJECTORY

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

desiredspeed

desiredacceleration

The geometrical path is usually described by an implicit equation

constraints

Trajectory planning

TRAJECTORY

PLANNER

Desired path

Desired kinematicconstraints

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

The control problem and the trajectory planner

Controller Actuator Gearbox Robot

Transducer

rq ( )tq

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

0( )tp

( )( )tπ p

Task-space path

( )( )tπ′ q

Joint-space path

Inverse Kinematics

Task-space and joint-space paths can be different, since the inverse kinematics function is 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

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.

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

0( )tq

Task Space

0( )tp

Joint Space

Joint space vs Task space

Joint space Task space

Task space vs Joint space

Task space Joint space

Elbow up

Elbow down

� Usually the PTP trajectory in the joint space is obtained

implementing a linear convex combination of the initial and

final values

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

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

00 ( ) ( ) ( ) 1

fs 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

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

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( ) 0s s t s s− ≤ ≤ >ɺ ɺ ɺ ɺ

max max max max( ) 0, 0s s t s s s− + − +− ≤ ≤ > >ɺɺ ɺɺ ɺɺ ɺɺ ɺɺ

Acceleration constraintsPositive acceleration may be different from negative

acceleration (deceleration)

Velocity constraints

Simple profile

( )s tɺ

( )s tɺɺ

maxsɺ

maxs +ɺɺ

maxs−ɺɺ

Acceleration is limited

Trapezoidal velocity

2-1-2 profile0

Area A

+B −B

0fA s s= −

fB B s+ − = ɺ

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 0

0 0 max

( ) 0 ( ) 1 1

( ) ( ) 0

( ) 0; ( )

( ) ; ( ) 0

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ɺ

Continuous Profile

2-1-2 profile

2-1-2 profile – An example

0 0.2 0.4 0.6 0.8-0.5

tempo (s)0 0.2 0.4 0.6 0.8

tempo (s)

0 0.2 0.4 0.6 0.8-6

Bang-bang profile – An example

0 0.2 0.4 0.6 0.8-6

tempo (s)

0 0.2 0.4 0.6 0.8

tempo (s)0 0.2 0.4 0.6 0.8

tempo (s)

Sampled Data Profile

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 32ROBOTICS 01PEEQW - 2016/2017

{ }0 1 1( ) , , , , ,

k Ns t s s s s

−→ … …

Discrete Time (sampled data) profile

Sampled profile

Sampled position profile (2-1-2)

113k =

222k = 43

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

Phase 1 Phase 2 Phase 3

Sampled velocity profile

maxsɺ

113k =

222k = 43

Sampled acceleration profile

maxs+ɺɺ

ksɺɺ

113k =

222k = 43

maxs−ɺɺ

Practical problems

Interpolation schemes

Incremental Interpolation

Which one?

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

This plot shows the error between the

two values; as one can see, during the

constant velocity phase, no error arises

Absolute Interpolation

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

Approximation of commutation instants

� Since the commutation times are rarely an exact multiple

of 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

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

Transition from phase 1 to phase 2

� Transition from phase 1 (max acceleration) to phase 2

(constant velocity):

max max maxIF THEN ELSE

1 1 1k k k ks s s s s s s T++ + +> = = +ɺ ɺ ɺ ɺ ɺ ɺ ɺɺ

Condition TRUE

Go to phase 2

Condition FALSE

Remain in phase 1

The transition acceleration is

+−= <ɺ ɺ

ɺɺ ɺɺmax

trans max

The max velocity should not be exceeded

maxsɺ

The max velocity should not be exceeded

maxsɺ

Transition from phase 2 to phase 3

� Transition from phase 2 (constant velocity) to phase 3

(max deceleration) :

Condition TRUE

Go to phase 3

Condition FALSE

Remain in phase 2

The transition deceleration is

( )* 2

1 11 1 2d

k k k k Ds s s s T s T+ += − = − + −ɺ ɺɺ

( )IF THEN < >

ELSE 1

k max k

s s T s

START DECELERATION

Braking space

s−=ɺ

The max deceleration should not be exceeded

maxsɺ

Max deceleration

exceeded

The zero final velocity must be attained from above

maxsɺ

Velocity becomes

negative

An example – velocity profile

Exact commutation time

Approximate commutation time

An example – acceleration profile

The acceleration profiles approximately

follows the standard profile

Joint trajectory planning

Joint point-to-point trajectory planning

Point-to-point joint trajectory

Continuous time

Discrete time

Joint point-to-point trajectory planning

Example: point-to-point

1i−q

−= →

This is also called a

convex combination

Technological constrains on actuators

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)

Robotics 2017 06 Trajectory Planning 1 - polito.it · Trajectory planning Basilio Bona -DAUIN...

Documents

Robotics: Cartesian Trajectory Planning

Asymptotic stability and chaotic motions in trajectory following … · 2020. 1. 31. · hence the 2R robot cannot follow the desired trajectory and be asymptotically stable. Address

Robotics Chapter 5 – Path and Trajectory Planning Dr. Amit Goradia

SYNTHESIS OF OPTIMAL TRAJECTORY OF …optimal control and the optimal trajectory for the desired robot's movement and the realization of this optimal trajectory by the conventional

Introduction to Robotics Path Planning - uni-stuttgart.deipvs.informatik.uni-stuttgart.de/mlr/17-Robotics/05-pathPlanning... · Robotics Path Planning Path ﬁnding vs. trajectory

Trajectory Control of a Class of Articulated Aerial Robots · Trajectory Control of a Class of Articulated Aerial Robots Marin Kobilarov Laboratory for Computational Sensing and Robotics

ROBOTICS, CONTROL AND - WSEAS€¦ · ROBOTICS, CONTROL AND ... Gasbag Polishing Trajectory Planning for Free-form Surface Mould on ... Integrating UML and GPSS for Business Process

Journal Title Contact-Implicit Trajectory Optimization using Variational Integrators · 2019-04-20 · stepping methods with any desired order of integration accuracy. Preliminaries

Trajectory Planning Notes(Robotics)

Robotics 2013 07 Trajectory Planning 1 - PoliTO

Transition State Clustering: Unsupervised Surgical Trajectory Segmentation … · 2016-05-14 · Trajectory Segmentation, Surgical Robotics, Robot Learning Introduction The adoption

USMC Ground Robotics Current and Desired Future · PDF fileUSMC Ground Robotics Current Capabilities • EOD current capabilities – Man transportable robotics systems › Designed

society for ecological restoration intern a t i o n a l · tion of those processes which will return the ecosys-tem to its intended trajectory. When the desired trajectory is realized,

Results and Indicators - European Commission. results-and... · Results and Indicators EuropeAid ... change over time. ... -Milestones are the desired trajectory from baseline to

A Direct Method for Trajectory Optimization of Rigid ...groups.csail.mit.edu/robotics-center/public_papers/Posa13.pdf · A Direct Method for Trajectory Optimization of Rigid Bodies

The MAV3DSim: A Simulation Platform for Research ... · UAV Trajectory Desired path Fig. 3. Map visualization of the UAV desired and actual trayectory. _ = V N R _ = V E R cos (1)

Deep Local Trajectory Replanning and Control for Robot ...iliad.stanford.edu › pdfs › publications › pokle2019deep.pdf · the robot, and the coordinates of a desired destination

IEEE TRANSACTIONS ON ROBOTICS 1 Trajectory Planning for

Heuristic Optimization Algorithms in Roboticscdn.intechopen.com/pdfs/34406/InTech-Heuristic...Heuristic Optimization Algorithms in Robotics 315 (Kalra et al., 2006), trajectory generation

Prof. Alessandro De Luca - uniroma1.itdeluca/rob1_en/15_KinematicControl.pdf · features of kinematic control laws Simulink© block diagram desired reference trajectory: two types