[Pegasus] PI 제어기 Zero 위치 선정의 트레이드오프

시스템 요구 사항 정의

내가 목표로 삼을 제어 스펙이다. 실제 플랜트의 움직임을 먼저 분석한 후, 세틀링 타임이 어느 정도 짧아지면서 반응성이 높아지면 좋을지, 정상 상태 오차가 얼마나 있으면 좋을지 등을 결정한다.
$T_s$ : 1s
%$OS$ : 10%
$e_{\infty}$ : 0

아래는 계산을 위한 실제 파라미터들이다. 인터넷에 나와 있는 자료를 활용 했으며, 일부 값들은 이상적인 조건에서 역연산 되어 계산되었다.

파라미터	값	단위	파라미터	값	단위	파라미터	값	단위
$m_b$	102	$kg$	$r$	0.08	$m$	$b$	0.01	$N$$\cdot m \cdot s$
$m_w$	4	$kg$	$R$	4.18	$\Omega$	$N$	25	.
$I_b$	7.8412	$kg \cdot m^2$	$L$	0.012	$H$	$K_t$	0.0322	$N \cdot m/A$
$I_{wb}$	0.3664	$kg \cdot m^2$	$\omega_s$	8.0634	$rad/s$	$K_e$	0.0322	$V/(rad/s)$
$I_w$	0.0128	$kg \cdot m^2$	$T_s$	6.4489	$N \cdot m$	$V_{in}$	48	$V$
$d$	0.375	$m$	$\omega_m$	201.585	$rad/s$	$I_s$	8	$A$
$l$	0.3	$m$	$T_m$	0.258	$N \cdot m$	$P_s$	52	$W$

제어기 설계

전달함수와 시스템 요구 사항이 정해졌으니, 제어기를 설계할 수 있다.

병진 운동 전달함수부터 제어기 설계를 시작해보자.

% 차체 요잉 회전시 파라미터
mb = 102;
mw = 4;
r = 0.08;
d = 0.375;
l = 0.3;

Ib = 1/12 * mb * (4*d^2 + 4*l^2);
Iwb = mw*l^2 + 1/4*mw*r^2;
Iw = 1/2 * mw * r^2;
Ieq = Ib + 2 * Iwb;

M = mb + 2 * mw;
b = 0.01;

% DC 모터 파라미터
N = 25;
V_in = 48;
P_s = 52;
I_s = 8;
w_s = 77 * 2 * pi / 60;
T_s = P_s / w_s;

T_m = T_s / N;
w_m = w_s * N;

K_t = T_m / I_s;
K_e = K_t;
R = 4.18;
L = 0.012;

% 병진 운동 전달함수
num = [2 * V_in * K_t * N * r];
den = [(M * r^2 + 2 * Iw) * L, ((M * r^2 + 2 * Iw) * R + 2 * b * L), (2 * b * R + 2 * N^2 * K_e * K_t)];

sys = tf(num, den);

disp('================= 병진 운동 전달함수 ================');
sys
disp('===================================================');

위와 같이 파라미터들을 정리하고 병진 운동 전달 함수를 구해보면,
이제 이 전달함수의 루트 로커스를 그려서 안정성과 근의 움직임을 판별해보자.

