Optymalna kontrola dla prostego wahadła

15

Studiuję różne optymalne metody sterowania (i implementuję je w Matlabie), a jako przypadek testowy wybieram (na razie) prosty wahadło (przymocowane do podłoża), które chcę kontrolować do górnej pozycji.

Udało mi się to kontrolować za pomocą „prostej” metody sprzężenia zwrotnego (odchylenie w oparciu o kontrolę energii + stabilizacja LQR dla górnej pozycji), a trajektoria stanu pokazana jest na rysunku (zapomniałem opisu osi: x to theta, y to theta kropka.

Swing-up + trajektoria stanu kontroli LQR

Teraz chcę wypróbować „pełną” optymalną metodę sterowania, zaczynając od iteracyjnej metody LQR (którą znalazłem zaimplementowaną tutaj http://homes.cs.washington.edu/~todorov/software/ilqg_det.m )

Metoda wymaga jednej funkcji dynamicznej i jednej funkcji kosztu ( x = [theta; theta_dot], ujest momentem obrotowym silnika (tylko jeden silnik)):

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

Kilka informacji na temat wahadła: początek mojego systemu polega na tym, że wahadło jest przymocowane do podłoża. Kąt theta wynosi zero w pozycji stabilnej (a pi w pozycji niestabilnej / bramkowej). mmasa BOB, lma długość pręta, djest współczynnik tłumienia (dla uproszczenia umieścić m=1, l=1, d=0.3)

Mój koszt jest prosty: ukarać kontrolę + błąd końcowy.

Tak nazywam funkcję 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);

To jest wynik

Czas od 0 do 10. Warunki początkowe: (0.785398,0.000000). Cel: (-3,141593,0,000000) Długość: 1,000000, masa: 1,000000, tłumienie: 0,300000

Korzystanie z iteracyjnej kontroli LQR

Iteracje = 5; Koszt = 88230673.8003

trajektoria nominalna (czyli trajektoria optymalna znaleziona przez kontrolę) wynosi

Optymalna trajektoria ILQR

Kontrola jest „wyłączona” ... nawet nie próbuje osiągnąć celu ... Co robię źle? (wydaje się, że algorytm z Todorowa działa ... przynajmniej z jego przykładami)

Francesco
źródło

Odpowiedzi:

2

Bez przechodzenia przez cały kod (byłoby to zbyt wiele jak prawdziwa praca), mam wrażenie, że wyważyłeś swój wysiłek kontrolny na tyle mocno, że najmniejszym kosztem jest nic nie robić i żyć z błędem.

Tak, wiem - wszystkie twoje wyraźne ciężary to jedność. Ale nadal - spróbuj nadać wysiłkowi kontrolnemu mniejszą wagę lub błąd pozycji wyższy.

Ponownie, bez zagłębiania się w twój kod, twoja funkcja ilrq może nie „zrozumieć” nieliniowej natury rzeczy, którą kontrolujesz. W związku z tym może nie znaleźć sposobu na osiągnięcie pozycji wahadła w pozycji pionowej, i ponownie może się nie powieść.

Podejście, które najpierw próbowałeś, aby wprowadzić odpowiednią ilość energii do wahadła, a następnie optymalnie wyregulować, gdy wahadło zostanie wyprostowane, jest prawdopodobnie najlepszym sposobem: wiesz, że przy braku tarcia, system z idealnie idealnie ilość energii skończy się, stojąc nieruchomo na górze (choć krótko), więc wydaje się to rozsądnym miejscem do rozpoczęcia.

TimWescott
źródło
Dziękuję za Twój komentarz. Jak powiedziałem, komentując drugą odpowiedź, to pytanie jest dość stare i może powinienem je usunąć. Problem polega na tym, że nigdy go nie rozwiązałem, nawet ponieważ przeniosłem się na inne algorytmy. Jeśli chodzi o twój komentarz na temat energii. Prawdziwym celem nie jest kontrolowanie odwróconego wahadła, ale wykorzystanie go jako stanowiska testowego dla algorytmów ocp. (system niskiego wymiaru, ale nieliniowy i niestabilny)
Francesco
1

iLQR to metoda iteracyjna, ale tak naprawdę nie wydaje się, aby była iteracyjna. Todorov dostarcza skrypt testowy, który powinien wyjaśnić podejście, choć może wymagać dostosowania do twojego systemu.

DaemonMaker
źródło
Pierwszą rzeczą, którą wypróbowałem, kiedy wdrożyłem metodę iLQG, jest test Todorowa i zadziałał. Teraz .. to pytanie jest z końca stycznia .. może powinienem je zamknąć .. przeszedłem z tej metody i z Matlaba na NLP
Francesco
Przepraszam, że nie widziałem tego wcześniej. Re: zamknięcie go, odradzam to, ponieważ inni mogą nadal uważać to za pomocne.
DaemonMaker
1
@DeamonMaker tak .. to jest powód, dla którego zostawiłem to otwarte ... :)
Francesco