Control óptimo para un péndulo simple

Estoy estudiando varios métodos de control óptimos (y los implementa en Matlab), y como caso de prueba elijo (por ahora) un péndulo simple (fijado al suelo), que quiero controlar en la posición superior.

Logré controlarlo usando un método de retroalimentación "simple" (basculante basado en el control de energía + estabilización LQR para la posición superior), y la trayectoria del estado se muestra en la figura (olvidé la descripción del eje: x es theta, y es theta punto.

Ahora quiero probar un método de control óptimo "completo", comenzando con un método iterativo LQR (que encontré implementado aquí http://homes.cs.washington.edu/~todorov/software/ilqg_det.m )

El método requiere una función dinámica y una función de costo ( x = [theta; theta_dot], ues el par motor (solo un motor)):

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

Alguna información sobre el péndulo: el origen de mi sistema es donde el péndulo se fija al suelo. El ángulo theta es cero en la posición estable (y pi en la posición inestable / objetivo). mes la masa de la sacudida, les la longitud de la varilla, des un factor de amortiguación (por simplicidad puse m=1, l=1, d=0.3)

Mi costo es simple: penalizar el control + el error final.

Así es como llamo a la función 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);

Esta es la salida

Tiempo de 0 a 10. Condiciones iniciales: (0.785398,0.000000). Objetivo: (-3.141593,0.000000) Longitud: 1.000000, masa: 1.000000, amortiguación: 0.300000

Usando el control iterativo LQR

Iteraciones = 5; Costo = 88230673.8003

la trayectoria nominal (que es la trayectoria óptima que encuentra el control) es

El control está "apagado" ... ni siquiera intenta alcanzar la meta ... ¿Qué estoy haciendo mal? (el algoritmo de Todorov parece funcionar ... al menos con sus ejemplos)

Sin pasar por todo su código (eso sería demasiado como un trabajo real), mi intuición es que ha sopesado su esfuerzo de control lo suficiente como para que lo más económico sea no hacer nada y vivir con el error.

Sí, lo sé, todos sus pesos explícitos son la unidad. Pero aún así, intente darle al esfuerzo de control un peso menor o el error de posición uno más alto.

Nuevamente, sin entrar profundamente en su código, su función ilrq puede no "comprender" la naturaleza no lineal de lo que está controlando. Como tal, es posible que no vea una manera de llegar a la posición vertical del péndulo, y nuevamente, puede fallar.

El enfoque que probó por primera vez, poner la cantidad justa de energía en el péndulo, y luego regularlo de manera óptima una vez que el péndulo está erecto, es probablemente la mejor manera: usted sabe que, en ausencia de fricción, un sistema con la perfección perfecta la cantidad de energía terminará parada en la parte superior (aunque sea brevemente), por lo que parecería un buen lugar para comenzar.

iLQR es un método iterativo pero, de hecho, no parece estar iterando. Todorov proporciona un script de prueba que debe dilucidar el enfoque, aunque puede ser necesario personalizarlo para su sistema.