thiết kế robot leo trụ và điều khiển

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 LUẬN VĂN THẠC SĨ VÕ VĂN MẾN THIẾT KẾ ROBOT LEO TRỤ VÀ ĐIỀU KHIỂN NGÀNH: KỸ THUẬT ĐIỆN - 60520202 S K C0 Tp Hồ Chí Minh, tháng 05/2015 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 LUẬN VĂN THẠC SĨ VÕ VĂN MẾN THIẾT KẾ ROBOT LEO TRỤ VÀ ĐIỀU KHIỂN NGÀNH: KỸ THUẬT ĐIỆN - 60520202 Hướng dẫn khoa học: T.S NGUYỄN THANH PHƯƠNG Tp.Hồ Chí Minh, tháng 04 năm 2015 THIẾT KẾ ROBOT LEO TRỤ VÀ ĐIỀU KHIỂN LÝ LỊCH KHOA HỌC I LÝ LỊCH SƠ LƯỢC Họ tên: VÕ VĂN MẾN Giới tính: Nam Ngày, tháng, năm sinh: 09/10/1990 Nơi sinh: Bến Tre Quê quán: Xã Phong Nẫm - H Giồng Trôm - Bến Tre Dân tộc: Kinh Chỗ riêng địa liên lạc: 559A CMT8, P15, Q.10, Tp HCM Điện thoại: 0909 065 405 E-mail: menvanvo@gmail.com II QUÁ TRÌNH ĐÀO TẠO Đại học: Hệ đào tạo: Chính qui Thời gian đào tạo từ: 08/2008 đến 08/2012 Nơi học (trường, thành phố): Đại học Công Nghiệp Tp Hồ Chí Minh Ngành học: Kỹ Thuật Điện Tên luận án: Điều khiển giám sát dây chuyền phân loại sản phẩm theo chiều cao Ngày nơi bảo vệ luận án: 07/2012 trường ĐH Công Nghiệp Tp Hồ Chí Minh Người hướng dẫn:Th.S Lý Bình Sơn III QUÁ TRÌNH CÔNG TÁC CHUYÊN MÔN KỂ TỪ KHI TỐT NGHIỆP ĐẠI HỌC: Thời gian Nơi công tác ii Công việc đảm nhiệm THIẾT KẾ ROBOT LEO TRỤ VÀ ĐIỀU KHIỂN LỜI CAM ĐOAN Tôi cam đoan công trình nghiên cứu Các số liệu, kết nêu luận văn trung thực chưa công bố công trình khác Tp Hồ Chí Minh, ngày 25 tháng 03năm 2015 (Ký tên ghi rõ họ tên) iii THIẾT KẾ ROBOT LEO TRỤ VÀ ĐIỀU KHIỂN LỜI CẢM ƠN Trong suốt trình học tập hoàn thành luận văn này, nhận hướng dẫn giúp đỡ quý báu thầy cô, gia đình, bạn bè Với lòng kính trọng biết ơn sâu sắc, xin bày tỏ lời cảm ơn chân thành tới: Ban giám hiệu, phòng đào tạo sau đại học, khoa Điện-điện tử trường Đại Học Sư Phạm Kỹ Thuật Tp Hồ Chí Minh tạo điều kiện cho trình học tập hoàn thành luận văn tốt nghiệp Tiến sĩ Nguyễn Thanh Phương, trưởng khoa Điện-điện tử trường Đại Học Công Nghệ Tp Hồ chí Minh, người thầy kính mến hết lòng giúp đỡ, động viên hỗ trợ điều kiện thuận lợi cho hoàn thành luận văn tốt nghiệp Xin chân thành cảm ơn thầy cô hội đồng phản biện đóng góp ý kiến quý báu để hoàn chỉnh luận văn Xin chân thành cảm ơn bạn bè lớp cao học Kỹ thuật điện 2013A gắn bó giúp đỡ lúc khó khăn Tp Hồ Chí Minh, ngày 25, tháng 03, năm 2015 Võ Văn Mến iv THIẾT KẾ ROBOT LEO TRỤ VÀ ĐIỀU KHIỂN TÓM TẮT Theo đà phát triển nhanh chóng khoa học, robot ngày sử dụng phổ biến sản xuất đời sống người Robot chiếm vị trí quan trọng khó thay được, giúp người làm việc với suất cao điều kiện khó khăn, nguy hiểm …Lĩnh vực robot di động ngày chiếm nhiều quan tâm nhà nghiên cứu xã hội Từ thực tế việc xây dựng điều khiển cho robot di động trở nên yêu cầu thiết yếu Trong nghiên cứu thiết kế chế tạo robot có khả leo trụ thách thức đặt robot phải bám trụ, trụ trụ sắt côn tròn Đồng thời, robot sử dụng ba bánh xe để di chuyển lên xuống thách thức đặt ba động DC phải chạy tốc độ Nội dung đề tài thiết kế điều khiển đồng tốc độ ba động điều khiển bám vị trí nhằm giúp robot bám di chuyển lên trụ ABSTRACT Nowaday, robot is widely used to industry and human life It has taken an important part and hardly to be replaced, it helps human to increase the yield and to work in dangerous or difficult conditions…The field of Moving – Robot has attracted a lot of attention of reseachers and society From the fact of that, designing the controllers of Moving – Robot has became an important problem In design research and manufacture of capable tree climbing is the biggest challenge is set robot must be able to cling with tree, tree is a iron tree round Moreover, robot used to three wheels to move up and down,is the biggest challenge is set three motors to run with a speed The main contents of project is design the controllers synchrizing three – motors control and hold the position control to help robot holding and moving on the tree v THIẾT KẾ ROBOT LEO TRỤ VÀ ĐIỀU KHIỂN MỤC LỤC Trang tựa TRANG Quyết định giao đề tài i Lý lịch cá nhân ii Lời cam đoan .iii Lời cảm ơn iv Tóm tắt v Mục lục vi Danh sách hình vii Danh sách bảng .viii Chương 1: TỔNG QUAN 1.1 Các kết nghiên cứu nước 1.2 Mục tiêuvà đối tượng nghiên cứu .8 1.3 Nhiệm vụ đề tài phạm vi nghiên cứu 1.4 Phương pháp nghiên cứu Chương 2: CƠ SỞ LÝ THUYẾT 10 2.1 Cơ sở chọn phần mềm mô – lập trình – giám sát .10 2.2Phương pháp điều chế độ rộng xung PWM 14 2.3 Phương pháp điều khiển PID 18 2.4 Điều khiển đồng tốc độ nhiều động theophương pháp ELS 21 2.5 Phương pháp điều khiển bám vị trí 24 Chương 3: KẾT QUẢ MÔ PHỎNG 27 3.1 Kết mô điều khiển đồng tốc độ hai động 27 3.1.1 Các thông số mô 27 3.1.2 Kết mô với tỉ lệ vị trí vận tốc Kx = 0.7 .32 3.1.3 Kết mô với tỉ lệ vị trí vận tốc Kx = 33 3.1.4 Nhận xét kết mô 34 3.2 Kết mô điều khiển bám vị trí .34 3.2.1 Các thông số mô 34 3.2.2 Kết mô 36 3.2.3Nhận xét kết mô .38 vi THIẾT KẾ ROBOT LEO TRỤ VÀ ĐIỀU KHIỂN Chương 4: THIẾT KẾ KẾT CẤU CƠ KHÍ .39 4.1 Phân tích phương án chọn kết cấu khí 39 4.2 Chọn trụ mẫu 39 4.3 Chọn vật liệu gia công robot 40 4.4 Chọn động truyền động 42 4.4.1 Phân tích lựa chọn kết cấu động 42 4.4.2 Tính toán lựa chọn công suất động 43 4.5 Chọn bánh xe chủ động 46 4.6Chọn trục ép 48 Chương 5: THIẾT KẾ BỘ ĐIỀU KHIỂN 49 5.1 Chọn cảm biến .49 5.1.1 Encorder 49 5.1.2 SRF05 .51 5.2Chọn vi điều khiển 53 5.2.1 dsPIC30F4011 54 5.2.2 PIC16F628A 55 5.3 Mạch công suất 57 5.4 Thiết kế giao diện giám sát Visual Basic 6.0 58 Chương 6: THUẬT TOÁN ĐIỀU KHIỂN 60 6.1Thuật toán điều khiển bám vị trí cho trục ép 60 6.2 Thuật toán điều khiển đồng tốc ba động 62 6.3 Công thức sử dụng chương trình điều khiển 65 Chương 7: KẾT QUẢ THỰC NGHIỆM .67 7.1 Nhận xét trình di chuyển 67 7.2 Nhận xét kết giám sát 69 Chương 8: KẾT LUẬN 70 8.1 Những kết đạt 70 8.2 Hạn chế đề tài 70 8.3 Hướng phát triển đề tài 70 TÀI LIỆU THAM KHẢO 71 PHỤ LỤC 72 vi THIẾT KẾ ROBOT LEO TRỤ VÀ ĐIỀU KHIỂN DANH SÁCH CÁC HÌNH Hình 1.1: Robot rắn Hình 1.2: Mẫu robot RISE V2 Hình 1.3: Mẫu robot RISE V3 Hình 1.4: Robot Treebot Hình 1.5: Robot UT-PCR Hình 2.1: Cửa sổ tra cứu thư viện Simulink Hình 2.2: Quá trình lập trình, biên dịch nạp cho PIC Hình 2.3: Cửa sổ làm việc Visual Basic Hình 2.4: Đồ thị dạng xung điều chế PWM Hình2.5 :Sơ đồ nguyên tắc điều khiển tải dùng PWM Hình 2.6: Mạch nguyên lý điều khiển tải PWM Hình 2.7: Giản đồ xung khóa điều khiển đầu Hình 2.8: Điều khiển hồi tiếp với điều khiển PID Hình 2.9: Đáp ứng nấc hệ hở có dạng chữ S Hình 2.10: Đáp ứng nấc hệ kín K = Kgh Hình 2.11: Sơ đồ cấu trúc điều khiển Electronic Lineshaft Hình 2.12: Ứng dụng điều khiển tùy động vị trí hệ thống bám trụ robot Hình 3.1: Sơ đồ cấu trúc tốc độ động Follower theo động Master Hình 3.2: Mô hình động chiều kích từ không đổi Hình 3.3: Mạch vòng điều khiển nối tầng dòng điện, tốc độ vị trí Hình 3.4: Mô hình mô truyền động đồng vị trí hai động DC theo phương pháp ELS Hình 3.5: Vị trí thực động Master Follower Hình 3.6: Tốc độ động Master Follower Hình 3.7: Vị trí thực động Master Follower Hình 3.8: Tốc độ động Master Follower Hình 3.9: Sơ đồ cấu trúc tốc độ động từ thông không đổi Hình 3.10: Sơ đồ cấu trúc mạch vòng dòng điện Hình 3.11: Mô hình mô hệ điều khiển bám vị trí điều khiển PID vii THIẾT KẾ ROBOT LEO TRỤ VÀ ĐIỀU KHIỂN Hình 3.12: Tín hiệu điều khiển Hình 3.13: Vị trí phản hồi sử dụng điều khiển PID Hình 3.14: Tốc độ động sử dụng điều khiển PID Hình 3.15: Dòng điện phản hồi sử dụng điều khiển PID Hình 4.1: Trụ mẫu cho robot Hình 4.2: Bản vẽ tổng quát thiết kế robot Hình 4.3: Thân robot Hình 4.4: Cấu tạo động điện DC trục vít bánh xe Hình 4.5: Góc nghiêng mặt trụ so với mặt đất Hình 4.6: Sơ đồ phân tích lực robot leo trụ Hình 4.7: Động DC trục vít bánh xe dùng cho robot Hình 4.8: Bánh xe từ với đĩa nam châm vĩnh cửu, đĩa sắt từ từ thông Hình 4.9: Độ giảm lực từ bánh xe bao phủ băng cao su Hình 4.10: Bánh xe chủ động dùng cho robot Hình 4.11: Trục ép hành trình 150mm Hình 4.12: Thiết kế khí hoàn thiện Hình 5.1: Cấu tạo Encoder Hình 5.2: Giản đồ xung hai đếm Avà B Hình 5.3: Encoder 100 xung Hình 5.4: Cảm biến siêu âm SRF05 Hình 5.5: Chế độ làm việc thứ SRF05 Hình 5.6: Chế độ làm việc thứ SRF05 Hình 5.7: Sơ đồ chân dsPIC30F4011 Hình 5.8: Sơ đồ chân PIC16F628A Hình 5.9: Mạch nạp PICKIT2-PLUS Hình 5.10: Sơ đồ mạch nguyên lý Eagle Hình 5.11: Sơ đồ mạch cấu tạo Eagle Hình 5.12: Sơ đồ mạch điều khiển hoàn thiện Hình 5.13: Giao diện giám sát Visual Basic 6.0 Hình 7.1: Quá trình di chuyển robot vii THIẾT KẾ ROBOT LEO TRỤ VÀ ĐIỀU KHIỂN Chƣơng KẾT QUẢ THỰC NGHIỆM 7.1 NHẬN XÉT QUÁ TRÌNH DI CHUYỂN Thao tác thực điều khiển robot: - Bước 1: Cấp nguồn cho mạch điều khiển kết nối cáp nối tiếp với PC - Bước 2: Ấn giữ nút FAR để trục ép đẩy ba bánh xe không tiếp xúc với chu vi đáy trụ dừng lại, lắp robot vào đáy trụ - Bước 3: Ấn nút START để chuyển trục ép sang chế độ tự động điều chỉnh lực ép vào trụ - Bước 4: Điều chỉnh biến trở để cài đặt tốc độ động cho phù hợp, thông số tốc độ hiển thị LCD - Bước 5: Ấn giữ nút UP để robot di chuyển lên ấn giữ nút DOWN để robot di chuyển xuống - Bước 6: Khi robot xuống đáy trụ, ấn nút STOP để chuyển trục ép sang chế độ điều khiển tay - Bước 7: Ấn giữ nút FAR để trục ép đẩy ba bánh xe không tiếp xúc với chu vi đáy trụ dừng lại, tháo robot khỏi trụ Hình 7.1: Quá trình di chuyển robot Trang 67 THIẾT KẾ ROBOT LEO TRỤ VÀ ĐIỀU KHIỂN Trong suốt trình robot di chuyển lên xuống, trục ép tự động điều chỉnh chạy vào để đảm bảo lực ép vào trụ không đổi.Lực ép tạo từ lực nén lò xo, lò xo nén chiều dài L Robot di chuyển làm chiều dài lò xo thay đổi L ± ΔL, cảm biến SRF05 điều chỉnh để triệt tiêu khoảng cách ΔL, giữ lực ép không đổi Sai số khoảng cách SRF05 trường hợp dao động tối đa 1mm Thời gian điều khiển trục ép nhanh giúp robot di chuyển với tốc độ cao Thuật toán điều khiển đồng tốc độ ba động giúp robot giữ cân tốt trình di chuyển, tránh tượng bánh xe bị trượt Khi xảy sai số tốc độ, điều khiển PID triệt tiêu sai số nhằm đưa ba động tốc độ Thời gian để ba động đồng tốc nhanh hay chậm phụ thuộc vào sai số lớn hay nhỏ Thông số khoảng cách tốc độ ba động hiển thị hình LCD giao diện giám sát Visual Basic 6.0, trình truyền nhận liệu nhanh, không xảy lỗi Khi robot di chuyển lên, trọng lượng robot lớn nên ta cần cài đặt tốc độ cao để động đủ momen kéo robot lên Khi robot di chuyển xuống, ta nên cài đặt tốc độ động thấp để tránh tượng bánh xe bị trượt Để kiểm tra đáp ứng điều khiển đồng tốc ta thực sau: - Cho ba động chạy không tải với tốc độ cố định, ta cho động Master mang tải, tốc độ động Master giảm xuống Ta thấy tốc độ hai động Follower giảm theo tức - Ngược lại, ta cho động Follower mang tải, tốc độ động Follower giảm xuống nhanh chóng tăng momen để vọt lên với tốc độ động Master Trang 68 THIẾT KẾ ROBOT LEO TRỤ VÀ ĐIỀU KHIỂN 7.2 NHẬN XÉT KẾT QUẢ GIÁM SÁT Thao tác điều khiển giao diện: Bước 1: Khởi động giao diện Visual basic PC Bước 2: Kết nối cáp truyền liệu từ PC đến CHIP xử lý mạch điều khiển, đồng thời bật nguồn cho mạch điều khiển Bước 3: Trên giao diện ta chọn cổng COM cần kết nối bấm KẾT NỐI Bước 4: Để ngắt kết nối PC với CHIP xử lý, ta bấm NGẮT Khi bắt đầu bấm KẾT NỐI, tất liệu từ Chip xử lý gửi liên tục lên giao diện với tốc độ 50ms/lần.Hiển thị số tốc độ ba động khoảng cách SRF05, đồng thời kèm theo đồ thị tốc độ đồ thị bám khoảng cách Trang 69 THIẾT KẾ ROBOT LEO TRỤ VÀ ĐIỀU KHIỂN Chƣơng KẾT LUẬN 8.1 NHỮNG KẾT QUẢ ĐẠT ĐƢỢC - Hiểu thêm kết nghiên cứu robot leo trụ giới ứng dụng robot thực tiễn - Đưa hướng thiết kế kết cấu khí robot leo trụ - Tìm phương pháp điều khiển phù hợp với mô hình - Kết mô góp phần lựa chọn thông số điều khiển phù hợp - Bổ sung thêm kiến thức lập trình C cho CHIP xử lý PIC với phần mềm hổ trợ - Kết hợp điều khiển – giám sát với mô hình, ghi nhận kết thực nghiệm Từ đó, khắc phục khuyết điểm đưa hướng phát triển 8.2 HẠN CHẾ CỦA ĐỀ TÀI - Phần thiết kế khí chưa đẹp, tồn sai số thiết kế, độ bền học chưa cao, trọng lượng lớn - Động truyền động sử dụng không mang tải thời gian dài - Mô chưa sát với thực tế - Chưa lắp ráp thiết bị sơn vệ sinh lên robot 8.3 HƢỚNG PHÁT TRIỂN CỦA ĐỀ TÀI - Thiết kế lại phần cứng robot, giảm trọng lượng - Chọn động có momen lớn, chế độ hoạt động dài hạn - Ứng dụng thêm phương pháp điều khiển Fuzzy logic, PID – Fuzzy,… để tăng tính ổn định cho động Trang 70 THIẾT KẾ ROBOT LEO TRỤ VÀ ĐIỀU KHIỂN TÀI LIỆU THAM KHẢO [1] Điều chỉnh truyền động điện tự động– Bùi Quốc Khánh - Nhà xuất Khoa học & Kỹ thuật – Hà nội – 1996 [2] Nghiên cứu nâng cao chất lượng hệ thống điều khiển truyền động nhiều động cơ- Ngô Xuân Hưng - Luận văn thạc sỹ - Đại học Hàng hải Việt Nam – 2008 [3] Nghiên cứu tổng hợp điều chỉnh mờ lai sử dụng hệ tùy động vị trí – Lê Quốc Hùng – Luận văn Thạc sĩ – Đại học Đà Nẵng [4].Lý thuyết điều khiển tự động thông thường đại - Quyển 1: Hệ tuyến tính- PGS TS Nguyễn Thương Ngô – Nhà xuất Khoa học & Kỹ thuật – Hà nội – 2004 [5].Lý thuyết điều khiển tuyến tính–GS TS Nguyễn Doãn Phước- Nhà xuất Khoa học & Kỹ thuật – Hà nội – 2009 [6] Lý thuyết điều khiển tự động – T.S Nguyễn Thị Phương Hà – Th.S Huỳnh Thái Hoàng – Nhà xuất ĐH quốc gia Tp Hồ Chí Minh - 2005 [7] MATLAP & SIMULINK dành cho kỹ sư điều khiển tự động – Nguyễn Phùng Quang [8] Hướng dẫn sử dụng chương trình lập trinh C cho vi điều khiển PIC – Trần Xuân Trường [9] Visual Basic – Created by mercury [10] SIEI – DGFC – ELS Electric line shaft instruction manual [11] LS – Cable – Technical Transfer Document [12] Design and Construction of a tree climbing Robot - Justin Gostanian, Erick Read, Michael A Gennert [13] Realization of a new compact magnetic wheeled climbing robot – Ursin Hutter [14] Kinematics Modeling of a Wheel-Based Pole Climbing Robot (UT-PCR)Ali Baghani, Majid Nili Ahmadabadi, Ahad Harati [15] Datashit dsPIC30F4011, PIC16F628A, SRF05 Trang 71 THIẾT KẾ ROBOT LEO TRỤ VÀ ĐIỀU KHIỂN PHỤ LỤC A)CHƢƠNG TRÌNH ĐIỀU KHIỂN CỦA DSPIC30F4011 #include #include #include #include #FUSES NOWDT, XT, NOPUT, NOPROTECT, NODEBUG, NOBROWNOUT //#use rs232(baud=4800,parity=n,bits=8,stop=1,uart2) //#use rs232(baud=4800,parity=n,bits=8,stop=1,uart1) #use rs232(baud=4800, xmit=PIN_f3, rcv=PIN_f2, parity=N, BITS=8, ERRORS, STREAM=pc) #use rs232(baud=4800, xmit=PIN_f5, rcv=PIN_f4, parity=N, BITS=8, ERRORS, STREAM=srf05) #use delay(clock=4000000) unsigned int tocdo=0,chieudai=0; unsigned int docadc=0; int nhan=0,snhan=0,nhanpc=0; int tam1=0,tam2=0,tam3=0; int dsrf05[4]={}; int i=0; int xs1=0,xs2=0,xs3=0,tang=0; int tr=0,ch=0,gui=0; int u1,u_11,u2,u_12,u3,u_13,e1,e_11,e_21,e2,e_12,e_22,e3,e_13,e_23; float A1,A2,A3,T,Kp,Ki,Kd; //****************khai bao cac chan I/O******************* #define nghich input(pin_d2) #define thuan input(pin_d3) #define dao1 pin_c13 #define dao2 pin_c14 #define dao3 pin_b5 void ADC_INIT()//khai bao adc { setup_adc(ADC_CLOCK_INTERNAL | ADC_TAD_MUL_31); setup_adc_ports(sAN4,VSS_VDD); //khai bao chan an0 la chan inout adc docadc= read_adc(); //doc gia tri adc set_adc_channel(4); } /*****************************************************/ #INT_EXT0 //vector ngat ngoai void ngat_ngoai0() { tam1++; } /*****************************************************/ Trang 72 THIẾT KẾ ROBOT LEO TRỤ VÀ ĐIỀU KHIỂN #INT_EXT1 //vector ngat ngoai void ngat_ngoai1() { tam3++; } /****************************************************/ #INT_EXT2 //vector ngat ngoai void ngat_ngoai2() { tam2++; } /***************************************************/ #INT_RDA // vector ngat noi tiep void RDA_isr() { nhanpc=fgetc(pc); if(nhanpc=='r') gui=1; if(nhanpc=='s') gui=0; } #INT_RDA2 // vector ngat noi tiep 628 void RDA2_isr() { nhan=fgetc(srf05); //fputc(nhan,pc); if(nhan=='*') snhan=1; if(nhan=='#') snhan=0; if(snhan==1){dsrf05[i]=nhan;i++;} if(snhan==0){i=0;chieudai=dsrf05[1];} } /******************************************************/ #INT_TIMER3 //vector ngat timer void interrupts_timer3() { SET_TIMER3(55535); tang++; docadc=read_adc(); tocdo=docadc/16; if(tang>=60) { tang=0; xs1=tam1; xs2=tam2; xs3=tam3; tam1=0; tam2=0; tam3=0; Trang 73 THIẾT KẾ ROBOT LEO TRỤ VÀ ĐIỀU KHIỂN } } //khai bao chuong trinh void ADC_INIT(); void RDA_isr(); void RDA2_isr(); void ngat_ngoai0(); void ngat_ngoai1(); void ngat_ngoai2(); void interrupts_timer2(); void dongtoc1(); void dongtoc2(); void PID_init(); void dongtoc3(); //khai bao chuong trinh chinh void main() { ADC_INIT(); //khoi dong adc lcd_init(); //khoi dong lcd PID_init();//khoi dong PID SETUP_TIMER3(TMR_INTERNAL|TMR_DIV_BY_1); //khoi dong timer SET_TIMER3(55535); //dat gia tri timer ENABLE_INTERRUPTS(INT_TIMER3); //khoi dong ngat timer enable_interrupts(int_rda); //khoi dong ngat noi tiep1 enable_interrupts(int_rda2); //khoi dong ngat noi tiep2 ENABLE_INTERRUPTS(INT_EXT2); //khoi dong ngat ngoai ENABLE_INTERRUPTS(INT_EXT1); ENABLE_INTERRUPTS(INT_EXT0); EXT_INT_EDGE(L_TO_H); //khoi dong ngat ngoai xung tu thap len cao ENABLE_INTERRUPTS(INT_EXT2); //khoi dong ngat ngoai enable_interrupts(INTR_GLOBAL); //khoi dong ngat toan cuc //lcd_gotoxy(1,1); //printf(lcd_putc,"LUAN VAN TH.S"); //lcd_gotoxy(1,2); //printf(lcd_putc,"ROBO LEO TRU"); setup_motor_pwm(1,MPWM_FREE_RUN,1,0,2000); set_motor_unit(1,1,MPWM_ENABLE_H|MPWM_FAULT_NO_CHANGE,0,0) ; set_motor_unit(1,2,MPWM_ENABLE_H|MPWM_FAULT_NO_CHANGE,0,0) ; set_motor_unit(1,3,MPWM_ENABLE_H|MPWM_FAULT_NO_CHANGE,0,0) ; set_motor_pwm_duty(1,1,0); set_motor_pwm_duty(1,2,0); set_motor_pwm_duty(1,3,0); while(1) Trang 74 THIẾT KẾ ROBOT LEO TRỤ VÀ ĐIỀU KHIỂN { lcd_gotoxy(1,1); printf(lcd_putc,"V/P:%3d",xs1); lcd_gotoxy(9,1); printf(lcd_putc,"%3d",xs2); lcd_gotoxy(14,1); printf(lcd_putc,"%3d",xs3); lcd_gotoxy(1,2); printf(lcd_putc,"T_on %4d",tocdo); lcd_gotoxy(11,2); printf(lcd_putc,"K/c%3d",chieudai); if(gui==1) { fputc(xs1,pc); fputc(xs2,pc); fputc(xs3,pc); fputc(chieudai,pc); } if(thuan==0) { output_low(dao1); output_low(dao2); output_low(dao3); delay_ms(15); set_motor_pwm_duty(1,1,tocdo); // set_motor_pwm_duty(1,2,tocdo); // set_motor_pwm_duty(1,3,tocdo); dongtoc3(); } if(nghich==0) { output_high(dao1); output_high(dao2); output_high(dao3); delay_ms(15); set_motor_pwm_duty(1,1,tocdo); // set_motor_pwm_duty(1,2,tocdo); // set_motor_pwm_duty(1,3,tocdo); dongtoc3(); } if((thuan==1)&&(nghich==1)) Trang 75 THIẾT KẾ ROBOT LEO TRỤ VÀ ĐIỀU KHIỂN { set_motor_pwm_duty(1,1,0); set_motor_pwm_duty(1,2,0); set_motor_pwm_duty(1,3,0); // u2=0; PID_init(); } } } void dongtoc3() { set_motor_pwm_duty(1,1,tocdo); set_motor_pwm_duty(1,2,u2); set_motor_pwm_duty(1,3,u3); e2= xs1-xs2 ; //chieudai-dem; // tinh sai so xac lap u2=u_12 + A1*e2 + A2*e_12 + A3*e_22; // tin hieu dieu khien //u(k)=u(k-1)+A1*e(k)+A2*e(k-1)+A3*e(k-2) u_12=u2; // cap nhat u(k-1), e(k-1), e(k-2) e_22=e_12; e_12=e2; if(u2>=4000) {u2=4000;u_12=4000;} if(u2=4000) {u3=4000;u_13=4000;} if(u389){near();} if((distance=87)) {stop();} if(distance[...]... dựng thuật toán điều khiển bám vị trí - Thiết kế mạch điều khiển và remote điều khiển - Lập trình vi điều khiển theo thuật toán của bộ điều khiển đã thiết kế - Kiểm tra kết quả đạt được và điều chỉnh sai số thông qua hoạt động thực tế của Robot - Nhận xét kết quả và kết luận 1.3.2 Phạm vi nghiên cứu Trang 8 THIẾT KẾ ROBOT LEO TRỤ VÀ ĐIỀU KHIỂN - Robot được thiết kế phù hợp với những trụ có đường kính... như trụ sắt, trụ gỗ, trụ bê tông, … 1.1.2 Phƣơng án thiết kế robot leo trụ Trước khi đưa ra hướng thiết kế, ta đã đưa ra nhiều ý tưởng có thể thực hiện.Sau khi lựa chọn được thiết kế tổng thể, ta bắt đầu thiết kế các bộ phận chi tiết khác nhau để hoàn thành thiết kế Các thiết kế chi tiếtbao gồm: - Thiết kế thân robot Trang 5 THIẾT KẾ ROBOT LEO TRỤ VÀ ĐIỀU KHIỂN - Lựa chọn cách thức di chuyển - Thiết kế. .. lượng robot nhẹ - Khả năng di chuyển nhanh - Robot có thể di chuyển trên nhiều bề mặt khác nhau như trụ sắt, trụ gỗ, trụ bê tông, … Nhược điểm: - Phải thiết kế hệ thống phanh hãm cho bánh xe - Kích thước robot phải phụ thuộc vào chu vi trụ 1.2 MỤC TIÊU VÀ ĐỐI TƢỢNG NGHIÊN CỨU Trang 7 THIẾT KẾ ROBOT LEO TRỤ VÀ ĐIỀU KHIỂN Mục tiêu của luận văn là mô phỏng, kiểm chứng, thiết kế và ứng dụng bộ điều khiển. .. Thứ hai, điều khiển động cơ tạo lực ép theo phương pháp điều khiển bám vị trí 1.3 NHIỆM VỤ CỦA ĐỀ TÀI VÀ PHẠM VI NGHIÊN CỨU 1.3.1 Nhiệm vụ của đề tài Để thực hiện mục tiêu trên, luận văn Thiết kế Robot leo tr và điều khiển tập trung các vấn đề sau: - Thiết kế kết cấu cơ khí cho Robot - Lựa chọn phương pháp điều khiển phù hợp - Mô phỏng để kiểm tra độ tin cậy của bộ điều khiển - Thiết kế cơ cấu tạo... nốivới robot thông qua mạng không dây Một người sẽ điều khiển chuyển động của robot Trang 2 THIẾT KẾ ROBOT LEO TRỤ VÀ ĐIỀU KHIỂN 1.1.1.3 Robot RiSE V3 RiSE V3 là một robot có chân được thiết kế chuyên dụng rất năng động, tốc độ leo trụ cao, ví dụ như một trụ điện thoại ngoài trời Robot có bốn chân mạnh mẽ, mỗi chân có hai khớp truyền động.Bước đi được điều chỉnh một cách linh hoạt [12] Hình 1.3: Mẫu robot. .. PHƢƠNG PHÁP ĐIỀU KHIỂN PID 2.3.1 Giới thiệu bộ điều khiển PID Bộ điều khiển PID được sử dụng rất rộng rãi trong thực tế để điều khiển các hệ SISO theo nguyên lý hồi tiếp Cácđối tượng điều khiển phổ biến như nhiệt độ lò nhiệt, tốc độ động cơ, mực chất lỏng trong bồn chứa w + e - PID u Đối tượng ĐK y Hình 2.8: Điều khiển hồi tiếp với bộ điều khiển PID Trang 18 THIẾT KẾ ROBOT LEO TRỤ VÀ ĐIỀU KHIỂN Nếu... nhiều chân Robot nhiều chân là một mẫu robot phân đoạn với hai chân cho mỗi phân đoạn Thiết kế dựa trên hoạt động của một con rết Thiết kế này sẽ cho phép chúng ta dễ dàng điều hướng cho robot trong lúc nó di chuyển [12] Thiết kế này sẽ không phù hợp với đề tài vì số lượng của bộ phận chuyển động và kích thước của robot sẽkhá lớn, và nó sẽ không thể hoàn Trang 6 THIẾT KẾ ROBOT LEO TRỤ VÀ ĐIỀU KHIỂN thànhvới... tốc độ nhiều động cơ (ELS) kết hợp với phương phápđiều xung PWM để điều khiển động cơ - Ứng dụng phương pháp điều khiển bám vị trí để điều khiển trục ép cho bánh xe truyền động chính - Sử dụng kết quả mô phỏng kết hợp với kết quả thực nghiệm để kiểm chứng độ tin cậy của bộ điều khiển Chƣơng 2 Trang 9 THIẾT KẾ ROBOT LEO TRỤ VÀ ĐIỀU KHIỂN CƠ SỞ LÝ THUYẾT 2.1 CƠ SỞ LỰA CHỌN PHẦN MỀM MÔ PHỎNG – LẬP TRÌNH... trước, kết quả đưa vào bộ điều khiển tốc độ của động cơ Follower Trong chế độ điều khiển vị trí thì bộ điều khiển này có vai trò như là bộ điều khiển bù nhiễu nhằm bù lại các sai lệch bậc cao về vị trí cho động cơ Follower Để tìm được hàm truyền tốc độ động cơ Follower theo động cơ Master, cấu trúc điều khiển động cơ Follower được đề xuất như Hình 3.1 Trang 23 THIẾT KẾ ROBOT LEO TRỤ VÀ ĐIỀU KHIỂN Tốc... và khôi phục đối tượng điều khiển một cách chính xác không có nhầm lẫn Ví dụ điều khiển cơ cấu ép trục cán trong quá trình cán kim loại, phải làm cho khe hở giữa hai trục cán có thể tiến hành tự điều chỉnh; điều khiển quỹ tích gia công của máy cắt điều khiển số và điều khiển bám của Trang 24 THIẾT KẾ ROBOT LEO TRỤ VÀ ĐIỀU KHIỂN máy cắt mô phỏng hình; cơ cấu nâng hạ có thể làm cho dừng chính xác ở những