Bai 1 - Dieu khien he phi tuyen

u – K oa ệ - ệ tử - H ác K oa PHCM BÀI THÍ NGHIỆM ĐIỀU KHIỂN HỆ PHI TUYẾN CON LẮC NGƯỢC QUAY I MỤC ĐÍCH Trong thí nghiệm này, sinh viên xây dựng mơ hình phi tuyến hệ lắc ngược quay phương pháp Euler-Lagrange Sinh viên gợi ý sử dụng Symbolic Toolbox MATLAB để kiểm tra lại trình tính tốn Dựa mơ hình tốn đưa ra, sinh viên xây dựng mơ hình mơ MATLAB Simulink thực kiểm chứng lại mơ hình tìm Ngồi ra, sinh viên cịn sử dụng phương pháp thiết kế điều khiển cân lắc ngược quay: tuyến tính hóa điểm làm việc tĩnh kết hợp thiết kế điều khiển LQR, thiết kế điều khiển hồi tiếp tuyến tính hóa thiết kế điều khiển trượt Bộ ước lượng trạng thái giới thiệu trường hợp tín hiệu hồi tiếp không quan sát đầy đủ, kết hợp với điều khiển LQR để điều khiển cân lắc Giải thuật swingup đưa lắc từ vị trí cân (cân bền) đến vị trí cân dựa phương pháp lượng giới thiệu thí nghiệm Mục tiêu sau hồn thành thí nghiệm  Biết cách xây dựng mơ hình tốn học mơ tả hệ thống học phương pháp Euler-Lagrange  Biết cách sử dụng Symbolic Toolbox MATLAB hỗ trợ cho việc thành lập mơ hình tốn hệ thống  Xây dựng kiểm chứng mơ hình hệ phi tuyến sử dụng MATLAB Simulink  Sử dụng phương pháp tuyến tính hóa điểm làm việc tĩnh để thiết kế điều khiển tuyến tính cho hệ phi tuyến Nhận xét ảnh hưởng ma trận số Q R luật điều khiển LQR  Thiết kế điều khiển hồi tiếp tuyến tính hóa, điều khiển trượt cho hệ phi tuyến, đánh giá chất lượng điều khiển  Thiết kế ước lượng trạng thái trường hợp hồi tiếp tín hiệu khơng đầy đủ có nhiễu  Sử dụng phương pháp lượng để xây dựng giải thuật swingup cho lắc lựa chọn điều kiện để chuyển đổi điều khiển swingup điều khiển cân 1/23 u – K oa ệ - ệ tử - H ác K oa PHCM II CƠ SỞ LÝ THUYẾT 2.1 Mơ hình tốn học mơ tả hệ thốngcon lắc ngược quay m2 ,J2 z θ2 ,T2 O l2 y θ1 ,T1 l1 DC MOTOR x m1,J1 Hình Mơ hình lắc ược quay thông số Moment quay trục quay O (J1) Moment lắc trục quay qua trọng tâm (J2) Chiều dài lắc (l1) Khoảng từ trục quay lắc đến trọng tâm lắc (l2) Khối lượng lắc (m2) Hệ số ma sát trục quay O (c1) Hệ số ma sát trục quay lắc (c2) Hằng số moment (động có hộp số 1:5) (Kt) Hằng số điện từ (Ke) Điện trở phần ứng (Ra) Gia tốc trọng trường (g) Bảng Các thơng số mơ hình lắc Góc lệch quay Góc lệch lắc Moment quay Moment tác động vào lắc Điện áp cấp cho động DC Bảng Các tín hiệu qua sát 2.48*10-2 3.86*10-3 0.15 0.148 0.075 10-4 2.8*10-4 0.09*5 0.09 7.8 9.8 ược quay θ1 θ2 T1 T2 u u n mơ hình lắc ược quay 2/23 u – K oa ệ - ệ tử - H ác K oa PHCM Tọa độ chất điểm m2 hệ tọa độ Oxyz gắn với trục quay động  x2  l1 cos1  l2 sin 1 sin    y2  l1 sin 1  l2 cos 1 sin   z  l cos   2 (1) Vận tốc chất điểm m2 hệ tọa độ Oxyz  x2  l11 sin 1  l21 cos1 sin   l2 sin 1 cos   y2  l11 cos 1  l21 sin 1 sin   l2 cos1 cos   z2  l2 sin   v2  x2  y2  z2  l1212  l2 22  l2 21 sin 2  2l1l212 cos2 (2) (3) Động quay J112 P1  K1  (4) Động lắc 1 J 2 2  m2v2 2 1  m2l1212   J  m2l2  2  m2l2 21 sin   m2l1l21 cos 2 2 K2  (5) P2  m2 gz2  m2 gl2 cos2 Toán tử Lagrange L = ∑K - ∑P L 1 1 J112  m2l1212   J  m2l2  2  m2l2 21 sin  2 2  m2l1l21 cos  m2 gl2 cos (6) Phương trình Euler – Lagrange cho chuyển động lắc d  L  L  Ti   dt  i  i (7) Trong đó, θi Ti góc quay tổng moment tác động lên lắc trục quay tương ứng 3/23 u – K oa ệ - ệ tử - H ác K oa PHCM Xét phương trình Euler-Lagrange với θ1 θ2 ta có phương trình tốn học mơ tả hệ thống (Tm moment tạo động DC, T1 tính cơng thức (30), T2 có moment ma sát cản xét trường hợp khơng có ngoại lực tác động)  2 2  J1  m2l1 m2l2 sin   m2l1l2 cos m2l2 1 sin(2 )  m2l1l2 sin   Tm  c11  m2l1l21 cos   J m2l2   m2l2 sin(2 )12  m2 gl2 sin    c2 2  (8) Xét động DC, ta có mối quan hệ La Ra Tm θ u Armature current e J ia Rotor + load Hình Sơ đồ tương đương động DC E  Ke1 Ke hệ số điện áp cảm ứng (Back-EMF) Tm  Kt ia Kt số moment động DC Bỏ qua ảnh hưởng điện cảm cuộn dây rotor, ta có mối quan hệ Tm  Kt KK u  E Kt  u  t e 1 Ra Ra Ra (9) Thay (9) vào (8) ta mơ hình tốn mơ tả hệ thống lắc ngược quay với đầu vào điện áp cấp cho động u Trong hệ đơn vị SI Kt = Ke 2.2 Tuyến tính hóa điểm cân Hệ thống phi tuyến mô tả phương trình trạng thái  x  f ( x(t ), u (t ))   y  h( x(t ), u (t )) (10) Tuyến tính hóa hệ thống điểm cân ta phương trình trạng thái tuyến tính sau 4/23 u – K oa ệ - ệ tử - H ác K oa PHCM  x  Ax  Bu   y  Cx (11) Với ma trận  f1   x1 x  x0 ,u u0 A    f  n  x1 x  x0 ,u u0  h C  x1 x  x0 ,u u0   x  x0 ,u u0     f n  xn x  x ,u u  0  f1 xn h x2 x  x0 ,u u0  f1  u   f  B   u    f n   u h xn   x  x0 ,u u0    x  x0 ,u u0       x  x0 ,u u0  (12)   x  x0 ,u u0   2.3 Điều khiển hồi tiếp tuyến tính hóa Điều khiển hồi tiếp tuyến tính hóa trước hết chuyển hệ thống phi tuyến dạng hệ thống tuyến tính, sau sử dụng kỹ thuật điều khiển tuyến tính biết để hồn chỉnh điều khiển hệ thống Phương pháp mơ tả xác thay đổi trạng thái hệ thống, không giống phương pháp xấp xỉ tuyến tính phần 2.2 Hệ thống phi tuyến biểu diễn phương trình trạng thái sau  x  f ( x)  g ( x).u   y  h( x ) (13) Trong u, y tín hiệu vào tín hiệu hệ thống vector trạng thái x   x1 x2 xn  T Nếu đối tượng có bậc tương đối n biểu diễn mối quan hệ vào đối tượng sau y ( n)  a( x)  b( x).u (14) Trong đó: 5/23 u – K oa ệ - ệ tử - H ác K oa PHCM a( x)  Lnf h( x)  n 1 b(x)  Lg L f h( x)  Chọn đầu vào u  (15) v  a( x) b( x ) (16) Hệ thống trở thành y (n)  v (17) Chọn luật điều khiển bám v  yd (n)  k1e(n 1)  k2e(n 2)   kn e (18) Đa thức đặc trưng sai số Δs = s(n) + k1s(n-1) + … + kn, chọn giá trị k cho đa thức đa thức Hurwitz, tức tất nghiệm Δs nằm bên trái mặt phẳng phức vị trí cực thỏa mãn yêu cầu độ 2.4 Điều khiển trượt Hệ thống phi tuyến biểu diễn phương trình trạng thái sau  x  f ( x)  g ( x).u   y  h( x ) (19) Trong u, y tín hiệu vào tín hiệu hệ thống vector trạng thái x   x1 x2 xn  T Nếu đối tượng có bậc tương đối n biểu diễn mối quan hệ vào đối tượng sau y ( n)  a( x)  b( x).u (20) Trong đó: a( x)  Lnf h( x)  n 1 b(x)  Lg L f h( x)  (21) Định nghĩa mặt trượt:   e( n1)  k1e( n2)   kn2e  kn1e (22) 6/23 u – K oa ệ - ệ tử - H ác K oa PHCM Δs = s(n) + k1s(n-1) + … + kn đa thức đặc trưng mặt trượt, ki chọn cho Δs đa thức Hurwitz (tất nghiệm nằm bên trái mặt phẳng phức) thỏa mãn yêu cầu đáp ứng độ Tín hiệu u(t) chọn sau sai số e tiến 0, hay tín hiệu đầu y bám theo tín hiệu đặt yd u  a( x)  yd( n )  k1e( n1)  b( x )  kn2e  kn1e  Ksign( )  (23) Để giảm tượng chattering, thay hàm sign(x) hàm sat(x) 2.5 Điều khiển swing-up dựa phương pháp lượng Định luật Newton cho chuyển động quay lắc (bỏ qua ma sát) J 2  m2 gl2 cos2  T1l1l2 cos2 (24) Trong đó, T1 moment quay tác động vào lắc Năng lượng lắc (chọn gốc điểm cân trên) E J 2 2  m2 gl2  cos  1 (25) Thay điều khiển góc lệch lắc hay vận tốc góc lắc, phương pháp lượng dựa vào việc điều khiển lượng lắc đạt đến giá trị E0 = (năng lượng vị trí cân trên) Xét đạo hàm lượng dE  J 2 2  m2 gl2 sin  2  T1l1l2 cos dt (26) Như vậy, để lượng lắc tăng, ta cần cung cấp moment cho quay T1 ngược hướng với dấu  cos (để đạo hàm dương) Tiếp tục ta xét hàm Lyapunov V  E  E0  dV   E  E0  E    E  E0  T1l1l2 cos dt (27) (28) Nếu ta chọn  T1   K sw  E  E0  sign 2 cos2  (K sw > 0) (29) 7/23 u – K oa ệ - ệ tử - H ác K oa PHCM dV  3   K sw l1l2  E  E0   cos  (     ,  ) dt 2 Kết luận với tín hiệu điều khiển T1 chọn lượng hệ thống ổn định đạo hàm lượng dương, tức E tăng đến giá trị E0 ổn định E0 Giá trị Ksw đặc trưng cho tốc độ swing-up, Ksw lớn tốc độ swing-up nhanh Mối quan hệ moment lắc đầu vào điện áp u T1  Tm  c11  Kt Kt Ke  Tm  R u  R 1 a a  KK  K  T1  t u   t e  c1 1 Ra  Ra  (30) Vậy luật điều khiển swing-up cho lắc ngược quay KK   K sw  E  E0  sign  cos    t e  c1 1  Ra  u Kt Ra   (31) 2.6 Giới thiệu thư viện Symbolic Math Toolbox MATLAB Các biến thông thường sử dụng MATLAB biến kiểu liệu số (double precision numeric variables), thư viện Symbolic Math Toolbox cho phép khai báo biến chữ (symbolic variables) thực phép toán: đạo hàm, tích phân, giải phương trình, …  Khai báo biến kiểu symbolic syms x y z; % x, y, z are symbolic variables  Kiểm tra kiểu biến workspace (kiểu double hay symbolic) whos;  Khai báo hàm : f(x,y) = 2x + 3y + cos(xy) f = 2*x + 3*y + cos(x*y); % f is now a function of x and y  Đạo hàm df/dx df_dx=diff(f, x); %the result is the derivative of f with respect to the variable x  Đạo hàm hàm hợp theo thời gian 8/23 u – K oa ệ - ệ tử - H ác K oa PHCM d_x = fulldiff(x); % declare that d_x is the derivative of x with respect to the variable t Lưu ý: ùy t eo phiên MATLAB mà hàm fulldiff có th tồn khơng, sinh viên có th dow load ặt workspace chạy: https://www.mathworks.com/matlabcentral/fileexchange/7174-fulldiff-m df df dx df dy  Đạo hàm hàm hợp nhiều biến theo thời gian dt  dx dt  dy dt df_dt = fulldiff(f, {x y}); to the variable t % df_dt is the derivative of f with respect  Tối giản hàm số df = simplify(df_dt); % simplify function Trong số trường hợp MATLAB không tự động tối giản hàm số được, trường hợp sinh viên kiểm tra lại thực tay • Thay giá trị cho biến hàm subs(df_dt, x, 0); % replace var x by its value in function dx_dt • Thay giá trị cho vector (nhiều biến) hàm subs(dx_dt, [x, y], [0, 0]); % replace vector [x, y] by their values [0, 0] in function dx_dt • Chuyển đổi giá trị symbolic sang giá trị số (double) sau thay tất giá trị cho biến symbolic double(df); % convert symbolic value to numeric value of object A III MƠ TẢ THÍ NGHIỆM Phần mềm sử dụng thí nghiệm phần mềm Matlab Simulink Sinh viên tạo mơ hình mơ Simulink hình vẽ thí nghiệm Sinh viên thực cài đặt thông số mô sau: vào menu Simulation Configuration Parameters, chọn mục Solver để cài đặt thông số thời gian mô (Simulation time) phương pháp mô (Solver Options) ình Thời gian mơ (khoanh trịn) thay đổi tùy theo thí nghiệm 9/23 u – K oa ệ - ệ tử - H ác K oa PHCM Hình Mơ hình mơ Simulink Hình Cài đặt thông số mô IV CHUẨN BỊ TRƯỚC KHI THÍ NGHIỆM 4.1 Xây dựng mơ hình tốn mơ tả hệ thống lắc ngược quay Để hiểu rõ phương pháp Euler-Lagrange, sinh viên thực lại đầy đủ bước để thành lập phương trình tốn mơ tả hệ thống lắc ngược quay công thức (32) Đầu vào hệ thống u (điện áp cấp cho động DC) Các biến trạng thái: góc quay quay θ1, góc lệch lắc θ2, vận tốc góc tương ứng quay lắc ̇ ̇ 10/23 u – K oa ệ - ệ tử - H ác K oa PHCM  1  f1 (1 , ,1 , , u)     f (1 , ,1 , , u) (32) - Sinh viên chạy thử để hiểu ý nghĩa câu lệnh thư viện Symbolic Math Toolbox giới thiệu phần 2.6 - Tạo m-file “pend_dynamics1.m” với nội dung sau Lưu ý: ếu lệnh fulldiff khơng có sẵn cầ ặt t v ệ tro t ục làm việc %% Define parameters (use symbolic variable of MATLAB) syms g; syms m1 m2 J1 J2 l1 l2 c1 c2; syms Ra La Kt Ke; %% Declare system variables (use symbolic variable of MATLAB) syms th1 dth1 d2th1; syms th2 dth2 d2th2; syms Tm; %% dth1= fulldiff(th1); d2th1= fulldiff(dth1); dth2= fulldiff(th2); d2th2= fulldiff(dth2); %% x2 = l1*cos(th1) + l2*sin(th1)*sin(th2); y2 = l1*sin(th1) - l2*cos(th1)*sin(th2); z2 = l2*cos(th2); %% dx2 = fulldiff(x2,{th1 th2}); dy2 = fulldiff(y2,{ th1 th2}); dz2 = fulldiff(z2,{ th1 th2}); %% v2_sq = simplify(dx2^2 + dy2^2 + dz2^2); v2_sq = simplify((l2*dth1)^2*sin(th2)^2+simplify(v2_sq+(l2*dth1)^2*(cos(th2)^21))); %% Potential Energy % Arm Ep_1 = 0; % Pendulum Ep_2 = g*m2*l2*cos(th2); % Total potential energy Ep = Ep_1 + Ep_2; %% Kinetic Energy % Kinetic energy of the Arm Ek_1_r = 0.5*J1*(dth1)^2; Ek_1_t = 0; 11/23 u – K oa ệ - ệ tử - H ác K oa PHCM Ek_1 = Ek_1_r + Ek_1_t; % Kinetic energy of the Pendulum Ek_2_r = 0.5*J2*(dth2)^2; Ek_2_t = 0.5*m2*v2_sq; Ek_2 = Ek_2_r + Ek_2_t; % Total kinetic energy of the system Ek = Ek_1 + Ek_2; %% Langrangian operator L = Ek - Ep; %% Theta % Derive the components that make up the Euler-Lagrange Equation dL_dtheta1 = simplify(diff(L,th1)); % d/dt(dL/ddtheta1) dL_ddtheta1_dt = simplify(fulldiff(diff(L,dth1),{th1 th2 dth1 dth2})); Ftheta1 = simplify(dL_ddtheta1_dt - dL_dtheta1 + c1*dth1 – Tm) %% Theta % Derive the components that make up the Euler-Lagrange Equation dL_dtheta2 = simplify(diff(L,th2)); % d/dt(dL/ddtheta1) dL_ddtheta2_dt = simplify(fulldiff(diff(L,dth2),{th1 th2 dth1 dth2})); Ftheta2 = simplify(dL_ddtheta2_dt - dL_dtheta2 + c2*dth2) - Kiểm tra lại kết thu từ MATLAB cơng thức (8) Kết phương trình tốn mơ tả hệ thống với biến trạng thái cho, tín hiệu điều khiển moment động DC Tm - Thay moment động (Tm) công thức (9) đặt số để thu gọn mơ hình, ta tiếp tục giải hệ phương trình (8) với ẩn d2θ1/dt2 d2θ2/dt2 a  J1  m2l12 , b  m2l1l2 , c  m2l2 d Kt K e K  c1 , e  t Ra Ra f  J  m2l2 , h  m2 gl2 - Tạo m-file “pend_dynamics2.m” để giải phương trình với nội dung sau %% Declare symbolic variables syms a b c d e f h c1 c2; syms vol th1 th2; syms dth1 dth2; %% vector X = [d2th1; d2th2] % Solve equation A.X = B A = [a+c*(sin(th2))^2 -b*cos(th2); -b*cos(th2) f] B = [-c*sin(2*th2)*dth1*dth2-b*dth2^2*sin(th2)-d*dth1+e*vol; (c*dth1^2*sin(2*th2))/2+h*sin(th2)-c2*dth2] %% Math model of system X = inv(A)*B; X = simplify(X) 12/23 u – K oa ệ - ệ tử - H ác K oa PHCM - Vector X vừa tìm phương trình tốn mơ tả hệ thống (cơng thức (32)) với biến trạng thái cho, tín hiệu điều khiển điện áp cấp cho động DC u 4.2 Xây dựng mơ hình mơ hệ thống với MATLAB Simulink - Sinh viên tạo m-file “pend_model_init.m” để khởi tạo thơng số mơ hình với số liệu cho bảng %% pend_model_init.m %% Initialize parameters of RIP J1 = 2.48*10^-2; J2 = 3.86*10^-3; m2 = 0.075; l1 = 0.15; l2 = 0.148; g = 9.8; c1 = 1*10^-4; c2 = 2.8*10^-3; Kt = 0.09*5; Ke = 0.09; Ra = 7.8; %% Some constants to simplify model a = J1 + m2*l1^2; b = m2*l1*l2; c = m2*l2^2; d = Kt*Ke/Ra + c1; e = Kt/Ra; f = J2 + m2*l2^2; h = m2*g*l2; - Xây dựng mô hình mơ Simulink “pend_model.mdl” hình Thay biến hệ thống phương trình tìm phần 4.1 tín hiệu tương ứng: u(1) th1, u(2) dth1, u(3)th2, u(4) dth2, u(5) vol nhập vào function block MATLAB hình - Khối Voltage Limit giới hạn tín hiệu áp động DC [-25, 25] Hình Mơ hình khảo sát hệ lắc ngược quay 13/23 u – K oa ệ - ệ tử - H ác K oa PHCM Hình Khối mơ lắc ngược quay Khảo sát đáp ứng đầu trạng thái trường hợp góc lệch ban đầu cho bên kiểm chứng tính đắn mơ hình dựa vào suy luận vật lý Lần chạy Giá trị khởi tạo θ1 900 1800 900 1800 Giá trị khởi tạo θ2 0 100 450 900 4.3 Tuyến tính hóa điểm cân Sinh viên tiếp tục sử dụng thư viện Symbolic Toolbox để thành lập phương trình trạng thái tuyến tính hệ thống lắc ngược cân điểm cân trên, tìm ma trận A, B - Sinh viên tạo m-file “pend_linear.m” có nội dung sau %% clear;clc; pend_model_init; %call pend_model_init.m to re-initialize %% Declare symbolic variables syms vol th1 th2; syms dth1 dth2; %% vector X = [d2th1; d2th2] % Solve equations A.X = B A = [a+c*(sin(th2))^2 -b*cos(th2); -b*cos(th2) f] 14/23 u – K oa ệ - ệ tử - H ác K oa PHCM B = [-c*sin(2*th2)*dth1*dth2-b*dth2^2*sin(th2)-d*dth1+e*vol; (c*dth1^2*sin(2*th2))/2+h*sin(th2)-c2*dth2] %% Math model of system X = inv(A)*B; X = simplify(X); %% Linearization at equilibrium point f1 = X(1); df1_dth1 = diff(f1, th1); df1_ddth1 = diff(f1, dth1); df1_dth2 = diff(f1, th2); df1_ddth2 = diff(f1, dth2); f2 = X(2); df2_dth1 = diff(f2, th1); df2_ddth1 = diff(f2, dth1); df2_dth2 = diff(f2, th2); df2_ddth2 = diff(f2, dth2); A1 = [0 0; subs(df1_dth1, [th1, dth1, th2, dth2, vol], [0, 0, 0, 0, 0]) subs(df1_ddth1, [th1, dth1, th2, dth2, vol], [0, 0, 0, 0, 0]) subs(df1_dth2, [th1, dth1, th2, dth2, vol], [0, 0, 0, 0, 0]) subs(df1_ddth2, [th1, dth1, th2, dth2, vol], [0, 0, 0, 0, 0]); 0 1; subs(df2_dth1, [th1, dth1, th2, dth2, vol], [0, 0, 0, 0, 0]) subs(df2_ddth1, [th1, dth1, th2, dth2, vol], [0, 0, 0, 0, 0]) subs(df2_dth2, [th1, dth1, th2, dth2, vol], [0, 0, 0, 0, 0]) subs(df2_ddth2, [th1, dth1, th2, dth2, vol], [0, 0, 0, 0, 0])]; %% A matrix As = double(A1); - Kết thu ma trận A công thức (11) Sinh viên thực tương tự để tìm ma trận B 4.4 Điều khiển hồi tiếp tuyến tính hóa điều khiển trượt Dựa vào phương trình tốn mơ tả hệ thống tìm phần chuẩn bị 4.1, sinh viên tìm biểu thức a(x) b(x) công thức (14) (20) điều khiển hồi tiếp tuyến tính hóa điều khiển trượt Xét trường hợp tín hiệu đầu cần điều khiển góc lệch θ2 V THỰC HIỆN THÍ NGHIỆM 5.1 Tuyến tính hóa điểm cân thiết kế điều khiển LQR - Sử dụng mơ hình tuyến tính hóa điểm cân chuẩn bị nhà phần 4.3 (các ma trận As, Bs tìm được), sinh viên khảo sát điều khiển LQR sau - Xây dựng mơ hình mơ Simulink “pend_lqr.mdl” hình Lưu ý c ọn chế Multiplication Matrix(K*u) cho khối Gain 15/23 u – K oa ệ - ệ tử - H ác K oa PHCM - Viết m-file “pend_lqr_init.m” sử dụng ma trận As, Bs lệnh lqr để tìm vector hồi tiếp trạng thái K Hình Điều khiển cân lắc ngược LQR a Khảo sát ảnh hưởng ma trận Q ma trận R Giá trị khởi tạo trạng thái (θ1 = dθ1/dt=dθ2/dt = 0, θ2 = 100), sinh viên thay đổi giá trị ma trận Q R tương ứng với trường hợp tính lại vector K, vẽ đáp ứng ngõ Lần chạy - Giá trị ma trận Q Q = diag([1, 1, 0.001, 1]) Q = diag([1, 1, 1, 1]) Q = diag([1, 1, 10^4, 1]) Giá trị ma trận R R=1 R=1 R=1 Q = diag([1, 1, 1, 1]) Q = diag([1, 1, 1, 1]) Q = diag([1, 1, 1, 1]) R = 0.01 R=1 R = 100 Nhận xét ảnh hưởng việc thay đổi ma trận Q đến đáp ứng góc lệch θ2? Nhận xét ảnh hưởng việc thay đổi ma trận R đến đáp ứng góc lệch θ2 ? b Khảo sát ảnh hưởng giá trị khởi tạo góc θ2 - Đặt giá trị cho ma trận Q = diag([1, 1, 1, 1]) ma trận R = tìm lại vector hồi tiếp trạng thái K - Giá trị khởi tạo trạng thái (θ1 = dθ1/dt=dθ2/dt = 0), thay đổi giá trị khởi tạo góc θ2 tương ứng trường hợp sau vẽ đáp ứng ngõ 16/23 u Lần chạy – K oa ệ - ệ tử - H ác K oa PHCM Giá trị khởi tạo góc θ2 θ2 = 100 θ2 = 150 θ2 = 200 θ2 = 250 Nhận xét ảnh hưởng giá trị khởi tạo θ2 đến đáp ứng hệ thống ? 5.2 Bộ ước lượng trạng thái - Xây dựng mơ hình Simulink ước lượng trạng thái “pend_state_estimator.mdl” hình hình Các giá trị nhiễu cộng vào tín hiệu tương ứng sau: nhiễu tín hiệu đo θ1 Noise1 với variance N1, tương tự cho θ2 Noise2 với variance N2 Tín hiệu hồi tiếp góc lệch quay lắc (θ1 θ2) - Thực tính lại vector hồi tiếp trạng thái K với ma trận Q = diag ([1 1 1]) ma trận R = - Giá trị khởi tạo trạng thái (θ1 = dθ1/dt=dθ2/dt = 0, θ2 = 100) - Tính độ lợi Kalman cho ước lượng thái đoạn code sau %% Kalman gain G = eye(4); Cs = [1 0 0;0 0]; Cm = [1 0;0 1]; QN = 10^-6*eye(4); %variance of process noise RN = [10^-3 0; 10^-3]; %variance of measurement noise Lo = lqe(As,G,Cs,QN,RN) Hình Ước lượng trạng thái điều khiển LQR cân lắc ngược quay 17/23 u – K oa ệ - ệ tử - H ác K oa PHCM Hình Cấu trúc ước lượng trạng thái Sinh viên đánh giá sai số ước lượng tín hiệu θ1 θ2 trường hợp sau Giá trị phương sai nhiễu N1 = 0.001, N2 = 0.001 Giá trị ma trận QN RN QN =10-6*eye(4) RN =10-3*eye(2) 5.3 Điều khiển swing-up lắc phương pháp lượng Sinh viên thực mơ hình mơ Simulink “pend_swingup.mdl” hình 10, khối chức hệ thống hoạt động sau Hình 10 Sơ đồ khối điều khiển swing-up điều khiển cân dùng LQR Khối swing-up thực luật điều khiển swing-up để đưa lắc đến vị trí cân trên, tốc độ swing-up đặc trưng giá trị Ksw thay đổi Sinh viên thực luật điều khiển swing-up cơng thức (31) vào khối swing-up hình 11 18/23 u – K oa ệ - ệ tử - H ác K oa PHCM Hình 11 Khối điều khiển swing-up lắc Bộ điều khiển LQR để giữ cân cho lắc vùng làm việc tuyến tính thực thí nghiệm 5.1 Khối switchC (khối Embedded Matlab Function Simulink) thực việc xem xét điều kiện góc quay vận tốc góc để chuyển đổi luật điều khiển swing-up luật điều khiển cân LQR lắc vào vùng tuyến tính hóa Lưu ý: ối signal specification cần thiết MATLAB báo lỗi thiếu ịnh ng ĩa ki u liệu c o ầu vào cho khối switchC function y = switchC(ang, vel) temp = mod(ang, 2*pi); if(temp>pi) temp=temp-2*pi; end if((abs(temp)