Kontrol optimal untuk bandul sederhana

Saya sedang mempelajari berbagai metode kontrol yang optimal (dan mengimplementasikannya dalam Matlab), dan sebagai test case saya memilih (untuk saat ini) pendulum sederhana (terpasang di tanah), yang ingin saya kontrol ke posisi atas.

Saya berhasil mengendalikannya menggunakan metode umpan balik "sederhana" (ayunan berdasarkan kontrol energi + stabilisasi LQR untuk posisi atas), dan lintasan keadaan ditampilkan dalam gambar (saya lupa deskripsi sumbu: x adalah theta, y adalah theta dot.

Sekarang saya ingin mencoba metode kontrol optimal "penuh", dimulai dengan metode LQR berulang (yang saya temukan diimplementasikan di sini http://homes.cs.washington.edu/~todorov/software/ilqg_det.m )

Metode ini memerlukan satu fungsi dinamis dan satu fungsi biaya ( x = [theta; theta_dot], uadalah torsi motor (satu motor saja)):

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

Beberapa info tentang bandul: asal sistem saya adalah tempat bandul itu dipasang ke tanah. Sudut theta adalah nol di posisi stabil (dan pi di posisi tidak stabil / tujuan). madalah massa bob, ladalah panjang batang, dmerupakan faktor redaman (untuk kesederhanaan saya menempatkan m=1, l=1, d=0.3)

Biaya saya sederhana: menghukum kontrol + kesalahan terakhir.

Ini adalah bagaimana saya memanggil fungsi 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);

Ini adalah output

Waktu Dari 0 hingga 10. Ketentuan awal: (0.785398,0.000000). Sasaran: (-3.141593,0.000000) Panjang: 1,000000, massa: 1,000000, redaman: 0,300000

Menggunakan kontrol LQR Iteratif

Iterasi = 5; Biaya = 88230673.8003

lintasan nominal (yaitu lintasan optimal yang ditemukan kontrol) adalah

Kontrolnya "mati" ... bahkan tidak berusaha mencapai tujuan ... Apa yang saya lakukan salah? (Algoritme dari Todorov tampaknya berfungsi .. setidaknya dengan contohnya)

Tanpa melalui semua kode Anda (itu akan menjadi seperti pekerjaan nyata), firasat saya adalah bahwa Anda telah cukup membebani upaya kontrol Anda sehingga hal yang paling murah untuk dilakukan adalah tidak melakukan apa-apa dan hidup dengan kesalahan.

Ya, saya tahu - semua bobot eksplisit Anda adalah satu. Tapi tetap saja - cobalah memberikan upaya kontrol bobot yang lebih rendah, atau kesalahan posisi yang lebih tinggi.

Sekali lagi tanpa masuk ke dalam kode Anda, fungsi ilrq Anda mungkin tidak "memahami" sifat nonlinier dari hal yang Anda kendalikan. Karena itu, ia mungkin tidak melihat cara untuk mencapai posisi tegak lurus, dan sekali lagi, mungkin gagal.

Pendekatan yang pertama kali Anda coba, untuk memasukkan jumlah energi yang tepat ke dalam bandul, kemudian mengatur secara optimal begitu bandul itu didirikan, mungkin merupakan cara terbaik: Anda tahu bahwa tanpa adanya gesekan, sebuah sistem dengan hak yang sempurna jumlah energi akan berakhir berdiri diam di atas (betapapun singkatnya), sehingga akan tampak tempat yang masuk akal untuk memulai.

iLQR adalah metode berulang tetapi Anda sebenarnya tidak tampak iterasi. Todorov menyediakan skrip uji yang harus menjelaskan pendekatan tersebut meskipun mungkin perlu disesuaikan untuk sistem Anda.