3.3. Xây dựng bộ điều khiển Swing_Up và cân bằng cho hệ Acrobot
3.3.1 Mô phỏng bộ điều khiển Swing-up và giữ cân bằng LQR
3.3.1.2. Mô phỏng bộ điều khiển Swing-up sử dụng phương pháp điều khiển hồi tiếp tuến tính hóa riêng phần và giữ cân bằng LQR
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5
-50 -40 -30 -20 -10 0 10 20 30 40 50
THỜI GIAN (s)
BIÊN ĐỘ (Nm)
TÍN HIEÄU ẹIEÀU KHIEÅN HEÄ ACROBOT
FUZZY & LQR CONTROL LAW
Hình 3.12: Mô hình mô phỏng Simulink bộ điều khiển ACROBOT
Hình 3.13: Của sổ mô phỏng hoạt họa của hệ Acrobot (Acrobot Animation)
*Đoạn code mô phỏng hoạt họa của hệ Acrobot:
function [sys,x0,str,ts,simStateCompliance]=acrobot(t,x,u,flag,ts) global Acrobot
if flag==2,
if any(get(0,'Children')==Acrobot),
if strcmp(get(Acrobot,'Name'),'ACROBOT ANIMATION'), set(0,'currentfigure',Acrobot);
temp=get(gca,'UserData');
hndl = temp(1);
hndl2 = temp(2);
b=4; c=4;
%xSFP=0; ySFP=0;
xSFP=0; ySFP=0;
xBFP=xSFP+b*sin(-u(1)); yBFP=ySFP+b*cos(-u(1));
xMC=xBFP+c*sin(-u(2)-u(1)); yMC=yBFP+c*cos(-u(2)-u(1));
x1=[xSFP xBFP];
y1=[ySFP yBFP];
x2=[xBFP xMC];
y2=[yBFP yMC];
set(hndl,'XData',x1,'YData',y1);
set(hndl2,'XData',x2,'YData',y2);
drawnow;
end end sys=[];
elseif flag==0,
% Initialize the figure for use with this simulation Acrobot = animinit('ACROBOT ANIMATION');
if ishghandle(Acrobot)
% bring figure to foreground figure(Acrobot);
end
axis([-12 12 -12 12]);
hold on;
% Set up the geometry for the problem % SFP=Space Fixed Pivot
% BFP=Body Fixed Pivot b=2; c=4;
xSFP=0; ySFP=0;
xBFP=xSFP; yBFP=ySFP-b;
xMC=xBFP; yMC=yBFP-c;
x1=[xSFP xBFP];
y1=[ySFP yBFP];
x2=[xBFP xMC];
y2=[yBFP yMC];
hndl=plot(x1,y1,'b', ...
'LineWidth',3, ...
'Marker','.', ...
'MarkerSize',20);
hndl2=plot(x2,y2,'r', ...
'LineWidth',3, ...
'Marker','.', ...
'MarkerSize',20);
set(gca,'DataAspectRatio',[1 1 1]);
set(gca,'UserData',[hndl hndl2]);
sys = [0 0 0 2 0 0 1];
x0 = [];
str = [];
ts = [-1, 0];
% specify that the simState for this s-function is same as the default simStateCompliance = 'DefaultSimState';
end
*Kết quả mô phỏng với điều kiện đầu (x1, x2, x3, x4)T (pi,0,pi/4,0)T :
Đáp ứng vị trí góc của hai thanh q1 và q2:
*Đáp ứng vị trí góc, vận tốc, điện áp điều khiển của hệ khi áp dụng bộ điều khiển hồi tiếp tuyến tính hóa riêng phần để Swing-up và LQR để cân bằng.
Hình 3.14: Đáp ứng vị trí góc của hệ khi áp dụng bộ điều khiển hồi tiếp tuyến tính hóa riêng phần để Swing-up và LQR để cân bằng.
Hình 3.15: Đáp ứng vận tốc góc của hệ khi áp dụng bộ điều khiển hồi tiếp tuyến tính hóa riêng phần để Swing-up và LQR để giữ cân bằng.
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5
-2 0 2 4
THỜI GIAN (s)
BIEÂN ẹOÄ (rad) q1
q1(x1)
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5
0 5 10
THỜI GIAN (s)
BIEÂN ẹOÄ (rad) q2
q2(x2)
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5
-20 -10 0 10 20
THỜI GIAN (s) BIÊN ĐỘ (rad/s) q1__dot
q1__dot(x3)
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5
-40 -20 0 20 40
THỜI GIAN (s) BIÊN ĐỘ (rad/s) q2__dot
q2__dot(x4)
Hình 3.16: Ngõ ra của bộ điều khiển.
*Kết quả mô phỏng với điều kiện đầu (x1, x2, x3, x4)T (pi,0,pi/4,0)T : Đáp ứng vị trí góc của hai thanh q1 và q2:
Hình 3.17: Đáp ứng vị trí góc của hệ khi áp dụng bộ điều khiển hồi tiếp tuyến tính hóa riêng phần để Swing-up và LQR để giữ cân bằng.
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5
-50 -40 -30 -20 -10 0 10 20 30 40 50
THỜI GIAN (s)
BIÊN ĐỘ (Nm)
TÍN HIEÄU ẹIEÀU KHIEÅN HEÄ ACROBOT
PFL & LQR CONTROL LAW
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5
-2 0 2 4
THỜI GIAN (s)
BIEÂN ẹOÄ (rad) q1
q1(x1)
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5
-5 0 5 10
THỜI GIAN (s)
BIEÂN ẹOÄ (rad) q2
q2(x2)
Hình 3.18: Đáp ứng vận tốc góc của hệ khi áp dụng bộ điều khiển hồi tiếp tuyến tính hóa riêng phần để Swing-up và LQR để giữ cân bằng.
Hình 3.19: Ngõ ra của bộ điều khiển.
*Kết quả mô phỏng với điều kiện đầu (x1, x2, x3, x4)T (pi,0,0,0)T :
Đáp ứng vị trí góc của hai thanh q1 và q2:
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5
-20 -10 0 10 20
THỜI GIAN (s) BIEÂN ẹOÄ (rad/s) q1__dot
q1__dot(x3)
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5
-40 -20 0 20 40
THỜI GIAN (s) BIEÂN ẹOÄ (rad/s) q2__dot
q2__dot(x4)
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5
-50 -40 -30 -20 -10 0 10 20 30 40 50
THỜI GIAN (s)
BIEÂN ẹOÄ (Nm)
TÍN HIEÄU ẹIEÀU KHIEÅN HEÄ ACROBOT
PFL & LQR CONTROL LAW
Hình 3.20: Đáp ứng vị trí góc của hệ khi áp dụng bộ điều khiển hồi tiếp tuyến tính hóa để Swing-up và LQR để giữ cân bằng hệ thống.
Hình 3.21: Đáp ứng vận tốc góc của hệ khi áp dụng bộ điều khiển hồi tiếp tuyến tính hóa riêng phần để Swing-up và LQR để giữ cân bằng.
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5
-2 0 2 4
THỜI GIAN (s)
BIEÂN ẹOÄ (rad) q1
q1(x1)
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5
-5 0 5 10
THỜI GIAN (s)
BIEÂN ẹOÄ (rad) q2
q2(x2)
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5
-20 -10 0 10 20
THỜI GIAN (s) BIEÂN ẹOÄ (rad/s) q1__dot
q1__dot(x3)
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5
-40 -20 0 20 40
THỜI GIAN (s) BIEÂN ẹOÄ (rad/s) q2__dot
q2__dot(x4)
Hình 3.22: Ngõ ra của bộ điều khiển.
Nhận xét: Từ kết quả các đáp ứng mô phỏng ta thấy bộ điều khiển Swing-up và bộ điều khiển giữ cân bằng LQR đã điều khiển Swing-up và cân bằng ở vị trí cân bằng thẳng đứng hướng lên ở những điều kiện đầu khác nhau. Trong phần mô phỏng này học viên đã mô phỏng ba trường hợp tiêu biểu điều kiện đầu [x1, x2, x3, x4] =[ pi, 0, pi/4, 0] và [pi, 0, pi/2, 0] và [pi, 0, 0, 0] các kết quả thu được khá khả thi sau tối đa là 3s hệ thống đã chuyển qua vị trí cân bằng trên và đứng yên tại vị trí cân bằng đó.
Các ma trận hệ số Q và R ảnh hưởng đến chất lượng điều khiển như sau:
Với việc lựa chọn hệ số ma trận Q tương ứng với vị trí của biến lớn hơn các vị trí còn lại thì luật điều khiển LQR sẽ ưu tiên điều khiển biến đó về không nhanh hơn các biến còn lại.
Nếu lựa chọn R có tỉ lệ lớn so với các hệ số trong Q thì đồng nghĩa với việc ưu tiên giảm thiểu năng lượng điều khiển, giá trị ngõ ra điều khiển sẽ giảm xuống.
Tuy nhiên việc lựa chọn các ma trận hệ số Q,R phải phù hợp đảm bảo hệ thống nằm trong vùng gần tuyến tính thì mới đạt được mục đích điều khiển.