최적 서보 계 (적분형 최적 서보)를 실장 해 보았다

소개

최적 제어 × 기계 학습과 언제 최적 제어의 내용을 게시하지 않았기 때문에
최적의 서보 시스템을 시도합니다.
조금 최적의 서보 시스템을 만들 기회가 있었기 때문에 정리할 것이라고 생각합니다.
교과서는 다변수 시스템 저:이케다 마사오라는 녀석을 참고로 하고 있습니다

목적

적분형 최적 서보계를 실장

모델

모터처럼
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}

편차계

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

\boldsymbol{A_e}=
\begin{bmatrix}
A & 0\\
-C & 0
\end{bmatrix}\\

\boldsymbol{B_e}=
\begin{bmatrix}
B\\
0
\end{bmatrix}\\

\boldsymbol{C_e}=
\begin{bmatrix}
-C & 0
\end{bmatrix}

평가 함수

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

\int_{0}^{\infty}\boldsymbol{e^TQ_1e+\tilde{w}^TQ_2\tilde{w}+\tilde{u}^TR\tilde{u}}dt

e는 편차를 나타냅니다.
물결표는 최종값(목적값)과의 편차를 나타냅니다.
이렇게하면 최적의 레귤레이터 이론을 적용 할 수 있습니다.
이 편차계가 0이 되는 것이 목표가 되기 때문에!
Q와 R의 값은 적절히 설정하십시오.

최적 입력

최적 입력은 다음과 같이 요구됩니다.
교과서를보십시오.
또는 적분 형 최적 서보의 논문을보십시오.

\boldsymbol{u(t)=F_ax(t)+G_aw(t)+H_ar-G_aP^{-1}_{22}P^T_{12}x_0-G_aw_0}

블록 다이어그램

이런 식으로 걸어

프로그램

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

care

후에는 입력을 만들어 룬게쿠타 하는 것 뿐이네요! !

servo.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_1_sub = [1 1];
Q_2_sub = [1 1];
Q_1 = diag(Q_1_sub);
Q_2 = diag(Q_2_sub);

[m_q2, n_q2] = size(Q_2);

Q=[C'*Q_1*C zeros(m_c ,n_q2); zeros(m_q2, n_c) Q_2];

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

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

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

P_11=P(1:m_a, 1:m_b);
P_12=P(1:m_a, n_p-m_b+1:n_p);

[m_p12, n_p12] = size(P_12);

P_22=P(m_p-m_a+1:m_p ,n_p-n_p12+1:n_p);

F_a=-inv(R)*(B')*P_11;
G_a=-inv(R)*(B')*P_12;
H_a=[-F_a+G_a\(P_22)*(P_12') eye(m_c)]*inv([A B;C zeros(m_c,n_b)])*[zeros(m_c,n_l);eye(m_c)];

%% シミュレーション
% シミュレーション時間
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);

% 変数（補償器の状態）
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;
    %ルンゲクッタ
    %入力
    U(:,i)=F_a*X(:,i)+G_a*OME(:,i)+H_a*r-G_a*inv(P_22)*(P_12)'*X_0-G_a*W_0;

    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)];


    % ルンゲクッタ（モデル化誤差あり）
    % 入力
    U_de(:,i)=F_a*X_de(:,i)+G_a*OME_de(:,i)+H_a*r-G_a*inv(P_22)*(P_12)'*X_0-G_a*W_0;

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

    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');

결과

이런 느낌이 되어 각각 모델화 오차가 있는 것으로 잘 목표치에 추종할 수 있는 것을 알 수 있습니다

마지막으로

잘 작동했지만 정가이므로 뭔가 당연한 것처럼 보입니다 ...
다음은 최적 추종계와 적분기를 구현해 보겠습니다.

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

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