Điều khiển robot 1 bánh

BỘ GIÁO DỤC VÀ ĐÀO TẠO TRƯỜNG ĐẠI HỌC CÔNG NGHỆ TP HCM - NGUYỄN XUÂN TIÊN ĐIỀU KHIỂN ROBOT BÁNH LUẬN VĂN THẠC SĨ Chuyên ngành : Kỹ Thuật Cơ Điện tư Mã số ngành: 60 52 01 14 TP HỒ CHÍ MINH – 01/2014 BỘ GIÁO DỤC VÀ ĐÀO TẠO TRƯỜNG ĐẠI HỌC CÔNG NGHỆ TP HCM - NGUYỄN XUÂN TIÊN ĐIỀU KHIỂN ROBOT BÁNH LUẬN VĂN THẠC SĨ Chuyên ngành : Kỹ Thuật Cơ Điện tư Mã số ngành: 60 52 01 14 HƯỚNG DẪN KHOA HỌC: PGS.TSKH Hồ Đắc Lộc TS Nguyễn Thanh Phương TP HỒ CHÍ MINH, ngày 30 tháng 12 năm 2013 CƠNG TRÌNH ĐƯỢC HỒN THÀNH TẠI TRƯỜNG ĐẠI HỌC CÔNG NGHỆ TP HCM Cán hướng dẫn khoa học : PGS.TSKH Hồ Đắc Lộc TS Nguyễn Thanh Phương Luận văn Thạc sĩ bảo vệ Trường Đại học Công nghệ TP HCM ngày 25 tháng 01 năm 2014 Cán chấm nhận xét 1: TS Ngô Cao Cường Cán chấm nhận xét 2: TS Võ Hoàng Duy Thành phần Hội đồng đánh giá Luận văn Thạc sĩ gồm: (Ghi rõ họ, tên, học hàm, học vị Hội đồng chấm bảo vệ Luận văn Thạc sĩ) PGS.TS Nguyễn Tấn Tiến Chủ tịch 2.TS Ngô Cao Cường Phản biện 3.TS Võ Hoàng Duy Phản biện 4.TS Nguyễn Quốc Hưng 5.TS Nguyễn Hùng Ủy viên Ủy viên, Thư ký Xác nhận của Chủ tịch Hội đồng đánh giá luận văn Khoa quản lý chuyên ngành sau luận văn sửa chữa (nếu có) Chủ tịch Hội đồng đánh giá LV chuyên ngành Quản lý TRƯỜNG ĐẠI HỌC CÔNG NGHỆ TP HCM PHÒNG QLKH - ĐTSĐH CỘNG HÒA XÃ HỘI CHỦ NGHĨA VIỆT NAM Độc lập - Tự - Hạnh phúc TP HCM, ngày 30 tháng 12 năm 2013 NHIỆM VỤ LUẬN VĂN THẠC SĨ Họ tên học viên: NGUYỄN XUÂN TIÊN Giới tính: Nam Ngày, tháng, năm sinh: 19/06/1965 Nơi sinh: Huê Chuyên ngành: Kỹ Thuật Cơ Điện tử MSHV: 1241840018 I- TÊN ĐỀ TÀI: ĐIỀU KHIỂN ROBOT BÁNH II- NHIỆM VỤ VÀ NỘI DUNG: - Nghiên cứu lý thut điều khiển hồi tiêp tun tính hố, điều khiển - LQR Áp dụng phương pháp điều khiển hổi tiêp tun tính hố, điều khiển - - LQR điều khiển hệ robot bánh tự cân bằng Mô kêt điều khiển đối tượng Matlab\Simulink Kiểm chứng kêt mô bằng thực nghiệm điều khiển đối tượng thực III- NGÀY GIAO NHIỆM VỤ: 12/06/2013 IV- NGÀY HOÀN THÀNH NHIỆM VỤ: 30/12/2013 V- CÁN BỘ HƯỚNG DẪN: PGS.TSKH Hồ Đắc Lộc TS Nguyễn Thanh Phương Cán hướng dẫn khoa học PGS.TSKH Hồ Đắc Lộc Quản lý chuyên ngành TS Nguyễn Thanh Phương LỜI CAM ĐOAN Tôi xin cam đoan rằng Luận văn với nội dung “Điều khiển robot bánh ” cơng trình nghiên cứu riêng dưới hướng dẫn PGS.TSKH Hồ Đắc Lộc TS Nguyễn Thanh Phương Tôi xin cam đoan rằng giúp đỡ cho việc thực Luận văn cảm ơn thông tin trích dẫn Luận văn rõ nguồn gốc Các số liệu, kết nêu luận văn trung thực, có nguồn trích dẫn chưa từng cơng bố cơng trình khác Tp Hồ Chí Minh, ngày 30 tháng 12 năm 2013 Học viên thực luận văn Nguyễn Xuân Tiên LỜI CÁM ƠN Trong trình thực luận văn, gặp nhiều khó khăn nhờ sự hướng dẫn tận tình của Thầy PGS.TSKH Hồ Đắc Lộc, Thầy TS.Ngũn Thanh Phương, tơi hồn thành luận văn thời gian quy định Để hoàn thành luận văn tơi xin bày tỏ lòng biêt ơn sâu sắc Thầy PGS.TSKH.Hồ Đắc Lộc, Thầy TS.Nguyễn Thanh Phương, Thầy người tận tâm hêt lòng học viên, hướng dẫn nhiệt tình cung cấp cho tơi tài liệu vô quý giá thời gian thực luận văn Xin chân thành cám ơn tập thể thầy cô giáo trường Đại học Công Nghệ TP Hồ Chí Minh, giảng dạy truyền đạt kiên thức cho tôi, giúp học tập nghiên cứu trình học cao học trường Xin chân thành cảm ơn Ban giám hiệu, Phòng quản lý Khoa học - Đào tạo sau đại học phòng ban của trường Đại học Cơng Nghệ TP Hồ Chí Minh, giúp đỡ, tạo điều kiện thuận lợi cho trình học tập làm luận văn cao học trường Xin chân thành cảm ơn anh, chị đồng nghiệp hỗ trợ giúp đỡ cho trình thực luận văn Xin chân thành cảm ơn bạn học viên lớp cao học chuyên ngành “Kỹ thuật Cơ Điện tử -12SCĐ11” đóng góp ý kiên cho tơi q trình thực luận văn TP Hồ Chí Minh, ngày 30 tháng 12 năm 2013 Học viên thực luận văn Nguyễn Xuân Tiên TÓM TẮT LUẬN VĂN Hiện Robot ứng dụng phổ biên sản xuất công nghiệp Robot bánh tự cân bằng gọi Ballbot nhà khoa học nghiên cứu đưa vào ứng dụng số lĩnh vực Do sự động của nó, robot bánh cao hẹp trở thành ứng cử viên lý tưởng cho robot di động cá nhân Nó hoạt động robot dịch vụ hiệu nhà văn phòng Do đó, robot bánh có loạt ứng dụng ngành cơng nghiệp giải trí bao gồm đồ chơi Luận văn sâu vào nghiên cứu khái quát hệ robot bánh tự cân bằng , nghiên cứu mặt lý thuyêt điều khiển hệ robot bánh tự cân bằng thông qua phương pháp điều khiển hồi tiêp tuyên tính hóa, phương pháp điều khiển LQR, thuật tốn lọc Kalman rời rạc tiên hành mô kiểm tra hệ thống điều khiển Luận văn nghiên cứu điều khiển phần cứng chủ yêu dựa vào vi điều khiển ATmega 328 dạng chip 28 chân phần mềm ngơn ngữ lập trình dựa ngơn ngữ C++ , nhằm mục đích giám sát điều khiển hệ robot bánh để ổn định thăng bằng di chuyển không bị té ngã Luận văn tập trung chủ yêu xây dựng mơ hình thực nghiệm robot bánh tự cân bằng robot bánh thực tê bao gồm thi công thực nghiệm phần cứng bao gồm : Cơ cấu lái bóng chọn lựa bóng; Mạch cảm biên; Mạch vi điều khiển; Mạch nguồn; Mạch công suất điều khiển động khung phần thân robot Đồng thời tiên hành viêt chương trình điều khiển để điều khiển mơ hình thực nghiệm hệ robot bánh giữ ổn định thăng bằng di chuyển theo mong muốn Thông qua luận văn này, hy vọng cung cấp mơ hình thực nghiệm hệ robot bánh tự cân bằng số kiên thức hữu ích cho kỹ sư, sinh viên v.v… học tập nghiên cứu hệ thống robot ABSTRACT Nowadays, robot has been applying popularly in production industry A self-balancing ball robot called ballbot is used by scientists in a number of fields Due to its dynamic, a tall and thin ballbot could become an ideal candidate for personal mobile robot It can be operated efficiently as a serviced robot at home and in offices Therefore, ballbot would have various applications in the entertainment industry, including toys My thesis studies deeply the general of a self-balancing ball robot system Studying about control theory of a self-balancing ball robot system via linearised feedback methods, the ways to control the Linear quadra regulator, unconnected Kalman filter algorithm and to conduct simulation system inspection for each controller.This thesis studies the hardware drivers mtic reainly based on ATmega 328 micro-controller, is a 28-pins micro chip, and C++ programing language software Aiming to monitor and control one-ball robot system in order to stablize balancing and move robot without being falling This thesis focuses mainly to build a self- balancing ball robot model as a practical ball robot covering hardware experimental execution including : balloon driver and balloon selection structure; sensor circuit; micro-controller circuit; power circuit, motor power rating circuit and robot meromorphic At the same time, conducting controlling program in order to controll experimental model of one-ball robot system, keeping balance stability and moving as expecting Through this thesis, I also hope to provide some experimental models of a self-balancing ball robot system and other useful knowledge for students, engineers etc who are studing and doing research about ballbot systems MỤC LỤC Tên đề mục Trang Lời cam đoan .i Lời cảm ơn .ii Tóm tắt luận văn iii Abstract iv Mục lục v Danh mục từ viêt tắt ix Danh mục bảng biểu x Danh mục sơ đồ, hình ảnh xi Chương 1: Tổng quan hướng nghiên cứu 1.1 Đặt vấn đề .1 1.2 Thê robot bánh tự cân bằng 1.3 Tính cấp thiêt của đề tài, ý nghĩa khoa học thực tiễn của đề tài 1.4 Tình hình nghiên cứu nhu cầu thực tê 1.5 Mục đích nghiên cứu , khách thể , đối tượng nghiên cứu 1.6 Nhiệm vụ nghiên cứu giới hạn của đề tài 1.6.1.Nhiệm vụ nghiên cứu 1.6.2.Giới hạn của đề tài .9 y = y = y = y r −1 = Ta hệ thống tương ứng với biên trạng thái không quan sát Nêu hệ thống ổn định hệ thống cực tiểu pha Ta áp dụng luật điều khiển hồi tiêp tuyên tính hóa (3.8) 3.2 Lý thút bợ lọc Kalman 3.3.1 Giới thiệu bộ lọc Kalman Vào năm 1960, R.E.Kalman cho xuất nghiên cứu của mình, đưa giải pháp đệ qui để rời rạc hóa liệu lọc tun tính Kể từ đó, việc giải qut toán kỹ thuật số, lĩnh vực rộng lớn, trở nên dễ dàng nhiều Bộ lọc Kalman mở rộng nghiên cứu ứng dụng, đặc biệt lĩnh vực tự động hay hỗ trợ việc tự định vị Bộ lọc Kalman thu thập kêt hợp linh động tín hiệu từ cảm biên thành phần Mỗi mẫu thống kê nhiễu cảm biên xác nhận, lọc Kalman cho ước lượng giá trị tối ưu (do loại nhiễu), có độ phân bổ ổn định Trong đề tài tín hiệu đưa vào lọc lấy từ hai cảm biên: cảm biên gia tốc (accelerometers) cho giá trị góc đo cảm biên vận tốc góc (rate gyro), tín hiệu ngõ của lọc tín hiệu xử lý lẫn lọc; dựa vào mối quan hệ: vận tốc góc = đạo hàm của góc Bộ lọc Kalman thuật tốn xử lí liệu hồi quy tối ưu Nó tối ưu chi tiêt cụ thể bất kỳ tiêu chuẩn có nghĩa Bộ lọc Kalman tập hợp tất thông tin cung cấp tới nó, xử lý giá trị sẵn có, ước lượng giá trị quan tâm, sử dụng hiểu biêt động học, thiêt bị giá trị hệ thống, để mô tả số liệu thống kê của hệ thống nhiễu thông tin bất kỳ điều kiện ban đầu của giá trị cần ước lượng Đối với lọc Kalman, thuật ngữ “lọc” khơng có ý nghĩa lọc khác Đây giải thuật tính tốn ước lượng thống kê tối ưu tất thông tin ngõ vào cung cấp tới để có giá trị đáng tin cậy cho việc xử lý tiêp theo Do lọc Kalman sử dụng để loại bỏ tín hiệu nhiễu mà mơ hình tín hiệu nhiễu trắng tất dải thơng mà nhận từ ngõ vào, dựa thống kê trước chuẩn trực lại giá trị ước lượng bằng giá trị đo với độ lệch pha gần khơng tồn có độ lợi tối thiểu xấp xỉ tín hiệu ngõ vào không đáng tin cậy Mặc dù phải tốn nhiều thời gian xử lý lệnh, với tốc độ của vi điều khiển thời gian thực làm việc tính tốn ước lượng tối ưu của lọc trở nên đơn giản đáng tin cậy nhiều Nhờ có chê tự cập nhật giá trị sở (bias) thời điểm tính tốn, xác định sai lệch của kêt đo trước với kêt đo sau nên giá trị đo ln ổn định, xác, gần khơng bị sai số độ lợi độ lệch pha của tín hiệu Hơn thê, xây dựng hàm trạng thái, lọc Kalman kêt hợp khơng hai tín hiệu từ hai cảm biên, mà kêt hợp nhiều cảm biên đo dải tần khác của giá trị đại lượng vật lý Chính điều này, làm lọc Kalman trở nên phổ dụng tất lọc khác viêc xử lý tín hiệu xác của cảm biên tọa độ, cảm biên la bàn, GPS, góc, gyro… Sau sơ đồ mơ hình hóa hoạt động của mạch lọc Kalman (hình 3.1) Hình 3.1: Mơ hình hóa mạch lọc Kalman Chúng ta có tín hiệu đo được, có mơ hình của tín hiệu đo (đòi hỏi tun tính) sau áp dụng vào hệ thống phương trình của mạch lọc để ước lượng trạng thái quan tâm Thực tín hiệu đo khơng khó, phương trình có sẵn, cần mơ hình hố hệ thống Để ứng dụng cách hiệu mạch lọc Kalman phải mơ hình hóa cách tuyên tính sự thay đổi của trạng thái cần ước lượng (estimate) ước đoán (predict) 3.3.2 Thuật toán Kalman rời rạc Bộ lọc Kalman đề cập đên toán tổng quát ước lượng trạng thái của trình mơ hình hóa cách rời rạc theo thời gian bằng phương trình ngẫu nhiên tun tính sau xk = A * xk −1 + B * uk −1 + wk −1 kêt đo đạc : zk = H * xk + vk (3.33) (3.34) Trong w v vector biên ngẫu nhiên đại diện cho nhiễu hệ thông nhiễu đo đạc biên ngẫu nhiên độc lập giả sử tuân theo phân bố Gauss với trung bình bằng ma trận hiệp biên (covariance) Q R w ~N(0,Q) v ~N(0,R) Nêu vector trạng thái x có kích thước n, ma trận A có kích thước ( n x n).Ma trận B (n x l) ma trận phụ thuộc vào điều khiển tối ưu u với u vector có kích thước l Vector đo đạc z có kích thước m nên ma trận H (m x n) Chú ý rằng ma trận Q, R, A,H thay đổi theo thời gian (từng bước k), chúng giả sử khơng đổi Đên ta thấy tốn lọc Kalman tìm giá trị ược lượng ước đoán của trạng thái x ta biêt sự biên thiên của ta đo đai lượng z mà phụ thuộc tuyên tính vào x Ví dụ tốn chuyển động, ta khơng biêt qui luật thay đổi của vận tốc, ta lại đo sự thay đổi của vị trí Khi đó, ta cần tìm vận tốc ước lượng Nêu ta giả sử − x$k $k x giá trị tiên nghiệm giá trị tiền nghiệm ước lượng của giá trị x thời điểm k Giá trị tiên nghiệm thu dựa vào mơ hình hệ thống (1), giá trị hậu nghiệm giá trị thu sau có kêt đo đạc zk (2) Khi sai số của ước đoán tiên nghiệm hậu nghiệm − ek− = xk − x$k (3.35) ek = xk − x$k Ma trận hiệp biên của sai số tính theo cơng thức Pk− = E (ek−ek−T ) (3.36) Pk = E (ek ek T ) Mục đích của tìm hệ số K cho thỏa mản phương trình sau − − (3.37) x$k = x$k + K * ( z − H * x$k ) k Phương trình (3.37) có nghĩa giá trị nghiệm sau của ước lượng x tính bằng giá trị tiên nghiệm của sau thêm bớt tí dựa vào sai số − giá trị đo giá trị đo đạc ước đốn H * x$k K độ lợi (gain) của mạch lọc Kalman Câu hỏi đặt làm thê để chọn K tối ưu Tối ưu theo nghĩa covariance của sai số của ước lượng nghiệm sau − ek = K *( zk − H * x$k ) nhỏ Bằng cách thay sau lấy đạo hàm của ek vào biểu thức tính Pk , Pk theo K, ta tìm giá trị K mà tương ứng với Pk nhỏ K k = Pk− H T ( HPk− H T + R ) −1 Kk thay đổi theo thời gian k độ lợi cần tìm của mạch lọc Kalman ước đốn Tóm lại mạch lọc Kalman bao gồm bước : 1- Ước đoán trạng thái tiên nghiệm 2- Dựa vào kêt đo để hiệu chỉnh lại ước đốn Ta tóm tắt lại hoạt động của mạch lọc Kalman bằng phương trình sau: Giả sử bạn có giá trị ước đoán x$k −1 thời điểm (k-1) biêt giá trị điều khiển uk −1 (Giá trị ban đầu thời điểm chọn x$0 = H * z0 ) Lúc bạn việc tiên hành tính tốn từ đên bước từ đên bước hình Hình 3.2: Hai trình của mạch lọc Kalman Hình tóm tắt của mạch lọc Kalman Khó khăn của mạch lọc Kalman làm thê để mơ hình hóa trạng thái đo đạc để có phương trình (3.33) (3.34) từ áp dụng thuật tốn Kalman Phụ lục 3: Chương trình m.file chạy Matlab điều khiển bằng phương pháp điều khiển LQR clc; clear; % Hang so g = 9.81; % gia toc truong % Tham so Ballbot Mb = 0.5; % Khoi luong qua bong [kg] Rb = 0.11; % Ban kinh qua bong [don vi m] % Ib = 2*Mb*Rb^2/3 Ib = 0.0266; % Moment quan tinh cua qua bong [kg.m^2] IBx = 5.6; % Moment quan tinh body theo truc x [kg.m^2] IBy = 5.7; % Moment quan tinh body theo truc y [kg.m^2] MB = 5,0; % Khoi luong body [kg] L = 0.2; % Khoang cach tu tam body den tam qua bong [don vi m] n = 25/26; % Ti so truyen UBb = 0.2; % He so ma sat giua body va bong UBg = 0.01; % He so ma sat giua body va mat dat IM = 4.62*10^-5; % Moment quan tinh motor [kg.m^2] Kb = 0.04768; % He so suc dien dong motor [V/rad/sec] Kt = 0.0742; % He so moment xoan Motor [Nm/A] Rm = 1.1; % Dien tro phan ung [Ohm] Dx = IBx*IM + IM*Ib - 2*IM*Ib*n + IBx*Ib*n^2 + IM*Ib*n^2 + IM*L^2*MB + IM*MB*Rb^2+ IM*Mb*Rb^2 + Ib*L^2*MB*n^2 + IBx*MB*n^2*Rb^2 + IM*MB*n^2*Rb^2 + IBx*Mb*n^2*Rb^2+ IM*Mb*n^2*Rb^2 + 2*IM*L*MB*Rb - 2*IM*MB*n*Rb^2 2*IM*Mb*n*Rb^2 + L^2*MB*Mb*n^2*Rb^2-2*IM*L*MB*n*Rb; % Ma tran A Ax(3,1)= g*L*MB*(IM + Ib*n^2 + MB*n^2*Rb^2 + Mb*n^2*Rb^2 )/Dx; Ax(4,1)=-g*L*MB*(IM + Ib*n + MB*n*Rb^2 + Mb*n*Rb^2 + L*MB*n*Rb )/Dx; Ax(3,3)=-UBg*(IM + Ib*n^2 + MB*n^2*Rb^2 + Mb*n^2*Rb^2 )/Dx; Ax(4,3)=UBg*(IM + Ib*n + MB*n*Rb^2 + Mb*n*Rb^2 + L*MB*n*Rb)/Dx; Ax(3,4)=(UBb+Kb*Kt/Rm)*(IM + Ib*n + MB*n*Rb^2 + Mb*n*Rb^2 + L*MB*n*Rb)/Dx; Ax(4,4)= -(UBb+Kb*Kt/Rm)*(IBx + IM + Ib + L^2*MB + MB*Rb^2 + Mb*Rb^2 + 2*L*MB*Rb)/Dx; Bx(3,1)= (-Kt*(IM + Ib*n + MB*n*Rb^2 + Mb*n*Rb^2 + L*MB*n*Rb))/ (Dx*Rm); Bx(4,1)= Kt*(IBx + IM + Ib + L^2*MB + MB*Rb^2 + Mb*Rb^2 + 2*L*MB*Rb)/(Dx*Rm); Dy = IBy*IM + IM*Ib - 2*IM*Ib*n + IBy*Ib*n^2 + IM*Ib*n^2 + IM*L^2*MB + IM*MB*Rb^2+ IM*Mb*Rb^2 + Ib*L^2*MB*n^2 + IBy*MB*n^2*Rb^2 + IM*MB*n^2*Rb^2 + IBy*Mb*n^2*Rb^2+ IM*Mb*n^2*Rb^2 + 2*IM*L*MB*Rb - 2*IM*MB*n*Rb^2 2*IM*Mb*n*Rb^2 + L^2*MB*Mb*n^2*Rb^2-2*IM*L*MB*n*Rb; Ay(3,1)= g*L*MB*(IM + Ib*n^2 + MB*n^2*Rb^2 + Mb*n^2*Rb^2 )/Dy; Ay(4,1)=-g*L*MB*(IM + Ib*n + MB*n*Rb^2 + Mb*n*Rb^2 + L*MB*n*Rb )/Dy; Ay(3,3)=-UBg*(IM + Ib*n^2 + MB*n^2*Rb^2 + Mb*n^2*Rb^2 )/Dy; Ay(4,3)=UBg*(IM + Ib*n + MB*n*Rb^2 + Mb*n*Rb^2 + L*MB*n*Rb)/Dy; Ay(3,4)=(UBb+Kb*Kt/Rm)*(IM + Ib*n + MB*n*Rb^2 + Mb*n*Rb^2 + L*MB*n*Rb)/Dy; Ay(4,4)= -(UBb+Kb*Kt/Rm)*(IBx + IM + Ib + L^2*MB + MB*Rb^2 + Mb*Rb^2 + 2*L*MB*Rb)/Dy; By(3,1)= -Kt*(IM + Ib*n + MB*n*Rb^2 + Mb*n*Rb^2 + L*MB*n*Rb)/ (Dy*Rm); By(4,1)= Kt*(IBy + IM + Ib + L^2*MB + MB*Rb^2 + Mb*Rb^2 + 2*L*MB*Rb)/(Dy*Rm); A = [0 0 0 ; 00000100; 00000010; 00000001; Ax(3,1) 0 Ax(3,3) Ax(3,4) 0 ; 0 Ay(3,1) 0 Ay(3,3) Ay(3,4) ; Ax(4,1) 0 Ax(4,3) Ax(4,4) 0 ; 0 Ay(4,1) 0 Ay(4,3) Ay(4,4) ]; B=[ 00; 00; 00; 00; Bx(3,1) ; Bx(4,1) ; By(3,1) ; By(4,1)]; Q =[ 1000 0 0 0 ; 01000000; 0 1000 0 0 ; 0 0 0; 0 0 0 0; 0 0 0; 0 0 0 10 0; 0 0 0 10]; % R = 6e3 * (180/pi)^2; C = eye(8); D = zeros (8,2); R =[5 ; 10]; K = lqr(A,B,Q,R); Phụ lục 4: Chương trình vi điều khiển AVR (Atmega 328) #include #define MPU6050_AUX_VDDIO 0x01 // R/W #define MPU6050_SMPLRT_DIV 0x19 // R/W #define MPU6050_CONFIG 0x1A // R/W #define MPU6050_GYRO_CONFIG 0x1B // R/W #define MPU6050_ACCEL_CONFIG 0x1C // R/W #define MPU6050_FF_THR 0x1D // R/W #define MPU6050_FF_DUR 0x1E // R/W #define MPU6050_MOT_THR 0x1F // R/W #define MPU6050_MOT_DUR 0x20 // R/W #define MPU6050_ZRMOT_THR 0x21 // R/W #define MPU6050_ZRMOT_DUR 0x22 // R/W #define MPU6050_FIFO_EN 0x23 // R/W #define MPU6050_I2C_MST_CTRL 0x24 // R/W #define MPU6050_I2C_SLV0_ADDR 0x25 // R/W #define MPU6050_I2C_SLV0_REG 0x26 // R/W #define MPU6050_I2C_SLV0_CTRL 0x27 // R/W #define MPU6050_I2C_SLV1_ADDR 0x28 // R/W #define MPU6050_I2C_SLV1_REG 0x29 // R/W #define MPU6050_I2C_SLV1_CTRL 0x2A // R/W #define MPU6050_I2C_SLV2_ADDR 0x2B // R/W #define MPU6050_I2C_SLV2_REG 0x2C // R/W #define MPU6050_I2C_SLV2_CTRL 0x2D // R/W #define MPU6050_I2C_SLV3_ADDR 0x2E // R/W #define MPU6050_I2C_SLV3_REG 0x2F // R/W #define MPU6050_I2C_SLV3_CTRL 0x30 // R/W #define MPU6050_I2C_SLV4_ADDR 0x31 // R/W #define MPU6050_I2C_SLV4_REG 0x32 // R/W #define MPU6050_I2C_SLV4_DO 0x33 // R/W #define MPU6050_I2C_SLV4_CTRL 0x34 // R/W #define MPU6050_I2C_SLV4_DI 0x35 // R #define MPU6050_I2C_MST_STATUS 0x36 // R #define MPU6050_INT_PIN_CFG 0x37 // R/W #define MPU6050_INT_ENABLE 0x38 // R/W #define MPU6050_INT_STATUS 0x3A // R #define MPU6050_ACCEL_XOUT_H 0x3B // R #define MPU6050_ACCEL_XOUT_L 0x3C // R #define MPU6050_ACCEL_YOUT_H 0x3D // R #define MPU6050_ACCEL_YOUT_L 0x3E // R #define MPU6050_ACCEL_ZOUT_H 0x3F // R #define MPU6050_ACCEL_ZOUT_L 0x40 // R #define MPU6050_TEMP_OUT_H 0x41 // R #define MPU6050_TEMP_OUT_L 0x42 // R #define MPU6050_GYRO_XOUT_H 0x43 // R #define MPU6050_GYRO_XOUT_L 0x44 // R #define MPU6050_GYRO_YOUT_H 0x45 // R #define MPU6050_GYRO_YOUT_L 0x46 // R #define MPU6050_GYRO_ZOUT_H 0x47 // R #define MPU6050_GYRO_ZOUT_L 0x48 // R #define MPU6050_EXT_SENS_DATA_00 0x49 // R #define MPU6050_EXT_SENS_DATA_01 0x4A // R #define MPU6050_EXT_SENS_DATA_02 0x4B // R #define MPU6050_EXT_SENS_DATA_03 0x4C // R #define MPU6050_EXT_SENS_DATA_04 0x4D // R #define MPU6050_EXT_SENS_DATA_05 0x4E // R #define MPU6050_EXT_SENS_DATA_06 0x4F // R #define MPU6050_EXT_SENS_DATA_07 0x50 // R #define MPU6050_EXT_SENS_DATA_08 0x51 // R #define MPU6050_EXT_SENS_DATA_09 0x52 // R #define MPU6050_EXT_SENS_DATA_10 0x53 // R #define MPU6050_EXT_SENS_DATA_11 0x54 // R #define MPU6050_EXT_SENS_DATA_12 0x55 // R #define MPU6050_EXT_SENS_DATA_13 0x56 // R #define MPU6050_EXT_SENS_DATA_14 0x57 // R #define MPU6050_EXT_SENS_DATA_15 0x58 // R #define MPU6050_EXT_SENS_DATA_16 0x59 // R #define MPU6050_EXT_SENS_DATA_17 0x5A // R #define MPU6050_EXT_SENS_DATA_18 0x5B // R #define MPU6050_EXT_SENS_DATA_19 0x5C // R #define MPU6050_EXT_SENS_DATA_20 0x5D // R #define MPU6050_EXT_SENS_DATA_21 0x5E // R #define MPU6050_EXT_SENS_DATA_22 0x5F // R #define MPU6050_EXT_SENS_DATA_23 0x60 // R #define MPU6050_MOT_DETECT_STATUS 0x61 // R #define MPU6050_I2C_SLV0_DO 0x63 // R/W #define MPU6050_I2C_SLV1_DO 0x64 // R/W #define MPU6050_I2C_SLV2_DO 0x65 // R/W #define MPU6050_I2C_SLV3_DO 0x66 // R/W #define MPU6050_I2C_MST_DELAY_CTRL 0x67 // R/W #define MPU6050_MOT_COUNT_0 (0) #define MPU6050_MOT_COUNT_1 (bit(MPU6050_MOT_COUNT0)) #define MPU6050_MOT_COUNT_2 (bit(MPU6050_MOT_COUNT1)) #define MPU6050_MOT_COUNT_3 (bit(MPU6050_MOT_COUNT1)| bit(MPU6050_MOT_COUNT0)) #define MPU6050_MOT_COUNT_RESET MPU6050_MOT_COUNT_0 #define MPU6050_FF_COUNT_0 (0) #define MPU6050_FF_COUNT_1 (bit(MPU6050_FF_COUNT0)) #define MPU6050_FF_COUNT_2 (bit(MPU6050_FF_COUNT1)) #define MPU6050_FF_COUNT_3 (bit(MPU6050_FF_COUNT1)| bit(MPU6050_FF_COUNT0)) #define MPU6050_FF_COUNT_RESET MPU6050_FF_COUNT_0 #define MPU6050_ACCEL_ON_DELAY_0 (0) #define MPU6050_ACCEL_ON_DELAY_1 (bit(MPU6050_ACCEL_ON_DELAY0)) #define MPU6050_ACCEL_ON_DELAY_2 (bit(MPU6050_ACCEL_ON_DELAY1)) #define MPU6050_ACCEL_ON_DELAY_3 (bit(MPU6050_ACCEL_ON_DELAY1)| bit(MPU6050_ACCEL_ON_DELAY0)) // Alternative names for the ACCEL_ON_DELAY #define MPU6050_ACCEL_ON_DELAY_0MS MPU6050_ACCEL_ON_DELAY_0 #define MPU6050_ACCEL_ON_DELAY_1MS MPU6050_ACCEL_ON_DELAY_1 #define MPU6050_ACCEL_ON_DELAY_2MS MPU6050_ACCEL_ON_DELAY_2 #define MPU6050_ACCEL_ON_DELAY_3MS MPU6050_ACCEL_ON_DELAY_3 #define MPU6050_SIG_COND_RESET MPU6050_D0 #define MPU6050_I2C_MST_RESET MPU6050_D1 #define MPU6050_FIFO_RESET MPU6050_D2 #define MPU6050_I2C_IF_DIS MPU6050_D4 #define MPU6050_I2C_MST_EN MPU6050_D5 #define MPU6050_FIFO_EN MPU6050_D6 #define MPU6050_CLKSEL0 MPU6050_D0 #define MPU6050_CLKSEL1 MPU6050_D1 #define MPU6050_CLKSEL2 MPU6050_D2 #define MPU6050_TEMP_DIS MPU6050_D3 #define MPU6050_CYCLE MPU6050_D5 #define MPU6050_SLEEP MPU6050_D6 #define MPU6050_DEVICE_RESET MPU6050_D7 #define MPU6050_CLKSEL_0 (0) #define MPU6050_CLKSEL_1 (bit(MPU6050_CLKSEL0)) #define MPU6050_CLKSEL_2 (bit(MPU6050_CLKSEL1)) #define MPU6050_CLKSEL_3 (bit(MPU6050_CLKSEL1)| bit(MPU6050_CLKSEL0)) #define MPU6050_CLKSEL_4 (bit(MPU6050_CLKSEL2)) #define MPU6050_CLKSEL_5 (bit(MPU6050_CLKSEL2)| bit(MPU6050_CLKSEL0)) #define MPU6050_CLKSEL_6 (bit(MPU6050_CLKSEL2)| bit(MPU6050_CLKSEL1)) #define MPU6050_CLKSEL_7 (bit(MPU6050_CLKSEL2)| bit(MPU6050_CLKSEL1)|bit(MPU6050_CLKSEL0)) #define MPU6050_CLKSEL_INTERNAL MPU6050_CLKSEL_0 #define MPU6050_CLKSEL_X MPU6050_CLKSEL_1 #define MPU6050_CLKSEL_Y MPU6050_CLKSEL_2 #define MPU6050_CLKSEL_Z MPU6050_CLKSEL_3 #define MPU6050_CLKSEL_EXT_32KHZ MPU6050_CLKSEL_4 #define MPU6050_CLKSEL_EXT_19_2MHZ MPU6050_CLKSEL_5 #define MPU6050_CLKSEL_RESERVED MPU6050_CLKSEL_6 #define MPU6050_CLKSEL_STOP MPU6050_CLKSEL_7 #define MPU6050_STBY_ZG MPU6050_D0 #define MPU6050_STBY_YG MPU6050_D1 #define MPU6050_STBY_XG MPU6050_D2 #define MPU6050_STBY_ZA MPU6050_D3 #define MPU6050_STBY_YA MPU6050_D4 #define MPU6050_STBY_XA MPU6050_D5 #define MPU6050_LP_WAKE_CTRL0 MPU6050_D6 #define MPU6050_LP_WAKE_CTRL1 MPU6050_D7 #define MPU6050_LP_WAKE_CTRL_0 (0) #define MPU6050_LP_WAKE_CTRL_1 (bit(MPU6050_LP_WAKE_CTRL0)) #define MPU6050_LP_WAKE_CTRL_2 (bit(MPU6050_LP_WAKE_CTRL1)) #define MPU6050_LP_WAKE_CTRL_3 (bit(MPU6050_LP_WAKE_CTRL1)|bit(MPU6050_LP_WAKE_CTRL0)) #define MPU6050_LP_WAKE_1_25HZ MPU6050_LP_WAKE_CTRL_0 #define MPU6050_LP_WAKE_2_5HZ MPU6050_LP_WAKE_CTRL_1 #define MPU6050_LP_WAKE_5HZ MPU6050_LP_WAKE_CTRL_2 #define MPU6050_LP_WAKE_10HZ MPU6050_LP_WAKE_CTRL_3 #define MPU6050_I2C_ADDRESS 0x68 typedef union accel_t_gyro_union { struct { uint8_t x_accel_h; uint8_t x_accel_l; uint8_t y_accel_h; uint8_t y_accel_l; uint8_t z_accel_h; uint8_t z_accel_l; uint8_t t_h; uint8_t t_l; uint8_t x_gyro_h; uint8_t x_gyro_l; uint8_t y_gyro_h; uint8_t y_gyro_l; uint8_t z_gyro_h; uint8_t z_gyro_l; } reg; struct { int x_accel; int y_accel; int z_accel; int temperature; int x_gyro; int y_gyro; pinMode(encoder0Pin0_B, INPUT); pinMode(encoder0Pin1_A, INPUT); pinMode(encoder0Pin1_B, INPUT); attachInterrupt(0,doEncoder0_A, CHANGE); attachInterrupt(1,doEncoder1_A, CHANGE); // Serial.begin(19200); Wire.begin(); error = MPU6050_read (MPU6050_WHO_AM_I, &c, 1); error = MPU6050_read (MPU6050_PWR_MGMT_2, &c, 1); MPU6050_write_reg (MPU6050_PWR_MGMT_1, 0); calibrate_sensors(); set_last_read_angle_data(millis(), 0, 0, 0, 0, 0, 0); } // void loop() { int error; double dT; accel_t_gyro_union accel_t_gyro; error = read_gyro_accel_vals((uint8_t*) &accel_t_gyro); unsigned long t_now = millis(); float FS_SEL = 131; float gyro_x = (accel_t_gyro.value.x_gyro - base_x_gyro)/FS_SEL; float gyro_y = (accel_t_gyro.value.y_gyro - base_y_gyro)/FS_SEL; float gyro_z = (accel_t_gyro.value.z_gyro - base_z_gyro)/FS_SEL; float accel_x = accel_t_gyro.value.x_accel; float accel_y = accel_t_gyro.value.y_accel; float accel_z = accel_t_gyro.value.z_accel; float RADIANS_TO_DEGREES = 180/3.14159; float accel_angle_y = atan(-1*accel_x/sqrt(pow(accel_y,2) + pow(accel_z,2)))*RADIANS_TO_DEGREES; float accel_angle_x = atan(accel_y/sqrt(pow(accel_x,2) + pow(accel_z,2)))*RADIANS_TO_DEGREES; float accel_angle_z = 0; float dt =(t_now - get_last_time())/1000.0; float gyro_angle_x = gyro_x*dt + get_last_x_angle(); float gyro_angle_y = gyro_y*dt + get_last_y_angle(); float gyro_angle_z = gyro_z*dt + get_last_z_angle(); float unfiltered_gyro_angle_x = gyro_x*dt + get_last_gyro_x_angle(); float unfiltered_gyro_angle_y = gyro_y*dt + get_last_gyro_y_angle(); float unfiltered_gyro_angle_z = gyro_z*dt + get_last_gyro_z_angle(); float alpha = 0.96; float angle_x = alpha*gyro_angle_x + (1.0 - alpha)*accel_angle_x; float angle_y = alpha*gyro_angle_y + (1.0 - alpha)*accel_angle_y; float angle_z = gyro_angle_z; set_last_read_angle_data(t_now, angle_x, angle_y, angle_z, unfiltered_gyro_angle_x, unfiltered_gyro_angle_y, unfiltered_gyro_angle_z); /* Serial.print(F("DEL:")); Serial.print(dt, DEC); //*********************************** // Viet code chuong trinh o doan //*********************************** bu_goc(); } else { encoder1Pos = encoder1Pos - 1; // CCW } } //Serial.print(encoder1Pos); // use for debugging - remember to comment out } //+++++++++++++++++++++++++++++++++++++ void doEncoder1_B(){ // look for a low-to-high on channel B if (digitalRead(encoder0Pin1_B) == HIGH) { // check channel A to see which way encoder is turning if (digitalRead(encoder0Pin1_A) == HIGH) { encoder1Pos = encoder1Pos + 1; // CW } else { encoder1Pos = encoder1Pos - 1; // CCW } } // Look for a high-to-low on channel B else { // check channel B to see which way encoder is turning if (digitalRead(encoder0Pin1_A) == LOW) { encoder1Pos = encoder1Pos + 1; // CW } else { encoder1Pos = encoder1Pos - 1; // CCW } ... cứu 1. 8 Nội dung luận văn Chương 2: Mơ hình hóa tốn học hệ robot mợt bánh .11 2 .1. Mơ hình đơn giản hóa robot bánh 11 2 .1. 1 Các giả định 11 2 .1. 2 Phương pháp... .11 2 .1. 3 Tham số vật lý .14 2 .1. 4 Các quan hệ tọa độ .15 2 .1. 4 .1 Đối với bóng 15 2 .1. 4.2 Đối với thân robot .16 2 .1. 4.3 Đối với động 16 Phương... khiển động Hình 4 .10 : Mạch cơng suất điều khiển động Hình 4 .11 : Lưu đồ giải thuật hệ robot bánh vi điều khiển 29 29 30 30 31 31 32 32 35 35 36 36 37 37 38 38 39 39 40 40 41 41 42 42 43 43 45 45