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
2/24Romansy’06 , Warsaw University of Technology, June 24-26 2006
Outline
• Introduction
• Modelling
• Augmented Lagrangian Approach
• Decoupled Formulation
• Example
• Conclusions et perspectives
3/24Romansy’06 , Warsaw University of Technology, June 24-26 2006
Introduction: Offline Programming Framework
4/24Romansy’06 , Warsaw University of Technology, June 24-26 2006
Flight Simulator Serie 500 by CAE
Example of a PKMIntroduction:
5/24Romansy’06 , Warsaw University of Technology, June 24-26 2006
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:
6/24Romansy’06 , Warsaw University of Technology, June 24-26 2006
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
7/24Romansy’06 , Warsaw University of Technology, June 24-26 2006
+⎥⎥⎦
⎤
⎢⎢⎣
⎡
⎥⎥⎦
⎤
⎢⎢⎣
⎡+
⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡−
⎥⎥⎦
⎤
⎢⎢⎣
⎡=
−−
×
×
××
××+ )( ) ,( )( 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
8/24Romansy’06 , Warsaw University of Technology, June 24-26 2006
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
9/24Romansy’06 , Warsaw University of Technology, June 24-26 2006
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=
,
,
10/24Romansy’06 , Warsaw University of Technology, June 24-26 2006
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
11/24Romansy’06 , Warsaw University of Technology, June 24-26 2006
= ) , , , , ,( 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
12/24Romansy’06 , Warsaw University of Technology, June 24-26 2006
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
13/24Romansy’06 , Warsaw University of Technology, June 24-26 2006
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
14/24Romansy’06 , Warsaw University of Technology, June 24-26 2006
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−
+
15/24Romansy’06 , Warsaw University of Technology, June 24-26 2006
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:
16/24Romansy’06 , Warsaw University of Technology, June 24-26 2006
( )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 :
17/24Romansy’06 , Warsaw University of Technology, June 24-26 2006
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
18/24Romansy’06 , Warsaw University of Technology, June 24-26 2006
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
19/24Romansy’06 , Warsaw University of Technology, June 24-26 2006
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
20/24Romansy’06 , Warsaw University of Technology, June 24-26 2006
Numerical Simulations
Augmented Lagrangian outcomeskinematic simulation outcomes
Example: 2-dof PKM
21/24Romansy’06 , Warsaw University of Technology, June 24-26 2006
AL (Minimum Energy)AL (Minimum Time Energy)
Numerical SimulationsExample: 2-dof PKM
22/24Romansy’06 , Warsaw University of Technology, June 24-26 2006
AL with initial mass mp =200 kg
Numerical Simulations
AL with modified mass mp =300 kg
Example: 2-dof PKM
23/24Romansy’06 , Warsaw University of Technology, June 24-26 2006
Numerical Simulations
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.
24/24Romansy’06 , Warsaw University of Technology, June 24-26 2006
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