최적 추종계 + 적분 보상기(최적 서보) 실장해 보았다

소개

최적 제어 × 기계 학습과 언제 최적 제어의 내용을 게시하지 않았기 때문에
전회는 적분형 최적 서보계였지만 이번은 최적추종형＋적분 보상기로 서보계를 구성합니다

목적

최적추종형＋적분보상기를 실장

모델

모터처럼
1차 지연계로 합니다

\dot{\boldsymbol{x}}=\boldsymbol{Ax}+\boldsymbol{Bu}\\
\boldsymbol{y}=\boldsymbol{x}

\boldsymbol{A}=
\begin{bmatrix}
-1/0.54 & 0\\
0 & -1/0.54
\end{bmatrix}\\

\boldsymbol{B}=
\begin{bmatrix}
1/0.54 & 0\\
0 & 1/0.54
\end{bmatrix}

비교 모델

모델화 오차가 있어도 괜찮다고 증명하고 싶기 때문에, 모델화 오차 있어도 해 둡니다.

\acute{\boldsymbol{A}}=
\begin{bmatrix}
-1/0.54+1 & 0\\
0 & -1/0.54+1
\end{bmatrix}\\

\acute{\boldsymbol{B}}=
\begin{bmatrix}
1/0.54 & 0\\
0 & 1/0.54
\end{bmatrix}

목표 신호

정치 r입니다! 정치가 아니어도 할 수 있습니다.

\boldsymbol{r}=
\begin{bmatrix}
5\\
3
\end{bmatrix}

정상상태

정상상태에서는 이와 같은 식이 되므로, 이 식과 원래의 상태방정식의 차이를 취하여 편차계를 구성합니다

\boldsymbol{0=Ax_\infty+ Bu_\infty\\
r=Cx_\infty}

편차계

여기가 적분형 최적 서보와는 크게 다릅니다.
우선 단순한 최적 추종형을 구성합니다

\dot{\boldsymbol{\tilde{x}}}=\boldsymbol{{A_e}\tilde{x}}+\boldsymbol{B_e\tilde{u}}\\
\boldsymbol{e}=\boldsymbol{C_e\tilde{x}}

\boldsymbol{A_e=A}\\

\boldsymbol{B_e=B}\\

\boldsymbol{C_e = -C}

블록 다이어그램 참조!



여기에도 쓰여 있듯이 이것이라면, FF 요소(피드 포워드 요소)에 대한 피드백이 없기 때문에, 단지 출력이 나오고 있는지 모릅니다
단, 이런 편차계를 구성하면 최적 레귤레이터 이론을 적용할 수 있으므로 이대로 평가 함수를 구성합니다.

평가 함수

다음 평가 함수를 최소화합니다.

\int_{0}^{\infty}\boldsymbol{e^TQe+\tilde{u}^TR\tilde{u}}dt

e는 편차를 나타냅니다.
물결표는 최종값(목적값)과의 편차를 나타냅니다.
Q와 R의 값은 적절히 설정하십시오.

최적 입력

최적 입력은 다음과 같이 요구됩니다.
교과서를보십시오.

\boldsymbol{u(t)=F_0x(t)+H_0r\\
F_0=-R^{-1}B^TP\\
H_0 = \begin{bmatrix}
F_0 & \boldsymbol{I}
\end{bmatrix}
\begin{bmatrix}
A & \boldsymbol{B}\\
\boldsymbol{C} & \boldsymbol{0}
\end{bmatrix}^{-1}
\begin{bmatrix}
0\\
\boldsymbol{I}
\end{bmatrix}
}

적분 보상기 추가

나중에 시뮬레이션에서도 확인하지만, 이것이라면 모델링 오차가없는 경우는 매우 잘 가지만, 있으면 잘 작동하지 않습니다.
그래서 적분 보상기를 추가합니다.
자세한 설명은 논문 등에 맡기기로 하겠지만 적분보상기를 추가한 최적추종형+적분보상기의 입력은 다음 식으로 나타낼 수 있습니다.

\begin{align}
\boldsymbol{u(t)=F_0x(t)+H_0r+Gz(t)\\
z(t)=w(t)+F_1x(t)-F_1x_0-w_0}
\end{align}

블록 다이어그램

프로그램

Matlab에서 작성했습니다.
편차계에 대해서, 리카치 방정식을 요구하고 있습니다!
리카치 방정식의 해는 이제 matlab로 요구됩니다.

