Contrôle optimal pour un pendule simple

15

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.

Swing-up + LQR control state trajectory

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

Trajectoire optimale ILQR

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)

Francesco
la source

Réponses:

2

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.

TimWescott
la source
Merci pour votre commentaire. Comme je l'ai dit en commentant l'autre réponse, cette question est assez ancienne et je devrais peut-être la supprimer. Le problème est que je ne l'ai jamais résolu, même parce que je suis passé à d'autres algorithmes. Concernant votre commentaire sur l'énergie .. Le vrai but n'est pas de contrôler un pendule inversé, mais de l'utiliser comme banc d'essai pour les algorithmes ocp. (système de faible dimension mais non linéaire et instable)
Francesco
1

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.

DaemonMaker
la source
La première chose que j'ai essayée lorsque j'ai implémenté la méthode iLQG est le test todorov et cela a fonctionné. Maintenant .. cette question est de fin janvier .. peut-être que je devrais la fermer .. Je suis passé de cette méthode et de matlab aux méthodes PNL
Francesco
Je suis désolé de ne pas l'avoir vu plus tôt. Re: en le fermant, je recommanderais de ne pas le faire car d'autres peuvent toujours le trouver utile.
DaemonMaker
1
@DeamonMaker ouais .. c'est la raison pour laquelle je l'ai laissé ouvert ... :)
Francesco