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

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

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

Теперь я хочу попробовать «полный» метод оптимального управления, начиная с итеративного метода 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

Немного информации о маятнике: источник моей системы находится там, где маятник прикреплен к земле. Угол тета равен нулю в устойчивом положении (и пи в нестабильном положении / положении цели). 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 - это итеративный метод, но на самом деле вы, кажется, не делаете итерацию. Тодоров предоставляет тестовый скрипт, который должен прояснить подход, хотя, возможно, его необходимо настроить для вашей системы.