루트로커스를 보면
- pole이 -347.9, -0.45인 것을 알 수 있다.
- 오른쪽 응답은 H=1 인 경우 피드백 시스템일 때의 스텝 응답이다. 루트로커스에도 알 수 있지만, 폐루프 응답을 구해보면 극점이 약 -350과 -0.5 정도 나온다.
- $e^{at}$ 형태에서 a에 -350은 바로 사라지는 것으로 볼 수 있기 때문에 시스템은 원점과 가까운 -0.5에의해 좌우된다.
- 따라서 응답이 앞서 예산했듯이 1차 시스템의 응답 형태로 근사 되는 것을 볼 수 있다.
폐루프 step input일때 정상상태 오차가 약 0.2 정도 있는 것을 확인 할 수 있다.
1차 시스템은 구조적으로 오버슈트나 진동이 없기 때문에 이를 억제할 필요가 없다.
따라서 우리는 PI 제어기만 설계를 하면 될 것 같다.
PI 제어기를 설계 할 때, $\frac{1}{s}$ 에의해 Angle Condition이 만족되지 않아 응답이 달라질 수 있는데
- $\theta_{net} = \theta_{zero} - \theta_{pole} \approx 0$ 를 만족하기 위해서 원점 근처에 제로를 추가 해주면 응답을 크게 변화시키지 않으면서 정상상태 오차만 제거된 안정적인 시스템을 만들 수 있다.
우리의 목표는 %OS가 10%인 $\zeta$ = 0.5912일 때이다.
- 위 그래프는 임의로 제로가 -1, -2.5, -5일때의 루트로커스이고, 제타가 0.5912일 때 게인 값들을 구하면
- 제타 0.5912 근처의 값을 임의로 찍고, 그때의 게인은 0.327, 1.35, 2.65이다.
  1. 제로를 원점에 가깝게 두면, Ts 1초 이내 조건을 달성하기 힘들어진다.
    - 제로가 원점에 가까우면 시스템 응답에 아주 느린 성분($e^{-0.1t}$ 같은)이 남게 되어 지배적이게 되어 느린 응답을 만든다.
      - 제로는 분자항이라서 역라플라스시 직접적으로 저런 지수함수 형태를 만들 수 없지만, 루트로커스에서 제로가 있다면, 게인이 점점 커지는 상황에서는 폴 중 누군가는 제로 근처에 찍힐 수 밖에 없기 때문에 폴이 제로 근처에 찍히는데 원점 근처라면, 예시의 지수함수처럼 느린 지배적 응답이 될 수도 있기 때문이다.
    - 극점 바로 위에 제로를 두어 상쇄 시키는 방법도 있지만, 루트 로커스를 원하는 대로 휘게 할 수 없으므로 LHP의 더 안정적이고 빠른영역인 왼쪽으로 루트 로커스를 당겨올 수 없게 된다.
    - 따라서 극점 -0.5 보다 더 작은 영역에서 오버슈트와 응답 속도를 줄타기 할 수 있는 -1, -2.5, -5를 선정해본 것이다.
이제 이 게인들로 응답 그래프를 그려보면
- 어림 잡기 위해서 루트로커스 그래프 툴팁의 파란 글자 오버슈트 값을 보면서 Gain을 선정했지만, 실제 터미널 결과가 달라서 왜 이런일이 일어났는지 알아 보았다.
  - 툴팁의 값은 정해진 공식에 값을 대입하는 구조이기 때문에 2차 근사일뿐 실제는 stepinfo를 통해서 구해야 한다는 정보를 매트랩 공식 포럼에서 찾을 수 있었다.
- %OS가 적당하면서 $T_s$ 가 1초를 만족해야 하므로 응답이 빠른 제로가 -2.5 일때를 기준으로 설계해보자.
이제 제로를 -2.5로 고정한 후, 전체이득 $K_{pi}$ 게인 값을 조절하면서 목표에 맞춰보자.v
- 게인 값을 2부터 10까지 순차적으로 할당해서 응답을 그려봤다.
- $K_{pi}$ 가 6일때 10% 오버슈트 이내, Ts 1초 이내를 달성했고, $e_{\infty}$ 도 원점에 pole이 추가되면서 0인 것을 확인할 수 있다.
최종적으로 $K_{pi}$ = 6을 선정했고 제로는 -2.5를 선정하였다.
따라서 $G_c(s) = K_p + \frac{K_i}{s} = \frac{K_p(s+\frac{K_i}{K_P})}{s} = \frac{6(s+2.5)}{s}$ 이므로 $K_p = 6, \quad K_i = 15$ 를 선정한다.
같은 방법으로 회전 운동 전달함수도 제어기를 설계해보자.

루트로커스를 보면
- pole이 -347.9, -0.45인 것을 알 수 있다.
- $e_{\infty}$ 도 약 0.07 정도 존재한다.
- 임의의 제로 -1, -2, -3 일때
- $K_{pi}$ 게인이 0.09, 0.231, 0.479이므로 이 게인들로 실제 stepinfo를 얻어보면
- 제로가 -3 일때를 기준으로 $K_{pi}$ 게인을 선정해보자.
  - $K_{pi}$ 값이 2부터 목표에 도달한다.
