104 lines
1.7 KiB
Mathematica
104 lines
1.7 KiB
Mathematica
|
clc
|
||
|
|
clear all
|
||
|
|
close all
|
||
|
|
|
||
|
|
%% System properties
|
||
|
|
g = 100000;
|
||
|
|
w0 = 2*pi*.5; % MinusK BM1 tablle
|
||
|
|
l = 0.5; %[m]
|
||
|
|
la = 1; %[m]
|
||
|
|
h = 1.7; %[m]
|
||
|
|
ha = 1.7;% %[m]
|
||
|
|
m = 400; %[kg]
|
||
|
|
k = 15e3;%[N/m]
|
||
|
|
kv = k;
|
||
|
|
kh = 15e3;
|
||
|
|
I = 115;%[kg m^2]
|
||
|
|
dampv = 0.03;
|
||
|
|
damph = 0.03;
|
||
|
|
s = tf('s');
|
||
|
|
|
||
|
|
%% State-space model
|
||
|
|
M = [m 0 0
|
||
|
|
0 m 0
|
||
|
|
0 0 I];
|
||
|
|
|
||
|
|
la = l;
|
||
|
|
ha = h;
|
||
|
|
kv = k;
|
||
|
|
kh = k;
|
||
|
|
|
||
|
|
%Jacobian of the bottom sensor
|
||
|
|
Js1 = [1 0 h/2
|
||
|
|
0 1 -l/2];
|
||
|
|
|
||
|
|
%Jacobian of the top sensor
|
||
|
|
Js2 = [1 0 -h/2
|
||
|
|
0 1 0];
|
||
|
|
|
||
|
|
%Jacobian of the actuators
|
||
|
|
Ja = [1 0 ha/2 %Left horizontal actuator
|
||
|
|
%1 0 h/2 %Right horizontal actuator
|
||
|
|
0 1 -la/2 %Left vertical actuator
|
||
|
|
0 1 la/2]; %Right vertical actuator
|
||
|
|
Jah = [1 0 ha/2];
|
||
|
|
Jav = [0 1 -la/2 %Left vertical actuator
|
||
|
|
0 1 la/2]; %Right vertical actuator
|
||
|
|
Jta = Ja';
|
||
|
|
Jtah = Jah';
|
||
|
|
Jtav = Jav';
|
||
|
|
K = kv*Jtav*Jav + kh*Jtah*Jah;
|
||
|
|
C = dampv*kv*Jtav*Jav+damph*kh*Jtah*Jah;
|
||
|
|
|
||
|
|
E = [1 0 0
|
||
|
|
0 1 0
|
||
|
|
0 0 1]; %projecting ground motion in the directions of the legs
|
||
|
|
|
||
|
|
AA = [zeros(3) eye(3)
|
||
|
|
-M\K -M\C];
|
||
|
|
|
||
|
|
BB = [zeros(3,3)
|
||
|
|
M\Jta ];
|
||
|
|
|
||
|
|
% CC = [[Js1;Js2] zeros(4,3)];
|
||
|
|
CC = [[Jah;Jav] zeros(3,3)];
|
||
|
|
|
||
|
|
% DD = zeros(4,3);
|
||
|
|
DD = zeros(3);
|
||
|
|
|
||
|
|
G = ss(AA,BB,CC,DD);
|
||
|
|
%% Modal coordinates
|
||
|
|
[V,D] = eig(M\K);
|
||
|
|
Mm = V'*M*V; % Modal mass matrix
|
||
|
|
Dm = V'*C*V; % Modal damping matrix
|
||
|
|
Km = V'*K*V; % Modal stiffness matrix
|
||
|
|
|
||
|
|
Bm = inv(Mm)*V'*Jta;
|
||
|
|
% Cm = [Js1;Js2]*V;
|
||
|
|
Cm = [Jah;Jav]*V;
|
||
|
|
|
||
|
|
|
||
|
|
omega = real(sqrt(inv(Mm)*Km));
|
||
|
|
zeta = real(0.5*inv(Mm)*Dm*inv(omega));
|
||
|
|
|
||
|
|
Gm = [1/(s^2+2*zeta(1,1)*omega(1,1)*s+omega(1,1)^2),0,0;
|
||
|
|
0,1/(s^2+2*zeta(2,2)*omega(2,2)*s+omega(2,2)^2),0;
|
||
|
|
0,0,1/(s^2+2*zeta(3,3)*omega(3,3)*s+omega(3,3)^2)];
|
||
|
|
figure(1)
|
||
|
|
bode(G,Cm*Gm*Bm)
|
||
|
|
figure(2)
|
||
|
|
bode(G,Gm)
|
||
|
|
|
||
|
|
%% Controller
|
||
|
|
s = tf('s');
|
||
|
|
w0 = 2*pi*0.1;
|
||
|
|
Kc = 100/(1+s/w0);
|
||
|
|
Knet = inv(Bm)*Kc*inv(Cm);
|
||
|
|
Gc = -lft(G,Knet);
|
||
|
|
isstable(Gc)
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
|