Optymalna kontrola dla prostego wahadła

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.

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

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)

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.

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.