THIẾT KẾ VÀ MÔ PHỎNG BỘ ĐIỀU KHIỂN LQG CHO HỆ CON LẮC NGƯỢC

THIẾT KẾ VÀ MÔ PHỎNG BỘ ĐIỀU KHIỂN LQG CHO HỆ CON LẮC NGƯỢC Lời cảm ơn i Mục lục ii Danh sách hình ảnh iii Danh sách bảng iv Chương 1. PHƯƠNG TRÌNH VI PHÂN VÀ CÁC THÔNG SỐ HỆ THỐNG 1 1.1 THÔNG SỐ VÀ MÔ TẢ TOÁN HỌC CỦA HỆ THỐNG 1 1.1.1 Thông số hệ con lắc ngược 1 1.1.2 Mô hình toán học của hệ con lắc ngược 1 1.2 THÀNH LẬP HỆ PHƯƠNG TRÌNH CỦA HỆ THỐNG 3 Chương 2. THIẾT KẾ BỘ ĐIỀU KHIỂN LQR 6 2.1 THIẾT KẾ BỘ LQR 6 2.1.1 Mô hình hoá hệ con lắc ngược bằng phương trình toán học 6 2.1.2 Cơ sở lý thuyết 7 2.1.3 Xây dựng và chọn thông số Q R theo các trường hợp và thiết kế bộ LQR 7 2.2 THIẾT KẾ BỘ LỌC KALMAN 13 2.2.1 Cơ sở lý thuyết 13 2.2.2 Xây dựng bộ lọc Kalman 14 2.3 THIẾT KẾ BỘ LQG 15 2.3.1 Cơ sở lý thuyết 15 2.3.2 Xây dựng bộ LQG 18 Chương 3. NHẬN XÉT 21 TÀI LIỆU THAM KHẢO 22

