Điều khiển tay máy robot có số bậc tự do dư

BỘ GIÁO DỤC VÀ ĐÀO TẠO TRƯỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘI - Đào Việt Trung ĐIỀU KHIỂN TAY MÁY ROBOT CÓ SỐ BẬC TỰ DO DƯ LUẬN VĂN THẠC SĨ KHOA HỌC Chuyên ngành: Điều khiển Tự động hóa NGƯỜI HƯỚNG DẪN KHOA HỌC TS Nguyễn Phạm Thục Anh Hà Nội – 2011 LỜI CẢM ƠN Với tình cảm chân thành, em xin bày tỏ lòng biết ơn sâu sắc đến: Ban giám hiệu Nhà trường, Giáo sư, Phó Giáo Sư, Tiến Sĩ, Thầy giáo, Cơ giáo Viện Điện, Viện Đào tạo Sau đại học trườngĐại học Bách Khoa Hà Nội tận tình giảng dạy, dẫn cho em suốt thời gian học tập, nghiên cứu làm luận văn tốt nghiệp Đặc biệt em xin bày tỏ lòng tri ân sâu sắc Tiến Sĩ Khoa Học Nguyễn Phạm Thục Anh, cô giáo tận tâm hướng dẫn, bảo, giúp đỡ em suốt trình triển khai nghiên cứu hoàn chỉnh đề tài Mặc dù cố gắng trình nghiên cứu, thực đề tài luận văn em khó tránh khỏi sai sót khơng mong muốn Kính mong nhận góp ý bảo thầy, giáo để luận văn em hoàn thiện Việt trì, Ngày 08 tháng 12 năm 2011 Học viên Đào Việt Trung DANH MỤC MỘT SỐ KÝ HIỆU, CHỮ VIẾT TẮT Tên đầy đủ Stt Ký hiệu, chữ viết tắt RB Robot ĐK Điều khiển {A} Khung tọa độ A RRR khớp quay Rot – Rot – Rot Robot Planar MATLAB SCARA TH1, TH2 𝐴+ 10 D-H 11 Obstacle Robot hoạt động mặt phẳng MATrix and LABoratory Selective Compliance Asembly Robot Arm Trường hợp 1, trường hợp Ma trận giả nghich đảo ma trận A Phép biểu diễn Danevit – Hartenbeg Vật cản DANH MỤC CÁC HÌNH VẼ [Hình 1.1] Robot A1600 III [Hình 1.2] Robot hàn điểm ST200 [Hình 1.3] Robot hàn hồ quang IRB 1410 [Hình 1.4] Robot phun sơn IRB580 [Hình 1.5] Robot lắp ráp mạch điện tử JR2000-nert [Hình 1.6] Máy đo tọa độ CMM MicroHite 3D PC-DMIS [Hình 1.7] Robot PUMA 500 [Hình 1.8] Robot T3-786 [Hình 1.9] Robot SCARA [Hình 1.10] Các dạng khớp robot 12 [Hình 1.11] Phạm vi làm việc robot không gian 13 [Hình 1.12] Cấu trúc cổ tay robot 13 [Hình 1.13] Hệ thống điều khiển vị trí 14 [Hình 1.14] Tay máy Robot có bậc tự dư trườn qua lỗ hẹp mà không va chạm vào tường 16 [Hình 1.15] Tay máy Robot sử dụng bậc tự dư di chuyển để tác động bề mặt đối tượng hình trịn xoay 16 [Hình 1.16] Tay máy Robot sử dụng bậc tự dư có thao tác đối tượng nhiều góc cạnh 17 [Hình 2.1] Vị trí tương đối khung tọa độ 19 [Hình 2.2] Vị trí điểm P hai khung tọa độ 22 [Hình 2.3] Thiết kế khung tọa độ nối 24 [Hình 2.4] Biểu diễn đặt khung tọa độ lên thân Robot 26 [Hình 2.5] Bảng D-H Robot Planar RRR 27 [Hình 3.1] Thanh nối thứ I Robot 33 [Hình 4.1] Tiêu chuẩn ổn định Lyapunov 44 [Hình 4.2] Sơ đồ khối hệ thống điều khiển không gian khớp 45 [Hình 4.3] Sơ đồ khối hệ thống điều khiển khơng gian làm việc 46 [Hình 4.3] Sơ đồ khối tổng quát hệ thống điều khiển phản hồi 48 [Hình 4.4] Sơ đồ cấu trúc hệ thống điều khiển robot với điều khiển PD bù trọng lực 52 [Hình 4.6] Sơ đồ khối hệ thống điều khiển động lực học ngược thích nghi 53 [Hình 4.7] Sơ đồ khối hệ thống điều khiển theo ma trận Jacobian đảo 58 [Hình 4.8] Sơ đồ khối hệ thống điều khiển theo ma trận Jacobian chuyển vị 60 [Hình 5.1] Robot Planar nối RRR 63 [Hình 5.2] Robot Planar nối RRR 77 [Hình 5.3] Cấu hình Robot để tránh vật cản 78 [Hình 6.1] Sơ đồ mơ hệ thống điều khiển Robot Planar RRR 83 [Hình 6.3]: Chuyển động Robot Planar không dùng bậc tự dư 84 [Hình 6.4]: Chuyển động Robot Planar với bậc tự dư tránh vật cản 85 [Hình 6.5] Chuyển động Robot Planar khơng dùng bậc tự dư 86 [Hình 6.6] Chuyển động Robot Planar dùng bậc tự dư 87 MỤC LỤC Trang phụ bìa Lời cảm ơn Danh mục ký hiệu, chữ viết tắt Danh mục hình vẽ, đồ thị Mục lục MỞ ĐẦU NỘI DUNG Chương Giới thiệu chung Robot công nghiệp 1.1 Ứng dụng đặc tính robot cơng nghiệp 1.1.1 Ứng dụng Robot công nghiệp 1.1.2 Các đặc tính robot công nghiệp 1.2 Tổng quan kết cấu khí điều khiển Robot 1.2.1 Kết cấu khí robot 10 1.2.2 Giới thiệu chung điều khiển robot 14 1.3 Giới thiệu chung Robot có số bậc tự dư 15 1.3.1 Giới thiệu tay máy Robot có số bậc tự dư 15 1.3.2 Ứng dụng tay máy Robot có số bậc tự dư 16 Chương Động học vị trí Robot 2.1 Các phép biến đổi tọa độ 19 2.1.1 Mơ tả vị trí hướng hệ trục tọa độ 19 2.1.2 Các phép biến đổi 20 2.2 Động học thuận vị trí robot Planar RRR 25 2.3 Động học ngược vị trí robot Planar RRR 29 Chương Động lực học Robot 3.1 Những khái niệm chung 32 3.1.1 Bài toán động lực học 32 3.1.2 Phương trình động lực học Euler-Lagrance 32 3.2 Phương trình động lực học robot Planar RRR 34 3.2.1 Xác định động nối 34 3.2.2 Phương trình Lagrance 37 3.2.3 Phương trình động lực học robot Planar RRR 38 Chương Điều khiển chuyển động Robot 4.1 Những vấn đề chung điều khiển chuyển động Robot 44 4.1.1 Tiêu chuẩn ổn định Lyapunov 44 4.1.2 Bài toán điều khiển chuyển động 45 4.2 Giới thiệu số hệ thống điều khiển robot 48 4.2.1 Hệ thống điều khiển bù trọng lực với điều khiển PD 48 4.2.2 Hệ thống điều khiển thích nghi dựa theo động lực học ngược 52 4.2.3 Hệ thống điều khiển dựa theo ma trận Jacobian đảo 56 4.2.4 Hệ thống điều khiển dựa theo ma trận Jacobian chuyển vị 58 Chương Điều khiển tay máy có số bậc tự dư 5.1 Những khái niệm chung 60 5.1.1 Khái niệm ma trận Jacobian 60 5.1.2 Tính tốn tốc độ khớp từ giá trị đặt vị trí khâu tác động cuối 63 5.2 Điều khiển robot Planar với bậc tự dư 67 5.2.1 Phân tích cơng việc thành nhiệm vụ nhỏ với quyền ưu tiên khác 67 5.2.2 Các phương trình 68 5.2.3 Nhiệm vụ thứ hai xác định quỹ đạo đặt 71 5.2.4 Nhiệm vụ thứ hai dược xác định hàm tiêu chuẩn 73 5.2.5 Xác định phương pháp tối ưu hóa tức thời 74 5.3 Sử dụng bậc tự dư để tránh vật cản điểm cực hệ thống 75 5.3.1 Tránh vật cản 75 5.3.2 Tránh điểm cực 78 Chương Mô Robot Planar RRR tránh vật cản điểm cực 6.1 Giới thiệu chung MATLAB thư viện Simulink 80 6.2 Mô robot Planar RRR tránh vật cản điểm cực 81 6.2.1 Sơ đồ mô khối chức 81 6.2.2 Kết mô chuyển động Robot Planar tránh vật cản 83 6.2.3 Kết mô chuyển động Robot Planar tránh điểm cực 83 KẾT LUẬN 89 Tài liệu tham khảo 90 Phụ lục: Mã lệnh Matlab 91 MỞ ĐẦU 1.Lý chọn đề tài Thuật ngữ “Robot” xuất sớm năm đầu kỷ 20, kịch nhà văn người Séc: Kerel Capek, thuật ngữ dùng để mô tả máy tạo để phục vụ người Tuy nhiên phải 20 năm sau đó, người Mỹ lần đầu tạo Robot dạng tay máy phục vụ cơng việc chép hình điều khiển từ xa Robot công nghiệp thực xuất vào năm 1961, nhà máy sản xuất ô tô Hoa Kỳ Nhưng từ đến nay, với phát triển mạnh mẽ kỹ thuật vi xử lý công nghệ thông tin, robot công nghiệp tạo ngày đại có độ xác cao Khi thực nhiệm vụ dây chuyền sản xuất công nghiệp hay ngành khoa học khác, yêu cầu đặt Robot cần phải tránh vật cản trình di chuyển Đây lý xuất phương pháp điều khiển Robot với bậc tự dư (Redudancy), phương pháp điều khiển làm tăng độ linh hoạt, khơng giúp cánh tay Robot tránh vật cản mà cịn tránh vị trí mà giá trị đặt khớp vượt giới hạn cho phép ảnh hưởng xấu đến kết cấu Robot Tay máy Robot có số bậc tự dư có lợi ích lớn thực tế sản xuất cơng nghiệp nhiều ngành, lĩnh vực khác, nhiên Việt Nam chưa có nhiều tác giả nghiên cứu phương pháp điều khiển cho Robot có số bậc tự dư, em chọn đề tài: “ Điều khiển tay máy Robot có số bậc tự dư’’ với mong muốn góp phần làm rõ phương pháp điều khiển Robot có số bậc tự dư ứng dụng để giúp Robot tránh vật cản, điểm cực hệ thống Lịch sử nghiên cứu Robot có số bậc tự dư có ứng dụng quan thực tế sản xuất công nghiệp nhiều lĩnh vực khác Có nhiều tài liệu nhiều tác giả khác sâu phân tích tìm hiểu phương pháp điều khiển cho tay máy Robot có số bậc tự dư: - Tác giả H Hanafusa, T Yoshikawa, Y Nakamura coi người nhắc đến khái niệm Robot có số bậc tự dư, với phương pháp điều khiển viết tài liệu: ‘’ Analysis and Control of Articulated Robot Arms with Redudancy’’ vào năm 1981 Tài liệu bổ sung tái nhiều năm sau - Tác giả Maciejew C.A Klei viết phần vận dụng lý thuyết điều khiển tay máy Robot có số bậc tự dư để tránh vật cản tài liệu: “Obstacle Advoidance for Kinematically Redudant Manipulators’’, tài liệu ấn hành năm 1985 - Tới năm 1986, hai tác giả H Hanafusa, T Yoshikawa viết tài liệu đề tài tránh điểm cực với tay máy Robot có số bậc tự dư: “ Inverse Kinematic Solusions with singularity Robustness for Robot manipulator Control’ Từ năm 1986 đến có nhiều báo tác giả khác ứng dụng phương pháp điều khiển tay máy Robot có số bậc tự dư vào cấu hình Robot cụ thể nhằm thực toán làm tăng độ tinh hoạt, thực toán tối ưu khác Ở Việt Nam, nhiều tài liệu viết lĩnh vực Robot có nhắc đến Robot có số bậc tự dư, tác giả đề cập đến ưu điểm nhược điểm Robot có số bậc tự dư, mà chưa thực đề cập đến phương pháp điều khiển biểu thức toán học cụ thể liên quan đến vấn đề Trong luận văn này, nội dung đề cập cụ thể phương pháp điều khiển tay máy Robot có số bậc tự dư dựa tài liệu liên quan liệt kê danh mục tài liệu tham khảo, sau vận dụng lý thuyết vào mơ chuyển động cho Robot Planar RRR nối Mục đích, đối tượng phạm vi nghiên cứu [Hình 6.3a]: Chuyển động Robot Planar không dùng bậc tự dư [Hình 6.3b]: Tọa độ x(t), y(t) khâu tác động cuối không dùng bậc tự dư b Khi sử dụng bậc tự dư Vẫn sử dụng hệ số khuyếch đại khâu tỷ lê, đạo hàm cũ, với chọn 𝑘𝑘1 = 35 Chuyển động robot hình [Hình 6.4] 82 [Hình 6.4a]: Chuyển động Robot Planar với bậc tự dư tránh vật cản [Hình 6.4b]: Tọa độ x(t), y(t) khâu tác động cuối với bậc tự dư c Nhận xét: Trên [Hình 6.3] ta thấy, với vị trí vật cản bố trí hình vẽ, khơng sử dụng bậc tự dư, di chuyển khâu tác động cuối bám theo quỹ đạo đặt trước, nối va vào vật cản Trên [Hình 6.4] ta thấy sử dụng bậc tự dư với giá trị đặt tốc độ khớp: 𝑞𝑞̇ 𝑑𝑑 = 𝐽𝐽1 + � � + (𝐼𝐼3 − 𝐽𝐽1 + 𝐽𝐽1 )𝐻𝐻2 (𝑦𝑦2𝑑𝑑 − 𝑦𝑦2 ) −6(1 − 𝑡𝑡)𝑡𝑡𝑦𝑦𝑜𝑜 83 Trong 𝑘𝑘1 = 35; 𝐻𝐻2 = 𝑘𝑘1 𝐼𝐼2 , di chuyển khâu tác động cuối bám theo quỹ đạo đặt trước, khớp quay đạt tới giá trị đặt: 𝜃𝜃1 45o �𝜃𝜃2 � = �−70o � 𝜃𝜃3 0o Do tránh va chạm với vận cản 6.3 Kết mô robot Planar RRR tránh điểm cực Giá tri đặt tốc độ quay để tránh điểm cực theo biểu thức (5.32) là: 𝑞𝑞̇ 𝑑𝑑 = 𝐽𝐽1 + 𝑦𝑦̇1𝑑𝑑 + (𝐼𝐼 − 𝐽𝐽1 + 𝐽𝐽1 )𝜉𝜉𝑘𝑘𝑝𝑝 a Khi không sử dụng bậc tự dư: Chọn hệ số khuyếch đại khâu tỷ lệ: KP=[1 0 ; ; 0 ]; Hệ số khuyếch đại khâu đạo hàm: KD=[5 0 ; 10 ; 0 ]; Trường hợp không sử dụng bậc tự dư tương ứng trường hợp 𝑘𝑘𝑝𝑝 = [Hình 6.5a] Chuyển động Robot Planar không dùng bậc tự dư 84 [Hình 6.5b] Chuyển động Robot Planar khơng dùng bậc tự dư b Khi sử dụng bậc tự dư Vẫn sử dụng hệ số khuyếch đại khâu tỷ lê, đạo hàm cũ, giá trị đặt tốc độ biểu thức (5.32), với hệ số (𝑘𝑘𝑝𝑝 = 23.5) Chuyển động cánh tay robot thu [Hình 6.6a] [Hình 6.6a] Chuyển động Robot Planar dùng bậc tự dư 85 [Hình 6.6b] Chuyển động Robot Planar dùng bậc tự dư c Nhận xét Trên [Hình 6.5] trường hợp cánh tay robot chuyển động từ vị trí ban đầu 𝑟𝑟𝑜𝑜 = [𝑥𝑥𝑜𝑜 , 𝑦𝑦𝑜𝑜 ]𝑇𝑇 = [0.28, 0.17]𝑇𝑇 Tương ứng với giá trị khớp: 𝑞𝑞 = [180𝑜𝑜 , 170𝑜𝑜 , −10𝑜𝑜 ]𝑇𝑇 Khâu tác động cuối cánh tay robot chuyển động bám theo quỹ đạo có phương trình: 𝑦𝑦1𝑑𝑑 (𝑡𝑡) = � 𝑥𝑥𝑜𝑜 � , ≤ 𝑡𝑡 ≤ 𝑦𝑦𝑜𝑜 − (3 − 2𝑡𝑡)𝑡𝑡 (𝑦𝑦𝑜𝑜 + 0.1) Sử dụng lượng đặt tốc độ là: 𝑞𝑞̇ 𝑑𝑑 = 𝐽𝐽1 + 𝑦𝑦̇1𝑑𝑑 + (𝐼𝐼 − 𝐽𝐽1 +𝐽𝐽1)𝜉𝜉𝑘𝑘𝑝𝑝 Ta nhận thấy không sử dụng bậc tự dư (𝑘𝑘𝑝𝑝 = 0), giá trị góc quay khớp 𝜃𝜃1 thay đổi giá trị từ 180𝑜𝑜 đến 200𝑜𝑜 , điều thực tế khó xảy giới hạn kết cấu khí cánh tay Robot, góc quay hai nối 86 𝑙𝑙1 𝑙𝑙2 khó vượt 180𝑜𝑜 Hay nói cánh khác giá trị góc quay khớp vượt q 180𝑜𝑜 nối với khớp va dập vào gây hỏng cấu hình Robot Trên [Hình 6.6a] mơ chuyển động cánh tay Robot sử dụng bậc tự dư, tương ứng với hệ số phương trình giá trị đặt tốc độ (5.32) (𝑘𝑘𝑝𝑝 = 23.5) Ta nhận thấy giá trị góc quay khớp nhỏ 180𝑜𝑜 Điều nghĩa chuyển động nối phạm vi giới hạn kết cấu khí Tuy nhiên khâu tác động cuối không thực bám theo quỹ đạo đặt trước, điều thể rõ [Hình 6.6b] 87 KẾT LUẬN Sáu chương luận văn trình bày kết q trình nghiên cứu: - Động học vị trí, động lực học, phương pháp thiết kế điều khiển Robot cơng nghiệp Vận dụng tính tốn động học vị trí, động lực học Robot Planar RRR nối - Phương pháp điều khiển tay máy Robot với bậc tự dư: Phân tích nhiệm vụ Robot thành nhiệm vụ với quyền ưu tiên khác Tìm giá trị đặt tốc độ khớp phù hợp để thực nhiệm vụ tránh vật cản điểm cực hệ thống - Vận dụng phương pháp điều khiển tay máy với bậc tự dư để điều khiển cho Robot Planar RRR bậc tự tránh vật cản điểm cực hệ thống Với điều khiển PD bù trọng trường, giá trị đặt tốc độ khớp tính tốn theo phương pháp điều khiển tay máy với bậc tự dư - Sử dụng phần mềm MATLAB-Simulink để mô kết nghiên cứu mơ hình Robot Planar RRR nối Kết mô cho thấy điều khiển Robot planar RRR nối với bậc tự dư Robot tránh vật cản điểm cực hệ thống Tuy nhiên khâu tác động cuối chưa thực bám sát với quỹ đạo đặt trước, dừng lại phương pháp thiết kế hệ thống điều khiển PD bù trọng trường mà chưa sâu vào phương pháp điều khiển tối ưu 88 TÀI LIỆU THAM KHẢO TS Nguyễn Mạnh Tiến, Điều khiển Robot Công nghiệp, Nhà xuất Khoa học kỹ thuật (2009) GS TS Phạn Công Hiền, TS Nguyễn Phạm thục Anh, Mơ hình hóa hệ thống mô phỏng, Nhà xuất Khoa học Kỹ Thuật Hà nội (2006) A Liegcois, K.E, Automatic Supervisory Control of the Configuration and behavior of Multi-body Mechanisms, IEEE Transactions on Systems (1977), pp 868-871 C.A Clein, Obstacle Advoidance for Kinematically Redudant Manipulator, (1985) , pp 109-117 T Yoshikawa, ‘’Analysis and control of Robot Manipulator with Redudancy’’ in Robotic research: The first International Symposium (1985), p.p 735-747 PGS TS Nguyễn Phùng Quang, MATLAB Simulink cho kỹ sư điều khiển tự động, Nhà xuất Khoa học kỹ thuật (2004) Tsuneo Yoshikawa, Foundations of Robotics: Analysis and Control, (1941) 89 PHỤ LỤC Mã lệnh chương trình MATLAB sử dụng mơ Hàm tính giá trị đặt, sai số vị trí tốc độ (ML Function: dqd1) function out=dqd1(in) parasys1; y0=1.39; I2=[1 0;0 0;0 1]; I3=[1 0;0 0;0 1]; q1=in(1); q2=in(2); q3=in(3); q=[q1;q2;q3]; y2=[q1;q2;q3];% chinh la q dq1=in(4); dq2=in(5); dq3=in(6); tt=in(7); dq=[dq1;dq2;dq3]; % Gia tri dat y2d -y2d=[45*pi/180;-70*pi/180;0]; % He so bac tu du H2=35*I2;%40 % Matran gia nghich dao -J11=-l1*sin(q1)-l2*sin(q1+q2)-l3*sin(q1+q2+q3); J12=-l2*sin(q1+q2)-l3*sin(q1+q2+q3); J13=-l3*sin(q1+q2+q3); J21=l1*cos(q1)+l2*cos(q1+q2)+l3*cos(q1+q2+q3); J22=l2*cos(q1+q2)+l3*cos(q1+q2+q3); J23=l3*cos(q1+q2+q3); J1=[J11 J12 J13;J21 J22 J23]; J1c=pinv(J1); % Tính phan errq va errdq -if tt0 s2=sqrt(1-c2^2); else s2=-sqrt(1-c2^2); end s1=((py-0.3*ny)*(1+c2)-s2*(px-0.3*nx))/((1+c2)^2+s2^2); c1=(px-0.3*nx+s1*s2)/(1+c2); 90 qd1=atan2(s1,c1); qd2=atan2(s2,c2); qd3=phi-qd1-qd2; qd=[qd1;qd2;qd3]; dqd=J1c*[0;-6*(1-t)*t*y0]+(I3-J1c*J1)*H2*(y2d-y2); errq=qd-q; errdq=dqd-dq; outtt=[errq;errdq]; end; if tt>1 t=1; px=1.69; py=y0-(3-2*t)*t^2*y0; phi=q1+q2+q3; nx=cos(phi);ny=sin(phi); c2=((px-0.3*nx)^2+(py-0.3*ny)^2-2)/2; if q2>0 s2=sqrt(1-c2^2); else s2=-sqrt(1-c2^2); end s1=((py-0.3*ny)*(1+c2)-s2*(px-0.3*nx))/((1+c2)^2+s2^2); c1=(px-0.3*nx+s1*s2)/(1+c2); qd1=atan2(s1,c1); qd2=atan2(s2,c2); qd3=phi-qd1-qd2; qd=[qd1;qd2;qd3]; dqd=J1c*[0;-6*(1-t)*t*y0]+(I3-J1c*J1)*H2*(y2d-y2); errq=qd-q; errdq=dqd-dq; outtt=[errq;errdq]; end; out=outtt; Hàm tính H^-1 function out=acce1(in) %da check t1 clc parasys1; tvg(1)=in(1); tvg(2)=in(2); tvg(3)=in(3); tvg=tvg'; q1=in(4); q2=in(5); q3=in(6); H(1,1)=m1*lc1^2+m2*l1^2+m2*lc2^2+m3*l1^2+m3*l2^2+m3*lc3^2+J1+J2+J3+2*(m2*l1*lc2+m3*l1*l2) *cos(q2)+2*m3*l1*lc3*cos(q2+q3)+2*m3*l2*lc3*cos(q3); 91 H(1,2)=m2*lc2^2+m3*l2^2+m3*lc3^2+J2+J3+(m2*l1*lc2+m3*l1*l2)*cos(q2)+m3*l1*lc3*cos(q2+q3)+2* m3*l2*lc3*cos(q3); H(1,3)=m3*lc3^2+J3+m3*l1*lc3*cos(q2+q3)+m3*l2*lc3*cos(q3); H(2,1)=m2*lc2^2+m3*l2^2+m3*lc3^2+J2+J3+(m2*l1*lc2+m3*l1*l2)*cos(q2)+m3*l1*lc3*cos(q2+q3)+2* m3*l2*lc3*cos(q3); H(2,2)=m2*lc2^2+m3*l2^2+m3*lc3^2+J2+J3+2*m3*l2*lc3*cos(q3); H(2,3)=m3*lc3^2+J3+m3*l2*lc3*cos(q3); H(3,1)=m3*lc3^2+J3+m3*l1*lc3*cos(q2+q3)+m3*l2*lc3*cos(q3); H(3,2)=m3*lc3^2+J3+m3*l2*lc3*cos(q3); H(3,3)=m3*lc3^2+J3; out=H\tvg; Hàm tính V(Q,DQ) (ML Function: v_cal1) function out=v_cal1(in) parasys1; dq1=in(1); dq2=in(2); dq3=in(3); q1=in(4); q2=in(5); q3=in(6); h112=-2*(m2*l1*lc2+m3*l1*l2)*sin(q2)-2*m3*l1*lc3*sin(q2+q3); h122=-(m2*l1*lc2+m3*l1*l2)*sin(q2)-m3*l1*lc3*sin(q2+q3); h113=-2*m3*l1*lc3*sin(q2+q3)-2*m3*l2*lc3*sin(q3); h123=-m3*l1*lc3*sin(q2+q3)-2*m3*l2*lc3*sin(q3)-m3*l1*lc3*sin(q2+q3); h133=-m3*l1*lc3*sin(q2+q3)-m3*l2*lc3*sin(q3); h233=-m3*l2*lc3*sin(q3); h212=-(m2*l1*lc2+m3*l1*l2)*sin(q2)-m3*l1*lc3*sin(q2+q3); h213=-m3*l1*lc3*sin(q2+q3)-2*m3*l2*lc3*sin(q3); h223=-2*m3*l2*lc3*sin(q3); h312=-m3*l1*lc3*sin(q2+q3); h313=-m3*l1*lc3*sin(q2+q3)-m3*l2*lc3*sin(q3); h323=-m3*l2*lc3*sin(q3); out(1)=h112*dq1*dq2+h122*dq2^2+h113*dq1*dq3+h123*dq2*dq3+h133*dq3^2; out(2)=h233*dq3^2+h212*dq1*dq2+h213*dq1*dq3+h223*dq2*dq3; out(3)=h312*dq1*dq2+h313*dq1*dq3+h323*dq2*dq3; out=[out(1);out(2);out(3)]; 92 Hàm tính G(Q) (ML Function g_cal1) function out=g_cal1(in) parasys1; q1=in(1); q2=in(2); q3=in(3); out(1)=(m1*g*lc1+m2*g*l1+m3*g*l1)*cos(q1)+(m2*g*lc2+m3*g*l2)*cos(q1+q2)+m3*g*lc3*cos(q1+q2+ q3); out(2)=(m2*g*lc2+m3*g*l2)*cos(q1+q2)+m3*g*lc3*cos(q1+q2+q3); out(3)=m3*g*lc3*cos(q1+q2+q3); out=[out(1);out(2);out(3)]; Hàm tính mơ men điều khiển trường hợp tránh vật cản(ML Function: Kd1) function out =Kd1(in) parasys1; hesodieuchinh1; e1=in(1); e2=in(2); e3=in(3); de1=in(4); de2=in(5); de3=in(6); g1=in(7); g2=in(8); g3=in(9); out=KP*[e1;e2;e3]+KD*[de1;de2;de3]+[g1;g2;g3]; Hàm mô chuyển động Robot (ML Function: ktd) function out=ktd(in) parasys1; q1=in(1); q2=in(2); q3=in(3); xO=0; xA=l1*cos(q1); xB=l1*cos(q1)+l2*cos(q2+q1); xC=l1*cos(q1)+l2*cos(q2+q1)+l3*cos(q1+q2+q3); yO=0; yA=l1*sin(q1); yB=l1*sin(q1)+l2*sin(q2+q1); yC=l1*sin(q1)+l2*sin(q2+q1)+l3*sin(q1+q2+q3); x=[xO;xA;xB;xC];y=[yO;yA;yB;yC]; A=[0.5;0.5;1.2;1.2;0.5]; B=[0;-0.3;-0.3;0;0]; r=plot(x,y,A,B); hold on; text(0.7,-0.15,'\rightarrow Obstacle', 'HorizontalAlignment','left'); out=r; 93 Hàm nhập thông số Robot (ML function: parasys) global m1 m2 m3 l1 l2 g J1 J2 J3 m1=2; m2=; m3=0.7;%kg J1=0.0003; J2=0.00015; J3=0.00015;%kg.m2 l1=1; lc1=0.5; l2=1;%m lc2=0.5; l3=0.3; lc3=0.15; g=9.81; Hàm nhập thông số điều chỉnh (ML function: hesodieuchinh1) global KD KP KP=[100 0 ; 100 ; 0 ]; KD=[50 0 ; 50 ; 0 40 ]; Hàm tính mơ men điều khiển trường hợp tránh điểm cực (ML Function: Kd2) function out=dqd2(in) %1 Thong so robot parasys1; %2 Gia tri dat goc khop(rad) q01=180*pi/180; q02=-170*pi/180; q03=-10*pi/180; %3 Vi tri ban dau: y0=l1*sin(q01)+l2*sin(q01+q02)+l3*sin(q01+q02+q03); x0=l1*cos(q01)+l2*cos(q01+q02)+l3*cos(q01+q02+q03); %4 Ma tran don vi I3=[1 0;0 0;0 1]; %5 Goc khop q1=in(1); q2=in(2); q3=in(3); q=[q1;q2;q3]; %6 Gia tri van toc khop dq1=in(4); dq2=in(5); dq3=in(6); dq=[dq1;dq2;dq3]; %7 Thoi gian tt=in(7); kp=23.5; 94 % Matran gia nghich dao J11=-l1*sin(q1)-l2*sin(q1+q2)-l3*sin(q1+q2+q3); J12=-l2*sin(q1+q2)-l3*sin(q1+q2+q3); J13=-l3*sin(q1+q2+q3); J21=l1*cos(q1)+l2*cos(q1+q2)+l3*cos(q1+q2+q3); J22=l2*cos(q1+q2)+l3*cos(q1+q2+q3); J23=l3*cos(q1+q2+q3); J1=[J11 J12 J13;J21 J22 J23]; J1c=pinv(J1); % Tinh V(q) -JJ=J1*J1'; JJinv=inv(JJ); V=sqrt(det(JJ)); q11=JJinv(1,1); q12=JJinv(1,2); q21=JJinv(2,1); q22=JJinv(2,2); % Tinh Cxi J11=[-l1*sin(q1)-l2*sin(q1+q2)-l3*sin(q1+q2+q3) -l2*sin(q1+q2)-l3*sin(q1+q2+q3) -l3*sin(q1+q2+q3)]; J12=[l1*cos(q1)+l2*cos(q1+q2)+l3*cos(q1+q2+q3) l2*cos(q1+q2)+l3*cos(q1+q2+q3) l3*cos(q1+q2+q3)]; J111=[-l1*cos(q1)-l2*cos(q1+q2)-l3*cos(q1+q2+q3) -l2*cos(q1+q2)-l3*cos(q1+q2+q3) l3*cos(q1+q2+q3)]; J121=[-l1*sin(q1)-l2*sin(q1+q2)-l3*sin(q1+q2+q3) -l2*sin(q1+q2)-l3*sin(q1+q2+q3) -l3*sin(q1+q2+q3)]; J122=[-l2*sin(q1+q2)-l3*sin(q1+q2+q3) -l2*sin(q1+q2)-l3*sin(q1+q2+q3) -l3*sin(q1+q2+q3)]; J112=[-l2*cos(q1+q2)-l3*cos(q1+q2+q3) -l2*cos(q1+q2)-l3*cos(q1+q2+q3) -l3*cos(q1+q2+q3)]; J113=[-l3*cos(q1+q2+q3) -l3*cos(q1+q2+q3) -l3*cos(q1+q2+q3)]; J123=[-l3*sin(q1+q2+q3) -l3*sin(q1+q2+q3) -l3*sin(q1+q2+q3)]; cxi1=0.5*V*(q11*(J111*J11'+J111*J11')+q12*(J111*J12'+J121*J11')+q21*(J121*J11'+J111*J12')+q22*(J 121*J12'+J121*J12')); cxi2=0.5*V*(q11*(J112*J11'+J112*J11')+q12*(J112*J12'+J122*J11')+q21*(J122*J11'+J112*J12')+q22*(J 122*J12'+J122*J12')); cxi3=0.5*V*(q11*(J113*J11'+J113*J11')+q12*(J113*J12'+J123*J11')+q21*(J123*J11'+J113*J12')+q22*(J 123*J12'+J123*J12')); cxi=[cxi1;cxi2;cxi3]; % Tính phan errq va errdq if tt0 s2=sqrt(1-c2^2); else s2=-sqrt(1-c2^2); end s1=((py-0.3*ny)*(1+c2)-s2*(px-0.3*nx))/((1+c2)^2+s2^2); c1=(px-0.3*nx+s1*s2)/(1+c2); qd1=atan2(s1,c1); qd2=atan2(s2,c2); qd3=phi-qd1-qd2; qd=[qd1;qd2;qd3]; dqd=J1c*[0;-(y0+0.1)*6*t*(1-t)]+(I3-J1c*J1)*cxi*kp; errq=qd-q; 95 errdq=dqd-dq; outtt=[errq;errdq]; end; if tt>1 t=1; px=x0; py=y0-(3-2*t)*t^2*(y0+0.1); phi=q1+q2+q3; nx=cos(phi);ny=sin(phi); c2=((px-0.3*nx)^2+(py-0.3*ny)^2-2)/2; if q2>0 s2=sqrt(1-c2^2); else s2=-sqrt(1-c2^2); end s1=((py-0.3*ny)*(1+c2)-s2*(px-0.3*nx))/((1+c2)^2+s2^2); c1=(px-0.3*nx+s1*s2)/(1+c2); qd1=atan2(s1,c1); qd2=atan2(s2,c2); qd3=phi-qd1-qd2; qd=[qd1;qd2;qd3]; dqd=J1c*[0;-(y0+0.1)*6*t*(1-t)]+(I3-J1c*J1)*cxi*kp; errq=qd-q; errdq=dqd-dq; outtt=[errq;errdq]; end; out=outtt; 96 ... gian tác động, điều chỉnh lực tác động… 15 1.3.2.Ứng dụng tay máy Robot có số bậc tự dư Trên [Hình 14] số ứng dụng tay máy Robot có số bậc tự dư: [Hình 1.14] Tay máy Robot có bậc tự dư trườn qua... tốn liên quan đến Robot có số bậc tự dư, sau tập trung phân tích phương pháp điều khiển tay máy Robot có số bậc tự dư, vận dụng phương pháp điều khiển để điều khiển tay máy Robot Planar RRR tránh... mà bậc tự dư mang lại, người sử bậc tự dư cho tay máy Robot cơng nghiệp Cần có bậc tự đủ cho việc điều khiển đặt định hướng cấu tác động cuối tên cánh tay Robot, tay máy với bậc tự dư coi có bậc