Optimale Kontrolle für ein einfaches Pendel

Ich studiere verschiedene optimale Steuerungsmethoden (und implementiere sie in Matlab) und wähle als Testfall (vorerst) ein einfaches Pendel (am Boden befestigt), das ich in die obere Position steuern möchte.

Ich habe es geschafft, es mit einer "einfachen" Rückkopplungsmethode zu steuern (Hochschwingen basierend auf Energiesteuerung + LQR-Stabilisierung für die obere Position), und die Zustandsbahn ist in der Abbildung dargestellt (ich habe die Achsenbeschreibung vergessen: x ist Theta, y ist Theta Punkt.

Jetzt möchte ich eine "vollständige" Methode zur optimalen Steuerung ausprobieren, beginnend mit einer iterativen LQR-Methode (die ich hier implementiert gefunden habe: http://homes.cs.washington.edu/~todorov/software/ilqg_det.m ).

Die Methode erfordert eine dynamische Funktion und eine Kostenfunktion ( x = [theta; theta_dot], uist das Motordrehmoment (nur ein Motor)):

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

Einige Infos zum Pendel: Der Ursprung meines Systems ist dort, wo das Pendel am Boden befestigt ist. Der Winkel Theta ist in der stabilen Position Null (und pi in der instabilen / Zielposition). mMasse ist die bob, lwird die Stablänge, dwird ein Dämpfungsfaktor (zur Vereinfachung I setzen m=1, l=1, d=0.3)

Meine Kosten sind einfach: Bestrafen Sie die Kontrolle + den endgültigen Fehler.

So rufe ich die ilqr-Funktion auf

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

Dies ist die Ausgabe

Zeit von 0 bis 10. Anfangsbedingungen: (0,785398,0.000000). Ziel: (-3,141593,0,000000) Länge: 1,000000, Masse: 1,000000, Dämpfung: 0,300000

Verwenden der iterativen LQR-Steuerung

Iterationen = 5; Kosten = 88230673.8003

Die nominale Flugbahn (das ist die optimale Flugbahn, die die Steuerung findet) ist

Die Steuerung ist "aus" ... sie versucht nicht einmal, das Ziel zu erreichen ... Was mache ich falsch? (Der Algorithmus von Todorov scheint zu funktionieren. Zumindest mit seinen Beispielen.)

Ohne Ihren gesamten Code durchzugehen (das wäre zu viel für echte Arbeit), habe ich das Gefühl, dass Sie Ihren Kontrollaufwand so stark gewichtet haben, dass es am kostengünstigsten ist, nichts zu tun und mit dem Fehler umzugehen.

Ja, ich weiß - alle Ihre expliziten Gewichte sind Einheit. Versuchen Sie dennoch, dem Steuerungsaufwand ein geringeres oder dem Positionsfehler ein höheres Gewicht zu geben.

Auch hier kann es vorkommen, dass Ihre ilrq-Funktion die nichtlineare Natur der von Ihnen gesteuerten Objekte nicht "versteht", ohne sich eingehend mit Ihrem Code zu befassen. Als solches kann es sein, dass es keinen Weg sieht, um in die aufrechte Pendelposition zu gelangen, und es kann erneut fehlschlagen.

Der Ansatz, den Sie zuerst versucht haben, um genau die richtige Energiemenge in das Pendel zu stecken und dann, sobald das Pendel aufgerichtet ist, optimal zu regulieren, ist wahrscheinlich der beste Weg: Sie wissen, dass ein System ohne Reibung genau das Richtige ist Die Energiemenge wird (jedoch kurzzeitig) stillstehen, so dass dies ein vernünftiger Ausgangspunkt zu sein scheint.

iLQR ist eine iterative Methode, aber Sie scheinen nicht zu iterieren. Todorov liefert ein Testskript , das den Ansatz verdeutlicht, obwohl es möglicherweise für Ihr System angepasst werden muss.