care

후에는 입력을 만들어 룬게쿠타 하는 것 뿐이네요! !
첫 번째는 최적 추적 시스템 만
두 번째는 적분 보상기 포함

siteki_track.m

clc
clear 
close all

%% Model
% 時定数
tau = 0.54;
tau_de = 0.54;
A = [-1/tau 0; 0 -1/tau];
B = [1/tau 0; 0 1/tau];
C = [1 0; 0 1];

% モデル化誤差ありver
A_de = [-1/tau_de+1 0; 0 -1/tau_de+1];
B_de = [1/tau_de 0; 0 1/tau_de];

%% 要件チェック
% 可制御性確認
[m_a, n_a] = size(A);%行列Aのサイズ

Ct = ctrb(A, B)
C_rank = rank(Ct)

if C_rank < m_a
    disp('Not controlable')
    quit
end

% 可観測性確認
Ob = obsv(A, C)
O_rank = rank(Ob)

if O_rank < n_a
    disp('Not observable')
    quit
end

% 入力と出力の数確認
[m_b, n_b] = size(B);
[m_c, n_c] = size(C);

if n_b < m_c
    disp('Not controlable')
    quit
end

% 今回は定値を想定（条件4は無条件にパス）
J = 0;
L = [1 0; 0 1];

[m_l, n_l] = size(L);

%% 目標値
%　x方向の速度
r_x = 5;

% y方向の速度
r_y = 3;

% 合わせたもの
r = [r_x; r_y];

%% 制御系構成
% 重みづけ
R_sub = [0.1 0.1];
R = diag(R_sub);

Q_sub = [1 1];
Q = diag(Q_sub); 

%% リカッチ方程式解法
lqr(A, B, Q, R);

[P,L,G]=care(A, B, Q, R)%連続リカッチ方程式

% 制御則導出
[m_p, n_p] = size(P);

