TMHP51 Servomechanisms (HT2012)
Lecture 03
Numerics
Model algorithm
Model refinement
Magnus Sethson
magnus.sethson@liu.se
1
1
Numerics
magnus.sethson@liu.se
2
2
Explicit Euler Method (Forward Euler)
The explicit Euler method is a general method for integrating
functions on a discreet grid. In simulation techniques the grid is
often one-dimensional time.
x[(n + 1) T ] = x[n T ] +
magnus.sethson@liu.se
T ẋ[n T ]
3
3
Implicit Euler Method (Backward Euler)
The implicit Euler method is also a general method for
integrating functions on a discreet grid. The numerical stability
is far better than the implicit, but it often comes with a need of
solving non-linear functions numerically in every time step.
x[(n + 1) T ] = x[n T ] +
magnus.sethson@liu.se
T ẋ[(n + 1) T ]
4
4
Explicit Trapetzoidal Method
The trapetzoidal method is a natural extension to the explicit
Euler method. It has far better frequency performance. It also
shares some characteristics with the transmission line modeling
and is therefore a highly suitable numeric method for hydraulic
systems simulation.
T
x[(n + 1) T ] = x[n T ] +
(ẋ[(n + 1) T ] + ẋ[n T ])
2
magnus.sethson@liu.se
5
5
Model Simulation
f
magnus.sethson@liu.se
t
6
6
Non-linear Model
8
p
A
B
ẋ
F
=
M
ẍ
>
L
p
p
p
L
t
p
>
>
<
q = Ap ẋp + 41 Vet ṗL
>
q
>
>
: q = Cq wxv 1 (Ps pL )
⇢
xv : Input signal
xp : Output signal
FL : External disturbance
xp , ẋp , pL : Internal states
magnus.sethson@liu.se
7
7
Explicit Euler Method
magnus.sethson@liu.se
8
8
Solving using Explicit Euler Method
0
1
0
1
xp [(n + 1) T ]
xp [n T ]
@ vp [(n + 1) T ] A = @ vp [n T ] A +
pL [(n + 1) T ]
pL [n T ]
0
1
0
ẋp
B
@ v̇p A = B
B
@
ṗL
magnus.sethson@liu.se
0
ẋp [n T ]
T @ v̇p [n T ] A
ṗL [n T ]
1
vp
4 e
Vt
⇣
p L Ap Bp v p F L
Mt
Cq wxv
q
1
⇢ (Ps
pL )
1
Ap v p
C
C
C
⌘ A
9
9
Sort equations, find iterative algorithm
q[(n + 1) T ] = Cq wxv [(n + 1) T ]
ṗL [(n + 1)
pL [(n + 1)
ẍp [(n + 1)
ẋp [(n + 1)
xp [(n + 1)
r
1
(Ps
⇢
pL [n T ])
4 e
T] =
(q[(n + 1) T ] Ap ẋp [n T ])
Vt
T ] = pL [n T ] + T ṗL [(n + 1) T ]
1
T] =
(pL [(n + 1) T ]Ap Bp ẋp [n T ]
Mt
T ] = ẋp [n T ] + T ẍp [(n + 1) T ]
T ] = xp [n T ] + T ẋp [(n + 1) T ]
FL [(n + 1) T ])
t
magnus.sethson@liu.se
10
10
From model, to algorithm, to code
Unr
ead
able
void servo_system(unsigned int nsamp) {
/* parameters and variables */
double pL=0; /* load pressure, initially zero, system state */
double Ps=70e5; /* supply pressure, 70 [bar] */
double rho=790; /* oil density, 790 [kg/m^3] */
double xv=0; /* spool displacement, (system input), initially zero*/
double w=8e-3; /* area gradient, [-] */
double Cq=0.67; /* flow coefficient, standard value, 0.67 [-] */
double dT=0; /* numerical integration time step size */
double T=0; /* time [s] */
double q=0; /* flow, [m^3/s] */
double Ap=0.001963495408; /* piston area, [m^2] (value from 50mm piston diameter) */
double vp=0; /* piston velocity, initially zero, system state */
double betae=1.5e9; /* bulk modulus of oil, [N/m^2] */
double Vt=0.001570796327; /* total cylinder volume, [m^3] (value from 50mm piston and 0.8m cylinder length) */
double Bp=5e4; /* system damping, [Ns/m] */
double Mt=500; /* system mass load, [kg] */
double xp=0; /* piston position within cylinder, [m] (main output), system state */
double FL=0; /* external load */
/* iteration variable */
unsigned long int n=0; /* iteration counter */
/* set up time step */
dT=0.2/nsamp;
/* write first line for T=0 */
printf("\"Te%u\",\"XVe%u\",\"PLe%u\",\"VPe%u\",\"XPe%u\"\n",nsamp,nsamp,nsamp,nsamp,nsamp);
printf("%.16f,%.16f,%.16f,%.16f,%.16f\n",T,xv,pL,vp,xp);
/* iterate and write result for nsamp time steps */
for(n=1;n<nsamp;n++) {
if(0.01<=T) xv=0.0001; /* on step T=0.01 the input is set to 0.1mm valve displacement */
if(0.08<=T) xv=0; /* on step T=0.08 the input is reset and valve is closed */
q=Cq*w*xv*sqrt((Ps-pL)/rho);
pL=pL+(q-Ap*vp)*4*betae/Vt*dT;
vp=vp+(pL*Ap-Bp*vp-FL)/Mt*dT;
xp=xp+vp*dT;
T=T+dT; /* increment time */
printf("%.16f,%.16f,%.16f,%.16f,%.16f\n",T,xv,pL,vp,xp);
}
}
magnus.sethson@liu.se
11
11
DEMO: Solving the model in plain C
Unr
ead
able
#include <stdio.h>
#include <math.h>
void servo_system(unsigned int nsamp) {
DEMO
DEMO
DEMO
/* parameters and variables */
double pL=0; /* load pressure, initially zero, system state */
double Ps=70e5; /* supply pressure, 70 [bar] */
double rho=790; /* oil density, 790 [kg/m^3] */
double xv=0; /* spool displacement, (system input), initially zero*/
double w=8e-3; /* area gradient, [-] */
double Cq=0.67; /* flow coefficient, standard value, 0.67 [-] */
double dT=0; /* numerical integration time step size */
double T=0; /* time [s] */
double q=0; /* flow, [m^3/s] */
double Ap=0.001963495408; /* piston area, [m^2] (value from 50mm piston diameter) */
double vp=0; /* piston velocity, initially zero, system state */
double betae=1.5e9; /* bulk modulus of oil, [N/m^2] */
double Vt=0.001570796327; /* total cylinder volume, [m^3] (value from 50mm piston and 0.8m cylinder length) */
double Bp=5e4; /* system damping, [Ns/m] */
double Mt=500; /* system mass load, [kg] */
double xp=0; /* piston position within cylinder, [m] (main output), system state */
double FL=0; /* external load */
/* iteration variable */
unsigned long int n=0; /* iteration counter */
/* set up time step */
dT=0.2/nsamp;
/* write first line for T=0 */
printf("\"Te%u\",\"XVe%u\",\"PLe%u\",\"VPe%u\",\"XPe%u\"\n",nsamp,nsamp,nsamp,nsamp,nsamp);
printf("%.16f,%.16f,%.16f,%.16f,%.16f\n",T,xv,pL,vp,xp);
/* iterate and write result for nsamp time steps */
for(n=1;n<nsamp;n++) {
if(0.01<=T) xv=0.0001; /* on step T=0.01 the input is set to 0.1mm valve displacement */
if(0.08<=T) xv=0; /* on step T=0.08 the input is reset and valve is closed */
q=Cq*w*xv*sqrt((Ps-pL)/rho);
pL=pL+(q-Ap*vp)*4*betae/Vt*dT;
vp=vp+(pL*Ap-Bp*vp-FL)/Mt*dT;
xp=xp+vp*dT;
T=T+dT; /* increment time */
printf("%.16f,%.16f,%.16f,%.16f,%.16f\n",T,xv,pL,vp,xp);
}
}
int main(int argc,char** argv) {
unsigned int nsamp=2000;
if(2==argc) sscanf(argv[1],"%u",&nsamp);
servo_system(nsamp);
return 0;
}
gcc -Wall -pedantic -o servo_explicit_euler servo_explicit_euler.c -lm
./servo_explicit_euler > servo_explicit_euler.csv
magnus.sethson@liu.se
12
12
Results Explicit Euler Method
Simulation Example, Explicit Euler Method
0.10
35
2.0
30
1.8
10
1.6
25
0.08
1.4
20
1.2
10
1.0
Xp [mm]
15
Vp [mm/s]
0.06
PL [bar]
Xv [mm]
5
0.8
0.04
0
5
0
0.02
Variables
Xv vs Time
PL vs Time
0
0
0.02
0.04
0.06
0.08
0.10
0.12
0.14
0.16
0.18
−5
Vp vs Time
Xp vs Time
0.20
0.22
0.6
0.4
−5
0.2
−10
0
Time [s]
magnus.sethson@liu.se
13
13
Results, Step Comparison
Simulation example, Euler's explicit method
2.0
Position Xp [mm]
1.5
1.0
0.5
Timesteps
ΔT=0.0167 (NaN)
ΔT=0.0111 (NaN)
ΔT=0.0091 (NaN)
ΔT=0.0080
ΔT=0.0040
ΔT=0.0020
ΔT=0.0010
ΔT=0.0001
0
−0.5
−1.0
0
magnus.sethson@liu.se
0.02
0.04
0.06
0.08
0.10 0.12
Time [s]
0.14
0.16
0.18
0.20
0.22
14
14
Implicit Euler Method
magnus.sethson@liu.se
15
15
Solving using Implicit Euler Method
0
1
0
1
xp [(n + 1) T ]
xp [n T ]
@ vp [(n + 1) T ] A = @ vp [n T ] A +
pL [(n + 1) T ]
pL [n T ]
0
1
0
ẋp [(n + 1) T ]
B
@ v̇p [(n + 1) T ] A = B
B
@
ṗL [(n + 1) T ]
magnus.sethson@liu.se
0
1
ẋp [(n + 1) T ]
T @ v̇p [(n + 1) T ] A
ṗL [(n + 1) T ]
vp [(n + 1) T ]
4 e
Vt
⇣
pL [(n+1) T ]Ap Bp vp [(n+1) T ] FL [(n+1) T ]
Mt
Cq wxv [(n + 1) T ]
q
1
⇢ (Ps
pL [(n + 1) T ])
Ap vp [(n + 1) T ]
1
C
C
C
⌘ A
16
16
Solving using Implicit Euler Method (cont.)
0
0
B
B
B
@
vp [(n + 1) T ]
4 e
Vt
⇣
ẋp [(n + 1) T ]
@ v̇p [(n + 1) T ] A =
ṗL [(n + 1) T ]
1
pL [(n+1) T ]Ap Bp vp [(n+1) T ] FL [(n+1) T ]
Mt
Cq wxv [(n + 1) T ]
magnus.sethson@liu.se
q
1
⇢ (Ps
pL [(n + 1) T ])
1
Ap vp [(n + 1) T ]
C
C
C
⌘ A
17
17
Newton-Raphson Method
The Newton-Raphson method for solving non-linear equations
numerically is widely adopted in many applications. Even if
originally defined in one dimension it is available in multidimensional versions also. It iterates and moves an initial guess
towards a near-by root-locus.
f (xk )
f 0 (xk )
xk+1 = xk
xk+1 = xk
J
1
f (xk )
One-dimensional
Multi-dimensional
@fi (x1...N )
Jij =
@xj
Hint:
f (xk+1 )
xk+1
f (xk )
⇡ f 0 (xk )
xk
magnus.sethson@liu.se
18
18
The Jacobian
0
B
B
TB
@
4 e
Vt
f (xp [(n + 1) T ], vp [(n + 1) T ], pL [(n + 1) T ]) =
0
1 0
1
xp [(n + 1) T ]
xp [n T ]
@ vp [(n + 1) T ] A @ vp [n T ] A
pL [(n + 1) T ]
pL [n T ]
1
vp [(n + 1) T ]
C
pL [(n+1) T ]Ap Bp vp [(n+1) T ] FL [(n+1) T ]
C
C
Mt
⇣
⌘ A
q
Cq wxv [(n + 1) T ] ⇢1 (Ps pL [(n + 1) T ]) Ap vp [(n + 1) T ]
magnus.sethson@liu.se
19
19
The Jacobian (cont.)
xk+1
J11 = 1
J12 =
xk = J
1
f (xk )
T
The Newton-Raphson method is often
solved like a linear set of equations,
avoiding the full Jacobian inverse.
J13 = 0
J21 = 0 (No spring involved!)
Bp
J22 = 1 +
T
Mt
Ap
J23 =
T
Mt
J31 = 0
4 e Ap
J32 =
T
Vt
4 e ⇢Cq wxv [(n + 1) T ][k]
p
J33 = 1 +
T
Vt 2 Ps pL [(n + 1) T ][k]
magnus.sethson@liu.se
J(xk ) dk+1 = J(xk )[xk+1
xk ] =
f (xk )
20
20
From model, to algorithm, to code
Unr
ead
able
void servo_system(unsigned int nsamp) {
/* parameters and variables */
double pL=0; /* load pressure, initially zero, system state */
double Ps=70e5; /* supply pressure, 70 [bar] */
double rho=790; /* oil density, 790 [kg/m^3] */
double xv=0; /* spool displacement, (system input), initially zero*/
double w=8e-3; /* area gradient, [-] */
double Cq=0.67; /* flow coefficient, standard value, 0.67 [-] */
double dT=0; /* numerical integration time step size */
double T=0; /* time [s] */
double Ap=0.001963495408; /* piston area, [m^2] (value from 50mm piston diameter) */
double vp=0; /* piston velocity, initially zero, system state */
double betae=1.5e9; /* bulk modulus of oil, [N/m^2] */
double Vt=0.001570796327; /* total cylinder volume, [m^3] (value from 50mm piston and 0.8m
cylinder length) */
double Bp=5e4; /* system damping, [Ns/m] */
double Mt=500; /* system mass load, [kg] */
double xp=0; /* piston position within cylinder, [m] (main output), system state */
double FL=0; /* external load */
gsl_vector_set(b,0,(xpk-xp-dT*vpk));
gsl_vector_set(b,1,(vpk-vp-dT*(pLk*Ap-Bp*vpk-FL)/Mt));
gsl_vector_set(b,2,(pLk-pL-dT*4*betae/Vt*(Cq*w*xv*sqrt((Ps-pL)/rho)-Ap*vpk)));
gsl_matrix_set(J,0,0,1.0);
gsl_matrix_set(J,0,1,-dT);
gsl_matrix_set(J,0,2,0.0);
gsl_matrix_set(J,1,0,0.0);
gsl_matrix_set(J,1,1,1+Bp/Mt*dT);
gsl_matrix_set(J,1,2,-Ap/Mt*dT);
gsl_matrix_set(J,2,0,1.0);
gsl_matrix_set(J,2,1,-4*betae*Ap/Vt*dT);
gsl_matrix_set(J,2,2,1.0+2*betae/Vt*rho*Cq*w*xv/sqrt(Ps-pLk)*dT);
gsl_matrix_memcpy(A,J);
gsl_blas_dgemv(CblasNoTrans, 1.0, J, xk, -1.0, b);
gsl_linalg_LU_decomp(J,p,&s);
gsl_linalg_LU_solve(J,p,b,x);
gsl_linalg_LU_refine(A,J,p,b,x,xk);
xpk=gsl_vector_get(x,0);
vpk=gsl_vector_get(x,1);
pLk=gsl_vector_get(x,2);
k++;
} while(k<20 && (abs(gsl_vector_get(x,0)/(xp+EPS))>RAC || abs(gsl_vector_get(x,1)/(vp
+EPS))>RAC || abs(gsl_vector_get(x,2)/(pL+EPS))>RAC));
/* GSL related structures */
gsl_matrix* J=NULL;
gsl_matrix* A=NULL;
gsl_vector* b=NULL;
gsl_vector* x=NULL;
gsl_vector* xk=NULL;
int s=0;
gsl_permutation* p=NULL;
double xpk=0;
double vpk=0;
double pLk=0;
/* iteration variable */
unsigned long int n=0; /* iteration counter */
unsigned long int k=0; /* numerical counter */
/* set up time step */
dT=0.2/nsamp;
xp=xpk;
vp=vpk;
pL=pLk;
/* set up GSL parts */
J=gsl_matrix_alloc(3,3);
A=gsl_matrix_alloc(3,3);
b=gsl_vector_alloc(3);
x=gsl_vector_alloc(3);
xk=gsl_vector_alloc(3);
p=gsl_permutation_alloc(3);
}
/* write first line for T=0 */
printf("\"Ti%u\",\"XVi%u\",\"PLi%u\",\"VPi%u\",\"XPi%u\"\n",nsamp,nsamp,nsamp,nsamp,nsamp);
printf("%.16f,%.16f,%.16f,%.16f,%.16f\n",T,xv,pL,vp,xp);
/* iterate and write result for nsamp time steps */
for(n=1;n<nsamp;n++) {
if(T>=0.01) xv=0.0001; /* on step T=0.01 the input is set to 0.1mm valve displacement */
if(T>=0.08) xv=0; /* on step T=0.08 the input is reset and valve is closed */
}
T=T+dT; /* increment time */
printf("%.16f,%.16f,%.16f,%.16f,%.16f\n",T,xv,pL,vp,xp);
/* clean up GSL */
gsl_permutation_free(p);
gsl_vector_free(xk);
gsl_vector_free(x);
gsl_vector_free(b);
gsl_matrix_free(A);
gsl_matrix_free(J);
/* implicit iteration loop */
k=0;
xpk=xp;
vpk=vp;
pLk=pL;
do {
gsl_vector_set(xk, 0, xpk);
gsl_vector_set(xk, 1, vpk);
gsl_vector_set(xk, 2, pLk);
magnus.sethson@liu.se
21
21
Results Implicit Euler Method
Simulation Example, Implicit Euler Method
0.10
35
2.0
30
1.8
10
1.6
25
0.08
1.4
20
1.2
10
1.0
Xp [mm]
15
Vp [mm/s]
0.06
PL [bar]
Xv [mm]
5
0.8
0.04
0
5
0
0.02
Variables
Xv vs Time
PL vs Time
0
0
0.02
0.04
0.06
0.08
0.10
0.12
0.14
0.16
0.18
−5
Vp vs Time
Xp vs Time
0.20
0.22
0.6
0.4
−5
0.2
−10
0
Time [s]
magnus.sethson@liu.se
22
22
Explicit Trapetzoid Method
magnus.sethson@liu.se
23
23
From model, to algorithm, to code
void servo_system(unsigned int nsamp) {
/* parameters and variables */
double pL=0; /* load pressure, initially zero, system state */
double Ps=70e5; /* supply pressure, 70 [bar] */
double rho=790; /* oil density, 790 [kg/m^3] */
double xv=0; /* spool displacement, (system input), initially zero*/
double w=8e-3; /* area gradient, [-] */
double Cq=0.67; /* flow coefficient, standard value, 0.67 [-] */
double dT=0; /* numerical integration time step size */
double T=0; /* time [s] */
double q=0; /* flow, [m^3/s] */
double Ap=0.001963495408; /* piston area, [m^2] (value from 50mm piston diameter) */
double vp=0; /* piston velocity, initially zero, system state */
double betae=1.5e9; /* bulk modulus of oil, [N/m^2] */
double Vt=0.001570796327; /* total cylinder volume, [m^3] (value from 50mm piston and 0.8m cylinder length) */
double Bp=5e4; /* system damping, [Ns/m] */
double Mt=500; /* system mass load, [kg] */
double xp=0; /* piston position within cylinder, [m] (main output), system state */
double FL=0; /* external load */
Unr
ead
able
/* trapetzoid related storage */
double npL=0;
double nvp=0;
double nxp=0;
double spL=0;
double svp=0;
double sxp=0;
/* iteration variable */
unsigned long int n=0; /* iteration counter */
/* set up time step */
dT=0.2/nsamp;
/* write first line for T=0 */
printf("\"Te%u\",\"XVe%u\",\"PLe%u\",\"VPe%u\",\"XPe%u\"\n",nsamp,nsamp,nsamp,nsamp,nsamp);
printf("%.16f,%.16f,%.16f,%.16f,%.16f\n",T,xv,pL,vp,xp);
/* iterate and write result for nsamp time steps */
for(n=1;n<nsamp;n++) {
if(0.01<=T) xv=0.0001; /* on step T=0.01 the input is set to 0.1mm valve displacement */
if(0.08<=T) xv=0; /* on step T=0.08 the input is reset and valve is closed */
spL=npL;
svp=nvp;
sxp=nxp;
q=Cq*w*xv*sqrt((Ps-pL)/rho);
npL=(q-Ap*vp)*4*betae/Vt;
pL=pL+0.5*(npL+spL)*dT;
nvp=(pL*Ap-Bp*vp-FL)/Mt;
vp=vp+0.5*(nvp+svp)*dT;
nxp=vp;
xp=xp+0.5*(nxp+sxp)*dT;
T=T+dT; /* increment time */
printf("%.16f,%.16f,%.16f,%.16f,%.16f\n",T,xv,pL,vp,xp);
}
}
magnus.sethson@liu.se
24
24
Results Explicit Trapetzoid Method
Simulation Example, Explicit Trapetzoid Method
0.10
35
2.0
30
1.8
10
1.6
25
0.08
1.4
20
1.2
10
1.0
Xp [mm]
15
Vp [mm/s]
0.06
PL [bar]
Xv [mm]
5
0.8
0.04
0
5
0
0.02
Variables
Xv vs Time
PL vs Time
0
0
0.02
0.04
0.06
0.08
0.10
0.12
0.14
0.16
0.18
−5
Vp vs Time
Xp vs Time
0.20
0.22
0.6
0.4
−5
0.2
−10
0
Time [s]
magnus.sethson@liu.se
25
25
Results, Method Comparison
Simulation Example, Method Comparison (ΔT=20μs)
2.0
0.10
1.8
10
1.6
1.80
0.08
1.4
0.091
0.092
1.76
4.8
0.8
0.04
0
4.6
Variables
Xv vs Time
PL,implicit vs Time
PL,trapetz vs Time
PL,explicit vs Time
Xp,implicit vs Time
Xp,trapetz vs Time
Xp,explicit vs Time
4.4
0.02
0.038
0
1.0
Xp [mm]
0.06
0.090
1.2
5
PL [bar]
Xv [mm]
1.78
0
0.02
0.04
0.039
0.06
0.040
4.2
0.08
0.10
0.12
0.14
0.16
0.18
0.20
0.6
0.4
−5
0.2
0.22
0
Time [s]
magnus.sethson@liu.se
26
26
Model Extensions
magnus.sethson@liu.se
27
27
Add Physics!!!
•
•
•
•
•
•
•
•
•
•
•
•
magnus.sethson@liu.se
Internal leakage
External springs
Asymmetric cylinder
End-of-Stroke conditions
Laminar flow
Valve dynamics
Supply system dynamics
Asymmetric valve orifice
Separate meter-in and meter-out
Piston friction
Long lines between cylinder and valve
Wave propagation in lines
28
28
Dynamic Mathematical Model Principles
Orifice
r
2
q = Cq wxv
(Pa
⇢
Pb )
Continuity
qin
qout
dV
V dp
=
+
dt
e dt
Force Balance
p 1 A1
magnus.sethson@liu.se
P2 A 2
FL
BL ẋp = Mt ẍp
29
29
Textual Simulation Computer Languages
C
magnus.sethson@liu.se
Matlab
VHDL-AMS
Haskell
C++
Modelica
Fortran 77
Haskell
Phyton
30
30
DEMO: Simple hydraulic servo system in plain C
/* An example of a simulation program for a very simple hydraulic servo
system */
#include <stdlib.h>
#include <stdio.h>
#include <math.h>
DEMO
DEMO
DEMO
#define PI 3.141592653589793
void warning(const char* msg,const char* fn,const int line) {
fprintf(stderr, "WARNING! %s(%d) : %s\n",fn,line,msg);
}
void cavitation(double p,const char* comp,const char* fn,const int line) {
if(p<0.7e5) {
fprintf(stderr, "WARNING! %s(%d) : CAVITATION in %s (p=%.16f)
\n",fn,line,comp,p);
}
}
double orifice(double Cq,double w,double xv,double rho,double p1,double p2) {
double q=0;
if(xv<0) return 0;
if(Cq<0) return 0;
if(w<0) return 0;
if(rho<0) return 0;
if(p1<0) return 0;
if(p2<0) return 0;
cavitation(p1, "orifice p1",__FILE__,__LINE__);
cavitation(p2, "orifice p2",__FILE__,__LINE__);
if((p1-p2)>0) {
q=+Cq*w*xv*sqrt(2/rho*(p1-p2));
} else {
q=-Cq*w*xv*sqrt(2/rho*(p2-p1));
}
return q;
}
double area(double Dp,double Dr) {
double A;
if(Dp<0) warning("Negative diameter in area()",__FILE__,__LINE__);
if(Dr<0) warning("Negative diameter in area()",__FILE__,__LINE__);
A=PI*(Dp*Dp-Dr*Dr)*0.25;
return A;
}
double volume(double Dp,double D,double L) {
double V=0;
if(L<0) warning("Negative length in volume()",__FILE__,__LINE__);
if(L>=0) V=area(Dp,D)*L;
return V;
}
double compression(double q,double V,double dV,double betae) {
double dp=0;
if(V<0) warning("Negative volume in compression()",__FILE__,__LINE__);
if(V>0) dp=(q-dV)*betae/V;
return dp;
}
double piston(double p1,double p2,double Dp,double d1,double d2) {
double Fc=0;
double A1=0;
double A2=0;
magnus.sethson@liu.se
A1=area(Dp,d1);
A2=area(Dp,d2);
Fc=p1*A1-p2*A2;
return Fc;
Unr
ead
able
double dp2=0;
double dV1=0;
double dV2=0;
}
/* trapetzoid numerical integration */
void trapetz(double dx,double state[3],double dT) {
state[2]=state[1];
state[1]=dx;
state[0]+=0.5*(state[1]+state[2])*dT;
}
double value(double state[3]) {
return state[0];
}
void initialize(double state[3],double x0) {
state[0]=x0;
state[1]=0;
state[2]=0;
}
double spring(double k,double L0,double x) {
double Fs=0;
if(k<0) warning("Negative spring coefficient in
spring()",__FILE__,__LINE__);
Fs=k*(x-L0);
return Fs;
}
void servo_system(unsigned int nsamp,double Tmax) {
/* system states */
double p1[3]; /* positive cylinder chamber pressure */
double p2[3]; /* positive cylinder chamber pressure */
double vp[3]; /* piston velocity */
double xp[3]; /* piston position */
/* parmeters and variables */
double Ps=70e5; /* supply pressure, 70 [bar] */
double Pt=1e5; /* tank pressure, 70 [bar] */
double rho=790; /* oil density, 790 [kg/m^3] */
double xv=0; /* spool displacement, (system input), initially zero*/
double w=8e-3; /* area gradient, [-] */
double Cq=0.67; /* flow coefficient, standard value, 0.67 [-] */
double dT=0; /* numerical integration time step size */
double Dp=0.050; /* piston diameter [m] */
double Dr1=0.020; /* rod diameter in chamber 1 [m] */
double Dr2=0.020; /* rod diameter in chamber 2 [m] */
double V01=0.0001; /* dead volume chamber 1 [m^3] */
double V02=0.0001; /* dead volume chamber 2 [m^3] */
double V1=0; /* total volume in chamber 1 */
double V2=0; /* total volume in chamber 2 */
double L=0.8; /* cylinder length */
double Fc=0; /* total applied force on piston */
double FL=0; /* external load force */
double T=0; /* time [s] */
double q1=0; /* flow into chamber 1, [m^3/s] */
double q2=0; /* flow into chamber 2, [m^3/s] */
double betae=1.5e9; /* bulk modulus of oil, [N/m^2] */
double Bp=5e4; /* system damping, [Ns/m] */
double Mt=500; /* system mass load, [kg] */
unsigned long int n=0; /* iteration counter */
/* temporaries */
double dp1=0;
initialize(p1,
initialize(p2,
initialize(vp,
initialize(xp,
Pt);
Pt);
0);
0);
dT=Tmax/nsamp;
printf("\"T\",\"x_v\",\"p_1\",\"p_2\",\"v_p\",\"x_p\",\"q_1\",
\"q_2\"\n");
n=0;
do {
T=n*dT;
if(0.01<=T) xv=0.0001; /* open valve */
if(0.08<=T) xv=0; /* close valve */
if(xv>=0) {
q1=orifice(Cq, w, xv, rho, Ps, value(p1));
q2=orifice(Cq, w, xv, rho, Pt, value(p2));
} else {
q1=orifice(Cq, w, -xv, rho, Pt, value(p1));
q2=orifice(Cq, w, -xv, rho, Ps, value(p2));
}
if(value(xp)<-0.5*L) {
initialize(xp, -0.5*L);
initialize(vp, 0);
}
if(value(xp)>+0.5*L) {
initialize(xp, +0.5*L);
initialize(vp, 0);
}
dV1=+value(vp)*area(Dp, Dr1);
dV2=-value(vp)*area(Dp, Dr2);
V1=volume(Dp, Dr1, 0.5*L+value(xp))+V01;
V2=volume(Dp, Dr2, 0.5*L-value(xp))+V02;
dp1=compression(q1, V1, dV1, betae);
dp2=compression(q2, V2, dV2, betae);
trapetz(dp1, p1, dT);
trapetz(dp2, p2, dT);
Fc=piston(value(p1), value(p2), Dp, Dr1, Dr2)-FL-Bp*value(vp);
trapetz(Fc/Mt, vp, dT);
trapetz(value(vp), xp, dT);
printf("%.12f,%.12f,%.12f,%.12f,%.12f,%.12f,%.12f,%.12f
\n",T,xv,value(p1),value(p2),value(vp),value(xp),q1,q2);
n++;
} while(n<nsamp);
}
int main(int argc, const char * argv[]) {
unsigned int nsamp=2000;
double Tmax=0.2;
if(2<=argc) sscanf(argv[1],"%u",&nsamp);
if(3==argc) sscanf(argv[2],"%lg",&Tmax);
servo_system(nsamp,Tmax);
return 0;
}
31
31
Simulation Programs
HOPSAN NG (FluMeS, LiU)
Dymola (Dassault Systems)
Amesim (LMS Intl.)
Easy5 (MSC Software)
magnus.sethson@liu.se
Flowmaster (Flowmaster Inc.)
Matlab/Simulink (Mathworks Inc)
32
32
magnus.sethson@liu.se
33
33
Examples
https://mechatronic.iei.liu.se/svn/TMHP_simulation/
magnus.sethson@liu.se
34
34
Next Lecture:
08:15, 2012-11-08, P34
Magnus Sethson
magnus.sethson@liu.se
35
35