ĐỒ ÁN PID CỦA THẦY NGUYỄN HẢI ĐÔNG

BỘ GIÁO DỤC VÀ ĐÀ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 ĐỒ ÁN TỐT NGHIỆP NGÀNH: CÔNG NGHỆ ĐIỆN TỰ ĐỘNG ĐỀ TÀI: MƠ HÌNH ROBOT HAI BÁNH TỰ CÂN BẰNG GVHD : TS NGUYỄN MINH TÂM SVTH: Cù Minh Phước Nguyễn Khắc Mẫn MSSV: 09118042 09118032 Tp Hồ Chí Minh 07/2013 Phần A: Giới thiệu ĐỒ ÁN ĐƯỢC HOÀN THÀNH TẠI TRƯỜNG ĐẠI HỌC SƯ PHẠM KỸ THUẬT TP HỒ CHÍ MINH PHẦN A Cán hướng dẫn: GIỚI THIỆU Sinh viên thực hiện: TS Nguyễn Minh Tâm Cù Minh Phước Nguyễn Khắc Mẫn Cán chấm phản biện: …………………………… Đồ án tốt nghiệp bảo vệ HỘI ĐỒNG CHẤM BẢO VỆ ĐỒ ÁN TỐT NGHIỆP TRƯỜNG ĐẠI HỌC SƯ PHẠM KỸ THUẬT TP HỒ CHÍ MINH Đồ án tốt nghiệp Trang Ngày tháng 07 năm 2013 TRƯỜNG ĐH SƯ PHẠM KỸ THUẬT CỘNG HÒA Xà HỘI CHỦ NGHĨA VIỆT NAM TP HỒ CHÍ MINH ĐỘC LẬP - TỰ DO - HẠNH PHÚC KHOA ĐIỆN - ĐIỆN TỬ Tp HCM, ngày … tháng năm 2013 NHIỆM VỤ ĐỒ ÁN TỐT NGHIỆP Họ tên sinh viên: Cù Minh Phước MSSV: 09118042 Nguyễn Khắc Mẫn MSSV: 09118032 Chuyên ngành: Điện tự động Mã ngành: 18 Hệ đào tạo: Đại học quy Mã hệ: Khóa: 2009 Lớp: 091180 I TÊN ĐỀ TÀI: MƠ HÌNH ROBOT HAI BÁNH TỰ CÂN BẰNG II NHIỆM VỤ VÀ NỘI DUNG: Đồ án tốt nghiệp Trang III NGÀY GIAO NHIỆM VỤ: 10/04/2013 IV NGÀY HOÀN THÀNH NHIỆM VỤ: 15/7/2013 V HỌ VÀ TÊN CÁN BỘ HƯỚNG DẪN:TS Nguyễn Minh Tâm CÁN BỘ HƯỚNG DẪN BM TỰ ĐỘNG ĐIỀU KHIỂN TS NGUYỄN MINH TÂM TS NGUYỄN MINH TÂM LỜI CẢM ƠN ˜&˜ Trong suốt trình thực đề tài, gặp phải nhiều khó khăn giúp đỡ, hỗ trợ kịp thời từ quý Thầy Cô bạn nên Đồ án tốt nghiệp hồn thành tiến độ Nhóm thực xin chân thành cảm ơn thầy Nguyễn Minh Tâm tận tình hướng dẫn, bảo kinh nghiệm quý báu hỗ trợ phương tiện thí nghiệm suốt trình tìm hiểu, nghiên cứu đề tài Đồng thời Nhóm thực xin chân thành cảm ơn Thầy Cô Khoa Điện - Điện Tử tạo điều kiện, cung cấp cho nhóm kiến thức bản, cần thiết để chúng em có điều kiện đủ kiến thức để thực trình nghiên cứu Bên cạnh đó, nhóm xin cảm ơn thành viên lớp 091180 có ý kiến đóng góp, bổ sung, động viên khích lệ giúp nhóm hồn thành tốt đề tài Ngồi ra, nhóm nhận bảo anh (chị) trước Các anh (chị) hướng dẫn giới thiệu tài liệu tham khảo thêm việc thực nghiên cứu Mặc dù nhóm thực đề tài cố gắng hoàn thiện đồ án, trình soạn thảo kiến thức hạn chế nên nhiều Đồ án tốt nghiệp Trang thiếu sót Nhóm thực đề tài mong nhận đóng góp ý kiến quý thầy cô bạn sinh viên Sau nhóm thực xin chúc Thầy sức khoẻ, thành công tiếp tục đào tạo sinh viên giỏi đóng góp cho đất nước Chúc anh (chị), bạn sức khỏe, học tập thật tốt để không phụ cơng lao Thầy Cơ giảng dạy Nhóm thực xin chân thành cảm ơn Trân trọng Nhóm thực đồ án Cù Minh Phước Nguyễn Khắc Mẫn Đồ án tốt nghiệp Trang NHẬN XÉT CỦA GIÁO VIÊN PHẢN BIỆN Tp HCM, ngày … tháng … năm 2013 Giáo viên phản biệnNHẬN XÉT CỦA GIÁO VIÊN HƯỚNG DẪN Đồ án tốt nghiệp Trang Tp HCM, ngày … tháng … năm 2013 Giáo viên hướng dẫn TS Nguyễn Minh Tâm Đồ án tốt nghiệp Trang PHẦN B NỘI DUNG Danh sách hình ảnh sử dụng đồ án: Hình 1.1: Robot dạng bánh xe di chuyển địa hình phẳng Hình 1.2: Robot dạng bánh xe xuống dốc Hình 1.3: Robot dạng bánh xe lên dốc Hình 1.4: Robot bánh di chuyển địa hình khác theo hướng bảo tồn thăng Hình 1.5: nBot Hình 1.6: JOE Hình 1.7: NXTway-GS LEGO MINDSTORMS Hình 1.8: Xe Segway I2, I2 cargo, X2 Adventure Hình 1.9: Xe Winglet Hình 1.10: Xe Iswing Hình 2.1: Mơ hình robot bánh tự cân mặt phẳng Hình 2.2: Mơ hình phi tuyến robot hai bánh tự cân Matlab Simulink Hình 2.3: Bên khối “Two Wheeled Balancing Robot (Non-Linear Model)” Hình 2.4: Bên khối “DeCoupling” Hình 2.5: Quy trình thực lọc Kalman Hình 2.6: Tổng quan chu trình thực lọc Kalman hồn chỉnh Hình 2.7: Cấu trúc điều khiển PID cho hệ robot hai bánh tự cân Hình 2.8: Cấu trúc bên điều khiển PID rời rạcvới thơng số cố định Hình 2.9: Robot hai bánh tự cân sử dụng điều khiển PID cố định Hình 2.10: Đáp ứng ngõ vị trí với tín hiệu đặt vị trí xung vng Đồ án tốt nghiệp Trang Hình 2.11: Đáp ứng ngõ góc nghiêng với tín hiệu đặt góc nghiêng xung vng Hình 2.12: Đáp ứng ngõ góc xoay với tín hiệu đặt góc xoay xung vng Hình 2.13: Đáp ứng ngõ vị trí với tín hiệu đặt vị trí sóng sine Hình 2.14: Đáp ứng ngõ góc nghiêng với tín hiệu đặt góc nghiêng sóng sine Hình 2.15: Đáp ứng ngõ góc xoay với tín hiệu đặt góc xoay sóng sine Hình 2.16: đáp ứng ngõ vị trí trường hợp tín hiệu đặt vị trí xung vng Hình 2.17: đáp ứng ngõ góc nghiêng trường hợp tín hiệu đặt góc nghiêng xung vng Hình 2.18: đáp ứng ngõ vị trí trường hợp tín hiệu đặt vị trí xung vng Hình 2.19: đáp ứng ngõ góc nghiêng trường hợp tín hiệu đặt góc nghiêng xung vng Hình 2.20: Pin Li-Po 11.1V/2200mA Hình 2.21: DSP TMS320F28335 Hình 2.22: Razor IMU 9DOF Hình 2.23: Sơ đồ khối cảm biến gia tốc ADXL345 Hình 2.24: Phương chiều trục gia tốc tương ứng Hình 2.25: Đáp ứng giá trị gia tốc ngõ tương ứng hướng trọng lực Hình 2.26: Sơ đồ chân ngõ cảm biến gia tốc Hình 2.27: Sơ đồ khối cảm biến ITG3200 Hình 2.28: Cảm biến HMC5883L Hình 2.29: Sơ đồ nguyên lý mạch cầu H Hình 2.30: Mạch cầu H thực tế Hình 2.31: Encoder Hình 2.32: Module Bluetooth HC06 slave Hình 2.33: Động Hình 3.1: Bản vẽ 3D mơ hình robot bánh tự cân Đồ án tốt nghiệp Trang Hình 3.2: Mơ hình robot thực tế Hình 3.3: Cấu trúc điều khiển phần cứng robot bánh tự cân Hình 3.4: Sơ đồ kết nối phần cứng Hình 3.5: Mơ hình lọc Kalman với biến trạng thái Hình 3.6: Mơ hình lọc Kalman Matlab Simulink Hình 3.7: Tín hiệu vào-ra lọc Kalman Hình 3.8: So sánh cho tín hiệu góc nghiêng ngõ vào lọc Kalman Hình 3.9: Lưu đồ giải thuật cho mơ hình robot bánh tự cân Hình 3.10: Thư viện Target Support Package cho DSP F28335 Hình 3.11: Sơ đồ tổng quát Hình 3.12: Khối đọc nút nhấn Hình 3.13: Bên khối calibre Hình 3.14: Khối đọc cảm biến IMU Hình 3.15: Khối lọc Kalman Hình 3.16: Tổng quát Khối điều khiển trung tâm Hình 3.17: điều khiển Bên Khối đọc xung encoder Hình 3.18: Bên Khối xuất tín hiệu Hình 3.19: Khối gửi liệu Hình 3.20: Ngõ góc nghiêng điện áp động chạy mơ hình thực tế Hình 3.21: Giao diện chương trình giao tiếp máy tính Danh sách bảng sử dụng đồ án : Bảng 2.1: Ký hiệu ý nghĩa đại lượng Bảng 3.1: Bảng giá trị đại lượng robot bánh tự cân Bảng 3.2: Các khối chức sử dụng chương trình Đồ án tốt nghiệp Trang Nó chạy ước lượng giá trị qua hàm giá trị: [ ][ ´ gyro−gyrobias X´ = angle = ´ bias gyro ] [3.5] Và cập nhật ma trận covariance qua hàm : ´ AP+ P A ' +Q P= [3.6] ´ A P+ P A' +Q lý thuyết là( P=A P A T +Q ) P= [3.7] ´ A Jacobian X với giá trị mong đợi : [ ´ d ( angle) d ( angle) A= d( gyro ´ bias ) d ( angle) ][ ´ d ( angle) d ( gyrobias ) = −1 d ( gyro ´ bias ) 0 d ( gyrobias ) ] [3.8] Để dễ dàng việc lập trình, ma trận P khai triển đến mức tối thiểu : [ ´ Q angle −P [ ][ ] −P [ ] [0] −P [ ] [1] P= −P [ ] [1] Q gyro [ ][ ] [3.9] ][ ] [ ][ Q ¿ −1 P [ ] [0] P [ ] [1] + P [ ] [0] P [ ] [1] 0 + angle 0 P [ ] [0] P [ ] [1] P [ ] [0] P [ ] [1] −1 0 Q gyro [ ][ ][ Q ¿ −P [ ] [ ] −P [ ][ ] + −P [ ][ ] + angle 0 Qgyro −P [ ][ ] ] ] [3.10] Lưu trữ giá trị ước lượng chưa bias gyro : rate=q=qm −q bias [3.11] (Với rate sai số góc tiên đốn) Cập nhật ước lượng góc : Đồ án tốt nghiệp Trang 56 ´ dt=angle+rate dt angle=angle+ angle [3.12] Cập nhật ma trận covariance : P=P+ P´ dt ][ [ ] Q −P[0][1]−P[1][0] −P [ ] [1] ¿ P [ ] [0] P [ ] [ 1] + angle dt P [ ] [0 ] P [ ] [1] −P [ ] [1] Q gyro [3.13]  “Giai đoạn cập nhật giá trị lọc Kalman“ Ma trận C ma trận 1x2 (giá trị x trạng thái), ma trận Jacobian giá trị đo lường với giá trị mong đợi Trong trường hợp C : [ C= ] d (angle m ) d ( angle) d (angle m ) = [1 ] d(gyrobias ) (C H) [3.14] Vì giá trị góc đáp ứng trực tiếp với góc ước lượng giá trị góc khơng quan hệ với giá trị gyro bias nên C_0 cho thấy giá trị trạng thái quan hệ trực tiếp với trạng thái ước lượng nào, C_1 cho thấy giá trị trạng thái không quan hệ với giá trị sở gyro ước lượng Error giá trị khác giá trị đo lường giá trị ước lượng Trong trường hợp này, khác hai gia tốc kế đo góc góc ước lượng angl e error=angle m−angle [3.15] Tính sai số ước lượng Từ lọc Kalman : [ ][ ] E=CP CT + R= [ ] P [ ][ ] P [ ] [ ] + R=P [ ] [ ] + R P [ ][ ] P [ ][ ] [3.16] PCt 0=C [ ] x P[0][0 ] [3.17] PCt 1=C [ ] x P[1][0] [3.18] Ước tính lọc Kalman đạt từ lý thuyết lọc Kalman : [ ][ K = PCt / E K1 PCt / E Đồ án tốt nghiệp ] [3.19] Trang 57 P=( 1− KH ) P=P−KHP=P−KCP [3.20] Ta có phép nhân điểm trơi (floating point) [ ] CP=[ ] P [ ] [ 0] P [ ] [1] =[ P [ ] [0] P [ ] [1] ]=[ t t ] P [ ] [ 0] P [ ] [1] [ ][ ] [ K 0.t K t P= P [ ] [0] P [ ] [1] − K [ t t ]=P− K1 K t K t P [ ] [0] P [ ] [1] ] [3.21] [3.22] Cập nhật giá trị ước lượng Lần nữa, từ Kalman : [ ] [ angle err X = angle = X + K ( Z measure −H X estimate )=X + K gyrobias angle err ] err angle err =q¿ [3.23] [3.24] 3.2.1.2 Mô lọc Kalman lập trình Sử dụng Matlab Simulink để kiểm tra tính đắn lọc Kalman lập trình phần 3.2.1.1 Mơ hình lọc Kalman Matlab Simulink xây dựng sau : Hình 3.6: Mơ hình lọc Kalman Matlab Simulink Khi cho nhiễu tác động, ngõ lọc Kalman lọc nhiễu tốt: Đồ án tốt nghiệp Trang 58 Hình 3.7: Tín hiệu vào-ra lọc Kalman 3.2.1.3 Ứng dụng lọc Kalman vào mơ hình thật Áp dụng lọc Kalman để ước lượng góc nghiêng so với điểm mơ hình robot bánh tự cân thực tế, chu kì lấy mẫu 1ms, ma trận trọng số chọn thử sai trình thực nghiệm kết tốt sau: Q= [0.001 0; 0.003]; R=0.08 dt=0.064 s Đồ án tốt nghiệp Trang 59 Kết quan sát hệ thống thực giá trị góc nghiêng đo từ cảm biến IMU sử dụng mơ hình Hình 3.8: So sánh cho tín hiệu góc nghiêng ngõ vào lọc Kalman ( đường màu xanh góc nghiêng chưa qua lọc, đường màu đỏ tín hiệu góc nghiêng sau qua lọc) Đồ án tốt nghiệp Trang 60 3.2.2 Lưu đồ giải thuật Hình 3.9: Đồ án tốt nghiệp Lưu đồ giải thuật cho mơ hình robot bánh tự cân Trang 61 3.2.3 Bộ điều khiển nhúng cho robot bánh tự cân 3.2.3.1 Giới thiệu Quá trình điều khiển robot thực vi điều khiển DSP TMS320F28335 với hỗ trợ thưviện Target Support Package Matlab –hỗ trợ cho vi điều khiển họ TI C2000 Bộ điều khiển thiết kế Simulink miền rời rạc,sau ứng dụng Embedded IDE Link biên dịch sang mã ngôn ngữ C tương thích với thư viện hỗ trợ Code Composer, sau tiếp tục Code Composer biên dịch thành mã máy nhúng vào DSP TMS320F28335 Hình 3.10 : Thư viện Target Support Package cho DSP F28335 3.2.3.2 Các khối chức sử dụng chương trình Biểu tượng Chức Nhận tín hiệu digital vào: nút start, stop, calibrate Xuất tín hiệu digital: đèn hiển thị Nhận tín hiệu truyền thơng UART: đọc cảm biến IMU, nhận lệnh từ module bluetooth HC06 Xuất tín hiệu truyền thơng UART: gửi giá trị máy tính để theo dõi, giám sát Điều chế độ rộng xung, đảo chiều động Đọc encoder Bảng 3.2: Đồ án tốt nghiệp Các khối chức sử dụng chương trình Trang 62 3.2.3.3 Chương trình Matlab Simulink Tổng qt điều khiển mơ hình robot bánh tự cân Matlab Simulink Hình 3.11: Sơ đồ tổng quát Bộ điều khiển bao gồm khối sau: Khối đọc nút nhấn: Start, Calibre Hình 3.12: Khối đọc nút nhấn Khối Calibre: cấp điện, robot chưa hoạt động liên tục cập nhật giá Đồ án tốt nghiệp Trang 63 trị offset Hình 3.13: Bên khối calibre Khối đọc cảm biến IMU: đọc phân tách giá trị cảm biến IMU Hình 3.14: Khối đọc cảm biến IMU Khối lọc Kalman: lấy thành phần gia tốc từ cảm biến gia tốc tính góc xoay alpha kết hợp với vận tốc góc từ trục đưa vào lọc Kalman góc nghiêng loại bỏ nhiễu Hình 3.15: Khối lọc Kalman Khối điều khiển trung tâm : Điều khiển khâu vị trí, khâu góc xoay, khâu góc nghiêng Đồ án tốt nghiệp Trang 64 Hình 3.16:Tổng quát Khối điều khiển trung tâm Khối đọc xung encoder: đọc xung từ encoder tính tốn giá trị cần thiết Hình 3.17: Bên Khối đọc xung encoder Khối xuất tín hiệu điều khiển: gồm xuất xung PWM DIR cho động trái phải Đồ án tốt nghiệp Trang 65 Hình 3.18: Bên Khối xuất tín hiệu điều khiển Khối gửi liệu máy tính: gửi thơng số từ điều khiển lên máy tính phục vụ giao diện giám sát, canh chỉnh PID Hình 3.19: Khối gửi liệu 3.2.3.4 Kết thu mơ hình thực tế: Với điều khiển PID thông số cố định, robot giữ thăng chỗ, di chuyển tiến lùi với tốc độ chậm địa hình phẳng với khả đáp ứng tác động ngoại lực biên độ nhỏ Đồ án tốt nghiệp Trang 66 Hình 3.20: Ngõ góc nghiêng điện áp động chạy mơ hình thực tế 3.2.4 Giao diện máy tính Trên mơ hình có sử dụng module bluetooth để truyền liệu không dây giá trị gửi từ robot, từ vẽ biểu đồ so sánh nhằm đối chiếu, đánh giá kết thực Giao diện máy tính xây dựng phần mềm Visual Basic 6.0, có sử dụng thêm thư viện MSCOMM32.OCX để giao tiếp với cổng COM thư viện NTGRAPH.OCX để vẽ biểu đồ Phần mềm giao diện đơn giản dễ sử dụng, có nút để điều khiển phần mềm:  Connect: Thiết lập kết nối đến cổng com, sẵn sàng để nhận giá trị từ robot gửi  Draw: Vẽ biểu đồ theo thời gian Đồ án tốt nghiệp Trang 67 Hình 3.21: Giao diện chương trình giao tiếp máy tính Đồ án tốt nghiệp Trang 68 Chương Kết luận hướng phát triển đề tài 4.1 Những kết đạt  Thiết kế hồn chỉnh, hồn thiện mơ hình robot bánh tự cân  Thiết lập mơ hình tốn học, hàm trạng thái mơ hình mơ thành cơng mơ hình Matlab Simulink  Các thành phần mạch điện vi điều khiển hoạt động tốt:  Giá trị cảm biến đọc xác, khơng bị trễ , sai sót, frame truyền  Mạch cầu H hoạt động ổn định không bị tải, nhiệt  Đọc encoder xác, khơng bị sai lệch  Vi điều khiển hoạt động ổn định, không tự reset ngắt kết nối  Giao tiếp với máy tính UART vẽ biểu đồ hoạt động robot  Mơ hình giữ thăng chỗ 4.2 Hạn chế đề tài Giữ cân chưa tốt, dễ bị vọt lố Chịu tác động ngoại lực yếu Hệ thống chưa đáp ứng đựợc hệ số ma sát khối lượng M robot thay đổi Chỉ giữ thăng chỗ mà chưa di chuyển 4.3 Kết luận Mô hình robot bánh tự cân đề tài không đặt nhiều thách thức nghiên cứu chế tạo Để thiết kế mơ hình robot bánh tự cân hồn chỉnh, linh hoạt làm sở để chế tạo xe bánh tự cân chở người cần phải tính tốn thật xác kết cấu khí, phải có thơng số thật xác động cơ, encoder, cảm biến điều khiển thích nghi mạng neuron… 4.4 Hướng phát triển Trong tương lai mơ hình robot bánh tự cân phát triển sau:  Mơ hình điều khiển thơng qua máy tính nhờ kết nối bluetooth giao diện máy tính thân thiện trực quan  Tối ưu hóa khối lượng, thuật tốn để mơ hình hoạt động trơn tru, linh hoạt, dễ điều khiển, tiết kiệm lượng  Có thể gắn thêm camera gps để robot định vị, ghi hình, xử lý ảnh tự hoạt động khơng gian lớn  Mơ hình robot bánh tự cân tiền đề để phát triển thành xe bánh tự cân chở người di chuyển linh hoạt Đồ án tốt nghiệp Trang 69 PHỤ LỤC TÀI LIỆU THAM KHẢO  Tài liệu nuớc: Huỳnh Thái Hoàng, Lý thuyết điều khiển tự động, Nhà xuất Đại học Quốc Gia,2005 Huỳnh Thái Hồng; “ Mơ Hình Hóa Và Nhận Dạng Hệ Thống”; Đại học bách khoa Tp HCM Nguy ễn Phùng Quang; “ Matlab Simulink”; NXB khoa học kỹ thuật Dương Hoài Nghĩa; “Điều khiển hệ thống đa biến”; Nhà xuất Đại học Quốc Gia TP.HCM, 2007 Huỳnh Thái Hồng; “ Mơ Hình Hóa Và Nhận Dạng Hệ Thống”; Đại học bách khoa Tp HCM  Tài liệu nước ngoài: Greg WELCH and Gary BISHOP , An Introduction to the Kalman Filter, University of North Carolina at Chapel Hill, 2004  Tài liệu website: http://www.segway.com http://sourceforge.net http://www.ti.com http://www.mathworks.com/matlabcentral/answers/ Đồ án tốt nghiệp Trang 67 ... chấm phản biện: …………………………… Đồ án tốt nghiệp bảo vệ HỘI ĐỒNG CHẤM BẢO VỆ ĐỒ ÁN TỐT NGHIỆP TRƯỜNG ĐẠI HỌC SƯ PHẠM KỸ THUẬT TP HỒ CHÍ MINH Đồ án tốt nghiệp Trang Ngày tháng 07 năm 2013 TRƯỜNG ĐH... phương khác Fl , r Nm Moment phát động động bánh trái, phải A Dòng điện động bánh trái, phải V Điện áp động bánh trái, phải il , ir vl , vr Bảng 2.1: Đồ án tốt nghiệp Ký hiệu ý nghĩa đại lượng Trang... Tp HCM, ngày … tháng … năm 2013 Giáo viên hướng dẫn TS Nguyễn Minh Tâm Đồ án tốt nghiệp Trang PHẦN B NỘI DUNG Danh sách hình ảnh sử dụng đồ án: Hình 1.1: Robot dạng bánh xe di chuyển địa