TMHP51 Servomechanisms (HT2012)
Lecture 03
Numerics
Model algorithm
Model refinement
Magnus Sethson
[email protected]
1
1
Numerics
[email protected]
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 ] +
[email protected]
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 ] +
[email protected]
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
[email protected]
5
5
Model Simulation
f
[email protected]
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
[email protected]
7
7
Explicit Euler Method
[email protected]
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
[email protected]
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
[email protected]
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);
}
}
[email protected]
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
[email protected]
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]
[email protected]
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
[email protected]
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
[email protected]
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 ]
[email protected]
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 ]
[email protected]
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
[email protected]
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 ]
[email protected]
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]
[email protected]
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);
[email protected]
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]
[email protected]
22
22
Explicit Trapetzoid Method
[email protected]
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);
}
}
[email protected]
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]
[email protected]
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]
[email protected]
26
26
Model Extensions
[email protected]
27
27
Add Physics!!!
•
•
•
•
•
•
•
•
•
•
•
•
[email protected]
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
[email protected]
P2 A 2
FL
BL ẋp = Mt ẍp
29
29
Textual Simulation Computer Languages
C
[email protected]
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;
[email protected]
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)
[email protected]
Flowmaster (Flowmaster Inc.)
Matlab/Simulink (Mathworks Inc)
32
32
[email protected]
33
33
Examples
https://mechatronic.iei.liu.se/svn/TMHP_simulation/
[email protected]
34
34
Next Lecture:
08:15, 2012-11-08, P34
Magnus Sethson
[email protected]
35
35