차량 로봇의 횡방향 제어

clc; clear; close; clf;
%%
% 7.5 차량 로봇의 횡방향 제어
%%
syms L1 L2 Cf Cr Vx m lz
a1 = (2*L1*Cf-2*L2*Cr)/Vx;
a2 = (2*Cf+2*Cr)/Vx;
a3 = (2*L1^2*Cf+2*L2^2*Cr)/Vx;
a11= -a2/m;
a12 = -(Vx+a1/m);
a21 = -a1/lz;
a22 = -a3/lz;
A = [a11 a12; a21 a22]
%%
b1 = 2*Cf/m;
b2 = 2*L1*Cf/lz;
B = [b1; b2]
%%
L1 = 4.76; L2=6.35; Cf=9000; Cr=9000; lz=5860; m=5760; Vx=10;
subs(A)
subs(B)
%% Matlab을 이용해 제어
a = [-0.625 -9.5030; 0.4884 -19.3453];
b = [3.125; 14.6212];
c = [1 0; 0 1];
d = [0; 0];
[num,den]=ss2tf(a,b,c,d)
    % ss2tf: 상태공간 표현식 -> 전달함수
    %   - 연속시간 시스템: 라플라스변환
    %   - 이산시간 시스템: z변환 전달함수
    
    % num : Numerator
    % den : Denominator 

%% 제어 가능성, 가제어성(Controllability)
Controllability=rank(ctrb(a,b)) %full rank이므로, 제어 가능하다.

%% 관측 가능성, 가관측성(Observaility)
Observaility=rank(obsv(a,c)) %full rank이므로, 관측 가능하다.
%% 차량 로봇의 LQR & 신경망 제어
%Q=I; 
Q=eye(2);
R=1;
[k,s,e]=lqr(a,b,Q,R)
Ac=a-b*k;
%%
sys = ss(a,b,c,d) % ss: Create state-space model, convert to state-space model
sys1 =tf([num(1,1) num(1,2) num(1,3)],[den(1) den(2) den(3)]); %추가
sys2 =tf([num(2,1) num(2,2) num(2,3)],[den(1) den(2) den(3)]); %추가

%% 전달함수 G_n(s)에 대해

G1=num(1,:)/den(1,:)
G2=num(2,:)/den(1,:)
%% Plot
figure(1)
impulse(Ac,b,c,d)

% figure(2)
% rlocus(sys1)
% 
% figure(3)
% rlocus(sys2)
% 
% figure(4)
% bode(sys1,{0.1,1e9})
% 
% figure(5)
% bode(sys2,{0.1,1e9})
% 
% figure(6)
% nyquist(sys1)
% 
% figure(7)
% nyquist(sys2)

figure(8)
step(sys) % Step response plot of dynamic system; step response data

---

A =
 
[       -(2*Cf + 2*Cr)/(Vx*m), - Vx - (2*Cf*L1 - 2*Cr*L2)/(Vx*m)]
[-(2*Cf*L1 - 2*Cr*L2)/(Vx*lz),  -(2*Cf*L1^2 + 2*Cr*L2^2)/(Vx*lz)]
 
 
B =
 
    (2*Cf)/m
(2*Cf*L1)/lz
 
 
ans =
 
[     -5/8,       -3041/320]
[1431/2930, -5668209/293000]
 
 
ans =
 
    25/8
4284/293
 

num =

         0    3.1250  -78.4912
         0   14.6212   10.6645


den =

    1.0000   19.9703   16.7321


Controllability =

     2


Observaility =

     2


k =

   -0.7281    0.6638


s =

    0.2888   -0.1115
   -0.1115    0.0692


e =

   -3.3688
  -24.0322


sys =
 
  A = 
           x1      x2
   x1  -0.625  -9.503
   x2  0.4884  -19.35
 
  B = 
          u1
   x1  3.125
   x2  14.62
 
  C = 
       x1  x2
   y1   1   0
   y2   0   1
 
  D = 
       u1
   y1   0
   y2   0
 
연속시간 상태공간 모델입니다.


G1 =

   -1.8402


G2 =

    0.6920

 

Fig1.