Tài liệu Tổng quan về acrobot

1 CHƢƠNG I: TỔNG QUAN VỀ ACROBOT Acrobot là rô bốt phẳng, hai bậc tự do nhưng chỉ có khớp thứ hai được điều khiển như hình vẽ 1. Hệ phương trình động lực học của acrobot là một hệ phi tuyến có hai bậc tự do và một đầu vào. Vấn đề đặt ra là tìm thuật toán điều khiển cho khớp thứ 2 để lắc đưa acrobot từ điểm ban đầu bất kỳ lên vị trí thẳng đứng và điều khiển duy trì nó ở trạng thái thẳng đứng cân bằng không ổn định này. Hình 1.1: Sơ đồ cấu trúc Acrobot 2 CHƢƠNG II MÔ HÌNH HÓA HỆ THỐNG ACROBOT 2.1 Hệ thống Acrobot Sơ đồ cấu trúc của acrobot [3] được thể hiện như hình vẽ 2.1 Than h2 g l c q 2 2 Than h1 l c 1 q 1 Hình 2.1 Hệ thống Acrobot Trong đó thông số cho bởi bảng sau: l1 ( m ) l2 ( m ) m1 ( kg ) m2 ( kg ) I1 ( kgm2 ) I 2 ( kgm2 ) 3 Đặt l1 l2  lc1  ; lc 2  2 2  c  m l 2  m l 2  I ; c  m l 2  I ; c  m l l ; c  m l  m l ; c  m l 1 c1 21 1 2 2 c2 2 3 2 1 c2 4 1 c1 21 5 2 c2 1 (2.1) Mô hình toán của Acrobot theo [6]: x  f  x  g  xu (2.2) Trong đó  x3    x f   4  và  f3     f4  0   0 g    g3     g4  2.2 Luật điều khiển cho acrobot Hệ thống có bốn điểm cân bằng ác định bởi: x e ij  M : x1  i , x2  j , x3  0, x4  0 i, j = 0, 1 X t tổng n ng lượng của acrobot: E ( x)  2 3 x x2 (c1  c2  2c3 cos x2 )  x3 x4 (c2  c3 cos x2 )  4 c2  c4 g cos x1  c5 g cos( x1  x2 ) (2.3) 2 2 Theo đó, n ng lượng tại vị trí cân bằng thẳng đứng theo e chiều hướng lên trên x00 là Ed = (c4 +c5)g. Đặt Ê( ) = E( )-Ed. Thế x2 = x4 = 0 vào Ê(x), phương trình Ê(x) = 0 sau đó sẽ rút gọn thành: x32 (c1  c2  2c3 )  g (c4  c5 ) cos x1  Ed  0 2 Lấy đạo hàm phương trình (3) theo th i gian ta có (2.4) 4 . x3 (c1  c2  2c3 )  g (c4  c5 )sin x1  0 (2.5) Phương trình (2.5) mô tả chuyển động của con lắc với độ dài l chịu ảnh hưởng của trọng lực , trong đó l = (c1+c2+2c3)/(c4+c5) và ĝ = - g. x  M : x2  x4  0 và Π Đặt N x  N : Ê( x)  0 . X t hàm Lyapunov V : M  R được ác định bởi V ( x)   1 k p x22  kd x42  ke Ê 2 ( x) 2  (2.6) Tại đó kp, kd và ke là nh ng hằng số dương. Chú V là hàm số bán ác định dương. Đạo hàm (2.6) theo qu đạo phương trình . . V  k p x2 x4  kd x4 x 4  ke Ê( x) x4u (2.2) ta có: .  x4 (k p x2  kd x 4  ke Ê( x)u ). (2.7) . k p x2  kd x 4  ke Ê( x)u   x4 Đặt . (2.8)  V   x4 2 (2.9) D ng phương trình (2.2) ta có (2.8) được viết lại thành k x  k ( f (x)  g (x)u)  k Ê(x)u  x . p 2 d 4 4 e Từ (2.10) rút ra luật điều khiển u như sau: u   x4  k p x2  kd f 4 ( x) kd g 4 ( x)  ke Ê( x) 4 (2.10) 5   ( x2 )  x4  k p x2  kd f 4 ( x) k (c1  c2  2c3 cos x2 )  ke ( x2 )Ê( x) (2.11) 2.3 Chƣơng trình mô phỏng acrobot trong không gian 2 chiều function [sys, x0, str, ts]=animdemo(t, x, u, flag) l1 = 2; l2 = 4; global xP1 xP2 yP1 yP2 R P xLink1 xLink2 yLink1 yLink2 xCenter yCenter global Link1 Link2 Center global AnimDemoFigure str=[]; ts=[.01 0]; P0 = [8 8]; if flag==2 if any(get(0,'Children')==AnimDemoFigure) set(0, 'CurrentFigure', AnimDemoFigure); xP1new = l1*cos(u(1)+pi/2) + P0(1); yP1new = l1*sin(u(1)+pi/2) + P0(2); R = [cos(u(1)+pi/2) sin(u(1)+pi/2);-sin(u(1)+pi/2) cos(u(1)+pi/2)]; P = [l2*cos(u(2)) l2*sin(u(2))]*R+[xP1new yP1new]; xP2new = P(1); yP2new = P(2); xLink1 = [P0(1) xP1new]; yLink1 = [P0(2) yP1new]; xLink2 = [xP1new xP2new]; yLink2 = [yP1new yP2new]; n = 100; t = 0:2*pi/n:2*pi*(1-1/n); xCenter = xP1new + .02*cos(t); 6 yCenter = yP1new + .02*sin(t); set(Link1, 'XData', xLink1,'YData', yLink1 ); set(Link2, 'XData', xLink2,'YData', yLink2 ); set(Center,'XData',xCenter,'YData',yCenter); drawnow end sys=[]; elseif flag==0 [fig, flag]=figflag('Animation Demo Figure', 0); if flag AnimDemoFigure=fig; cla reset; else AnimDemoFigure=figure; end set(AnimDemoFigure, ... 'Name', 'Acrobat',... 'NumberTitle', 'off') plot([0 16],[0 0],'k') n = 100; t = 0:2*pi/n:2*pi*(1-1/n); x = 8 + .02*cos(t); y = 8 + .02*sin(t); plot(x,y,'k','LineWidth',5); set(gca, ... 'Visible', 'off',... 'DrawMode','fast',... 'XLim', [0 16],... 'YLim', [0 16]); 7 xLink1 = [8 8]; yLink1 = [8 10]; xLink2 = [8 12]; yLink2 = [10 10]; n = 100; t = 0:2*pi/n:2*pi*(1-1/n); xCenter = 8 + .5*cos(t); yCenter = 10 + .5*sin(t); hold on; Link1 =plot(xLink1, yLink1,'b','LineWidth',6); Link2 =plot(xLink2, yLink2,'r','LineWidth',3); Center = plot(xCenter,yCenter,'g','LineWidth',5) sys=[0 0 0 4 0 0 1]; x0=[]; end pause(.001) Mỗi khi mô phỏng hệ thống chúng ta sẽ nhìn thấy hình ảnh động của acrobot. 8 CHƢƠNG III: THIẾT KẾ BỘ ĐIỀU KHIỂN 3.1 . Tuyến tính hóa mô hình tại điểm cân bằng không ổn định Hình 3.1 Điểm cân bằng không ổn định của hệ thống Tại điểm cân bằng không ổn định phía trên ta có:   q1  2  q2  0 Do đó ta có mô hình trạng thái ấp ỉ của acrobot ở lân cận điểm cân bằng không ổn định 0 là: 0 0 x  J x x  J uu    a31   a41 0 0 a32 a42 1 0 0  0 0 1 x   u b3  0 0    0 0 b4  9 3.2 . Thiết kế bộ điều khiển phản hồi trạng 3.2.1. Phƣơng pháp gán điểm cực X t mô hình tuyến tính như sau:  x  Ax  bu  u   Kx 1  2 Các bước thiết kế bộ điều khiển: - Bƣớc 1: Chọn trước các điểm cực - Bƣớc 2: Xây dựng phương trình đặc tính - Bƣớc 3: Tìm K 3.2.2. Ứng dụng Matlab tìm bộ điều khiển cho acrobot Ta có mô hình trạng thái ấp ỉ của acrobot ung quanh điểm cân bằng không ổn định là: x  J x x  Juu (3.2) Chọn bộ điểm cực mong muốn của hệ kín là: p=[-4 -4 -4 -4] Sử dụng lệnh acker trong Matlab ta có thể tìm được K: K=acker(A,b,p) 10 Ta có bộ điều khiển dạng như sau: K = [k1 k2 k3 k4] 3.3 . Bộ điều khiển LQR 3.3.1. Phƣơng trình Riccati đại số Để đảm bảo hệ thống ổn định tại gốc tọa độ và có khả n ng chống ảnh hưởng của nhiễu, một hàm mục tiêu được lựa chọn như sau:  1 min J    xT Qx+uT Ru  dt 20 Trong đó Q và R là các ma trận đối ứng ác định dương. Chọn luật điều khiển hồi tiếp trạng thái u = -Kx, K là v c tơ hằng số  1 J   xT  Q  K T RK  xdt 20 Chọn hàm Lyapunov là J ta được: V  x t    1 T x  Q  K T RK  xdt  20 Phương trình Lyapunov trở thành phương trình đại số Riccati: AT P  PA  PbR1bT P  Q  0 3.3.2. Sử dụng Matlab tìm ma trận P và K D ng lệnh [K,P,e]=lqr(A,b,Q,R) trong Matlab để giải bài toán cực tiểu:  min J    xT Qx  uT Ru dt 0 Và tìm nghiệm của phương trình đại số Riccati: 11 AT P  PA  PbR1bT P  Q  0 Trong đó: e là giá trị riêng của ma trận A  bK Đặt 1 0 Q 0  0 0 0 0 1 0 0  , R  1 0 1 0  0 0 1 A   0 0 1 0;0 0 0 1; a31 a32 0 0; a41 a42 0 0; b   0;0; b3 ; b4  ; Q  1 0 0 0;0 1 0 0;0 0 1 0;0 0 0 1; R  1 ;  K , P, e  lqr  A, b, Q, R  Ta được: K   k1 k2 k3 k4  12 CHƢƠNG IV: MÔ PHỎNG 4.1 . Bài toán 1 Trong phần này chúng tôi sẽ thiết kế bộ điều khiển phản hồi trạng thái, bộ điều khiển phi tuyến (2.11) cho acrobot với các thông số đã cho trong bài báo [3]. Bảng 1: Các thông số của hệ thống Acrobot l1 ( m ) l2 ( m ) m1 ( kg ) m2 ( kg ) I1 ( kgm2 ) I 2 ( kgm2 ) 1 2 1 2 0.083 0.667 4.1.1 Bộ điều khiển phản hồi trạng thái theo phƣơng pháp gán điểm cực Chọn bộ điểm cực: p=[-4 -4 -4 -4] Sử dụng lệnh acker trong Matlab: A=[0 0 1 0;0 0 0 1;11.76 -17.64 0 0;-13.23 38.22 0 0]; b=[0;0;-2.1;4.05]; p=[-4 -4 -4 -4]; K=acker(A,b,p) Ta thu được bộ điều khiển: K = [-513.4706 -230.2196 -227.3579 -113.9473] Mô phỏng hệ thống với điều kiện đầu x1   , x2   , x3  0, x4  0. 8 6 13 Kết quả 4 10 x1 x2 3 8 2 6 1 4 2 0 0 -1 -2 -2 -4 -3 -4 -6 0 1 2 3 4 5 6 7 8 9 10 -8 0 1 2 3 4 5 6 7 8 9 10 40 x3 60 x4 30 40 20 20 10 0 0 -20 -10 -40 -20 -30 -60 0 1 2 3 4 5 6 7 8 9 10 -80 0 1 2 3 4.1.2. Bộ điều khiển LQR Chọn: 1 0 Q 0  0 0 0 0 1 0 0  . , R  1 0 1 0  0 0 1 4 5 6 7 8 9 10 14 D ng lệnh  K , P, e  lqr  A, b, Q, R  ta thu được: K   541.8813 240.891 243.3184 121.5755  Mô phỏng với điều kiện đầu x1  4 8  , x2  6 , x3=0, x4=0. 10 x1 x2 3 8 2 6 4 1 2 0 0 -1 -2 -2 -4 -3 -4 -6 0 1 2 3 4 5 6 7 8 40 -8 0 1 2 3 4 5 6 7 60 x3 x4 30 40 20 20 10 0 0 -20 -10 -40 -20 -30 8 -60 0 1 2 3 4 5 6 7 8 -80 0 1 2 3 4 5 6 7 8 Ta dễ dàng nhận thấy Acrobot đạt tới trạng thái cân bằng không ổn định sau 6 giây 4.1.3. Bộ điều khiển phi tuyến Dựa vào các thông số (4.1) chúng tôi thiết kế bộ điều khiển phi tuyến theo luật (2.11). Trong quá trình mô phỏng, chúng tôi sử dụng một khâu chuyển mạch. Khi hệ thống bắt đầu làm việc bộ điều khiển phi tuyến sẽ được kết nối với hệ thống để điều khiển. Khi mà acrobot đạt tới v ng lân cận của điểm cân bằng không ổn định thì 15 khâu chuyển mạch sẽ ngắt bộ điều khiển phi tuyến ra khỏi hệ thống và kết nối bộ điều khiển phản hồi trạng thái với hệ thống. Chúng tôi sẽ mô phỏng với điều kiện đầu ở gần điểm cân bằng ổn định. Đây là vị trí gần như là a điểm cân bằng không ổn định nhất. Điều kiện đầu như sau: x1   , x2  0, x3  0, x4  0.1. Kết quả mô phỏng 10 15 x1 x1 8 6 10 4 5 2 0 0 -2 -5 -4 -6 -10 -8 -15 -10 0 5 10 15 20 25 30 35 40 45 50 0 5 10 15 20 25 30 35 40 45 50 10 20 x3 x4 8 15 6 10 4 2 5 0 0 -2 -5 -4 -10 -6 -15 -8 -10 0 5 10 15 20 25 30 35 40 45 50 -20 0 5 10 15 20 25 30 35 40 45 50 Ta nhận thấy các qu đạo trạng thái đều tiến về không sau khoảng 37 giây. 4.2 . Bài toán 2 Trong phần này chúng tôi đã thiết kế và chế tạo một mô hình acrobot. Trên cơ sở acrobot thực này chúng tôi sẽ ác định các tham 16 số cho mô hình toán của nó. Sau đó sẽ thiết kế bộ điều khiển và mô phỏng kết quả trong Matlab/Simulink. Bảng 2: Các thông số của Acrobot thực l1 ( m ) l2 ( m ) m1 (kg) m2 ( kg ) I 2 ( kgm2 ) IT ( kgm2 ) 0.285 0.203 0.245 0.03 0.0001 0.00019 Bộ điều khiển phản hồi trạng thái theo phƣơng pháp gán 4.2.1 điểm cực Chọn bộ điểm cực: p=[-4 -4 -5 -5]. Sử dụng lệnh acker trong Matlab với A=[0 0 0 1 0;0 0 0 1; 56.26 -0.53 0 0;-57.28 2.15 0 0]; b=[0;0;-387.41;457.13]; p=[-4 -4 -5 -5]; K=acker(A,b,p) Ta tìm được bộ điều khiển K = [-0.7061 -0.2059 -0.2079 -0.1368] Mô phỏng với điều kiện đầu x1  2  8  , x2  6 , x3  0, x4  0 1 x1 x2 0 1 -1 0 -2 -1 -3 -2 -4 -3 -4 -5 0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 -6 0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 17 10 30 x4 x3 5 25 0 20 -5 15 -10 -15 10 -20 5 -25 0 -5 -30 0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 -35 0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 Hệ thống đạt tới trạng thái ổn định sau khoảng 3 giây. 4.2.2. Khảo sát khi có nhiễu tác động x1   , x2  0, x3  0, x4  0.1 Mô phỏng với điều kiện đầu 14 2 x2 x1 12 1 10 0 8 -1 6 -2 -3 4 -4 2 -5 0 -6 -2 -4 -7 -8 -6 0 1 2 3 4 5 6 7 8 9 10 0 x3 40 1 2 3 4 5 6 7 8 9 10 30 x4 30 20 20 10 10 0 0 -10 -10 -20 Ư -20 -30 -30 0 1 2 3 4 5 6 7 8 9 10 -40 0 1 2 3 4 5 6 7 8 9 10 Chúng ta có thể thấy với mô hình toán này, chỉ cần bộ điều khiển phản hồi trạng thái là đủ để đưa acrobot về trạng thái cân bằng không ổn định từ bất kỳ trạng thái ban đầu nào. 18 CHƢƠNG V: GIẢI PHÁP THIẾT KẾ MÔ HÌNH THỰC CHO ACROBOT 5.1 Thiết kế hệ thống điều khiển Acrobot Để phục vụ cho việc nghiên cứu tôi đã thiết kế và chế tạo hệ thống điều khiển Acrobot gồm có hai khớp, trong đó chỉ có khớp thứ 2 được điều khiển để lắc đưa Acrobot về vị trí cân bằng. 5.2. Mô hình thí nghiệm Acrobot Cấu trúc mô hình thí nghiệm Acrobot gồm: - Máy tính xách tay: Core i5, 1,8Hz, Ram 4G – Phần mềm Matlab 2013a. - Card điều khiển Arduino. - Động cơ DC Encoder 334 ung. - IC L298N. - IC SN74HC08N - Động cơ DC gear motor encoder - Bo mạch đa n ng. 19 5.2.6. Thiết kế bộ điều khiển PI cho Acrobot Sử dụng máy tính và Card Ardiuno điều khiển Acrobot. Các thông số được chọn bằng phương pháp thực nghiệm. 5.2.7. Kết quả thực nghiệm 50 Vi tri goc cua link 2 40 30 20 10 0 -10 -20 -30 -40 -50 0 10 20 30 40 50 60 70 80 90 100 Hình 5.9 Vị trí góc của thanh 2 Ta nhận thấy khi đưa Acrobot lệch khỏi vị trí cân bằng, sau một khoảng th i gian tác động (khoảng 10 giây) thì Acrobot đã điều khiển được thanh 2 về vị trí cân bằng. 20 CHƢƠNG 6: KẾT LUẬN VÀ KIẾN NGHỊ 6.1. Kết luận - Viết được chương trình mô phỏng acrobot trong không gian 2 chiều. - Thiết kế, mô phỏng và kiểm tra được các kết quả trong bài báo [3] là đúng và chính xác. - Thiết kế và chế tạo được một mô hình acrobot thực. - Thử nghiệm các thuật toán để điều khiển trên acrobot thực. - Điều khiển được Acrobot ở lân cận vị trí cân bằng về vị trí cân bằng. 6.2. Kiến nghị Ứng dụng thuật toán điều khiển Acrobot để điều khiển các robot 2 bậc 3 bậc tự do.