최종적으로 $K_{pi}$ = 2 을 선정했고 제로는 -3를 선정하였다.
따라서 $G_c(s) = K_p + \frac{K_i}{s} = \frac{K_p(s+\frac{K_i}{K_P})}{s} = \frac{2(s+3)}{s}$ 이므로 $K_p = 2, \quad K_i = 6$ 를 선정한다.

코드 전문

```matlab
% 차체 요잉 회전시 파라미터
mb = 102;
mw = 4;
r = 0.08;
d = 0.375;
l = 0.3;

Ib = 1/12 * mb * (4*d^2 + 4*l^2);
Iwb = mw*l^2 + 1/4*mw*r^2;
Iw = 1/2 * mw * r^2;
Ieq = Ib + 2 * Iwb;

M = mb + 2 * mw;
b = 0.01;

% DC 모터 파라미터
N = 25;
V_in = 48;
P_s = 52;
I_s = 8;
w_s = 77 * 2 * pi / 60;
T_s = P_s / w_s;

T_m = T_s / N;
w_m = w_s * N;

K_t = T_m / I_s;
K_e = K_t;
R = 4.18;
L = 0.012;

% 병진 운동 전달함수
num = [2 * V_in * K_t * N * r];
den = [(M * r^2 + 2 * Iw) * L, ((M * r^2 + 2 * Iw) * R + 2 * b * L), (2 * b * R + 2 * N^2 * K_e * K_t)];

sys = tf(num, den);

disp('================= 병진 운동 전달함수 ================');
sys
disp('===================================================');

% 병진 운동 전달함수 루트 로커스와 폐루프 스텝 응답

sys2 = feedback(sys, 1);

poles = pole(sys);
disp(['극점 개수: ', num2str(length(poles))]);
disp('극점 좌표:');
disp(poles);

figure(1);
rlocus(sys);
title('병진 운동 전달함수 오픈루프 루트 로커스');

figure(2);
step(sys2,100);
title('병진 운동 전달함수 폐루프 스텝 응답');
grid on;

% PI 제어기 설계
% 목표 제타 구하는 식
POS = 0.1; % 10% 오버슈트
zeta = -log(POS)/sqrt(pi^2 + (log(POS))^2)

% 제어기 영점 후보
z_PI = [1 2.5 5];

figure(3);
for i = 1:length(z_PI)
    G_PI = tf([1 z_PI(i)], [1 0]);
    sys_PI = sys * G_PI;
    rlocus(sys_PI);
    sgrid(0.5912, 0.1:1:15);
    axis([-15 3 -8 8]);
    hold on;
end

% K_pi 값에 따른 응답 특성들
K_PI = [0.327 1.35 2.65];

for i = 1:length(z_PI)
    Gc_PI = tf([1 z_PI(i)], [1 0]);
    sys_PI = sys * Gc_PI;
    T_PI(i) = feedback(K_PI(i) * sys_PI, 1);
    data_PI = stepinfo(T_PI(i));
    OS_PI(i) = data_PI.Overshoot;
    Ts_PI(i) = data_PI.SettlingTime;
    Tr_PI(i) = data_PI.RiseTime;
    hold on;
end

figure(4);
for i = 1:length(z_PI)
    step(T_PI(i), 5);
    hold on;
end
hold off;
title('PI 제어기 폐루프 스텝 응답');
legend('제로 값 1', '제로 값 2.5', '제로 값 5');
grid on;

disp('Step information of Closed loop TF');
fprintf('%-10s %-10s %-10s %-10s\n', 'zero', 'pOS', 'Ts', 'Tr');
fprintf('---------------------------------------------------\n');
for i = 1:length(z_PI)
    fprintf('%-10.2f %-10.4f %-10.4f %-10.4f\n', ...
        z_PI(i), OS_PI(i), Ts_PI(i), Tr_PI(i));
end
disp('===================================================');

% 제로 값 선정 후 K_PI 값 구하기
z_PI = 2.5;
K_PI = [2 3 4 5 6 7 8 9 10];
for i = 1:length(K_PI)
    Gc_PI = tf([1 z_PI], [1 0]);
    sys_PI = sys * Gc_PI;
    T_PI(i) = feedback(K_PI(i) * sys_PI, 1);
    data_PI = stepinfo(T_PI(i));
    OS_PI(i) = data_PI.Overshoot;
    Ts_PI(i) = data_PI.SettlingTime;
    Tr_PI(i) = data_PI.RiseTime;
end

figure(5);
for i = 1:length(K_PI)
    step(T_PI(i), 5);
    hold on;
end
hold off;
title('PI 제어기 폐루프 스텝 응답');
legend('K_PI = 2', 'K_PI = 3', 'K_PI = 4', 'K_PI = 5', 'K_PI = 6', 'K_PI = 7', 'K_PI = 8', 'K_PI = 9', 'K_PI = 10');
grid on;

disp('Step information of Closed loop TF');
fprintf('%-10s %-10s %-10s %-10s\n', 'K_PI', 'pOS', 'Ts', 'Tr');
fprintf('---------------------------------------------------\n');
for i = 1:length(K_PI)
    fprintf('%-10.2f %-10.4f %-10.4f %-10.4f\n', ...
        K_PI(i), OS_PI(i), Ts_PI(i), Tr_PI(i));
end
disp('===================================================');

% 회전 운동 전달함수
num = [2 * V_in * r * l * K_t * N];
den = [(Ieq * r^2 + 2 * l^2 * Iw) * L, ((Ieq * r^2 + 2 * l^2 * Iw) * R + 2 * l^2 * b * L), (2 * l^2 * b * R + 2 * N^2 * l^2 * K_e * K_t)];

sys = tf(num, den);

disp('================= 회전 운동 전달함수 ================');
sys
disp('===================================================');

% 회전 운동 전달함수 루트 로커스와 폐루프 스텝 응답

sys2 = feedback(sys, 1);

poles = pole(sys);
disp(['극점 개수: ', num2str(length(poles))]);
disp('극점 좌표:');
disp(poles);

figure(6);
rlocus(sys);
title('회전 운동 전달함수 오픈루프 루트 로커스');

figure(7);
step(sys2, 15);
title('회전 운동 전달함수 폐루프 스텝 응답');
grid on;

% PI 제어기 설계
% 목표 제타 구하는 식
POS = 0.1; % 10% 오버슈트
zeta = -log(POS)/sqrt(pi^2 + (log(POS))^2)

% 제어기 영점 후보
z_PI = [1 2 3];

figure(8);
for i = 1:length(z_PI)
    G_PI = tf([1 z_PI(i)], [1 0]);
    sys_PI = sys * G_PI;
    rlocus(sys_PI);
    sgrid(0.5912, 0.1:1:15);
    axis([-8 3 -4 4]);
    hold on;
end

% K_PI 값에 따른 응답 특성들
K_PI = [0.09 0.231 0.479];

for i = 1:length(z_PI)
    Gc_PI = tf([1 z_PI(i)], [1 0]);
    sys_PI = sys * Gc_PI;
    T_PI(i) = feedback(K_PI(i) * sys_PI, 1);
    data_PI = stepinfo(T_PI(i));
    OS_PI(i) = data_PI.Overshoot;
    Ts_PI(i) = data_PI.SettlingTime;
    Tr_PI(i) = data_PI.RiseTime;
    hold on;
end

figure(9);
for i = 1:length(z_PI)
    step(T_PI(i), 5);
    hold on;
end
hold off;
title('PI 제어기 폐루프 스텝 응답');
legend('제로 값 1', '제로 값 2', '제로 값 3');
grid on;

disp('Step information of Closed loop TF');
fprintf('%-10s %-10s %-10s %-10s\n', 'zero', 'pOS', 'Ts', 'Tr');
fprintf('---------------------------------------------------\n');
for i = 1:length(z_PI)
    fprintf('%-10.2f %-10.4f %-10.4f %-10.4f\n', ...
        z_PI(i), OS_PI(i), Ts_PI(i), Tr_PI(i));
end
disp('===================================================');

% 제로 값 선정 후 K_PI 값 구하기
z_PI = 3;
K_PI = [1 2 3 4 5 6 7 8 9 10];
for i = 1:length(K_PI)
    Gc_PI = tf([1 z_PI], [1 0]);
    sys_PI = sys * Gc_PI;
    T_PI(i) = feedback(K_PI(i) * sys_PI, 1);
    data_PI = stepinfo(T_PI(i));
    OS_PI(i) = data_PI.Overshoot;
    Ts_PI(i) = data_PI.SettlingTime;
    Tr_PI(i) = data_PI.RiseTime;
end

figure(10);
for i = 1:length(K_PI)
    step(T_PI(i), 5);
    hold on;
end
hold off;
title('PI 제어기 폐루프 스텝 응답');
legend('K_PI = 1', 'K_PI = 2', 'K_PI = 3', 'K_PI = 4', 'K_PI = 5', 'K_PI = 6', 'K_PI = 7', 'K_PI = 8', 'K_PI = 9', 'K_PI = 10');
grid on;

disp('Step information of Closed loop TF');
fprintf('%-10s %-10s %-10s %-10s\n', 'K_PI', 'pOS', 'Ts', 'Tr');
fprintf('---------------------------------------------------\n');
for i = 1:length(K_PI)
    fprintf('%-10.2f %-10.4f %-10.4f %-10.4f\n', ...
        K_PI(i), OS_PI(i), Ts_PI(i), Tr_PI(i));
end
disp('===================================================');

```