% 必要な係数計算
F_0 = -inv(R)*(B')*P;

F_1 = C*inv(A+B*F_0);

[m_f, n_f] = size(F_0)
temp1_H_0 = [-F_0, eye(m_f)];
temp2_H_0 = inv([A, B; C, zeros(m_c, n_b)]);
temp3_H_0 =[zeros(m_c, n_b); eye(m_c)];
H_0 = temp1_H_0*temp2_H_0*temp3_H_0;

%% シミュレーション
% シミュレーション時間
k = 100;
i = 0;

% sampling 時間
dt = 0.05;

% 変数（ロボットの状態）
v_x = zeros(1, k);
v_y = zeros(1, k);
X = zeros(2, k);
U = zeros(2, k);

% 変数（ロボットの状態）モデル化誤差ありver
v_x_de = zeros(1, k);
v_y_de = zeros(1, k);
X_de = zeros(2, k);
U_de = zeros(2, k);

% グラフ用（目標値）
r_graph = zeros(2, k);
r_graph(:, 1) = r;

% 初期値
i = 0;
X_0 = [0; 0];%対象システムの状態量
X_ex(:,1) = [X_0];%拡張系の状態量
X_ex_de(:,1) = [X_0];%拡張系の状態量モデル化誤差あり

for t = 0:dt:dt*(k-1)
    i  = i + 1;
    %ルンゲクッタ
    %入力
    U(:,i)=F_0*X(:,i)+H_0*r;

    X1 = X_ex(:,i);       k1 = dt*(A*X1+B*U(:,i));
    X2 = X_ex(:,i)+k1/2;  k2 = dt*(A*X2+B*U(:,i));
    X3 = X_ex(:,i)+k1/2;  k3 = dt*(A*X3+B*U(:,i));
    X4 = X_ex(:,i)+k3;    k4 = dt*(A*X4+B*U(:,i));

    X_ex(:,i+1)=X_ex(:,i) + (k1+2*k2+2*k3+k4)/6;

    v_x(i+1) = X_ex(1,i+1);
    v_y(i+1) = X_ex(2,i+1);
    r_graph(:, i+1) = r;

    X(:,i+1) = [v_x(i+1); v_y(i+1)];    

    % ルンゲクッタ（モデル化誤差あり）
    %入力
    U_de(:,i)=F_0*X_de(:,i)+H_0*r;

    % 外乱
    d = [3; 2; 0 ;0];

    X1 = X_ex_de(:,i);       k1 = dt*(A_de*X1+B_de*U_de(:,i) );
    X2 = X_ex_de(:,i)+k1/2;  k2 = dt*(A_de*X2+B_de*U_de(:,i));
    X3 = X_ex_de(:,i)+k1/2;  k3 = dt*(A_de*X3+B_de*U_de(:,i));
    X4 = X_ex_de(:,i)+k3;    k4 = dt*(A_de*X4+B_de*U_de(:,i));

    X_ex_de(:,i+1)=X_ex_de(:,i) + (k1+2*k2+2*k3+k4)/6;

    v_x_de(i+1) = X_ex_de(1,i+1);
    v_y_de(i+1) = X_ex_de(2,i+1);
    r_graph(:, i+1) = r;

    X_de(:,i+1) = [v_x_de(i+1); v_y_de(i+1)];

end

%% Figure

% GUIのフォント
set(0, 'defaultUicontrolFontName', 'Times New Roman');
% 軸のフォント
set(0, 'defaultAxesFontName','Times New Roman');
% タイトル、注釈などのフォント
set(0, 'defaultTextFontName','Times New Roman');
% GUIのフォントサイズ
set(0, 'defaultUicontrolFontSize', 18);
% 軸のフォントサイズ
set(0, 'defaultAxesFontSize', 20);
% タイトル、注釈などのフォントサイズ89
set(0, 'defaultTextFontSize', 20);

% グラフ用時間
time=0:dt:(k)*dt;

% x方向速度時刻歴
figure('Name','v_x','NumberTitle','off')%車体速度の時刻歴
plot(time , v_x,'LineWidth',3, 'Color', [1, 0.3, 0]);
hold on
plot(time , v_x_de,'LineWidth',3, 'Color',[0, 0.3, 1]);
hold on
plot(time, r_graph(1, :),'LineWidth',3, 'Color',[0, 0, 0], 'Linestyle', ':');
hold on
grid on
hold on
box on
hold on
xlabel('time [s]','FontSize',20);
ylabel('{\it v_x} [m/s]','FontSize',20);
legend('\it v_x', '\it v_x delta', '\it r_x');

% y方向速度時刻歴
figure('Name','v_y','NumberTitle','off')%車体速度の時刻歴
plot(time , v_y,'LineWidth',3, 'Color', [1, 0.3, 0]);
hold on
plot(time , v_y_de,'LineWidth',3, 'Color',[0, 0.3, 1]);
hold on
plot(time, r_graph(2, :),'LineWidth',3, 'Color',[0, 0, 0],  'Linestyle', ':');
hold on
grid on
hold on
box on
hold on
xlabel('time [s]','FontSize',20);
ylabel('{\it v_y} [m/s]','FontSize',20);
legend('\it x_y', '\it v_y delta', '\it r_y');

이것이 적분 보상기 포함

servo_siteki_track.m

clc
clear 
close all

%% Model
% 時定数
tau = 0.54;
tau_de = 0.54;
A = [-1/tau 0; 0 -1/tau];
B = [1/tau 0; 0 1/tau];
C = [1 0; 0 1];

% モデル化誤差ありver
A_de = [-1/tau_de+1 0; 0 -1/tau_de+1];
B_de = [1/tau_de 0; 0 1/tau_de];

%% 要件チェック
% 可制御性確認
[m_a, n_a] = size(A);%行列Aのサイズ

Ct = ctrb(A, B)
C_rank = rank(Ct)

if C_rank < m_a
    disp('Not controlable')
    quit
end

% 可観測性確認
Ob = obsv(A, C)
O_rank = rank(Ob)

if O_rank < n_a
    disp('Not observable')
    quit
end

% 入力と出力の数確認
[m_b, n_b] = size(B);
[m_c, n_c] = size(C);

if n_b < m_c
    disp('Not controlable')
    quit
end

% 今回は定値を想定（条件4は無条件にパス）
J = 0;
L = [1 0; 0 1];

[m_l, n_l] = size(L);

%% 目標値
%　x方向の速度
r_x = 5;

% y方向の速度
r_y = 3;

% 合わせたもの
r = [r_x; r_y];

%% 制御系構成
%% 偏差系
% モデル化誤差なし
A_rel = [A, zeros(m_a, m_l); -C, zeros(m_c, m_l)];
B_rel = [B; zeros(m_l ,n_b)];
r_rel = [zeros(m_c, n_l); eye(m_c)];
C_rel = [-C, zeros(m_c, n_l)];

% モデル化誤差あり
A_rel_de = [A_de, zeros(m_a, m_l); -C, zeros(m_c, m_l)];
B_rel_de = [B_de; zeros(m_l ,n_b)];
r_rel_de = [zeros(m_c, n_l); eye(m_c)];
C_rel_de = [-C, zeros(m_c, n_l)];

% 重みづけ
R_sub = [0.1 0.1];
R = diag(R_sub);

Q_sub = [1 1];
Q = diag(Q_sub); 

%% リカッチ方程式解法
lqr(A, B, Q, R);

[P,L,G]=care(A, B, Q, R)%連続リカッチ方程式

% 制御則導出
[m_p, n_p] = size(P);

% 必要な係数計算
F_0 = -inv(R)*(B')*P;

F_1 = C*inv(A+B*F_0);

[m_f, n_f] = size(F_0);
temp1_H_0 = [-F_0, eye(m_f)];
temp2_H_0 = inv([A, B; C, zeros(m_c, n_b)]);
temp3_H_0 =[zeros(m_c, n_b); eye(m_c)];
H_0 = temp1_H_0*temp2_H_0*temp3_H_0;

G = -inv(R)*(F_1*B)'*[1 0;0 1];

%% シミュレーション
% シミュレーション時間
k = 200;
i = 0;

% sampling 時間
dt = 0.05;

% 変数（ロボットの状態）
v_x = zeros(1, k);
v_y = zeros(1, k);
X = zeros(2, k);
U = zeros(2, k);
V = zeros(2, k);%仮想入力

% 変数（ロボットの状態）モデル化誤差ありver
v_x_de = zeros(1, k);
v_y_de = zeros(1, k);
X_de = zeros(2, k);
U_de = zeros(2, k);

% 変数（補償器の状態）
ome_x = zeros(1, k);
ome_y = zeros(1, k);
OME = zeros(2, k);

% 変数（補償器の状態）モデル化誤差ありver
ome_x_de = zeros(1, k);
ome_y_de = zeros(1, k);
OME_de = zeros(2, k);

% グラフ用（目標値）
r_graph = zeros(2, k);
r_graph(:, 1) = r;

% 初期値
i = 0;
X_0 = [0; 0];%対象システムの状態量
W_0 = [0; 0];%補償器の状態量
X_ex(:,1) = [X_0;W_0];%拡張系の状態量
X_ex_de(:,1) = [X_0;W_0];%拡張系の状態量モデル化誤差あり

for t = 0:dt:dt*(k-1)
    i  = i + 1;    
    % 仮想入力の計算
    V(:, i) = G * (OME(:, i) + (F_1) * X(:, i) - F_1 * X_0 - W_0);

    % 入力
    U(:,i) = F_0*X(:,i) + H_0*r + V(:,i);

    % 状態更新
    X1 = X_ex(:,i);       k1 = dt*(A_rel*X1+B_rel*U(:,i) + r_rel*r);
    X2 = X_ex(:,i)+k1/2;  k2 = dt*(A_rel*X2+B_rel*U(:,i) + r_rel*r);
    X3 = X_ex(:,i)+k1/2;  k3 = dt*(A_rel*X3+B_rel*U(:,i) + r_rel*r);
    X4 = X_ex(:,i)+k3;    k4 = dt*(A_rel*X4+B_rel*U(:,i) + r_rel*r);

    X_ex(:,i+1)=X_ex(:,i) + (k1+2*k2+2*k3+k4)/6;

    v_x(i+1) = X_ex(1,i+1);
    v_y(i+1) = X_ex(2,i+1);
    ome_x(i+1) = X_ex(3,i+1);
    ome_y(i+1) = X_ex(4,i+1);
    r_graph(:, i+1) = r;

    X(:,i+1) = [v_x(i+1); v_y(i+1)];
    OME(:,i+1) = [ome_x(i+1);ome_y(i+1)];

    % ルンゲクッタ（モデル化誤差あり）
    % 仮想入力の計算
    V(:, i) = G * (OME_de(:, i) + (F_1) * X_de(:, i) - F_1 * X_0 - W_0);

    % 入力
    U_de(:,i) = F_0*X(:,i) + H_0*r + V(:,i);

    X1 = X_ex_de(:,i);       k1 = dt*(A_rel_de*X1+B_rel_de*U_de(:,i) + r_rel*r);
    X2 = X_ex_de(:,i)+k1/2;  k2 = dt*(A_rel_de*X2+B_rel_de*U_de(:,i) + r_rel*r);
    X3 = X_ex_de(:,i)+k1/2;  k3 = dt*(A_rel_de*X3+B_rel_de*U_de(:,i) + r_rel*r);
    X4 = X_ex_de(:,i)+k3;    k4 = dt*(A_rel_de*X4+B_rel_de*U_de(:,i) + r_rel*r);

    X_ex_de(:,i+1)=X_ex_de(:,i) + (k1+2*k2+2*k3+k4)/6;

    v_x_de(i+1) = X_ex_de(1,i+1);
    v_y_de(i+1) = X_ex_de(2,i+1);
    ome_x_de(i+1) = X_ex_de(3,i+1);
    ome_y_de(i+1) = X_ex_de(4,i+1);
    r_graph(:, i+1) = r;

    X_de(:,i+1) = [v_x_de(i+1); v_y_de(i+1)];
    OME_de(:,i+1) = [ome_x_de(i+1);ome_y_de(i+1)];

end

%% Figure

% GUIのフォント
set(0, 'defaultUicontrolFontName', 'Times New Roman');
% 軸のフォント
set(0, 'defaultAxesFontName','Times New Roman');
% タイトル、注釈などのフォント
set(0, 'defaultTextFontName','Times New Roman');
% GUIのフォントサイズ
set(0, 'defaultUicontrolFontSize', 18);
% 軸のフォントサイズ
set(0, 'defaultAxesFontSize', 20);
% タイトル、注釈などのフォントサイズ89
set(0, 'defaultTextFontSize', 20);

% グラフ用時間
time=0:dt:(k)*dt;

% x方向速度時刻歴
figure('Name','v_x','NumberTitle','off')%車体速度の時刻歴
plot(time , v_x,'LineWidth',3, 'Color', [1, 0.3, 0]);
hold on
plot(time , v_x_de,'LineWidth',3, 'Color',[0, 0.3, 1]);
hold on
plot(time, r_graph(1, :),'LineWidth',3, 'Color',[0, 0, 0], 'Linestyle', ':');
hold on
grid on
hold on
box on
hold on
xlabel('time [s]','FontSize',20);
ylabel('{\it v_x} [m/s]','FontSize',20);
legend('\it v_x', '\it v_x delta', '\it r_x');

% y方向速度時刻歴
figure('Name','v_y','NumberTitle','off')%車体速度の時刻歴
plot(time , v_y,'LineWidth',3, 'Color', [1, 0.3, 0]);
hold on
plot(time , v_y_de,'LineWidth',3, 'Color',[0, 0.3, 1]);
hold on
plot(time, r_graph(2, :),'LineWidth',3, 'Color',[0, 0, 0],  'Linestyle', ':');
hold on
grid on
hold on
box on
hold on
xlabel('time [s]','FontSize',20);
ylabel('{\it v_y} [m/s]','FontSize',20);
legend('\it x_y', '\it v_y delta', '\it r_y');

결과

이런 느낌이 듭니다
우선, 최적 추종형만입니다만, 이것으로는 모델화 오차가 있는 경우 편차가 남아 있는 것을 확인할 수 있습니다




한편, 최적 추종형으로 적분 보상기를 추가하면 잘 작동하는 것으로 나타났습니다.
모델링 오차가 없는 경우는 적분의 정보는 필요 없기 때문에, 적분형 최적 서보계보다,
이쪽이 응답 성은 좋을 것 같습니다.

마지막으로

잘 작동했지만 적분형 최적 서보 때와 같이 정치이므로 뭔가라는 느낌입니다.
정치가 아니어도 할 수 있으므로, 이번에는 그쪽으로 도전합니다
그 전에 NN에서도 놀자고 생각합니다.

오늘의 프로그램(gituhub)
htps : // 기주 b. 코 m / 슈니치 09 / 쿠이타 / t Ree / Ma s r / 혼 t l / r r ら

트위터
htps : // 라고 해서 r. 코m/슈니치세키구 1