Contrôle optimal pour un pendule simple

J'étudie différentes méthodes de contrôle optimales (et les implémente dans Matlab), et comme cas de test, je choisis (pour l'instant) un simple pendule (fixé au sol), que je veux contrôler en position haute.

J'ai réussi à le contrôler en utilisant une méthode de rétroaction "simple" (basculement basé sur le contrôle de l'énergie + stabilisation LQR pour la position supérieure), et la trajectoire de l'état est montrée sur la figure (j'ai oublié la description de l'axe: x est thêta, y est thêta point.

Maintenant, je veux essayer une méthode de contrôle optimale "complète", en commençant par une méthode LQR itérative (que j'ai trouvée implémentée ici http://homes.cs.washington.edu/~todorov/software/ilqg_det.m )

La méthode nécessite une fonction dynamique et une fonction de coût ( x = [theta; theta_dot], uc'est le couple moteur (un moteur uniquement)):

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

Quelques informations sur le pendule: l'origine de mon système est l'endroit où le pendule est fixé au sol. L'angle thêta est nul en position stable (et pi en position instable / but). mest la masse de plomb, lest la longueur de la tige, dest un facteur d' amortissement (pour plus de simplicité je mets m=1, l=1, d=0.3)

Mon coût est simple: pénaliser le contrôle + l'erreur finale.

Voici comment j'appelle la fonction 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);

Ceci est la sortie

Temps De 0 à 10. Conditions initiales: (0.785398,0.000000). Objectif: (-3.141593,0.000000) Longueur: 1.000000, masse: 1.000000, amortissement: 0.300000

Utilisation du contrôle LQR itératif

Itérations = 5; Coût = 88230673.8003

la trajectoire nominale (c'est-à-dire la trajectoire optimale trouvée par le contrôle) est

Le contrôle est "désactivé" ... il n'essaie même pas d'atteindre le but ... Qu'est-ce que je fais mal? (l'algorithme de Todorov semble fonctionner .. au moins avec ses exemples)

Sans passer par tout votre code (ce serait trop comme du vrai travail), mon intuition est que vous avez suffisamment pondéré votre effort de contrôle pour que la chose la moins coûteuse à faire soit de ne rien faire et de vivre avec l'erreur.

Oui, je sais - tous vos poids explicites sont l'unité. Mais tout de même - essayez de donner à l'effort de contrôle un poids plus faible ou l'erreur de position plus élevée.

Encore une fois sans entrer profondément dans votre code, votre fonction ilrq peut ne pas "comprendre" la nature non linéaire de la chose que vous contrôlez. En tant que tel, il peut ne pas voir un moyen d'atteindre la position verticale du pendule, et encore une fois, il peut échouer.

L'approche que vous avez d'abord essayée, de mettre juste la bonne quantité d'énergie dans le pendule, puis de régler de manière optimale une fois le pendule en érection, est probablement la meilleure façon: vous savez qu'en l'absence de frottement, un système avec juste ce qu'il faut parfaitement la quantité d'énergie va finir par rester immobile au sommet (même brièvement), ce qui semble un bon point de départ.

iLQR est une méthode itérative mais vous ne semblez pas en fait itérer. Todorov fournit un script de test qui devrait clarifier l'approche, mais il peut être nécessaire de le personnaliser pour votre système.