111Equation Chapter Section 1BỘ GIÁO DỤC & ĐÀO TẠO TRƯỜNG ĐẠI HỌC SƯ PHẠM KỸ THUẬT TP HỒ CHÍ MINH KHOA ĐIỆN – ĐIỆN TỬ BỘ MÔN TỰ ĐỘNG ĐIỀU KHIỂN -⸙∆⸙ - BÀI KIỂM TRA QUÁ TRÌNH NHẬN DẠNG VÀ ĐIỀU KHIỂN HỆ THỐNG ĐỀ TÀI: THIẾT KẾ VÀ MÔ PHỎNG BỘ ĐIỀU KHIỂN LQG CHO HỆ CON LẮC NGƯỢC GVHD: TS TRẦN ĐỨC THIỆN SVTH: PHAN MINH ĐIỀN MSSV: 19151937 Tp Hồ Chí Minh tháng năm 2022 Lời cảm ơn Mục lục Lời cảm ơn i Mục lục .ii Danh sách hình ảnh iii Danh sách bảng iv Chương PHƯƠNG TRÌNH VI PHÂN VÀ CÁC THƠNG SỐ HỆ THỐNG 1.1 THÔNG SỐ VÀ MÔ TẢ TOÁN HỌC CỦA HỆ THỐNG 1.1.1 Thông số hệ lắc ngược .1 1.1.2 Mơ hình tốn học hệ lắc ngược 1.2 THÀNH LẬP HỆ PHƯƠNG TRÌNH CỦA HỆ THỐNG Chương THIẾT KẾ BỘ ĐIỀU KHIỂN LQR 2.1 THIẾT KẾ BỘ LQR 2.1.1 Mô hình hố hệ lắc ngược phương trình tốn học 2.1.2 Cơ sở lý thuyết 2.1.3 Xây dựng chọn thông số Q R theo trường hợp thiết kế LQR 2.2 THIẾT KẾ BỘ LỌC KALMAN 13 2.2.1 Cơ sở lý thuyết 13 2.2.2 Xây dựng lọc Kalman 14 2.3 THIẾT KẾ BỘ LQG 15 2.3.1 Cơ sở lý thuyết 15 2.3.2 Xây dựng LQG 18 Chương NHẬN XÉT 21 TÀI LIỆU THAM KHẢO 22 Danh sách hình ảnh Hình 1.1: Mơ hình tốn học hệ lắc ngược xe Hình 2.1:Mơ hình hố hệ lắc ngược Simulink Hình 2.2 Bộ điều khiển LQR cho hệ lắc ngược phần mềm mô Simulink .9 Hình 2.3: Biểu đồ góc quay vận tốc góc hệ thống trường hợp 1.9 Hình 2.4: Đồ thị vị trí vận tốc hệ thống trường hợp 10 Hình 2.5: Đồ thị lực tác dụng vào hệ thống trường hợp 10 Hình 2.6: Biểu đồ góc quay vận tốc góc hệ thống trường hợp 12 Hình 2.7: Đồ thị vị trí vận tốc hệ thống trường hợp 12 Hình 2.8: Đồ thị lực tác dụng vào hệ thống trường hợp 13 Hình 2.9: Sơ đồ khối lọc Kalman 13 Hình 2.10: Xây dụng lọc Kalman Simulink 14 Hình 2.11: Biểu đồ góc quay hệ thống ngẫu nhiêu có lọc 14 Hình 2.12: Biểu đồ vị trí hệ thống ngẫu nhiêu có lọc 15 Hình 2.13: Bộ điều khiển LQG cho hệ lắc ngược .18 Hình 2.14: Thơng số khối nhiễu góc quay 18 Hình 2.15: Thơng số khối nhiễu vị trí 19 Hình 2.16: Biểu đồ góc quay vận tốc gốc hệ lắc ngược sử dụng điều khiển LQG trường hợp có nhiễu 19 Hình 2.17: Biểu đồ vị trí vận tốc hệ lắc ngược sử dụng điều khiển LQG trường hợp có nhiễu 20 Hình 2.18: Biểu đồ lực tác đơngj hệ lắc ngược sử dụng điều khiển LQG trường hợp có nhiễu 20 Danh sách bảng No table of figures entries found CHƯƠNG PHƯƠNG TRÌNH VI PHÂN VÀ CÁC THƠNG SỐ HỆ THỐNG Chương 1.PHƯƠNG TRÌNH VI PHÂN VÀ CÁC THÔNG SỐ HỆ THỐNG 1.1 THÔNG SỐ VÀ MÔ TẢ TỐN HỌC CỦA HỆ THỐNG 1.1.1 Thơng số hệ lắc ngược Hình 1.1: Mơ hình tốn học hệ lắc ngược xe Thông số hệ lắc ngược M  1.0kg : Trọng lượng xe m  0.1kg : Trọng lượng lắc l  1.0m : Chiều dài lắc u : Lực tác động vào xe  N  g : Gia tốc trọng trường  m / s  x : Vị trí xe  m   : Góc giữ lắc phương thẳng đứng  rad  1.1.2 Mơ hình toán học hệ lắc ngược Cách 1: Dùng định luật Newton: NHẬN DẠNG VÀ ĐIỀU KHIỂN HỆ THỐNG CHƯƠNG PHƯƠNG TRÌNH VI PHÂN VÀ CÁC THƠNG SỐ HỆ THỐNG Gọi ( xp , yp ) toạ độ vật nặng m đầu lắc, ta có: x p  x  l sin  y p  x  l cos  212\* MERGEFORMAT (.) Áp dụng định luật II Newton cho cho chuyển động theo phương , ta có: d xp d 2x M m u dt dt Thay xp 313\* MERGEFORMAT (.) biểu thức 12 vào biểu thức 13 suy ra: M d 2x d ( x  l sin  )  m u dt dt 414\* MERGEFORMAT (.) Khai triển đạo hàm biểu thức 14 rút gọn ta được:  M  m  x&& ml  sin   &2  ml (cos  )&& u 515\* MERGEFORMAT (.) Mặt khác, áp dụng định luật II Newton cho chuyển động quay lắc quanh trục ta được: m d xp dt l cos   m d yp dt l sin   mgl sin  616\* MERGEFORMAT (.) Thay 12 vào 16 suy ra:  d  x p  l sin     d  l cos    m  l cos   m  l sin   mgl sin  dt dt     717\* MERGEFORMAT (.) Khai triển đạo hàm biểu thức 17 rút gọn ta ta được: & mg sin  &cos   ml& mx& 818\* MERGEFORMAT (.) Từ 15 18, ta tính được: g u  ml (sin  )   mg cos sin  x M  m  m(cos  ) gg g u cos   ( M  m) g sin   ml (cos  sin  )   ml (cos  )  ( M  m)l gg 919\* MERGEFORMAT (.) Cách 1: Dùng công thức Euler – Lagrange: Động vật nặng đầu lắc: NHẬN DẠNG VÀ ĐIỀU KHIỂN HỆ THỐNG CHƯƠNG PHƯƠNG TRÌNH VI PHÂN VÀ CÁC THƠNG SỐ HỆ THỐNG g g g 1 g g m xP  m yP  m( x  l  cos  )  m(l  sin  ) 2 2 2 g g g g 1  TP  m xP  ml x  cos   ml  2 TP  10110\* MERGEFORMAT (.) Động xe: g Tc  M x 2 11111\* MERGEFORMAT (.) Động hệ thống: T  TP  TC  g g g g 1 ( M  m) x  ml x  cos   ml  2 12112\* MERGEFORMAT (.) Thế hệ thống vật nặng đầu lắc: U  mgl cos  13113\* MERGEFORMAT (.) Toán tử Lagramgian: L  T U  g g g g 1 ( M  m) x  ml x  cos   ml   mgl cos  2 14114\* MERGEFORMAT (.) Phương trình Euler – Lagrange : d  L  L F   dt   xg   x  d   L  L 0   dt   g     15115\* MERGEFORMAT (.) Thay 114 vào 115 gg gg g ( M  m) x  ml cos    ml (sin  )   u gg gg m x cos   ml   mg sin   16116\* MERGEFORMAT (.) Suy ra: g u  ml (sin  )   mg cos  sin  x M  m  m(cos  ) gg g u cos   ( M  m) g sin   ml (cos  sin  )   ml (cos  )  ( M  m)l gg 17117\* MERGEFORMAT (.) NHẬN DẠNG VÀ ĐIỀU KHIỂN HỆ THỐNG CHƯƠNG PHƯƠNG TRÌNH VI PHÂN VÀ CÁC THƠNG SỐ HỆ THỐNG Như phương trình tốn học suy từ cách khác giống 1.2 THÀNH LẬP HỆ PHƯƠNG TRÌNH CỦA HỆ THỐNG Tiến hành chuyển phương trình tìm 117 dạng chuẩn:  x1     f1      f  d d x d & x  f ( x, u, t )          dt dt  x3  dt  x   f         x&  f   x4  18118\* MERGEFORMAT (.) Đặt biến trạng thái g x1   , x2   g x3  x, x4  x 19119\* MERGEFORMAT (.) Trong đó: f1  x2 f2  u cos x1  ( M  m) g sin x1  ml (cos x1 sin x1 ) x2 ml (cos x1 )  ( M  m)l f  x4 f4  u  ml (sin x1 ) x2  mg cos x1 sin x1 M  m  m(cos x1 )2 20120\* MERGEFORMAT (.) Phương trình trạng thái phi tuyển x2    xg   2    u cos x1   M  m  g sin x1  ml  cos x1 sin x1  x2   g   ml  cos x1    M  m  l  x2      g  x4   x3    u  ml  sin x1  x2  mg cos x1 sin x1  g    x4    M  m  m  cos x1    21121\* MERGEFORMAT (.) Phương trình tuyến tính hóa quanh điểm cân thẳng đứng (góc lệch nhỏ 10 o, x0  0, u0  ) NHẬN DẠNG VÀ ĐIỀU KHIỂN HỆ THỐNG CHƯƠNG THIẾT KẾ BỘ ĐIỀU KHIỂN LQR theta_d = ; theta = 0.1; x_d = 0; x = 0; %% Ma Tran so lieu tuyen tinh hoa A = [ 0; 10.78 0 0; 0 1; -0.98 0 0]; B = [0; -1 ; 0; 1]; C = [1 0 0; 0 0]; %% Ma tran so Q = [q11 0 0; 0; 0 q33 0; 0 1]; R = r1; %% Tinh toan ma tran LQR P = care(A,B,Q,R); K = lqr(A,B,Q,R); Tiến hành biên dịch chương trình ta tìm nghiệm Ricatti liên tục: 182.5688 56.7551 10.7009   22.3931  56.7551  17.8215 3.4063 7.1205   P  10.7009 3.4063 2.4109 2.4063    2.4063 4.7096   22.3931 7.1205 K   34.3620  10.7009  1.0000  2.4109 30130\* MERGEFORMAT (.) Xây dựng Simulink NHẬN DẠNG VÀ ĐIỀU KHIỂN HỆ THỐNG CHƯƠNG THIẾT KẾ BỘ ĐIỀU KHIỂN LQR Hình 2.3 Bộ điều khiển LQR cho hệ lắc ngược phần mềm mơ Simulink Hình 2.4: Biểu đồ góc quay vận tốc góc hệ thống trường hợp NHẬN DẠNG VÀ ĐIỀU KHIỂN HỆ THỐNG 10 CHƯƠNG THIẾT KẾ BỘ ĐIỀU KHIỂN LQR Hình 2.5: Đồ thị vị trí vận tốc hệ thống trường hợp Hình 2.6: Đồ thị lực tác dụng vào hệ thống trường hợp Trường hợp 2: Ta tiến hành chọn thông số Q R sau NHẬN DẠNG VÀ ĐIỀU KHIỂN HỆ THỐNG 11 CHƯƠNG THIẾT KẾ BỘ ĐIỀU KHIỂN LQR 1 0 Q 0  0 0 0  100   0 1 0 R 1 31131\* MERGEFORMAT (.) Code matlab By Phan Minh Dien %%Tim thong so P va K clc; %% Khai bao cac thong so thong so mo hinh M = 1; m = 0.1; l = 1; g = 9.81; q11 = 1; q33 =100; r1 = 1; %% cai dat gia tri khoi tao theta_d = ; x_d = 0; x = 0; theta = 0.1; %% Tuyen tinh hoa A = [ 0; 10.78 0 0; 0 1; -0.98 0 0]; B = [0; -1 ; 0; 1]; C = [1 0 0; 0 0]; %% Ma tran so Q = [q11 0 0; 0; 0 q33 0; 0 1]; R = r1; %% Tinh toan ma tran LQR P = care(A,B,Q,R); K = lqr(A,B,Q,R); Tiến hành biên dịch chương trình ta tìm nghiệm Ricatti liên tục:  769.1196  243.9052   P  221.0906    173.7696   K   70.1356  243.9052 77.5354  70.5670  55.4263   22.1091  221.0906  173.7696  70.5670   55.4263  110.5142   60.5670   60.5670   44.3749  10.0000  11.0514 32132\* MERGEFORMAT (.) NHẬN DẠNG VÀ ĐIỀU KHIỂN HỆ THỐNG 12 CHƯƠNG THIẾT KẾ BỘ ĐIỀU KHIỂN LQR Hình 2.7: Biểu đồ góc quay vận tốc góc hệ thống trường hợp Hình 2.8: Đồ thị vị trí vận tốc hệ thống trường hợp NHẬN DẠNG VÀ ĐIỀU KHIỂN HỆ THỐNG 13 CHƯƠNG THIẾT KẾ BỘ ĐIỀU KHIỂN LQR Hình 2.9: Đồ thị lực tác dụng vào hệ thống trường hợp 2.2 THIẾT KẾ BỘ LỌC KALMAN 2.2.1 Cơ sở lý thuyết Bộ lọc Kalman thực phương pháp truy hồi chuỗi giá trị đầu vào bị nhiễu, nhằm tối ưu hóa giá trị ước đốn trạng thái hệ thống Bộ lọc Kalman ứng dụng rộng rãi kỹ thuật, phổ biến ứng dụng định hướng, định vị điều khiển phương tiện di chuyển Hình 2.10: Sơ đồ khối lọc Kalman Trong giá trinh thể sau NHẬN DẠNG VÀ ĐIỀU KHIỂN HỆ THỐNG 14 CHƯƠNG THIẾT KẾ BỘ ĐIỀU KHIỂN LQR  xˆ& Axˆ  t   Bu  t   L  y  t   yˆ  t     yˆ  t   Cxˆ  t  33133\* MERGEFORMAT (.) Với L   C T RN1 A   AT   C T RN1C   QN  34134\* MERGEFORMAT (.) 2.2.2 Xây dựng lọc Kalman Hình 2.11: Xây dụng lọc Kalman Simulink Kết thu sử dụng lọc Kalman Hình 2.12: Biểu đồ góc quay hệ thống ngẫu nhiêu có lọc NHẬN DẠNG VÀ ĐIỀU KHIỂN HỆ THỐNG 15 CHƯƠNG THIẾT KẾ BỘ ĐIỀU KHIỂN LQR Hình 2.13: Biểu đồ vị trí hệ thống ngẫu nhiêu có lọc 2.3 THIẾT KẾ BỘ LQG 2.3.1 Cơ sở lý thuyết Bộ điều khiển LQG kết hợp lọc Kalman, tức hàm ước lượng tuyến tính bậc hai (LQE), với điều chỉnh tuyến tính bậc hai (LQR) Nguyên tắc tách biệt đảm bảo chúng thiết kế tính tốn độc lập Điều khiển LQG áp dụng cho hệ thống tuyến tính thời gian bất biến hệ thống tuyến tính thời gian biến đổi Việc áp dụng vào hệ thống tuyến tính biến đổi theo thời gian tiếng Việc áp dụng hệ thống tuyến tính thời gian biến đổi cho phép thiết kế điều khiển phản hồi tuyến tính cho hệ thống phi tuyến khơng ổn định Bài tốn LQG đặt tìm tín hiệu điều khiển đầu x(t0 )  x0 trạng thái cuối x (t f )  u( t ) điều chỉnh hệ thống từ trạng thái cho tiêu hàm chất lượng tối thiểu Cách giải toán điều khiển LQG Nguyên lý tách rời: Bài toán tối ưu LQG giải cách giải riêng toán điều khiển tối ưu tiền định toán ước lượng trạng thái tối ưu: LQG  LQR  Kalman 35135\* MERGEFORMAT (.) Giả thiết toán thiết kế điều khiển LQG cho hệ lắc ngược: - Hệ thống hoạt động miền tuyến tính Giả sử đo góc lệch lắc vị trí xe Có nhiễu tác động vào hệ thống Nhiễu đo vị trí xe có phương sai 0.01, nhiễu đo góc lệch lắc có phương sai 0.001 Dùngng lọc Kalman để ước lượng trạng thái lọc nhiễu NHẬN DẠNG VÀ ĐIỀU KHIỂN HỆ THỐNG 16 CHƯƠNG THIẾT KẾ BỘ ĐIỀU KHIỂN LQR Ta có Bộ lọc Kalman:  xˆ& Axˆ  t   Bu  t   L  y  t   yˆ  t   a  yˆ  t   Cxˆ  t  1 0  C  0 0 36136\* MERGEFORMAT (.) Độ lợi ước lượng L   C T RNT 37137\* MERGEFORMAT (.) Phương trình Ricatti A   AT   C T RN1C   QN  38138\* MERGEFORMAT (.) Dựa vào điều kiện đưa phần giả thuyết, ta chọn thông số a 1 0 QN  0.000001  0  0 0 0  0.01  RN   0.01  0  0  1 39139\* MERGEFORMAT (.) Code Matlab: %% By Phan Minh Dien clc; %% Khai bao bo thong so mo hinh M = 1; m = 0.1; l = 1; g = 9.81; q11 = 1; q33 =100; r1 = 1; %% Cai dat gia tri khoi tao thetadot_init = 0; theta_init = pi/18; xdot_init = 0; x_init = 0; %% Tuyen so lieu tinh hoa A = [ 0; 10.78 0 0; 0 1; -0.98 0 0]; B = [0; -1 ; 0; 1]; C = [1 0 0; 0 0]; %% Ma tran so Q = [ q11 0 0; 0; 0 q33 0; 0 1]; R = r1; NHẬN DẠNG VÀ ĐIỀU KHIỂN HỆ THỐNG 17 CHƯƠNG THIẾT KẾ BỘ ĐIỀU KHIỂN LQR %% Ma tran LQR P = care(A,B,Q,R) K = lqr(A,B,Q,R) %% Bo loc Kalman G=[1 0 0;0 0;0 0;0 0 1]; Qn=0.000001*G; Rn=[0.001 0;0 0.01]; %% Ma tran LQG L = lqe(A,G,C,Qn,Rn) Tiến hành biên dịch ta thu thơng số phương trình Ricatti sau:  0.0066 0.0215 0.0006 0.0020   0.0215 0.0707 0.0019 0.0064    0.0006 0.0019 0.0015 0.0013     0.0020 0.0064 0.0003 0.0006  40140\* MERGEFORMAT (.)  6.5617 0.0517   21.5437 0.1876   L  0.5713 0.1470     1.9568 0.0271   769.1196  243.9052   P  221.0906    173.7696   41141\* MERGEFORMAT (.) 173.7696  55.4263  60.5670   44.3749  K   70.1356  22.1091  10.0000  11.0514 243.9052 221.0906  77.5354  70.5670   70.5670  110.5142   55.4263   60.5670   42142\* MERGEFORMAT (.) NHẬN DẠNG VÀ ĐIỀU KHIỂN HỆ THỐNG 18 CHƯƠNG THIẾT KẾ BỘ ĐIỀU KHIỂN LQR 2.3.2 Xây dựng LQG Hình 2.14: Bộ điều khiển LQG cho hệ lắc ngược Thơng số nhiễu  Hình 2.15: Thơng số khối nhiễu góc quay Thơng số nhiễu x NHẬN DẠNG VÀ ĐIỀU KHIỂN HỆ THỐNG 19 CHƯƠNG THIẾT KẾ BỘ ĐIỀU KHIỂN LQR Hình 2.16: Thơng số khối nhiễu vị trí Kết mơ phỏng: Hình 2.17: Biểu đồ góc quay vận tốc gốc hệ lắc ngược sử dụng điều khiển LQG trường hợp có nhiễu NHẬN DẠNG VÀ ĐIỀU KHIỂN HỆ THỐNG 20 CHƯƠNG THIẾT KẾ BỘ ĐIỀU KHIỂN LQR Hình 2.18: Biểu đồ vị trí vận tốc hệ lắc ngược sử dụng điều khiển LQG trường hợp có nhiễu Hình 2.19: Biểu đồ lực tác đơngj hệ lắc ngược sử dụng điều khiển LQG trường hợp có nhiễu NHẬN DẠNG VÀ ĐIỀU KHIỂN HỆ THỐNG 21 CHƯƠNG KẾT QUẢ MÔ PHỎNG VÀ NHẬN XÉT Chương 3.NHẬN XÉT Nhờ lọc Kalman ước lượng trạng thái lọc nhiễu, nên đáp ứng hệ thống điều khiển LQG cho tín hiệu tốt •Khi tín hiệu vị trí góc lệch gửi đến xử lí mà có thành phần nhiễu khơng mong muốn lúc điều khiển LQR khơng đáp ứng ngõ từ ta phải thành lập điều khiển LQG gồm LQR+ lọc Kalman để lọc nhiễu NHẬN DẠNG VÀ ĐIỀU KHIỂN HỆ THỐNG 22 TÀI LIỆU THAM KHẢO ... (.) NHẬN DẠNG VÀ ĐIỀU KHIỂN HỆ THỐNG CHƯƠNG THIẾT KẾ BỘ ĐIỀU KHIỂN LQR Chương 2.THIẾT KẾ BỘ ĐIỀU KHIỂN LQR 2.1 THIẾT KẾ BỘ LQR 2.1.1 Mơ hình hố hệ lắc ngược phương trình tốn học Dựa vào phương trình... 60.5670   42142* MERGEFORMAT (.) NHẬN DẠNG VÀ ĐIỀU KHIỂN HỆ THỐNG 18 CHƯƠNG THIẾT KẾ BỘ ĐIỀU KHIỂN LQR 2.3.2 Xây dựng LQG Hình 2.14: Bộ điều khiển LQG cho hệ lắc ngược Thơng số nhiễu  Hình 2.15: Thơng... VÀ ĐIỀU KHIỂN HỆ THỐNG 20 CHƯƠNG THIẾT KẾ BỘ ĐIỀU KHIỂN LQR Hình 2.18: Biểu đồ vị trí vận tốc hệ lắc ngược sử dụng điều khiển LQG trường hợp có nhiễu Hình 2.19: Biểu đồ lực tác đơngj hệ lắc ngược