Basit bir sarkaç için Optimal Kontrol

Çeşitli optimal kontrol yöntemlerini inceliyorum (ve bunları Matlab'da uygular) ve test durumu olarak (şimdilik) üst konuma kontrol etmek istediğim basit bir sarkaç (yere sabit) seçiyorum.

Bunu "basit" geri besleme yöntemi (enerji kontrolü + LQR stabilizasyonu üst pozisyon için dayalı) ile kontrol etmeyi başardım ve durum yörüngesi şekilde gösterilir (eksen açıklamasını unuttum: x teta, y teta nokta.

Şimdi, yinelenen bir LQR yöntemiyle (burada uygulandığını bulduğum http://homes.cs.washington.edu/~todorov/software/ilqg_det.m ) bir "tam" optimal kontrol yöntemi denemek istiyorum

Yöntem bir dinamik fonksiyon ve bir maliyet fonksiyonu gerektirir ( x = [theta; theta_dot], umotor torku (sadece bir 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

Sarkaç hakkında bazı bilgiler: Sistemimin kökeni, sarkaçın yere sabitlendiği yerdir. Teta açısı sabit konumda sıfırdır (ve kararsız / hedef konumunda pi). mbob kütlesi olan lçubuk uzunluğu, bir dsönümleme faktörü olduğu (basitlik açısından bir koyun m=1, l=1, d=0.3)

Maliyetim basit: kontrolü + son hatayı cezalandır.

Ben ilqr fonksiyonunu böyle çağırıyorum

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);

Bu çıktı

Süre 0 ila 10 arası. Başlangıç ​​koşulları: (0.785398,0.000000). Hedef: (-3.141593,0.000000) Uzunluk: 1.000000, kütle: 1.000000, sönümleme: 0.300000

Yinelemeli LQR kontrolünü kullanma

Yinelemeler = 5; Maliyet = 88230673.8003

nominal yörünge (kontrolün bulduğu en uygun yörünge)

Kontrol "kapalı" ... hedefe ulaşmaya bile çalışmıyor ... Neyi yanlış yapıyorum? (Todorov'un algoritması işe yarıyor gibi görünüyor ... en azından onun örnekleri ile)

Tüm kodunuzdan geçmeden (gerçek iş gibi çok fazla olurdu), bağırsak hissim, kontrol çabanızı yapmak için en az maliyetli şeyin hiçbir şey yapmamak ve hatayla yaşamak kadar ağır bir şekilde ağırlıklandırmanızdır.

Evet, biliyorum - tüm açık ağırlıklarınız birliktir. Ancak yine de - kontrol çabasına daha düşük bir ağırlık veya konum hatası daha yüksek bir ağırlık vermeye çalışın.

Yine kodunuza derinlemesine girmeden, ilrq fonksiyonunuz kontrol ettiğiniz şeyin doğrusal olmayan doğasını "anlamayabilir". Bu nedenle, sarkaçın dik pozisyonuna ulaşmanın bir yolunu görmeyebilir ve yine başarısız olabilir.

Sarkaç içine doğru miktarda enerji koymak için ilk önce denediğiniz yaklaşım, sonra sarkaç kurulduktan sonra en iyi şekilde düzenler, muhtemelen en iyi yoldur: sürtünmenin yokluğunda, sadece mükemmel bir şekilde doğru olan bir sistemin enerji miktarı hala tepede duracak (ancak kısaca), bu yüzden başlamak için mantıklı bir yer gibi görünecektir.

iLQR yinelemeli bir yöntemdir, ancak aslında yineliyormuş gibi görünmüyorsunuz. Todorov , sisteminiz için özelleştirilmesi gerekebilse de yaklaşımı aydınlatması gereken bir test komut dosyası sağlar.