Điều khiển xe cân bằng hai bánh dùng dsp tms320f28335

ĐẠI HỌC QUỐC GIA TP.HCM TRƯỜNG ĐẠI HỌC BÁCH KHOA _o0o _ BK TP.HCM NGUYỄN LÝ TUYẾT HƯƠNG ĐIỀU KHIỂN XE CÂN BẰNG HAI BÁNH DÙNG DSP TMS320F28335 CHUN NGÀNH: TỰ ĐỘNG HĨA Niên khóa: 2008 LUẬN VĂN THẠC SĨ Tp Hồ Chí Minh, tháng 12 năm 2010 CƠNG TRÌNH ĐƯỢC HỒN THÀNH TẠI TRƯỜNG ĐẠI HỌC BÁCH KHOA ĐẠI HỌC QUỐC GIA TP.HỒ CHÍ MINH Cán hướng dẫn khoa học: TS NGUYỄN VĨNH HẢO (ghi rõ họ tên, học hàm, học vị chữ ký) Cán chấm nhận xét 1: (ghi rõ họ tên, học hàm, học vị chữ ký) Cán chấm nhận xét 2: (ghi rõ họ tên, học hàm, học vị chữ ký) Luận văn thạc sĩ bảo vệ HỘI ĐỒNG CHẤM BẢO VỆ LUẬN VĂN THẠC SĨ TRƯỜNG ĐẠI HỌC BÁCH KHOA, ngày ….tháng…năm 2010 TRƯỜNG ĐẠI HỌC BÁCH KHOA KHOA ĐIỆN – ĐIỆN TỬ CỘNG HOÀ Xà HỘI CHỦ NGHIà VIỆT NAM Độc Lập - Tự Do - Hạnh Phúc -oOo Tp HCM, ngày tháng năm NHIỆM VỤ LUẬN VĂN THẠC SĨ Họ tên học viên: NGUYỄN LÝ TUYẾT HƯƠNG Phái: ……NỮ……………… Ngày, tháng, năm sinh: 03-06-1966 Nơi sinh: Bến Tre Chuyên ngành: TỰ ĐỘNG HÓA MSHV: 01508340 1- TÊN ĐỀ TÀI: ĐIỀU KHIỂN XE CÂN BẰNG HAI BÁNH DÙNG DSP TMS320F28335 2- NHIỆM VỤ LUẬN VĂN: • Mơ hình hóa hệ Robot bánh tự cân • Mơ hệ thống robot bánh tự cân dùng Matlab/Simulink sử dụng giải thuật: điều khiển LQR, LQR thêm khâu tích phân vị trí,PI kết hợp giải thuật Back stepping,PI kết hợp Back stepping mạng thần kinh suy diễn mờ thích nghi ANFIS So sánh kết điều khiển • Xây dựng lọc Kalman mơ hình biến trạng thái để ước lượng giá trị góc nghiêng vận tốc góc nghiêng điều khiển thực đối tượng robot • Xây dựng điều khiển nhúng cho xe cân bánh sử dụng vi điều khiển thời gian thực TMS320F28335 Texas Instrument với giải thuật điều khiển: điều khiển LQR LQR kết hợp mạng thần kinh suy diễn mờ thích nghi ANFIS • Đánh giá kết dựa mơ hình thực nghiệm, so sánh kết hai điều khiển 3- NGÀY GIAO NHIỆM VỤ : 02-02-2010 4- NGÀY HOÀN THÀNH NHIỆM VỤ : 10-12-2010 5- HỌ VÀ TÊN CÁN BỘ HƯỚNG DẪN (Ghi đầy đủ học hàm, học vị ): TS NGUYỄN VĨNH HẢO Nội dung đề cương Luận văn thạc sĩ Hội Đồng Chuyên Ngành thông qua CÁN BỘ HƯỚNG DẪN (Họ tên chữ ký) TS Nguyễn Vĩnh Hảo CHỦ NHIỆM BỘ MÔN QUẢN LÝ CHUYÊN NGÀNH (Họ tên chữ ký) KHOA QL CHUYÊN NGÀNH (Họ tên chữ ký) GVC.TS Nguyễn Đức Thành PGS.TS Nguyễn Hữu Phúc Nguyễn Lý Tuyết Hương  Luận văn Thạc sĩ LỜI CẢM ƠN Xin gởi lời cảm ơn chân thành tới TS Nguyễn Vĩnh Hảo, người tận tình hướng dẫn, bảo giúp đỡ tác giả suốt thời gian thực luận văn Và quan trọng hết, TS Nguyễn Vĩnh Hảo người gợi mở cho tác giả đề tài, hướng nghiên cứu mẻ, độc đáo, có tính ứng dụng cao Bên cạnh tác giả xin chân thành cảm ơn quý thầy cô môn Điều Khiển Tự Động trang bị cho tác giả kiến thức quý báu để hoàn thành tốt luận văn Lời cảm ơn cuối xin dành cho người thân, bạn bè hỗ trợ tác giả suốt thời gian thực luận văn Sự giúp đỡ bạn mặt chuyên môn nguồn động lực không nhỏ cho tác giả suốt thời gian qua Nguyễn Lý Tuyết Hương TÓM TẮT LUẬN VĂN Xe cân hai bánh nghiên cứu nhiều giới đặc tính mơ hình tóan có tính phi tuyến cao, làm tảng cho nhiều thiết kế tự động hóa, theo nhiều giải thuật điều khiển: LQR, điều khiển trượt, điều khiển mờ, điều khiển back stepping; điều khiển mạng thần kinh nhân tạo….Sự phát triển việc chế tạo tích hợp mạch điện tử lẫn công nghệ phần mền tạo chip vi điều khiển thời gian thực có tốc độ tính tóan xử lý cao DSP LMS320F28335 hổ trợ mạnh cho việc thiết kế nhúng giải thuật điều khiển lý thuyết vào thực tế Luận văn trình bày đáp ứng mơ giải thuật điều khiển LQR, Bacs stepping, mạng thần kinh suy diễn mờ ANFIS nhúng giải thuật điều khiển vào mơ hình thực xe hai bánh tự cân di chuyển địa hình Trong luận văn có trình bày giải thuật điều khiển mang thần kinh suy diễn mờ thích nghi dùng để cập nhật online trọng số mô hình để đối tượng thích nghi tốt với tác động tác động bên ngồi khơng biết sai số mơ hình tóan Kết điều khiển thực minh chứng tính ưu việt mạng thần kinh suy diễn mờ thich nghi ABSTRACT Balanced two-wheeled vehicles have been extensively studied in the world because mathematical modeling characteristic are highly nonlinear, on the basis of many design automation, according to various control algorithms: LQR, sliding mode control, fuzzy control, back stepping control and artificial neural network …The growth in manufacturing integrated circuits and software technologies have created the real time microcontroller chip having high calculated speed like DSP TMS320F28335,which supported for the design and embedded control algorithms into practice This thesis presents simulation responses of algorithms such as LQR control, back stepping, adaptive neural fuzzy inference system and embedded control algorithms into the seft balancing two wheeled vehicle moving over the terrain This thesis presents a new control algorithm ‘Adaptive neural fuzzy inference system ‘ is used to updates online weights of the object model Real control results demonstrate the superiority of system adaptive neural fuzzy inference Luận văn Thạc Sĩ Nguyễn Lý Tuyết Hương   MỤC LỤC Trang Chương 1: Tổng quan………………………………………………… 1.1 Giới thiệu hệ robot bánh tự cân bằng……………… …… … 1.2 Tìm hiểu xe hai bánh cân ………………………… ….… 1.3 Các cơng trình nghiên cứu liên quan ……………………… 1.3.1 JOE – A mobile inverted pendulum………………… 1.3.2 Nbot Legway ……………………………………… … 1.3.3 Balancing robot(Bbot) …………………………… ……….4 1.3.4 Balibot ……………………………………………….…….5 1.3.5 Điều khiển xe cân hai bánh dùng tự chỉnh định PID 1.3.6 Grepl ………………………………………………… … 1.4 Giới thiệu cấu hình mơ hình xe hai bánh tự cân luận văn …………………………………………………………… 1.4 Phần cứng ……………………………………………… 1.4.2 Mục tiêu luận văn ……………………… 10 1.5 Nội Dung luận văn thạc sỹ ……………………… 12 Chương 2: Xây dựng mơ hình tóan hệ thống robot…………………………13 2.1 Động chiều ……………………… 13 2.2 Hệ phương trình trạng thái cho xe cân hai bánh ……….… 15 2.3 Khảo sát ảnh đặc tính phi tuyến nhiễu lên đối tượng …….….25 Chương 3: Cơ sở lý thuyết…………………………………………………….30 3.1 Lý thuyết mạng thần kinh nhân tạo …………………………… 30 3.1.1 Tế bào thần kinh ………………………………………… 31 3.1.2 Phân loại mạng thần kinh nhân tạo ……………………….34 3.1.3 Các phương pháp huấn luyện mạng thần kinh nhân tạo… 34 3.1.4 Mạng truyền thẳng lớp…………………………… 36 3.1.5 Mạng truyền thẳng nhiều lớp …………………………… 38 Luận văn Thạc Sĩ Nguyễn Lý Tuyết Hương   3.1.6 Cấu trúc mạng hàm sở xuyên tâm…………………45 3.1.7 Mạng thần kinh suy diễn mờ thích nghi ……………….….49 3.2 Phương pháp ổn định Lyapunov ………………………………51 3.3 Phương Pháp điều khiển chiếu Back stepping ………… 54 Hệ mờ …………………………………………………………56 3.4.1 Sơ đồ khối …………………………………………………56 3.4.2 Mờ hóa ……………………………………………………56 3.4.3 Giải mờ ……………………………………………………59 3.5 Cơ sở lý thuyết lọc Kalman ………………………………… 72 3.5.1 Giới thiệu lọc Kalman …………………… ….… 72 3.5.2 Quá trình ước lượng………………………………… … 73 3.5.3 Bản chất xác suất lọc….………………… …… 75 3.5.4 Thuật toán Kalman rời rạc… …………………… …… 75 Chương 4: Giải thuật điều khiển cho hệ robot bánh tự cân bằng………….79 4.1 Cấu trúc hệ thống điều khiển……………………………… …… 79 4.2 Mơ hình tóan hệ thống robot ………………………… …… 79 4.3 Giải thuật điều khiển PI ………………………… ……… ….… 81 4.4 Giải thuật điều khiển Back stepping ………………………… ….88 4.5 Giải thuật mạng thần kinh suy diễn mờ thích nghi ANFIS ….… 91 4.6 Sơ đồ giải thuật Backstepping+PI+Neural ….………………… 97 Chương 5:Đáp ứng mơ hình thực…………………………………………… 105 5.1 Giới thiệu mơ hình robot luận văn…………………… 105 5.2 Thiết kế mơ hình robot bánh tự cân bằng………………………105 5.3 Bộ lọc Kalman……………………………………………………113 5.4 Hệ thống điều khiển nhúng DSP……………….……………… 118 5.4 Tóm lược chương……………………………………….……… 123 Chương 6: Kết đạt hướng phát triển ……………………….…124 6.1 Kết đạt được…………………………………………………124 6.2 Một số hạn chế……………………………………………… …125 Luận văn Thạc Sĩ Nguyễn Lý Tuyết Hương   6.3 Hướng phát triển…………………………………………………126 Phụ lục 1……………………………………………………………………127 Phụ lục 2……………………………………………………………………133 Tài liệu tham khảo CHƯƠNG Tổng Quan CHƯƠNG : TỔNG QUAN 1.1 ĐẶT VẤN ĐỀ: Từ lâu người quan tâm đến tính độc đáo hệ thống lắc ngược tính chất khơng ổn định hệ thống.Việc triển khai hệ thống thông minh để kiểm sóat ổn định hệ thống cần thiết Thế giới nghiên cứu, phát minh hòan thiện nhiều lọai robot hai chân (dựa mơ hình lắc ngược) để thay người công việc Điều khiển cân xe hai bánh dùng điều khiển tín hiệu số DSP TMS320F28335 nghiên cứu kết hợp tính xử lý mạnh, mền dẻo, tốc độ xử lý cao, có khả xử lý dấu chấm động giúp cải thiện khả tính tóan số thực, ưu điểm lớn tạo tiền đề cho việc nhúng giải thuật điều khiển đại DSP vào việc kiểm sóat khống chế, điều khiển, ổn định hệ thống thực tế Mục tiêu cuối đề tài điều khiển xe hai bánh tự xử lý tình phát sinh để tự cân theo chương trình điều khiển.Thơng qua đề tài, giới thiệu công nghệ xử lý tín hiệu số IC DSP, cảm biến dùng hệ thống cân gyroscope IDG-500, ADXL 202 Mơ hình xe có hai bánh đặt dọc trục với (khác với xe đạp trục hai bánh xe song song) Trên mơ hình sử dụng cảm biến để đo góc nghiêng thân xe, vận tốc quay sàn xe quanh trục bánh vận tốc di chuyển xe Nhờ cảm biến chương trình điều khiển ( điều khiển LQR kết hợp mạng ANFIS) mà xe tự giữ cân di chuyển.Với cấu trúc này, trọng tâm mơ hình phải ln nằm vùng đỡ bánh xe để cân di chuyển bề mặt địa hình từ đơn giản đến phức tạp 1.2 Tìm hiểu xe hai bánh cân : Đối với xe ba bốn bánh, việc thăng ổn định chúng nhờ trọng tâm nằm mặt phẳng chân đế bánh xe tạo Còn xe hai CBHD : TS Nguyễn Vĩnh Hảo HVTH : Nguyễn Lý Tuyết Hương -1- CHƯƠNG Kết Luận Hướng Phát Triển - Trìển khai giải thuật điều khiển thích nghi ANFIS vào hệ thống robot xe hai bánh tự cân - Trong giải thuật điều khiển neural mờ thich nghi, tác giả nhận thấy ảnh hưởng số qui luật mờ mạnh neural: tăng số luật mờ lên hệ thống đáp ứng tốt ổn định hơn, học thích nghi tốt Tác giả xây dựng lọc Kalman biến kết hợp liệu từ hai cảm biến đo gia tốc góc nghiêng vận tốc góc nghiêng thu để dưa giá trị góc nghiêng xác phục vụ cho giải thuật điều khiển, triển khai thực tế lý thuyết điều khiển vào mơ hình thực Việc nhúng giải thuật điều khiển vào vi điều khiển TMS320F28335 mở đường cho nghiên cứu ứng dụng sau tính linh họat, hổ trợ mạnh thiết kế, lập trình họ vi điều khiển C2000 hãng Texas Mơ hình thực vận hành thực tế chức năng: Swing up, giữ cân bằng, di chuyển tới lui, quay phải, quay trái với giải thuật LQR, LQR kết hợp mạng thần kinh suy diễn mờ thích nghi Với kết mơ chi tiết, kết điều khiển thực tốt, chúng tỏ giải thuật điều khiển giải thuật mạng thần kinh suy diễn mờ thích nghi tác giả triển khai đúng, đáp ứng mục tiêu luận văn đề Như vậy, đến tác giả hòan thành nhiệm vụ giao luận văn Tuy nhiên, trình thực luận văn, khả thời gian nên luận văn có bị số hạn chế 6.2 Một số hạn chế: Vận tốc di chuyển robot chưa Mach điều khiển chưa ổn áp nguốn cấp cho motor DC nên việc điều khiển bị ảnh hưởng nhiều Chưa khảo sát ảnh hưởng việc thay đổi thời gian lấy mẫu điều khiển đối tượng Chưa kịp khảo sát điều khiển PI+Back stepping+mang thần kinh suy diễn mờ thích nghi Chưa khảo sát mơ hình thực địa hình khác CBHD : TS Nguyễn Vĩnh Hảo HVTH : Nguyễn Lý Tuyết Hương 125 CHƯƠNG Kết Luận Hướng Phát Triển Đây kit vi điều khiển nhỏ nên nhớ Ram bị giới hạn, chương trình điều khiển lớn phải tính thêm việc mở rộng nhớ để chương trình điều khiển không bị tràn Để khắc phục hạn chế trên, tác giả đưa hướng phát triển cho nhà nghiên cứu để phát triển ngành tự động hóa Việt Nam 6.3 Hướng phát triển: Thiết kế mạch viết thêm chương trình kiểm sóat nguồn pin để hệ thống họat động ổn định Khi di chuyển địa hình tự nhiên, cần khảo sát thêm hệ số ma sát, cách thay đổi momen quay động để thích nghi với địa hình Nghiên cứu thêm cảm biến hồng ngọai để tăng khả thơng minh xử lý tình hụống đối tượng Mở rộng nhớ kit vi điều khiển để chương trình điều khiển mạnh Với đề nghị hướng phát triển trên, tác giả mong kết đạt luận văn sử dụng làm tiền đề cho nghiên cứu sâu lĩnh vực xe hai bánh tự cân nói riêng ngành tự động hóa Việt Nam nói chung CBHD : TS Nguyễn Vĩnh Hảo HVTH : Nguyễn Lý Tuyết Hương 126 Phụ Lục I PHỤ LỤC Mơ hình tóan hệ thống robot: Theo lý thuyết chương trên, ta có mơ hình tóan cho hệ thống phi tuyến sau: 2k m k e (I p + M p l )θ p − 2k m Rr x + Rr Va = (2M w + 2I w r 2k m Va + M p gl sin θ p = − M p lx cos θ p R + M p )x + 2k m k e Rr x + M p lθ p cos θ p − M p lθ p sin θ p (2.31) (2.32) Hệ phương trình trạng thái tuyến tính hệ xe hai bánh cân quanh điểm cân bằng: ⎡0 ⎢ ⎡ x ⎤ ⎢ 2k m k e (M p lr − Ip − M p l ) x ⎥ ⎢0 ⎢ Rr 2α ⎢ φ ⎥ = ⎢0 ⎢ ⎥ 2k m k e (rβ − M p l) ⎣φ⎦ ⎢ ⎢0 Rr 2α ⎣ β = (2M w + 2I w r2 0⎤ ⎡ ⎤ ⎥ ⎢ 2k (I + M l2 − M lr) ⎥ x p p ⎥ 0⎥ ⎡⎢ x ⎤⎥ ⎢ m p ⎥ α ⎢ ⎥ V (2.35) α Rr + 1⎥ ⎢ φ ⎥ ⎢ ⎥ a  ⎢ ⎥ φ M p glβ ⎥ ⎣ ⎦ ⎢ 2k m (M p l − rβ) ⎥ 0⎥ ⎢ ⎥ α Rrα ⎣ ⎦ ⎦ M2p gl2 I ⎤ ⎡ α = ⎢ I pβ + 2M p l2 (M w + w2 ) ⎥ r ⎦ ⎣ + M p ); Ta qui đổi hai hệ phương trình phi tuyến tuyến tính gốc tọa độ φ quanh điểm cân bằng: θ = π + φ vào hệ phương trình phi tuyến (2.31) (2.32), ta được: (I p + M p l )θ p − 2k m Rr 2k m k e x + Rr Va = (2M w + 2I w r 2k m R Va − M p gl sin φ = M p lx cos φ + M p )x + 2k m k e Rr x − M p lθ p cos φ + M p lθ p sin φ (2.36) (2.37) Từ (2.37), ta có: B3 * x = − 2k m k e Rr x + M p lθ p cos φ − M p lθ p sin φ + 2km V Rr a (2.38) Thế vào phương trình (2.36) (Ip + Mpl2 )φ− 2kmke Rr x + 2km R Va − Mpglsin φ = Mplcos φ CBHD : TS Nguyễn Vĩnh Hảo 2k k 2k φcos φ− m2 e x − Mplφ sin φ+ m Va {Mpl B3 Rr Rr HVTH : Nguyễn Lý Tuyết Hương 127 Phụ Lục I [A1* B3 − (M p * l * cos φ)2 ] φ=+ 2k m k e Rr  * r − Mp * l * cos φ) x(B − (M p * l)2 φ sin φ * cos φ + 2k m Rr (2.39) Va (M p * l* cos φ − r * B3) + M p *g *l* B3 *sin φ Tương tự, ta có: 2km * ke *km Mp *l*cos φ x − M p *l* φ sin φ + Va + Mp*l*  x *cos φ R *r A1 R *r (2.40) Mp *l*cos φ *km * ke * km { x − Va + Mp*g *l*sin φ} + A1 R *r R B3 *  x=− Suy ra: [A1* B3 − (Mp *l*cos φ)2 ]*  x= − Mp *l* φ sin φ * A1 + 2km * ke * x *(r * Mp *l*cos φ − A1) R * r2 2km * Va (A1 − Mp *l* r *cos φ) + (Mp *l)2 *g *sin φ *cos φ R *r (2.41) Đặt : A1= Ip+Mp*l2 ; A2= 2*km*ke/(R*r) ; A3 = A4 = km/R ; A5 = Mp*g*l*sinφ ; A6 = Mp*l*cosφ ; A7=A1*B3-(Mp*l*cosφ)2 B1 = B2 = km/R*r ; B3 = 2*Mw+2*Iw/r2+Mp ; B4 = 2*km*ke/(R*r2) ; B5 = Mp*l*cosφ ; B6 = Mp * l * φ * sin φ ; Z1 = B3 − A * B5 A1 Z2 = A1 − ; A * B5 ; B3 Ta hệ phương trình phi tuyến(đã đổi gốc trục tọa độ trùng hệ phương trình tuyến tính) CBHD : TS Nguyễn Vĩnh Hảo HVTH : Nguyễn Lý Tuyết Hương 128 Phụ Lục I ⎡0 ⎢ B5 *A2 ⎡x ⎤ ⎢ B4 − ⎢ ⎢ ⎥ A1 ⎢ x⎥ ⎢0 − Z ⎢ ⎥ ⎢ ⎢φ ⎥ = ⎢0 ⎢ ⎥ ⎢ ⎢ ⎢ A6 *B4 φ⎥ A2 − ⎢ ⎥ ⎢ ⎣ ⎦ ⎢0 B3 ⎢ Z2 ⎢⎣ 0 0 ⎤ ⎤ ⎡ ⎥ ⎥ ⎢ ⎡ ⎤ ⎥ ⎡x ⎤ ⎢ B5 *A3 B5*A4 ⎥ ⎢ ⎥ B1− B2 − ⎥ B6 ⎥ ⎢ ⎥ ⎢ ⎢ ⎥ B *A 5 A1 A1 − ⎥ ⎢x ⎥ ⎢ ⎥ ⎡Var ⎤ ⎢ ⎥ Z1 ⎥ Z1 Z1 ⎥ ⎢ ⎥ ⎢ A7 ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ φ⎥ + ⎢ ⎥⎢ ⎥+⎢ ⎥ 0 ⎥⎢ ⎥ ⎢ ⎥⎢ ⎥ ⎢ ⎥ ⎥ ⎢⎥ ⎢ ⎥ ⎣ Val ⎦ ⎢ A6 *B6 ⎥ φ ⎢ A6 *B1 A6 *B2 A5 *B3 ⎥ ⎥ ⎢ ⎥ − − A A ⎢ ⎥ ⎣ ⎦ ⎥ ⎢ ⎥ 3 B B B A ⎢ ⎥ − ⎢ ⎥ ⎣ ⎦ Z2 ⎥ Z2 Z2 ⎥⎦ ⎢⎣ ⎥⎦ Triển khai hệ phi tuyến theo vận tốc góc bánh xe trái bánh xe phải thực sau: Dùng định luật Newton, ta có: δ = (Hl − Hr ) * Ta có: δ = D * Ipdel r (θ − θ wr ) ; D wl (D.1) x= * r * (θ wl + θ wr ) Ta có: M w * x l = − I km * ke  km θ wl + V al − w θ wl − H l R *r R *r r (D.2) M w * x r = − I km * ke  km θ wr + V ar − w θ wr − H r R *r R *r r (D.3) (D.3)-(D.2), ta có: H l − H r = M w * (x r − x l ) − I km * ke  km (θ wl − θ wr ) + (Val − Var ) − w (θ wl − θ wr ) (D.4) R *r R *r r Từ (D.1) (D.4), ta có: I δ= r (θ −θ  ) = D {−M *r(θ −θ  ) − km*ke (θ −θ ) + km (V − V ) − w (  )} (D.5) θwl −θ wl wr w wl wr wl wr al ar wr D 2*Ipdel R*r R*r r Suy ra: Iw Iw * r * Ipdel * r * Ipdel   θwl ( + + = θ + + M * r ) ( M * r ) w wr w r r D2 D2 km * ke  km − (θwl − θ wr ) + (Val − Var) R *r R *r CBHD : TS Nguyễn Vĩnh Hảo (D.6) HVTH : Nguyễn Lý Tuyết Hương 129 Phụ Lục I Theo hệ phương trình phi tuyến (I), ta có: r x = (θ wl + θ wr ) = A 22 x + A 24 φ + B 21 * Var + B 22 Val + f r  r (θ wl + θ wr ) = A 22 (θ wl + θ wr ) + A 24 φ + B 21 * Var + B 22 Val + f 2 Đặt M = ( (D.7) Iw * r * Ipdel + M * r + ) , ta được: w r D2 A A 22 km * ke A22 km * ke  θwl = θ wl ( − + ) + θ wr ( ) + 24 φ r 2*R *r *M 2*R *r *M B22 km B21 km f4 + Val ( + ) + Var ( − )+ r r r 2*R *r *M 2*R *r *M (D.8) A A22 km * ke A22 km * ke  θwr = θ wl ( + − ) + θ wr ( ) + 24 φ r 2*R *r *M 2*R *r *M B22 km B21 km f4 + Val ( − ) + Var ( + )+ r r r 2*R *r *M 2*R *r *M (D.9) Với: A22 = − B4 − B5 * A2 A1 = − * km * ke * A1 − Mp * l * cos φ * * km * ke * r Z1 R * r * {A1* B3 − (Mp * l * cos φ) } M p * l * φ * A1* sin φ B6 A24 = − =− Z1 A1* B3 − (M * l * cos φ) p B21 = B22 = B1 − B5 * A3 A1 = km * A1 − Mp * l * km * r * cos φ Z1 R * r{A1* B3 − (M p * l * cos φ) 2 A5 * B5 M p * l * g * cos φ * sin φ = f4 = A7 A1* B3 − (M p * l * cos φ) Đặt biến trạng thái sau X=[ θ wl ,.θ wr , φ , φ ] , ta hệ phương trình phi tuyến biểu diễn robot: CBHD : TS Nguyễn Vĩnh Hảo HVTH : Nguyễn Lý Tuyết Hương 130 Phụ Lục I km * ke ⎡ A 22 ⎢ − 2*R *r *M ⎡θ ⎤ ⎢ ⎢ wl ⎥ ⎢ ⎢ ⎥ ⎢ A 22 + km * ke ⎢θ wr ⎥ ⎢ 2*R *r *M ⎢ ⎥=⎢ ⎢  ⎥ ⎢ A * B4 ⎢ φ ⎥ ⎢ A2 − B3 * r ⎢ ⎥ ⎢ * Z2 ⎢⎣ φ ⎥⎦ ⎢ ⎢ ⎢⎣ A 22 km * ke + 2*R *r *M A 24 r A 22 km * ke − 2*R *r *M A 24 r A * B4 B3 * r * Z2 A2 − − km ⎡ B22 ⎢( r + * R * r * M ) ⎢ ⎢ km ⎢( B22 − ) ⎢ r 2*R *r *M +⎢ ⎢ A * B2 − A4 ⎢ ⎢ ( B3 ) Z2 ⎢ ⎢ ⎢⎣ A * B6 B3 * Z2 ⎤ 0⎥ ⎥ ⎡ θ ⎤ ⎥ ⎢ wl ⎥ 0⎥ ⎢  ⎥ ⎥ ⎢θ wr ⎥ ⎥⎢ ⎥ + ⎥⎢  ⎥ ⎥⎢ φ ⎥ 0⎥ ⎢ ⎥ ⎥ ⎢⎣ φ ⎥⎦ ⎥ 0⎥⎦ B21 km ⎤ − )⎥ ⎡ f4 ⎤ 2*R *r *M r ⎥ ⎥ Val ⎢ r ⎤ ⎡ ⎥ ⎢ ⎥ ⎢ ⎥ B21 km ⎥ ⎢ f4 ⎥ + ( )⎥ ⎢ ⎥ ⎢ * R * r * M ⎥⎢ r ⎥+⎢ r ⎥ ⎥ ⎢ ⎥ ⎢ ⎥ ⎥ A6 * B1 ⎢ ⎥ ⎢ A5 * B3 ⎥ − A3 ⎥ ⎢ ⎥ ⎢ B ( ) ⎥ ⎢Var ⎥ ⎢ A7 ⎥⎥ ⎦ Z2 ⎥⎣ ⎢ ⎥ ⎥ ⎥⎦ ⎢ ⎣ ⎥⎦ ( Tuyến tính hóa hệ thống quanh điểm cân zero: cos φ = 1;sin φ = φ; ( ⎡ A22tt km*ke ⎢ − 2 *R *r *M ⎡ θwl ⎤ ⎢ ⎢ ⎢ ⎥ ⎢ ⎥ ⎢ A22tt km*ke + ⎢θwr ⎥ ⎢ 2 *R *r *M ⎢ ⎥=⎢  ⎢ ⎢ φ ⎥ ⎢ ⎥ ⎢ km*ke**(r * β − Mp*l) ⎢⎣ φ ⎥⎦ ⎢ R *r * α ⎢ ⎢ ⎣ A22tt km*ke + 2 *R *r *M A22tt km*ke − 2 *R *r *M km*ke*(r * β − Mp*l) R *r * α 0 CBHD : TS Nguyễn Vĩnh Hảo dφ ) = 0; dt M2p *l2 *g ⎤ ⎥ r *α ⎥ ⎡ θ ⎤ ⎥ ⎢ wl ⎥ M2p *l2 *g ⎥ ⎢  ⎥ ⎥ ⎢θwr ⎥ r *α ⎥⎢ ⎥+ ⎥ ⎢ φ ⎥ Mp *g *l* β ⎥ ⎢ ⎥ ⎥ ⎢⎣ φ ⎥⎦ α ⎥ ⎥ ⎦ HVTH : Nguyễn Lý Tuyết Hương 131 Phụ Lục I ⎡ km*(I + M *l2 − M *l *r) km p p p ⎢( + ) 2*R *r *M ⎢ R * r *α ⎢ ⎢ km*(I + M *l2 − M *l *r) km p p p ⎢( − ) ⎢ 2* R * r *M R * r *α ⎢ ⎢ km*(Mp *l − r *β) ⎢ ( ) ⎢ R * r *α ⎢ ⎢ ⎣ Với: A22tt = β = (2M w + M=( * km * ke * (M p * l * r − I p − M p * l ) 2I w r2 ⎤ km − )⎥ ( 2* R * r *M ⎥ R * r *α ⎥⎡Val⎤ ⎥⎢ ⎥ km*(Ip + Mp *l2 − Mp *l * r) ⎢ ⎥ km + )⎥⎥⎢ ⎥ ( 2*R *r * M ⎢ ⎥ R * r *α ⎥ ⎥⎢ ⎥ km*(Mp*l − r *β ⎥⎢ ⎥ ) ( ⎥⎢⎣Var⎥⎦ R * r *α ⎥ ⎥ ⎦ km*(Ip + Mpl2 − Mp *l *r) R * r2 *α + M p ); ; I ⎤ ⎡ α = ⎢ I pβ + 2M p l2 (M w + w2 ) ⎥ r ⎦ ⎣ Iw * r * Ipdel + M * r + ) w r D2 CBHD : TS Nguyễn Vĩnh Hảo HVTH : Nguyễn Lý Tuyết Hương 132 PHỤ LỤC Cập nhật thông số Fuzzy neural PHỤ LỤC 2: CẬP NHẬT THÔNG SỐ FUZZY NEURAL Xét trường hợp biến vào trạng thái mờ/ngõ vào, ta có mạng neural 16 luật: − X1: ( x1 − c11 )2 μ11 = e X3: − μ12 = e − X2: δ11 − ( x − c 31 ) − ( x − c 32 ) − ( x − c 41 ) − ( x − c 42 ) μ 31 = e ( x1 − c12 ) 2 δ12 μ 32 = e δ 231 δ 232 ( x − c 21 ) δ 221 μ 21 = e − μ 22 = e ( x − c 22 ) X4: μ 41 = e δ 222 μ 42 = e CBHD : TS Nguyễn Vĩnh Hảo δ 241 δ 242 HVTH : Nguyễn Lý Tuyết Hương 133 PHỤ LỤC Cập nhật thông số Fuzzy neural Luật : 01) w1*u11*u21*u31*u41/M 02) w2*u11*u21*u31*u42/M 03) w3*u11*u21*u32*u41/M 04) w4*u11*u21*u32*u42/M 05) w5*u11*u22*u31*u41/M 06) w6*u11*u22*u31*u42/M 07) w7*u11*u22*u32*u41/M 08) w8*u11*u22*u32*u42/M 09) w9*u12*u21*u31*u41/M 10) w10*u12*u21*u31*u42/M 11) w11*u12*u21*u32*u41/M 12) w12*u12*u21*u32*u42/M 13) w13*u12*u22*u31*u41/M 14) w14*u12*u22*u31*u42/M 15) w15*u12*u22*u32*u41/M 16) w16*u12*u22*u32*u42/M Với M = u11*u21*u31*u41+ u11*u21*u31*u42 + u11*u21*u32*u41 + u11*u21*u32*u42 + u11*u22*u31*u41 + u11*u22*u31*u42 + u11*u22*u32*u41 + u11*u22*u32*u42 + u12*u21*u31*u41 + u12*u21*u31*u42 +u12*u21*u32*u41 + u12*u21*u32*u42 + u12*u22*u31*u41 + u12*u22*u31*u42 + u12*u22*u32*u41 + u12*u22*u32*u42 = u11*u21*u31*(u41+u42) + u11*u21*u32*(u41+u42) + + u11*u22*u31*(u41+u42) + u11*u22*u32*(u41+u42) + u12*u21*u31*(u41+u42) + u12*u21*u32*(u41+u42) + u12*u22*u31*(u41+u42) + u12*u22*u33*(u41+u42) = (u41+u42)[ u11*u21*(u31+u32) + u11*u22*( u31+u32)+ CBHD : TS Nguyễn Vĩnh Hảo HVTH : Nguyễn Lý Tuyết Hương 134 PHỤ LỤC Cập nhật thông số Fuzzy neural + u12*u21*(u31 +u32) + u12*u22*( u31+u32)] = (u41 + u42)*(u31 + u32)*[ u11(u21+u22) + u12( u21+u22)] M = (u41 + u42)*(u31 + u32)*(u21 + u22)*(u11 +u22) *Cập nhật trọng số ngõ ra: ∂E + α * Δw ( t − 1) ∂w w ( t + 1) = w ( t ) + Δw ( t ) Δw ( t ) = − η * Với: ∂E ∂E ∂h ∂ζ ∂ζi = ∂w i ∂h ∂ζ ∂ζ i ∂w i ∂E = h * (−1) * 1* Zq ∂w i Trong đó: Zq = ui * u j * uk * ul M *Cập nhật thông số tâm Cij: ΔC ij ( t ) = −η * ∂E + α * ΔC ij ( t − 1) ∂Cij C ij ( t + 1) = C ij ( t ) + ΔC ij ( t ) Với * * ∂μ ij ∂C ij = 2* ( x i − C ij ) δ ij2 ∂E ∂E ∂h ∂ζ ∂ζ ij ∂μ ij = ∂C ij ∂h ∂ζ ∂ζ ij ∂μ ij ∂C ij ∂ζ ij 2( xi − Cij) ∂E = h * (−1) * 1* * ∂C ij ∂μ ij δ2 ij ′ a '*b − b'*a ⎛a⎞ Áp dụng công thức: ⎜ ⎟ = b2 ⎝b⎠ Ví dụ: ζ 11 = w1* u11 * u 21 * u 31 * u 41 w * u11 * u 21 * u 31 * u 42 + M M CBHD : TS Nguyễn Vĩnh Hảo HVTH : Nguyễn Lý Tuyết Hương 135 PHỤ LỤC Ỉ Cập nhật thông số Fuzzy neural ∂ζ 11 ∂μ11 + w * u11 * u 21 * u 32 * u 41 w * u11 * u 21 * u 32 * u 42 + M M + w * u11 * u 22 * u 31* u 41 w * u11* u 22 * u 31* u 42 + M M + w * u11* u 22 * u 32 * u 41 w * u11* u 22 * u 32 * u 42 + M M 2 1 [ { = (∑ μ 2i * ∑ μ 3i * ∑ μ 4i ) * μ12 * μ 21 * μ 31 ( w1* μ 41 + w * μ 42 ) { + μ 32 ( w * μ 41 + w * μ 42 ) } + μ 22 * μ 31 * ( w * μ 41 + w * μ 42 ) + μ 32 ( w * μ 41 + w * μ 42 ) }]=A Tương tự trên, ta thiết lập ∂ζ ij ∂μ ij i=1,4 ; j=1,2 *Cập nhật thông số δij: Δδ ij ( t ) = −η * ∂E + α * Δδ ij ( t − 1) ∂δij δ ij ( t + 1) = δ ij ( t ) + Δδ ij ( t ) Với * * ∂μ ij ∂δ ij = 2* μ ij δ ij3 * ( x i − C ij ) ∂E ∂E ∂h ∂ζ ∂ζ ij ∂μ ij = ∂δ ij ∂h ∂ζ ∂ζ ij ∂μ ij ∂δ ij ∂ζ ij 2μ ij ( x i − C ij ) ∂E = h * (−1) * * * ∂δ ij ∂μ ij δ3 ij CBHD : TS Nguyễn Vĩnh Hảo HVTH : Nguyễn Lý Tuyết Hương 136 Tài Liệu Tham Khảo TÀI LIỆU THAM KHẢO 1- TS.Huỳnh Thái Hoàng,” Hệ Thống Điều Khiển Thông Minh “,Nhà xuất Đại Học Quốc Gia TP HCM ,2006 2- G.S Dương Hòai Nghĩa,”Điều khiển Hệ Thống Đa Biến “,Nhà xuất Đại Học Quốc Gia TP HCM ,2007 3- Rich Chi Ooi,”Balancing a Two-Wheeled Autonomous Robot “, The university of Western Austrlia School of Mechanical Engineering ,2003 4- Eric Minnaert,Brian Hemmelman,Dan Dolan,”Inverted Pendulum design With Hardware Fuzzy Logic controller”, 5- S.W.Nawawi,M.N Ahmad & H.S Osman ;”Real-Time Control of a Two-wheeled Inverted Pendulum Mobile Robot” ,Proceedings of World Academemy of science,Engineering and technology volume 29 may 2008 ISSN 1307-6884 6- Felix Grasser,Aldo D’Arrigo,Silvio Colombi,Alfred C.Rufer ,”JOE:A mobile,Inverted Pendulum” ,IEEE TRANSACTIONS ON INDUSTRIAL ELECTRONICS,VOL 49,NO.1,FEBRUARY 2002 7- Yorihisa Yamamotor,”NXT way-GS Model-Based Design-control of self-balancing two-wheeled robot built with LEGO Mindstorms NXT”,CYBERNET SYSTEM CO.,LTD 8-Nawawi S.W,Ahmad M.N,Osman J.H.S,”Control of Two-wheels Inverted Pendulum Mobile robot Using Full Order Sliding Mode Control”, Proceedings of international Conference on Man-Machine system 2006,September 15-16-2006, Langkawi, Malaysia 9- Nawawi S.W,Ahmad M.N,Osman J.H.S,”Control of Two-wheels Inverted Pendulum Mobile robot Using PISMC”, Faculty of Electrical Engineering,University Technology Malaysia 10- Wei-SongLin,”Adaptive Critic Neuro-Fuzzy Control of Two-wheel Vehicle”,2006 IEEE International Conference on Fuzzy System Sheraton Vancouver Wall Centre Hotel,Vancouver,BC,Canada,July 16-21,2006 11- Valluru B Rao, “ C++ Neural Network and Fuzzy Logic ”, M&T Books, IDG Books Worldwide, Inc 12-Xiaogang Ruan,Jianxian Cai,”Fuzzy Backstepping for Two-Wheeled Self-Balancing Robot”,2009 International Asia Conference on Informatics in Control ,Automation and Robotics 13-T.S.Li,B.G Hong and G.Y.Shi,”DSC-backstepping based robust adaptive NN control for strict-feeback nonlinear system via small gain theorem “,Int.J.System,control and Communication,Vol.1,No.1,2008 14-Ching-Hung Lee,Jen-Chieh Chien,Hao-Han Chang,Che-Ting Kuo,and Hua-Hsiang Chang,”Direct Adaptive Backstepping Control for a Class of MIMO Non-affine CBHD : TS Nguyễn Vĩnh Hảo HVTH : Nguyễn Lý Tuyết Hương Tài Liệu Tham Khảo System Using Recurrent Neural Networks”, Proceedings of the International MutiConference of Engineers and Computer Scientists 2009 Vol I IMECS 2009, March 18-20,2009,Hong Kong 15-Tsai-Jiun Ren,Tien-Chi Chen,Chun-Jung Chen,”Motion control for a two-wheeled vehicle using a self-tuning PID controller”,Control Engineering Practice 16(2008) 365-375 16-Jin Seok Noh,Geun Hyung Lee,and Seul Jung,” Position Control of a Mobile Inverted Pendunlum System Using Radial Basis Function Network”,International Journal of Control,Automation and Systems(2010) 8(1):157-162 DOI 10.1007/s 12555-010-0120-0 17-Jose Luis Corona Miranda,”APPLICATION OF KALMAN FILTERING AND PID CONTROL FOR DIRECT INVERTED PENDULUM CONTROL”,A project Presrnted to the Faculty of California State University,Chico,2009 18-DAN SIMON,” Kalman Filtering”,Embedded systems Programming,June 2001 19-Albert-Jan Baerveldt and Robert Klang,”A Low-cost and Low-weight Attitude Estimation System for an Autonomous Helicopter “,Center for Computer System Archtecture,Halmsad University,Box 823,S-30118 Halmsad,Sweden 20-Sanghyuk Park and Jonathan How,”Examples of Estimation Filters from Recent Aircraft Projects at MIT”,November 2004 21- Texas instrument : datashhet DSP TMS320F28335 22- Yung C Shin, Chengying Xu, “ Intelligent Systems –Modeling, Optimization and Control”, CRC Press Taylor& Francis Group 23- 15-S.N Sivanandam, S.Sumathi and S.N Deepa, “ Introduction to Fuzzy Logic using MATLAB ”, Springer 24-Leonid Reznik, “ Fuzzy Controllers ”, Newnes – Oxford, 1997 CBHD : TS Nguyễn Vĩnh Hảo HVTH : Nguyễn Lý Tuyết Hương LÝ LỊCH TRÍCH NGANG Họ tên : NGUYỄN LÝ TUYẾT HƯƠNG Phái Ngày sinh : 03/06/1966 Địa liên lạc : Điện thoại : Nữ Nơi sinh : BẾN TRE 430/71G ĐIỆN BIÊN PHỦ P.17 Q.BÌNH THẠNH TP.HCM : 0908.106.603 QUÁ TRÌNH ĐÀO TẠO Từ 1984-1989 sinh viên trường đại học Bách Khoa thành phố Hồ Chí Minh chun ngành Tự động hóa( khoa Điện Điện tử) 2008-2010: Học viên Cao Học trường Đại Học Bách Khoa chun ngành Tự Động Hóa Q TRÌNH CƠNG TÁC Từ 1990-2000: Nhân viên kỹ thuật công ty Cơ Điện Thủ Đức Từ 2000 đến 2010: làm việc công ty TNHH Máy Công Cụ Và Thiết Bị TAT Nay : Làm việc công ty TNHH Máy công nghiệp HTD CBHD : TS Nguyễn Vĩnh Hảo HVTH: Nguyễn Lý Tuyết Hương ... Thuật Điều Khiển Giải thuật sử dụng hai khối điều khiển PD: khối PD điều khiển vị trí bánh xe, khối PD điều khiển góc nghiêng robot Tổng hai khối điều khiển PD dùng để điều khiển hệ cân xe robot hai. .. TÀI: ĐIỀU KHIỂN XE CÂN BẰNG HAI BÁNH DÙNG DSP TMS320F28335 2- NHIỆM VỤ LUẬN VĂN: • Mơ hình hóa hệ Robot bánh tự cân • Mô hệ thống robot bánh tự cân dùng Matlab/Simulink sử dụng giải thuật: điều khiển. .. Nbot dùng sensor quán tính encoder motor để cân xe Ý tưởng điều khiển để cân bằng: bánh xe phải chạy theo hướng mà phần robot ngã Các thông số ngõ vào để xác định họat động vị trí xe cân hai bánh