TECS(Total Energy Control System) 논문 정리 및 구현 (1) - 정리 에 이어서 구현이다.
종축에 대한 2 DOF 모델을 모의하고, TECS를 구현해보면 다음과 같다.
모의 모델의 특징을 정리하면 다음과 같다.
- 축의 정의는 전방을 X, 윗 방향을 Z라 정의한다.
- 피치 자세는 비행 경로각으로 모의한다.
- 추력 명령은 0~1 범위로 진행방향에 대해서 최대 1 N의 힘을 낸다.
- 엘리베이터 명령은 -1~1의 범위로 진행방향에 직교한 방향으로 최대 1N의 힘을 낸다. 양의 엘리베이터는 기수 올림으로 정의하자.
- TECS에 대해서 Anti Wind-up 을 적용하여 응답 성능을 개선한다.
이로부터 운동 방정식을 정리하면 다음과 같다.
$$ \cases{\vec{v}_{t+1} = \vec{v}_{t} + \Delta t \cdot \vec{a}_{t} \\ \vec{p}_{t+1} = \vec{p}_{t} + \Delta t \cdot \vec{v}_t}$$
$$ \theta_t \equiv \gamma_t = \text{atan2}\left(v_{z,t}, v_{x,t} \right) $$
항력은 속도에 대한 함수로 정의한다.
$$ \vec{D}_{t} = - C_d \left( \vec{v}_{t} \right)^T \vec{v}_{t} \frac{\vec{v}_{t}}{ | \vec{v}_{t} | }$$
추력은 속도 방향과 평행하게, 엘리베이터의 효과는 속도 방향($\gamma_t$)과 직교하게 기수 올림 힘을 (+)으로 정의하자.
$$\cases{ \vec{T}_{t} = \delta_{T,t} \left[ \matrix{\text{cos}\gamma_t \\ \text{sin}\gamma_t } \right] \\ \vec{N}_{t} = \delta_{e,t} \left[ \matrix{ -\text{sin}\gamma_t \\ \text{cos}\gamma_t } \right] } $$
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
|
clear;clc;
% close all;
figure;
dt = 1/10;
EndTime = 200;
g = 9.80665;
cd = 0.001;
time = zeros((EndTime/dt)+1);
px = zeros((EndTime/dt)+1);
pz = zeros((EndTime/dt)+1);
vx = zeros((EndTime/dt)+1);
vz = zeros((EndTime/dt)+1);
vt = zeros((EndTime/dt)+1);
ax = zeros((EndTime/dt)+1);
az = zeros((EndTime/dt)+1);
r = zeros((EndTime/dt)+1);
% Commands
Vc = 10;
Hc = 0;
% Gains
It = 0;
Ie = 0;
kh = 0.5;
kv = 1.0;
ktk = 2;
ktp = 1.0*ktk;
kti = 0.02*ktk;
kek = 10.0;
kep = 1.0*kek;
kei = 0.02*kek;
for i = 0 : (EndTime/dt)
% Initialization
if i == 0
time = 0;
px = 0;
pz = 0;
vx = 10;
vz = 1;
ax = 0;
az = 0;
vt = sqrt(vx.^2 + vz.^2);
r = atan2(vz,vx);
dthr = 0;
de = 0;
continue;
end
% Change Commands
if (i*dt < 50)
Vc = 10;
Hc = 0;
elseif(i*dt < 100)
Vc = 12;
elseif(i*dt < 150)
Hc = 30;
else
Vc = 11;
Hc = 0;
end
% Translate To TECS
he = Hc - pz(i);
re = (he / vt(i));
ve = (Vc - vt(i));
rc = min(max(-5/57.296, kh * re), 10/57.296);
dvc= kv * ve;
dv = norm(dot([ax(i) az(i)], [vx(i), vz(i)])) / norm([vx(i), vz(i)]);
er = rc - r(i);
edvg = (dvc - dv) / g;
% TECS Architecture
% It = max(min(1, kti * (edvg + er)),0);
% Ie = max(min(1, kei * (edvg - er)),-1);
% dthr(i+1) = max(min(1, ktp * (vt(i)/g + r(i)) + It),0);
% de(i+1) =-max(min(1, kep * (vt(i)/g - r(i)) + Ie),-1);
% TECS Architecture with Anti Wind-up
Pt = ktp * (edvg + er);
Pe = kep * (edvg - er);
It = It + kti * (edvg + er);
Ie = Ie + kei * (edvg - er);
if(Pt + It > 1)
It = 1 - Pt;
elseif(Pt + It < 0)
It = 0 - Pt;
end
if(Pe + Ie > 1)
Ie = 1 - Pe;
elseif(Pe + Ie < -1)
Ie = -1 - Pe;
end
dthr(i+1) = (Pt + It);
de(i+1) =-(Pe + Ie);
% Drag Force
dx = - cd * vx(i) * abs(vx(i));
dz = - cd * vz(i) * abs(vz(i));
% Longitudinal Motion
ax(i+1) = dx - de(i) * sin(r(i)) + dthr(i+1) * cos(r(i));
az(i+1) = dz + de(i) * cos(r(i)) + dthr(i+1) * sin(r(i));
% Kinematic Equations
time(i+1) = i * dt;
vx(i+1) = vx(i) + ax(i) * dt;
vz(i+1) = vz(i) + az(i) * dt;
px(i+1) = px(i) + vx(i) * dt;
pz(i+1) = pz(i) + vz(i) * dt;
r(i+1) = atan2(vz(i+1),vx(i+1));
vt(i+1) = sqrt(vx(i+1).^2 + vz(i+1).^2);
end
%%
at = dot([ax; az], [vx; vz])./vt;
subplot(321);
plot(time, vt); grid on; ylim([9 13]);
xlabel('Time [sec]'); ylabel('Vt [m/s]');
subplot(322);
plot(px, pz); grid on;
xlabel('pX [m]'); ylabel('pZ [m]');
subplot(323);
plot(time, r*57.296); grid on; ylim([-10 15]);
xlabel('Time [sec]'); ylabel('\gamma [deg]');
subplot(324);
plot(time, at); grid on;
xlabel('Time [sec]'); ylabel('At [m/s2]');
subplot(325);
plot(time, dthr*100); grid on;
xlabel('Time [sec]'); ylabel('dthr [%]');
subplot(326);
plot(time, de*100); grid on;
xlabel('Time [sec]'); ylabel('de [%]');
|
위 모델은 50초에 속력 명령을 12 m/s으로, 100초에 고도 명령을 30m으로, 150초에 속력 명령을 11m/s으로 인가한다.
튜닝의 문제인지 명령 인가로 인해 다른 상태가 영향을 크게 받는다.
최적 제어 문제로 정의하고 한번 풀어봐야겠다.
나의 생각
논문에 따르면 TECS의 입력을 변환할 때, 고도와 속력 오차 간의 올바른 에너지 관계는 $k_h = k_v$ 로 선정함으로써 보존된다고 한다. 또한 고도와 속력 제어는 원리 상 해당 비례 이득 $k_h$와 $k_v$의 역수에 비례한다.내 생각에, 고도 제어 응답보다 속력 제어 응답이 월등히 좋기 때문에, 이러한 특성을 반영해야하지 않나.. 싶다.또한 조종면이 고정되어있으며, 에너지가 일정하다면 다음의 관계식으로부터 $k_h$와 $h_v$는 다음과 같아야할 것이라 생각한다.
$$\dot{B} = -\dot{E}_p + \dot{E}_k \approx - \gamma + \dot{V}/g = 0$$
$$\dot{h}_c = k_h \cdot h_e , \hspace{5mm} \gamma_c =\dot{h}/V \rightarrow \gamma_c = k_h * h_e /V$$