Simulink 모델 만들기

식(6) 병진 운동 전달함수를 한 번에 피드백 구조로 만든 다이어그램(상단)과 직접 항 하나하나 다이어그램(하단)으로 모델을 만들었다.
- 하단의 첫 피드백은 수식에 있는 뺄셈 연산을 위한 것이 아니라 피드백을 위해 출력 값을 받아와서 입력값과 빼는 것이고
- del_t 부분의 빼기 연산은 오직 수식의 뺄셈 연산을 구현하기 위한 것이다.
가장 오른쪽 끝의 scope를 통해 두 모델의 응답을 확인할 수 있다.
- 구분을 위해 하단 다이어그램의 값을 0.1초 지연 후 그려지게 했다.
- 두 응답의 결과가 같다는 것으로 부터 모델링이 잘 되었다는 것을 알 수 있다.
subsystem 기능을 이용하여 plant와 pid 제어기를 각각 분리해서 모델링하였다.
- 시스템 전달함수 구조
  - 이 모델에서 ($\delta_t, \delta_r)$는 모터 입력 전압을 $V_{in}$에 대해 정규화한 조작량(무차원)이며, $V_R = V_{in}(\delta_t+\delta_r)$, $V_L = V_{in}(\delta_t-\delta_r)$로 좌/우 바퀴 전압 명령을 구성한다. 따라서 $K_p$는 속도 오차(m/s 또는 rad/s)를 정규화 입력(무차원)으로 변환하는 단위를 갖는다.
