Xây dựng mô hình và mô phỏng robot song song sử dụng ngôn ngữ matlab

ĐẠI HỌC CÔNG NGHỆ ĐỖ TRẦN THẮNG XÂY DỰNG MÔ HÌNH VÀ MƠ PHỎNG ROBOT SONG SONG SỬ DỤNG NGƠN NGỮ MATLAB NGƯỜI HƯỚNG DẪN: PGS.TS ĐINH VĂN PHONG LUẬN VĂN ThS CƠ HỌC HÀ NỘI 2007 Xây dựng mô hình mơ Robot song song sử dụng ngơn ngữ MATLAB Mục lục Mở đầu 1.1 1.2 1.3 Đặt vấn đề Mục đích luận văn Nội dung luận văn Tổng quan 2.1 Cở sở Robot 2.1.1 Hệ thống Robot 2.1.2 Các vấn đề liên quan với hệ thống Robot 2.1.3 Phân loại Robot 16 2.2 Robot song song 22 Robot song song SPS 27 3.1 3.2 Mô hình Robot song song SPS 30 Vấn đề hình học, động học Robot song song SPS 47 3.2.1 Hình học 47 3.2.2 Động học 57 3.3 Công cụ MATLAB/SIMULINK 61 Kết đạt 64 4.1 4.2 Thư viện hàm giải vấn đề (các m file) 64 Mơ hình, mơ hệ thống Robot PR6-01 67 4.2.1 Kết tính tốn động học ngược 71 4.2.2 Kết tính tốn động học thuận 73 4.2.3 Các kết khác 74 Kết luận 76 Tài liệu tham khảo 78 Phụ lục 79 7.1 7.2 Phụ lục A 79 Phụ lục B 82 II Thank you for evaluating AnyBizSoft PDF Splitter A watermark is added at the end of each output PDF file To remove the watermark, you need to purchase the software from http://www.anypdftools.com/buy/buy-pdf-splitter.html Xây dựng mô hình mơ Robot song song sử dụng ngơn ngữ MATLAB Mở đầu 1.1 Đặt vấn đề Các cấu có cấu trúc động học song song nhiều nhà khoa học giới ý tập trung nghiên cứu từ năm 60 kể từ Stewart sử dụng cấu trúc song song bậc tự để mô buồng tập lái máy bay Cơ cấu có nhiều ưu điểm vượt trội so với cấu chuỗi truyền thống là: khả tải lớn yêu cầu kích thước, kết cấu lại nhỏ gọn cấu chuỗi; tốc độ đáp ứng nhanh với độ tin cậy cao; độ xác lớn nhiều so với cấu chuỗi (vì khơng có sai số cộng tích luỹ hai khâu liên tiếp Robot chuỗi, không yêu cầu có thiết bị dẫn hướng cân cồng kềnh tải trọng làm việc lớn); kết cấu thường đối xứng có nhiều chi tiết giống (các khớp trượt, khớp cầu, khớp đăng, ) thuận tiện cho chế tạo theo hướng mơ đun hố; thiết bị dây nối điều khiển đơn giản, gọn nhẹ Tuy vậy, nay, việc đưa loại cấu trúc vào ứng dụng có hiệu thực tế cịn gặp nhiều khó khăn số nhược điểm sau: có nhiều điểm kỳ dị khó xác định chúng xác (điểm kỳ dị làm độ cứng kết cấu dẫn tới điều khiển làm hạn chế khơng gian làm việc cấu), tốn điều khiển chúng phức tạp đòi hỏi tốc độ xử lý lớn, ảnh hưởng từ trình phát sinh nhiệt lúc Robot hoạt động tới hệ thống cần phải quan tâm Mặc dù cịn hạn chế mặt khơng gian làm việc bé, cấu có cấu trúc động học song song có tiềm ứng dụng lớn cơng việc địi hỏi tính linh hoạt, độ xác, độ ổn định là: quân (bệ đỡ súng tàu thuỷ, xe tăng, xe quân sự, ); gia cơng khí (máy gia cơng cắt gọn kim loại điều khiển số: máy phay, máy tiện, ); công nghệ mô thiết bị tập lái (buồng tập lái máy bay, buồng tập lái xe tăng, ); y tế (các thiết bị hỗ trợ trình phẫu thuật) Robot cơng nghiệp khác Xây dựng mơ hình mơ Robot song song sử dụng ngôn ngữ MATLAB Trong tương lai, với phát triển khoa học công nghệ, đặc biệt khoa học máy tính, cơng nghệ thơng tin, điều khiển, tự động hoá, hứa hẹn giải bước nhược điểm loại cấu trúc 1.2 Mục đích luận án Để sâu nghiên cứu Robot tương lai, mục đích tác giả luận án tự xây dựng củng cố sở lý thuyết hệ thống Robot tích hợp nói chung, sâu vào vấn đề hình học học Robot song song, làm tiền đề cho nghiên cứu chuyên sâu sau với nguyện vọng ứng dụng Robot song song vào thực tế có hiệu Từ sở xây dựng, tác giả phát triển thư viện hàm giải vấn đề Robot song song bậc tự dạng SPS (sẽ giải thích rõ phần 3.1), viết ngơn ngữ MATLAB®: viết phương trình động học giải tốn động học ngược, động học thuận, tìm kiếm không gian làm việc Robot (theo hạn chế giới hạn khớp, va chạm khâu Robot, điểm kỳ dị), viết hàm tiện ích khác Các hàm thư viện hỗ trợ việc mơ hình hố hệ thống Robot song song bậc tự dạng SPS mơ mơi trường MATLAB có sử dụng thêm SIMULINK tuỳ theo mục đích riêng Cuối luận án chọn mơ hình hệ thống Robot có thật, lấy thông số kỹ thuật hệ thống sử dụng hàm thư viện xây dựng, tiến hành mơ hình MATLAB/SIMULINK, mơ phỏng, lấy hiển thị kết 1.3 Nội dung luận án Luận án bao gồm phần sau: Phần mở đầu: giới thiệu vấn đề mà luận án quan tâm, mục đích luận án nội dung luận án thực Xây dựng mơ hình mơ Robot song song sử dụng ngôn ngữ MATLAB Phần tổng quan: tổng hợp, giới thiệu lại hệ thống Robot nói chung, vấn đề liên quan tới hệ thống Robot phân loại Robot, sở phục vụ cho luận án Cuối phần đề cập tới loại Robot song song kiểu SPS hay gọi Hexapod SPS, Robot luận án sâu nghiên cứu Phần sở lý thuyết: tập trung sâu vào công cụ toán học, học liên quan tới Robot phần mềm MATLAB/SIMULINK, phương thức sử dụng chúng để giải vấn đề mà luận án đưa Xây dựng mơ hình đối tượng Robot phần mềm MATLAB, xây dựng viết hàm giải vấn đề động học, hình học, hàm tiện ích khác đối tượng Robot phần mềm MATLAB (các m file) Phần kết đạt được: nêu kết đạt luận án Sử dụng kết để tiến hành mô hệ thống Robot cụ thể đưa kết cụ thể Mơ hình Robot thật mà tác giả chọn để mô hệ thống Robot PR6-01, chi tiết sau Phần kết luận: tổng kết lại nội dung thực luận án theo mục đích đề ra, trình bày dự kiến nghiên cứu tương lai Phần phụ lục: đưa Phương trình ràng buộc chân Robot số chương trình MATLAB giải vấn đề động học ngược, động học thuận tìm biên không gian làm việc Robot cụ thể Xây dựng mơ hình mơ Robot song song sử dụng ngôn ngữ MATLAB Tổng quan 2.1 Cở sở Robot Thuật ngữ Robot có từ năm 1923, đến có nhiều định nghĩa giải thích cho thuật ngữ Tác giả xin trích dẫn định nghĩa Robot Robotics Institute of America - "A Robot is a re-programmable multi-function manipulator designed to move material, parts, tools, or specialized devices, through variable programmed motions for the performance of a variety of tasks." - tạm dịch là: "Một Robot tay máy đa chức lập trình lại, thiết kế để di chuyển vật liệu, chi tiết khí, cơng cụ thiết bị đặc biệt theo chuyển động lập trình để thực nhiều công việc khác nhau" Ngày quan điểm, cách nhìn Robot xã hội lồi người thay đổi nhiều thân thiện đời sống, thông minh ứng dụng trợ giúp thay người Robot, Khơng có để bàn cãi khẳng định Robot ngày tương lai tiếp tục phát triển độ thông minh khả ứng dụng đa nhiệm vụ a) Robot công nghiệp b) Robot thiết bị tập lái c) Robot dạng người Hình 2.1 Một số ứng dụng Robot Hình 2.1 minh hoạ số loại Robot ứng dụng nhiều lĩnh vực khác thực tế Hình 2.1 a) loại Robot tay máy sử dụng công nghiệp, Hình 2.1 b) Robot có cấu trúc song song ứng dụng việc mô Xây dựng mô hình mơ Robot song song sử dụng ngơn ngữ MATLAB thiết bị tập lái, Hình 2.1 c) Robot ASIMO dạng người sử dụng giải trí 2.1.1 Hệ thống Robot Robot cấu trúc máy sáng tạo để phục vụ lợi ích người Máy bao gồm khâu (thường vật rắn không biến dạng, nhiên công việc đòi hỏi tải trọng lớn tốc độ đáp ứng cao, ảnh hưởng biến dạng vật liệu cần phải đưa vào xem xét) nối ghép với khớp tạo thành chuỗi động học hay gọi cấu Mỗi cấu có nhiệm vụ biến đổi chuyển động, truyền tải lực mômen từ khâu ban đầu (khâu đầu vào - thường cố định) đến khâu cuối (khâu đầu ra) để thực theo nhiệm vụ xác định Robot - máy tích hợp nối ghép cấu (gọi thiết bị biến đổi chuyển động) với thiết bị điều khiển (máy tính, vi điều khiển, thiết bị điều phối chuyển động chuyên dụng ), thiết bị biến đổi lượng, thiết bị ghép nối (cáp truyền thông tin, cáp điện), thiết bị khuyếch đại công suất, thiết bị chấp hành (xilanh, chân trượt vítme, ), thiết bị cảm biến (đầu đo lực, đầu đo vị trí, camera, ) thiết bị khác để biến đổi lượng thành cơng có ích đạt mục đích sử dụng người Tuỳ theo mục đích ứng dụng cụ thể mà hệ thống đơn giản vô phức tạp sơ đồ nối ghép lẫn thành phần, nhiên mặt bản, hệ thống Robot thường có cấu trúc minh hoạ Hình 2.2 Từ sơ đồ Hình 2.2, thấy hệ thống Robot điển hình gồm có phần chính: 1) Máy tính với phần mềm 2) Bộ điều khiển 3) Mơ hình khí Robot Máy tính lưu trữ trạng thái khâu cuối Robot theo thời gian (xe , y e , z e , ϕ e ,θ e ,ψ e ) , tính tốn trạng thái biến khớp chủ động truyền kết xuống điều khiển (θ1 ,θ ,θ , ) Dựa liệu mà máy tính gửi xuống, điều Xây dựng mơ hình mơ Robot song song sử dụng ngôn ngữ MATLAB khiển điều khiển cấu chấp hành cho Robot thực công việc mong muốn Máy tính Bộ điều khiển Robot Thiết bị theo dõi (bằng Lazer hình ảnh) Hình 2.2 Sơ đồ cấu trúc hệ thống Robot điển hình Nhận xét từ sơ đồ trên, đọc giả nhận thấy điều, khoa học Robot khoa học liên ngành, bao gồm kết hợp nhiều ngành khoa học công nghệ khác như: Cơ học, Điều khiển, Thơng tin, Điện tử, Chính xuất nhiều vấn đề liên quan tới hệ thống Robot cần giải 2.1.2 Các vấn đề liên quan với hệ thống Robot Để đọc giả có nhìn khái qt vấn đề cần giải với hệ thống Robot, tác giả muốn nhắc lại ngắn gọn vấn đề quan trọng sau: a) Cấu trúc Cấu trúc vấn đề liên quan tới việc xây dựng sơ đồ ghép nối khâu Robot Mơ hình khí Robot phận hệ thống Robot bao gồm: Các khâu (thường vật rắn) liên kết với khớp (còn gọi cặp động học) thiết kế để khâu tác dụng cuối thực cơng việc mong đợi Xây dựng mơ hình mơ Robot song song sử dụng ngôn ngữ MATLAB Để làm Robot di chuyển, cần phải làm số khớp Robot chuyển động nhờ cấu chấp hành (động cơ) - khớp gọi khớp chủ động giới hạn hành trình cho phép chúng Các khớp khớp chủ động gọi khớp thụ động Một số loại khớp hay sử dụng Robot là: khớp trượt (P - Prismatic), khớp quay (R - Revolution), khớp cầu (S - Spherical), khớp đăng (U - Universal), … a) Khớp trượt (P) b) Khớp lề (R) c) Khớp cầu (S) d) Khớp đăng (U) Hình 2.3 Các loại khớp hay dùng Robot Trên Hình 2.3 mơ tả số loại khớp hay sử dụng Robot Hình 2.3 a) loại khớp trượt có bậc tự chuyển động tương đối, Hình 2.3 b) loại khớp có bậc tự do, chuyển động tương đối chuyển động quay quanh trục khớp Hai loại khớp thường sử dụng Robot dạng chuỗi Hình 2.3 c) loại khớp cầu có ba bậc tự ứng với ba Xây dựng mơ hình mơ Robot song song sử dụng ngôn ngữ MATLAB Kết luận Phần kết luận tổng kết lại nội dung mà luận án thực theo mục đích đề ban đầu Phần Tổng quan giới thiệu cấu trúc hệ thống Robot nói chung, vấn đề liên quan tới hệ thống Robot cần xem xét, phân loại Robot theo tiêu chí khác cuối luận án giới thiệu Robot có cấu trúc song song, loại Robot có triển vọng ứng dụng nhiều tương lai Từ sở phần Tổng quan, tác giả chọn cấu trúc Robot cụ thể có khả ứng dụng lớn tương lai tiến hành sâu thêm vào vấn đề hình học học loại Robot này-Robot Robot song song kiểu SPS Tiếp theo, tác giả tiến hành tự xây dựng mơ hình Robot song song SPS tham số hoá, xây dựng thuật toán thư viện hàm giải vấn đề hình học động học cho loại Robot dạng m file chạy môi trường MATLAB/SIMULINK Sử dụng mơ hình Robot thư viện hàm giải vấn đề hình học, động học phần Cơ sở lý thuyết SIMULINK cho phép xây dựng hệ thống Robot song song đơn giản mô nó, hiển thị kết phần mềm MATLAB/SIMULINK Trong phần Kết đạt được, luận án sử dụng kết đạt phần Cơ sở lý thuyết để mô hoạt động hệ thống Robot thật hiển thị kết Đó Robot PR6-01, sản phẩm đề tài: “Nghiên cứu, thiết kế chế tạo Robot Cơ điện tử cấu song song ứng dụng kỹ nghệ” - cấp viện Khoa học Công nghệ Việt Nam năm 2002, tập thể cán nghiên cứu thuộc phòng Cơ điện tử, viện Cơ học nghiên cứu chế tạo Những kết luận án sở ban đầu giúp tác giả tiếp tục sâu vào nghiên cứu Robot tương lai Tuy nhiên, với mong muốn đưa nghiên cứu vào ứng dụng thực tế hiệu quả, tác giả tự nhận thấy nhiều vấn đề phức tạp cần quan tâm nghiên cứu nghiêm túc là: vấn đề động lực học Robot để phục vụ điều khiển nâng cao hệ thống Robot 76 Xây dựng mơ hình mơ Robot song song sử dụng ngơn ngữ MATLAB thiết kế khí Robot; vấn đề mơ hình giải số giải tích Robot hay mơ hình hỗn hợp giải số giải tích Robot, thuật tốn giải số hệ nhiều vật (có thể vật rắn vật biến dạng), tốc độ thuật giải độ xác mơ hình tốn với mơ hình Robot thật; vấn đề xây dựng, thiết lập quỹ đạo cho Robot; … Mặc dù cịn nhiều khó khăn thử thách phía trước, tác giả ln hy vọng vào đam mê, tâm giúp đỡ bạn bè, thầy giáo, cô giáo, đồng nghiệp để biến ý tưởng thành thực phục vụ lợi ích người 77 Xây dựng mơ hình mơ Robot song song sử dụng ngôn ngữ MATLAB Tài liệu tham khảo [1] Lung Wen Tsai: Robot analysis - The Mechanics of Serial and Parallel Manipulators A Wiley - Interscience Publication 1999 [2] Ahmed A Shabana: Dynamics of Multibody System Cambridge University Press 1998 [3] Phillip John McKerrow: Introduction to Robotics Publishing Company 1991 [4] Robert H Bishop - Editor-in-Chief: The Mechatronics Handbook CRC Press LLC 2002 [5] Richard C Dorf and Robert H Bishop: Modern Control Systems, Seventh Edition Addision - Wesley Publishing Company [6] Hubert Hahn: Rigid Body Dynamics of Mechanisms, Theoretical Basis Springer-Verlag Berlin Heigelberg 2002 [7] Parviz E Nikravesh: Computer-Aided Analysis of Mechanical Systems Pretice-Hall A Division of Simon & Schuster Asia Pte Ltd., Singapore 1988 [8] Edward J Haug: Computer Aided Kinematics and Dynamics of Mechanical Systems Volume I: Basic Methods Allyn and Bacon A Division of Simon & Schuster, Massachusetts 1988 [9] Phan Nguyên Di, Nguyễn Văn Khang: Giáo trình động lực học máy, Nhà xuất Khoa học Kỹ thuật 2001 Addison-Wesley [10] Đào Văn Hiệp: Kỹ thuật Robot Nhà xuất Khoa học Kỹ thuật 2003 [11] Phạm Văn Bạch Ngọc, Vũ Thanh Quang, Đỗ Trần Thắng, Phạm Anh Tuấn: Thiết kế robot cấu song song ứng dụng gia công khí xác, Tuyển tập Báo cáo Hội nghị Cơ học toàn quốc kỷ niệm 25 năm thành lập Viện Cơ học, 2004 [12] Le Xuan Huy, Hoang Nga, Do Tran Thang, Pham Anh Tuan: Determination of control data based on dynamic simulation for Hexapod “Proceedings: The 8th International Conference on Mechatronics Technology”, ICMT 2004, Hanoi, Vietnam, Page 107-112 78 Xây dựng mơ hình mơ Robot song song sử dụng ngôn ngữ MATLAB Phụ lục 7.1 Phụ lục A Phụ lục A đưa số chương trình viết phần mềm MATLAB Giải tốn động học ngược MATLAB hàm Inverse.m so sánh kết với việc giải phần mềm SACR Robot=struct('RB',250, 'RP',175, 'RS',25, %Ban kinh cua chan truot 'anpha',40*pi/180, 'beta',30*pi/180, 'hB',0.0, %Chieu cao tu mat tren cua tam co dinh toi tam cac khop cau tren tam co dinh 'hP',0.0, %Chieu cao tu tam cac khop cau tren tam di dong den mat tren cua tam di dong 'hNominal',450, %Chieu cao tu tam tam co dinh den tam tam di dong tam co dinh va tam di dong song song voi 'TeltaBmax',45*pi/180, %Goc quay lon nhat o cac khop cau lien ket tren be mat co dinh 'TeltaPmax',25*pi/180, %Goc quay lon nhat o cac khop cau lien ket tren be mat di dong 'Lmax',850, %Chieu dai chan lon nhat 'Lmin',650)%Chieu dai chan ngan nhat fid = fopen('C:\Quydao.txt', 'r');%Toa chuyen dong cua be mat chuyen dong while feof(fid)==0 c = fscanf(fid,'%e',[7 inf]) end fclose(fid); a=c' [m,n]=size(a) M=[]%Bien luu anh chuyen dong cua ROBOT K=zeros(m,6) for i=1:m PlatformCor(1,1)=a(i,2)*1000; PlatformCor(1,2)=a(i,3)*1000; PlatformCor(1,3)=a(i,4)*1000; PlatformCor(1,4)=a(i,5) PlatformCor(1,5)=a(i,6) PlatformCor(1,6)=a(i,7) [L]=Inverse(Robot,PlatformCor)%Ham giai bai toan dong hoc nguoc K(i,:)=L(1,:)%Luu giu chieu dai chan theo thoi gian [Flag]=PoseCheck(Robot,PlatformCor)%Kiem tra vi tri ROBOT [F]=DrawRobot(Robot,PlatformCor,20);%Ve ROBOT 79 Xây dựng mơ hình mơ Robot song song sử dụng ngôn ngữ MATLAB M=[M F]; end movie(M) fid = fopen('C:\6LegDisp.txt', 'r');%Lay ket qua tinh toan tu phan mem SACR while feof(fid)==0 d = fscanf(fid,'%e',[7 inf]) end fclose(fid); b=d' %Ket qua tinh toan chieu dai chan bang phan mem SACR figure hold on plot(a(:,1),b(:,2)*1000,'-r') plot(a(:,1),b(:,3)*1000,'-g') plot(a(:,1),b(:,4)*1000,'-b') plot(a(:,1),b(:,5)*1000,'-y') plot(a(:,1),b(:,6)*1000,'-m') plot(a(:,1),b(:,7)*1000,'-c') %Ket qua tinh toan chieu dai chan bang ham Inverse.m figure hold on plot(a(:,1),K(:,1),'-r') plot(a(:,1),K(:,2),'-g') plot(a(:,1),K(:,3),'-b') plot(a(:,1),K(:,4),'-y') plot(a(:,1),K(:,5),'-m') plot(a(:,1),K(:,6),'-c') figure hold on plot(a(:,1),b(:,2)*1000,'-r') plot(a(:,1),K(:,1),'-r') figure hold on line(a(:,2)*1000,a(:,3)*1000,a(:,4)*1000) DrawRobot(Robot,PlatformCor,20); Tìm kiếm vẽ biên không gian làm việc Robot PR6-01 hàm OWSpace.m Robot=struct('RB',250, 'RP',175, 'RS',25, %Ban kinh cua chan truot 'anpha',40*pi/180, 'beta',30*pi/180, 'hB',0, %Chieu cao tu mat tren cua tam co dinh toi tam cac khop cau tren tam co dinh 'hP',0, %Chieu cao tu tam cac khop cau tren tam di dong den mat tren cua tam di dong 'hNominal',450, %Chieu cao tu tam tam co dinh den tam tam di dong tam co dinh va tam di dong song song voi 'TeltaBmax',45*pi/180, %Goc quay lon nhat o cac khop cau lien ket tren be mat co dinh 80 Xây dựng mơ hình mơ Robot song song sử dụng ngôn ngữ MATLAB 'TeltaPmax',25*pi/180, %Goc quay lon nhat o cac khop cau lien ket tren be mat di dong 'Lmax',850, %Chieu dai chan lon nhat 'Lmin',650)%Chieu dai chan ngan nhat DeltaPhi=0.05 DeltaTelta=0.05 DeltaC3=0.08 Epsilon=0.07 OrientE=zeros(1,3) OrientE(1,2)=0.0 [WSEnvelopX WSEnvelopY WSEnvelopZ]=OWSpace(Robot,OrientE,DeltaPhi,DeltaTelta,DeltaC3,Epsilon) tri = delaunay(WSEnvelopX, WSEnvelopY) figure trisurf(tri,WSEnvelopX, WSEnvelopY, WSEnvelopZ) figure trimesh(tri,WSEnvelopX, WSEnvelopY, WSEnvelopZ) OrientE=zeros(1,3) OrientE(1,2)=0.1 [WSEnvelopX WSEnvelopY WSEnvelopZ]=OWSpace(Robot,OrientE,DeltaPhi,DeltaTelta,DeltaC3,Epsilon) tri = delaunay(WSEnvelopX, WSEnvelopY) figure trisurf(tri,WSEnvelopX, WSEnvelopY, WSEnvelopZ) figure trimesh(tri,WSEnvelopX, WSEnvelopY, WSEnvelopZ) OrientE=zeros(1,3) OrientE(1,1)=0.1 [WSEnvelopX WSEnvelopY WSEnvelopZ]=OWSpace(Robot,OrientE,DeltaPhi,DeltaTelta,DeltaC3,Epsilon) tri = delaunay(WSEnvelopX, WSEnvelopY) figure trisurf(tri,WSEnvelopX, WSEnvelopY, WSEnvelopZ) figure trimesh(tri,WSEnvelopX, WSEnvelopY, WSEnvelopZ) Giải toán động học thuận Robot sử dụng hàm Forwards.m Robot=struct('RB',250, 'RP',175, 'RS',25, %Ban kinh cua chan truot 'anpha',40*pi/180, 'beta',30*pi/180, 'hB',0, %Chieu cao tu mat tren cua tam co dinh toi tam cac khop cau tren tam co dinh 81 Xây dựng mơ hình mơ Robot song song sử dụng ngôn ngữ MATLAB 'hP',0, %Chieu cao tu tam cac khop cau tren tam di dong den mat tren cua tam di dong 'hNominal',450, %Chieu cao tu tam tam co dinh den tam tam di dong tam co dinh va tam di dong song song voi 'TeltaBmax',45*pi/180, %Goc quay lon nhat o cac khop cau lien ket tren be mat co dinh 'TeltaPmax',25*pi/180, %Goc quay lon nhat o cac khop cau lien ket tren be mat di dong 'Lmax',850, %Chieu dai chan lon nhat 'Lmin',650)%Chieu dai chan ngan nhat fid = fopen('C:\Quydao.txt', 'r'); while feof(fid)==0 c = fscanf(fid,'%e',[7 inf]) end fclose(fid); a=c' PlatformCor=zeros(1,6) PlatformCor(1,1)=a(1,2)*1000; PlatformCor(1,2)=a(1,3)*1000; PlatformCor(1,3)=a(1,4)*1000; PlatformCor(1,4)=a(1,5) PlatformCor(1,5)=a(1,6) PlatformCor(1,6)=a(1,7) L=zeros(1,6) L(1,1)=100 L(1,2)=150 L(1,3)=180 L(1,4)=150 L(1,5)=120 L(1,6)=100 M=[] [PositionB]=Forwards(Robot,PlatformCor,L,20) for i=1:21 [F]=DrawRobot(Robot,PositionB(i,:),20) [F]=DrawRobot(Robot,PlatformCor,20) M=[M F] end figure line(PositionB(:,1),PositionB(:,2),PositionB(:,3)) 7.2 Phụ lục B Phụ lục B đưa phương trình ràng buộc chân Robot luận án 82 Xây dựng mơ hình mơ Robot song song sử dụng ngơn ngữ MATLAB Phương trình (3.40) ⎧ l1 ⎪C1 = R1 − sϕ1,2.sθ 1,2 − c⎛⎜ π − α ⎞⎟.R = B ⎪ ⎝3 2⎠ ⎪ ⎪ l11 1,2 1,2 ⎛ π α ⎞ ⎪C = R1 + 2 cϕ sθ − s⎜⎝ − ⎟⎠.RB = ⎪ ⎪ ⎪ l1 1,2 − h = ⎪C = R3 − cθ B ⎪ ⎪C = R1 + d sϕ1,2.sθ 1,2 − R = 1 ⎪ ⎪ , , − R22 = ⎪C = R2 − d1.cϕ sθ ⎪ + d cθ 1,2 − R = ⎪C = R3 ⎪ ⎪ 2 ⎛⎜ l1 ⎞⎟ ⎪ ⎛ ⎛π α ⎞ ⎞ ⎛ ⎞ ⎛π α ⎞ ⎛ ⎞ ⎪C = ⎜⎜ R1 − c⎜ − ⎟.RB ⎟⎟ + ⎜⎜ R2 − s⎜ − ⎟.RB ⎟⎟ + ⎜ R3 − hB ⎟ − ⎜ ⎟ = ⎝ ⎠ ⎜2⎟ ⎝3 2⎠ ⎝3 2⎠ ⎠ ⎝ ⎠ ⎝ ⎪ ⎝ ⎠ ⎪ 2 ⎪ ⎛ 2 1⎞ ⎞ ⎛ ⎪C = ⎜ R1 − R11 ⎞⎟ + ⎛⎜ R22 − R1 ⎟⎠ + ⎜⎝ R3 − R3 ⎟⎠ − d1 = ⎝ ⎠ ⎝ ⎪ ⎪ ⎛ 13 ⎛β ⎞ ⎪ 13.cψ 13 − c⎛⎜ β ⎞⎟.R cθ 13.sϕ13.sψ 13 − s⎛⎜ β ⎞⎟.R cϕ13.sψ 13 ⎞⎟ ⎜ R c R c + ϕ ⎜ ⎟ ⎪ P P P ⎟ ⎝2⎠ ⎝2⎠ ⎝2⎠ ⎪C = ⎜⎜ ⎟ + ⎪ β ⎛ ⎞ 13 13 13 13 13 ⎜ ⎟ ⎪ ⎜ − s⎜ ⎟.RP cθ cψ sϕ + hP sϕ sθ − R1 ⎟ ⎨ ⎝ ⎝ ⎠ ⎠ ⎪ ⎪⎛ 13 ⎛β ⎞ 13 13 ⎛ β ⎞ 13 13 13 ⎛ β ⎞ 13 13 ⎞ ⎪⎜ R2 + c⎜ ⎟.RP cψ sϕ + c⎜ ⎟.RP cϕ cθ sψ − s⎜ ⎟.RP sϕ sψ ⎟ 2 ⎜ ⎟ ⎝ ⎠ ⎝ ⎠ ⎝ ⎠ ⎪ ⎟ + ⎪⎜ ⎛ β ⎞ ⎟ ⎪⎜ + s⎜ ⎟.R cϕ13.cθ 13.cψ 13 − h cϕ13.sθ 13 − R P ⎟ ⎪⎜⎝ ⎝ ⎠ P ⎠ ⎪ ⎪ ⎛ l1 ⎞2 ⎞ ⎪⎛ 13 ⎛β ⎞ ⎛β ⎞ ⎜ 2⎟ 13 13 13 13 13 ⎜ ⎟ ⎪⎜ R3 + c⎜ ⎟.RP sθ sψ + s⎜ ⎟.RP cψ sθ + hP cθ − R3 ⎟ − ⎜ ⎟ = ⎜ ⎟ ⎝ ⎠ ⎝ ⎠ ⎠ ⎪⎝ ⎝ ⎠ ⎪ ⎪ ⎞ ⎛ 13 ⎛β ⎞ ⎛β ⎞ ⎛β ⎞ ⎪ ⎜ R + c⎜ ⎟.RP cϕ13.cψ 13 − c⎜ ⎟.RP cθ 13.sϕ13.sψ 13 − s⎜ ⎟.RP cϕ13.sψ 13 ⎟ ⎪ 10 ⎜ ⎟ ⎝2⎠ ⎝2⎠ ⎝2⎠ ⎪C = ⎜ ⎟ + ⎪ ⎟ ⎜ − s⎛⎜ β ⎞⎟.R cθ 13.cψ 13.sϕ13 + h sϕ13.sθ 13 − c⎛⎜ π − α ⎞⎟.R P P B ⎟ ⎜ ⎪ ⎝2⎠ ⎝3 2⎠ ⎠ ⎝ ⎪ ⎪ ⎪⎛⎜ R13 + c⎛⎜ β ⎞⎟.R cψ 13.sϕ13 + c⎛⎜ β ⎞⎟.R cϕ13.cθ 13.sψ 13 − s⎛⎜ β ⎞⎟.R sϕ13.sψ 13 ⎞⎟ P P P ⎪⎜ ⎟ ⎝2⎠ ⎝2⎠ ⎝2⎠ ⎪⎜ ⎟ + ⎪⎜ ⎛ β ⎞ 13.cθ 13.cψ 13 − h cϕ13.sθ 13 − s⎛⎜ π − α ⎞⎟.R ⎟ s R c + ϕ ⎜ ⎟ ⎪⎜ P P B ⎟ ⎝3 2⎠ ⎠ ⎪⎝ ⎝ ⎠ ⎪ 2 ⎛ l1 + l1 ⎞ ⎪⎛ ⎞ β β ⎛ ⎞ ⎛ ⎞ ⎟ ⎜ 13 13 13 13 13 13 ⎪⎜ R + c⎜ ⎟.R sθ sψ + s⎜ ⎟.R cψ sθ + h cθ − h ⎟ − P P P B ⎟ ⎜⎜ + d1 ⎟⎟ = ⎪⎜⎝ ⎝2⎠ ⎝2⎠ ⎠ ⎠ ⎝ ⎩ ( ) 83 Xây dựng mơ hình mô Robot song song sử dụng ngôn ngữ MATLAB Phương trình (3.41) ⎧ l2 ⎪C11 = R3 − sϕ 3,4.sθ 3,4 − c⎛⎜ π − α ⎞⎟.R = B ⎪ ⎝3 2⎠ ⎪ ⎪ l2 ⎛π α ⎞ ⎪C12 = R23 + cϕ 3,4.sθ 3,4 + s⎜ − ⎟.RB = ⎪ ⎝3 2⎠ ⎪ ⎪ 13 3,4 − h = l1 ⎪C = R3 − cθ B ⎪ ⎪C14 = R3 + d sϕ 3,4.sθ 3,4 − R = ⎪ ⎪ 15 , , − R24 = ⎪C = R2 − d 2.cϕ sθ ⎪ ⎪C16 = R33 + d 2.cθ 3,4 − R34 = ⎪ ⎪ 2 ⎛⎜ l ⎞⎟ ⎪ 17 ⎛ ⎞ ⎛ ⎞ ⎛π α ⎞ ⎛π α ⎞ ⎛ ⎞ ⎪C = ⎜⎜ R1 − c⎜ − ⎟.RB ⎟⎟ + ⎜⎜ R2 + s⎜ − ⎟.RB ⎟⎟ + ⎜ R3 − hB ⎟ − ⎜ ⎟ = ⎝ ⎠ ⎜ ⎟ ⎝3 2⎠ ⎝3 2⎠ ⎠ ⎝ ⎠ ⎝ ⎪ ⎝ ⎠ ⎪ 2 ⎪ 18 ⎛ ⎪C = ⎜ R1 − R13 ⎞⎟ + ⎛⎜ R24 − R23 ⎞⎟ + ⎛⎜ R34 − R33 ⎞⎟ − d = ⎝ ⎠ ⎝ ⎠ ⎝ ⎠ ⎪ ⎪ ⎛ 13 ⎛β ⎞ ⎪ 13.cψ 13 − c⎛⎜ β ⎞⎟.R cθ 13.sϕ13.sψ 13 + s⎛⎜ β ⎞⎟.R cϕ13.sψ 13 ⎞⎟ ⎜ R c R c + ϕ ⎜ ⎟ ⎪ P P P ⎟ ⎝2⎠ ⎝2⎠ ⎝2⎠ ⎪C19 = ⎜⎜ ⎟ + ⎪ β ⎛ ⎞ 13 13 13 13 13 ⎜ ⎟ ⎪ ⎜ + s⎜ ⎟.RP cθ cψ sϕ + hP sϕ sθ − R1 ⎟ ⎨ ⎝ ⎝ ⎠ ⎠ ⎪ ⎪⎛ 13 ⎛β ⎞ 13 13 ⎛ β ⎞ 13 13 13 ⎛ β ⎞ 13 13 ⎞ ⎪⎜ R2 + c⎜ ⎟.RP cψ sϕ + c⎜ ⎟.RP cϕ cθ sψ + s⎜ ⎟.RP sϕ sψ ⎟ 2 ⎜ ⎟ ⎝ ⎠ ⎝ ⎠ ⎝ ⎠ ⎪ ⎟ + ⎪⎜ ⎛ β ⎞ ⎟ ⎪⎜ − s⎜ ⎟.R cϕ13.cθ 13.cψ 13 − h cϕ13.sθ 13 − R P ⎟ ⎪⎜⎝ ⎝ ⎠ P ⎠ ⎪ ⎪ ⎛ l ⎞2 ⎞ ⎪⎛ 13 ⎛β ⎞ ⎛β ⎞ ⎜ 2⎟ 13 13 13 13 13 ⎜ ⎟ ⎪⎜ R3 + c⎜ ⎟.RP sθ sψ − s⎜ ⎟.RP cψ sθ + hP cθ − R3 ⎟ − ⎜ ⎟ = ⎜ ⎟ ⎝ ⎠ ⎝ ⎠ ⎠ ⎪⎝ ⎝ ⎠ ⎪ ⎪ ⎞ ⎛ 13 ⎛β ⎞ ⎛β ⎞ ⎛β ⎞ ⎪ ⎜ R + c⎜ ⎟.RP cϕ13.cψ 13 − c⎜ ⎟.RP cθ 13.sϕ13.sψ 13 + s⎜ ⎟.RP cϕ13.sψ 13 ⎟ ⎪ 20 ⎜ ⎟ ⎝2⎠ ⎝2⎠ ⎝2⎠ =⎜ ⎪C ⎟ + ⎪ ⎟ ⎜ + s⎛⎜ β ⎞⎟.R cθ 13.cψ 13.sϕ13 + h sϕ13.sθ 13 − c⎛⎜ π − α ⎞⎟.R P P B ⎟ ⎜ ⎪ ⎝2⎠ ⎝3 2⎠ ⎠ ⎝ ⎪ ⎪ ⎪⎛⎜ R13 + c⎛⎜ β ⎞⎟.R cψ 13.sϕ13 + c⎛⎜ β ⎞⎟.R cϕ13.cθ 13.sψ 13 + s⎛⎜ β ⎞⎟.R sϕ13.sψ 13 ⎞⎟ P P P ⎪⎜ ⎟ ⎝2⎠ ⎝2⎠ ⎝2⎠ ⎪⎜ ⎟ + ⎪⎜ ⎛ β ⎞ 13.cθ 13.cψ 13 − h cϕ13.sθ 13 + s⎛⎜ π − α ⎞⎟.R ⎟ s R c − ϕ ⎜ ⎟ ⎪⎜ P P B ⎟ ⎝3 2⎠ ⎠ ⎪⎝ ⎝ ⎠ ⎪ 2 ⎛ l2 + l2 ⎞ ⎪⎛ ⎞ β β ⎛ ⎞ ⎛ ⎞ ⎟ ⎜ 13 13 13 13 13 13 ⎪⎜ R + c⎜ ⎟.R sθ sψ − s⎜ ⎟.R cψ sθ + h cθ − h ⎟ − P P P B ⎟ ⎜⎜ + d ⎟⎟ = ⎪⎜⎝ ⎝2⎠ ⎝2⎠ ⎠ ⎠ ⎝ ⎩ ( ) 84 Xây dựng mơ hình mơ Robot song song sử dụng ngơn ngữ MATLAB Phương trình (3.42) ⎧ l3 π α ⎪C 21 = R15 − sϕ 5,6 sθ 5,6 − c⎛⎜ + ⎞⎟.RB = ⎪ ⎝3 2⎠ ⎪ ⎪ 22 5,6 5,6 + s⎛⎜ π + α ⎞⎟.R = l1 ⎪C = R2 + cϕ sθ B ⎝3 2⎠ ⎪ ⎪ ⎪C 23 = R5 − l1 cθ 5,6 − h = B ⎪ ⎪ 24 5,6 5,6 − R = ⎪C = R1 + d3.sϕ sθ ⎪ 25 5 , , − R26 = ⎪C = R2 − d3.cϕ sθ ⎪ ⎪C 26 = R35 + d3.cθ 5,6 − R36 = ⎪ ⎪ 2 ⎛⎜ l13 ⎞⎟ ⎛ ⎛π α ⎞ ⎞ ⎞ ⎪ 27 ⎛⎜ ⎛ π α ⎞ ⎜ ⎟ ⎟ + − − =0 C R c R + + + R s R R h = − + ⎜ ⎟ ⎜ ⎟ B ⎜ ⎜ ⎪ ⎜ 2⎟ ⎠ B ⎟⎠ ⎠ B ⎟⎠ ⎝ ⎝ ⎝ ⎝ ⎪ ⎝ ⎠ ⎪ 2 2 28 6 ⎪C = R − R 1 + R2 − R2 + R3 − R3 − (d3 ) = ⎪ ⎪ ⎛ 13 ⎛ π β ⎞ ⎞ ⎛π β ⎞ ⎛π β ⎞ ⎪ ⎜ R1 − c⎜ + ⎟.RP cϕ13.cψ 13 + c⎜ + ⎟.RP cθ 13.sϕ13.sψ 13 + s⎜ + ⎟.RP cϕ13.sψ 13 + ⎟ ⎪ 29 ⎜ ⎟ ⎝3 2⎠ ⎝3 2⎠ ⎝3 2⎠ ⎪C = ⎜ ⎟ + π β ⎛ ⎞ ⎪ ⎜ s⎜ + ⎟.R cθ 13.cψ 13.sϕ13 + h sϕ13.sθ 13 − R ⎟ P ⎜ P ⎟ ⎪⎪ ⎝ ⎠ ⎝ ⎠ ⎨ ⎪⎛ ⎪⎜ R13 − c⎛⎜ π + β ⎞⎟.R cψ 13.sϕ13 − c⎛⎜ π + β ⎞⎟.R cϕ13.cθ 13.sψ 13 + s⎛⎜ π + β ⎞⎟.R sϕ13.sψ 13 − ⎞⎟ P P P ⎪⎜ ⎟ ⎝3 2⎠ ⎝3 2⎠ ⎝3 2⎠ ⎪⎜ ⎟ + π β ⎛ ⎞ ⎪⎜ s 13 13 13 13 13 ⎟ ⎟ ⎪⎜ ⎜⎝ + ⎟⎠.RP cϕ cθ cψ − hP cϕ sθ − R2 ⎠ ⎝ ⎪ ⎪ ⎛ 3⎞ ⎪⎛ 13 ⎛ π β ⎞ 13 13 ⎛ π β ⎞ 13 13 13 6⎞ ⎜ l2 ⎟ ⎪⎜⎜ R3 − c⎜ + ⎟.RP sθ sψ − s⎜ + ⎟.RP cψ sθ + hP cθ − R3 ⎟⎟ − ⎜ ⎟ = 3 ⎝ ⎠ ⎝ ⎠ ⎠ ⎪⎝ ⎝ ⎠ ⎪ ⎪ ⎛ 13 ⎛ π β ⎞ ⎞ ⎛π β ⎞ ⎛π β ⎞ ⎜ R1 − c⎜ + ⎟.RP cϕ13.cψ 13 + c⎜ + ⎟.RP cθ 13.sϕ13.sψ 13 + s⎜ + ⎟.RP cϕ13.sψ 13 + ⎟ ⎪ ⎟ ⎝3 2⎠ ⎝3 2⎠ ⎝3 2⎠ ⎪ 30 ⎜ ⎟ + ⎪C = ⎜ π β π α ⎛ ⎞ ⎛ ⎞ 13 13 13 13 13 ⎜ s⎜ + ⎟.R cθ cψ sϕ + + h sϕ sθ − c⎜ + ⎟.R ⎟ ⎪ P B ⎜ P ⎟ ⎪ ⎝3 2⎠ ⎠ ⎝ ⎝ ⎠ ⎪ ⎪⎛ ⎛π β ⎞ 13 13 ⎛ π β ⎞ 13 13 13 ⎛ π β ⎞ 13 13 ⎞ ⎪⎜ R13 − c⎜ + ⎟.RP cψ sϕ − c⎜ + ⎟.RP cϕ cθ sψ + s⎜ + ⎟.RP sϕ sψ − ⎟ ⎪⎜ ⎟ ⎝ ⎝ ⎠ ⎝ ⎠ ⎠ ⎪⎜ ⎟ + π β π α ⎛ ⎞ ⎛ ⎞ 13 13 13 13 13 ⎪⎜ s⎜ + ⎟.R cϕ cθ cψ − h cϕ sθ + s⎜ + ⎟.R ⎟ P B ⎟ ⎪⎜⎝ ⎝ ⎠ P ⎝3 2⎠ ⎠ ⎪ ⎪ ⎛ 3 ⎞ ⎞ ⎪⎛⎜ 13 ⎛ π β ⎞ 13 13 ⎛ π β ⎞ 13 13 13 ⎜ l1 + l2 ⎟ ⎪⎜ R3 − c⎜ + ⎟.RP sθ sψ − s⎜ + ⎟.RP cψ sθ + hP cθ − hB ⎟⎟ − ⎜ + d3 ⎟ = ⎝ ⎠ ⎝ ⎠ ⎠ ⎪⎩⎝ ⎝ ⎠ ( ( ) ( ) ( ) 85 ) Xây dựng mơ hình mơ Robot song song sử dụng ngơn ngữ MATLAB Phương trình (3.43) ⎧ l4 ⎪C 31 = R − sϕ 7,8.sθ 7,8 + c⎛⎜ α ⎞⎟.R = B ⎪ ⎝2⎠ ⎪ ⎪ l4 ⎛α ⎞ ⎪C 32 = R + cϕ 7,8.sθ 7,8 + s⎜ ⎟.RB = 2 ⎪ ⎝2⎠ ⎪ l ⎪ 33 cθ 7,8 − h = ⎪C = R3 − B ⎪ ⎪C 34 = R + d sϕ 7,8.sθ 7,8 − R8 = ⎪ 1 ⎪ 35 , , − R8 = ⎪C = R2 − d cϕ sθ ⎪ ⎪C 36 = R + d cθ 7,8 − R8 = 3 ⎪ ⎪ 2 ⎪ ⎛⎜ l ⎞⎟ ⎛ ⎞ ⎛ ⎞ α α ⎛ ⎞ ⎛ ⎞ 37 7 ⎛ ⎞ ⎪C = ⎜ R + c⎜ ⎟.R ⎟ + ⎜ R + s⎜ ⎟.R ⎟ + ⎜ R − h ⎟ − ⎜ ⎟ =0 B⎟ B⎟ B⎠ ⎜ ⎜ ⎪ ⎝ ⎜ ⎟ ⎝2⎠ ⎝2⎠ ⎠ ⎝ ⎠ ⎝ ⎝ ⎠ ⎪ ⎪ 2 ⎪C 38 = ⎛ R8 − R ⎞ + ⎛ R8 − R ⎞ + ⎛ R8 − R ⎞ − d = ⎜ ⎜ ⎜ ⎪ ⎟⎠ ⎟⎠ ⎟⎠ ⎝ ⎝ ⎝ ⎪ ⎪ ⎞ ⎛ 13 ⎛π β ⎞ ⎛π β ⎞ ⎛π β ⎞ ⎜ R − c⎜ − ⎟.RP cϕ13.cψ 13 + c⎜ − ⎟.RP cθ 13.sϕ13.sψ 13 + s⎜ − ⎟.RP cϕ13.sψ 13 ⎟ ⎪ ⎟ ⎪ 39 ⎜ ⎝3 2⎠ ⎝3 2⎠ ⎝3 2⎠ ⎟ + ⎪C = ⎜ π β ⎛ ⎞ ⎟ ⎜ + s⎜ − ⎟.R cθ 13.cψ 13.sϕ13 + h sϕ13.sθ 13 − R8 ⎪⎪ P P ⎟ ⎜ ⎨ ⎠ ⎝ ⎝3 2⎠ ⎪ ⎪⎛ ⎞ π β π β π β ⎪⎜ R13 − c⎛⎜ − ⎞⎟.R cψ 13.sϕ13 − c⎛⎜ − ⎞⎟.R cϕ13.cθ 13.sψ 13 + s⎛⎜ − ⎞⎟.R sϕ13.sψ 13 ⎟ P P P ⎪⎜ ⎟ ⎝3 2⎠ ⎝3 2⎠ ⎝3 2⎠ ⎪⎜ ⎟ + π β ⎞ ⎛ 13 13 13 13 13 ⎪⎜ − s⎜ − ⎟.R cϕ cθ cψ − h cϕ sθ − R ⎟ P ⎟ ⎪⎜⎝ ⎝ ⎠ P ⎠ ⎪ ⎪ ⎛l ⎞ ⎪⎛ 13 ⎛π β ⎞ ⎜ ⎟ 13 13 ⎛ π β ⎞ 13 13 13 8⎞ ⎪⎜⎜ R3 − c⎜ − ⎟.RP sθ sψ − s⎜ − ⎟.RP cψ sθ + hP cθ − R3 ⎟⎟ − ⎜ ⎟ = ⎜ ⎟ ⎝3 2⎠ ⎝3 2⎠ ⎪⎝ ⎠ ⎝ ⎠ ⎪ ⎪ ⎞ ⎛ 13 ⎛π β ⎞ ⎛π β ⎞ ⎛π β ⎞ ⎪ ⎜ R − c⎜ − ⎟.RP cϕ13.cψ 13 + c⎜ − ⎟.RP cθ 13.sϕ13.sψ 13 + s⎜ − ⎟.RP cϕ13.sψ 13 ⎟ ⎪ ⎟ ⎜ ⎝3 2⎠ ⎝3 2⎠ ⎝3 2⎠ ⎪C 40 = ⎜ ⎟ + ⎪ ⎟ ⎜ + s⎛⎜ π − β ⎞⎟.R cθ 13.cψ 13.sϕ13 + h sϕ13.sθ 13 + c⎛⎜ α ⎞⎟.R ⎪ P P B ⎟ ⎜ 2 ⎠ ⎝ ⎠ ⎝ ⎠ ⎝ ⎪ ⎪ ⎪⎛ 13 ⎛π β ⎞ 13 13 ⎛ π β ⎞ 13 13 13 ⎛ π β ⎞ 13 13 ⎞ ⎪⎜ R2 − c⎜ − ⎟.RP cψ sϕ − c⎜ − ⎟.RP cϕ cθ sψ + s⎜ − ⎟.RP sϕ sψ ⎟ ⎟ ⎜ ⎝ ⎠ ⎝ ⎠ ⎝ ⎠ ⎪ ⎟ + ⎪⎜ ⎛ π β ⎞ 13 13 13 13 13 ⎛ α ⎞ ⎟ ⎪⎜⎜ − s⎜ − ⎟.RP cϕ cθ cψ − hP cϕ sθ + s⎜ ⎟.RB ⎟ ⎝2⎠ ⎪⎝ ⎝ ⎠ ⎠ ⎪ 2 ⎛ 4 ⎪ ⎞ ⎪⎛⎜ R13 − c⎛⎜ π − β ⎞⎟.R sθ 13.sψ 13 − s⎛⎜ π − β ⎞⎟.R cψ 13.sθ 13 + h cθ 13 − h ⎞⎟ − ⎜ l1 + l2 + d ⎟ = ⎜ P P P B⎟ ⎟⎟ ⎪⎜ ⎜ ⎝3 2⎠ ⎝3 2⎠ ⎠ ⎪⎩⎝ ⎠ ⎝ ( ) 86 Xây dựng mơ hình mơ Robot song song sử dụng ngơn ngữ MATLAB Phương trình (3.44) ⎧ l5 ⎪C 41 = R9 − sϕ 9,10 sθ 9,10 + c⎛⎜ α ⎞⎟.R = B ⎪ ⎝2⎠ ⎪ ⎪ l5 ⎪C 42 = R9 + cϕ 9,10 sθ 9,10 − s⎛⎜ α ⎞⎟.R = B 2 ⎪ ⎝2⎠ ⎪ ⎪ l5 ⎪C 43 = R9 − cθ 9,10 − h = B ⎪ ⎪ 44 , 10 9 sθ ,10 − R10 = = R + d5.sϕ ⎪C 1 ⎪ ⎪C 45 = R9 − d cϕ 9,10 sθ 9,10 − R10 = 2 ⎪ ⎪ 46 + d cθ 9,10 − R10 = C R = ⎪ 3 ⎪ ⎪ 2 ⎛⎜ l ⎞⎟ ⎪ 47 ⎛ ⎞ ⎛ ⎞ ⎛α ⎞ ⎛α ⎞ ⎞ ⎛ = ⎜⎜ R + c⎜ ⎟.RB ⎟⎟ + ⎜⎜ R − s⎜ ⎟.RB ⎟⎟ + ⎜ R − hB ⎟ − ⎜ ⎟ = ⎪C ⎠ ⎝ ⎝2⎠ ⎝2⎠ ⎜ 2⎟ ⎝ ⎠ ⎝ ⎠ ⎪ ⎝ ⎠ ⎪ 2 ⎪ 48 10 10 10 ⎪C = ⎛⎜ R − R ⎞⎟ + ⎛⎜ R − R ⎞⎟ + ⎛⎜ R − R ⎞⎟ − d5 = 1⎠ 2⎠ 3⎠ ⎝ ⎝ ⎝ ⎪ ⎪ ⎞ ⎛ 13 ⎪ ⎛π β ⎞ ⎛π β ⎞ ⎛π β ⎞ ⎜ R − c⎜ − ⎟.RP cϕ13.cψ 13 + c⎜ − ⎟.RP cθ 13.sϕ13.sψ 13 − s⎜ − ⎟.RP cϕ13.sψ 13 ⎟ ⎪ ⎟ ⎝3 2⎠ ⎝3 2⎠ ⎝3 2⎠ ⎪C 49 = ⎜ ⎟ + ⎜ ⎪ π β ⎛ ⎞ ⎟ ⎜ − s⎜ − ⎟.R cθ 13.cψ 13.sϕ13 + h sϕ13.sθ 13 − R10 ⎪ P P ⎟ ⎜ ⎨ ⎠ ⎝ ⎝3 2⎠ ⎪ ⎪⎛ ⎞ ⎪⎜ R13 − c⎛⎜ π − β ⎞⎟.R cψ 13.sϕ13 − c⎛⎜ π − β ⎞⎟.R cϕ13.cθ 13.sψ 13 − s⎛⎜ π − β ⎞⎟.R sϕ13.sψ 13 ⎟ P P P ⎪⎜ ⎟ ⎝3 2⎠ ⎝3 2⎠ ⎝3 2⎠ ⎪⎜ ⎟ + π β ⎞ ⎛ ⎪⎜ + s⎜ − ⎟.R cϕ13.cθ 13.cψ 13 − h cϕ13.sθ 13 − R10 ⎟ P ⎟ ⎪⎜ ⎝ ⎠ P ⎝ ⎠ ⎪ ⎪ 2 ⎛ l5 ⎞ ⎪⎛ ⎞ ⎜ ⎟ ⎛π β ⎞ ⎛π β ⎞ ⎪⎜ R13 − c⎜ − ⎟.RP sθ 13.sψ 13 + s⎜ − ⎟.RP cψ 13.sθ 13 + hP cθ 13 − R10 ⎟ − ⎜ ⎟ = ⎜ ⎟ ⎠ ⎪⎝ ⎝3 2⎠ ⎝3 2⎠ ⎜ 2⎟ ⎝ ⎠ ⎪ ⎪ ⎛ 13 ⎞ ⎪ ⎛π β ⎞ ⎛π β ⎞ ⎛π β ⎞ ⎜ R − c⎜ − ⎟.RP cϕ13.cψ 13 + c⎜ − ⎟.RP cθ 13.sϕ13.sψ 13 − s⎜ − ⎟.RP cϕ13.sψ 13 ⎟ ⎪ ⎟ ⎝3 2⎠ ⎝3 2⎠ ⎝3 2⎠ ⎪C 50 = ⎜ ⎜ ⎟ + ⎪ π β α ⎜ − s⎛⎜ − ⎞⎟.R cθ 13.cψ 13.sϕ13 + h sϕ13.sθ 13 + c⎛⎜ ⎞⎟.R ⎟ ⎪ P B P ⎜ ⎟ ⎝2⎠ ⎪ ⎝ ⎝3 2⎠ ⎠ ⎪ ⎪⎛ 13 ⎞ ⎛π β ⎞ ⎛π β ⎞ ⎛π β ⎞ ⎪⎜ R − c⎜ − ⎟.RP cψ 13.sϕ13 − c⎜ − ⎟.RP cϕ13.cθ 13.sψ 13 − s⎜ − ⎟.RP sϕ13.sψ 13 ⎟ ⎟ ⎝3 2⎠ ⎝3 2⎠ ⎝3 2⎠ ⎪⎜ ⎟ + ⎪⎜ π β α ⎛ ⎞ ⎛ ⎞ ⎟ ⎪⎜ + s⎜ − ⎟.R cϕ13.cθ 13.cψ 13 − h cϕ13.sθ 13 − s⎜ ⎟.R P B ⎟ ⎪⎜⎝ ⎝ ⎠ P ⎝2⎠ ⎠ ⎪ ⎪ ⎛ l5 + l5 ⎞ ⎪⎛ 13 ⎞ ⎟ ⎜1 ⎛π β ⎞ ⎛π β ⎞ 13 13 13 13 13 ⎜ ⎟ + d5 ⎟ = ⎪⎜ R3 − c⎜ − ⎟.RP sθ sψ + s⎜ − ⎟.RP cψ sθ + hP cθ − hB ⎟ − ⎜ ⎝3 2⎠ ⎝3 2⎠ ⎟ ⎜ ⎪⎝ ⎠ ⎠ ⎝ ⎩ ( ) 87 Xây dựng mơ hình mơ Robot song song sử dụng ngơn ngữ MATLAB Phương trình (3.45) ⎧ l6 ⎪C 51 = R11 − sϕ11,12 sθ 11,12 − c⎛⎜ π + α ⎞⎟.R = B ⎪ ⎝3 2⎠ ⎪ ⎪ l ⎪C 52 = R11 + cϕ11,12 sθ 11,12 − s⎛⎜ π + α ⎞⎟.R = B ⎪ ⎝3 2⎠ ⎪ ⎪ l6 ⎪C 53 = R11 − cθ 11,12 − h = B ⎪ ⎪ 54 11 11 , 12 11 sθ ,12 − R112 = ⎪C = R1 + d sϕ ⎪ ⎪C 55 = R11 − d cϕ11,12 sθ 11,12 − R12 = ⎪ ⎪ 56 11 , 12 11 12 − R3 = ⎪C = R3 + d cθ ⎪ ⎪ 2 ⎛⎜ l ⎞⎟ ⎛ ⎞ ⎛ ⎞ π α π α ⎛ ⎞ ⎛ ⎞ ⎪C 57 = ⎜ R11 − c⎜ + ⎟.R ⎟ + ⎜ R11 − s⎜ + ⎟.R ⎟ + ⎛ R11 − h ⎞ − ⎜ ⎜ ⎟ =0 B⎟ B⎟ B ⎟⎠ ⎜ ⎜ ⎪ ⎝ ⎝3 2⎠ ⎝3 2⎠ ⎜ ⎟ ⎝ ⎠ ⎝ ⎠ ⎪ ⎝ ⎠ ⎪ 2 ⎪ 58 ⎛ 12 11 ⎞ 11 ⎞ 11 ⎞ ⎛ 12 ⎛ 12 ⎪C = ⎜ R1 − R1 ⎟ + ⎜ R2 − R2 ⎟ + ⎜ R3 − R3 ⎟ − d = ⎠ ⎝ ⎠ ⎝ ⎠ ⎝ ⎪ ⎪ ⎛ 13 ⎞ ⎛π β ⎞ ⎛π β ⎞ ⎛π β ⎞ ⎪ ⎜ R − c⎜ + ⎟.RP cϕ13.cψ 13 + c⎜ + ⎟.RP cθ 13.sϕ13.sψ 13 − s⎜ + ⎟.RP cϕ13.sψ 13 ⎟ ⎪ ⎜ ⎟ ⎝3 2⎠ ⎝3 2⎠ ⎝3 2⎠ ⎪C 59 = ⎜ ⎟ + ⎪ ⎜ − s⎛⎜ π + β ⎞⎟.R cθ 13.cψ 13.sϕ13 + h sϕ13.sθ 13 − R12 ⎟ ⎪ P P ⎜ ⎟ ⎨ ⎝ ⎝3 2⎠ ⎠ ⎪ ⎪⎛ ⎞ ⎛π β ⎞ ⎛π β ⎞ ⎛π β ⎞ ⎪⎜ R13 − c⎜ + ⎟.RP cψ 13.sϕ13 − c⎜ + ⎟.RP cϕ13.cθ 13.sψ 13 − s⎜ + ⎟.RP sϕ13.sψ 13 ⎟ 3 ⎟ ⎪⎜ ⎝ ⎠ ⎝ ⎠ ⎝ ⎠ ⎟ + ⎪⎜ π β ⎛ ⎞ 13 13 13 13 13 12 ⎟ ⎜ ⎪ + s⎜ + ⎟.R cϕ cθ cψ − h cϕ sθ − R P ⎟ ⎪⎜⎝ ⎝ ⎠ P ⎠ ⎪ ⎪ ⎛ 6⎞ ⎪⎛ 13 ⎛π β ⎞ ⎛π β ⎞ 12 ⎞⎟ − ⎜ l2 ⎟ = ⎪⎜⎜ R − c⎜ + ⎟.RP sθ 13.sψ 13 + s⎜ + ⎟.RP cψ 13.sθ 13 + hP cθ 13 − R3 ⎜ ⎟ ⎟ ⎝3 2⎠ ⎝3 2⎠ ⎪⎝ ⎠ ⎜ ⎟ ⎝ ⎠ ⎪ ⎪ ⎛ 13 ⎛π β ⎞ ⎪ 13.cψ 13 + c⎛⎜ π + β ⎞⎟.R cθ 13.sϕ13.sψ 13 − s⎛⎜ π + β ⎞⎟.R cϕ13.sψ 13 ⎞⎟ ⎜ ϕ R c R c − + ⎜ ⎟ ⎪ P P P ⎟ ⎝3 2⎠ ⎝3 2⎠ ⎝3 2⎠ ⎪C 60 = ⎜⎜ ⎟ + ⎪ π β π α ⎛ ⎞ ⎛ ⎞ 13 13 13 13 13 ⎜ ⎟ θ ψ ϕ ϕ θ s R c c s h s s c R − + + − + ⎜ ⎟ ⎜ ⎟ ⎪ P P B ⎜ ⎟ ⎝3 2⎠ ⎪ ⎝ ⎝3 2⎠ ⎠ ⎪ ⎪⎛ 13 ⎛π β ⎞ 13 13 ⎞ 13 13 13 ⎛ π β ⎞ 13 13 ⎛ π β ⎞ ⎪⎜ R2 − c⎜ + ⎟.RP cψ sϕ − c⎜ + ⎟.RP cϕ cθ sψ − s⎜ + ⎟.RP sϕ sψ ⎟ ⎟ ⎝3 2⎠ ⎝3 2⎠ ⎝3 2⎠ ⎪⎜ ⎟ + ⎪⎜ ⎛ π β ⎞ π α ⎞ ⎛ 13 13 13 13 13 ⎟ ⎪⎜ + s⎜ + ⎟.R cϕ cθ cψ − h cϕ sθ − s⎜ + ⎟.R P B ⎟ ⎪⎜⎝ ⎝ ⎠ P ⎝3 2⎠ ⎠ ⎪ ⎪ ⎞ ⎛ l6 + l6 ⎪⎛ 13 ⎞ ⎜1 ⎟ ⎛π β ⎞ ⎛π β ⎞ 13 13 13 13 13 + d6 ⎟ = ⎪⎜⎜ R3 − c⎜ + ⎟.RP sθ sψ + s⎜ + ⎟.RP cψ sθ + hP cθ − hB ⎟⎟ − ⎜ ⎝3 2⎠ ⎝3 2⎠ ⎜ ⎟ ⎠ ⎪⎝ ⎝ ⎠ ⎩ ( ) 88 Thank you for evaluating AnyBizSoft PDF Splitter A watermark is added at the end of each output PDF file To remove the watermark, you need to purchase the software from http://www.anypdftools.com/buy/buy-pdf-splitter.html Thank you for evaluating AnyBizSoft PDF Merger! To remove this page, please register your program! Go to Purchase Now>> AnyBizSoft PDF Merger  Merge multiple PDF files into one  Select page range of PDF to merge  Select specific page(s) to merge  Extract page(s) from different PDF files and merge into one ... cần đưa vào xem xét 24 Xây dựng mơ hình mơ Robot song song sử dụng ngôn ngữ MATLAB a) Robot song song gia cơng khí b) Robot song song y học c) Robot song song thiên văn d) Robot song song hàng... mơ hình Robot trình bày phần này, tác giả tiến hành xây dựng mơ hình Robot phần mềm MATLAB Hình 3.5 46 Xây dựng mơ hình mơ Robot song song sử dụng ngơn ngữ MATLAB Hình 3.5 Mơ hình Robot song song... loại Robot tay máy sử dụng cơng nghiệp, Hình 2.1 b) Robot có cấu trúc song song ứng dụng việc mô Xây dựng mơ hình mơ Robot song song sử dụng ngôn ngữ MATLAB thiết bị tập lái, Hình 2.1 c) Robot