181 lines
6.4 KiB
Matlab
181 lines
6.4 KiB
Matlab
|
## Omnidirectional Modeling API of Octave by Adnr
|
|||
|
|
function obj = omnidirectional(
|
|||
|
|
%% viscous friction coefficient related to v
|
|||
|
|
%B v (N/m/s)
|
|||
|
|
cBv%0.94
|
|||
|
|
%% viscous friction coefficient related to v n
|
|||
|
|
%B vn (N/m/s)
|
|||
|
|
, cBvn%0.96
|
|||
|
|
%% viscous friction coefficient related to ω
|
|||
|
|
%B ω (N/rad/s)
|
|||
|
|
, cBw%0.01
|
|||
|
|
%% coulomb friction coefficient related to v
|
|||
|
|
%C v (N )
|
|||
|
|
, cCv%2.2
|
|||
|
|
%% coulomb friction coefficient related to v n
|
|||
|
|
%C vn (N )
|
|||
|
|
, cCvn%1.5
|
|||
|
|
%% coulomb friction coefficient related to ω
|
|||
|
|
%C ω (N.m)
|
|||
|
|
, cCw%0.099
|
|||
|
|
%% radius of the robot
|
|||
|
|
%b(m)
|
|||
|
|
, cb%0.1
|
|||
|
|
%% mass of the robot
|
|||
|
|
%M (kg)
|
|||
|
|
, cM%1.5
|
|||
|
|
%% inertia moment of the robot
|
|||
|
|
%I n (kg.m 2 )
|
|||
|
|
, cIn%0.025
|
|||
|
|
%% angle
|
|||
|
|
%δ
|
|||
|
|
, cdlt%30
|
|||
|
|
%% radius of the wheels
|
|||
|
|
%r 1 , r 2 , r 3 (m)
|
|||
|
|
, cri%0.035
|
|||
|
|
%% reduction of the motors
|
|||
|
|
%l 1 , l 2 , l 3
|
|||
|
|
, cli%19
|
|||
|
|
%% motor’s armature inductance
|
|||
|
|
%L a 1...3 (H)
|
|||
|
|
, cLi%0.00011
|
|||
|
|
%% motor’s armature resistance
|
|||
|
|
%R a 1...3 (Ω)
|
|||
|
|
, cRai%1.69
|
|||
|
|
%% motor’s emf constant
|
|||
|
|
%K v 1...3 (V olts/rad/s)
|
|||
|
|
, cKv%0.0059
|
|||
|
|
%% motor’s torque constant
|
|||
|
|
%K t 1...3 (N.m/A)
|
|||
|
|
, cKt%0.0059
|
|||
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|||
|
|
%% Criterion of canonical second-orde system
|
|||
|
|
, zeta
|
|||
|
|
, omega
|
|||
|
|
%% Input constrain
|
|||
|
|
, InCon
|
|||
|
|
, path_file
|
|||
|
|
)
|
|||
|
|
|
|||
|
|
pkg load control
|
|||
|
|
obj = struct();
|
|||
|
|
|
|||
|
|
%%%%%%% Origin builder
|
|||
|
|
|
|||
|
|
obj.origin = struct();
|
|||
|
|
obj.origin.A = [-((3*(cli^2)*(cKt^2))/(2*cM*cRai*(cri^2)))-(cBv/cM),0,0;
|
|||
|
|
0, -((3*(cli^2)*(cKt^2))/(2*cM*cRai*(cri^2)))-(cBvn/cM),0;
|
|||
|
|
0,0,-((3*(cb^2)*(cli^2)*(cKt^2))/(2*cIn*cRai*(cri^2)))-(cBw/cIn)];
|
|||
|
|
obj.origin.B = [0, (cos(cdlt*pi/180)/cM), -(cos(cdlt*pi/180)/cM);
|
|||
|
|
-1/cM, sin(cdlt*pi/180)/cM, sin(cdlt*pi/180)/cM;
|
|||
|
|
cb/cIn, cb/cIn, cb/cIn];
|
|||
|
|
obj.origin.K = [-cCv/cM, 0,0;
|
|||
|
|
0, -cCvn/cM, 0;
|
|||
|
|
0,0,-cCw/cM];
|
|||
|
|
obj.LENGTH_STATE = size(obj.origin.A,1);
|
|||
|
|
obj.LENGTH_INTPUT = size(obj.origin.B,1);
|
|||
|
|
Q = [500 0 0; 0 500 0; 0 0 5;]
|
|||
|
|
R = eye(3)*0.3;
|
|||
|
|
kst_ = lqr(obj.origin.A,obj.origin.B,Q,R);
|
|||
|
|
nst_ = -pinv(eye(size(obj.origin.A))*inv(obj.origin.A-obj.origin.B*kst_)*obj.origin.B);
|
|||
|
|
syscl = ss(obj.origin.A-obj.origin.B*kst_, obj.origin.B*nst_, eye(3),0);
|
|||
|
|
% step(syscl);
|
|||
|
|
obj.origin.Kst = kst_;
|
|||
|
|
obj.origin.Nst = nst_;
|
|||
|
|
obj.origin.syscl = syscl
|
|||
|
|
%%%%%%%
|
|||
|
|
|
|||
|
|
A = obj.origin.A;
|
|||
|
|
B = obj.origin.B;
|
|||
|
|
K = obj.origin.K;
|
|||
|
|
obj.LENGTH_STATE = size(A,1)+3;
|
|||
|
|
obj.LENGTH_INPUT = size(B,1);
|
|||
|
|
obj.A = [zeros(size(A)),eye(size(A));zeros(size(A)),A;];
|
|||
|
|
obj.B = [zeros(size(B));B;];
|
|||
|
|
obj.K = [zeros(size(B,2),size(B,2));K];
|
|||
|
|
obj.getState = @(o) zeros(size(A,1)+3,1);
|
|||
|
|
obj.getInput = @(o) zeros(size(B,2),1);
|
|||
|
|
obj.InCon = ones(obj.LENGTH_INPUT)*InCon;
|
|||
|
|
|
|||
|
|
|
|||
|
|
%%%%%%%%% Make Controller
|
|||
|
|
obj.controllabilityTest = @(a,b) rank(ctrb(a,b));
|
|||
|
|
obj.observabilityTest = @(a,c) rank(obsv(a,c));
|
|||
|
|
s = tf('s');
|
|||
|
|
obj.Gref = omega^2/(s^2+(2*zeta*omega)*s+omega^2);
|
|||
|
|
p = pole(obj.Gref);
|
|||
|
|
Kst = zeros(3,6);
|
|||
|
|
Nst = zeros(3,6);
|
|||
|
|
Q = {
|
|||
|
|
[ 100 0; 0 10;], %x
|
|||
|
|
[ 100 0; 0 10;], %y
|
|||
|
|
[2000 0; 0 10;] %theta
|
|||
|
|
};
|
|||
|
|
R = eye(3)*1;
|
|||
|
|
for i = [1:3]
|
|||
|
|
A_ = [0 1;
|
|||
|
|
0 A(i,i)];
|
|||
|
|
B_ = [0 0 0;
|
|||
|
|
B(i,1) B(i,2) B(i,3)];
|
|||
|
|
kst_ = lqr(A_,B_,Q{i},R);
|
|||
|
|
nst_ = -pinv(eye(size(A_))*inv(A_-B_*kst_)*B_);
|
|||
|
|
% syscl = ss(A_-B_*kst_, B_*nst_, eye(2),0); % uncomment for debugging
|
|||
|
|
% step(syscl);
|
|||
|
|
j = ((i-1)*2);
|
|||
|
|
Kst(:,1+j:2+j) = kst_;
|
|||
|
|
Nst(:,1+j:2+j) = nst_;
|
|||
|
|
endfor
|
|||
|
|
obj.Kst = [Kst(:,1) Kst(:,3) Kst(:,5) Kst(:,2) Kst(:,4) Kst(:,6)];
|
|||
|
|
obj.Nst = [Nst(:,1) Nst(:,3) Nst(:,5) Nst(:,2) Nst(:,4) Nst(:,6)];
|
|||
|
|
obj.syscl = ss(obj.A-obj.B*obj.Kst,obj.B*obj.Nst,eye(size(obj.A)),[0]);
|
|||
|
|
|
|||
|
|
%%%%%%%%% Generate discrete state space function
|
|||
|
|
% sgn function, in octave comparison (x compare value) return integer
|
|||
|
|
% result of comparison used to address to value in matrix
|
|||
|
|
obj.sgn = @(x) [0,1,-1](4-(1*(x<0))-(2*(x>0))-(3*(x==0)));
|
|||
|
|
obj.origin.sgn = obj.sgn;
|
|||
|
|
obj.limit_motor = @(x) [x,6,-6](4-(1*(x<-6))-(2*(x>6))-(3*(x<6 & x>-6)));
|
|||
|
|
obj.origin.limit_motor = obj.limit_motor;
|
|||
|
|
|
|||
|
|
% Kinematic function for coordinate robot to global
|
|||
|
|
obj.rotationOrtogonal = @(the) [cos(the) sin(the) 0; -sin(the) cos(the) 0; 0 0 1];
|
|||
|
|
|
|||
|
|
% https://en.wikibooks.org/wiki/Control_Systems/State-Space_Equations#Discretization
|
|||
|
|
% x[k+1] = (1 + AT)x[k] + TBu[k] + TKsgn(x[k])
|
|||
|
|
% Not using function from control package becouse the system have K term
|
|||
|
|
obj.Aterm = @(o,T,st) (eye(o.LENGTH_STATE,o.LENGTH_STATE)+(o.A*T))*st;
|
|||
|
|
obj.Bterm = @(o,T,st,in) (T*o.B)*in;
|
|||
|
|
obj.Kterm = @(o,T,st,in) ((T*o.K)*[o.sgn(st(4));o.sgn(st(5));o.sgn(st(6))]);
|
|||
|
|
% Original state space robot
|
|||
|
|
obj.disDotState = @(o,T,st,in) o.Aterm(o,T,st)+o.Bterm(o,T,st,in)+o.Kterm(o,T,st,in);
|
|||
|
|
|
|||
|
|
obj.Uterm = @(o,st,ref) (o.Nst*ref - o.Kst*st);
|
|||
|
|
obj.Cons = @(u) [u,InCon,-InCon](1+(2*(u<-InCon)+(1*(u>InCon))));
|
|||
|
|
obj.UCons = @(o,u) [o.Cons(u(1)); o.Cons(u(2)); o.Cons(u(3))];
|
|||
|
|
obj.UIn = @(o,st,ref) o.UCons(o,o.Uterm(o,st,ref));
|
|||
|
|
% State space included controller
|
|||
|
|
obj.disDotStateCl = @(o,T,st,ref) o.disDotState(o,T,st,o.UCons(o,o.Uterm(o,st,ref)));
|
|||
|
|
obj.Y = @(o,st) [o.rotationOrtogonal(st(3))', zeros(size(A))]*st;
|
|||
|
|
|
|||
|
|
|
|||
|
|
% Time eq
|
|||
|
|
obj.disTime = @(T,ti) (ti + abs(T));
|
|||
|
|
obj.testStabilityExplicit = @(h) abs(eye(size(A))+(h*(A+K)));
|
|||
|
|
obj.testStabilityImplicit = @(h) abs(eye(size(A))-(h*(A+K)));
|
|||
|
|
|
|||
|
|
%%%%%%%%% PRINT the matrix to file used matrix in python app
|
|||
|
|
csvwrite(strcat(path_file,'A.matrix'),obj.A);
|
|||
|
|
csvwrite(strcat(path_file,'B.matrix'),obj.B);
|
|||
|
|
csvwrite(strcat(path_file,'C.matrix'),[eye(size(A)), zeros(size(A))]);
|
|||
|
|
csvwrite(strcat(path_file,'K.matrix'),obj.K);
|
|||
|
|
csvwrite(strcat(path_file,'Kst.matrix'),obj.Kst);
|
|||
|
|
csvwrite(strcat(path_file,'Nst.matrix'),obj.Nst);
|
|||
|
|
|
|||
|
|
[y,t] = step(obj.Gref);
|
|||
|
|
csvwrite(strcat(path_file,'Gref.csv'),[y,t]);
|
|||
|
|
csvwrite(strcat(path_file,'poleGref.csv'),pole(obj.Gref));
|
|||
|
|
|
|||
|
|
|
|||
|
|
endfunction
|
|||
|
|
|