- pid 제어기 구조
- 전체 다이어그램
  - 레퍼런스 입력 : $v, w$
  - 플랜트 제어 입력 : $\delta_t, \delta_r$
  - 최종 병진 운동 응답을 보니 매트랩으로 그린 제어기 설계 후 응답과 동일하게 나왔고, 알맞게 모델링 됐음을 알 수 있다.

파라미터 변화에 따른 응답 변화

$K_P$ 값을 1, 2, 3으로 설정 후 $K_I$ 는 동일하게 6인 경우
- P 게인이 커질수록 반응성도 빠르고 오버슈트도 줄어든다.
$K_P$ 값을 2로 설정 $K_I$ 는 동일하게 6, $K_D$ 가 0, 1, 2인 경우
- D 제어기를 사용함과 동시에 불안정해진다.
- 분자에 추가된 s가 step input이 들어오는 시점에 기울기를 거의 $\infty$ 로 보기 때문에 아주 짧은 시간 동안 전압 명령이 매우 커져서 오히려 불안정해지는 것이다.
$K_P$ 값을 2로 설정 $K_I$ 는 6, 7, 8, $K_D$ 가 0 인 경우
- 오버슈트가 증가하지만 $T_s$ 는 조금씩 감소한다.

차동 구동 로봇의 병진과 회전의 Decoupling 분석

