Ich studiere verschiedene optimale Steuerungsmethoden (und implementiere sie in Matlab) und wähle als Testfall (vorerst) ein einfaches Pendel (am Boden befestigt), das ich in die obere Position steuern möchte.
Ich habe es geschafft, es mit einer "einfachen" Rückkopplungsmethode zu steuern (Hochschwingen basierend auf Energiesteuerung + LQR-Stabilisierung für die obere Position), und die Zustandsbahn ist in der Abbildung dargestellt (ich habe die Achsenbeschreibung vergessen: x ist Theta, y ist Theta Punkt.
Jetzt möchte ich eine "vollständige" Methode zur optimalen Steuerung ausprobieren, beginnend mit einer iterativen LQR-Methode (die ich hier implementiert gefunden habe: http://homes.cs.washington.edu/~todorov/software/ilqg_det.m ).
Die Methode erfordert eine dynamische Funktion und eine Kostenfunktion ( x = [theta; theta_dot], u
ist das Motordrehmoment (nur ein 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
Einige Infos zum Pendel: Der Ursprung meines Systems ist dort, wo das Pendel am Boden befestigt ist. Der Winkel Theta ist in der stabilen Position Null (und pi in der instabilen / Zielposition).
m
Masse ist die bob, l
wird die Stablänge, d
wird ein Dämpfungsfaktor (zur Vereinfachung I setzen m=1
, l=1
, d=0.3
)
Meine Kosten sind einfach: Bestrafen Sie die Kontrolle + den endgültigen Fehler.
So rufe ich die ilqr-Funktion auf
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);
Dies ist die Ausgabe
Zeit von 0 bis 10. Anfangsbedingungen: (0,785398,0.000000). Ziel: (-3,141593,0,000000) Länge: 1,000000, Masse: 1,000000, Dämpfung: 0,300000
Verwenden der iterativen LQR-Steuerung
Iterationen = 5; Kosten = 88230673.8003
Die nominale Flugbahn (das ist die optimale Flugbahn, die die Steuerung findet) ist
Die Steuerung ist "aus" ... sie versucht nicht einmal, das Ziel zu erreichen ... Was mache ich falsch? (Der Algorithmus von Todorov scheint zu funktionieren. Zumindest mit seinen Beispielen.)