(Đồ án hcmute) xe hai bánh tự cân bằng giải thuật lqr, ứng dụng xử lý ảnh để bám theo quả bóng

BỘ GIÁO DỤC VÀ ĐÀO TẠO TRƯỜNG ĐẠI HỌC SƯ PHẠM KỸ THUẬT THÀNH PHỐ HỒ CHÍ MINH ĐỒ ÁN TỐT NGHIỆP NGÀNH ĐIỀU KHIỂN VÀ TỰ ĐỘNG HÓA XE HAI BÁNH TỰ CÂN BẰNG GIẢI THUẬT LQR, ỨNG DỤNG XỬ LÝ ẢNH ĐỂ BÁM THEO QUẢ BÓNG GVHD:VŨ VĂN PHONG SVTT:NGUYỄN HUY HOÀNG MSSV:16151304 SVTH:NGUYỄN HUY HOÀNG MSSV:16151092 SKL 0 Tp Hồ Chí Minh, tháng 07/2020 an 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 ĐÀO TẠO CHẤT LƯỢNG CAO  ĐỒ ÁN TỐT NGHIỆP Đề Tài: XE HAI BÁNH TỰ CÂN BẰNG GIẢI THUẬT LQR, ỨNG DỤNG XỬ LÝ ẢNH ĐỂ BÁM THEO QUẢ BÓNG GVHD : TS Vũ Văn Phong SVTH : Nguyễn Huy Hoàng MSSV:16151304 Phạm Văn Trân MSSV:16151092 Chuyên ngành : Điều khiển Tự động hóa Hệ đào tạo : Chính Quy Khóa đào tạo : 2016 – 2020 TP Hồ Chí Minh, ngày 27 tháng 07 năm 2020 an CỘNG HÒA XÃ HỘI CHỦ NGHĨA VIỆT NAM Độc lập – Tự – Hạnh phúc ******* Tp Hồ Chí Minh, ngày tháng năm 2020 Họ tên Sinh Viên : : NHIỆM VỤ ĐỒ ÁN Nguyễn Huy Hoàng Phạm Văn Trân MSSV: 16151304 MSSV: 16151092 Giảng viên hướng dẫn: Vũ Văn Phong SĐT: 0981479507 Tên đề tài: Xe hai bánh tự cân giải thuật LQR, ứng dụng xử lý ảnh để bám theo bóng Các số liệu, tài liệu ban đầu:…………………………………………………………… ……………………………………………………………………………………………… ……………………………………………………………………………………………… Nội dung thực đề tài:……………………………………………………………… ……………………………………………………………………………………………… ……………………………………………………………………………………………… Sản phẩm:……………………………………………………………………………… ……………………………………………………………………………………………… ……………………………………………………………………………………………… TRƯỞNG NGÀNH GIẢNG VIÊN HƯỚNG DẪN Khoa ĐT CLC – ĐH SPKT TP.HCM an CỘNG HÒA XÃ HỘI CHỦ NGHĨA VIỆT NAM Độc lập – Tự – Hạnh phúc ******* PHIẾU NHẬN XÉT CỦA GIÁO VIÊN HƯỚNG DẪN Họ tên Sinh viên: Nguyễn Huy Hoàng MSSV:16151304 Phạm Văn Trân MSSV:16151092 Ngành: CNKT Điều Khiển Tự Động Hóa Tên đề tài: Xe hai bánh tự cân giải thuật LQR, ứng dụng xử lý ảnh để bám theo bóng Họ tên Giáo viên hướng dẫn: Vũ Văn Phong NHẬN XÉT Về nội dụng đề tài khối lượng thực hiện: ……………………………………………………………………………………………… ……………………………………………………………………………………………… ……………………………………………………………………………………………… ……………………………………………………………………………………………… Ưu điểm: ……………………………………………………………………………………………… ……………………………………………………………………………………………… ……………………………………………………………………………………………… ……………………………………………………………………………………………… Khuyết điểm: ……………………………………………………………………………………………… ……………………………………………………………………………………………… ……………………………………………………………………………………………… Đề nghị cho bảo vệ hay không? ……………………………………………………………………………………………… Đánh giá loại: ……………………………………………………………………………………………… Điểm ………………(Bằng chữ:……………………………………………………… ) Tp Hồ Chí Minh, ngày tháng năm 2020 Khoa ĐT CLC – ĐH SPKT TP.HCM an CỘNG HÒA XÃ HỘI CHỦ NGHĨA VIỆT NAM Độc lập – Tự – Hạnh phúc ******* PHIẾU NHẬN XÉT CỦA GIÁO VIÊN PHẢN BIỆN Họ tên Sinh viên :………………………………………… MSSV:………………… :……………….………………………….MSSV:………………… Ngành……………………………………………………………………………………… Tên đề tài:………………………………………………………………………………… ……………………………………………………………………………………………… Họ tên Giáo viên phản biện:…………………………………………………………… ……………………………………………………………………………………………… NHẬN XÉT Về nội dụng đề tài khối lượng thực hiện: ……………………………………………………………………………………………… ……………………………………………………………………………………………… ……………………………………………………………………………………………… ……………………………………………………………………………………………… Ưu điểm: ……………………………………………………………………………………………… ……………………………………………………………………………………………… ……………………………………………………………………………………………… ……………………………………………………………………………………………… Khuyết điểm: ……………………………………………………………………………………………… ……………………………………………………………………………………………… ……………………………………………………………………………………………… Đề nghị cho bảo vệ hay không? ……………………………………………………………………………………………… Đánh giá loại: ……………………………………………………………………………………………… Điểm ………………(Bằng chữ:……………………………………………………… ) Tp Hồ Chí Minh, ngày tháng năm 2020 Khoa ĐT CLC – ĐH SPKT TP.HCM an MỤC LỤC LỜI CẢM ƠN i LỜI MỞ ĐẦU ii TÓM TẮT iii ABSTRACT iv DANH SÁCH HÌNH ẢNH v DANH MỤC TỪ VIẾT TẮT ix TÀI LIỆU THAM KHẢO x CHƯƠNG 1: TỔNG QUAN ĐỀ TÀI 1.1 Đặt vấn đề 1.2 Mục tiêu 1.3 Giới hạn 1.4 Phương pháp nghiên cứu 1.5 Nội dung CHƯƠNG 2: CƠ SỞ LÝ THUYẾT 2.1 Nguyên lý xe hai bánh tự cân 2.2 Xây dựng phương trình toán học 2.3 Bộ điều khiển LQR 10 2.3.1 LQR liên tục 10 2.3.2 LQR rời rạc 11 2.3.3 Xây dựng điều khiển cho hệ xe cân 11 2.4 Bộ lọc Kalman 14 2.5 Giao tiếp I2C 17 2.5.1 Giới thiệu giao tiếp I2C 17 2.5.2 Phương thức giao tiếp I2C 18 2.5.3 Hoạt động giao thức I2C 21 2.5.4 Các chế độ hoạt động I2C 22 2.6 Giao tiếp UART 24 2.7 Ngôn ngữ lập trình Python thư viện Opencv 26 2.7.1 Ngơn ngữ lập trình Pthon 26 an 2.7.2 Thư viện Opencv 28 2.8 Phương pháp nhận dạng vật theo màu sắc 29 2.8.1 Không gian màu RGB HSV 29 2.9.2 Nhận dạng màu vật không gian màu HSV 30 2.9 Tìm vị trí đường trịn 33 2.9.1 Tích chập Non – Maximum Suppression 33 2.9.3 Theo dõi đường biên đối tượng 38 2.9.2 Tìm tâm đường trịn 42 2.10 Bộ điều khiển PID 44 2.11 Phần mềm lập trình Arduino 47 CHƯƠNG 3: TÍNH TỐN VÀ THIẾT KẾ HỆ THỐNG 49 3.1 Tính tốn thiết kế hệ thống 49 3.2 Phần cứng xứ lý ảnh 60 CHƯƠNG 4: GIẢI THUẬT ĐIỀU KHIỂN 65 4.1 Quy trình cơng nghệ 65 4.2 Lưu đồ giải thuật 65 CHƯƠNG 5: KẾT QUẢ THỰC NGHIỆM 70 5.1 Kết xử lý ảnh 70 5.2 Mô matlab 72 5.3 Mơ hình thực tế 74 CHƯƠNG 6: KẾT LUẬN VÀ HƯỚNG PHÁT TRIỂN 78 6.1 Kết luận 78 6.2 Hướng phát triển 78 an ĐỒ ÁN TỐT NGHIỆP GVHD: TS.VŨ VĂN PHONG LỜI CẢM ƠN Trước hết chúng em xin cảm ơn TS Vũ Văn Phong có dẫn, đóng góp ý kiến vô quý báu động viên khích lệ tinh thần thầy truyền cảm hứng nguồn động lực lớn để nhóm hồn thành đồ án môn học Chúng em xin gửi lời cảm ơn đến Thầy, Cô Khoa Điện dạy chúng em năm tháng đại học để em có kiến thức vững tiền đề để em hồn thành đồ án mơn học ngày hơm Lời cảm ơn đến người bạn, giúp đỡ có đóng góp để em hồn thành đồ án môn học tốt i an ĐỒ ÁN TỐT NGHIỆP GVHD: TS.VŨ VĂN PHONG LỜI MỞ ĐẦU Đề tài “Xe hai bánh tự cân giải thuật LQR, ứng dụng xử lý ảnh để bám theo bóng”, nhóm phát triển từ lý thuyết lắc ngược Nhóm định phát triển từ điều khiển PID sang điều khiển LQR Đồng thời, nhóm nâng cấp cho hệ xe cân có khả theo dõi bám sát đối tượng xử lý ảnh máy tính Raspberry Pi Qua nghiên cứu, nhóm xử lý ảnh Raspberry gửi liệu Arduino giao thức I2C để tính tốn khoảng cách robot đối tượng Để robot theo dõi bóng tennis trì khoảng cách mong muốn, cần có kết hợp điều khiển PID điều khiển LQR để robot cân theo dõi bóng lúc ii an ĐỒ ÁN TỐT NGHIỆP GVHD: TS.VŨ VĂN PHONG TÓM TẮT Đề tài “Xe hai bánh tự cân giải thuật LQR, ứng dụng xử lý ảnh để bám theo bóng” cầu nối kinh nghiệm từ mơ hình thăng lắc ngược đến việc nghiên cứu chế tạo xe hai bánh nằm song song với Nhóm nghiên cứu thực xe hai bánh tự cân bằng điều khiển LQR với sử dụng phần mềm hỗ trợ việc giải thuật điều khiển Đồng thời, nhóm tiếp tục phát triển hệ xe có khả nhận dạng mục tiêu theo màu sắc bám sát mục tiêu máy tính Raspberry Pi Để robot bám theo bóng tennis trì khoảng cách mong muốn, cần kết hợp với điều khiển PID vào điều khiển LQR để robot vừa bám vừa bám theo trái banh Mơ hình gồm thân mang hai động Nisca NFS5475E, Arduino Mega 2560, cảm biến góc nghiêng MPU 6050, module cầu H XY-160D L298 7A 160W, giảm áp LM2596, module Bluetooth HC-05 iii an ĐỒ ÁN TỐT NGHIỆP GVHD: TS.VŨ VĂN PHONG CHƯƠNG 4: GIẢI THUẬT ĐIỀU KHIỂN 4.1 Quy trình cơng nghệ Hình 4.1: Quy trình công nghệ điều khiển xe hai bánh tự cân Hình 4.2: Quy trình cơng nghệ xử lý, tính tốn, xuất liệu ảnh CAMERA: Camera có chức thu video thực tế RASPBERRY: Chức lấy liệu từ camera để thực việc xử lý video BLUETOOTH: Chức điều khiển xe truyền từ phần mềm điện thoại ARDUINO: Chức tính tốn thơng số điều khiển Robot DC SERVO: Sau Arduino tính tốn xong tín hiệu điều khiển cho động XỬ LÝ HÌNH ẢNH: Đây khâu lấy liệu ảnh từ camera để chuyển sang hệ màu HSV, lọc nhiễu hình ảnh để xử lý PHÂN TÍCH HÌNH ẢNH: Đến dùng thuật tốn để phân tích hình ảnh tìm đường viền, nhận hình trịn, tính tốn tham số ảnh XUẤT THAM SỐ ĐIỀU KHIỂN: Sau tính tốn cấc tham số tọa độ tâm, bán kính hình trịn gửi giá trị đến Arduino thông qua giao tiếp UART 4.2 Lưu đồ giải thuật ❖ Điều khiển Robot cân 65 an ĐỒ ÁN TỐT NGHIỆP GVHD: TS.VŨ VĂN PHONG Hình 4.3: Lưu đồ giải thuật xe hai bánh tự cân Khai báo biến cần thiết xử lý chương trình Arduino Lưu ý khai báo biến cần phải xác định biến gì, bit, có dấu hay khơng có dấu Xác định điều để tối ưu thời gian xử lý chương trình, tránh tình trạng khai báo nhiều, xử lý nhiều khiến cho chương trình chậm xử lý gặp nhiều vấn đề MPU6050 đọc trả giá trị độ Ta chuyển qua Radian cách: (kalAngleY – offset)*PI/180; với kalAngleY giá trị góc đọc từ cảm biến offset hệ số đặt cảm biến, pin có sai số khiến cho xe khơng thể cân Vì ta trừ lấy góc đọc trừ sai số giá trị đặt điều khiển cho góc nghiêng 66 an ĐỒ ÁN TỐT NGHIỆP GVHD: TS.VŨ VĂN PHONG Đọc encoder động Nisca NF5475E trả vòng 200 xung ✓ Góc_tới = (Xung_R + Xung_L)∗360 2∗200 ✓ Chuyển qua Radian: theta = Góc_tới*PI/180 góc để điều khiển Robot tới lui ✓ Góc_xoay = 360𝑅 (Xung_R – Xung_L) 200𝑊 ✓ Chuyển qua Radian: phi = Góc_xoay*PI/180 Đây góc để điều khiển Robot xoay trái phải Với Xung_R, Xung_L số xung encoder hai động bên phải bên trái Tất tham số đưa vào điều khiển LQR Nếu xe bị nghiêng động đảo chiều để xuất tín hiệu điều khiển để Robot khơng bị đổ Tín hiệu xung PWM Arduino từ 0-255 (vì ghi analog Arduino có bit) Giao tiếp UART Arduino Mega 2560 R3 để lấy tham số truyền từ thiết bị ngoại vi HC-05 (Bluetooth), Raspberry • Bluetooth truyền Arduino kí tự F (Forward) tới, B (Back) lùi, L (Left) rẽ trái, R (Right) rẽ phải • Raspberry truyền Arduino tham số bán kính tọa độ tâm • Để điều khiển tới lui giá trị đặt bán kính 40, trả giá trị lớn 40 xe lùi, nhỏ 40 xe tới • Để điều khiển xe xoay dựa vào tọa độ trục hoành X ảnh Do ảnh có pixel (X=400,Y=300) nên để xoay theo bóng tâm bóng lúc có tọa độ X=200 Nếu lớn 200 xe quay phải, nhỏ 200 xe quay trái ❖ Xử lý ảnh Cài đặt hệ điều hành Rasbian thẻ nhớ sau cắm vào Raspberry Pi Kết nối hình, chuột, bàn phím để truy cập vào hệ điều hành Rasbian Cài đặt ngơn ngữ lập trình Python thư viện cần thiết: Opencv, Picamera (thư viện cho phép sử dụng camera raspberry), serial (thư viện sử dụng giao tiếp UART Raspberry), numpy (thư viện sử dụng mảng ma trận để xử lý) 67 an ĐỒ ÁN TỐT NGHIỆP GVHD: TS.VŨ VĂN PHONG Hình 4.4: Lưu đồ giải thuật nhận dạng bóng • Khai báo thư viện đề cập trên, sử dụng tập lệnh ngơn ngữ Python để thực việc xử lý, phân tích ảnh • Để lấy liệu từ camera thư viên Picamera sử dụng tâp lệnh sau: • vs = WebcamVideoStream(src=0).start() • frame = vs.read() • frame = imutils.resize(frame, width=400) 68 an ĐỒ ÁN TỐT NGHIỆP GVHD: TS.VŨ VĂN PHONG • Sau làm giảm nhiễu hạt lọc Gaussian với lệnh: cv.GaussianBlur(frame,(11,11),0) • Chuyển ảnh từ RGB sang HSV lệnh: cv.cvtColor(blur,cv.COLOR_BGR2HSV) Ta dò các tham số truyền vào để tìm màu sắc cần nhận dạng • Khi có màu sắc nhận dạng việc tiêp theo sử dụng thuật tốn tìm tọa độ tâm, bán kính truyền qua giao tiếp UART đến Arduino đê xử lý 69 an ĐỒ ÁN TỐT NGHIỆP GVHD: TS.VŨ VĂN PHONG CHƯƠNG 5: KẾT QUẢ THỰC NGHIỆM 5.1 Kết xử lý ảnh Việc sử dụng xử lý ảnh cho xe hai bánh tự cân bằng, ta phải thơng qua máy tính nhúng để thực việc lập trình xử lý ảnh Ở nhóm sử dụng máy tính nhúng phổ biến Raspberry Pi Sau cài đặt hệ điều hành Rasbian khởi động xong Raspberry Pi, kết nối với laptop thông qua giao thức mạng Ethernet, wifi Đơn giản sử dụng cổng HDMI kết nối trực tiếp với Raspberry để có giao diện làm việc Trong giao diện hệ điều hành Rasbian mở môi trường lập trình ngơn ngữ Python IDLE thực việc viết code Cài đặt thư viện gọi để thực lập trình Hình 5.1: Chuyển ảnh sang hệ màu HSV sau giảm nhiễu hạt Chuyển khung hình qua hệ màu HSV nhằm mục đích chọn tham số dải màu tìm bóng tennis Việc nhận dạng màu dễ bị yếu tố nhiễu hạt làm cho việc tìm biên bóng trở nên khó khăn Nên sử dụng lọc làm mờ để giảm nhiễu hạt lại 70 an ĐỒ ÁN TỐT NGHIỆP GVHD: TS.VŨ VĂN PHONG Hình 5.2: Hình nhận dạng bóng Hình 5.3: Nhận dạng bóng Sau xác định đường viền bóng, sử dụng lệnh có sẵn Opencv để vẽ lại đường viền tìm bán kính, tọa độ tâm trái bóng Khi có đầy đủ thơng số, ta gửi thơng số xuống xe hai bánh tự cân với giao tiếp UART Xe chạy tới lui làm thay đổi hệ số bán kính, xa bán kính bé, lại gần bán tính to Từ đặc điểm điều khiển tới lui cho xe Để xe quay trả 71 an ĐỒ ÁN TỐT NGHIỆP GVHD: TS.VŨ VĂN PHONG tọa độ tâm bóng, lấy giá trị tọa độ trục hoành để điều khiển cho xe Tọa độ tục hoành trái qua phải (0-400), để tâm bóng ln nằm gia trị đặt 200 Thuật toán nhỏ 200 xe xoay bên trái ngược lại 5.2 Mơ matlab Hình 5.4: Mơ xe hai bánh tự cân 72 an ĐỒ ÁN TỐT NGHIỆP GVHD: TS.VŨ VĂN PHONG Hình 5.5: Sơ đồ khối mô hệ xe hai bánh giải thuật LQR Sau thực tính tốn động lực học cho robot tự cân Ta mô kết vừa tìm phần mêm matlab/simulink ❖ Giải thích sơ đồ matlab/simulink: Hình 4.4b mơ hình xe hai bánh tự cân sau tính tốn động lực học Trong khối Fcn phương trình đạo hàm cấp hai ba góc theta, psi, phi Các khối Interator khối tích phân giúp đưa về nguyên đưa đạo hàm góc giá trị góc theta, psi, phi Các khối in, out tham số ngõ vào ngõ hệ Hình 4.4a điều khiển LQR Trong khối Atomic Subsystem sơ đồ hình b Tất tham số từ khối AtomicSubsystem đưa vào khối Gain để nhân với ma trận K tính tìm tham số điều khiển cho hệ xe hai bánh tự cân Hình 5.6: Đáp ứng ngõ mơ xe hai bánh tự cân LQR Trong đồ thị từ xuống ngõ góc theta, psi, phi theo thời gian lấy mẫu Các ngõ đáp ứng vị trí đặt ban đầu 73 an ĐỒ ÁN TỐT NGHIỆP GVHD: TS.VŨ VĂN PHONG 5.3 Mơ hình thực tế Hình 5.7: Mơ hình thực tế nhóm ❖ Góc psi Hình 5.8: Thay đổi góc nghiêng (psi) 74 an ĐỒ ÁN TỐT NGHIỆP GVHD: TS.VŨ VĂN PHONG Ban đầu đặt xe lệch só với góc nghiêng cho xe hoạt động Sau cho xe hoạt động xe đứng vị trí đặt Khi lấy tay lắc mạnh (tại mẫu 15000) dao động mạnh sau lại giao động quanh vị trí cân ❖ Góc theta Hình 5.9: Thay đổi góc tới (theta) Sau khởi động xe nằm vị trí ban đầu, đẩy xe đoạn giao động quay lại vị trí ban đầu Do tác động từ mơi trường bên ngồi nên xe bị đẩy nhẹ xung quanh vị trí ban đầu ❖ Góc phi 75 an ĐỒ ÁN TỐT NGHIỆP GVHD: TS.VŨ VĂN PHONG Hình 5.10: Thay đổi góc xoay (phi) ❖ Tác động theta đến hệ Khi xoay xe đoạn thả tay ra, xe quay lại vị trí ban đầu Và giao động quanh vị trí đặt theo thời gian Hình 5.11: Ảnh hưởng góc tới đến hệ Ở đẩy xe đoạn ta thấy góc theta chạy đoạn, giao động quan bị trí đặt đứng im ví trị lệch so với ban đầu Do xe bị tác động bới tác nhân xung quanh 76 an ĐỒ ÁN TỐT NGHIỆP GVHD: TS.VŨ VĂN PHONG ❖ Ảnh hưởng góc phi đến hệ Hình 5.12: Ảnh hưởng góc phi đến hệ Sau xoay đoạn với góc phi, psi khơng bị ảnh hưởng nhiều nhanh chóng quay vị trí đặt Góc theta thay đổi mạnh giao động xung quanh vị trí đặt đứng im đoạn lệch so với vị trí ban đầu 77 an ĐỒ ÁN TỐT NGHIỆP GVHD: TS.VŨ VĂN PHONG CHƯƠNG 6: KẾT LUẬN VÀ HƯỚNG PHÁT TRIỂN 6.1 Kết luận Qua thời gian nghiên cứu thực thi đề tài, nhóm có nhìn tổng qt lĩnh vực điều khiển hệ thống phi tuyến Từ yêu cầu đề tài, nhóm nghiên cứu thực vấn đề sau: • Thiết kế khung robot phù hợp việc cân bằng, khả di chuyển robot • Giao tiếp Arduino module Bluetooth, MPU6050 • Đọc giá trị góc tốt nhất, hạn chế nhiễu sử dụng lọc Kalman • Robot tự cân di chuyển hướng • Xử lý ảnh có đối tượng máy tính Raspberry Pi • Robot bám theo bóng với giá trị khoảng cách đặt sẵn • Ngồi ra, đề tài cịn có mặt hạn chế Robot bám theo bóng, chưa linh hoạt, khơng ổn định 6.2 Hướng phát triển • Giám sát theo dõi hình ảnh từ camera điện thoại nhằm quan sát đường robot • Xử lý ảnh, điều khiển robot chạy theo lane 78 an an ... ĐẠI HỌC SƯ PHẠM KỸ THUẬT TP HỒ CHÍ MINH KHOA ĐÀO TẠO CHẤT LƯỢNG CAO  ĐỒ ÁN TỐT NGHIỆP Đề Tài: XE HAI BÁNH TỰ CÂN BẰNG GIẢI THUẬT LQR, ỨNG DỤNG XỬ LÝ ẢNH ĐỂ BÁM THEO QUẢ BÓNG GVHD : TS Vũ Văn... góp để em hồn thành đồ án mơn học tốt i an ĐỒ ÁN TỐT NGHIỆP GVHD: TS.VŨ VĂN PHONG LỜI MỞ ĐẦU Đề tài ? ?Xe hai bánh tự cân giải thuật LQR, ứng dụng xử lý ảnh để bám theo bóng? ??, nhóm phát triển từ lý. .. TÓM TẮT Đề tài ? ?Xe hai bánh tự cân giải thuật LQR, ứng dụng xử lý ảnh để bám theo bóng? ?? cầu nối kinh nghiệm từ mơ hình thăng lắc ngược đến việc nghiên cứu chế tạo xe hai bánh nằm song song với