はじめに
本記事では下記の2つの記事で解説した手法を組み合わせて、外乱に対応した台車型倒立振子の制御を行います。
外乱が存在する中では、理想的なモデルに対して設計された制御器は思い通りに対象の状態をコントロールできません。もしも外乱がどのようなものであるかを予め知ることができれば、外乱の効果を打ち消すような制御入力を計算することでこれに対処できそうです。 しかし一般的に、外乱はどのような大きさのものが生じているかは分からないため、これを推定してやる必要があります。推定に関する記事と、理想的なモデルに関するLQRの記事の問題設定をそのまま流用して実験を行います。
理想的な状況下での制御
制御対象のシステム
まず制御対象の状態空間モデルは $$ \dot x = A x + B u $$ と与えられているとします。また観測方程式は $$ y = Cx + Du $$ です。台車式倒立振子の方程式は、倒立が成功している状態の近傍にて下記のように表されます。本来は非線形方程式ですが、今回は線形化することでLQRを用いていきます。
$$ \begin{align} A _ {23} &= -\frac{3mg}{4M+m} \\ A _ {43} &= \frac{6(M+m)g}{4Ml+ml} \\ A &= \begin{bmatrix} 0 & 1 & 0 & 0 \\ 0 & 0 & A _ {23} & 0 \\ 0 & 0 & 0 & 1 \\ 0 & 0 & A _ {43} & 0 \end{bmatrix} \\ B _ 2 &= \frac{4}{4M+m} \\ B _ 4 &= -\frac{6}{4Ml+ml}\\ B &= \begin{bmatrix} 0 \\ B _ 2 \\ 0 \\ B _ 4 \end{bmatrix} \\ C &= \begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} \\ D &= \begin{bmatrix} 0 \\ 0 \\ 0 \\ 0 \end{bmatrix} \end{align} $$
上記の数式に関しては下記の記事を参考にしています。
外乱が存在しない例
外乱が存在しない場合での制御は下記の図のように、適切にターゲット位置に台車を持っていきつつ、振子が真上になる状態に収束しています。こちらはLQRの記事で解説した内容をターゲットを変えたに過ぎない例で、予想通り上手く行っています。
外乱が存在するが既知の例
外乱が存在する場合は、下記のようにシステム方程式が修正されることになります。
$$ \dot x = A x + B u + B _ d d $$
$d$ が外乱の項で、$B _ d$ が外乱が発生したとして状態にどのような作用を及ぼすかを表した行列です。 今回は、制御の際に台車に力を作用させるシステムを想定しています。簡単のため、外乱と呼ばれるものは同様に台車に対して何らかの力が作用するケースを想定します。これは台車の移動の際に摩擦があるとか、空気抵抗があるとか、機械的なガタで力がうまく伝わらない等が考えられます。台車に外乱として力が作用すると考えた場合、その外乱の状態への作用の仕方は制御入力 $u$ と同等であるため $B _ d = B$ と考えて差し支えないでしょう。
すると
$$ \dot x = A x + B (u + d) $$
が実システムの正体であることになります。制御設計時には $\dot x = A x + B u$ という外乱を想定しないコントローラが導出されているため、このままではシステムを上手くコントロールできません。今コントローラは$u = K x$ と設計されているとしましょう(LQRは最適な $K$ を求めてくれています)。そうするとコントローラを用いたシステムの方程式は
$$ \dot x = A x + B (Kx + d) $$
となっているはずです。理想的なシステムは $\dot x = Ax + BKx$ なのですから $d$ をどのように打ち消そうかという問題ですが、今回のケースでは非常に簡単です。$u = K x$ の代わりに
$$ u = Kx - d $$
となっていれば良く、仮に外乱 $d$ が既知なのであれば、単にLQRのコントローラが導出する制御入力 $u$ に外乱の項を差し引いてやれば良いだけになります。 こうすることで状態方程式が理想的な状況と同様になるため、倒立振子の振る舞いは上記の例と一致します。(ただし、外乱を打ち消せるほどの出力をアクチュエータが出せるかは別問題なので、ハード構成のことは要検討)
現実の状況下での制御
さて、実際には外乱の値に関しては未知なので、上記のような理想的な打ち消しは不可能です。代わりに外乱オブザーバーによって外乱値を推定してあげて、それを使って制御入力を修正することを考えてみましょう。まずは外乱を推定せずにほったらかしにするとどうなるのかを見て、それと推定値を入れた場合を比較しましょう。
外乱が存在するが考慮しない例
外乱としては $d = 1.5 \cos(2\pi f t)$ において $f = 0.01$ を選びました。緩やかに外乱は変動しますが、制御周期から見たらほぼ一定値のように見える問題です。この未知外乱を放置するとどうなるのかを下の図に示しました。 素早く参照位置に辿り着こうとしますが、制御器は外乱を考慮したゲイン設計になっていないため、最終的には参照位置から離れたところに引き戻されてしまっています。外乱と、フィードバック制御による力がつり合ったところで留まってしまう振る舞いです。
外乱を推定し補償した例
さて、外乱を推定するために下記の手法を用います。
拡張状態空間モデルは
$$ \begin{align} A_e &= \begin{bmatrix} 0 & 1 & 0 & 0 & 0 \\ 0 & 0 & -\frac{3mg}{4M+m} & 0 & 0 \\ 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & \frac{6(M+m)g}{(4M+m)l} & 0 & 0 \\ 0 & 0 & 0 & 0 & 1 \end{bmatrix} \\ B_e &= \begin{bmatrix} 0 \\ \frac{4}{4M+m} \\ 0 \\ -\frac{6}{(4M+m)l} \\ 0 \end{bmatrix} \\ C_e &= \begin{bmatrix} 1 & 0 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 & 0 \\ 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 1 & 0 \end{bmatrix} \end{align} $$
となります。これに対してカルマンフィルタを適用し、状態量の最後の成分を取り出すと外乱が推定されていることになります。(詳しい解説は貼った記事を参考にしてください)
下の図は、カルマンフィルタにより逐次推定された外乱と、実際に加わっている外乱を比較したものです。倒立振子の位置と角度の状態推定は、観測値を得られている設定であるため予測と照合するだけとなります。予測には自身が1期前に推定した外乱も利用されるため、予測と観測はほぼ一致し、結果としてフィルタリングされた値も遅れなく推定できています(図の上2つ)。一方で外乱は観測ができないため、観測できている情報から逆算をせざるを得ません。カルマンフィルタは観測値を信じるかモデルの予測値を信じるかのいい塩梅を逐次見つける手法です。最初は外乱なんて無いと考えているところから、次第に状態の観測値の情報から逆算して外乱が発生していることに気づいていく様子が見えます(図の一番下)。
この推定値をフィードバック制御の入力の補償に用いた場合の振る舞いを下図に示しました。最終的に参照位置まで適切に移動できていそうなのが見て伺えます。
コード例
clear % 倒立振子の物理的パラメータ M = 1.0; % カートの質量 [kg] m = 0.1; % 振子の質量 [kg] b = 0.001; % カートとレールの摩擦係数 [N/(m/s)] l = 0.5; % 振子の長さ [m] g = 9.81; % 重力加速度 [m/s^2] % 摩擦なし重心位置が棒にある A23 = -3*m*g/(4*M+m); A43 = 6*(M+m)*g/(4*M*l+m*l); A = [0 1 0 0; 0 0 A23 0;0 0 0 1; 0 0 A43 0]; B2 = 4/(4*M+m); B4 = -6/(4*M*l+m*l); B = [0;B2;0;B4]; C = diag([1,1,1,1]); D = zeros(1, 1); % シミュレーションの設定 Tf = 10; % シミュレーション時間 [s] Ts = 0.05; % サンプリング時間 [s] T = 0:Ts:Tf; % シミュレーションの時間軸 N = length(T); % シミュレーションの時間ステップ数 % 離散化 sysc = ss(A,B,C,D); sysd = c2d(sysc, Ts); A = sysd.A; B = sysd.B; C = sysd.C; D = sysd.D; B_d = B; % 外乱付きの拡大状態空間モデル [A_e, B_e, C_e] = make_expansion_sys(A,B,C,B_d); rank(obsv(A_e,C_e)) % == 5 可観測 % 離散最適レギュレータの設計 Q = diag([1, 0.1, 100, 0.1]); R = 0.1; [K, S, E] = dlqr(A, B, Q, R); % 参照信号の定義 x_ref = zeros(4, N); target_pos = 1.0 x_ref(1, :) = target_pos; % x_ref(1, :) = 2*sin(2*pi*T); % 初期状態の定義 x0 = [0; 0; pi/4; 0]; % 制御入力の初期化 u = zeros(1, N-1); % 外乱 d = 1.5*cos(2*pi*0.01*T); % 拡大状態カルマンフィルタのパラメータ x_e = zeros(5, N); Q_e = 1000 * eye(5); R_e = 0.001; P_e = Q_e; % シミュレーションの実行 x = zeros(4, N); x(:, 1) = x0; x_e(:, 1) = [x0; 0]; d_e = zeros(1, N); for k = 2:N % 1つ前の状態と参照信号の取得 xk = x(:, k-1); x_ref_k = x_ref(:, k-1); % 状態フィードバックによる制御入力の計算 u_lqr = -K*(xk - x_ref_k); d_effect = B_d*d(k-1); compensation = - d_e(:,k-1); % compensation = 0; u(k-1) = u_lqr + compensation; c_effect = B * u(k-1); % シミュレーションの実行 現在の状態が求まる x(:, k) = A*xk + c_effect + d_effect; % Measurement update y = C * x(:, k); % Kalman filter prediction 拡大状態空間モデルの予測値 [x_e(:,k), P_e] = kalman_pred(A_e,B_e,Q_e,P_e,x_e(:,k-1),u(k-1)); % Kalman filter update 観測情報との照合 [x_e(:,k), P_e, ~] = kalman_filt(C_e, P_e, R_e, x_e(:,k), y); d_e(:,k) = x_e(end,k); end figure; subplot(3, 1, 1); plot(T, x(1, :), 'LineWidth', 2); hold on; plot(T, x_ref(1, :), 'r--', 'LineWidth', 2); xlabel('Time [s]'); ylabel('Position [m]'); legend('Cart position', 'Reference'); grid on; subplot(3, 1, 2); plot(T, x(3, :), 'LineWidth', 2); hold on; plot(T, x_ref(3, :), 'r--', 'LineWidth', 2); xlabel('Time [s]'); ylabel('Angle [rad]'); legend('Pole angle', 'Reference'); grid on; subplot(3,1,3) plot(T(1:end-1), u, 'LineWidth', 2); xlabel('Time [s]'); ylabel('Force [N]'); grid on; % Plot results figure; subplot(3, 1, 1); plot(1:N, x(1, :), 'b', 1:N, x_e(1, :), 'r'); xlabel('Time Step'); ylabel('State x1'); legend('True state x1', 'Estimated state x1'); title('State Estimation'); subplot(3, 1, 2); plot(1:N, x(3, :), 'b', 1:N, x_e(3, :), 'r'); xlabel('Time Step'); ylabel('State x2'); legend('True state x2', 'Estimated state x2'); title('State Estimation'); subplot(3, 1, 3); plot(1:N, d, 'b', 1:N, x_e(5, :), 'r'); xlabel('Time Step'); ylabel('Disturbance d'); legend('True disturbance', 'Estimated disturbance'); title('Disturbance Estimation'); % アニメーションの描画 figure; axis_limit = 2; % カートと振子の表示範囲 x = x'; frames = zeros(400, 400, 3, N, 'uint8'); for i = 1:N cla; cart_x = x(i, 1); pendulum_x = cart_x + l * sin(x(i, 3)); pendulum_y = l * cos(x(i, 3)); plot([cart_x, pendulum_x], [0, pendulum_y], '-', 'LineWidth', 2); hold on; yline(0, "LineWidth", 2) plot(cart_x, 0, 'ks', 'LineWidth', 2, 'MarkerSize', 20); xline(target_pos, "-", "target pos") hold off; xlim([-axis_limit, axis_limit]); ylim([-axis_limit, axis_limit]); xlabel('x [m]'); ylabel('y [m]'); title('Inverted Pendulum Animation'); drawnow; end function [A_e, B_e, C_e] = make_expansion_sys(A,B,C,Bd) A_e = [A, Bd; zeros(1, size(A,2)), eye(1)]; B_e = [B; zeros(1, 1)]; C_e = [C, zeros(size(C,1), 1)]; end function [x, P] = kalman_pred(A,B,Q,P,x,u) x = A * x + B * u; P = A * P * A' + Q; end function [x, P, K] = kalman_filt(C, P, R, x, y) K = P * C' / (C * P * C' + R); x = x + K * (y - C * x); P = (eye(size(P,1)) - K * C) * P; end
さいごに
今回は外乱を台車に作用している力だとして扱いました。これにより、外乱補償は推定値をそのまま減算すれば良いという単純な問題に設定できました。 実際の問題では制御入力への反映の仕方は変わってきたり、あるいは推定値が信用できるかも怪しいので、適切にゲインを設定したりします。余裕のある方は、観測値にノイズを含ませたり変動の早い外乱を用いるなどして実験してみてください。
一旦、このシリーズは終わろうかなと思います。
次は眠らせた状態のクープマン作用素を用いたシステム同定の話をうまくつなげて、システム方程式が未知の場合にそれを同定し、制御するという問題ができたら良いなと思います。クープマン作用素に関しては下記の記事を参考にしてください。