Tính toán động học và điều khiển Robot SCARA

Đồ án tốt nghiệp Tính tốn động học điều khiển Robot SCARA MỤC LỤC Lời nói đầu Chương I Những kiến thức sở Robot công nghiệp I Lịch sử phát triển robot công ngiệp II Định nghĩa phân loại Robot .8 II.1 Định nghĩa Robot .8 II.2 Phân loại .9 III.Ứng dông Robot công nghiệp .12 III.1 Mục tiêu ứng dụng Robot công nghiệp 12 III.2 Các lĩnh vực ứng dụng Robot công nghiệp .13 IV Hệ thống Robot 13 V Ba toán Robot 14 V.1 Động học 14 V.2 Tĩnh học 15 V.3 Động lực học 15 VI Cấu trúc động học cấu 16 VI.1 Khâu khớp 16 VI.2 Chuỗi động, cấu Robot 17 Sinh viên thực hiện: Nguyễn Văn Thành - Đặng Thị Dung Đồ án tốt nghiệp Tính tốn động học điều khiển Robot SCARA VI.3 Bậc tự cấu Robot 18 Chương II Bài toán động học Robot 19 I Động học thuận Robot 19 I.1 Một số khái niệm 19 I.1.1 Ma trận cosin hướng 19 I.1.2 Các tính chất 21 I.2 Các toạ độ 22 I.2.1 Định nghĩa toạ độ .23 I.2.2 Ma trận quay 23 I.2.3 Các ma trận quay tịnh tiến .24 I.2.4 Phép quay tổng hợp 25 I.3 Một số phép quay đặc biệt ma trận biến đổi 26 I.3.1 Cỏc góc Euler ma trận quay 26 I.3.2 Cỏc gúc R-P-Y (Roll – Pitch – Yaw) ma trận quay 28 I.4 Bộ thông số Denavit Hartenberg ma trận Denavit Hartenberg 29 I.4.1 Các tham số động học Denavit Hartenberg 29 I.4.2 Ma trận Denavit Hartenberg 33 Sinh viên thực hiện: Nguyễn Văn Thành - Đặng Thị Dung Đồ án tốt nghiệp Tính tốn động học điều khiển Robot SCARA I.4.3 Ma trận quan hệ 34 I.4.4 Phương trình xác định vị trớ khõu thao tác Robot .34 I.5 Bài toán động học thuận 34 II Động học ngược 35 II.1 Bài toán .35 II.2 Phương pháp giải 36 III Động lực học .38 III.1 Vận tốc gia tốc 38 III.1.1 Vận tốc 38 III.1.2 Gia tốc .39 III.2 Động tay máy 39 III.3 Thế tay máy .40 III.4 Phương trình vi phân chuyển động Robot 41 Chương III Tính tốn động học thuận động học ngược Robot SCARA 47 I Giới thiệu Robot SCARA 47 I.1 Một số thông số 47 I.2 Kết cấu khí 47 I.3.Hệthống điều khiển .48 Sinh viên thực hiện: Nguyễn Văn Thành - Đặng Thị Dung Đồ án tốt nghiệp Tính tốn động học điều khiển Robot SCARA II Tính tốn động học thuận Robot SCARA 48 II.1 Đặt toán 48 II.2 Sơ đồ động học 48 II.3.Xỏc định vị ttrớ hướng bàn kẹp .49 III Động học ngược Robot SCARA 51 III.1 Đặt toán .51 III.2 Giải toán động học ngược 51 IV Tính lực tác dụng vào khâu khâu 52 IV Áp dụng cụ thể 55 Chương IV Điều khiển Robot SCARA 63 I.Động điện chiều (DC) 63 I.1 Đại cương động điện chiều (DC) 63 I.1.1 Động điện chiều (DC) 63 I.1.2 Điều khiển tốc độ động điện chiều 65 I.1.3 Đảo chiều quay 65 I.2 Điều khiển động DC .65 I.2.1 Thiết bị sử dụng 66 I.2.2 Các bước thực 70 II MOSFET 71 Sinh viên thực hiện: Nguyễn Văn Thành - Đặng Thị Dung Đồ án tốt nghiệp Tính tốn động học điều khiển Robot SCARA III Các phần tử logic 78 III.1 Phần tử AND 78 III.2 Phần tử No .79 IV Ghép nối với máy tính (LPT) 79 IV.1 Các đườcg dẫn vào 79 IV.2 Giao diện hai hướng cổng máy in 84 V Tìm hiểu ngơn ngữ lập trình DELPHI .85 VI Bộ phát xung .86 VII Giải thớch sơ đồ mạch điều khiển động DC .87 VIII Lập trình DELPHI 90 VIII.1 Lưu đồ thuật toán 90 VIII.2 Lập trình .92 Tài liệu tham khảo 113 Sinh viên thực hiện: Nguyễn Văn Thành - Đặng Thị Dung Đồ án tốt nghiệp Tính tốn động học điều khiển Robot SCARA LỜI NÓI ĐẦU Cùng với phát triển cách mạng khoa học kỹ thuật giới tiến lĩnh vực điều khiển tự động hoá sản xuất Một thành tựu bật q trình tự động hố cánh tay máy Robot Những Robot từ đơn giản đến phức tạp, đóng góp nhiều việc thay người môi trường làm việc cường độ cao, độc hại địi hỏi xác cao Để phát triển ứng dụng Robot rộng rãi sản xuất, bên cạnh kiến thức điện, điện tử…thỡ việc nghiờn cứu tính tốn động học điều khiển Robot yếu tố quan trọng làm sở tảng cho việc chế tạo Robot Trong đồ án chúng em tiến hành phân tích, tính tốn động học điều khiển Robot SCARA Đồ án gồm bốn chương: Chương I: Trình bày tổng quát Robot Chương II:Bài toán động học Robot Chương III:TÝnh toán động học thuận động học ngược Robot SCARA Chương IV: Điều khiển Robot SCARA Trong đồ án có sử dụng chương trình tính tốn MAPLE ngơn ngữ lập trình DELPHI Em xin chân thành cảm ơn TS Lê Văn Ngự tận tình hướng dẫn giúp đỡ em hoàn thành đồ án Em còng xin cảm ơn thầy Phạm Thành Chung, Bộ môn Cơ học ứng dụng – Trường Đại học Bách khoa Hà Nội cựng cỏc Thầy, Cụ giỏo Khoa Điện - Cơ - Điện tử tạo điều kiện để em hồn thành cơng việc Hà Nội, ngày 20 tháng 12 năm 2005 Sinh viên Nguyễn Văn Thành Sinh viên thực hiện: Nguyễn Văn Thành - Đặng Thị Dung Đồ án tốt nghiệp Tính tốn động học điều khiển Robot SCARA Đặng Thị Dung CHƯƠNG I NHỮNG KIẾN THỨC CƠ SỞ VỀ ROBOT CÔNG NGHIỆP I LỊCH SỬ PHÁT TRIỂN Ngay sau chiến tranh giới thứ hai, Hoa Kỡ xuất tay máy chộp hỡnh điều khiển từ xa cỏc phũng thí nghiệm vật liệu phóng xạ Vào năm 50 kỉ 20, bên cạnh tay máy chộp hỡnh khí đú, xuất loại tay máy chộp hỡnh thuỷ lực điện từ Năm 1961, robot công nghiệp đưa vào sử dụng nhà mỏy ụtụ General Motor Trenton, New Jersey, Hoa Kì Năm 1967, Nhật Bản nhập robot công nghiệp từ công ty AMF Hoa Kì Đến năm 1990, có 40 cơng ty Nhật Bản đưa thị trường quốc tế nhiều loại robot tiếng Từ năm 70, việc nghiên cứu nâng cao tính robot ý nhiều đến lắp đặt thờm cỏc cảm biến ngoại tín hiệu để nhận biết mơi trường làm việc Một lĩnh vực nhiều phịng thí nghiệm quan tâm robot tự hành Các cơng trình nghiên cứu tạo robot tự hành bắt chước chân người súc vật Các loại robot cịn chưa có nhiều ứng dụng công nghiệp, nhiên loại xe robot (robocar) lại nhanh chóng đưa vào ứng dụng hệ thống sản xuất tự động linh hoạt Từ năm 80, năm 90, áp dụng rộng rãi ứng dụng kĩ thuật vi xử lí cơng nghệ thơng tin, số lượng robot công nghiệp gia tăng, giá thành giảm rõ rệt, tính có nhiều bước tiến vượt bậc Nhờ robot cơng nghiệp có vị trí quan trọng cỏc dõy truyền tự động sản xuất đại Sinh viên thực hiện: Nguyễn Văn Thành - Đặng Thị Dung Đồ án tốt nghiệp Tính tốn động học điều khiển Robot SCARA II ĐỊNH NGHĨA VÀ PHÂN LOẠI II.1 Định nghĩa robot Có nhiều định nghĩa robot tồn tại, tham khảo số định nghĩa sau: • Định nghĩa theo từ điển New World College: “Robot kết cấu khí có hình dạng bất kì, xây dựng để thực công việc tay người” Các định nghĩa sau bao gồm cánh tay khí, cỏc mỏy điều khiển số, máy móc di chuyển theo kiểu bước mô hình dạng người khoa học viễn tưởng Các robot công nghiệp ngày thực phần cơng việc người Các robot ban đầu thường gọi tay máy (Manipulator) • Định nghĩa theo hiệp hội robot công nghiệp Nhật Bản: Định nghĩa mang tính khái quát tất định nghĩa sử dụng Nó bao gồm tất thiết bị tay máy để xem xét định nghĩa robot sau “Robot máy, cấu thường gồm số phân đoạn nối với phân đoạn khác khớp quay hay khớp trượt nhằm mục đích để gắp hay để di chuyển đối tượng, thường có số bậc tự Nó điều khiển nguồn kích hoạt, hệ điều khiển điện tử lập trình hay hệ thống logic đú” • Định nghĩa theo tiêu chuẩn AFNOR (Pháp): “Robot cấu chuyển đổi tự động chương trình hố, lặp lại chương trình, tổng hợp chương trình đặt trờn cỏc trục toạ độ; Sinh viên thực hiện: Nguyễn Văn Thành - Đặng Thị Dung Đồ án tốt nghiệp Tính tốn động học điều khiển Robot SCARA có khả định vị, định hướng, di chuyển đối tượng vật chất (chi tiết , dông cụ gá lắp ) theo hành trình thay đổi chương trỡnh hố nhằm thực nhiệm vụ cơng nghệ khác nhau” • Định nghĩa theo hiệp hôi robot công nghiệp Hoa Kỳ: “Robot tay máy nhiều chức lập trình, thiết kế để di chuyển vật liệu, phần tử, linh kiện, dụng cụ thiết bị đặc biệt thông qua việc thay đổi chương trình hoạt động lập để thực tác vụ khác nhau” • Định nghĩa theo hiệp hội robot Anh: “Robot cơng nghiệp thiết bị lập trình lại thiết kế để thực hai nhiệm vụ cầm nắm vận chuyển phần tử, linh kiện, dụng cụ công cụ chế tạo đặc biệt thông qua việc thay đổi chương trình hoạt động lập để thực tác vụ gia cơng khác nhau” • Định nghĩa theo GOST (Nga): “Robot máy tự động liên kết tay máy cụm điều khiển chương trình hố, thực chu trình cơng nghệ cách chủ động với điều khiển thay chức tương tự người” II.2 Phân loại robot Việc phân nhóm, phân loại robot dựa trờn sở kĩ thuật khác Dưới số cách phân loại chủ yếu: • Phân loại theo số bậc tự do: Một cách phân loại hiển nhiên robot phân loại theo bậc tự chúng Một cách lí tưởng, robot có bậc tự cầm nắm đối tượng tù không gian ba chiều Từ quan điểm này, gọi Sinh viên thực hiện: Nguyễn Văn Thành - Đặng Thị Dung Đồ án tốt nghiệp Tính tốn động học điều khiển Robot SCARA robot robot tổng quát (General Purpose Robot) nú cú sỏu bậc tự do, gọi robot dư (Redundant Robot) có nhiều sáu bậc tự gọi robot thiếu (Dốicient Robot) nú cú Ýt sáu bậc tự Mét robot linh hoạt di chuyển hoạt động khơng gian kín bị hạn chế Mặt khác, số ứng dụng đặc biệt việc lắp ráp mặt phẳng cần robot với bốn bậc tự đủ • Phân loại theo cấu trúc động học: Một phương pháp phân loại khác phân loại theo cấu trúc động học chúng Mét robot gọi robot tuần tù hay robot chuỗi hở cấu trúc động học chúng có dạng chuỗi động hở, gọi robot song song cấu trúc động học chúng có dạng chuỗi đóng gọi robot hỗn hợp bao gồm hai loại chuỗi hở chuỗi đóng Nhìn nhận cách tổng qt robot song song có nhiều ưu điểm vỡ chỳng cú độ cứng vững cao hơn, khả tải cao hơn, không gian làm việc nhỏ cấu trúc phức tạp • Phân loại theo hệ thống động học hay công nghệ di chuyển: - Hệ điện: Thường dùng động điện chiều, động bước hay động Servo Hệ lượng có đặc điểm hoạt động xác, tin cậy, đạt cơng suất cao có tính tuyến tính cao dễ điều khiển Hệ đảm bảo kết cấu gọn, truyền dẫn lượng trực tiếp Ngoài nú cũn đảm bảo an tồn vệ sinh mơi trường Nhằm nâng cao chất lượng hiệu sử dụng hệ cần tuân theo yêu cầu sau: Sinh viên thực hiện: Nguyễn Văn Thành - Đặng Thị Dung 10 Đồ án tốt nghiệp Tính tốn động học điều khiển Robot SCARA Out32(888, 0) ; end; until (m =abs(i)+1) or(Inp32(889) And 128 = 0); realtime(false); end else begin TIMEINITUS; repeat if (TIMEREADUS > 80000)or ((inp32(889)and 32)=0) then begin m:=abs(i)+1; Out32(888, 0) ; end; until (m =abs(i)+1)or(Inp32(889) And 128 = 128); realtime(false); end; end; m:=m+1; edit3.Text:=inttostr(m); Out32(888, 0) ; realtime(false); end; Sinh viên thực hiện: Nguyễn Văn Thành - Đặng Thị Dung 108 Đồ án tốt nghiệp Tính toán động học điều khiển Robot SCARA { } while (n 800000)or(inp32(889)and 16=0) then begin n:=abs(j)+1; Out32(888, 0) ; end; until (n=abs(j)+1) or(Inp32(889) And 64 = 64)or ((inp32(889)and 16)=0); realtime(false); end; n:=n+1; edit4.Text:=inttostr(n); Out32(888, 0) ; end; end; procedure TForm2.Button2Click(Sender: TObject); Sinh viên thực hiện: Nguyễn Văn Thành - Đặng Thị Dung 110 Đồ án tốt nghiệp Tính toán động học điều khiển Robot SCARA begin Out32(888, 0); timer2.Enabled:=false; timer1.Enabled:=false; timer3.Enabled:=false; realtime(false); end; procedure TForm2.FormCreate(Sender: TObject); begin Timer1.Interval:=1; Out32(888, 0); timer1.Enabled:=false; timer2.Enabled:=false; timer3.Enabled:=false; end; procedure TForm2.Timer1Timer(Sender: TObject); begin Out32(888, 0); edit7.Text:=inttostr(inp32(889)and 16); edit8.Text:=inttostr(inp32(889)and 32); edit9.Text:=inttostr(inp32(889)and 8); Sinh viên thực hiện: Nguyễn Văn Thành - Đặng Thị Dung 111 Đồ án tốt nghiệp Tính tốn động học điều khiển Robot SCARA end; procedure TForm2.Timer2Timer(Sender: TObject); begin if index2>listbox2.Count-1 then begin timer2.Enabled:=false; edit1.Text:=inttostr(-(acc_mt+300)); edit2.Text:=inttostr(-(acc_md+300)); button1.Click; end else begin edit1.Text:=listbox2.Items[index2]; edit2.Text:=listbox2.Items[index2+1]; listbox2.ItemIndex:=index+1; i:=strtoint(edit1.Text); j:=strtoint(edit2.Text); acc_mt:=i+acc_mt; acc_md:=j+acc_md; edit5.Text:=inttostr(acc_mt); Sinh viên thực hiện: Nguyễn Văn Thành - Đặng Thị Dung 112 Đồ án tốt nghiệp Tính tốn động học điều khiển Robot SCARA edit6.Text:=inttostr(acc_md); index2:=index2+2; m:=0; n:=0; while (m0 Then begin Checkbox1.Enabled :=true; data_MT := 4; end Else begin Checkbox1.Enabled:=false; data_MT := 8; end; realtime(true); Out32(888, data_MT); {400US va 800US} begin If (Inp32(889) And 128) = 128 Then begin TIMEINITUS; Sinh viên thực hiện: Nguyễn Văn Thành - Đặng Thị Dung 113 Đồ án tốt nghiệp Tính tốn động học điều khiển Robot SCARA repeat if (TIMEREADUS > 500000) then begin m:=abs(i)+1; Out32(888, 0) ; end; until (m =abs(i)+1) or(Inp32(889) And 128 = 0); realtime(false); end else begin TIMEINITUS; repeat if (TIMEREADUS > 500000) then begin m:=abs(i)+1; Out32(888, 0) ; end; until (m =abs(i)+1)or(Inp32(889) And 128 = 128); realtime(false); end; end; m:=m+1; Sinh viên thực hiện: Nguyễn Văn Thành - Đặng Thị Dung 114 Đồ án tốt nghiệp Tính tốn động học điều khiển Robot SCARA edit3.Text:=inttostr(m); Out32(888, 0) ; realtime(false); end; { } while (n 200000) then begin n:=abs(j)+1; Out32(888, 0) ; end; until (n=abs(j)+1) or(Inp32(889) And 64 = 64); realtime(false); end; n:=n+1; edit4.Text:=inttostr(n); Out32(888, 0) ; Sinh viên thực hiện: Nguyễn Văn Thành - Đặng Thị Dung 116 Đồ án tốt nghiệp Tính tốn động học điều khiển Robot SCARA end; { } end; {'timer2.Enabled:=false;} end; procedure TForm2.Button4Click(Sender: TObject); begin index2:=0; lanquay:=0; acc_mt:=0; acc_md:=0; timer1.Enabled:=false; timer3.Enabled:=false; timer2.Interval:=4000; timer2.Enabled:=true; end; [Load File] procedure TForm2.Button3Click(Sender: TObject); var myfile:textfile; i:integer; s:string[5]; Sinh viên thực hiện: Nguyễn Văn Thành - Đặng Thị Dung 117 Đồ án tốt nghiệp Tính tốn động học điều khiển Robot SCARA begin assignfile(myfile,'c:\data.txt'); reset(myfile); while not Eof(myfile) begin readln(myfile,s); listbox1.Items.Add(s); end; closefile(myfile); end; [goi den tinhtoan] procedure TForm2.Button5Click(Sender: TObject); begin luu_gocMT:=0; luu_gocMD:=0; timer3.Interval:=1; timer3.Enabled:=true; index:=0; end; [tinh toan] procedure TForm2.Timer3Timer(Sender: TObject); var canh_huyen:real; Goc_MT,goc_MTG: real; Sinh viên thực hiện: Nguyễn Văn Thành - Đặng Thị Dung 118 Đồ án tốt nghiệp Tính tốn động học điều khiển Robot SCARA Goc_MD,goc_MDG:real; toadox, toadoy,gocxy:real; begin toadox:= strtofloat(listbox1.Items[index]); toadoy:=strtofloat(listbox1.Items[index+1]); index:=index+2; listbox1.ItemIndex:=index; if index>listbox1.Count-1 then timer3.Enabled:=false; if (toadox=0) and (toadoy=0) then begin goc_MT:=0; goc_MD:=0; end else begin gocxy:= ArcTan((toadoy/toadox))*180/Pi; canh_huyen:=toadox/cos(arctan(toadoy/toadox)); Goc_Mt:=(2*ArcCos(canh_huyen/342)*180/pi)*316; Goc_Md:=(ArcCos(canh_huyen/342)*180/pi+gocxy)*315; end; Sinh viên thực hiện: Nguyễn Văn Thành - Đặng Thị Dung 119 Đồ án tốt nghiệp Tính tốn động học điều khiển Robot SCARA listbox2.Items.Add(floattostr(round(Goc_MT-luu_gocMT))); listbox2.Items.Add(floattostr(round(Goc_MD-luu_gocMD))); goc_MTG:= round(Goc_MT-luu_gocMT); goc_MdG:= round(Goc_Md-luu_gocMd); luu_gocMT:=(goc_MT); luu_gocMD:=(goc_Md); end; end Sinh viên thực hiện: Nguyễn Văn Thành - Đặng Thị Dung 120 Đồ án tốt nghiệp Tính tốn động học điều khiển Robot SCARA TÀI LIỆU THAM KHẢO [1] Nguyễn Thiện Phóc: Robot cơng nghiệp Nhà xuất khoa học kỹ thuật Hà nội, 2004 [2] Phạm Huy Điển - Đinh Thế Lục – Tạ Duy Phương: Hướng dẫn thực hành tính tốn chương trình MAPLE V Nhà xuất Giáo Dục, 1998 [3] Đỗ Xuân Thụ (Chủ biên): Kỹ thuật điện tử Nhà xuất Giáo Dục, 2002 [4] TS Lương Ngọc Hải: Giáo trình kỹ thuật xung – sè Nhà xuất Giáo Dục, 2004 [5] Phùng Khắc Khoa: Phân tích động học thuận ngược MAPLE Đồ án tốt nghiệp đại học, Trường đại học Bách Khoa Hà Nội [6] : Lê Trường Giang:Tính tốn động lực học mô Robot Đồ án tốt nghiệp đại học, Trường đại học Bách Khoa Hà Nội [7] Phan Thanh Thiết: Mô động học Robot công nghiệp Đồ án tốt nghiệp đại học, Trường đại học Bách Khoa Hà Nội [8] Lê Phương Lan - Hồng Đức Hải :Lập trình Delphi Sinh viên thực hiện: Nguyễn Văn Thành - Đặng Thị Dung 121 Đồ án tốt nghiệp Tính tốn động học điều khiển Robot SCARA Sinh viên thực hiện: Nguyễn Văn Thành - Đặng Thị Dung 122 ... tổng quát Robot Chương II:Bài toán động học Robot Chương III:TÝnh toán động học thuận động học ngược Robot SCARA Chương IV: Điều khiển Robot SCARA Trong đồ án có sử dụng chương trình tính tốn... tốn động học điều khiển Robot SCARA II Tính tốn động học thuận Robot SCARA 48 II.1 Đặt toán 48 II.2 Sơ đồ động học 48 II.3.Xỏc định vị ttrớ hướng bàn kẹp .49 III Động học. .. Thành - Đặng Thị Dung 19 Đồ án tốt nghiệp Tính tốn động học điều khiển Robot SCARA CHƯƠNG II BÀI TOÁN ĐỘNG HỌC CỦA ROBOT Bài toán động học robot bao gồm toán vị trí, tốn vận tốc, gia tốc Trong