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], u
adalah 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).
m
adalah massa bob, l
adalah panjang batang, d
merupakan 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)