[비선형제어]Feedback Linearization -3-

이번에도 FL의 예제와 함께하는 설명입니다.

 

예제

$\dot x_1 = sin(x_2)+(x_2+1)x_3$

$\dot x_2 = x_1^5 + x_3$

$\dot x_3 = x_1^2 + u$

$y = x_1$

 

이번에는 모든 상태를 피드백 받는것이 아닌 $y$을 피드백 받아 $y_d$를 추종하는 제어입력을 설계해야 합니다.

 

현재 제어입력 $u$는 $x_1$과 직접 연결되어 있지 않으니, $y$를 한번 미분해보겠습니다.

 

$\dot y = \dot x_1$ 이므로

$\dot y = sin(x_2) + (x_2+1)x_3$ 입니다.

 

만약 여기서 제어입력이 나왔다면 이전과 같이 우변을 모두 보조제어입력 $v$로 보고, u를 설계하면 됩니다.

하지만 제어입력이 없기 때문에 한번 더 미분해주겠습니다.

 

$\ddot y = cos(x_2)\dot x_2 + \dot x_2 x_3 + (x_2 + 1)\dot x_3$

$\ddot y = cos(x_2)(x_1^5+x_3)+(x_1^5+x_3)(x_3)+(x_2+1)(x_1^2+u)$

$\ddot y = (cos(x_2)+x_3)(x_1^5+x_3)+(x_2+1)x_1^2 + (x_2+1)u$

 

이제 제어입력을 제외한 항을 $f(x)=(cos(x_2)+x_3)(x_1^5+x_3) + (x_2+1)x_1^2$로 표현하면

$\ddot y = f(x)+(x_2+1)u$가 됩니다.

 

제어입력이 나왔으니, 이제 감이 오시죠?

$\ddot y = v$로 잡고

$v = f(x)+(x_2+1)u$ 니까

$u = (v-f(x))/(x_2+1)$입니다.

 

만약 $y_d$가 trajectory라면, $\dot y_d$, $\ddot y_d$가 존재할 것이고,

error dynamics 가 $\ddot e + k_2 \dot e + k_1 e = 0$인 보조 제어입력 $v$는

 

$v = \ddot y_d - k_1 e - k_2 \dot e$ 입니다.

 

결과

 

코드

clear all;close all;clc;

Tf = 10;
dt = 0.1;

idx = 1;
time = 0:dt:Tf;

states = zeros(3,Tf/dt);
states(1,1) = 0;
states(2,1) = 0;


k1 = 1;
k2 = 1;

y_d = sin(time);
y_d_dot = cos(time);
y_d_ddot = -sin(time);

for t = 0:dt:Tf-dt

    y = states(1,idx);
    y_dot(idx) = sin(states(2,idx)) + (states(2,idx)+1)*states(3,idx);

    
    e_dot = y_dot(idx) - y_d_dot(idx);
    e = y-y_d(idx);

    v = y_d_ddot(idx) -k1*e - k2*e_dot;
    f_x = (states(1,idx)^5+states(3,idx))*(states(3,idx)+ cos(states(2,idx))) + (states(2,idx)+1)*states(1,idx)^2;
    u = 1/(states(2,idx)+1)*(v-f_x);
    x1_dot = sin(states(2,idx)) + (states(2,idx)+1)*states(3,idx);
    x2_dot = states(1,idx)^5 + states(3,idx);
    x3_dot = states(1,idx)^2 + u;
    
    x_dot = [x1_dot;x2_dot;x3_dot];


    states(:,idx+1) = states(:,idx) + x_dot*dt;
    idx = idx + 1;
end

figure()
subplot(2,1,1)
plot(time(1:end),states(1,:),'b-');
hold on;
plot(time(1:end),y_d(:),'r--','LineWidth',2)
legend('y','y_d')
grid on

subplot(2,1,2)
plot(time(1:end-1),y_dot(:),'b-');
hold on;
plot(time(1:end),y_d_dot(:),'r--','LineWidth',2)
legend('y_dot','y_d_dot')
grid on