Điều khiển cân bằng con lắc ngược sử dụng bộ điều khiển LQR và bộ lọc kalman

Đ I H C ĐÀ N NG TR NGăĐ IăH CăBÁCHăKHOA CAOăXUỂNăC S NG ĐI UăKHI NăCỂNăB NGăCONăL C NG C D NG B ăĐI UăKHI N LQR VĨ B ăL CăKALMAN LU NăVĔNăTH CăSƾăK ăTHU T ĐƠăNẵngăậ Nĕmă2018 Đ I H C ĐÀ N NG TR NGăĐ IăH CăBÁCHăKHOA CAOăXUỂNăC S NG ĐI UăKHI NăCỂNăB NGăCONăL CăNG Că D NG B ăĐI U KHI NăLQRăVĨăB ăL CăKALMAN ChuyênăngƠnh:ăK ăthu tăđi uăkhi năvƠăt ăđ ngăhóa Mưăs :ă8520216 LU NăVĔNăTH CăSƾăK ăTHU T Ng iăh ngăd năkhoaăh c: TS.ăTR NăĐỊNHăKHỌIăQU C ĐƠăNẵngăậ Nĕmă2018 L IăCAMăĐOAN Tôi xin cam đoan cơng trình nghiên cứu riêng tơi Ngồi thơng tin sử dụng trích dẫn tài liệu tham khảo, kết nghiên cứu khác nêu luận văn trung thực chưa cơng bố cơng trình Tác giả luận văn CaoăXuơnăC ng ĐI UăKHI NăCỂNăB NGăCONăL CăNG CăS D NG B ăĐI UăKHI NăLQRă VĨăB ăL CăKALMAN H c viên: Cao Xuân C ng Chuyên ngành: Kỹ thuật điều khiển tự động hóa Mư số: 8520216 Khóa: K33 Tr ng Đ i h c Bách khoa - ĐHĐN Tómăt tăậ Luận văn trình bày việc thiết kế điều khiển cân lắc ng ợc sử dụng điều khiển lqr kết hợp với l c Kalman Kết thực nghiệm ph ơng pháp đề xuất đ ợc so sánh với điều khiển h i tiếp tr ng thái LQR để đánh giá chất l ợng điều khiển Hệ thống lắc ng ợc đ ợc xây dựng, mô đánh giá kết phần mềm Matlab - Simulink Dựa vào kết mô thu đ ợc từ phần mềm Matlab – Simulink tiến hành xây dựng l u đ , lập trình điều khiển mơ hình thực nghiệm đ ợc giám sát thông qua phần mềm Labview kết hợp với board m ch vi xử lý Arduino Kết thực nghiệm mơ hình cho thấy, đáp ứng hệ thống với điều khiển đề xuất cho kết tốt ph ơng pháp điều khiển LQR Với điều khiển lqr kết hợp với l c kalman, lắc có khả đ ợc điểu khiển cân t i v trí xác đ nh điều kiện có nhiễu đo l ng hệ lắc b dao động quanh điểm đặt với giải thuật điều khiển LQR T ăkhóaă- Con lắc ng ợc, điều khiển LQR, l c Kalman, phản h i v trí, bo m ch arduino BALANCE CONTROL OF AN INVERTED PENDULUM USING LQR AND KALMAN FILTER Abstract - This thesis presents the design of balance control for an inverted pendulum system using the Linear-Quadratic Regulator method (LQR) control law conbine with Kalman filter Experimental results of the proposed method was compared with LQR method to assess the quality of control Inverted pendulum system is designed, simulated and supervised by Matlab – Simulink software Based on simulation results obtained from Matlab – Simulink conducts programmatic, programming and control on experimental model by Labview software conbined with the Arduino single-board microcontrollers Experiments of stabilizing the inverted pendulum show that the proposed controller produces better response than the LQR controller The inverted pendulum can be stabilized at the predetermined position in the condition of noise measurement using the LQR controller combined with Kalman filter while The inverted pendulum fluctuates around the set point using the LQR controlalgorithm Key words - Inverted pendulum, LQR, Kalman filter, position feedback, arduino card M CăL C Trang phụ bìa Trang L i cam đoan Mục lục Danh mục từ viết tắt Danh mục hình Danh mục bảng Danh mục kí hiệu PH NăM ăĐ U I Đặt vấn đề II Đ nh h ớng đề tài III Nhiệm vụ luận văn IV Kết mong muốn đ t đ ợc PH NăN IăDUNG CH NGă1: GI IăTHI UăBĨIăTOÁNăCONăL CăNG C 1.1 Lý thuyết lắc ng ợc 1.2 Mơ hình tốn h c cho hệ lắc ng ợc 1.2.1 Mơ hình tốn h c lắc ng ợc 1.2.2 Mơ hình tốn h c động DC 1.2.3 Mơ hình tốn h c t ng hợp cho hệ lắc ng ợc 1.3 Mô lắc ng ợc matlab CH NG 2: PH NGăPHÁPăĐI UăKHI N 2.1 Ph ơng pháp điều khiển PID 12 2.2 Ph ơng pháp điều khiển LQR 13 2.2.1 Điều khiển tối u 13 2.2.2 Thiết kế điều khiển LQR 14 2.2.2.1 Điều khiển LQR liên tục 14 2.2.2.2 Thiết kế điều khiển LQR liên tục 16 2.3 Thiết kế điều khiển số 17 2.3.1 Xây dựng mơ hình gián đo n cho hệ lắc ng ợc 17 2.3.2 Thiết kế điều khiển LQR gián đo n 19 2.4 Bộ l c Kalman 21 2.4.1 Bộ l c Kalman r i r c 22 2.4.2 Bộ l c Kalman liên tục 23 2.5 Điều khiển swing-up 24 CH NGă3: THI TăK ăB ăĐI UăKHI NăCỂNăB NGăCONăL CăNG C 3.1 Thiết kế điều khiển LQR 26 3.1.1 Hệ ph ơng trình tr ng thái lắc ng ợc 26 3.1.2 Bộ điều khiển LQR 27 3.2 Thiết kế điều khiển LQR kết hợp l c Kalman 33 3.3 Mô đánh giá kết 38 CH NGă4: XỂYăD NGăB ăĐI UăKHI N CONăL CăNG CăVĨăK TăQU ă TH CăNGHI M 4.1 Mơ hình lắc ng ợc 48 4.1.1 Giới thiệu bo m ch điều khiển Arduino UNO R3 50 4.1.2 Động truyền động 51 4.1.3 Hệ thống cảm biến l c sử dụng cho cảm biến 52 4.1.3.1 Cảm biến Encoder 52 4.1.3.2 Ph ơng pháp l c tín hiệu từ cảm biến dùng l c Kalman gián đo n (Discrete Kalman) 54 4.1.4 Thiết kế điều khiển LQR gián đo n 58 4.1.5 Điều khiển swing-up 60 4.1.6 Sơ đ kết nối thiết b 60 4.1.7 Thuật toán điều khiển lắc ng ợc dùng Arduino UNO R3 61 4.2 Giám sát mơ hình lắc ng ợc phần mềm LabVIEW 64 4.3 Kết thực nghiệm 66 4.3.1 Con lắc ho t động không tác động bên 66 4.3.2 Con lắc ho t động có tác động bên ngồi 67 PH NăK TăLU Nầầầầầầầầầầầầầầầầầầầầầầầầầầ 69 TĨIăLI U THAMăKH Oầầầầầầầầầầầầầầầầầầầầầầầầ70 PH ăL Cầầầầầầầầầầầầầầầầầầầầầầầầầầầầầầ 71 QUY TăĐ NHăGIAOăĐ TĨIăLU NăVĔNă(b n sao) ầầầầầầầầầầầầ.82 DANHăM CăCÁCăT ăVI TăT T PID : PROPOTIONAL INTEGRAL DERIVATIVE LQR : LINEAR QUADRATIC REGULATOR ZOH : ZERO ORDER HOLD DANHăM CăCÁCăHỊNH Tênăhình STT Trang Hìnhă1.1: Mơ hình lắc ng ợc Hìnhă1.2: Hình ảnh lắc ng ợc thực tế Hìnhă1.3: Các lực tác động vào hệ lắc 4 Hìnhă1.4: Sơ đ m ch điện t ơng đ ơng động điện chiều Hìnhă1.5ă: Mơ hình mơ lắc ng ợc Hìnhă1.6ă: Cấu trúc bên khối Mơ hình mơ lắc ng ợc Hìnhă1.7ă: Sơ đ mơ tả góc lệch lắc ng ợc 10 Hìnhă1.8ă: Sơ đ mơ tả v trí xe 10 Hìnhă 1.9ă :ă Đáp ứng góc θ hệ lắc ng ợc khơng có điều khiển 11 10 Hình 2.1 : Cấu trúc điều khiển PID 12 11 Hìnhă2.2: Ph ơng pháp điều khiển LQR 15 12 Hìnhă2.3: Mơ hình gián đo n hệ thống lắc ng ợc với khâu ZOH 17 13 Hìnhă2.4: Mơ hình gián đo n t ơng đ ơng hệ lắc ng ợc 18 14 Hìnhă2.5: Mơ hình gián đo n t ơng đ ơng hệ thống 20 15 Hìnhă2.6 : Sơ đ khối l c Kalman 21 16 Hìnhă2.7: Sơ đ khối l c Kalman r i r c 23 17 Hìnhă2.8: Sơ đ khối l c Kalman liên tục 24 18 Hìnhă3.1ă: Thuật tốn điều khiển LQR 30 19 Hìnhă3.2ă: Đáp ứng góc θ hệ thống Q(1,1)=1 Q(3,3)=1 31 20 Hìnhă3.3ă: Đáp ứng v trí x hệ thống Q(1,1)=1 Q(3,3)=1 31 21 Hìnhă3.4ă: Đáp ứng góc θ hệ thống Q(1,1)=500 Q(3,3)=1 32 22 Hìnhă3.5ă: Đáp ứng v trí x hệ thống Q(1,1)=500 Q(3,3)=1 33 23 Hìnhă3.6 : Đáp ứng góc θ hệ thống Q(1,1)=3000 Q(3,3)=3000 34 24 25 Hìnhă3.7ă: Đáp ứng v trí x hệ thống Q(1,1)=3000 Q(3,3)=3000 Hìnhă3.8ă: Bộ điều khiển LQR kết hợp l c Kalman 34 35 26 Hìnhă3.9ă: Đáp ứng góc θ hệ thống Q(1,1)=100 Q(3,3)=1 36 27 Hìnhă3.10ă: Đáp ứng v trí x hệ thống Q(1,1)=100 Q(3,3)=1 37 28 Hìnhă3.11ă: Đáp ứng góc θ hệ thống Q(1,1)=10 Q(3,3)=1000 38 29 Hìnhă3.12ă: Đáp ứng v trí x hệ thống Q(1,1)=10 Q(3,3)=1000 38 30 31 32 Hình 3.13 : Đáp ứng góc θ hệ thống Q(1,1)=1000 Q(3,3)=1000000 Hìnhă3.14ă: Đáp ứng v trí x hệ thống Q(1,1)=1000 Q(3,3)=1000000 Hìnhă3.15ă: Bộ điều khiển LQR LQG 39 40 41 Hìnhă3.16ă: 33 Đáp ứng hệ thống sử dụng điều khiển LQR Hìnhă3.17ă: 34 35 37 38 43 (với nhiễu � = 0.001, nhiễu xc = 0.1) Đáp ứng hệ thống sử dụng điều khiển LQR + Kalman Hìnhă3.22ă: 43 (với nhiễu � = 0.001, nhiễu xc = 0.01) Đáp ứng hệ thống sử dụng điều khiển LQR Hìnhă3.21ă: 42 (với nhiễu � = 0.001, nhiễu xc = 0.1) Đáp ứng hệ thống sử dụng điều khiển LQR + Kalman Hìnhă3.20ă: 42 (với nhiễu � = 0.001, nhiễu xc = 0.01) Đáp ứng hệ thống sử dụng điều khiển LQR Hìnhă3.19ă: 36 (với nhiễu � = 0.001, nhiễu xc = 0.01) Đáp ứng hệ thống sử dụng điều khiển LQR + Kalman Hìnhă3.18ă: 41 44 (với nhiễu � = 0.001, nhiễu xc = 0.1) 39 Đáp ứng hệ thống sử dụng điều khiển LQR 44 40 (với nhiễu � = 0.001, nhiễu xc = 0.1) 45 Hìnhă3.23ă: Đáp ứng hệ thống sử dụng điều khiển LQR + Kalman Hìnhă3.24ă: 41 Đáp ứng hệ thống sử dụng điều khiển LQR Hìnhă3.25ă: 42 44 45 46 48 48 (với nhiễu � = 0.1, nhiễu xc = 0.0) Đáp ứng hệ thống sử dụng điều khiển LQR Hìnhă3.31ă: 47 (với nhiễu � = 0.1, nhiễu xc = 0.0) Đáp ứng hệ thống sử dụng điều khiển LQR + Kalman Hìnhă3.30ă: 47 (với nhiễu � = 0.01, nhiễu xc = 0.01) Đáp ứng hệ thống sử dụng điều khiển LQR Hìnhă3.29ă: 46 (với nhiễu � = 0.01, nhiễu xc = 0.01) Đáp ứng hệ thống sử dụng điều khiển LQR + Kalman Hìnhă3.28ă: 46 (với nhiễu � = 0.01, nhiễu xc = 0.01) Đáp ứng hệ thống sử dụng điều khiển LQR Hìnhă3.27ă: 45 (với nhiễu � = 0.01, nhiễu xc = 0.01) Đáp ứng hệ thống sử dụng điều khiển LQR + Kalman Hìnhă3.26ă: 43 (với nhiễu � = 0.001, nhiễu xc = 0.1) 48 (với nhiễu � = 0.1, nhiễu xc = 0.0) 48 Đáp ứng hệ thống sử dụng điều khiển LQR + Kalman 49 49 (với nhiễu � = 0.1, nhiễu xc = 0.0) Hìnhă4.1: Mơ hình thực nghiệm lắc ng ợc 48 50 Hìnhă4.2:ăCác thiết b cần thiết mơ hình 49 51 Hìnhă4.3: Bo m ch Arduino UNO R3 50 52 Hìnhă4.4:ăĐộng DC servo NISCA - NF5475E 52 53 Hìnhă4.5: Sơ đ nguyên lý m ch cầu H 52 68 Hình 4.21 mơ tả dao động lắc ng ợc b tác động bên với lực tác động lớn làm xoay lắc Lúc hệ lắc b dao động m nh giống nh q trình swing up Tuy nhiên, lắc tr l i v trí cẩn khoảng th i gian ngắn Hìnhă4.22: Con lắc b tác động bên với lực tác động lớn làm xoay lắc Các kết thí nghiệm cho thấy mơ hình lắc ho t động n đ nh Đặc biệt lắc có khả tự cân khoảng th i gian ngắn kh i động nh b tác động 69 K TăLU N Nh vậy, với kết thu đ ợc từ mô thực nghiệm Đề tài “ĐI Uă KHI NăCỂNăB NGăCONăL CăNG CăS D NG B ăĐI UăKHI NăLQRă VĨăB ăL CăKALMAN” đư thực đ ợc nội dung sau:  Xây dựng mơ hình tốn h c lắc ng ợc  Xây dựng đ ợc mô hình mơ hệ lắc ng ợc phần mềm MATLAB để nghiên cứu đặc tính đặc tính làm việc, th i gian xác lập giải thuật  Xây dựng đ ợc mơ hình giám sát hệ thống LabView  Điều khiển cân cho hệ lắc ng ợc Bộ điều khiển mà đề tài đư nghiên cứu hoàn toàn đáp ứng đ ợc yêu cầu chất l ợng điều khiển cho lắc ng ợc Nh trình thực đề tài đư giải đ ợc vấn đề đặt Tuy nhiên, với th i gian nghiên cứu h n chế đề tài ch a đ t đ ợc mức tối u nhất, ch a nghiên cứu đ ợc nhiều điều khiển khác, ch a đề cập đến việc nhận d ng thông số lắc ng ợc mà ch n lắc ng ợc với thơng số biết tr ớc… Chính vậy, để đề tài hoàn thiện hơn, cần tập phát triển phần cứng, đ ng th i nghiên cứu kỹ việc nhận d ng thông số hệ thống lắc ng ợc nhằm thiết kế điều khiển ho t động với hệ lắc 70 TĨIăLI UăTHAMăKH O [1] Huỳnh Thái Hoàng – Nguyễn Th Ph ơng Hà, Lý thuyết điều khiển tự động, NXB ĐH Quốc Gia TP.HCM, 2005 [2] Bùi Quốc Khánh – Nguyễn Văn Liễn, Cơ sở truyền động điện, NXB Khoa H c Kỹ Thuật, 2006 [3] Trần Anh Dũng, Điều khiển đại lý thuyết ứng dụng, Nhà xuất Giao thông vận tải, 2013 [4] Nguyễn Th Ph ơng Hà, Lý thuyết điều khiển đại, Nhà xuất Đ i H c Quốc Gia Thành Phố H Chí Minh, 2007 [5] Huỳnh Thái Hồng, Hệ thống điều khiển thơng minh, Nhà xuất Đ i H c Quốc Gia Thành Phố H Chí Minh, 2006 [6] K.J Åstrưm, K Furuta, Swinging up a pendulum by energy control, Automatica, Volume 36, Issue 2, 2000, Pages 287-295, ISSN 0005-1098 [7] Nguyễn Phùng Quang, MATLAB Simulink dành cho kỹ sư điều khiển tự động, Nhà xuất Khoa h c Kỹ thuật, 2006 [8] Đặng Hữu Th nh, Thiết kế điều khiển lắc ngược, Tr ng đ i h c Bách khoa, 2014 [9] Greg Welch and Gary Bishop, An Introduction to the Kalman Filter, University of North Carolina at Chapel Hill, Chapel Hill, 2006 [10] Richard C.Doft, Robert H.Bishop, Modern control system, 12th edition, Prentice Hall, 2013 [11] Elisa Sara Varghese et al, Optimal control of inverted pendulum system using PID controller, LQR and MPC, IOP Conference Series: Materials Science and Engineering 263 052007, 2017 [12] Nguyễn Văn Khanh et al, Điều khiển cân lắc ngược sử dụng thuật tốn PD mờ, T p chí Khoa h c Tr ng Đ i h c Cần Thơ, 2013 [13] Richard C.Doft, Robert H.Bishop, Modern control system, 12th edition, Prentice Hall, 2013 71 PH ăL C Ph ăl că1: Ch ơng trình điều khiển Arduino #include #include #include SimpleKalmanFilter bo_loc(2, 2, 0.001); float x=0.0,x_k1,x_dot=0.0,x1; float teta=0.0,teta_k1,teta_dot=0.0,E=0,g=0.0; float u=0,k1=54,k2=42,k3=83,k4=9; Encoder x_Enco(2, 4); Encoder teta_Enco(3, 13); void setup() { pinMode(6,OUTPUT); pinMode(7,OUTPUT); pinMode(8,OUTPUT); Timer1.initialize(5000); Timer1.attachInterrupt(tinhtoan); Serial.begin(115200); Serial.println("LQR control inverted pendulum"); } long oldPosition_x = 0; long oldPosition_teta = -720; void loop() { if (teta =-0.35) { balance(); Serial.println("balance"); controlHbrige(); 72 } else { swing(); Serial.println("swing"); controlHbrige(); } //controlHbrige(); //x1=80*x; } void tinhtoan() { long newPosition_x = bo_loc.updateEstimate (x_Enco.read()); x_k1=x; if (newPosition_x != oldPosition_x) oldPosition_x = newPosition_x; x=oldPosition_x*0.0534/1000; x_dot=(x-x_k1)*200; long newPosition_teta =720+ bo_loc.updateEstimate (teta_Enco.read()); teta_dot= 200*3.14*(newPosition_teta-oldPosition_teta)/720; teta_k1=teta; if (newPosition_teta != oldPosition_teta) { oldPosition_teta = newPosition_teta; if (oldPosition_teta>720) { oldPosition_teta=oldPosition_teta-1440; teta_Enco.write(oldPosition_teta-720); } if (oldPosition_teta=0.20) { if (u>0) u=0; if (u0) u=0; if (u