Đồ án nâng cao chất lượng điều khiển robot scara

ĐỒ ÁN TỐT NGHIỆP NÂNG CAO CHẤT LƯỢNG ĐIỀU KHIỂN ROBOT SCARA MỤC LỤC MỞ ĐẦU CHƯƠNG I: TỔNG QUAN VỀ ROBOT VÀ ĐIỀU KHIỂN ROBOT 1.1 Robot robot công nghiệp 1.1.1 Vài nét lịch sử phát triển robot robot công nghiệp 1.1.2 Robot công nghệ cao 1.1.3 Định nghĩa robot công nghiệp .8 1.2 Các phương pháp điều khiển robot .10 1.2.1 Phương pháp điều khiển động lực học ngược 10 1.2.2 Phương pháp điều khiển phản hồi phân ly phi tuyến 12 1.2.3 Phương pháp điều khiển thích nghi theo sai lệch 13 1.2.4 Phương pháp điều khiển thích nghi theo mơ hình mẫu 14 1.2.5 Phương pháp điều khiển động lực học ngược thích nghi 15 1.2.6 Phương pháp điều khiển trượt .16 CHƯƠNG II: MÔ TẢ ĐỐI TƯỢNG - ROBOT SCARA SERPENT 17 2.1 Một số loại robot Scara hãng sản xuất 17 2.2 Các thông số vùng làm việc robot Scara Serpent 19 2.2.1 Cấu tạo tay máy robot Scara Serpent 20 2.2.1.1 Cấu hình robot Scara Serpent 20 2.2.1.2 Các thông số kỹ thuật robot Scara Serpent .20 2.2.2 Giới hạn không gian làm việc robot Scara Serpent .21 2.3 Động học robot Scara Serpent 23 2.3.1 Động học thuận .23 2.3.2 Động học ngược 26 2.4 Động lực học robot Scara Serpent .28 2.4.1 Hàm Euler - Lagrange vấn đề động lực học 29 2.4.2 Động lực học robot Scara Serpent 30 2.4.2.1 Tính tốn động cho khớp 30 2.4.2.2 Phương trình động lực học 33 2.5 Mô tả đối tượng hệ phương trình trạng thái 37 CHƯƠNG III: XÂY DỰNG MƠ HÌNH HỆ THỐNG ĐIỀU KHIỂN 39 3.1 Cấu trúc hệ thống điều khiển robot .39 3.2 Xây dựng quỹ đạo chuyển động chuẩn 40 3.2.1 Xác định giá trị q02 qc1 42 3.2.2 Phương trình đoạn cd .42 3.2.3 Phương trình đoạn ac .43 3.2.4 Phương trình đoạn df .43 3.3 Thiết kế điều khiển cho tay máy robot Scara Serpent ba bậc tự .44 3.3.1 Hệ phương trình động lực học Lagrange 44 3.3.2 Hệ phương trình trạng thái 45 3.3.3 Lựa chọn phương pháp điều khiển điều khiển PID 48 CHƯƠNG IV: MƠ PHỎNG VỚI MƠ HÌNH ROBOT SCARA SERPENT 51 4.1 Đặt vấn đề 51 4.2 Sơ đồ mô hình hóa khâu hệ thống 51 4.2.1 Mơ hình chung robot .51 4.2.2 Mơ hình khối tạo quỹ đạo chuyển động chuẩn .51 4.2.3 Mơ hình điều khiển 52 4.3 Giao diện chương trình mơ robot Scara Serpent 52 4.4 Kết mô hệ thống Matlab-Simulink 58 4.4.1 Thông số robot quỹ đạo chuyển động 58 4.4.2 Đặc tính hệ thống robot làm việc với tải khác .58 KẾT LUẬN 79 TÀI LIỆU THAM KHẢO .80 PHỤ LỤC 1: CÁC SƠ ĐỒ KHỐI 81 PHỤ LỤC 2: CÁC CHƯƠNG TRÌNH M FILES 86 MỞ ĐẦU Theo trình phát triển xã hội, nhu cầu nâng cao sản xuất chất lượng sản phẩm ngày đòi hỏi ứng dụng rộng rãi phương tiện tự động hóa sản xuất Xu hướng tạo dây chuyền thiết bị tự động có tính linh hoạt cao hình thành phát triển mạnh mẽ…Vì ngày tăng nhanh nhu cầu ứng dụng người máy để tạo hệ sản xuất tự động linh hoạt Robot ứng dụng rộng rãi đóng vai trị quan trọng sản xuất đời sống Robot cấu đa chức có khả lập trình dùng để di chuyển nguyên vật liệu, chi tiết, dụng cụ thông qua truyền động lập trình trước Khoa học robot chủ yếu dựa vào phép tốn đại số ma trận  Robot thao tác người hợp tác với cánh thơng minh  Robot có cánh tay với nhiều bậc tự thực chuyển động tay người điều khiển máy tính điều khiển chương trình nạp sẵn chip bo mạch điều khiển robot Để hệ điều khiển robot có độ tin cậy, độ xác cao, giá thành hạ tiết kiệm lượng nhiệm vụ hệ điều khiển robot phải đảm bảo giá trị yêu cầu đại lượng điều chỉnh điều khiển Ngoài ra, hệ điều khiển robot phải đảm bảo ổn định động tĩnh, chống nhiễu ngồi, đồng thời khơng gây tác hại cho mơi trường như: tiếng ồn mức quy định, sóng hài điện áp dòng điện lớn cho lưới điện v.v Khi thiết kế hệ điều khiển robot mà sử dụng hệ điều chỉnh tự động truyền động, cần phải đảm bảo hệ thực tất yêu cầu công nghệ, tiêu chất lượng yêu cầu kinh tế Chất lượng hệ thống thể trạng thái tĩnh trạng thái động Trạng thái tĩnh yêu cầu quan trọng độ xác điều chỉnh Trạng thái động có u cầu độ ổn định tiêu chất lượng động độ điều chỉnh, tốc độ điều chỉnh, thời gian điều chỉnh số lần dao động Đối với hệ điều khiển robot, việc lựa chọn sử dụng biến đổi, loại động điện, thiết bị đo lường, cảm biến, điều khiển đặc biệt phương pháp điều khiển có ảnh hưởng lớn đến chất lượng điều khiển bám xác quỹ đạo hệ Các cơng trình nghiên cứu hệ thống điều khiển robot tập trung chủ yếu theo hai hướng sử dụng mơ hình có đặc tính phi tuyến ước lượng để đơn giản việc phân tích thiết kế đề thuật toán điều khiển nhằm nâng cao chất lượng đáp ứng robot Đặc điểm hệ thống điều khiển robot thực điều khiển bám theo quỹ đạo phức tạp đặt trước khơng gian, nhiên dịch chuyển trọng tâm chuyển động thành phần mơmen qn tính hệ thay đổi, điều dẫn đến thơng số động học hệ thay đổi theo quỹ đạo chuyển động đồng thời xuất lực tác động qua lại, xuyên chéo chuyển động thành phần hệ với Các yếu tố tác động làm cho hệ điều khiển robot mang tính phi tuyến mạnh, gây cản trở lớn cho việc mơ tả nhận dạng xác hệ thống điều khiển robot Do vậy, điều khiển robot bám theo quỹ đạo đặt trước phải giải vấn đề sau:  Khắc phục lực tương tác phụ thuộc vào vận tốc, gia tốc quỹ đạo riêng chuyển động thành phần quỹ đạo chung hệ như: lực quán tính, lực ly tâm, lực ma sát v.v  Khi trọng tâm chuyển động thành phần hệ thay đổi theo quỹ đạo riêng chung kéo theo thay đổi thông số động học hệ, điều địi hỏi phải có biến thiên tham số đưa vào điều khiển tương ứng để đảm bảo cân bằng, ổn định bền vững đồng thời bám theo quỹ đạo đặt Với cơng cụ tốn vi phân người ta phân tích tính điều khiển được, tính quan sát cho hệ phi tuyến Nội dung đề tài nghiên cứu sau: Tên đề tài: Nâng cao chất lượng điều khiển cho robot Scara Cơ sở khoa học thực tiễn đề tài: Ứng dụng phương pháp điều khiển động lực học ngược cho điều khiển bền vững quỹ đạo robot Mục đích đề tài: Xây dựng cấu trúc thuật toán điều khiển robot Scara Nội dung đề tài, vấn đề cần giải quyết: - Xây dựng mô hình tốn học cho robot Scara Serpent bậc tự - Xây dựng hệ thống điều khiển quỹ đạo đạt độ xác cao - Đánh giá chất lượng hệ thống mô Nội dung luận văn đề cập tới vấn đề “Nâng cao chất lượng điều khiển robot Scara” với mục tiêu điều khiển bền vững bám xác quỹ đạo chuyển động Luận văn trình bày thành chương với nội dung chương tóm tắt sau: Chương I – Tổng quan robot điều khiển robot: Mơ tả tổng quan robot Phân tích ưu, nhược điểm số phương pháp điều khiển robot áp dụng thực tiễn để nâng cao độ xác điều khiển quỹ đạo robot Chương II – Mơ tả tốn học đối tượng robot Scara: Nghiên cứu số robot họ Scara sâu vào phân tích mơ hình Robot Scara Serpent để phục vụ việc nghiên cứu kiểm chứng sở lý thuyết phương pháp điều khiển lựa chọn Chương III – Xây dựng mơ hình hệ thống điều khiển: Thiết kế điều khiển để nâng cao chất lượng hệ thống điều khiển quỹ đạo cho robot Xây dựng mơ hình hệ thống điều khiển cho robot Scara Serpent, xây dựng mơ hình mơ sử dụng phần mềm Matlab-Sumulink Chương IV – Mô với mô hình robot Scara Serpent: Định hình kiểm chứng mặt lý thuyết sở thực tiễn đề tài tính khả thi phương pháp điều khiển lựa chọn áp dụng cho hoạt động bền vững điều khiển quỹ đạo robot Scara Serpent bậc tự CHƯƠNG I TỔNG QUAN VỀ ROBOT VÀ ĐIỀU KHIỂN ROBOT 1.1 Robot robot công nghiệp 1.1.1 Vài nét lịch sử phát triển robot robot cơng nghiệp Nhìn ngược dịng thời gian nhận thấy từ “Robot” xuất từ lâu Năm 1921 nhà viết kịch Karelcapek người Séc viết kịch với tựa đề R.U.R (Rossums Universal Robot) mô tả loạn cỗ máy phục dịch Từ “Robot” có nghĩa máy móc biết làm việc người Có lẽ gợi ý cho nhà sáng chế kỹ thuật thực mơ ước cỗ máy bắt chước thao tác lao động bắp người Thời gian sau cấu điều khiển từ xa (Teleoperator) đời ngày phát triển hoàn thiện Teleoperator cấu sinh học, bao gồm khâu, khớp với dây chằng gắn liền với hệ điều hành cánh tay người điều khiển thông qua cấu khuếch đại khí Teleoperator cầm nắm, nâng hạ, dịch chuyển, xoay lật đối tượng không gian hoạt động định Tuy thao tác tinh vi, khéo léo tốc độ hoạt động chậm, lực tác dụng hạn chế hệ điều khiển tuý khí Từ thập kỷ 50, phát triển đầy hứa hẹn kỹ thuật điều khiển theo chương trình số cứng ngành vật liệu làm chỗ dựa vững cho đời cấu điều khiển vô cấp (servo mechanism) hệ điện toán (computation) Ngay ý tưởng kết hợp hệ điều khiển NC (Numerical control) với cấu điều khiển từ xa (Teleoperator) hình thành triển khai nghiên cứu Sự phối hợp tuyệt vời khả linh hoạt khéo léo Teleoperator với độ thông minh nhạy bén hệ điều khiển NC đưa kết hệ máy móc tự động cao cấp với tên gọi “Robot” Năm 1961 người máy công nghiệp (IR- industrial Robot) đưa thị trường Tiếp theo nước khác bắt đầu sản xuất robot công nghiệp theo quyền Mỹ, Anh (1967), Thụy Điển, Nhật (1968), Đức (1971) Ngày nay, giới có khoảng 200 cơng ty sản xuất IR, Nhật có 70, nước Tây âu có 90, Mỹ có 30 Nhờ áp dụng rộng rãi tiến khoa học kỹ thuật vi xử lý, tin học vật liệu nên số lượng robot cơng nghiệp tăng lên nhanh chóng, giá thành giảm rõ rệt, tính có nhiều cải tiến Robot công nghiệp phát huy mạnh lĩnh vực hàn hồ quang, đúc, lắp ráp, sơn phủ, hệ thống tự động điều khiển liên hợp 1.1.2 Robot công nghệ cao Robot công nghệ cao khái niệm sản xuất tự động hố đại Các nước cơng nghiệp phát triển đưa chiến lược dùng tự động hố đại (IR+High Tech) kéo xí nghiệp cơng nghiệp đầu tư nước ngồi (trước lý lương thợ rẻ mạt) trở quốc (dùng lao động robot cơng nghiệp) Chính phủ nước áp dụng biện pháp hỗ trợ hữu hiệu như: coi robot công nghiệp ngành công nghiệp quan trọng, xây dựng nhiều chương trình nhà nước áp dụng tiến khoa học kỹ thuật vào sản xuất robot Khuyến khích cách ưu tiên thuế đặt quy chế có lợi cho người sản xuất người sử dụng robot công nghiệp Nhờ sau thời gian ngắn sử dụng robot công nghiệp trở nên rộng lớn đa dạng với sở nguồn động lực phát triển “lực đẩy” công nghệ “lực kéo” thị trường Một đặc điểm quan trọng robot công nghiệp chúng cho phép dễ dàng kết hợp việc phụ trình sản xuất thành dây chuyền tự động So với phương tiện tự động hoá khác, dây chuyền tự động dùng robot có nhiều ưu điểm dễ dàng thay đổi chương trình làm việc, có khả tạo dây chuyền tự động từ máy vạn năng, tự động hố tồn phần Khi sử dụng robot vào dây chuyền tự động, khâu chuẩn bị kỹ thuật rút ngắn Trong với thời gian từ lúc định phương án đến lúc thiết kế xong dây chuyền máy tự động, mặt hàng quy trình cơng nghệ trở thành lạc hậu Theo số liệu chuyên gia Mỹ nghiên cứu vấn đề khảo sát 70 đề án thiết kế với nửa số phương án dùng máy tự động chuyên dụng phải tốn năm Vì thế, phương án dùng robot Unimate với máy tự động vạn đưa vào sử dụng phát huy hiệu to lớn Kỹ thuật robot có ưu điểm quan trọng tạo nên khả linh hoạt hố sản xuất Việc sử dụng máy tính điện tử, robot máy điều khiển theo chương trình cho phép tìm phương thức mẻ để tạo nên dây chuyền tự động cho sản xuất hàng loạt với nhiều mẫu, loại sản phẩm Dây chuyền tự động “cứng” gồm nhiều thiết bị tự động chuyên dùng đòi hỏi vốn đầu tư lớn, tốn nhiều thời gian để thiết kế chế tạo, lúc quy trình cơng nghệ ln ln cải tiến, nhu cầu chất lượng quy cách sản phẩm ln thay đổi Bởi vậy, nhu cầu “mềm” hố hay linh hoạt hóa dây chuyền sản xuất ngày tăng Kỹ thuật robot cơng nghiệp máy tính đóng vai trị quan trọng cơng việc tạo dây chuyền tự động linh hoạt Xuất phát từ nhu cầu khả linh hoạt hoá sản xuất, năm gần nhà khoa học mà nhà sản xuất tập trung ý vào việc hình thành áp dụng hệ sản xuất tự động linh hoạt, gọi tắt hệ sản xuất linh hoạt Hệ sản xuất linh hoạt FMS (Flexible Manufacturing System) ngày thường bao gồm thiết bị gia cơng điều khiển chương trình số, phương tiện vận chuyển kho chứa phân xưởng tự động hố nhóm robot cơng nghiệp vị trí trực tiếp với thiết bị gia công thực nguyên công phụ Việc điều khiển kiểm tra hoạt động toàn hệ sản xuất linh hoạt thực máy tính Ưu điểm bật hệ sản xuất linh hoạt thích hợp với quy mơ nhỏ vừa, thích hợp với u cầu ln ln thay đổi chất lượng sản phẩm quy trình cơng nghệ Bởi vậy, ngày hệ sản xuất linh hoạt thu hút ý nước phát triển mà nước phát triển Trong số tài liệu nước FMS hệ sản xuất tương lai (Future Manufactring System) 1.1.3 Định nghĩa robot công nghiệp Robot công nghiệp máy tự động linh hoạt thay phần toàn hoạt động bắp hoạt động trí tuệ người nhiều khả thích nghi khác Về mặt khí điều khiển điện tử, robot công nghiệp tổ hợp khả hoạt động linh hoạt cấu điều khiển từ xa với tốc độ phát triển ngày cao hệ thống điều khiển theo chương trình số kỹ thuật chế tạo cảm biến, cơng nghệ lập trình phát triển trí tuệ nhân tạo, hệ chun gia Robot cơng nghiệp có khả chương trình hố linh hoạt nhiều trục chuyển động, biểu thị cho số bậc tự chúng Robot công nghiệp trang bị bàn tay máy cấu chấp hành giải nhiệm vụ xác định q trình cơng nghệ: trực tiếp tham gia nguyên công (sơn, hàn, lắp ráp cụm thiết bị ) phục vụ q trình tổ chức dịng lưu thơng vật chất (chi tiết, dao cụ, gá lắp ) với thao tác cầm nắm, vận chuyển trao đổi đối tượng vật chất với trạm công nghệ hệ thống máy tự động linh hoạt Ta điểm qua vài định nghĩa robot công nghiệp sau: - Định nghĩa theo tiêu chuẩn AFNOR Fracais: Robot công nghiệp 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ục toạ độ, có khả định vị, định hướng, di chuyển đối tượng vật chất theo chương trình thay đổi chương trình hố nhằm thực nhiệm vụ công nghệ khác - Định nghĩa theo tiêu chuẩn VDI 2806/BRD: Robot công nghiệp thiết bị có nhiều trục, thực chuyển động tự động chương trình hố nối ghép chuyển động chúng khoảng cách tuyến tính hay phi tuyến động trình Chúng điều khiển hợp ghép nối với nhau, có khả học nhớ chương trình, chúng trang bị dụng cụ phương tiện công nghệ khác để thực nhiệm vụ sản xuất trực tiếp gián tiếp - Định nghĩa theo IOTC - 1980: Robot công nghiệp 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 Bản chất định nghĩa cho ta thấy ý nghĩa quan trọng: robot công nghiệp phải liên hệ chặt chẽ với máy móc, công cụ thiết bị công nghệ tự động khác hệ thống tự động tổng hợp Trong q trình phân tích thiết kế khơng thể quan niệm robot đơn vị cấu trúc biệt lập, trái lại phải 10 Sơ đồ khối điều khiển PID cho khớp Sơ đồ khâu robot Scara Serpent 84 Sơ đồ khối tín hiệu Nhập giá trị tham số cho mơ hình 85 86 87 PHỤ LỤC 2: CÁC CHƯƠNG TRÌNH M FILES (chạy Matlab/Simulink) Tính tốn Quỹ đạo chuyển động chuẩn % Tinh toan Quy dao chuyen dong chuan % Xay dung quy dao chuyen dong chuan cho khop quay cua Robot Serpent % qi0, qic: goc quay dau va cuoi cua khop quay thu i function qdc=qdcdc(u) % Toa diem dau: DDau=[X0;Y0] X0=u(1); Y0=u(2); % Toa diem cuoi: DCuoi=[Xc;Yc] Xc=u(3); Yc=u(4); % td: Dat thoi gian chuyen dong td=u(5); % Thoi gian gia toc (hoac giam toc) ta=u(6); % Thong so cua Robot: a1,a2 a1=u(7); %(m) a2=u(8); %(m) t=u(9); % thoi gian thuc nx=u(10); % CHUONG TRINH CHINH % Dong hoc nguoc va tinh cac goc quay cua tung khop tai diem dau va diem cuoi q20=Gocquay2(X0,Y0,a1,a2); q2c=Gocquay2(Xc,Yc,a1,a2); q10=Gocquay1(X0,Y0,q20,a1,a2); 88 q1c=Gocquay1(Xc,Yc,q2c,a1,a2); q40=Gocquay4(q10,q20,nx); q4c=Gocquay4(q1c,q2c,nx); %Khoi tao cac he so cua quy dao chuan cua khop h1=Init(q10,q1c,ta,td); h2=Init(q20,q2c,ta,td); h4=Init(q40,q4c,ta,td); %Quy dao chuyen dong chuan khop quay cua Robot Serpent qdc1= Quydaochuan(t,h1,ta,td); qdc2= Quydaochuan(t,h2,ta,td); qdc4= Quydaochuan(t,h4,ta,td); qdc=[qdc1;qdc2;qdc4]; %KET THUC CHUONG TRINH CHINH %CHUONG TRINH TINH GIA TRI %Tinh cac gia tri tuong ung goc quay 2: q20,q2c function gq2=Gocquay2(x,y,a1,a2) C2=(x^2+y^2-a1^2-a2^2)/(2*a1*a2); S2=sqrt(1-C2^2); gq2=atan2(S2,C2); %Tinh cac gia tri tuong ung goc quay 1: q10,q1c function gq1=Gocquay1(x,y,q2,a1,a2) C1=((a1+a2*cos(q2))*x+a2*sin(q2)*y)/(x^2+y^2); S1=((a1+a2*cos(q2))*y-a2*sin(q2)*x)/(x^2+y^2); gq1=atan2(S1,C1); 89 %Tinh cac gia tri tuong ung goc quay 4: q40,q4c function gq4=Gocquay4(q1,q2,nx) KK=sqrt(1-nx^2); tetaphu=atan2(KK,nx); gq4=q1+q2-tetaphu; %Xay dung quy dao chuyen dong chuan %qac=a0ac+a3ac*t^3+a4ac*t^4 %qcd=a0cd+a1cd*(t-2ta) %qdf=a0df+a1df*t+a3df*t^3+a4df*t^4 function quydao=Quydaochuan(t,h,ta,td) a0ac=h(1);a3ac=h(2);a4ac=h(3); a0cd=h(4);a1cd=h(5); a0df=h(6);a1df=h(7);a3df=h(8);a4df=h(9);qcuoi=h(10); if td0)&&(td=0)&&(ttd/2)&&(ttd quydao=qcuoi; else 90 quydao=-1; %Error end else %if td>(4*ta) %Gom giai doan: gia toc, chay deu, giam toc if (t>=0)&&(t=2*ta)&&(t(td-2*ta))&&(ttd quydao=qcuoi; else quydao=-1;%Error end end %Tinh cac he so cua quy dao chuan ung voi tung khop function heso=Init(qdau,qcuoi,ta,td) a0ac=0;a3ac=0;a4ac=0;a0cd=0;a1cd=0;a0df=0;a1df=0;a3df=0;a4df=0; if (td>0)&&(td(4*ta) %Quy dao doan ac: a0ac=qdau; a3ac=(qcuoi-qdau)/(4*ta^2*(td-2*ta)); a4ac=-(qcuoi-qdau)/(16*ta^3*(td-2*ta)); %Quy dao doan cd: a0cd=qdau+(qcuoi-qdau)*ta/(td-2*ta); a1cd=(qcuoi-qdau)/(td-2*ta); %Quy dao doan df: a0df=qcuoi-(qcuoi-qdau)*ta/(td-2*ta); a1df=(qcuoi-qdau)/(td-2*ta); a4df=-(3*qcuoi-3*a0df-4*a1df*ta)/(16*ta^4); a3df=-(a1df+32*a4df*ta^3)/(12*ta^2); end heso=[a0ac,a3ac,a4ac,a0cd,a1cd,a0df,a1df,a3df,a4df,qcuoi]; 92 Tính momem chuẩn % Tinh toan momem chuan function momem=momemchuan(u) q2d=u(1); dq1d=u(2);ddq1d=u(3); dq2d=u(4);ddq2d=u(5); dq4d=u(6);ddq4d=u(7); mt=u(8);Jt=u(9); % Thong so cua robot J1=0.021; J2=0.0034; J3=0.006; J40=0.001; J4=J40+Jt; J124=J1+J2+J4; J24=J2+J4; m1=4; m2=1.5; m3=2; m40=0.6; m4=m40+mt; m1234=m1+m2+m3+m4; m234=m2+m3+m4; m34=m3+m4; l1=0.25; l2=0.15; % Tinh T T=-(m234*l1*l2*sin(q2d)); 93 % Tinh toan cac gia tri Hij H11=m1234*l1^2+m234*l2^2+J124+2*m234*l1*l2*cos(q2d); H12=m234*l2^2+J24+m234*l1*l2*cos(q2d); H13=J4; H21=H12; H22=m234*l2^2+J24; H23=J4; H31=J4; H32=J4; H33=J4; DH=J4^2*(2*H12-H11-H22)+J4*(H11*H22-H12^2); % Dau % Tinh momem chuan quy dao dat u1m=H11*ddq1d+H12*ddq2d+H13*ddq4d+T*dq2d^2+2*T*dq1d*dq2d; u2m=H21*ddq1d+H22*ddq2d+H23*ddq4d-T*dq1d^2; u4m=H31*ddq1d+H32*ddq2d+H33*ddq4d; momem =[u1m;u2m;u4m]; Tính góc quay ban đầu %Tinh toan goc quay ban dau function init=gocquaybd(u) x=u(1);%m y=u(2);%m a1=u(3);%(m) a2=u(4);%(m) nx=u(5); %q20=Gocquay2(X0,Y0,a1,a2) %q10=Gocquay1(X0,Y0,q20,a1,a2); 94 %q40=Gocquay4(q10,q20,nx); %Tinh cac gia tri tuong ung goc quay 2: q20 C2=(x^2+y^2-a1^2-a2^2)/(2*a1*a2); S2=sqrt(1-C2^2); q20=atan2(S2,C2); %Tinh cac gia tri tuong ung goc quay 1: q10 C1=((a1+a2*cos(q20))*x+a2*sin(q20)*y)/(x^2+y^2); S1=((a1+a2*cos(q20))*y-a2*sin(q20)*x)/(x^2+y^2); q10=atan2(S1,C1); %Tinh cac gia tri tuong ung goc quay 4: q40 KK=sqrt(1-nx^2); thetaphu=atan2(KK,nx); q40=q10+q20-thetaphu; %Dau init=[q10;q20;q40]; Tính góc điều khiển thực % Tinh toan goc dieu khien thuc function ddtheta=tinh_ddtheta(u) % Momem dieu khien u1=u(1); u2=u(2); u4=u(3); % Thong so cua tai mt=u(4); Jt=u(5); % Goc quay theta2 thuc te q1tt=u(6);q2tt=u(7);q4tt=u(8); 95 % Dao ham goc quay thuc te dq1tt=u(9);dq2tt=u(10); % Goc quay ban dau q10=u(11);q20=u(12);q40=u(13); % CHUONG TRINH CHINH % Tinh cac he so heso=tinh_heso(q2tt,dq1tt,dq2tt,mt,Jt); % Tinh dao ham cap hai cua goc quay thuc ddtheta=tinh_ddthetathuc(heso,u1,u2,u4,q10,q20,q40); % Chuong trinh tinh cac he so function HS=tinh_heso(q2tt,dq1tt,dq2tt,mt,Jt) % Thong so cua robot J1=0.021; J2=0.0034; J3=0.006; J40=0.001; J4=J40+Jt; J124=J1+J2+J4; J24=J2+J4; m1=4; m2=1.5; m3=2; m40=0.6; m4=m40+mt; m1234=m1+m2+m3+m4; m234=m2+m3+m4; m34=m3+m4; l1=0.25; l2=0.15; 96 % Tinh T T=-(m234*l1*l2*sin(q2tt)); % Tinh toan cac gia tri Hij H11=m1234*l1^2+m234*l2^2+J124+2*m234*l1*l2*cos(q2tt); H12=m234*l2^2+J24+m234*l1*l2*cos(q2tt); H13=J4; H21=H12; H22=m234*l2^2+J24; H23=J4; H31=J4; H32=J4; H33=J4; DH=J4^2*(2*H12-H11-H22)+J4*(H11*H22-H12^2); % Cac gia tri dau bij b11=DH^-1*(H22*H33-H23*H32); b12=DH^-1*(H13*H32-H12*H33); b13=DH^-1*(H12*H23-H13*H22); b21=DH^-1*(H23*H31-H21*H33); b22=DH^-1*(H11*H33-H13*H31); b23=DH^-1*(H13*H21-H11*H23); b31=DH^-1*(H21*H32-H31*H22); b32=DH^-1*(H12*H31-H11*H32); b33=DH^-1*(H11*H22-H12^2); % Cac gia tri h1,h2 h1=T*dq2tt^2+2*T*dq1tt*dq2tt; h2=-(T*dq1tt^2); 97 % Cac gia tri a1=-(b11*h1+b12*h2); a2=-(b21*h1+b22*h2); a4=-(b31*h1+b32*h2); HS =[b11,b12,b13,b21,b22,b23,b31,b32,b33,a1,a2,a4]; % Chuong trinh tinh dao ham cap hai cua goc quay thuc function d2theta=tinh_ddthetathuc(heso,u1,u2,u4,q10,q20,q40) b11=heso(1);b12=heso(2);b13=heso(3); b21=heso(4);b22=heso(5);b23=heso(6); b31=heso(7);b32=heso(8);b33=heso(9); a1=heso(10);a2=heso(11);a4=heso(12); % Tinh dao ham cap hai cua goc quay thuc d2theta1=a1+b11*u1+b12*u2+b13*u4; d2theta2=a2+b21*u1+b22*u2+b23*u4; d2theta4=a4+b31*u1+b32*u2+b33*u4; d2theta=[d2theta1;q10;d2theta2;q20;d2theta4;q40]; 98 ... mạch điều khiển robot Để hệ điều khiển robot có độ tin cậy, độ xác cao, giá thành hạ tiết kiệm lượng nhiệm vụ hệ điều khiển robot phải đảm bảo giá trị yêu cầu đại lượng điều chỉnh điều khiển. .. Xây dựng mơ hình hệ thống điều khiển: Thiết kế điều khiển để nâng cao chất lượng hệ thống điều khiển quỹ đạo cho robot Xây dựng mô hình hệ thống điều khiển cho robot Scara Serpent, xây dựng mơ... quan robot điều khiển robot: Mô tả tổng quan robot Phân tích ưu, nhược điểm số phương pháp điều khiển robot áp dụng thực tiễn để nâng cao độ xác điều khiển quỹ đạo robot Chương II – Mô tả toán