Thế kỉ XXI đánh dấu sự phát triển vượt bậc của khoa học công nghệ hiện đại, với phương hướng phát triển trên toàn thế giới là xây dựng nền công nghiệp hiện đại 4.0 với sự tham gia chủ yếu vào sản xuất là các thiết bị máy móc tự động. Để thực hiện được công nghiệp 4.0 thì sự phát triển của Robot công nghiệp giữ một vai trò rất quan trọng. Robot là một phát minh vĩ đại của con người, hiện nay cụm từ “Robot” không còn xa lạ gì nữa khi trong hầu hết các lĩnh vực của cuộc sống đều có sự tham gia của robot. Đặc biệt là trong ngành công nghiệp thì sự có mặt của robot đã làm thay đổi hoàn toàn về tư duy sản xuất cũng như hướng phát triển kinh tế. Robot được ứng dụng trong sản xuất công nghiệp để giảm thiểu sức lao động của con người vào sản xuất, trong những công việc nặng nhọc, ô nhiễm và nguy hiểm thì robot công nghiệp được lựa chọn để thay thế con người vì thế càng ngày robot công nghiệp càng được đầu tư phát triển hiện đại hóa và tối ưu để phục vụ tốt nhất cho con người. Thế nhưng để chế tạo và đưa vào sử dụng hoàn chỉnh một robot thì phải trải qua rất nhiều công đoạn quan trọng từ thiết kế tính toán đến chế tạo điều khiển. Nhằm đáp ứng được xu thế phát triển và tận dụng những thành tựu khoa học của thế giới vào chế tạo robot. Em đã lựa chọn đề tài nghiên cứu “Tính toán động học, động lực học và mô phỏng chuyển động của Robot General electric P60 (Robot Ge P60)” làm đề tài tốt nghiệp.
ĐH Giao Thông Vận Tải Cơ Điện Tử - K55 LỜI CẢM ƠN Em xin gửi lời cảm ơn chân thành đến tồn thể thầy, mơn Kĩ Thuật Máy tận tình giảng dạy chúng em thời gian chúng em ngồi ghế nhà trường; kiến thức mà thầy, cô truyền đạt cho chúng em hành trang quý báu giúp chúng em bước đời, bắt đầu sống nhiều thử thách Trong trình làm đề tài luận văn này, em gặp nhiều khó khăn, lần đầu em tiếp xúc với giải thuật ngơn ngữ lập trình Nhưng hướng dẫn tận tình hết lòng giảng viên hướng dẫn cô TS Đinh Thị Thanh Huyền bạn sinh viên lớp Cơ Điên Tử - K55, dạy thầy cô khoa, em hoàn thành xong luận văn tốt nghiệp Lời cuối cùng, chúng xin dành để cảm ơn Cha Mẹ, Đấng Sinh Thành dưỡng dục quan tâm, ủng hộ để chúng có ngày hơm Do hạn chế thời gian, kiến thức thiếu kinh nghiệm nên luận văn tránh khỏi sai sót, em mong quan tâm, giúp đỡ đóng góp thầy Chân thành cảm ơn.! ĐH Giao Thông Vận Tải Cơ Điện Tử - K55 Mục Lục LỜI CẢM ƠN Mục Lục Danh Sách Hình Vẽ Mở Đầu 1) Đặt vấn đề 2) Mục đích đề tài 3) Ý nghĩa đề tài 4) Nội dung đề tài CHƯƠNG 1: Tổng Quan Về Robot .3 1.1) Sơ lược q trình phát triển robot cơng nghiệp .3 1.2) Ứng dụng robot sản xuất công nghiệp sống 1.2.1) Ứng dụng công nghệ hàn .4 1.2.2) Ứng dụng lĩnh vực khác 1.3) Phân loại robot .7 1.3.1) Phân loại theo kết cấu 1.3.2) Phân loại theo điều khiển .10 1.3.3) Phân loại theo ứng dụng 11 1.4) Cấu trúc robot công nghiệp 11 1.4.1) Kết cấu chung .12 1.4.2) Bậc tự tọa độ suy rộng 12 1.5) Robot General electric P60 15 Chương 2: Phân Tích Động Học Robot Ge P60 16 2.1) Phân tích cấu trúc thiết lập hệ tọa độ cho robot 16 2.1.1) Cấu trúc robot Ge P60 16 2.1.2) Đặt hệ tọa độ cho từng khâu robot .16 2.2) Phân tích động học robot Ge P60 giải tốn động học cho robot 17 2.2.1) Bảng thơng số Denavit-Hartenberg(DH) 17 2.2.2) Các ma trận biến đổi 18 2.2.3) Bài toán động học thuận 19 2.3) Mô chuyển động Robot 21 2.3.1) Giới thiệu tổng quan phần mềm SolidWorks Matlab 21 2.3.2) Mô chuyển động Robot 21 ĐH Giao Thông Vận Tải Cơ Điện Tử - K55 2.4) Bài toán động học ngược 25 Chương 3: Động Lực Học Robot 31 3.1) Giới thiệu động lực học robot phương pháp tính tốn 31 3.2) Bài toán động lực học robot Ge P60 31 3.2.1) Tính tốn thông số cho từng khâu Robot 31 3.2.2) Lập hệ phương trình vi phân cho Robot 33 Phụ Lục 49 Kết Luận Và Hướng Phát Triển 63 Tài Liệu Tham Khảo .64 ĐH Giao Thông Vận Tải Cơ Điện Tử - K55 Danh Sách Hình Hình 1.1: Robot hàn cơng nghiệp sản xuất oto Hình 1.2: Robot hàn hồ quang .6 Hình 1.3: Ứng dụng robot phẫu thuật Hình 1.4: Úng dụng robot thám hiểm không gian Hình 1.5: Robot kiểu tọa độ decac .8 Hình 1.6: Robot kiểu tọa độ trụ Hình 1.7: Robot kiểu tọa độ cầu Hình 1.8: Robot SCARA .9 Hình 1.9: Tay máy kiểu tay người 10 Hình 1.10: Một dạng Robot điều khiển servo 10 Hình 1.11: Robot phun sơn 11 Hình 1.12: Robot tự hành NASA 11 Hình 1.13: Hình dạng điển hình phận robot cơng nghiệp 13 Hình 1.14: Các tọa độ suy rộng Robot 13 Hình 1.15: Vùng làm việc cơng tác 14 Hình 1.16: Robot RP 15 Hình 1.17: Ứng dụng robot Ge P60 công nghệ hàn 16 Y Hình 2.1: Robot Ge P60 .17 Hình 2.2: Sơ đồ robot hệ tọa độ khâu .18 Hình 2.3: Mơ hình 3D Robot .23 Hình 2.4: Sơ đồ Simulink 24 Hình 2.5: Sơ đồ mơ Simulink có đầu vào .25 Hình 6: Đồ thị chuyển vị biến khớp 25 Hình 2.7: Đồ thị vận tốc khớp .26 Hình 2.8: Đồ thị tọa độ điểm thao tác cuôi 26 Hình 2.9: Vị trí điểm Po5 vector hệ tọa độ 27 ĐH Giao Thông Vận Tải Cơ Điện Tử - K55 Mở Đầu 1) Đặt vấn đề Thế kỉ XXI đánh dấu phát triển vượt bậc khoa học công nghệ đại, với phương hướng phát triển toàn giới xây dựng công nghiệp đại 4.0 với tham gia chủ yếu vào sản xuất thiết bị máy móc tự động Để thực cơng nghiệp 4.0 phát triển Robot cơng nghiệp giữ vai trò quan trọng Robot phát minh vĩ đại người, cụm từ “Robot” khơng xa lạ hầu hết lĩnh vực sống có tham gia robot Đặc biệt ngành công nghiệp có mặt robot làm thay đổi hoàn toàn tư sản xuất hướng phát triển kinh tế Robot ứng dụng sản xuất công nghiệp để giảm thiểu sức lao động người vào sản xuất, công việc nặng nhọc, nhiễm nguy hiểm robot cơng nghiệp lựa chọn để thay người ngày robot công nghiệp đầu tư phát triển đại hóa tối ưu để phục vụ tốt cho người Thế để chế tạo đưa vào sử dụng hoàn chỉnh robot phải trải qua nhiều cơng đoạn quan trọng từ thiết kế tính tốn đến chế tạo điều khiển Nhằm đáp ứng xu phát triển tận dụng thành tựu khoa học giới vào chế tạo robot Em lựa chọn đề tài nghiên cứu “Tính tốn động học, động lực học mơ chuyển động Robot General electric P60 (Robot Ge P60)” làm đề tài tốt nghiệp 2) Mục đích đề tài - Giải toán động học thuận ngược - Mơ chuyển động cho mơ hình robot - Giải toán động lực học 3) Ý nghĩa đề tài Nội dung nghiên cứu đề tài phù hợp với xu hướng phát triển xã hội, có ý nghĩa thực tiễn người Trong nhà máy cơng nghiệp vai trò robot cơng nghiệp to lớn, robot giúp tăng suất lao động hiệu kinh tế, tăng chất lượng sản phẩm, giảm giá thành rút bớt nhân công làm việc môi trường khắc nghiệt Việc nghiên cứu phát triển robot công nghiệp việc mà xã hội ưu tiên hàng đầu 4) Nội dung đề tài Đề tài trình bày với nội dung cụ thể sau: Chương 1: “ Tổng quan robot ” Nội dung chủ yếu chương tìm hiểu đời phát triển robot, định nghĩa, ứng dụng robot thực tế Giới thiệu robot General electric P60 (Ge P60) GVHD: TS Đinh Thị Thanh Huyền SVTH: Đặng Hữu Trung ĐH Giao Thông Vận Tải Cơ Điện Tử - K55 Chương 2: “ Phân tích động học cho robot ” Nội dung chủ yếu chương giải toán động học thuận động học ngược robot Ge P60 Ứng dụng phần mềm Matlab SolidWorks để mô chuyển động robot theo động học thuận Chương 3: “ Động lực học robot ” Nội dung chủ yếu chương giải toán động lực học cho robot Ge P60 GVHD: TS Đinh Thị Thanh Huyền SVTH: Đặng Hữu Trung ĐH Giao Thông Vận Tải Cơ Điện Tử - K55 CHƯƠNG 1: Tổng Quan Về Robot 1.1) Sơ lược trình phát triển robot công nghiệp Thuật ngữ “Robot” sử dụng lần Karel Capek kịch ông Rossum’s Universal Robots xuất vào năm 1921 Có lẽ gợi ý ban đầu cho nhà sáng chế kỹ thuật việc sáng chế cấu, máy móc bắt chước hoạt động người Về mặt kỹ thuật, robot cơng nghiệp ngày có nguồn gốc từ hai lĩnh vực kỹ thuật đời sớm cấu điều khiển từ xa (Teleoperator) máy công cụ điều khiển số (NC- Numerically Controlled Machine Tool) Các cấu điều khiển từ xa (hay thiết bị kiểu chủ-tớ) phát triển mạnh chiến tranh giới lần thứ hai nhằm nghiên cứu vật liệu phóng xạ viện nghiên cứu hạt nhân Đó cấu sinh học bao gồm khâu khớp dây chằng gắn liền với hệ điều hành cánh tay người, thao tác thơng qua cấu khuyếch đại khí Cụ thể, gồm có kẹp bên (tớ) hai tay cầm bên (chủ) Cả hai, tay cầm kẹp, nối với cấu sáu bậc tự để tạo vị trí hướng tùy ý cho tay cầm kẹp Cơ cấu dùng để điều khiển kẹp theo chuyển động tay cầm Chính vậy, người thao tác tách biệt khỏi khu vực phóng xạ tường có vài cửa quan sát, nhìn thấy thực thao tác bên cách bình thường Vào khoảng năm 1949 máy công cụ điều khiển số đời nhằm đáp ứng nhu cầu gia công chi tiết ngành chế tạo máy bay Những robot thực chất nối kết khâu khí cấu điều khiển từ xa với khả lập trình máy cơng cụ điều khiển số Đầu thập kỷ 1960, công ty Mỹ AMF (American Machine and Foundry Company) cho đời robot công nghiệp đặt tên Versatran, Harry Johnson Veljko Milenkovic thiết kế Năm 1967 trường Đại học tổng hợp Stanford (Mỹ) chế tạo mẫu robot hoạt động theo mô hình “mắt-tay”, có khả nhận biết định hướng bàn kẹp theo vị trí vật kẹp nhờ cảm biến Năm 1974 công ty Mỹ Cincinnati đưa loại robot điều khiển băng máy vi tính, gọi robot T3 (The Tomorrow Tool: công cụ tương lai) Robot nâng vật có khối lượng lên đến 45kg Những năm 80, năm 90, áp dụng rộng rãi tiến 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ó bước tiến vượt bực Ngày chuyên ngành khoa học robot “robotics” trở thành lĩnh vực rộng khoa học, bao gồm vấn đề cấu trúc cấu động học, động lực học, lập trình quỹ đạo, cảm biến tín hiệu, điều khiển chuyển động… GVHD: TS Đinh Thị Thanh Huyền SVTH: Đặng Hữu Trung ĐH Giao Thông Vận Tải Cơ Điện Tử - K55 Định nghĩa robot công nghiệp Viện nghiên cứu robot Mỹ đề xuất sử dụng rộng rãi: “RBCN tay máy vạn năng, hoạt động theo chương trình lập trình lại để hồn thành nâng cao hiệu hồn thành nhiệm vụ khác cơng nghiệp, vận chuyển nguyên vật liệu, chi tiết, dụng cụ thiết bị chuyên dùng khác.” Ngoài ý trên, định nghĩa 0CT 25686-85 bổ sung cho RBCN chức điều khiển trình sản xuất: “ RBCN tay máy tự động đặt cố định hay di động bao gồm thiết bị dạng thừa hành tay máy có số bậc tự hoạt động thiết bị điều khiển theo chương trình, tái lập trình để hồn thành chức vận động điều khiển trình sản xuất” Chức vận động bao gồm hoạt động “cơ bắp” vận chuyển, định hướng, xếp đặt, gá kẹp, lắp ráp…đối tượng Chức điều khiển ám vai trò robot phương tiện điều hành sản xuất, cung cấp dịch vụ vật liệu, phân loại phân phối sản phẩm, trì sản xuất chí điêù khiển thiết bị liên quan Với đặc điểm lập trình lại, RBCN thiết bị tự động hóa khả trình ngày trở thành phận thiếu tế bào hệ thống sản xuất linh hoạt 1.2) Ứng dụng robot sản xuất công nghiệp sống 1.2.1) Ứng dụng công nghệ hàn Có thể nói, robot tổ hợp khả hoạt động linh hoạt cấu điều khiển từ xa, với mức độ “tri thức” ngày phong phú 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ệ chuyên gia… Tính hoạt động robot ngày nâng cao, khả nhận biết xử lý, nhiều loại robot có khả đặc biệt Số lượng robot ngày gia tăng, giá thành ngày giảm Trong công nghiệp gia công vật liệu, robot thực nhiệm vụ máy gia cơng Do tay robot gắn dụng cụ thay cho cấu kep Ứng dụng robot công nghiệp gia công vật liệu bao gồm công nghệ sau: Hàn điểm; hàn hồ quang liên tục… Hàn điểm ứng dụng phổ biến robot công nghiệp, đặc biệt công nghiệp lắp ráp ôtô GVHD: TS Đinh Thị Thanh Huyền SVTH: Đặng Hữu Trung ĐH Giao Thông Vận Tải Cơ Điện Tử - K55 Hình 1.1: Robot hàn cơng nghiệp sản xuất oto Hàn điểm thực hai phương pháp: dùng máy hàn điểm dùng súng hàn điểm Máy hàn điểm gồm hai điện cực ép chặt hai chi tiết cho dòng điện có giá trị lớn chạy qua, kết hai chi tiết hàn dính điểm Dùng hàn điểm gồm hai điện cực khung mở đóng hai điện cực, cáp lớn dẫn dòng chạy qua Hệ thống súng hàn điểm có trọng lượng kích thước lớn gây khó khăn cho người điều khiển dây chuyền sản xuất với tốc độ lớn Robot sử dụng hiệu công nghệ hàn điểm Ở dây chuyền lắp ráp ôtô, hàng chục robot hàn điểm làm việc với theo chương trình lập sẵn Robot hàn điểm phải có kích thước lớn, có khả mang tải trọng để điều khiển súng hàn có khối lượng lớn cách xác Robot cần phải đưa súng hàn vào vị trí hướng vị trí người khó thực Do số bậc tự robot phải lớn nhớ máy tính phải có dung lượng lớn Lợi ích tự động hóa cơng nghệ hàn điểm sử dụng robot nâng cao chất lượng sản phẩm, thao tác an toàn điều khiển tốt trình hàn Hàn hồ quang liên tục sử dụng công nghệ hàn đường: ghép hai phận kim loại hàn ống,…Môi trường làm việc người công nhân hàn hồ quang nguy hiểm độc hại: nhiệt độ cao Tia cực tím sinh trình hàn gây nguy hiểm đến thị giác người …Việc ứng dụng robot nghệ hàn hồ quang cải thiện đáng kể điều kiện làm việc người, đồng thời nâng cao suất chất lượng sản phẩm GVHD: TS Đinh Thị Thanh Huyền SVTH: Đặng Hữu Trung ĐH Giao Thông Vận Tải Cơ Điện Tử - K55 Hình 1.2: Robot hàn hồ quang Tuy nhiên số vấn đề kỹ thuật nâng cao chất lượng hàn có thay đổi thành phần vật liệu hàn vấn đề kinh tế, nên robot sử dụng công nghệ hàn hồ quang dây chuyền sản xuất có sản lượng trung bình lớn Hệ thống robot hàn gồm hai phận: robot hàn với que hàn, hệ thống cấp dây hàn phận giữ chi tiết hàn có khả định vị định hướng chi tiết hàn tương đối so với robot Hàn tự động có lợi ích lớn có độ xác suất cao, giảm thiểu độc hại ảnh hưởng trực tiếp đến người Hàn robot nâng cao độ tin cậy mối hàn Một lập trình hợp lý robot tạo mối hàn y vật hàn có kích thước hình dạng Việc tự động hóa chuyển động mỏ hàn làm giảm nguy mắc lỗi thao tác, giảm phế phẩm khối lượng công việc phải làm Robot làm việc nhanh mà làm việc suốt ngày đêm, hiệu nhiều so với thiết bị hàn tay 1.2.2) Ứng dụng lĩnh vực khác Hiện robot ứng dụng rộng rãi nhiều lĩnh vực xã hội với nhiều cấu hình mục đích sử dụng khác Nổi bật lĩnh vực như: vận chuyển, y tế, thám hiểm, quốc phòng… GVHD: TS Đinh Thị Thanh Huyền SVTH: Đặng Hữu Trung ĐH Giao Thông Vận Tải Cơ Điện Tử - K55 T34= [-sin(q4) -cos(q4) L3; cos(q4) -sin(q4) 0; 0 0; 0 1]; T45= [-sin(q5) -cos(q5) 0; 0 -1 -(L4+L5); cos(q5) -sin(q5) 0; 0 1]; T56= [cos(q6) -sin(q6) 0; 0 0; -sin(q6) -cos(q6) 0; 0 1]; T67= [1 0 0; 0; 0 L6; 0 1]; T07= simplify(T01*T12*T23*T34*T45*T56*T67) % Dong Luc Hoc clc; clear all; syms q1 q2 q3 q4 q5 q6 qd1 qd2 qd3 qd4 qd5 qd6 qdd1 qdd2 qdd3 qdd4 qdd5 qdd6 L1 L2 L3 L4 L5 L6 m1 m2 m3 m4 m5 m6 g % Vector gia toc truong Vg= [0; 0; -g; 1]; % Vector toa khoi tam he quy chieu cua khau Rg1= [0; 0; -L1/2; 1]; Rg2= [L2/2; 0; 0; 1]; Rg3= [L3/2; 0; 0; 1]; Rg4= [0; -L4/3; 0; 1]; Rg5= [0; 0; -L5/2; 1]; Rg6= [0; 0; L6/2; 1]; % Ma tran gia quan tinh Ixx1=m1*L1^2/12; Iyy1=m1*L1^2/12; Izz1=0; Ixx2=0; Iyy2=m2*L2^2/12; Izz2=m2*L2^2/12; Ixx3=0; Iyy3=m3*L3^2/12; Izz3=m3*L3^2/12; Ixx4=m4*L4^2/12; Iyy4=0; Izz4=m4*L4^2/12; Ixx5=m5*L5^2/12; Iyy5=m5*L5^2/12; Izz5=0; Ixx6=m6*L6^2/12; Iyy6=m6*L6^2/12; Izz6=0; J1=[(-Ixx1+Iyy1+Izz1)/2 0 0; (Ixx1-Iyy1+Izz1)/2 0; 0 (Ixx1+Iyy1Izz1)/2+m1*L1^2/4 -m1*L1/2; 0 -m1*L1/2 m1]; J2=[(-Ixx2+Iyy2+Izz2)/2+m2*L2^2/4 0 m2*L2/2; (Ixx2-Iyy2+Izz2)/2 0; 0 (Ixx2+Iyy2-Izz2)/2 0; m2*L2/2 0 m2]; J3=[(-Ixx3+Iyy3+Izz3)/2+m3*L3^2/4 0 m3*L3/2; (Ixx3-Iyy3+Izz3)/2 0; 0 (Ixx3+Iyy3-Izz3)/2 0; m3*L3/2 0 m3]; 38 GVHD: TS Đinh Thị Thanh Huyền SVTH: Đặng Hữu Trung ĐH Giao Thông Vận Tải Cơ Điện Tử - K55 J4=[(-Ixx4+Iyy4+Izz4)/2 0 0; (Ixx4-Iyy4+Izz4)/2+m4*L4^2/4 -m4*L4/2; 0 (Ixx4+Iyy4-Izz4)/2 0; -m4*L4/2 m4]; J5=[(-Ixx5+Iyy5+Izz5)/2 0 0; (Ixx5-Iyy5+Izz5)/2 0; 0 (Ixx5+Iyy5Izz5)/2+m5*L5^2/4 -m5*L5/2; 0 -m5*L5/2 m5]; J6=[(-Ixx6+Iyy6+Izz6)/2 0 0; (Ixx6-Iyy6+Izz6)/2 0; 0 (Ixx6+Iyy6Izz6)/2+m6*L6^2/4 m6*L6/2; 0 m6*L6/2 m6]; % Ma tran bien doi thuan nhat T01=[cos(q1) -sin(q1) 0; sin(q1) cos(q1) 0; 0 L1; 0 1]; T12= [-sin(q2) -cos(q2) 0; 0 -1 0; cos(q2) -sin(q2) 0; 0 1]; T23= [sin(q3) cos(q3) L2; -cos(q3) sin(q3) 0; 0 0; 0 1]; T34= [-sin(q4) -cos(q4) L3; cos(q4) -sin(q4) 0; 0 0; 0 1]; T45= [-sin(q5) -cos(q5) 0; 0 -1 -(L4+L5); cos(q5) -sin(q5) 0; 0 1]; T56= [cos(q6) -sin(q6) 0; 0 0; -sin(q6) -cos(q6) 0; 0 1]; T02= T01*T12; T03= T01*T12*T23; T04= T01*T12*T23*T34; T05= T01*T12*T23*T34*T45; T06= T01*T12*T23*T34*T45*T56; % He phuong trinh vi phan chuyen dong F1= trace(diff(T01,q1)*J1*transpose(diff(T01,q1)))*qdd1+ trace(diff(T02,q1)*J2*transpose(diff(T02,q1)))*qdd1+ trace(diff(T02,q2)*J2*transpose(diff(T02,q1)))*qdd2+ trace(diff(T03,q1)*J3*transpose(diff(T03,q1)))*qdd1+ trace(diff(T03,q2)*J3*transpose(diff(T03,q1)))*qdd2+ trace(diff(T03,q3)*J3*transpose(diff(T03,q1)))*qdd3+ trace(diff(T04,q1)*J4*transpose(diff(T04,q1)))*qdd1+ trace(diff(T04,q2)*J4*transpose(diff(T04,q1)))*qdd2+ trace(diff(T04,q3)*J4*transpose(diff(T04,q1)))*qdd3+ trace(diff(T04,q4)*J4*transpose(diff(T04,q1)))*qdd4+ trace(diff(T05,q1)*J5*transpose(diff(T05,q1)))*qdd1+ trace(diff(T05,q2)*J5*transpose(diff(T05,q1)))*qdd2+ trace(diff(T05,q3)*J5*transpose(diff(T05,q1)))*qdd3+ 39 GVHD: TS Đinh Thị Thanh Huyền SVTH: Đặng Hữu Trung ĐH Giao Thông Vận Tải Cơ Điện Tử - K55 trace(diff(T05,q4)*J5*transpose(diff(T05,q1)))*qdd4+ trace(diff(T05,q5)*J5*transpose(diff(T05,q1)))*qdd5+ trace(diff(T06,q1)*J6*transpose(diff(T06,q1)))*qdd1+ trace(diff(T06,q2)*J6*transpose(diff(T06,q1)))*qdd2+ trace(diff(T06,q3)*J6*transpose(diff(T06,q1)))*qdd3+ trace(diff(T06,q4)*J6*transpose(diff(T06,q1)))*qdd4+ trace(diff(T06,q5)*J6*transpose(diff(T06,q1)))*qdd5+ trace(diff(T06,q6)*J6*transpose(diff(T06,q1)))*qdd6+ trace(diff(diff(T01,q1),q1)*J1*transpose(diff(T01,q1)))*qd1*qd1+ trace(diff(diff(T02,q1),q1)*J2*transpose(diff(T02,q1)))*qd1*qd1+ 2*trace(diff(diff(T02,q1),q2)*J2*transpose(diff(T02,q1)))*qd1*qd2+ trace(diff(diff(T02,q2),q2)*J2*transpose(diff(T02,q1)))*qd2*qd2+ trace(diff(diff(T03,q1),q1)*J3*transpose(diff(T03,q1)))*qd1*qd1+ 2*trace(diff(diff(T03,q1),q2)*J3*transpose(diff(T03,q1)))*qd1*qd2+ 2*trace(diff(diff(T03,q1),q3)*J3*transpose(diff(T03,q1)))*qd1*qd3+ trace(diff(diff(T03,q2),q2)*J3*transpose(diff(T03,q1)))*qd2*qd2+ 2*trace(diff(diff(T03,q2),q3)*J3*transpose(diff(T03,q1)))*qd2*qd3+ trace(diff(diff(T03,q3),q3)*J3*transpose(diff(T03,q1)))*qd3*qd3+ trace(diff(diff(T04,q1),q1)*J4*transpose(diff(T04,q1)))*qd1*qd1+ trace(diff(diff(T04,q2),q2)*J4*transpose(diff(T04,q1)))*qd2*qd2+ trace(diff(diff(T04,q3),q3)*J4*transpose(diff(T04,q1)))*qd3*qd3+ trace(diff(diff(T04,q4),q4)*J4*transpose(diff(T04,q1)))*qd4*qd4+ 2*trace(diff(diff(T04,q1),q2)*J4*transpose(diff(T04,q1)))*qd1*qd2+ 2*trace(diff(diff(T04,q1),q3)*J4*transpose(diff(T04,q1)))*qd1*qd3+ 2*trace(diff(diff(T04,q1),q4)*J4*transpose(diff(T04,q1)))*qd1*qd4+ 2*trace(diff(diff(T04,q2),q3)*J4*transpose(diff(T04,q1)))*qd2*qd3+ 2*trace(diff(diff(T04,q2),q4)*J4*transpose(diff(T04,q1)))*qd2*qd4+ 2*trace(diff(diff(T04,q3),q4)*J4*transpose(diff(T04,q1)))*qd3*qd4+ trace(diff(diff(T05,q1),q1)*J5*transpose(diff(T05,q1)))*qd1*qd1+ trace(diff(diff(T05,q2),q2)*J5*transpose(diff(T05,q1)))*qd2*qd2+ trace(diff(diff(T05,q3),q3)*J5*transpose(diff(T05,q1)))*qd3*qd3+ 40 GVHD: TS Đinh Thị Thanh Huyền SVTH: Đặng Hữu Trung ĐH Giao Thông Vận Tải Cơ Điện Tử - K55 trace(diff(diff(T05,q4),q4)*J5*transpose(diff(T05,q1)))*qd4*qd4+ trace(diff(diff(T05,q5),q5)*J5*transpose(diff(T05,q1)))*qd5*qd5+ 2*trace(diff(diff(T05,q1),q2)*J5*transpose(diff(T05,q1)))*qd1*qd2+ 2*trace(diff(diff(T05,q1),q3)*J5*transpose(diff(T05,q1)))*qd1*qd3+ 2*trace(diff(diff(T05,q1),q4)*J5*transpose(diff(T05,q1)))*qd1*qd4+ 2*trace(diff(diff(T05,q1),q5)*J5*transpose(diff(T05,q1)))*qd1*qd5+ 2*trace(diff(diff(T05,q2),q3)*J5*transpose(diff(T05,q1)))*qd2*qd3+ 2*trace(diff(diff(T05,q2),q4)*J5*transpose(diff(T05,q1)))*qd2*qd4+ 2*trace(diff(diff(T05,q2),q5)*J5*transpose(diff(T05,q1)))*qd2*qd5+ 2*trace(diff(diff(T05,q3),q4)*J5*transpose(diff(T05,q1)))*qd3*qd4+ 2*trace(diff(diff(T05,q3),q5)*J5*transpose(diff(T05,q1)))*qd3*qd5+ 2*trace(diff(diff(T05,q4),q5)*J5*transpose(diff(T05,q1)))*qd4*qd5+ trace(diff(diff(T06,q1),q1)*J6*transpose(diff(T06,q1)))*qd1*qd1+ trace(diff(diff(T06,q2),q2)*J6*transpose(diff(T06,q1)))*qd2*qd2+ trace(diff(diff(T06,q3),q3)*J6*transpose(diff(T06,q1)))*qd3*qd3+ trace(diff(diff(T06,q4),q4)*J6*transpose(diff(T06,q1)))*qd4*qd4+ trace(diff(diff(T06,q5),q5)*J6*transpose(diff(T06,q1)))*qd5*qd5+ trace(diff(diff(T06,q6),q6)*J6*transpose(diff(T06,q1)))*qd6*qd6+ 2*trace(diff(diff(T06,q1),q2)*J6*transpose(diff(T06,q1)))*qd1*qd2+ 2*trace(diff(diff(T06,q1),q3)*J6*transpose(diff(T06,q1)))*qd1*qd3+ 2*trace(diff(diff(T06,q1),q4)*J6*transpose(diff(T06,q1)))*qd1*qd4+ 2*trace(diff(diff(T06,q1),q5)*J6*transpose(diff(T06,q1)))*qd1*qd5+ 2*trace(diff(diff(T06,q1),q6)*J6*transpose(diff(T06,q1)))*qd1*qd6+ 2*trace(diff(diff(T06,q2),q3)*J6*transpose(diff(T06,q1)))*qd2*qd3+ 2*trace(diff(diff(T06,q2),q4)*J6*transpose(diff(T06,q1)))*qd2*qd4+ 2*trace(diff(diff(T06,q2),q5)*J6*transpose(diff(T06,q1)))*qd2*qd5+ 2*trace(diff(diff(T06,q2),q6)*J6*transpose(diff(T06,q1)))*qd2*qd6+ 2*trace(diff(diff(T06,q3),q4)*J6*transpose(diff(T06,q1)))*qd3*qd4+ 2*trace(diff(diff(T06,q3),q5)*J6*transpose(diff(T06,q1)))*qd3*qd5+ 2*trace(diff(diff(T06,q3),q6)*J6*transpose(diff(T06,q1)))*qd3*qd6+ 2*trace(diff(diff(T06,q4),q5)*J6*transpose(diff(T06,q1)))*qd4*qd5+ 41 GVHD: TS Đinh Thị Thanh Huyền SVTH: Đặng Hữu Trung ĐH Giao Thông Vận Tải Cơ Điện Tử - K55 2*trace(diff(diff(T06,q4),q6)*J6*transpose(diff(T06,q1)))*qd4*qd6+ 2*trace(diff(diff(T06,q5),q6)*J6*transpose(diff(T06,q1)))*qd5*qd6- m1*transpose(Vg)*diff(T01,q1)*Rg1-m2*transpose(Vg)*diff(T02,q1)*Rg2m3*transpose(Vg)*diff(T03,q1)*Rg3- m4*transpose(Vg)*diff(T04,q1)*Rg4-m5*transpose(Vg)*diff(T05,q1)*Rg5m6*transpose(Vg)*diff(T06,q1)*Rg6; F2= trace(diff(T02,q1)*J2*transpose(diff(T02,q2)))*qdd1+ trace(diff(T02,q2)*J2*transpose(diff(T02,q2)))*qdd2+ trace(diff(T03,q1)*J3*transpose(diff(T03,q2)))*qdd1+ trace(diff(T03,q2)*J3*transpose(diff(T03,q2)))*qdd2+ trace(diff(T03,q3)*J3*transpose(diff(T03,q2)))*qdd3+ trace(diff(T04,q1)*J4*transpose(diff(T04,q2)))*qdd1+ trace(diff(T04,q2)*J4*transpose(diff(T04,q2)))*qdd2+ trace(diff(T04,q3)*J4*transpose(diff(T04,q2)))*qdd3+ trace(diff(T04,q4)*J4*transpose(diff(T04,q2)))*qdd4+ trace(diff(T05,q1)*J5*transpose(diff(T05,q2)))*qdd1+ trace(diff(T05,q2)*J5*transpose(diff(T05,q2)))*qdd2+ trace(diff(T05,q3)*J5*transpose(diff(T05,q2)))*qdd3+ trace(diff(T05,q4)*J5*transpose(diff(T05,q2)))*qdd4+ trace(diff(T05,q5)*J5*transpose(diff(T05,q2)))*qdd5+ trace(diff(T06,q1)*J6*transpose(diff(T06,q2)))*qdd1+ trace(diff(T06,q2)*J6*transpose(diff(T06,q2)))*qdd2+ trace(diff(T06,q3)*J6*transpose(diff(T06,q2)))*qdd3+ trace(diff(T06,q4)*J6*transpose(diff(T06,q2)))*qdd4+ trace(diff(T06,q5)*J6*transpose(diff(T06,q2)))*qdd5+ trace(diff(T06,q6)*J6*transpose(diff(T06,q2)))*qdd6+ trace(diff(diff(T02,q1),q1)*J2*transpose(diff(T02,q2)))*qd1*qd1+ 2*trace(diff(diff(T02,q1),q2)*J2*transpose(diff(T02,q2)))*qd1*qd2+ trace(diff(diff(T02,q2),q2)*J2*transpose(diff(T02,q2)))*qd2*qd2+ trace(diff(diff(T03,q1),q1)*J3*transpose(diff(T03,q2)))*qd1*qd1+ 2*trace(diff(diff(T03,q1),q2)*J3*transpose(diff(T03,q2)))*qd1*qd2+ 2*trace(diff(diff(T03,q1),q3)*J3*transpose(diff(T03,q2)))*qd1*qd3+ 42 GVHD: TS Đinh Thị Thanh Huyền SVTH: Đặng Hữu Trung ĐH Giao Thông Vận Tải Cơ Điện Tử - K55 trace(diff(diff(T03,q2),q2)*J3*transpose(diff(T03,q2)))*qd2*qd2+ 2*trace(diff(diff(T03,q2),q3)*J3*transpose(diff(T03,q2)))*qd2*qd3+ trace(diff(diff(T03,q3),q3)*J3*transpose(diff(T03,q2)))*qd3*qd3+ trace(diff(diff(T04,q1),q1)*J4*transpose(diff(T04,q2)))*qd1*qd1+ trace(diff(diff(T04,q2),q2)*J4*transpose(diff(T04,q2)))*qd2*qd2+ trace(diff(diff(T04,q3),q3)*J4*transpose(diff(T04,q2)))*qd3*qd3+ trace(diff(diff(T04,q4),q4)*J4*transpose(diff(T04,q2)))*qd4*qd4+ 2*trace(diff(diff(T04,q1),q2)*J4*transpose(diff(T04,q2)))*qd1*qd2+ 2*trace(diff(diff(T04,q1),q3)*J4*transpose(diff(T04,q2)))*qd1*qd3+ 2*trace(diff(diff(T04,q1),q4)*J4*transpose(diff(T04,q2)))*qd1*qd4+ 2*trace(diff(diff(T04,q2),q3)*J4*transpose(diff(T04,q2)))*qd2*qd3+ 2*trace(diff(diff(T04,q2),q4)*J4*transpose(diff(T04,q2)))*qd2*qd4+ 2*trace(diff(diff(T04,q3),q4)*J4*transpose(diff(T04,q2)))*qd3*qd4+ trace(diff(diff(T05,q1),q1)*J5*transpose(diff(T05,q2)))*qd1*qd1+ trace(diff(diff(T05,q2),q2)*J5*transpose(diff(T05,q2)))*qd2*qd2+ trace(diff(diff(T05,q3),q3)*J5*transpose(diff(T05,q2)))*qd3*qd3+ trace(diff(diff(T05,q4),q4)*J5*transpose(diff(T05,q2)))*qd4*qd4+ trace(diff(diff(T05,q5),q5)*J5*transpose(diff(T05,q2)))*qd5*qd5+ 2*trace(diff(diff(T05,q1),q2)*J5*transpose(diff(T05,q2)))*qd1*qd2+ 2*trace(diff(diff(T05,q1),q3)*J5*transpose(diff(T05,q2)))*qd1*qd3+ 2*trace(diff(diff(T05,q1),q4)*J5*transpose(diff(T05,q2)))*qd1*qd4+ 2*trace(diff(diff(T05,q1),q5)*J5*transpose(diff(T05,q2)))*qd1*qd5+ 2*trace(diff(diff(T05,q2),q3)*J5*transpose(diff(T05,q2)))*qd2*qd3+ 2*trace(diff(diff(T05,q2),q4)*J5*transpose(diff(T05,q2)))*qd2*qd4+ 2*trace(diff(diff(T05,q2),q5)*J5*transpose(diff(T05,q2)))*qd2*qd5+ 2*trace(diff(diff(T05,q3),q4)*J5*transpose(diff(T05,q2)))*qd3*qd4+ 2*trace(diff(diff(T05,q3),q5)*J5*transpose(diff(T05,q2)))*qd3*qd5+ 2*trace(diff(diff(T05,q4),q5)*J5*transpose(diff(T05,q2)))*qd4*qd5+ trace(diff(diff(T06,q1),q1)*J6*transpose(diff(T06,q2)))*qd1*qd1+ trace(diff(diff(T06,q2),q2)*J6*transpose(diff(T06,q2)))*qd2*qd2+ trace(diff(diff(T06,q3),q3)*J6*transpose(diff(T06,q2)))*qd3*qd3+ 43 GVHD: TS Đinh Thị Thanh Huyền SVTH: Đặng Hữu Trung ĐH Giao Thông Vận Tải Cơ Điện Tử - K55 trace(diff(diff(T06,q4),q4)*J6*transpose(diff(T06,q2)))*qd4*qd4+ trace(diff(diff(T06,q5),q5)*J6*transpose(diff(T06,q2)))*qd5*qd5+ trace(diff(diff(T06,q6),q6)*J6*transpose(diff(T06,q2)))*qd6*qd6+ 2*trace(diff(diff(T06,q1),q2)*J6*transpose(diff(T06,q2)))*qd1*qd2+ 2*trace(diff(diff(T06,q1),q3)*J6*transpose(diff(T06,q2)))*qd1*qd3+ 2*trace(diff(diff(T06,q1),q4)*J6*transpose(diff(T06,q2)))*qd1*qd4+ 2*trace(diff(diff(T06,q1),q5)*J6*transpose(diff(T06,q2)))*qd1*qd5+ 2*trace(diff(diff(T06,q1),q6)*J6*transpose(diff(T06,q2)))*qd1*qd6+ 2*trace(diff(diff(T06,q2),q3)*J6*transpose(diff(T06,q2)))*qd2*qd3+ 2*trace(diff(diff(T06,q2),q4)*J6*transpose(diff(T06,q2)))*qd2*qd4+ 2*trace(diff(diff(T06,q2),q5)*J6*transpose(diff(T06,q2)))*qd2*qd5+ 2*trace(diff(diff(T06,q2),q6)*J6*transpose(diff(T06,q2)))*qd2*qd6+ 2*trace(diff(diff(T06,q3),q4)*J6*transpose(diff(T06,q2)))*qd3*qd4+ 2*trace(diff(diff(T06,q3),q5)*J6*transpose(diff(T06,q2)))*qd3*qd5+ 2*trace(diff(diff(T06,q3),q6)*J6*transpose(diff(T06,q2)))*qd3*qd6+ 2*trace(diff(diff(T06,q4),q5)*J6*transpose(diff(T06,q2)))*qd4*qd5+ 2*trace(diff(diff(T06,q4),q6)*J6*transpose(diff(T06,q2)))*qd4*qd6+ 2*trace(diff(diff(T06,q5),q6)*J6*transpose(diff(T06,q2)))*qd5*qd6- m2*transpose(Vg)*diff(T02,q2)*Rg2-m3*transpose(Vg)*diff(T03,q2)*Rg3- m4*transpose(Vg)*diff(T04,q2)*Rg4-m5*transpose(Vg)*diff(T05,q2)*Rg5m6*transpose(Vg)*diff(T06,q2)*Rg6; F3= trace(diff(T03,q1)*J3*transpose(diff(T03,q3)))*qdd1+ trace(diff(T03,q2)*J3*transpose(diff(T03,q3)))*qdd2+ trace(diff(T03,q3)*J3*transpose(diff(T03,q3)))*qdd3+ trace(diff(T04,q1)*J4*transpose(diff(T04,q3)))*qdd1+ trace(diff(T04,q2)*J4*transpose(diff(T04,q3)))*qdd2+ trace(diff(T04,q3)*J4*transpose(diff(T04,q3)))*qdd3+ trace(diff(T04,q4)*J4*transpose(diff(T04,q3)))*qdd4+ trace(diff(T05,q1)*J5*transpose(diff(T05,q3)))*qdd1+ trace(diff(T05,q2)*J5*transpose(diff(T05,q3)))*qdd2+ trace(diff(T05,q3)*J5*transpose(diff(T05,q3)))*qdd3+ 44 GVHD: TS Đinh Thị Thanh Huyền SVTH: Đặng Hữu Trung ĐH Giao Thông Vận Tải Cơ Điện Tử - K55 trace(diff(T05,q4)*J5*transpose(diff(T05,q3)))*qdd4+ trace(diff(T05,q5)*J5*transpose(diff(T05,q3)))*qdd5+ trace(diff(T06,q1)*J6*transpose(diff(T06,q3)))*qdd1+ trace(diff(T06,q2)*J6*transpose(diff(T06,q3)))*qdd2+ trace(diff(T06,q3)*J6*transpose(diff(T06,q3)))*qdd3+ trace(diff(T06,q4)*J6*transpose(diff(T06,q3)))*qdd4+ trace(diff(T06,q5)*J6*transpose(diff(T06,q3)))*qdd5+ trace(diff(T06,q6)*J6*transpose(diff(T06,q3)))*qdd6+ trace(diff(diff(T03,q1),q1)*J3*transpose(diff(T03,q3)))*qd1*qd1+ 2*trace(diff(diff(T03,q1),q2)*J3*transpose(diff(T03,q3)))*qd1*qd2+ 2*trace(diff(diff(T03,q1),q3)*J3*transpose(diff(T03,q3)))*qd1*qd3+ trace(diff(diff(T03,q2),q2)*J3*transpose(diff(T03,q3)))*qd2*qd2+ 2*trace(diff(diff(T03,q2),q3)*J3*transpose(diff(T03,q3)))*qd2*qd3+ trace(diff(diff(T03,q3),q3)*J3*transpose(diff(T03,q3)))*qd3*qd3+ trace(diff(diff(T04,q1),q1)*J4*transpose(diff(T04,q3)))*qd1*qd1+ trace(diff(diff(T04,q2),q2)*J4*transpose(diff(T04,q3)))*qd2*qd2+ trace(diff(diff(T04,q3),q3)*J4*transpose(diff(T04,q3)))*qd3*qd3+ trace(diff(diff(T04,q4),q4)*J4*transpose(diff(T04,q3)))*qd4*qd4+ 2*trace(diff(diff(T04,q1),q2)*J4*transpose(diff(T04,q3)))*qd1*qd2+ 2*trace(diff(diff(T04,q1),q3)*J4*transpose(diff(T04,q3)))*qd1*qd3+ 2*trace(diff(diff(T04,q1),q4)*J4*transpose(diff(T04,q3)))*qd1*qd4+ 2*trace(diff(diff(T04,q2),q3)*J4*transpose(diff(T04,q3)))*qd2*qd3+ 2*trace(diff(diff(T04,q2),q4)*J4*transpose(diff(T04,q3)))*qd2*qd4+ 2*trace(diff(diff(T04,q3),q4)*J4*transpose(diff(T04,q3)))*qd3*qd4+ trace(diff(diff(T05,q1),q1)*J5*transpose(diff(T05,q3)))*qd1*qd1+ trace(diff(diff(T05,q2),q2)*J5*transpose(diff(T05,q3)))*qd2*qd2+ trace(diff(diff(T05,q3),q3)*J5*transpose(diff(T05,q3)))*qd3*qd3+ trace(diff(diff(T05,q4),q4)*J5*transpose(diff(T05,q3)))*qd4*qd4+ trace(diff(diff(T05,q5),q5)*J5*transpose(diff(T05,q3)))*qd5*qd5+ 2*trace(diff(diff(T05,q1),q2)*J5*transpose(diff(T05,q3)))*qd1*qd2+ 2*trace(diff(diff(T05,q1),q3)*J5*transpose(diff(T05,q3)))*qd1*qd3+ 45 GVHD: TS Đinh Thị Thanh Huyền SVTH: Đặng Hữu Trung ĐH Giao Thông Vận Tải Cơ Điện Tử - K55 2*trace(diff(diff(T05,q1),q4)*J5*transpose(diff(T05,q3)))*qd1*qd4+ 2*trace(diff(diff(T05,q1),q5)*J5*transpose(diff(T05,q3)))*qd1*qd5+ 2*trace(diff(diff(T05,q2),q3)*J5*transpose(diff(T05,q3)))*qd2*qd3+ 2*trace(diff(diff(T05,q2),q4)*J5*transpose(diff(T05,q3)))*qd2*qd4+ 2*trace(diff(diff(T05,q2),q5)*J5*transpose(diff(T05,q3)))*qd2*qd5+ 2*trace(diff(diff(T05,q3),q4)*J5*transpose(diff(T05,q3)))*qd3*qd4+ 2*trace(diff(diff(T05,q3),q5)*J5*transpose(diff(T05,q3)))*qd3*qd5+ 2*trace(diff(diff(T05,q4),q5)*J5*transpose(diff(T05,q3)))*qd4*qd5+ trace(diff(diff(T06,q1),q1)*J6*transpose(diff(T06,q3)))*qd1*qd1+ trace(diff(diff(T06,q2),q2)*J6*transpose(diff(T06,q3)))*qd2*qd2+ trace(diff(diff(T06,q3),q3)*J6*transpose(diff(T06,q3)))*qd3*qd3+ trace(diff(diff(T06,q4),q4)*J6*transpose(diff(T06,q3)))*qd4*qd4+ trace(diff(diff(T06,q5),q5)*J6*transpose(diff(T06,q3)))*qd5*qd5+ trace(diff(diff(T06,q6),q6)*J6*transpose(diff(T06,q3)))*qd6*qd6+ 2*trace(diff(diff(T06,q1),q2)*J6*transpose(diff(T06,q3)))*qd1*qd2+ 2*trace(diff(diff(T06,q1),q3)*J6*transpose(diff(T06,q3)))*qd1*qd3+ 2*trace(diff(diff(T06,q1),q4)*J6*transpose(diff(T06,q3)))*qd1*qd4+ 2*trace(diff(diff(T06,q1),q5)*J6*transpose(diff(T06,q3)))*qd1*qd5+ 2*trace(diff(diff(T06,q1),q6)*J6*transpose(diff(T06,q3)))*qd1*qd6+ 2*trace(diff(diff(T06,q2),q3)*J6*transpose(diff(T06,q3)))*qd2*qd3+ 2*trace(diff(diff(T06,q2),q4)*J6*transpose(diff(T06,q3)))*qd2*qd4+ 2*trace(diff(diff(T06,q2),q5)*J6*transpose(diff(T06,q3)))*qd2*qd5+ 2*trace(diff(diff(T06,q2),q6)*J6*transpose(diff(T06,q3)))*qd2*qd6+ 2*trace(diff(diff(T06,q3),q4)*J6*transpose(diff(T06,q3)))*qd3*qd4+ 2*trace(diff(diff(T06,q3),q5)*J6*transpose(diff(T06,q3)))*qd3*qd5+ 2*trace(diff(diff(T06,q3),q6)*J6*transpose(diff(T06,q3)))*qd3*qd6+ 2*trace(diff(diff(T06,q4),q5)*J6*transpose(diff(T06,q3)))*qd4*qd5+ 2*trace(diff(diff(T06,q4),q6)*J6*transpose(diff(T06,q3)))*qd4*qd6+ 2*trace(diff(diff(T06,q5),q6)*J6*transpose(diff(T06,q3)))*qd5*qd6- m3*transpose(Vg)*diff(T03,q3)*Rg3-m4*transpose(Vg)*diff(T04,q3)*Rg4- m5*transpose(Vg)*diff(T05,q3)*Rg5-m6*transpose(Vg)*diff(T06,q3)*Rg6; 46 GVHD: TS Đinh Thị Thanh Huyền SVTH: Đặng Hữu Trung ĐH Giao Thông Vận Tải Cơ Điện Tử - K55 F4= trace(diff(T04,q1)*J4*transpose(diff(T04,q4)))*qdd1+ trace(diff(T04,q2)*J4*transpose(diff(T04,q4)))*qdd2+ trace(diff(T04,q3)*J4*transpose(diff(T04,q4)))*qdd3+ trace(diff(T04,q4)*J4*transpose(diff(T04,q4)))*qdd4+ trace(diff(T05,q1)*J5*transpose(diff(T05,q4)))*qdd1+ trace(diff(T05,q2)*J5*transpose(diff(T05,q4)))*qdd2+ trace(diff(T05,q3)*J5*transpose(diff(T05,q4)))*qdd3+ trace(diff(T05,q4)*J5*transpose(diff(T05,q4)))*qdd4+ trace(diff(T05,q5)*J5*transpose(diff(T05,q4)))*qdd5+ trace(diff(T06,q1)*J6*transpose(diff(T06,q4)))*qdd1+ trace(diff(T06,q2)*J6*transpose(diff(T06,q4)))*qdd2+ trace(diff(T06,q3)*J6*transpose(diff(T06,q4)))*qdd3+ trace(diff(T06,q4)*J6*transpose(diff(T06,q4)))*qdd4+ trace(diff(T06,q5)*J6*transpose(diff(T06,q4)))*qdd5+ trace(diff(T06,q6)*J6*transpose(diff(T06,q4)))*qdd6+ trace(diff(diff(T04,q1),q1)*J4*transpose(diff(T04,q4)))*qd1*qd1+ trace(diff(diff(T04,q2),q2)*J4*transpose(diff(T04,q4)))*qd2*qd2+ trace(diff(diff(T04,q3),q3)*J4*transpose(diff(T04,q4)))*qd3*qd3+ trace(diff(diff(T04,q4),q4)*J4*transpose(diff(T04,q3)))*qd4*qd4+ 2*trace(diff(diff(T04,q1),q2)*J4*transpose(diff(T04,q4)))*qd1*qd2+ 2*trace(diff(diff(T04,q1),q3)*J4*transpose(diff(T04,q4)))*qd1*qd3+ 2*trace(diff(diff(T04,q1),q4)*J4*transpose(diff(T04,q4)))*qd1*qd4+ 2*trace(diff(diff(T04,q2),q3)*J4*transpose(diff(T04,q4)))*qd2*qd3+ 2*trace(diff(diff(T04,q2),q4)*J4*transpose(diff(T04,q4)))*qd2*qd4+ 2*trace(diff(diff(T04,q3),q4)*J4*transpose(diff(T04,q4)))*qd3*qd4+ trace(diff(diff(T05,q1),q1)*J5*transpose(diff(T05,q4)))*qd1*qd1+ trace(diff(diff(T05,q2),q2)*J5*transpose(diff(T05,q4)))*qd2*qd2+ trace(diff(diff(T05,q3),q3)*J5*transpose(diff(T05,q4)))*qd3*qd3+ trace(diff(diff(T05,q4),q4)*J5*transpose(diff(T05,q4)))*qd4*qd4+ trace(diff(diff(T05,q5),q5)*J5*transpose(diff(T05,q4)))*qd5*qd5+ 2*trace(diff(diff(T05,q1),q2)*J5*transpose(diff(T05,q4)))*qd1*qd2+ 47 GVHD: TS Đinh Thị Thanh Huyền SVTH: Đặng Hữu Trung ĐH Giao Thông Vận Tải Cơ Điện Tử - K55 2*trace(diff(diff(T05,q1),q3)*J5*transpose(diff(T05,q4)))*qd1*qd3+ 2*trace(diff(diff(T05,q1),q4)*J5*transpose(diff(T05,q4)))*qd1*qd4+ 2*trace(diff(diff(T05,q1),q5)*J5*transpose(diff(T05,q4)))*qd1*qd5+ 2*trace(diff(diff(T05,q2),q3)*J5*transpose(diff(T05,q4)))*qd2*qd3+ 2*trace(diff(diff(T05,q2),q4)*J5*transpose(diff(T05,q4)))*qd2*qd4+ 2*trace(diff(diff(T05,q2),q5)*J5*transpose(diff(T05,q4)))*qd2*qd5+ 2*trace(diff(diff(T05,q3),q4)*J5*transpose(diff(T05,q4)))*qd3*qd4+ 2*trace(diff(diff(T05,q3),q5)*J5*transpose(diff(T05,q4)))*qd3*qd5+ 2*trace(diff(diff(T05,q4),q5)*J5*transpose(diff(T05,q4)))*qd4*qd5+ trace(diff(diff(T06,q1),q1)*J6*transpose(diff(T06,q4)))*qd1*qd1+ trace(diff(diff(T06,q2),q2)*J6*transpose(diff(T06,q4)))*qd2*qd2+ trace(diff(diff(T06,q3),q3)*J6*transpose(diff(T06,q4)))*qd3*qd3+ trace(diff(diff(T06,q4),q4)*J6*transpose(diff(T06,q4)))*qd4*qd4+ trace(diff(diff(T06,q5),q5)*J6*transpose(diff(T06,q4)))*qd5*qd5+ trace(diff(diff(T06,q6),q6)*J6*transpose(diff(T06,q4)))*qd6*qd6+ 2*trace(diff(diff(T06,q1),q2)*J6*transpose(diff(T06,q4)))*qd1*qd2+ 2*trace(diff(diff(T06,q1),q3)*J6*transpose(diff(T06,q4)))*qd1*qd3+ 2*trace(diff(diff(T06,q1),q4)*J6*transpose(diff(T06,q4)))*qd1*qd4+ 2*trace(diff(diff(T06,q1),q5)*J6*transpose(diff(T06,q4)))*qd1*qd5+ 2*trace(diff(diff(T06,q1),q6)*J6*transpose(diff(T06,q4)))*qd1*qd6+ 2*trace(diff(diff(T06,q2),q3)*J6*transpose(diff(T06,q4)))*qd2*qd3+ 2*trace(diff(diff(T06,q2),q4)*J6*transpose(diff(T06,q4)))*qd2*qd4+ 2*trace(diff(diff(T06,q2),q5)*J6*transpose(diff(T06,q4)))*qd2*qd5+ 2*trace(diff(diff(T06,q2),q6)*J6*transpose(diff(T06,q4)))*qd2*qd6+ 2*trace(diff(diff(T06,q3),q4)*J6*transpose(diff(T06,q4)))*qd3*qd4+ 2*trace(diff(diff(T06,q3),q5)*J6*transpose(diff(T06,q4)))*qd3*qd5+ 2*trace(diff(diff(T06,q3),q6)*J6*transpose(diff(T06,q4)))*qd3*qd6+ 2*trace(diff(diff(T06,q4),q5)*J6*transpose(diff(T06,q4)))*qd4*qd5+ 2*trace(diff(diff(T06,q4),q6)*J6*transpose(diff(T06,q4)))*qd4*qd6+ 2*trace(diff(diff(T06,q5),q6)*J6*transpose(diff(T06,q4)))*qd5*qd6- 48 GVHD: TS Đinh Thị Thanh Huyền SVTH: Đặng Hữu Trung ĐH Giao Thông Vận Tải Cơ Điện Tử - K55 m4*transpose(Vg)*diff(T04,q4)*Rg4-m5*transpose(Vg)*diff(T05,q4)*Rg5m6*transpose(Vg)*diff(T06,q4)*Rg6; F5= trace(diff(T05,q1)*J5*transpose(diff(T05,q5)))*qdd1+ trace(diff(T05,q2)*J5*transpose(diff(T05,q5)))*qdd2+ trace(diff(T05,q3)*J5*transpose(diff(T05,q5)))*qdd3+ trace(diff(T05,q4)*J5*transpose(diff(T05,q5)))*qdd4+ trace(diff(T05,q5)*J5*transpose(diff(T05,q5)))*qdd5+ trace(diff(T06,q1)*J6*transpose(diff(T06,q5)))*qdd1+ trace(diff(T06,q2)*J6*transpose(diff(T06,q5)))*qdd2+ trace(diff(T06,q3)*J6*transpose(diff(T06,q5)))*qdd3+ trace(diff(T06,q4)*J6*transpose(diff(T06,q5)))*qdd4+ trace(diff(T06,q5)*J6*transpose(diff(T06,q5)))*qdd5+ trace(diff(T06,q6)*J6*transpose(diff(T06,q5)))*qdd6+ trace(diff(diff(T05,q1),q1)*J5*transpose(diff(T05,q5)))*qd1*qd1+ trace(diff(diff(T05,q2),q2)*J5*transpose(diff(T05,q5)))*qd2*qd2+ trace(diff(diff(T05,q3),q3)*J5*transpose(diff(T05,q5)))*qd3*qd3+ trace(diff(diff(T05,q4),q4)*J5*transpose(diff(T05,q5)))*qd4*qd4+ trace(diff(diff(T05,q5),q5)*J5*transpose(diff(T05,q5)))*qd5*qd5+ 2*trace(diff(diff(T05,q1),q2)*J5*transpose(diff(T05,q5)))*qd1*qd2+ 2*trace(diff(diff(T05,q1),q3)*J5*transpose(diff(T05,q5)))*qd1*qd3+ 2*trace(diff(diff(T05,q1),q4)*J5*transpose(diff(T05,q5)))*qd1*qd4+ 2*trace(diff(diff(T05,q1),q5)*J5*transpose(diff(T05,q5)))*qd1*qd5+ 2*trace(diff(diff(T05,q2),q3)*J5*transpose(diff(T05,q5)))*qd2*qd3+ 2*trace(diff(diff(T05,q2),q4)*J5*transpose(diff(T05,q5)))*qd2*qd4+ 2*trace(diff(diff(T05,q2),q5)*J5*transpose(diff(T05,q5)))*qd2*qd5+ 2*trace(diff(diff(T05,q3),q4)*J5*transpose(diff(T05,q5)))*qd3*qd4+ 2*trace(diff(diff(T05,q3),q5)*J5*transpose(diff(T05,q5)))*qd3*qd5+ 2*trace(diff(diff(T05,q4),q5)*J5*transpose(diff(T05,q5)))*qd4*qd5+ trace(diff(diff(T06,q1),q1)*J6*transpose(diff(T06,q5)))*qd1*qd1+ trace(diff(diff(T06,q2),q2)*J6*transpose(diff(T06,q5)))*qd2*qd2+ trace(diff(diff(T06,q3),q3)*J6*transpose(diff(T06,q5)))*qd3*qd3+ 49 GVHD: TS Đinh Thị Thanh Huyền SVTH: Đặng Hữu Trung ĐH Giao Thông Vận Tải Cơ Điện Tử - K55 trace(diff(diff(T06,q4),q4)*J6*transpose(diff(T06,q5)))*qd4*qd4+ trace(diff(diff(T06,q5),q5)*J6*transpose(diff(T06,q5)))*qd5*qd5+ trace(diff(diff(T06,q6),q6)*J6*transpose(diff(T06,q5)))*qd6*qd6+ 2*trace(diff(diff(T06,q1),q2)*J6*transpose(diff(T06,q5)))*qd1*qd2+ 2*trace(diff(diff(T06,q1),q3)*J6*transpose(diff(T06,q5)))*qd1*qd3+ 2*trace(diff(diff(T06,q1),q4)*J6*transpose(diff(T06,q5)))*qd1*qd4+ 2*trace(diff(diff(T06,q1),q5)*J6*transpose(diff(T06,q5)))*qd1*qd5+ 2*trace(diff(diff(T06,q1),q6)*J6*transpose(diff(T06,q5)))*qd1*qd6+ 2*trace(diff(diff(T06,q2),q3)*J6*transpose(diff(T06,q5)))*qd2*qd3+ 2*trace(diff(diff(T06,q2),q4)*J6*transpose(diff(T06,q5)))*qd2*qd4+ 2*trace(diff(diff(T06,q2),q5)*J6*transpose(diff(T06,q5)))*qd2*qd5+ 2*trace(diff(diff(T06,q2),q6)*J6*transpose(diff(T06,q5)))*qd2*qd6+ 2*trace(diff(diff(T06,q3),q4)*J6*transpose(diff(T06,q5)))*qd3*qd4+ 2*trace(diff(diff(T06,q3),q5)*J6*transpose(diff(T06,q5)))*qd3*qd5+ 2*trace(diff(diff(T06,q3),q6)*J6*transpose(diff(T06,q5)))*qd3*qd6+ 2*trace(diff(diff(T06,q4),q5)*J6*transpose(diff(T06,q5)))*qd4*qd5+ 2*trace(diff(diff(T06,q4),q6)*J6*transpose(diff(T06,q5)))*qd4*qd6+ 2*trace(diff(diff(T06,q5),q6)*J6*transpose(diff(T06,q5)))*qd5*qd6- m5*transpose(Vg)*diff(T05,q4)*Rg5-m6*transpose(Vg)*diff(T06,q4)*Rg6; F6= trace(diff(T06,q1)*J6*transpose(diff(T06,q6)))*qdd1+ trace(diff(T06,q2)*J6*transpose(diff(T06,q6)))*qdd2+ trace(diff(T06,q3)*J6*transpose(diff(T06,q6)))*qdd3+ trace(diff(T06,q4)*J6*transpose(diff(T06,q6)))*qdd4+ trace(diff(T06,q5)*J6*transpose(diff(T06,q6)))*qdd5+ trace(diff(T06,q6)*J6*transpose(diff(T06,q6)))*qdd6+ trace(diff(diff(T06,q1),q1)*J6*transpose(diff(T06,q6)))*qd1*qd1+ trace(diff(diff(T06,q2),q2)*J6*transpose(diff(T06,q6)))*qd2*qd2+ trace(diff(diff(T06,q3),q3)*J6*transpose(diff(T06,q6)))*qd3*qd3+ trace(diff(diff(T06,q4),q4)*J6*transpose(diff(T06,q6)))*qd4*qd4+ trace(diff(diff(T06,q5),q5)*J6*transpose(diff(T06,q6)))*qd5*qd5+ trace(diff(diff(T06,q6),q6)*J6*transpose(diff(T06,q6)))*qd6*qd6+ 50 GVHD: TS Đinh Thị Thanh Huyền SVTH: Đặng Hữu Trung ĐH Giao Thông Vận Tải Cơ Điện Tử - K55 2*trace(diff(diff(T06,q1),q2)*J6*transpose(diff(T06,q6)))*qd1*qd2+ 2*trace(diff(diff(T06,q1),q3)*J6*transpose(diff(T06,q6)))*qd1*qd3+ 2*trace(diff(diff(T06,q1),q4)*J6*transpose(diff(T06,q6)))*qd1*qd4+ 2*trace(diff(diff(T06,q1),q5)*J6*transpose(diff(T06,q6)))*qd1*qd5+ 2*trace(diff(diff(T06,q1),q6)*J6*transpose(diff(T06,q6)))*qd1*qd6+ 2*trace(diff(diff(T06,q2),q3)*J6*transpose(diff(T06,q6)))*qd2*qd3+ 2*trace(diff(diff(T06,q2),q4)*J6*transpose(diff(T06,q6)))*qd2*qd4+ 2*trace(diff(diff(T06,q2),q5)*J6*transpose(diff(T06,q6)))*qd2*qd5+ 2*trace(diff(diff(T06,q2),q6)*J6*transpose(diff(T06,q6)))*qd2*qd6+ 2*trace(diff(diff(T06,q3),q4)*J6*transpose(diff(T06,q6)))*qd3*qd4+ 2*trace(diff(diff(T06,q3),q5)*J6*transpose(diff(T06,q6)))*qd3*qd5+ 2*trace(diff(diff(T06,q3),q6)*J6*transpose(diff(T06,q6)))*qd3*qd6+ 2*trace(diff(diff(T06,q4),q5)*J6*transpose(diff(T06,q6)))*qd4*qd5+ 2*trace(diff(diff(T06,q4),q6)*J6*transpose(diff(T06,q6)))*qd4*qd6+ 2*trace(diff(diff(T06,q5),q6)*J6*transpose(diff(T06,q6)))*qd5*qd6- m6*transpose(Vg)*diff(T06,q4)*Rg6; % Don gian hoa he phuong trinh vi phan chuyen dong F1= simplify(F1) F2= simplify(F2) F3= simplify(F3) F4= simplify(F4) F5= simplify(F5) F6= simplify(F6) Kết Luận Và Hướng Phát Triển 1) Kết thu - Thành lập phương trình giải tốn động học thuận ngược robot bậc tự Ge P60 - Ứng dụng phần mềm SolidWorks dựng mơ hình 3D cho robot kết hơp với phần mềm Matlab mô chuyển động robot - Thành lập phương trình giải toán động lực học 2) Hướng phát triển 51 GVHD: TS Đinh Thị Thanh Huyền SVTH: Đặng Hữu Trung ĐH Giao Thông Vận Tải Cơ Điện Tử - K55 - Mô chuyển động robot theo động học ngược - Thiết kế quỹ đạo chuyển động cho robot - Thiết kế điều khiển PID - Thiết kế ứng dụng công nghệ CAD/CAM để gia công chế tạo robot - Tính tốn thơng số chọn động cho khớp robot Tài Liệu Tham Khảo Tiếng Việt [1] Phạm Hồng Vương (2015) Robot cơng nghiệp Nhà xuất Giao Thông Vận Tải [2] Nguyễn Trường Giang (2011) Điều khiển robot Nhà xuất Giao Thông Vận Tải [3] Trần Thế San (2003) Cơ sở nghiên cứu sáng tạo robot Nhà xuất Thống Kê 52 GVHD: TS Đinh Thị Thanh Huyền SVTH: Đặng Hữu Trung ... nghiên cứu Tính tốn động học, động lực học mơ chuyển động Robot General electric P60 (Robot Ge P60) ” làm đề tài tốt nghiệp 2) Mục đích đề tài - Giải toán động học thuận ngược - Mơ chuyển động cho... Dễ tính tốn điều khiển thực dễ dàng chuyển dịch trượt riêng rẽ tốn động học đưa toán phẳng 1.5) Robot General electric P60 Robot General electric (Robot Ge P60) phát triển chế tạo công ty General. .. động học cho robot ” Nội dung chủ yếu chương giải toán động học thuận động học ngược robot Ge P60 Ứng dụng phần mềm Matlab SolidWorks để mô chuyển động robot theo động học thuận Chương 3: “ Động