Thiết kế và điều khiển robot tự hành tránh vật cản

BỘ GIÁO DỤC VÀ ĐÀO TẠO ĐẠI HỌC QUỐC GIA THÀNH PHỐ HỒ CHÍ MINH TRƯỜNG ĐẠI HỌC BÁCH KHOA LÊ VŨ HẢI ĐỀ TÀI: THIẾT KẾ VÀ ĐIỀU KHIỂN ROBOT TỰ HÀNH TRÁNH VẬT CẢN LUẬN VĂN CAO HỌC CHUN NGÀNH: CHẾ TẠO MÁY NĂM 2002 LỜI CẢM ƠN ]] ^] Tôi xin chân thành cảm ơn Thầy TS NGUYỄN VĂN GIÁP tận tình hướng dẫn làm Luận văn tốt nghiệp Thầy dành cho giúp đỡ nhiệt tình suốt thời gian thực Luận văn Tôi xin chân thành cảm ơn thầy cô mộn Cơ Điện Tử tận tình giúp để thời gian làm luân văn Tôi xin chân thành cám ơn thầy TS LÊ HOÀI QUỐC TS NGUYỄN TIẾN DŨNG dành thời gian quý báu để nhận xét phản biện đề tài tốt nghiệp Tôi chân thành bày tỏ lòng biết ơn chân thành đến quý Thầy/Cô Chủ tịch, Phản biện y viên Hội đồng dành nhiều thời gian quý báu để nhận xét tham gia Hội đồng chấm Luận văn Chân thành cảm ơn LÊ VŨ HẢI TÓM TẮT Đối với robot tự hành tránh vật cản để hoạt động bao gồm tìm kiếm đường đi, di chuyển tới trước đến đích cách thông minh thông tin cần cảm nhận tự động phải liên quan đến cần điều khiển Dọc theo đường xa lộ đường tự tạo khác, cụ thể công nghiệp, q đạo robot tự hành phân thành đoạn đường liên tiếp Nếu robot di chuyển dọc theo quỹ đạo cách xác robot đến đích Cái cần điều khiển biến vật lý để lái hệ thống, thực chuyển động tịnh tiến quay mong muốn Trong luận văn trình bày cách biến điều khiển (góc lái vận tốc) robot rự hành bị giới hạn đường cong thay đổi để đạt vị trí hướng mong muốn Trong thực tế để điều khiển robot đích xác định trước robot cần phải nhận biết mội trường xung quanh cảm biến Sau thông tin cảm biến truyền phận xử lý Để giải vấn đề luận văn trình bày loại cảm biến sử dụn rộng rãi robot tự hành vi xử lý 8051 có nhiệm vụ xử lý tín hiệu cảm biến điều khiển phận thực chuyền động động bước Đối với robot tự hành thông thường di chuyển theo đường thẳng, gặp vật cản robot dừng lại bẻ lái, tiếp tục tới qua khỏi vật cản dừng lại bẻ lái trở lại, lại tiếp tục di chuyển Nếu không tối ưu thời gian di chuyển Để khắc phục điều luận văn có trình bày cách trơn hóa quỹ đạo di chuyển tối ưu hóa định robot gặp nhiều vật cản phía trước ABSTRCT For a mobile robot to act autonomously and intelligently while seeking out, moving toward, and reaching its goal, what can needs to be sensed must be automatically relateed to what can needs to be controlled Along highway systems and on other man-made paths, particularly within an urban industrial interior, the trajectories of a mobile robot may be decompresed into a sequence of “straight” line paths and “reorientation” that if actually moved along would enable it to reach its spatial goal What needs to be controlled are physical variables to drive system, which if there were no disturbances and error, would result in the desired translations and rotations Unless the vehicle can rotate without translation it has to move in order to turn The thises present how the control variables (speed and steer angle) of a path curvature limited mobile robot may be altered in order to achive desirable change in the global position and orientation In practice, a robot can only move to a specified place when it has realized its surrounding positions by a sensor (Then, the information sent from the sensor shall be transmitted to a analysis unit.) In this thesis, I shall represent some types pf sensors widely used in the mobile robot and a micro-controller 8051.This micro-controller undertakes the analysis of signals sent from the sensor and then orders another unit to operate the robot, called a step motor In addition, I also show some kinds of step motors and methods of controlling them As for the mobile robot, it usually moves in a straight line.When it meets an obstacle, it will stop and then turn right or left to avoid the ostacle After that, it will self-drive to the programmed straight line and go on moving to the destiation However, this causes the time of its movement not being optimum In order to improve this problem, in this thesis, I shall represent the way of - when the robot is moving and the way of optimization of making decision when the robot meets many obstacles Luân Văn Cao Học: ROBOT TỰ HÀNH TRÁNH VẬT CẢN MỤC LỤC CHƯƠNG TỔNG QUAN 1.1 ĐẶT VẤN ĐỀ 1.2 MỤC TIÊU NGHIÊN CỨU 1.3 MỘT SỐ CÔNG TRÌNH Đà NGHIÊN CỨU 1.3.1 Trong lónh vực thám hiểm 1.3.2 Trong lónh vực công nghiệp 1.3.3 Một số ứng dụng sinh hoạt 3 CHƯƠNG THIẾT KẾ ROBOT TỰ HÀNH TRÁNH VẬT CẢN 2.1 ĐỘNG LỰC HỌC ROBOT 2.1.1 Kết cấu khung robot 2.1.2 Bánh trước 2.1.3 Bánh sau 2.2 ĐỘNG LỰC LỰC HỌC ROBOT 2.2.1 Lực tác động lên bánh xe 2.2.2 Tính công suất chọn động truyền động KẾT LUẬN 6 12 12 13 CHƯƠNG ĐIỀU KHIỂN ROBOT 3.1 CÁC LOẠI CẢM BIẾN DÙNG TRONG ROBOT 18 3.1.1 Các loại cảm biến đo khoảng cách dựa thời gian truyền (time of flight - TOF) 18 3.1.2 Các loại cảm biến đo khoảng cách dựa kỹ thuật thay đổi pha 13 3.2 CÁC PHƯƠNG PHÁP ĐIỀU KHIỂN ĐỘNG CƠ 27 3.2.1 Động bước 27 3.2.2 Phương pháp điều động 30 3.3 SƠ ĐỒ MẠCH ĐIỀU KHIỂN VÀ XỬ LÝ TÍN HIỆU 35 3.3.1 Sơ lược chân 8051 35 3.1 Bảng giải mã điều khiển 36 3.3.3 mạch khuyếch đại công suất 36 KẾT LUẬN GVHD: TS.NGUYỄN VĂN GIÁP HV : LÊ VŨ HẢI Luân Văn Cao Học: ROBOT TỰ HÀNH TRÁNH VẬT CẢN CHƯƠNG GIẢI THUẬT TRÁNH VẬT CẢN 4.1 GIẢI THUẬT TRÁNH VẬT CẢN Đà BIẾT TRƯỚC 4.1.1 Giải thuật cho vật cản đa giác lồi 4.1.2 Giải thuật cho vật cản có dạng hình tròn 4.2 GIẢI THUẬT CHO VẬT CẢN CHƯA BIẾT TRƯỚC 4.2.1 Phương pháp theo đường biên vật cản 4.2.2 Phương pháp phát biên dạng đối tượng 4.2.3 Phương pháp chưa lưới 4.2.4 Phương pháp trường tiềm (Potetial Field Method) 4.3 GIẢI THUÂT DẪN HƯỚNG THÍCH NGHI CHO ROBOT TRÁNH VÂT CẢN 4.3.1 Chế độ di chuyển thích nghi 4.3.2 Lưu đồ giải thuật 4.4 TRƠN HÓA QUỸ ĐẠO 4.5 TỐI ƯU HOÁ RA QUYẾT ĐỊNH BẲNG PHƯƠNG PHÁP LẬPTRÌNH ĐỘNG NGƯC (BACK DYNAMIC PROGRAMING) 4.5.1 Thủ tục liệt kê 4.5.2Nguyên lý tối ưu KẾT LUẬN GVHD: TS.NGUYỄN VĂN GIÁP HV : LÊ VŨ HẢI 40 40 42 45 45 45 46 47 48 48 51 51 57 57 58 MỤC LỤC HÌNH TÊN HÌNH Hình 1.1: Robot làm việc hỏa Hình 1.2: Robot tự hành công nghiệp Hình 1.3: Robot tự hành có trang bị thêm tay máy Hình 1.4a: Xe lăn cho bệnh nhân mù Hình 1.4b: Đai thắt lưng cho người mù Hình 1.4c: Gậy cho người mù Hình 1.5: Robot chó Hình 2.1: Kết cấu phần khí Hình 2.2: Hình chiếu xe Hình 2.3: Bánh lái di chuyển dọc theo cung tròn Hình 2.4: Khoảng cách mà bánh trước di chuyển chiếu lên hệ tọa độ gắn với xe Hình 2.5: Tính toán bánh sau Hình 2.6: Phân bố lực Hình 3.1: Kit Polaroid OEM Hình 3.2: Bộ biến tónh điện mức thiết bị Polaroid Hình 3.3: Sơ đồ mạch module đo khoảng cách 6500 Hình 3.4: Sơ đồ thời gian module tính khoảng cách 6500 Hình 3.5: Sơ đồ thời gian module tính khkoảng cách 6500 chế độ multi- echo Hình 3.6: Sự liên hệ dạng sóng phản xạ, x khoảng cách tương ứng với thay đổi pha Hình 3.7: Mạch đo thay đổi pha Hình 3.8: Sơ đồ hệ thống đo khoảng cách ORS- Hình 3.9: Hệ thống đo khoảng cách ORS – Hình 3.10: Mạch nguyên lý động bước có từ trở thay đổi Hình 3.11: Đồ thị da91p ứng dòng điện Hình 3.12: Cách khắc phục tượng cảm ứng Hình 3.13: Mạch điều khiển chung Hình 3.14: Cách khắc phục tượng cảm ứng Hình 3.15: Mạch cầu Hình 3.16: Mạch điều khiển động bước lưỡng cực Hình 3.17: Mối hiên hệ vị trí lý thuyết thực tế TRANG 4 5 5 10 13 20 20 21 21 22 24 25 26 26 27 27 27 28 28 29 29 33 động Hình 3.18: Đáp ứng dòng điện Hình 3.19: Sơ đồ chân 8051 Hình 3.20: Mạch khuyếch đại công suất Hình 4.1: Vị trí điểm S D không gian R2 Hình 4.2: Sơ đồ bóng vật cản Hình 4.3: Sơ đồ tránh vật cản hình tròn Hình 4.4: Quỹ đạo xe theo đường biên vật cản Hình 4.5: Phương pháp xác định khoảng cách phương pháp chia lưới Hình 4.6: Xác định lực tổng hợp R Hình 4.7: Vị trí robot không gian thực Hình 4.8: Lưu đồ giải thuật Hình 4.9: Trình bày vị trí bắt đầu, kết thúc hướng bắt đầu, kết thúc robot sau áp dụng biên dạng hàm gaussian cho góc lái Sự thay đổi theo thời gian giá trị hàm gaussian Hàm có φmax t = tI Hình 4.10: Ảnh hưởng đến hướng vị trí cuối robot thay đổi biên độ, vận tốc độ lệch chuẩn cho trước không thay đổi Hình 4.11: Trình bày ảnh hưởng vị trí hướng cuối xe thay đổi vận tốc, độ lệch chuẩn biên độ không thay đổi Hình 4.12: Mối liên hệ vị trí đầu xe đưa hàm gaussian có biên độ thay đổi vào phương trình động học với vận tốc khác Hình 4.13: Kết hợp hai hàm gaussian có dấu ngược phương trình động học Hình 4.14: ng dụng hàm gaussian để tránh vật cản Hình 4.15: Ứng dụng hàn gaussian để đậu xe vào hai xe Hình 4.6: Cây định 34 35 37 40 42 43 45 46 47 49 51 52 53 53 54 55 55 56 57 LỜI NÓI ĐẦU Khoa học kỹ thuật giới ngày phát triển đòi hỏi kỹ thuật điều khiển ngày phát triển mà kỹ thuật điều khiển robot điển hình lónh vực Những đóng góp robot công nghiệp nhỏ, ví góp phần làm tăng xuất lao động, giảm chi phí sản xuất, đồng thời giải phóng người khỏi lao động cực nhọc, nguy hiểm Không sản xuất, lónh vực thám hiểm, nơi ngøi đến chưa đếùn đựơc robot thay người làm việc Trong lónh vực y khoa robot giúp người chụp hình ảnh bên thân thể người Trong lónh vực dân dụng robot giúp người công việc nấu nướng, lau nhà, giặt ủi Trong số lónh vực đòi hỏi robot phải di chuyển, tự xử lý tình xảy tránh chướng ngại vật đường đi, ngày đòi hỏi nhiều có nhiều nhà khoa học nghiên cứu Luận Văn Cao Học: ROBOT TỰ HÀNH TRÁNH VẬT CẢN CHƯƠNG TỔNG QUAN 1.1 ĐẶT VẤN ĐỀ 1.2 MỤC TIÊU NGHIÊN CỨU 1.3 MỘT SỐ CÔNG TRÌNH Đà NGHIÊN CỨU 1.1.1 Trong lónh vực thám hiểm 1.1.2 Trong lónh vực công nghiệp 1.1.3 Một số ứng dụng sinh hoạt 3 GVHD: TS.NGUYỄN VĂN GIÁP HV : LÊ VŨ HẢI Luận Văn Cao Học: ROBOT TỰ HÀNH TRÁNH VẬT CẢN Phương trình (4-9) giải thích “phần” nguyên lý tối ưu sau: Chi phí nhỏ trạng thái x, giai đoạn n, tìm thấy cách tối thiểu tổng số trạng thái đơn Ln(vn, x) chi phí nhỏ đến kết thúc trình từ kết trạng thái kế gn(x, vn) Chúng ta có htể thấy việc hàm chi phí tối thiểu ⎛ N−1 ⎞ I[n +1, gn (x, )] = ⎜⎜ ∑Lj (vj , y j ) + LN (yN )⎟⎟ vn+1, vN−1 ⎝ j=n+1 ⎠ ngược trạng thái thời điểm tìm giá trị nhỏ trạng thái giai đoạn việc tính trình thời điểm Hàm vn(x) mà đạt nhỏ phía bên phải phương trình (4-9) gọi phản hồi tối ưu (optional Feedback) thu điều khiển tối ưu vn, vn+1, …, vN-1 cho toán việc đặt v&n = v&n (x) v&n+1 = v&n+1 ( y&n+1 ) y&n+1 = g n ( x, v&n ) y&n+2 = g n+1 ( y&n+1 , v&n+1 ) 59 GVHD: TS.NGUYỄN VĂN GIÁP HV : LÊ VŨ HẢI Luận Văn Cao Học: ROBOT TỰ HÀNH TRÁNH VẬT CẢN KẾT LUẬN Giải thuật tránh vật cản biết trước + Giải thuật cho vật cản đa giác lồi Giải thuật xem đa giác lồi đa giác tạo cạnh đa giác nằm mặt phẳng Các đa giác lõm qui đa giác lồi Để thực giải thuật ta phải: tạo đồ thị ứng dụng giải thuật Dijkstra để tìm đường ngắn Giải thuật cho vật cản hình tròn: xác định bóng vật cản, xem điểm xuất phát có nằm vùng bóng hay không, không đường ngắn đường thẳng từ điểm xuất phát đến điểm đích, vùng bóng phải tính để chọn đường ngắn hai đường để đến đích điểm xuất phát Giải thuật cho vật cản chưa biết trước toạ độ Phương pháp theo đường biên vật cản: robot phát có vật cản coi tường di chuyển dọc theo tường vị trí ban đầu Phương pháp phát đường biên đối tượng: Phương pháp dựa kỹ thuật phát đường biên sau dùng giải thuật để tìm vị trí biên đối tượng, robot lái qua điểm biên Phương pháp Potetial Field: Vậït cản sinh lực đẩy, đích tạo lực hút tổng hai lực tác dụng lên robot Nếu lực R lớn giá trị đổi hướng robot tiếp tục di chuyển phía trước Giải thuật dẫn đường thích nghi: Robot di chuyển theo hai hệ thống dẫn đường: toàn cục cục Hệ thống dẫn đường toàn cục đường ngắn từ điểm xuất phát đến điểm đích Nhưng đường phát vật cản phải sử dụng hệ thống dẫn đường cục Hệ thống dẫn đường tùy thuộc vào hình dáng vị trí vật cản Trơn hoá quỹ đạo: Khi robot phát có vật cản, không đứng lại mà tiếp tục di chuyển đồng thời thay đổi góc lái, qua khỏi vật cản góc lái có giá trị ban đầu mà quay góc lái trở lại (ngược lại) Tối ưu hóa định phương pháp lập trình động ngược: Trên quản đường robot có k vật cản k thời điểm mà 60 GVHD: TS.NGUYỄN VĂN GIÁP HV : LÊ VŨ HẢI Luận Văn Cao Học: ROBOT TỰ HÀNH TRÁNH VẬT CẢN thời điểm có hai lựa chọn có 2k trường hợp Sau tính toán 2k trường hợp chọn trường hợp có đường ngắn Ta thấy toán trở nên phức tạp k lớn Vì vậy, người ta tính tối ưu cho trạng thái thứ k sau từ trạng thái ta ngược lại để chọn đường di chấp nhận (chứ chưa phải tối ưu) 61 GVHD: TS.NGUYỄN VĂN GIÁP HV : LÊ VŨ HẢI KẾT LUẬN Trong Luận văn làm việc sau : ™ Tạo mô hình robot di chuyển hai motor bước ™ Nghiên cứu ứng dụng loại cảm biến dùng cho robot ™ Tìm hiểu cảm biến đo khoảng cách tia hồng ngoại (Sharp infrared ranger GP2D02) ™ Nghiên cứu giải thuật tránh vật cản cho robot có sử dụng cảm biến khoảng cách ™ Tìm hiểu họ vi điều khiển 8051 Tuy nhiên đề tài số khuyết điểm : Việc cua phải trái robot tối ưu chỗ gặp vật cản chưa phải tối ưu cho toàn quỹ đạo - Khó khăn việc xác định vị trí robot mặt phẳng vị trí cuối Do xác định vị trí robot cách cộng dồn (điều khiển theo vòng hở) nên khó tránh khỏi sai số tích luỹ trình robot di chuyển Vì thời gian kiến thức hạn hẹp luân văn không tránh đước thiếu sót, mong nhận đóng góp quý thầy bạn bè - TÀI LIỆU THAM KHẢO [1] TỐN VĂN ÔN Họ Vi Điều Khiển 8051 NXB LAO ĐỘNG – Xà HỘI [2] I SCOTT MACKENZIE The 8051 Microcontroller PRENTICE HALL [3] DƯƠNG MINH TRÍ Sổ Tay Sơ Đồ Chân Linh Kiện Điện Tử [4] JAMES CROWLEY, MEMBER, IEEE IEEE JUORNAL ROBOTICS AND AUTOMATION, VOL RA1, NO1 MARCH 1985 [5] BARRY STEER ROBOTIC RESEARCH GROUP–DEPARTMENT OF ENGINEERING SCIENCE – UNIVERSITY OF OXFORD [6] THE MIT PRESS ROBOTICS RESEACH – VOLUME7 [7] THE MIT PRESS ROBOTICS RESEACH – VOLUME8 [8] DEVDAS SHETTY Mechatronic System Design Luân Văn Cao Học: ROBOT TỰ HÀNH TRÁNH VẬT CẢN PHỤ LỤC CHƯƠNG TRÌNH ĐỌC CẢM BIẾN ORG 0000H CLR A MOV P0, A MOV P1, A MOV P2, A MOV P3, A BATDAU: CLR A MOV R0, A MOV R1, A MOV R2,A MOV R3, #8; BIEN DEM SO LAN DOC MOV P1, #0FFH ACALL DELAY15mS MOV P1, #00H ACALL DELAY70mS LOOP1:MOV P1, #0FFH ACALL DELAY02mS MOV P1, #00H NOP NOP MOV B, P2 JB B.0, BR0 CLR C MOV A, R0 RLC A MOV R0, A BR01:JB B.1, BR1 CLR C MOV A, R1 RLC A 62 GVHD: TS.NGUYỄN VĂN GIÁP HV : LÊ VŨ HẢI Luân Văn Cao Học: ROBOT TỰ HÀNH TRÁNH VẬT CẢN MOV R1, A BR02:JB B.2, BR2 CLR C MOV A, R2 RLC A MOV R2, A SJMP LOOP2 BR0:SETB C MOV A, R0 RLC A MOV R0, A SJMP BR01 BR1:SETB C MOV A, R1 RLC A MOV R1, A SJMP BR02 BR2:SETB C MOV A, R2 RLC A MOV R2, A LOOP2:ACALL DELAY01mS DJNZ R3, LOOP1 MOV P1, #0FFH ; GIAI MA KHOANG CACH ACALL GIAIMA CJNE R0, #01H, LOOP3 SETB P3.0 LOOP31:CJNE R1, #01H,LOOP4 SETB P3.1 LOOP41:CJNE R2, #01H,LOOP5 SETB P3.2 SJMP BATDAU LOOP3:CLR P3.0 SJMP LOOP31 63 GVHD: TS.NGUYỄN VĂN GIÁP HV : LÊ VŨ HẢI Luân Văn Cao Học: ROBOT TỰ HÀNH TRÁNH VẬT CẢN LOOP4:CLR P3.1 SJMP LOOP41 LOOP5:CLR P3.2 SJMP BATDAU DELAY70mS: MOV B, #70 LOOP70:ACALL DELAY1mS DJNZ B, LOOP70 RET DELAY01mS: MOV TMOD, #01H MOV TH0,#HIGH(-42) MOV TL0, #LOW(-42) SETB TR0 WAIT1: JNB TF0, WAIT1 CLR TF0 CLR TR0 RET DELAY02mS: MOV TMOD, #01H MOV TH0,#HIGH(-100) MOV TL0,#LOW(-100) SETB TR0 WAIT2: JNB TF0, WAIT2 CLR TF0 CLR TR0 RET DELAY1mS: MOV TMOD, #01H LOOP1mS: MOV TH0,#HIGH(-1000) MOV TL0, #LOW(-1000) SETB TR0 WAIT: JNB TF0, WAIT CLR TF0 CLR TR0 64 GVHD: TS.NGUYỄN VĂN GIÁP HV : LÊ VŨ HẢI Luân Văn Cao Học: ROBOT TỰ HÀNH TRÁNH VẬT CAÛN RET DELAY15mS: MOV TMOD, #01H MOV TH0,#HIGH(-2000) MOV TL0,#LOW(-2000) SETB TR0 WAIT15:JNB TF0, WAIT15 CLR TF0 CLR TR0 RET DELAY1S: MOV B, #20 LOOP1S:MOV TMOD, #01H MOV TH0,#HIGH(-50000) MOV TL0,#LOW(-50000) SETB TR0 WAIT1S:JNB TF0, WAIT1S CLR TF0 CLR TR0 DJNZ B, LOOP1S RET GIAIMA: MOV A, R0 MOV DPTR, #KHOANGCACH MOVC A, @A+DPTR MOV R0, A MOV A, R1 MOV DPTR, #KHOANGCACH MOVC A, @A+DPTR MOV R1, A MOV A, R2 MOV DPTR, #KHOANGCACH MOVC A, @A+DPTR MOV R2, A RET 65 GVHD: TS.NGUYỄN VĂN GIÁP HV : LÊ VŨ HẢI Luân Văn Cao Học: ROBOT TỰ HÀNH TRÁNH VẬT CẢN KHOANGCACH: DB 00H,00H,00H,00H,00H,00H,00H,00H,00H,00H,00H,00H,00H DB 00H,00H,00H,00H,00H,00H,00H,00H,00H,00H,00H,00H,00H DB 00H,00H,00H,00H,00H,00H,00H,00H,00H,00H,00H,00H,00H DB 00H,00H,00H,00H,00H,00H,00H,00H,00H,00H,00H,00H,00H DB 00H,00H,00H,00H,00H,00H,00H,00H,00H,00H,00H,00H,00H DB 00H,00H,00H,00H,00H,00H,00H,00H,00H,00H,00H,00H,00H DB 00H,00H,00H,00H,00H,00H,00H,00H,00H,00H,00H,00H,00H DB 00H,00H,00H,00H,00H,00H,00H,00H,00H,00H,00H,00H,00H DB 00H,00H,00H,00H,00H,00H,00H,00H,00H,00H,00H,00H,00H DB 00H,00H,00H,01H,01H,01H,01H,01H,01H,01H,01H,01H,01H DB 01H,01H,01H,01H,01H,01H,01H,01H,01H,01H,01H,01H,01H DB 01H,01H,01H,01H,01H,01H,01H,01H,01H,01H,01H,01H,01H DB 01H,01H,01H,01H,01H,01H,01H,01H,01H,01H,01H,01H,01H DB 01H,01H,01H,01H,01H,01H,01H,01H,01H,01H,01H,01H,01H DB 01H,01H,01H,01H,01H,01H,01H,01H,01H,01H,01H,01H,01H DB 01H,01H,00H,00H,00H,00H,00H,00H,00H,00H,00H,00H,00H DB 00H,00H,00H,00H,00H,00H,00H,00H,00H,00H,00H,00H,00H DB 00H,00H,00H,00H,00H,00H,00H,00H,00H,00H,00H,00H,00H DB 00H,00H,00H,00H,00H,00H,00H,00H,00H,00H,00H,00H,00H DB 00H,00H,00H,00H,00H,00H,00H,00H,00H END 66 GVHD: TS.NGUYỄN VĂN GIÁP HV : LÊ VŨ HẢI Luân Văn Cao Học: ROBOT TỰ HÀNH TRÁNH VẬT CẢN PHỤ LỤC CHƯƠNG TRÌNH ĐIỀU KHIỂN ĐỘNG CƠ ORG 0000H CLR A MOV R0, A MOV R1, A MOV R2, A MOV R3, A MOV R4, A MOV R5, A MOV R6, A MOV R7, A MOV P0, A MOV P1, A MOV P2, A MOV P3, A MOV B, A MOV 57H, A MOV 56H, A MOV 55H, A MOV 54H, A LOOP1: ACALL GIAIMA CJNE A, #01H, PHAI ACALL QUEOTRAI ACALL DEMTR SJMP LOOP1 PHAI:CJNE A, #02H, THANG ACALL QUEOPHAI ACALL DEMPH SJMP LOOP1 THANG:CJNE R1, #00H, I CJNE R2, #00H, II 67 GVHD: TS.NGUYỄN VĂN GIÁP HV : LÊ VŨ HẢI Luân Văn Cao Học: ROBOT TỰ HÀNH TRÁNH VẬT CẢN CJNE R3, #00H, III1 CJNE R4, #00H, III2 CJNE R5, #00H, III31 CJNE R6, #00H, III32 ACALL DITHANG SJMP LOOP1 I:ACALL CI SJMP LOOP1 II:ACALL CII SJMP LOOP1 III1:MOV A, 57H JZ QUA1 DEC A MOV 57H, A ACALL QUEOPHAI NOP ACALL QUEOPHAI QUA1:ACALL CIII1 SJMP LOOP1 III2:MOV A, 56H JZ QUA2 DEC A MOV 56H, A ACALL QUEOTRAI NOP ACALL QUEOTRAI QUA2:ACALL CIII2 SJMP LOOP1 III31:MOV A, 55H JZ QUA31 DEC A MOV 55H, A ACALL QUEOTRAI QUA31:ACALL CIII31 SJMP LOOP1 68 GVHD: TS.NGUYỄN VĂN GIÁP HV : LÊ VŨ HẢI Luân Văn Cao Học: ROBOT TỰ HÀNH TRÁNH VẬT CAÛN III32:MOV A, 54H JZ QUA32 DEC A MOV 54H, A ACALL QUEOPHAI QUA32:ACALL CIII32 SJMP LOOP1 DITHANG: MOV P3, #03H ACALL DITHANG2 RET QUEOTRAI: MOV P3, #02H ACALL DITHANG2 RET QUEOPHAI: MOV P3, #01H ACALL DITHANG2 RET CI: ACALL DITHANG DEC R1 INC R3 RET CII:ACALL DITHANG DEC R2 INC R4 RET CIII1:ACALL DITHANG DEC R3 INC R5 RET CIII2:ACALL DITHANG DEC R4 INC R6 RET 69 GVHD: TS.NGUYEÃN VĂN GIÁP HV : LÊ VŨ HẢI Luân Văn Cao Học: ROBOT TỰ HÀNH TRÁNH VẬT CẢN CIII31:ACALL DITHANG DEC R5 RET CIII32:ACALL DITHANG DEC R6 RET DITHANG2: MOV R7, #24 MOV DPTR, #THUAN MOV A, #00000000B LOOP3:MOV R0, A MOVC A, @A+DPTR MOV P1, A ACALL DELAY MOV A, R0 INC A DJNZ R7, LOOP3 RET DELAY: MOV TMOD, #01H MOV TH0, #HIGH(-10000) MOV TL0, #LOW(-10000) SETB TR0 WAIT: JNB TF0, WAIT CLR TF0 CLR TR0 RET GIAIMA:MOV A, P2 MOV DPTR, #CB MOVC A, @A+DPTR RET DEMTR: MOV R1, #16 MOV B, 57H MOV A, 55H 70 GVHD: TS.NGUYỄN VĂN GIÁP HV : LÊ VŨ HẢI Luân Văn Cao Học: ROBOT TỰ HÀNH TRÁNH VẬT CẢN INC B ; BIEN DEM SO LAN QUEO INC A MOV 57H, B MOV 55H, A RET DEMPH: MOV R2, #16 MOV B, 56H MOV A, 54H INC B ; BIEN DEM SO LAN QUEO INC A MOV 56H, B MOV 54H, A RET THUAN: DB 11H,33H,22H,66H,44H,0CCH,88H,99H,11H,33H,22H,66H,44H DB 0CCH,88H,99H,11H,33H,22H,66H,44H,0CCH,88H,99H,11H,33H DB 22H,66H,44H,0CCH,88H,99H,11H,33H,22H,66H,44H,0CCH,88H DB 99H,11H,33H,22H,66H,44H,0CCH,88H,99H,11H,33H,22H,66H DB 44H,0CCH,88H,99H,11H,33H,22H,66H,44H,0CCH,88H,99H CB: DB 03H, 02H, 02H, 02H, 01H, 01H, 01H, 01H END 71 GVHD: TS.NGUYỄN VĂN GIÁP HV : LÊ VŨ HẢI ... 1.5 Robot chó GVHD: TS.NGUYỄN VĂN GIÁP HV : LÊ VŨ HẢI Luân Văn Cao Học: ROBOT TỰ HÀNH TRÁNH VẬT CẢN CHƯƠNG THIẾT KẾ ROBOT TỰ HÀNH TRÁNH VẬT CẢN 2.1 ĐỘNG LỰC HỌC ROBOT 2.1.1 Kết cấu khung robot. .. Văn Cao Học: ROBOT TỰ HÀNH TRÁNH VẬT CẢN CHƯƠNG GIẢI THUẬT TRÁNH VẬT CẢN 4.1 GIẢI THUẬT TRÁNH VẬT CẢN Đà BIẾT TRƯỚC 4.1.1 Giải thuật cho vật cản đa giác lồi 4.1.2 Giải thuật cho vật cản có dạng... nhập giới, thiếu thành phần quan trọng sản xuất tự động robot Trong hệ thống sản xuất tự động có hai loại robot: Robot cố định robot tự hành ( Mobil robot ) Với robot tự hành robot di chuyển theo