Controllo ottimale per un pendolo semplice

Sto studiando vari metodi di controllo ottimali (e li implementa in Matlab), e come caso di test scelgo (per ora) un semplice pendolo (fissato a terra), che voglio controllare nella posizione superiore.

Sono riuscito a controllarlo usando un metodo di feedback "semplice" (oscillazione basato sul controllo dell'energia + stabilizzazione LQR per la posizione superiore), e la traiettoria di stato è mostrata in figura (ho dimenticato la descrizione dell'asse: x è theta, y è theta punto.

Ora voglio provare un metodo di controllo ottimale "completo", iniziando con un metodo LQR iterativo (che ho trovato implementato qui http://homes.cs.washington.edu/~todorov/software/ilqg_det.m )

Il metodo richiede una funzione dinamica e una funzione di costo ( x = [theta; theta_dot], uè la coppia del motore (solo un motore)):

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

Alcune informazioni sul pendolo: l'origine del mio sistema è dove il pendolo è fissato al suolo. L'angolo theta è zero nella posizione stabile (e pi nella posizione instabile / goal). mè la massa bob, lè la lunghezza dell'asta, dè un fattore di smorzamento (per semplicità metto m=1, l=1, d=0.3)

Il mio costo è semplice: penalizzare il controllo + l'errore finale.

Ecco come chiamo la funzione 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);

Questo è l'output

Tempo da 0 a 10. Condizioni iniziali: (0.785398,0.000000). Obiettivo: (-3.141593,0,000000) Lunghezza: 1,000000, massa: 1,000000, smorzamento: 0,300000

Usando il controllo LQR iterativo

Iterazioni = 5; Costo = 88230673.8003

la traiettoria nominale (che è la traiettoria ottimale trovata dal controllo) è

Il controllo è "spento" ... non prova nemmeno a raggiungere l'obiettivo ... Cosa sto facendo di sbagliato? (l'algoritmo di Todorov sembra funzionare .. almeno con i suoi esempi)

Senza esaminare tutto il codice (sarebbe troppo simile al vero lavoro), la mia sensazione è che tu abbia ponderato il tuo sforzo di controllo abbastanza pesantemente che la cosa più economica da fare è non fare nulla e vivere con l'errore.

Sì, lo so - tutti i tuoi pesi espliciti sono unità. Tuttavia, prova a dare un peso inferiore allo sforzo di controllo, oppure l'errore di posizione maggiore.

Ancora una volta senza approfondire il tuo codice, la tua funzione ilrq potrebbe non "capire" la natura non lineare della cosa che stai controllando. Come tale, potrebbe non vedere un modo per raggiungere la posizione verticale del pendolo e, di nuovo, potrebbe non riuscire.

L'approccio che hai provato per primo, di mettere la giusta quantità di energia nel pendolo, quindi di regolare in modo ottimale una volta che il pendolo è eretto, è probabilmente il modo migliore: sai che in assenza di attrito, un sistema con la giusta ragione la quantità di energia finirà per rimanere ferma in cima (anche se brevemente), quindi sembrerebbe un punto ragionevole per iniziare.

iLQR è un metodo iterativo ma in realtà non sembra essere iterativo. Todorov fornisce uno script di test che dovrebbe chiarire l'approccio sebbene potrebbe essere necessario personalizzarlo per il proprio sistema.