A Decoupled Approach to Optimal Time-Energy Trajectory ... · Romansy’06 , Warsaw University of...

1/24Romansy’06 , Warsaw University of Technology, June 24-26 2006

A Decoupled Approach to Optimal Time-Energy Trajectory Planning of

Parallel Kinematic Machines

Amar Khoukhi, Luc Baron and Marek Balazinski

Mechanical Engineering Dept.

École Polytechnique of Montréal

C. P. 6079, Succ. CV, Montréal, Canada, H3C 3A7

Tel. (514) 340-4711/ ext. 4271 Fax (514) 340-5867

(amar.khoukhi, luc.baron, marek.balazinski)@polymtl.ca


Outline

• Introduction

• Modelling

• Augmented Lagrangian Approach

• Decoupled Formulation

• Example

• Conclusions et perspectives


Introduction: Offline Programming Framework


Flight Simulator Serie 500 by CAE

Example of a PKMIntroduction:


z a 4 y a 5 a 3 P x a 6 a 2 l6 a 1 l5 l1 l4 l2 l3

b5 b4

b6

O W b3

b1 b2

Geometry of a PKM

Tttttztytxtq )]( ),( ),( ),( ),( ),([ )( ψθϕ=

Tlllllll ] , , , , ,[ 654321=

Forward Kinematics:

.lJq =

⋅

Inverse Kinematics: 1

. ⋅−= qJl

Modelling: Kinematics

Cartesian:

Joint:


In Task Space, the PKM’s Inverse Dynamics using Euler-Lagrange Formula is:

)( ) ,( )( qGqqNqqM cccm

−⋅−⋅⋅−++=τ

)( qM c : Inertia matrice ) ,( ⋅qqNc : Coriolis and centrifuge wrenches

)(qGc : Gravity forces )(tτ : Actuator torques

Inverse DynamicsFind torques as a function of motion displacement, velocity and acceleration

Direct Dynamics

⎥⎦

⎤⎢⎣

⎡−−=

−⋅⋅−−)( ) ,( )(

-1..qGqqqNqMq ccmc τ

From current values of motion displacement, velocity, acceleration and giventorques, Find Cartesian (or joint ) coordinates at a next sampling time.

Modelling: Dynamics


+⎥⎥⎦

⎤

⎢⎢⎣

⎡

⎥⎥⎦

⎤

⎢⎢⎣

⎡+

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡−

⎥⎥⎦

⎤

⎢⎢⎣

⎡=

−−

×

×

××

××+ )( ) ,( )( 2 1

-

211

1-

66

66

2

6666

66661 kckkckc

k

k

kk

k XGXXNXMIh

IhX

IOIhIX

kkc

k

kXM

Ih

Ih][ )( 2 1

1

66

66

2

τ⎥⎥⎦

⎤

⎢⎢⎣

⎡

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡ −−

×

×

[ ] [ ] [ ]mcccc XM

OXGXXNXM

XOOIOX τ

)(

)( ) ,( )(

0

11

66

12111

16

6666

6666

⎥⎥⎦

⎤

⎢⎢⎣

⎡+⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡

⎥⎦⎤

⎢⎣⎡ +

−⎥⎥⎦

⎤

⎢⎢⎣

⎡= −

×−

×

××

××⋅

Discrete Time State Representation

Continuous Time State Representation

) , ,( kkkdhXf

kτ=

Modelling: Discrete-Time Dynamics

Let TttttztytxtqtX )]( ),( ),( ),( ),( ),([ )( )(1 ψθϕ== )()( 1.

2 tXtX =,

TtXtXtX )]( ),([ )( 21=Robot State


Find actuator torques and sampling periods Continuous-Time

)),(( htτ ∈ Η×ℂ

Solution to⎪⎭

⎪⎬⎫

⎪⎩

⎪⎨⎧∫ ++

ℜ∈ℜ∈ +

t

t

TT

f

dttQXtXtRtttt

f

0

226

0

))()(21 ))()(( Min

)( ,

ιτττ

RιQ Positive definite matrices : Electric and Kinetic Energy Level-Headedness

Positive scalar : Time Level-Headedness

Discrete-Time ,1 ,),...,,( 621 ,...,NkT

kkkk == ττττFind actuator torques and

Sampling periods ),...,,( 21 Nhhh ) ( Min

122

6 ⎭⎬⎫

⎩⎨⎧∑ ++

=×

+ℜ∈ℜ∈

N

kkkkkk

N

N

hQXXR TT

h

ιτττ

Solution to

Modelling: Performance Index


LimitsLimits on on jacobianjacobian’’ss condition condition numbernumber

MaxMin )Cond( κκ << kX ))(Cond( )Cond( kk XJX =

Discret Discret dynamicdynamic modelmodel ),,(1 kkkdk hXfXk

τ=+ 110 −= ,...,N,k

Required passage configurations:Required passage configurations:(pl, Rl) Including Initial and Target states

pPassThlT pp ≤− l

RllT

l Teps PassTh )Rvect(R ≤=

Modelling: Associated Constraints

,

,...,Ll 1=

,

,


LimitsLimits on on intermediateintermediate strutstrut lengthslengths

{ }{ } 1 ,6,...,2,1 , such that , max,min, ,...,NkiD ikiikiad=∈<<= τττττ

LimitsLimits on on actuatoractuator torquestorques

{ }maxmin such that , hhhhD kkhad <<=

LimitsLimits on on samplingsampling periodsperiods

iMax i

kiMin lll <<

)( . XLLl iMaxiMaxiMaxMaxi Θ== ,...,N,ki 21 and ,6,...,1 ==

0 ) ,(3 ≤τXgi0 )(1 =Xsl

Equality and inequality constraints are noted as:

Modelling: Associated Constraints


= ) , , , , ,( zphXL λτμ [ ]∑=

++N

kk

Tkk

Tkk QXXh

122 U ιττ

{ }+−∑−

=++ ),,((

1

011

N

kkkkdk

Tk hXfX

kτλ+Ψ∑ ∑∑

−

= =

−

= ))( ,(

1

0

I

1

1

1

N

kk

li

ik

i

i

L

lk Xszh

Sμ

∑ ∑−

= =+Φ

1

0

J

1 )) ,( ,(

N

k jkkj

jk

jk Xgph

gτ

μ ))( ,())( ,( 222

111

SSN

LNNN

LNN XszhXszh

μμΨ+Ψ

Augmented Lagrangian:

Penalty functions:

bbaba TSS

)2 ( ) ,( μμ

+=Ψ ⎭⎬⎫

⎩⎨⎧ −+=Φ

22 ) ,0(Max

21 ) ,( ababa g

gg μ

μμ

Augmented Lagrangian Approach


OK so far,

Evaluate and derivate -with respect to 12 state components- theright hand member of the dynamic state equation) , ,( kkkd hXf

kτ

) ,( and )( of sExpression ,In 211 XXNXMf ccdktake several pages to display!!

What about their inverse ? The derivatives of the inverse ?

Karush-Kuhn-Tucker (KKT) conditions give: a solution

and Lagrange multipliers and penalty coefficients

kkk hX ,,τ

) , ,( kkk zpλ ) ,( Sg μμμ =

kk X

L∂∂=−

μλ 1Now Calculate the co-states

(Co-states Calculation)Augmented Lagrangian Approach


The control law :

)( ) ,( )( qGqqNvqM ccc−⋅−−

++=Γ

allows the robot to have a linear and decoupled behavior as:

vq ..=

where is an auxiliary input.v

[ ] [ ] 16

66

66

2

6666

66661 2

×

×

×

××

××+

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡+

⎥⎥⎦

⎤

⎢⎢⎣

⎡= k

k

k

kk

k vIh

Ih

XIOIhIX

This gives Linear Simple Dynamics

Decoupled Formulation


Transfer Non Linearity to Objective Function

kkcT

kckkckkc

N

kv

Dd vXMXGXXNvXME

kh

)((U))() ,()(([[Min 11211

1

0

−−−−−

=∈

++⎪⎩

⎪⎨⎧

= ∑∈ΗC

])( 1kc XG−

+ ] }kkTk hQXX 22 ι++

Constraints remain the same, Except for actuator torques:

MaxkckkckkcMin XGXXNvXM ττ 1211 )() ,( )( <++<−−−

Decoupled Formulation

) ,( 21 kkc XXN−

+


Set: initial and final state positions, Limits on strut lengths, torques, accelerations, and sampling periods, Lagrange multipliers. Feasible tolerance, Infeasible tolerance, Convergence tolerances, Total number of sampling periods, and iterations.

Primal Optimization: Time-energy-Feasible Solution

1st Th.shold

Backward computation of the co-states Compute gradients, kλ D

kμL

h∇ D

kvLμ∇

State ComputationTime Minimisation

] [ /PROJ 1 hk

hkhad dkhDkh σ−=+

) , ,( arg hkkkk

Dhk dhvXLMin σσ μ

σ+=

Acceleration Minimisation

}6,..,1{ ] [ /PROJ 1 ∈−=+ jdvDv kv

vjk

jlkvad

jk σ

) , ,( arg kvkkk

Dvk hdvXLMin σσ μ

σ+=

Data Reading

Cost minimized, Constraints not violated ?

Cost minimised, Constraints not violated ?With Optimum Tolerances

2nd Th.shold

Unfeas. Tol.

Dual OptimizationDisplay Optimal

Trajectory

tkk XX )( =∗

tkk hh )( =∗tkk vv )( =∗No Optimal Solution

Stop Progarm

tkX )(

Update Lag.Multipliers

) ,( zp

Update Penalty

) ,( Sg μμ

*T

Reduce Tol.

ttw η , Conv. Test

Max IterReached

Feas. Test

Yes

Yes

No

No

No Yes

ALD ArchitectureDecoupled Formulation:


( )yxJ

yyJ xl

2

1 =⎟⎟⎠

⎞⎜⎜⎝

⎛

⎥⎦

⎤⎢⎣

⎡=⎥

⎦

⎤⎢⎣

⎡−+−−−+== −

2221

1211

2

11

1 ) /() (1 ) /() (

JJJJ

yyRrxyyRxrJJJ xl

Example: 2-dof PKM

0 =lJ 0 ≠xJand 1yy = 2yy =or

When first or second leg is parallel to the x-axis

Four bars of the parallelogram in one of thetwo legs are parallel to each other

! RrL ≠+Design Issue0=lJ0=xJTTwo legs are both parallel to the x-axis:

and

0≠lJ 0=xJand

Singularity analysis:

Kinematic model :


1 )2-)1- (( ε>kkkk yyyy1st singularity

2 ) ))(sgn(( ε>+ r-Rkxkx2nd singularity

pPassThlT pp ≤− l

Workspace limitations:

MaxkMinx xx << MaxkMiny yy <<

Imposed passage through positions

Constraints Formulation

Example: 2-dof PKM

Pl, l=1,…,L

Passage threesholdpPassThlT


pm kg=200 Sm kg= 60 lm kg= 557

mr 750.0 =mR 2030.1 = mL 9725.1 =

Numerical Simulations:

Robot parameters:

]80. ,8.0[ mmx −∈ ]71814.0,71814.1[ mmy −−∈Workspace limitations:

ι R Q Set: Time, Electric and Kinetic Energy Level-headness to 1, I2x2

Algorithm parameters:

TInner Maximum Number of outer iterations

Task requirements:Vmax = 0.2m/sec Tinit = 2sec

(m) 0.1)- ,47.0( ) ,( 00 −=yx (m) 1.6)- ,7.0( ) ,( FF =yx

-3**1* 10 === ηηwConvergence tolerances , Unfeasible tolerances 1.2 2=γ

Simulation Data:

Example: 2-dof PKM

00 , zpSet initial Lagrange parameters to zero, Feasible tolerances 10-2

N Number of trajectory discretisations 1010

*T Maximum Number of outer iterations 15


Initialisation of sampling periods

Ntt

tth fkkk

01

−=−= + 121 −= ,...,N,k

Initialisation of actuator torques:

From the feasible velocity profile initial trajectory

And corresponding sampling periods

Compute a initial torque sequence from inverse dynamics1,..., −No ττ

No XX ,...,

No hh ,...,

Numerical SimulationsExample: 2-dof PKM

Then apply Augmented Lagrangian with Decoupling


Numerical Simulations

Augmented Lagrangian outcomeskinematic simulation outcomes

Example: 2-dof PKM


AL (Minimum Energy)AL (Minimum Time Energy)

Numerical SimulationsExample: 2-dof PKM


AL with initial mass mp =200 kg


AL with modified mass mp =300 kg

Example: 2-dof PKM



Remarks

Initial solution : kinematically feasible,

Augmented lagrangian, smooth and monotonous increasing of energy consumption variations

Multi-criteria approach Minimum time is 15% faster than for the trajectorycomputed with only minimum energy criterion.

pm kg=300Needed actuator torques & necessary energy & time to achieve thesame task are bigger. Nonetheless, the algorithm converges and gets to the target with an acceptable precision of order of 10-3.

Modified mass

Example: 2-dof PKM

Corresponding torques: Outside the admissible domain.


Conclusions

Multi-objective offline programming system

Under several constraints related to the robot-task-workspaceRobot dynamic model with Lagrange formalism includingmoving platform and struts

Optimal time-energy trajectory planning

Validation through a 2 dof planar parallel robot

Variational Calculus ApproachNon linear and Non convex optimal control problem.Solution: Augmented Lagrangian with Decoupling

Short term PerspectiveInclude Obstacle Avoidance in the Optimal Trajectory Planning System

Date post:	01-Jun-2020
Category:	Documents
Upload:	others
View:	1 times
Download:	0 times

A Decoupled Approach to Optimal Time-Energy Trajectory ... · Romansy’06 , Warsaw University of...

Documents