Controle ideal para um pêndulo simples

Estou estudando vários métodos ótimos de controle (e implementando-os no Matlab) e, como caso de teste, escolho (por enquanto) um pêndulo simples (fixo ao solo), que desejo controlar na posição superior.

Consegui controlá-lo usando o método de feedback "simples" (oscilação baseada no controle de energia + estabilização LQR para a posição superior), e a trajetória do estado é mostrada na figura (esqueci a descrição do eixo: x é theta, y é theta ponto.

Agora, quero tentar um método de controle ideal "completo", começando com um método LQR iterativo (que eu encontrei implementado aqui http://homes.cs.washington.edu/~todorov/software/ilqg_det.m )

O método requer uma função dinâmica e uma função de custo ( x = [theta; theta_dot], ué o torque do motor (apenas um 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

Algumas informações sobre o pêndulo: a origem do meu sistema é onde o pêndulo é fixado no chão. O ângulo teta é zero na posição estável (e pi na posição instável / objetivo). mé a massa do prumo, lé o comprimento da haste, dé um factor de amortecimento (para simplicidade Pus m=1, l=1, d=0.3)

Meu custo é simples: penalize o controle + o erro final.

É assim que eu chamo a função 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 é a saída

Tempo De 0 a 10. Condições iniciais: (0.785398,0.000000). Objetivo: (-3.141593,0.000000) Comprimento: 1.000000, massa: 1.000000, amortecimento: 0.300000

Usando o controle LQR iterativo

Iterações = 5; Custo = 88230673.8003

a trajetória nominal (que é a trajetória ideal que o controle encontra) é

O controle está "desligado" ... nem tenta alcançar a meta ... O que estou fazendo de errado? (o algoritmo de Todorov parece funcionar .. pelo menos com seus exemplos)

Sem passar por todo o seu código (isso seria muito parecido com um trabalho real), minha intuição é que você ponderou seu esforço de controle com força suficiente para que a coisa de menor custo a fazer é não fazer nada e viver com o erro.

Sim, eu sei - todos os seus pesos explícitos são unidade. Mas ainda assim - tente atribuir um menor peso ao esforço de controle ou um erro de posição maior.

Novamente, sem se aprofundar no seu código, sua função ilrq pode não "entender" a natureza não-linear da coisa que você está controlando. Como tal, pode não haver uma maneira de chegar à posição vertical do pêndulo e, novamente, pode falhar.

A abordagem que você tentou pela primeira vez, colocar a quantidade certa de energia no pêndulo e depois regular de maneira ideal quando o pêndulo estiver ereto, é provavelmente a melhor maneira: você sabe que, na ausência de atrito, um sistema com o perfeito perfeito quantidade de energia vai acabar parada no topo (ainda que brevemente), de modo que parece um bom lugar para começar.

O iLQR é um método iterativo, mas na verdade você não parece estar iterando. Todorov fornece um script de teste que deve elucidar a abordagem, embora possa precisar ser personalizado para o seu sistema.