Simulink 모델링 중, 회전 입력 $\delta_r$을 변화시켜 각속도가 발생하고 있음에도 불구하고, 병진 속도의 그래프는 전혀 영향을 받지 않고 일정하게 유지되는 것을 발견했다. 분명 내가 설계한 플랜트 다이어그램 내부적으로는 왼쪽/오른쪽 모터가 $v$ 와 $\omega$ 의 영향을 동시에 받고 있는데 최종 출력단에서는 $\delta_r$ 을 바꿔도 출력 속도 응답 그래프에 영향이 없이 서로 독립적인 것처럼 보이는 이유를 분석해봤다.

수식적 원인 분석

1. 입력 전압단에서의 상쇄
- 로봇을 앞으로 밀어주는 직진 힘은 양쪽 모터 전압의 합에 비례한다.
- 이때 회전 제어 입력 $\delta_r$ 은 우측엔 더해지고 좌측엔 빼지므로, 합을 구하는 순간 소거된다.
  
  $V_{sum} = V_R + V_L \propto (\delta_t + \mathbf{\delta_r}) + (\delta_t - \mathbf{\delta_r}) = 2\delta_t$
- 즉, 직진 운동 방정식의 입력항에는 오직 직진 명령 $\delta_t$ 만 살아남게 된다.
2. 피드백 루프에서의 상쇄
- 역기전력(Back-EMF) 피드백 역시 직진 운동에 방해가 되는 요소이므로, 좌우 역기전력의 합이 전체 시스템의 감속 요인이 된다.
- 각 바퀴의 속도는 $v$ 와 $\omega$ 의 선형 결합으로 표현되는데, 이를 합치면 회전 성분이 상쇄된다.
  
  $\begin{aligned} V_{emf, total} &\propto \Omega_R + \Omega_L \ &\propto (v + l\omega) + (v - l\omega) \ &= 2v \end{aligned}$
- 결과적으로 직진 운동을 방해하는 역기전력 항에서도 $\omega$ 성분은 사라지고 오직 $v$성분만 남는다.
공학적으로
- 시스템의 직교성 (Orthogonality)
  - 차동 구동 시스템이 완벽하게 대칭이라고 가정할 때, 병진 운동과 회전 운동은 수학적으로 직교한다.
  - 이는 x축으로 이동한다고 해서 y축 좌표가 변하지 않는 것과 같은 원리이다.
- 내부와 외부의 관점 차이
  - Local 관점 (각 모터): 개별 모터는 $\delta_r$ 과 $\omega$ 의 영향을 받아 전압과 전류가 요동친다. (오른쪽은 힘들어지고, 왼쪽은 편해짐)
  - Global 관점 (로봇 몸체): 로봇 몸체의 직진 가속도는 두 모터 힘의 총합으로 결정된다. 내부적으로 에너지가 한쪽으로 쏠릴 뿐, 전체 에너지의 총합(직진 성분)은 변하지 않기 때문에 외부에서 볼 때는 영향이 없는 것처럼 보인다.
- 줄다리기로 비유하자면
  - 수레를 끌 때 오른쪽 사람은 더 세게 당기고, 왼쪽 사람은 살살 당겨도, 두 힘의 합이 일정하다면 수레가 회전할 뿐 앞으로 나가는 속도는 변하지 않는 것과 같다.
결론
- 선형 시스템 모델에서는 중첩의 원리에 의해 직진과 회전이 완벽하게 분리(Decoupling)된다.
- 실제 하드웨어에서는 모터 성능 차이, 배터리 전압 강하 등의 비선형성으로 인해 약간의 간섭이 발생할 수 있다고 한다.