単純な振り子の最適制御

私はさまざまな最適な制御方法を研究しており（Matlabでそれらを実装しています）、テストケースとして（今のところ）単純な振り子（地面に固定）を選択します。

「単純な」フィードバック法（エネルギー制御に基づくスイングアップ+上部位置のLQR安定化）を使用して制御し、状態軌跡を図に示します（軸の説明を忘れました：xはtheta、yはthetaドット。

次に、反復LQRメソッド（ここで実装されていることがわかりましたhttp://homes.cs.washington.edu/~todorov/software/ilqg_det.m）から始めて、「完全な」最適な制御方法を試したいと思います。

この方法には、1つの動的関数と1つのコスト関数x = [theta; theta_dot], uが必要です（モータートルク（1つのモーターのみ））：

function [xdot, xdot_x, xdot_u] = ilqr_fnDyn(x, u)
    xdot = [x(2);
        -g/l * sin(x(1)) - d/(m*l^2)* x(2) + 1/(m*l^2) * u];
    if nargout > 1
        xdot_x = [ 0, 1;
            -g/l*cos(x(1)), -d/(m*l^2)];
        xdot_u = [0; 1/(m*l^2)];
    end
end

function [l, l_x, l_xx, l_u, l_uu, l_ux] = ilqr_fnCost(x, u, t)
    %trying J = x_f' Qf x_f + int(dt*[ u^2 ])
    Qf = 10000000 * eye(2);
    R = 1;
    wt = 1;
    x_diff = [wrapToPi(x(1) - reference(1)); x(2)-reference(2)];

    if isnan(t)
        l = x_diff'* Qf * x_diff;
    else
        l = u'*R*u;
    end

    if nargout > 1
        l_x = zeros(2,1);
        l_xx = zeros(2,2);
        l_u = 2*R*u;
        l_uu = 2 * R;
        l_ux = zeros(1,2);

        if isnan(t)
            l_x = Qf * x_diff;
            l_xx = Qf;
        end
    end
end

振り子に関する情報：私のシステムの起源は、振り子が地面に固定されている場所です。角度シータは、安定した位置ではゼロです（不安定な/目標の位置ではpi）。 mボブの質量であり、lロッドの長さであり、d減衰係数である（簡単にするために私が入れm=1、l=1、d=0.3）

私のコストは単純です：コントロールにペナルティを課し、最終的なエラーを出します。

これがilqr関数の呼び出し方法です

tspan = [0 10];
dt = 0.01;
steps = floor(tspan(2)/dt);
x0 = [pi/4; 0];
umin = -3; umax = 3;
[x_, u_, L, J_opt ] = ilqg_det(@ilqr_fnDyn, @ilqr_fnCost, dt, steps, x0, 0, umin, umax);

これは出力です

時間0〜10。初期条件：（0.785398,0.000000）。目標：（-3.141593,0.000000）長さ：1.000000、質量：1.000000、減衰：0.300000

反復LQRコントロールの使用

反復= 5; コスト= 88230673.8003

公称軌道（つまり、コントロールが見つける最適な軌道）は

コントロールは「オフ」です...目標に到達しようとさえしません...何が間違っていますか？（トドロフのアルゴリズムは動作しているようです。少なくとも彼の例では）

すべてのコードを実行することなく（それは実際の作業に似すぎます）、私の直感は、あなたが制御努力を十分に重くして、最もコストのかからないことは何もせずにエラーに耐えることです。

はい、わかっています-明示的な重みはすべて統一されています。しかし、それでも、制御努力により小さい重みを与えるか、位置誤差に高い重みを与えてみてください。

繰り返しますが、コードに深く入らないと、ilrq関数は、制御しているものの非線形性を「理解」できません。そのため、振り子の直立位置に到達する方法が見えない場合があり、再び失敗する場合があります。

振り子に適切な量のエネルギーを投入し、振り子が直立したら最適に調整するために最初に試みたアプローチは、おそらく最良の方法です：摩擦がない場合、完全に正しいシステムエネルギーの量は（ただし簡単に言えば）上にとどまることになりますので、そこから始めるのが賢明だと思われます。

iLQRは反復的な方法ですが、実際には反復しているようには見えません。Todorovは、システムをカスタマイズする必要があるかもしれませんが、アプローチを説明するテストスクリプトを提供します。