Оптимальний контроль для простого маятника

Я вивчаю різні оптимальні методи управління (і реалізую їх у Matlab), і в якості тестового випадку вибираю (поки що) простий маятник (закріплений на землі), який хочу контролювати до верхнього положення.

Мені вдалося керувати ним, використовуючи «простий» метод зворотного зв’язку (розгортання, засноване на регулюванні енергії + стабілізація LQR для верхнього положення), а траєкторія стану показана на малюнку (я забув опис осі: x - тета, у - тета крапка.

Тепер я хочу спробувати "повний" оптимальний метод управління, починаючи з ітеративного методу LQR (який я знайшов, реалізованого тут http://homes.cs.washington.edu/~todorov/software/ilqg_det.m )

Метод вимагає однієї динамічної функції та однієї функції витрат ( x = [theta; theta_dot], uє крутний момент двигуна (лише один двигун)):

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 - це ітеративний метод, але на ділі ви, схоже, не повторюєте. Тодоров постачає тестовий сценарій, який повинен з'ясувати підхід, хоча він може знадобитися налаштувати для вашої системи.