Phân tích động học và mô phỏng số cơ cấu chấp hành song song 3Rps

ơ khí chính xác, máy công cụ CNC. + Ngành Bưu chính viễn thông: Giá đỡ ăngten, vệ tinh địa tĩnh. + Ngành Chế tạo ôtô: Hệ thống thử tải lốp ôtô, buồng tập lái ôtô. + Ngành Quân sự: Robot song song được dùng làm bệ đỡ ổn định được đặt trên tầu thuỷ, trên xe, trên máy bay, trên chiến xa và các tàu ngầm. Để giữ cân bằng cho ăngten, camera theo dõi mục tiêu, cho rada, cho các thiết bị đo laser, bệ ổn định cho phóng tên lửa, buồng tập lái máy bay, xe tăng, tàu chiến a. Một số ưu điểm của cơ cấu chấp hành song song Nhìn chung, tất cả các loại Robot có cấu trúc song song đều có nhiều ưu điểm và có thể được ứng dụng trong nhiều lĩnh vực, các bộ mô hình máy bay, các khung đỡ kiến trúc khớp nối điều chỉnh, các máy khai thác mỏ - Khả năng chịu được tải cao. Các thành phần cấu tạo nhỏ hơn nên khối lượng của các thành phần cũng nhỏ hơn. - Độ cứng vững cao do kết cấu hình học của chúng. - Tất cả các lực tác động đồng thời được chia sẻ cho tất cả các chân. - Cấu trúc động học một cách đặc biệt của các khớp liên kết cho phép chuyển tất cả lực tác dụng thành các lực kéo/nén của các chân. - Có thể thực hiện được các thao tác phức tạp và hoạt động với độ chính xác cao, với cấu trúc song song, sai số chỉ phụ thuộc vào sai số dọc trục của các cụm cơ cấu chân riêng lẻ và các sai số không bị tích luỹ. - Có thể thiết kế ở các kích thước khác nhau. - Đơn giản hoá các cơ cấu mày và giảm số lượng phần tử do các chân và khớp nối được thiết kế sẵn thành các cụm chi tiết tiêu chuẩn. - Cung cấp khả năng di động cao trong quá trình làm việc do có khối lượng và kích thước nhỏ gọn. - Các cơ cấu chấp hành đều có thể định vị trên tấm nền. - Tầm hoạt động của Robot cơ cấu song song rất rộng từ việc lắp ráp các chi tiết cực nhỏ tới các chuyển động thực hiện các chức năng phức tạp, đòi hỏi độ chính xác cao như: Phay, khoan, tiện, hàn, lắp ráp - Các Robot song song làm việc không cần bệ đỡ và có thể di chuyển tới mọi nơi trong môi trường sản xuất. Chúng có thể làm việc ngay cả khi trên thuyền và treo trên trần, tường - Giá thành của các cơ cấu chấp hành song song ứng dụng trong gia công cơ khí ít hơn so với máy CNC có tính năng tương đương. b. Nhược điểm Tuy nhiên, các cơ cấu chấp hành song song cũng có những nhược điểm nhất định khi so sánh với các Robot chuỗi như: - Không gian làm việc nhỏ và khó thiết kế. - Việc giải các bài toán động học, động lực học phức tạp. - Có nhiều điểm suy biến (kỳ dị) trong không gian. c. Xu hướng phát triển của cơ cấu chấp hành song song Ngày nay, xu hướng sản xuất các máy mô phỏng bay với số bậc tự do ít hơn 6 và các máy gia công sử dụng cơ cấu chấp hành song song đang ngày một phát triển. Tuy nhiên chúng vẫn là những sản phẩm mang nhiều tính nghiên cứu khoa học hơn là thực tiễn. Hiện nay, xu hướng chính là sản xuất các máy với cấu trúc “lai” với số trục ít hơn 6. Mặt khác, không thể không nhắc đến các Robot Haxapod với 6 chân có thể thay đổi chiều dài. Đây là sản phẩm của hãng FANUC Robotics và một số hãng khác vẫn đang nghiên cứu phát triển. Vì vậy, đã hình thành nhiều trung tâm và viện nghiên cứu về cơ cấu chấp hành song song: Tên trường Tên sản phẩm ứng dụng Queens – Canada Robot song song Y học Tuchemrut - Đức Hexanum Máy phay 5 trục Stuttgart Linapod Hexact Máy phay 5 trục Máy phay 3 trục Tokyo – Nhật Nghiên cứu về điều khiển Robot song song Columbia Chuyên về Robot phẫu thuật Maryland Các loại Hexapod Bảng 1.1 1.2.2 Phân loại cơ cấu song song a. Cơ cấu song song đối xứng Một cơ cấu chấp hành song song được gọi là đối xứng nếu nó thỏa mãn các điều kiện sau: - Số chân bằng số bậc tự do của bàn máy di động. - Số khớp và loại khớp trong tất cả các chân được sắp xếp giống nhau. - Số lượng và vị trí các khớp động trong tất cả các chân đều như nhau. Khi các điều kiện trên không thỏa mãn thì Robot được gọi là không đối xứng. Trong cơ cấu chấp hành đối xứng, số chân m bằng số bậc tự do f, đồng thời cũng bằng tổng số vòng (l+1) kể cả các vòng ngoại vi, nghĩa là: m = f = l+1 (1.1) Độ liên kết, Ck của một chân là các bậc tự do có quan hệ với tất cả các khớp trong chân đó: (1.2) Với j là số khớp trong cơ cấu. Từ hai công thức (1.1) và (1.2): (1.3) Với là số bậc tự do trong không gian mà cơ cấu hoạt động. Mỗi chân có độ liên kết thoả mãn: (1.4) b. Các cơ cấu song song phẳng Với các Robot song song phẳng ta có: l = 3; m = f = 3, thay vào (1.3) : C1+ C2 +C3 = 4.f-3=9 Đồng thời từ (1.4) : 3Ê Ck Ê 3 Do đó mỗi chân có 3 bậc tự do trong cách tạo nên chúng. Tóm lại, mỗi chân gồm 3 khâu và 2 khớp, mỗi khớp cần có một bậc tự do, sử dụng khớp quay và khớp trượt làm cặp đôikhớp, ta nhận thấy 7 loại chân có thể nhận được, đó là: RRR, RRP, RPR, PRR, RPP, PPR và PPP. Do đó, ta giới hạn cấu trúc chân của các Robot thì chỉ có 7 loại thuộc lớp Robot song song phẳng tự do có thể thực hiện được. Hình 1.6: Robot song song phẳng 3 bậc tự do với cấu trúc trục chân RRR Hình 1.7: Cấu trúc chấp hành song song 3PRP c. Các cơ cấu song song cầu Các cơ cấu cầu có ba bậc tự do. Do đó yêu cầu liên kết trong cơ cấu chấp hành song song cầu giống như các cơ cấu chấp hành song song phẳng. Trong cơ cấu chấp hành liên kết cầu loại khớp được phép là khớp quay, tất cả các trục khớp phải giao nhau tại một điểm chung, đó là tâm hình cầu, cấu trúc chân duy nhất được phép là cấu trúc RRR. Cơ cấu chấp hành mô tả trên hình 1.9 là cơ cấu chấp hành song song cầu 3RRR, 3 bậc tự do. Hình 1.8: Cấu trúc chấp hành kiểu cầu 3RRR Chú ý rằng một khớp cầu có thể được lắp ở tâm của cơ cấu hành song song cầu. Tuy nhiên, khớp cầu như thế chỉ có thể là khớp thụ động, vì các bộ tác động hiện hữu không thể truyền động cho khớp đó. Vì thế, nếu dùng 1 khớp cầu, cầu có thêm 3 chân để tác động song song bệ chuyển động. d) Các cơ cấu song song không gian Bàn di động Khớp cầu ống trượt trong ống truợt ngoài Khớp quay Đế cố định Hình 1.9: Cơ cấu chấp hành song song 3RPS Đối với Robot song song trong không gian, thay vào các hệ thức (3), (4) ta được: (1.5) 6 (1.6) Giải (1.5), (1.6) với các số nguyên dương Ck, k = 1,2,3.. Có thể phân loại cơ cấu chấp hành song song không gian ứng số bậc tự do và độ liên kết được nêu trên bảng (1.2): Bậc tự do F Số vòng L Tổng các bậc tự do khớp Công thức Độ liên kết Ck K = 1,2,3, 2 1 8 4,4 5,3 6,2 3 2 15 5,5,5 6,5,4 6,6,3 4 3 22 6,6,5,5 6,6,6,4 6 5 36 6,6,6,6,6,6 Bảng 1.2 1.2.3 Kết cấu Robot song song 3RPS Trong đề tài, yêu cầu tính toán và mô phỏng số cơ cấu chấp hành song song 3RPS thường được thiết kế để mang phôi gia công và được lắp đặt trên bàn gá phôi cầu máy phay. Ba chân với chiều dài có thể thay đổi được điểu bởi các xi lanh thủy lực sẽ dẫn bệ động mang phôi chuyển động theo quỹ đạo xác định trước. Hai đầu của các chân một đầu được liên kết với đế cố định bằng khớp bản lề và một đầu được liên kết với bệ di động bằng khớp cầu. Ưu điểm của loại Robot này là khối lượng nhỏ, cấu trúc gọn nhẹ, độ cứng vững cao, có 3 bậc tự do và độ chính xác cao. Tất cả các thành phần cơ khí được lựa chọn và thiết kế càng nhỏ gọn càng tốt và không có khe hở theo chiều dọc trục của các chân, các chân được điều khiển của Robot được dẫn động bằng các cơ cấu chấp hành tuyến tính. Hình (1.10) mô tả sơ đồ của cơ cấu này. - Chi tiết 1: Bàn di động có 3 bậc tự do trong không gian, trong trường hợp cụ thể ở đây là phần mặt dùng để gá dụng cụ cắt kim loại (đầu dao phay,) hoặc lắp đồ gá phôi (thước chia độ, kẹp phôi gia công,) có dạng tam giác (thường là tam giác đều). Trên bàn di động thường lắp các đồ gá để kẹp chi tiết hoặc lắp đặt động cơ - đài dao gia công. Bàn được thiết kế có các lỗ, chốt định để lắp đồ gá. Đồ gá được lắp chặt trên bàn di động bằng các Bulông. - Chi tiết 2: Là một xi lanh thủy lực một đầu nối với bàn cố định bằng khớp bản lề và một đầu còn lại được nối với bàn di động bằng khớp cầu. - Chi tiết 3: Là khớp cầu. - Chi tiết 4: Là khớp bản lề. - Chi tiết 5: Đế cố định. Hình 1.10: Cấu trúc chấp hành song song 3RPS 1.3 Cấu trúc của một cơ cấu chấp hành song song Cũng như các Robot thông thường, cơ cấu song song là loại Robot có cấu trúc vòng kín trong đó các khâu dạng thanh được nối với nhau bằng các khớp động. Sơ đồ động cơ cấu tay máy thông thường là chuỗi nối tiếp các khâu động, từ khâu cuối (là khâu trực tiếp thực hiện thao tác công nghệ) đến giá cố định còn trong cơ cấu song song, khâu cuối được nối với giá cố định bởi một số mạch động học, tức là nối song song với nhau. Sự khác nhau về sơ đồ động đó cũng tạo nên nhiều đặc điểm khác biệt về động học và động lực học. 1.3.1 Các khâu, khớp, chuỗi động - Khâu: Là phần có chuyển động tương đói với các phần khác trong cơ cấu. Chúng ta coi các khâu là vật rắn, điều đó làm cho việc nghiên cứu các kết cấu Robot được dễ dàng hơn. Tuy nhiên với các cơ cấu tốc độ cao hoặc mang tải lớn thì hiện tượng đàn hồi của vật liệu trở nên quan trọng đáng kể và chúng ta phải xét đến. - Khớp: Là chuỗi động giữa hai khâu. Tuỳ theo cấu trúc, môi trường hạn chế một số chuyển động giữa hai khâu, bề mặt tiếp xúc của mỗi khâu tại khớp gọi là thành phần khớp. Hai thành phần của khớp tạo thành một khớp động. Khớp động có thể phân thành khớp thấp và khớp cao tuỳ thuộc vào dạng tiếp xúc. + Khớp thấp: Nếu hai thành phần tiếp xúc là bề mặt. + Khớp cao: Nếu hai thành phần tiếp xúc là điểm hoặc đường. Có 6 loại khớp thấp và hai loại khớp cao cơ bản thường được dùng trong các cơ cấu máy và các Robot đó là: Khớp quay (Revolute Joint – R): Khớp để lại chuyển động của khaua này đối với khâu khác quanh một trụ quay. Nghĩa là khớp quay hạn chế 5 khả năng chuyển động giữa hai thành phần của khớp và có một bậc tự do. Khớp lăng trụ (Prismatic Joint – P): cho phép 2 khâu trượt trên một trục. Do đó khớp lăng trụ hạn chế 5 khả năng chuyển động tương đối giữa hai khâu và có một bậc tự do. Người ta cũng thường gọi khớp lăng trụ là khớp tịnh tiến. Khớp trụ (Cylindrical Joint – C): Cho phép hai chuyển động độc lập, gồm một chuyển động quay quanh một trục và một chuyển đọng tịnh tiến theo trục đó. Do đó, khớp trụ hạn chế 4 khả năng chuyển động giữa hai khâu và có hai bậc tự do. Khớp ren (helical Joint – H): Cho phép chuyển động quay quanh trục đồng thời tịnh tiến theo trục quay. Tuy nhiên chuyển động tịnh tiến phụ thuộc vào chuyển động quay bởi bước của ren vít. Do đó, khớp ren hạn chế 5 chuyển động tương đối giữa hai khâu và còn lại một bậc tự do. Khớp cầu (Sherical Joint – S): Cho phép chuyển động quay giữa hai thành phần khớp tâm cầu theo tất cả các hướng, nhưng không có chuyển động tịnh tiến giữa hai thành phần khớp này. Do đó, khớp cầu hạn chế 3 khả năng chuyển động và có 3 bậc tự do. Khớp phẳng (Plane Joint – E): Có hai khả năng chuyển động tịnh tiến theo hai trục trong mặt tiếp xúc và khả năng quay quanh trục vuông góc với mặt tiếp xúc. Do đó, khớp phẳng hạn chế 3 khả năng chuyển động và có 3 bậc tự do. Khớp bánh răng phẳng (Gear Pair – G): Cho hai bánh răng ăn khớp với nhau. Các mặt tiếp xúc đẩy nhau, chúng thường trượt lên nhau. Do đó, khớp bánh răng hạn chế 4 khả năng chuyển động tương đối giữa hai thành phần khớp và còn lại hai bậc tự do. Khớp cam phẳng (Cam pair – Cp): Tương tự như khớp bánh răng, hai thành phần khớp luôn tiếp xúc với nhau. Do đó, khớp cam phẳng có hai bậc tự do. Khớp quay, khớp lăng trụ, khớp trụ, khớp ren, khớp cầu và khớp phẳng là các khớp thấp, khớp bánh răng phẳng và các khớp cam phẳng là các khớp cao. Trong tài kiệu chuyên khảo có hai khái niệm cần phân biệt: - Cơ cấu: Là tập hợp các khâu được nối với nhau bằng các khớp động (chuỗi động). Robot nối tiếp có cấu trúc chuỗi hở còn Robot song song có cấu trúc chuỗi kín. Chuỗi động học được gọi là cơ cấu khi một trong các khâu nối là cố định. Trong cơ cấu có thể có một hoặc nhiều khâu được ấn định là khâu dẫn với các thông số cho trước. Sự chuyển động của các khâu dẫn là độc lập, sự chuyển động của các khâu khác là phụ thuộc vào chuyển động của khâu dẫn. Cơ cấu là một thiết bị truyền chuyển động từ một hay nhiều khâu dẫn tới các khâu khác. - Máy móc: Gồm một hoặc nhiều cơ cấu cùng các thành phần điện, thủy lực hoặc khí nén, được dùng để biến đổi năng lượng khác. Cơ cấu chấp hành của hệ thống Robot là cơ cấu, để cơ cấu này trở thành máy cần phải có một bộ điều khiển dựa trên bộ vi xử lý, bộ mã hoá và các cảm biến lực, cùng với các bộ phận khác chẳng hạn hệ thống quan sát phối hợp với nhau để chuyển đổi năng lượng bên ngoài thành công hữu ích. Mặc dù máy có thể gồm một hoặc nhiều cơ cấu nhưng cơ cấu không phải là máy do không thực hiện công, chỉ có chức năng truyền chuyển động. 1.3.2 Số bậc tự do Xét hai vật thể hay hai khâu A và B để rời nhau trong không gian gắn vào A một hệ toạ độ oy (hình 1.11) thì B sẽ có 6 khả năng chuyển động tương đối so với A, hay nói cách khác là giữa A và B có 6 khả năng chuyển động tương đối. Các khả năng chuyển động độc lập là: - Các chuyển động tịnh tiến dọc trục ox, oy, oz ký hiệu là Tx, Ty, Tz. - Các chuyển động quay quanh trục ox, oy, oz ký hiệu là Rx, Ry Rz. Hình vẽ 1.11: Các khả năng chuyển động tương đối giữa hai vật thể Định nghĩa: Bậc tự do của cơ cấu là số thông số độc lập tuyến tính cần thiết để hoàn toàn xác định vị trí của cơ cấu. Ta có thể xác định được biểu thức tổng quát về số bậc tự do của cơ cấu theo số khâu, số khớp, và kiểu khớp trong cơ cấu. Để thống nhất cho việc tính toán số bậc tự do của cơ cấu ta sử dụng các ký hiệu sau: ci: số ràng buộc của khớp i. f: số bậc tự do của cơ cấu. fi: số chuyển động tương đối được phép của khớp i. ji: số khớp trong cơ cấu. Ji: số khớp với i bậc tự do. l: số vòng độc lập trong cơ cấu. n: số khâu trong cơ cấu, kể cả khâu cố định. : số bậc tự do trong không gian làm việc của cơ cấu. Ta giả thiết tất cả các khớp đều là hai chiều, khớp ba chiều được coi là hai khớp hai chiều, khớp bốn chiều được coi là ba khớp hai chiều Ngoài ra, còn giả thiết một giá trị được dùng cho các chuyển động của tất cả các khâu chuyển động, chúng đều vận hành trong không gian làm việc, do đố = 6 đối với các khâu không gian và = 3 đối với cơ cấu phẳng và cơ cấu cầu. Giá trị bậc tự do của cơ cấu chính bằng số bậc tự do của tất cả các khâu dần trừ đi số ràng buộc bởi các khớp. Do đó, nếu các khâu đều tự do, số bậc tự do của cơ cấu n- khớp, với một khớp cố định, sẽ bằng (n-1). Tuy nhiên, tổng các ràng buộc của các khớp là bằng , do đó giá trị bậc tự do của cơ cấu được tính theo phương trình: (1.7) Số ràng buộc của 1 khâu và số bậc tự do của khâu đó bằng thông số chuyển động , do đó: (1.8) Do đó tổng ràng buộc của các khâu là: (1.9) Thay phương trình (1.9) vào phương trình (1.7) ta được: (1.10) Phương trình (1.10) được gọi là phương trình tiêu chuẩn Grubler hoặc Kutzbach. Tiêu chuẩn Grubler đúng cho trường hợp các ràng buộc tại các khớp là độc lập và không dư. Ví dụ 1 khớp quay cầu liên kết chuỗi với khớp quay có trục xuyên qua tâm của khớp cầu sẽ tạo ra một bậc tự do thừa. Keieur bậc tự do này gọi là bậc tự do thụ động, cho phép khâu trung gian quay tự do quanh trục xác định từ hai khớp hai khớp đó, Mặc dù khâu trung gian có khả năng truyền lực hoặc momen cho các khâu khác, nhưng nó không có khả năng truyền momen cho trục thụ động. Nói chung, các khâu hai chiều với các cặp S-S, E-E, S-E, đều có bậc tự do thụ động. Bảng (1.3) thống kê sự phối hợp các khâu loại hai chiều và các khớp S-S, E-E, S-E với các khớp cuối cùng của chúng có một bậc tự do. SST Kiểu Bậc tự do thụ động (thừa) 1 S-S Quay quanh trục đi qua các tâm của các khớp cầu 2 S-E Quay quanh trục đi qua các tâm của các khớp cầu và vuông góc với mặt phẳng cảu khớp phẳng. 3 E-E Trượt dọc trục song song với giao tuyến tạo bởi mặt phẳng của khớp phẳng. Nếu hai mặt phẳng này song song sẽ có 3 bậc tự do thụ động. Bảng 1.3 Bậc tự do thụ động không thể truyền momen và chuyển động cho trục thụ động. Khi có một khớp này tồn tại trong cơ cấu, cần trừ đi một bậc tự do từ phương trình tính bậc tự do. Giả sử là số bậc tự do chủ động trong cơ cấu là: (1.11) Ví dụ: Cơ cấu chấp hành song song Stewart – Gough Hình vẽ 1.12: Cơ cấu chấp hành song song Stewart – Gough 1.3.3 Mục tiêu của bài toán phân tích động học Robot là một ngành khoa học hay ngành học về công nghệ truyền thống kết hợp với lý thuyết và ứng dụng của các hệ thống Robot. Việc nghiên cứu bao gồm cả hai vấn đề về nghiên cứu lý thuyết và ứng dụng, những vấn đề đó chia ra công tác thiết kế Robot: cơ học kết cấu, thiết kế quĩ đạo và điều khiển, công tác lập trình cơ học là một nhánh nghiên cứu các vấn đề về năng lượng, lực và tác dụng của chúng đối với chuyển động của hệ thống cơ khí. Việc nghiên cứu bao gồm 3 vấn đề có quan hệ với nhau: động học, tĩnh học và động lực học. - Động học (Kinematics) Động học nghiên cứu các đặc trưng của chuyển động mà không quan tâm đến nguyên nhân gây ra chúng như lực, momen. Khoa học động học nghiên cứu về vị trí, vận tốc, gia tốc và các đạo hàm theo thời gian bậc cao của các biến vị trí cũng như các biến số khác. Sự thay đổi các khâu của Robot liên quan đến vị trí và hướng của khâu chấp hành cuối cùng bởi sự ràng buộc của các khớp. Những quan hệ động học đó là trọng tâm đáng chú ý của việc nghiên cứu động học các Robot. Việc nghiên động học có hai vấn đề: Đó là Phân tích động học (kinematic Analynis) và tổng hợp động học (Kinematic Syntheis). Tuy nhiên quá trình phân tích động học và tổng hợp động học luôn luôn liên quan với nhau. Một nhà thiết kế cần nắm bắt một cách sâu sắc những kỹ năng phân tích để có thể thay đổi các thông số thiết kế để chọn thiết kế tốt nhất. Do đó, vấn đề và điều kiện Robot là những vấn đề cần được quan tâm hơn bài toán động học. Phân tích động học là việc tìm nguồn gốc, các quan hệ chuyển của các khâu Robot. Vấn đề phân tích động học có hai bài toán là bài toán động học thuận (Direct Kinematic) và bài toán động học ngược (Inverse Kinematic). Tổng hợp động học chính là quá trình ngược lại của việc phân tích động học. Trong trường hợp này nhà thiết kế cần phải đặt ra những Robot hay máy mới, điều đó đòi hỏi những thay đổi nhất định về mặt động học. Cụ thể khi có trong tay các bộ thông số về vị trí và hướng của khâu chấp hành cuối cùng, chúng ta cần xác định các thay đổi tương ứng ở các khâu dần và cấu trúc hình học của tay máy. Hơn thế nữa, vấn đề tổng hợp động học có thể chia làm 3 giai đoạn quan hệ với nhau: Tổng hợp về kiểu, tổng hợp về số lượng, tổng hợp về kích thước - Tĩnh học (Statics) Trong nghiên cứu quan hệ về lực ở trạng thái cần bằng của các thành phần thay đổi của Robot. Một Robot có thể hoạt động nhờ tác động lực sinh ra từ các nguồn kích động khác nhau như: Trọng lực, tải trọng, ma sát, lực quán tính Những lực này cần phải được xem xét cẩn thận khi thiết kế và có thể làm cho Robot không đảm bảo chức năng đã định. Lực cân bằng phụ thuộc vào cấu tạo và đặc điểm của Robot mà không phụ thuộc vào thời gian. - Động lực học (Dynamics) Động lực học Robot cũng có hai vấn đề: Phân tích động lực học (Dynamic Analysis) và tổng hợp động lực học (Dynamic Syntheis). Có hai bài toán động lực học: Bài toán động lực học thuận (Direct Dynamic Analysis) và bài toán động lực học ngược (Inverse Dynamics). - Bài toán động lực học thuận: Có thể phát biểu như sau, đặt momen, lực vào các khâu dẫn, đi tìm kết quả đi tìm kết quả chuyển động như các hàm thời gian trên cơ cấu chấp hành cuối cùng. - Bài toán động lực học ngược: Cho quỹ đạo chuyển động của khâu chấp hành cuối cùng theo thời gian, đi tìm momen và lực đặt lên các khâu dẫn để có được sự chuyển động đó. Việc tính toán hiệu quả của bài toán động học thuận là không có giới hạn khi dùng máy tính để mô phỏng các Robot. Mặt khác hiệu quả của bài toán động học ngược vô cùng quan trọng trong thời gian thực, mô hình cơ sở của điều khiển Robot. Khi thay đổi các phương pháp phân tích, như các chương trình Lagrange có thể áp dụng cho việc phân tích động lực học Robot. Tổng hợp động lực học là quá trình ngược lại các quá trình phân tích động lực học. Cụ thể, một tay máy mới, được thiết kế phải đảm bảo các yêu cầu về các thông số chuyển động động lực học. Vấn đề tổng hợp động lực học phức tạp hơn nhiều so với vấn đề phân tích động lực học. Chương 2 Phân tích động học cơ cấu chấp hành song song không gian 3rps 2.1 Cơ sở lý thuyết về động học của hệ nhiều vật 2.1.1 Ma trận cosin chỉ hướng và vận tốc góc của vật rắn a)Ma trận cosin chỉ hướng Ta kí hiệu 3 vectơ đơn vị trên các trục của hệ quy chiếu R0 là và 3 vectơ đơn vị trên các trục của hệ quy chiếu R1 là (hình 2.1) O0 x0 y0 z0 x1 z1 y1 Hình 2.1 Định nghĩa ma trận côsin chỉ hướng Ma trận vuông cấp ba A = (2.1) được gọi là ma trận côsin chỉ hướng của vật rắn B đối với hệ quy chiếu R0 Nếu ta đưa vào kí hiệu , (i,j=1,2,3) (2.2) Thì ma trận cosin chỉ hướng (2.1) có dạng A= (2.3) ý nghĩa của ma trận cosin chỉ hướng Hình 2.2 Để thấy rõ ý nghĩa của ma trận cosin chỉ hướng, ta xét hai hệ quy chiếu Ro và R1 có cùng gốc toạ độ O (hình 2.1).Trong đó hệ quy chiếu động R1 được gắn chặt vào vật rắn B. Lấy một điểm P bất kì thuộc vật rắn B. Vị trí của diểm p được xác định bởi vector .Nếu kí hiệu toạ độ của điểm p trong hệ quy chiếu cố định Ro là u1(0), u2(0),u3(0), trong hệ quy chiếu động R1 là u1(1), u2(1),u3(1) thì ta có các biểu thức sau: (2.4) (2.5) Từ đó ta suy ra các véc tơ đại số Ta tìm mối quan hệ giữa các véc tơ đại số u(0) và u(1) Từ định nghĩa ma trận côsin chỉ hướng ta suy ra bảng sau Bảng 2.1. Bảng xác định ma trận cosin chỉ hướng Từ bảng trên ta dễ dàng thiết lập được các hệ thức liên hệ với (2.5)ta được ++)+++)+++) (2.6) So sánh các biểu thức (2.4) và (2.6) ta suy ra hệ phương trình sau (2.7) Hệ phương trình(2.7) có thể viết gọn như sau u(0) =Au(1) (2.8) Như thế ma trận cosin chỉ hướng xác định vị trí của vật rắn B (hệ quy chiếu động R1) đối với hệ quy chiếu R0. Từ (2.8) ta thấy ma trận cosin chỉ hướng A biết toạ độ của điểm P tuỳ ý thuộc vật rắn B trong hệ quy chiếu động R1 sang toạ độ của nó trong hệ qui chiếu cố định R0. b. Vận tốc góc của vật rắn Vật rắn B chuyển động trong hệ quy chiếu cố định . Lấy D là một điểm nào đó thuộc vật rắn B ( hình 2.3) Gọi và là vận tốc của điểm P và điểm D bất kì trên hệ cố định Ro Gọi A là ma trận cosin chỉ hướng của vật rắn B đối với hệ R0 Hình 2.3 Từ hình vẽ ta có (2.9) Đạo hàm phương trình (2.9) trong hệ qui chiếu cố định R0 ta được (2.10) Theo công thức định nghĩa vận tốc góc của vật rắn ta có Thay vào công thức (2.10) (2.11) Biểu thức (2.11) dưới dạng ngôn ngữ đại số (2.12) Mặt khác ta biểu diễn phương trình (2.9) dưới dạng đại số =+ (2.13) Do A là ma trận côsin chỉ hướng của vật rắn B nên =A. (2.14) Sp: là dạng đại số của vector trên hệ động Dxyz Thế (2.14) vào (2.13) ta được = +A.sp (2.15) Đạo hàm phương trình (2.15) theo t ta được (2.16) Vì A là ma trận cosin chỉ hướng nên là ma trận trực giao .Từ công thức (2.14) ta suy ra (2.17) Thay(2.17) vào (2.16) ta có (2.18) So sánh(2.12) và (2.18) thì (2.19) Như vậy nếu biết ma trận côsin chỉ hướng A của vật rắn B và ma trận , ta có thể xác định được các thành phần vận tốc góc của vật rắn B theo (2.19) 2.1.2 Các ma trận quay cơ bản Ta quy ước hướng quay dương là hướng quay ngược chiều kim đồng hồ như hình (2.4). Các phép quay quanh trục x,y,z của hệ tọa độ vuông góc oxyz được gọi là phép quay cơ bản. Hình 2.4 Ta xác định ma trận quay của phép quay quanh trục x một góc (hình 2.5) Hình 2.5 Theo công thức (2.1) ta có (2.20) được gọi là ma trận quay cơ bản quanh trục x Hoàn toàn tương tự ta xác định được ma trận quay cơ bản quanh trục y và z (hinh 2.6) như sau: Hình 2.6 (2.21) (2.22) 2.1.3 Phép quay Roll – Pitch – Yaw (RPY) a. Xác định các ma trận cosin chỉ hướng từ các góc RPY Gắn hệ toạ độ xyz lên thân một con tàu. Dọc theo thân tàu là trục z (hình 2.7) Roll là chuyển động lắc của thân tàu, tương ứng với việc quay thân tàu một góc quanh trục z. Pitch là sự bồng bềnh, tuơng đương với quay một góc quanh trục y. Yaw là sự lệch hướng, tương đương với quay một góc quanh trục x. Hình 2.7: Phép quay RPY Xác định thứ tự quay: Quay một góc quanh trục x, tiếp theo là quay một góc quanh trục y và sau đó quay một góc quanh trục z. Theo thứ tự quay đó có thể biểu diễn phép quay RPY. (2.23) Từ đó ta xác định được ma trận của phép quay RPY[5] (2.24) b. Xác định các góc RPY từ ma trận cosin chỉ hướng Ma trận cosin chỉ hướng có dạng A= (2.25) So sánh (2.24) và (2.25) ta tính được [2] (2.26) (2.27) (2.28) 2.14 Vận tốc và gia tốc thuộc một điểm của vật rắn a. Xác định vận tốc một điểm bất kì thuộc vật rắn Để xét chuyển động của vật rắn B ở trong hệ quy chiếu R0 = 0x0y0z0 ta lấy một điểm A bất kì thuộc vật rắn làm điểm định vị (điểm cực). Dựng hệ quy chiếu R1 = Axy1z1 gắn liền vào vật rắn B (hình 2.3) Vị trí của điểm P được xác định như sau (2.29) Đạo hàm theo thời gian hai vế của phương trình (2.29) ở trong hệ quy chiếu R0 ta được (2.30) Theo định nghĩa vận tốc góc của vật rắn, ta có (2.31) Thế (2.31) vào phương trình (2.30), ta được (2.32) Chiếu phương trình (2.32) lên hệ quy chiếu R0 ta có (2.33) Chiếu phương trình véctơ (2.32) lên hệ quy chiếu R1 ta được (2.34) Chú ý rằng b. Xác định gia tốc một điểm bất kì thuộc vật rắn Đạo hàm theo thời gian biểu thức (2.32) ở trong hệ quy chiếu Ro ta được (2.35) Từ đó suy ra (2.36) Chiếu phương trình véctơ (2.36) lên hệ quy chiếu R0 ta có phương trình ma trận (2.37) Chiếu phương trình véctơ (2.36) lên hệ quy chiếu R1 ta được (2.38) 2.2 Phân tích động học của cơ cấu chấp hành song song Bài toán động học cơ cấu chấp hành song song được xây dựng nhằm xác định quan hệ giữa các tọa độ của khâu cuối (bàn máy động) và các tọa độ của khâu dẫn. Bài toán động học thuận là bài toán xác định chuyển động của bàn máy động khi biết vị trí của khâu dẫn. Bài toán động học ngược là bài toán xác định vị trí của khâu dẫn khi biết chuyển động của bàn máy động. 2.2.1 Bài toán xác định vị trí Xét hệ gồm p vật rắn, f bậc tự do và sử dụng n (n > f) toạ độ suy rộng Giả sử phương trình liên kết dưới dạng sau (2.39) Phương trình (2.39) có thể viết dưới dạng (2.40) Tách s thành 2 nhóm là véc tơ chứa các toạ độ suy rộng độc lập. là véc tơ chứa các toạ độ suy rộng phụ thuộc. Khi đó phương trình (2.40) được viết dưới dạng F (z,q)=0 (2.41) Hệ phương trình (2.41) là hệ r phương trình phi tuyến. Giải hệ phương trình bằng phương pháp lặp Newton–Raphson [2] ta sẽ xác định được z. 2.2.2 Bài toán vận tốc Đạo hàm phương trình(2.41) theo thời gian ta được (2.42) Phương trình (2.42) có thể viết dưới dạng ma trận (2.43) Trong đó , (2.44) Hệ phương trình (2.43) là hệ phương trình đại số tuyến tính đối với . Giải hệ này bằng phương pháp khử Gauss [2] ta thu được 2.2.3 Bài toán gia tốc Đạo hàm phương trình (2.42) theo thời gian ta được (2.45) Phuơng trình (2.45) có thể viết dưới dạng ma trận như sau (2.46) Trong đó các phần tử của u được xác định theo công thức sau (2.47) Hệ phương trình (2.46) là hệ phương trình đại số tuyến tính đối với . Giải hệ phương trình bằng phương pháp khử Gauss [2] ta thu được Chương 3 Phân tích động học cơ cấu chấp hành song song 3RPS Trên cơ sở lý thuyết trong chương 2, chương này ta giải bài toán động học thuận và động học ngược cho một cơ cấu chấp hành song song 3RPS cụ thể. Kết quả được mô phỏng số bằng hệ chương trình MAPLE. 3.1 Đặc điểm kết cấu và sơ đồ động học Hình 3.1 là sơ đồ động học của cơ cấu chấp hành song song không gian 3 RPS. Ba chân được nối với bàn máy động tại các điểm Bi () bằng các khớp cầu và nối với giá cố định tại các điểm Ai () bằng các khớp quay. Mỗi chân gồm hai phần, khâu piston và cylinder được nối với nhau bằng khớp lăng trụ. Giả thiết hệ chỉ có các liên kết hôlônôm, giữ và dừng. A2 A3 x z q2 z3 q1 q3 O P u w v B3 B1 B2 y y3 y2 z2 z1 y1 A1 d3 d1 d2 Hình 3.1: Sơ đồ động học của cơ cấu chấp hành song song 3RPS Hai toạ độ Đề-các và được gắn tương ứng vào giá cố định và bàn máy động như hình 3.1. Ta đưa ra các giả thiết sau: Các điểm A1, A2, và A3 nằm trong mặt phẳng Oxy, các điểm B1, B2, và B3 nằm trong mặt phẳng Puv. Gốc O của hệ trục toạ độ cố định được chọn tại trọng tâm của tam giác A1A2A3, còn trục x đưược chọn theo phương của . Gốc P của hệ trục toạ độ động được chọn tại trọng tâm của tam giác B1B2B3, còn trục u được chọn theo phương của . Cả hai tam giác A1A2A3 và B1B2B3 là các tam giác đều. Hơn nữa, trục của mỗi khớp quay,, nằm trong mặt phằng Oxy và vuông góc với véc tơ . y x O y1 A1 y2 A2 y3 A3 v P u B1 B2 B3 a) Giá cố định b) Bàn máy động Hình 3.2: Hình chiếu bằng cơ cấu chấp hành song song 3RPS Theo công thức (1.4) ta tính được bậc tự do của cơ cấu là 3. Bàn máy động được xác định bằng véc tơ vị trí khối tâm và ba góc Roll – Pitch – Yaw là f, q, y. Ma trận côsin chỉ hướng của bàn máy động so với giá cố định có dạng (3.1) Ta chọn toạ độ suy rộng như sau (3.2) Trong đó là toạ độ của điểm P trên các trục Ox, Oy và Oz. là chiều dài các chân. Với cách chọn hệ toạ độ như vậy ta có số toạ độ suy rộng là và số toạ độ phụ thuộc là . 3.2 Phân tích động học thuận và ngược 3.2.1 Thiết lập các phương trình liên kết hình học Từ hình 3.1 ta thiết lập các liên kết hình học dưới dạng phương trình véc tơ cho khâu thứ i như sau , (3.3) Phương trình (3.3) có thể viết dưới dạng ma trận (3.4) Trong đó là véc tơ vị trí của điểm Ai trong hệ toạ độ cố định, là véc tơ vị trí của điểm P trong hệ toạ độ cố định, là véc tơ vị trí của điểm Bi trong hệ toạ độ động, là véc tơ vị trí của điểm Bi trong hệ toạ độ động gắn với khâu i với di là độ dài các chân. Ma trận côsin chỉ hướng của khâu i so với giá cố định có dạng (3.5) Thay (3.1) và (3.5) vào phương trình (3.4) ta được (3.6) Tam giác và là các tam giác đều nên từ hình 3.2 ta có , và . Khi đó ta viết phương trình (3.6) với và sử dụng các toạ độ suy rộng (3.2) ta thu được các phương trình liên kết sau (3.7) 3.2.2 Bài toán động học thuận a. Bài toán động học thuận Bài toán động học thuận là bài toán xác định chuyển động của bàn máy động khi biết vị trí của khâu dẫn. Nghĩa là cho biết và , cần tính và . b. Bài toán xác định vị trí áp dụng công thức (2.29) ta có (3.8) Trong đó (chứa các toạ độ độc lập) (chứa các toạ độ phụ thuộc) Hệ phương trình (3.8) là hệ 9 phương trình phi tuyến gồm có 9 ẩn số , giải bằng phương pháp lặp Newton – Raphson ta sẽ thu được , từ đó tính được , và . c. Bài toán xác định vận tốc áp dụng công thức (2.32) ta có (3.9) Trong đó và được thiết lập như sau , (3.10) Giải hệ phương trình (3.9) bằng phương pháp khử Gauss [2] ta thu được . Thuật giải: d. Bài toán xác định gia tốc áp dụng công thức (2.34) ta có (3.11) Trong đó các phần tử của được xác định theo công thức sau , (3.12) Giải hệ phương trình (3.11) bằng phương pháp khử Gauss [2] ta thu được . e. Thuật giải bài toán động học thuận Hình 3.2: Các bước giải bài toán vị trí Hình 3.4: Các bước giải bài toán vận tốc Hình 3.5: Các bước giải bài toán gia tốc 3.2.3. Bài toán động học ngược a.Bài toán động học ngược: Là bài toán xác định vị trí của khâu dẫn khi biết chuyển động của bàn máy động. Nghĩa là cho biết và , cần tính , ,, và . Cách tính tương tự bài toán thuận, tuy nhiên cần tách các ma trận hệ số ứng với và . Kết quả tính toán mô phỏng số bài toán động học thuận và động học ngược được trình trong chưong 4 b.Thuật giải bài toán động học ngược Hình 3.6 : Các bước giải bài toán vị trí 3.2.4 Tính toán vị trí vận tốc và gia tốc khối tâm các khâu - Xác định vị trí các khối tâm x y z O yi zi c1i c2i Ai C1 C2 xi Bi Hình 3.7: Cấu trúc chân Robot Từ hình (3.7) ta có phương trình xác định vị trí khối tâm của khâu piston và khâu cylinder như sau (3.13) (3.14) Trong đó , (3.15) Ta có thể viết phương trình (3.5) dưới dạng sau , (3.16) Thay các phương trình (3.15) và (3.16) vào phương trình (3.13) và (3.14) ta được (3.17) (3.18) Vị trí khối tâm P của bàn máy được xác định bởi (3.19) - Xác định vận tốc góc các khối tâm Vận tốc góc của khâu piston và khâu cylinder bằng vận tốc góc của chân thứ i, tính trong hệ toạ độ động gắn với chân i. Ta có , (3.20) Theo công thức (2.11) ta có (3.21) Từ phương trình (3.16) ta tính được (3.22) (3.23) Thay (3.22) và (3.23) vào (3.21) ta tính được (3.24) So sánh (3.24) với (3.21) ta thu được Do đó ta có , (3.25) Vận tốc góc của bàn máy động trong hệ toạ độ động gắn với bàn máy động Ta có Theo công thức (2.11) ta có (3.26) Thay phương trình (3.1) vào (3.26), với sự trợ giúp của hệ chương trình Maple ta tính được vận tốc góc của bàn máy động tính trên hệ toạ độ Puvw có dạng (3.27) Chương 4 Các kết quả tính toán mô phỏng số Trên cơ sở lý thuyết đã trình bày ở trên. Với sự trợ giúp của phần mềm Maple 8.0 trên máy tính Pentum IV CPU 1.70 GHz chương trình được triển khai tính toán để giải bài toán phân tích động học. 4.1 Thông số của cơ cấu Tham số Kí hiệu Giá trị Đơn vị Độ dài đoạn OAi 1.089 m Độ dài đoạn PBi 0.689 m Khối lượng của bàn máy động 32 kg Khối lượng của các khâu cylinder 2 kg Khối lượng của khâu piston 2 kg Độ dài đoạn AiC1i 0.3 m Độ dài đoạn AiC2i 0.3 m Ma trận của tenxơ quán tính của bàn máy động kg.m2 Ma trận của tenxơ quán tính của khâu cylinder kg.m2 Ma trận của tenxơ quán tính của khâu piston kg.m2 Bảng 4.1: Thông số của cơ cấu 4.2 Kết quả mô phỏng Quy luật chuyển dộng của bàn máy động trong bài toán động học ngược được cho dưới dạng Sau khi giải bài toán động học ngược ta thu được , , . Các giá trị này sẽ được xuất ra file làm đầu vào cho bài toán động học thuận. Kết quả tính toán thu được sẽ được so sánh với bài toán động học ngược ( Ta hoàn toàn có thể cho quy luật chuyển động của các chân theo thời gian để giải bài toán động học thuận và động học ngược). Kết quả mô phỏng được biểu diễn từ hình (4.1) đến (4.18). d[m] d2 d1 d3 t[s] ` Hình 4.1: Đồ thị độ dài các chân (động học ngược) q[rad] q2 q1 q3 t[s] Hình 4.2: Đồ thị góc xoay tại các khớp (động học ngược) t[s] py px p[m] Hình 4.3: Đồ thị (động học ngược) pz[m] t[s] Hình 4.4: Đồ thị (động học ngược) q f, q, y [rad] y t[s] f Hình 4.5: Đồ thị góc xoay bàn máy động (động học ngược) t[s] [m/s] t[s] Hình 4.6: Đồ thị vận tốc độ dài chân 1 (động học ngược) [rad/s] Hình 4.7: Đồ thị vận góc xoay chân 1 (động học ngược) [m/s] t[s] Hình 4.8: Đồ thị vận tốc ( động học ngược) [m/s] t[s] Hình 4.9: Đồ thị vận tốc độ (động học ngược) [rad/s] t[s] [m/s2] Hình 4.10: Đồ thị vận tốc các góc xoay bàn máy động (động học ngược) t[s] Hình 4.11: Đồ thị gia tốc độ dài chân 1 (động học ngược) [rad/s2] t[s] Hình 4.12: Đồ thị gia tốc độ góc xoay chân 1 (động học ngược) [m/s2] t[s] [m/s2] Hình 4.13: Đồ thị gia tốc (động học ngược) t[s] Hình 4.14: Đồ thị gia tốc (động học ngược) [rad/s2] t[s] p[m] Hình 4.15: Đồ thị gia tốc các góc xoay bàn máy động (động học ngược) px py t[s] Hình 4.16: Đồ thị (động học thuận) pz[m] t[s] Hình 4.17: Đồ thị (động học thuận) y q f, q, y [rad] t[s] f Hình 4.18: Đồ thị các góc xoay bàn máy động (động học thuận) Kết luận Do tính năng ưu việt của Robot song song, nên chúng ngày càng được sử dụng rộng rãi trong nền công nghiệp và trong nhiều lĩnh vực khác nhau. Vì vậy các nhà cơ học ngày càng quan tâm nghiên cứu các vẫn đề về động học của các cơ cấu chấp hành song song. Trong chương 1 đã trình bày tổng quan về cơ cấu chấp hành song song như định nghĩa, phân loại, bậc tự do, ưu nhược điểm của cơ cấu chấp hành song song. Trong chương 2 đã trình bày cơ sở lý thuyết để phân động học cơ cấu chấp hành song song như ma trận côsin chỉ hướng, các góc quay cơ bản. Do mô hình cơ học của cơ cấu chấp hành song song khá phức tạp, nên trong đồ án chỉ sử dụng công cụ tin học để tính toán và mô phỏng dùng phần mềm Maple. Đồ án giải quyết được bài toán động học thuận và động học ngược cơ cấu chấp hành song song 3RPS. Các kết quả thu được có thể sử dụng phục vụ cho việc điều khiển và thiết kế các cơ cấu chấp hành song song Trên cơ sở lý thuyết và kết quả thu được từ đồ án, một số vấn đề sau đây có thể tiếp tục nghiên cứu trong thời gian tới: Phân tích động lực học Tìm hiểu về miền làm việc của cơ cấu chấp hành song song ổn định cơ cấu chấp hành song song Điều khiển chuyển động của các cơ cấu chấp hành song song Tài liệu tham khảo [1] Nguyễn Văn Khang, Động lực học hệ nhiều vật, Nhà xuất bản khoa học và kỹ thuật, Hà Nội 2007. [2] Đinh Văn Phong, Phương pháp số trong cơ học, Nhà xuất bản khoa học và kỹ thuật, Hà Nội 2006. [3] Nguyễn Thiện Phúc, Robot công nghiệp, Nhà xuất bản khoa học kỹ thuật, Hà Nội 2002 [4] Phạm Huy Điển, Tính toán, lập trình và giảng dạy toán học trên Maple, Nhà xuất bản khoa học và kỹ thuật, Hà Nội 2002 [5] Ngô Quang Tuấn, Phân tích động lực học Robot song song bằng phương trình Lagrange dạng nhân tử, Luận văn Thạc sĩ Đại Học Bách khoa Hà Nội 2005. [6] Jorge Angeles, Fundamentals of Robotic Mechanical Systems,Theory, Methods, and Algorithms, Springer Lt 3240/2004 Phụ lục Phụ lục A Phương pháp Newton – Raphson sử dụng để giải hệ phương trình có thể khái quát như sau: Cho hệ phương trình có n phương trình n ẩn số: Fi ( x1,x2,,xn ) = 0 với i = 1..n Ta có thể trình bày hệ trên như sau: F(x) = 0 với Để giải hệ trên ta thực hiện các bước sau: Bước 1: Cho một giá trị x khởi gán ban đầu x(0) = [] Bước 2: Tính Với: , i = 0 Bước 3: Tính bằng cách giảihệ phương trình đại số tuyến tính: Bước 4: So sánh (k=1..n) (*) Với: n0 là cấp chính xác bài toán + Nếu (*) thoả mãn chuyển sang bước 5 + Nếu(*) không thoả mãn: Tính rồi quay lại bước 1. Bước 5: Đưa kết quả nghiệm của hệ phương trình , kết thúc chương trình Chú ý: Với giá trị khởi gán ban đầu phải thoả mãn det () 0. Phụ lục B Mã nguồn của chương trình mô phỏng động học thuận và động học ngược của cơ cấu chấp hành song song 3RPS bằng Maple. * Bài toán động học thuận: > restart: > > with(linalg):with(plots): Nhap du lieu > r0:=1.089:dt:=0.002: > rp:=0.689: > b1u:=rp;b1v:=0; > b2u:=-rp/2;b2v:=(sqrt(3)*rp/2); > b3u:=-rp/2;b3v:=-(sqrt(3)*rp/2); > a1x:=r0;a1y:=0; > a2x:=-r0/2;a2y:=(sqrt(3)*r0/2); > a3x:=-r0/2;a3y:=-(sqrt(3)*r0/2);a1z:=0:a2z:=0:a3z:=0: > Readfile:=readdata("DHnguoc.txt",float,1): > n:=round(Readfile[1]):t:=vector(n): > for i from 1 to n do t[i]:=Readfile[i+1]: od: > > for j from 1 to 3 do k:=j-1: for i from 1 to n do d||j[i]:=Readfile[i+n+1+(3*k)*n]: dd||j[i]:=Readfile[i+n+1+(3*k+1)*n]: ddd||j[i]:=Readfile[i+n+1+(3*k+2)*n]: od: od: > #print(ddd3); > fn:=vector(12):Jan:=matrix(12,12):tg2:=matrix(12,12):fnn:=vector(12):Jann:=matrix(12,12):Jannn:=matrix(12,12):Jannnn:=matrix(12,12): > ptlk:=proc(a1x,a1y,a2x,a2y,a3x,a3y,b1u,b1v,b2u,b2v,b3u,b3v,l) global eq1,eq2,eq3,eq4,eq5,eq6,eq7,eq8,eq9,eq10,eq11,eq12,f,Ja,Jat,Jalk,Jad,flk,fd,dv,JaJalk,JaJad,tg1,Jatt: eq1:=q7+cos(q10)*cos(q11)*b1u+(cos(q10)*sin(q11)*sin(q12)-sin(q10)*cos(q12))*b1v-a1x-q1*sin(q4): eq2:=q8+sin(q10)*cos(q11)*b1u+(sin(q10)*sin(q11)*sin(q12)+cos(q10)*cos(q12))*b1v-a1y: eq3:=q9-sin(q11)*b1u+cos(q11)*sin(q12)*b1v-q1*cos(q4): eq4:=q7+cos(q10)*cos(q11)*b2u+(cos(q10)*sin(q11)*sin(q12)-sin(q10)*cos(q12))*b2v-a2x+q2*sin(q5)/2: eq5:=q8+sin(q10)*cos(q11)*b2u+(sin(q10)*sin(q11)*sin(q12)+cos(q10)*cos(q12))*b2v-a2y-sqrt(3)*q2*sin(q5)/2: eq6:=q9-sin(q11)*b2u+cos(q11)*sin(q12)*b2v-a2z-q2*cos(q5):eq7:=q7+cos(q10)*cos(q11)*b3u+(cos(q10)*sin(q11)*sin(q12)-sin(q10)*cos(q12))*b3v-a3x+q3*sin(q6)/2: eq8:=q8+sin(q10)*cos(q11)*b3u+(sin(q10)*sin(q11)*sin(q12)+cos(q10)*cos(q12))*b3v-a3y+sqrt(3)*q3*sin(q6)/2: eq9:=q9-sin(q11)*b3u+cos(q11)*sin(q12)*b3v-a3z-q3*cos(q6):eq10:=q1-d1[l+1]: eq11:=q2-d2[l+1]: eq12:=q3-d3[l+1]: end: > cal_Ja:=proc(eq1,eq2,eq3,eq4,eq5,eq6,eq7,eq8,eq9,eq10,eq11,eq12,l) local j4,j5,i,j1,j2: global f,Ja,Jat,Jalk,Jad,flk,fd,dv,JaJalk,JaJad,Jatt,fn,fnn,Jan,Jann,b: f:=vector([eq1,eq2,eq3,eq4,eq5,eq6,eq7,eq8,eq9,eq10,eq11,eq12]): flk:=vector([eq1,eq2,eq3,eq4,eq5,eq6,eq7,eq8,eq9]): fd:=vector([eq10,eq11,eq12]): Ja:=jacobian(f,[q1,q2,q3,q4,q5,q6,q7,q8,q9,q10,q11,q12]): Jalk:=jacobian(flk,[q1,q2,q3,q4,q5,q6,q7,q8,q9,q10,q11,q12]): Jad:=jacobian(fd,[q1,q2,q3,q4,q5,q6,q7,q8,q9,q10,q11,q12]): Jat:=vector([dd1[l+1],dd2[l+1],dd3[l+1]]); b:=vector([0,0,0,0,0,0,0,0,0,dd1[l+1],dd2[l+1],dd3[l+1]]):Jatt:=vector([-ddd1[l+1],-ddd2[l+1],-ddd3[l+1]]): for i from 1 to 12 do fn[i]:=unapply(f[i],q1,q2,q3,q4,q5,q6,q7,q8,q9,q10,q11,q12): od: for j1 from 1 to 12 do for j2 from 1 to 12 do Jan[j1,j2]:=unapply(Ja[j1,j2],q1,q2,q3,q4,q5,q6,q7,q8,q9,q10,q11,q12): od:od: end: # Bai toan vi tri > vitri:=proc() local k,i,j1,j2,t,j3,l: global u,fn,Jan,deltax,T,dt,tg,x,JaJad,n,Jann,fnn,ii,dx; x[0]:=vector([1.570419200,1.558934618,1.528116514,-.2577114557,-.2593662929,-.2653086263,0,.25e-2,1.5,-.3629763906e-3,-.2693699074e-1,.2694677269e-1]): for l from 0 to n-1 do u[0]:=x[l]: for k from 0 to 5 do ptlk(a1x,a1y,a2x,a2y,a3x,a3y,b1u,b1v,b2u,b2v,b3u,b3v,l): cal_Ja(eq1,eq2,eq3,eq4,eq5,eq6,eq7,eq8,eq9,eq10,eq11,eq12,l): for i from 1 to 12 do fnn[i]:=fn[i](u[k][1],u[k][2],u[k][3] ,u[k][4],u[k][5],u[k][6], u[k][7],u[k][8],u[k][9],u[k][10],u[k][11],u[k][12]): od: for j1 from 1 to 12 do for j2 from 1 to 12 do Jann[j1,j2]:=Jan[j1,j2](u[k][1] ,u[k][2],u[k][3],u[k][4],u[k][5] ,u[k][6],u[k][7],u[k][8],u[k][9], u[k][10],u[k][11],u[k][12]): od:od: deltax:=linsolve(Jann,-fnn): for ii from 1 to 12 do u[k+1][ii]:=evalm(u[k][ii]+deltax[ii]): od: od: for j3 from 1 to 12 do x[l+1][j3]:=u[k][j3]: od: tg[l+1]:=l*dt: od: end: >vitri():#print(x);#print(eq12): # Bai toan van toc > vantoc:=proc(x) local j1,j2: global dx,Jannn,dt,tg,l: for l from 0 to n-1 do ptlk(a1x,a1y,a2x,a2y,a3x,a3y,b1u,b1v,b2u,b2v,b3u,b3v,l): cal_Ja(eq1,eq2,eq3,eq4,eq5,eq6,eq7,eq8,eq9,eq10,eq11,eq12,l): for j1 from 1 to 12 do for j2 from 1 to 12 do Jannn[j1,j2]:=Jan[j1,j2](x[l+1][1] ,x[l+1][2],x[l+1][3],x[l+1][4],x[l+1][5] ,x[l+1][6],x[l+1][7],x[l+1][8],x[l+1][9], x[l+1][10],x[l+1][11],x[l+1][12]): od:od: dx[l+1]:=linsolve(Jannn,b): od: end: > vantoc(x):#print(dx): > # Bai toan gia toc > giatoc:=proc(x,dx) local j3,j4,j1,j2,gan,ii: global ddx,Jannnn,dt,T,n,t,tg,tinh1,tinh2,tinh3,tinh5,tinh6,tinh7,tinh10,tinh11,tinh12,i,j,tinh13,tinh14,l: tinh14:=vector(12):tinh12:=vector(12): gan:=vector(12): for l from 0 to n-1 do ptlk(a1x,a1y,a2x,a2y,a3x,a3y,b1u,b1v,b2u,b2v,b3u,b3v,l): cal_Ja(eq1,eq2,eq3,eq4,eq5,eq6,eq7,eq8,eq9,eq10,eq11,eq12,l): for ii from 1 to 12 do gan[ii]:=dx[l+1][ii]:#nhan ma tran voi vecto od: for j1 from 1 to 12 do for j2 from 1 to 12 do Jannnn[j1,j2]:=Jan[j1,j2](x[l+1][1] ,x[l+1][2],x[l+1][3],x[l+1][4],x[l+1][5] ,x[l+1][6],x[l+1][7],x[l+1][8],x[l+1][9], x[l+1][10],x[l+1][11],x[l+1][12]): od:od: tinh1:=multiply(Jalk,gan): tinh2:=jacobian(tinh1,[q1,q2,q3,q4,q5,q6,q7,q8,q9,q10,q11,q12]): tinh3:=multiply(tinh2,gan): tinh5:=multiply(Jad,gan): tinh6:=jacobian(tinh5,[q1,q2,q3,q4,q5,q6,q7,q8,q9,q10,q11,q12]): tinh7:=multiply(tinh6,gan): tinh10:=Jatt: tinh11:=evalm(tinh7+tinh10): for i from 1 to 9 do tinh12[i]:=tinh3[i]: od: for j from 1 to 3 do tinh12[j+9]:=tinh11[j]: od: for j3 from 1 to 12 do tinh13[j3]:=unapply(tinh12[j3],q1,q2,q3,q4,q5,q6,q7,q8,q9,q10,q11,q12): tinh14[j3]:=tinh13[j3](x[l+1][1],x[l+1][2],x[l+1][3] ,x[l+1][4],x[l+1][5],x[l+1][6], x[l+1][7],x[l+1][8],x[l+1][9],x[l+1][10],x[l+1][11],x[l+1][12]): od: ddx[l+1]:=linsolve(Jannnn,-tinh14): od: end: > giatoc(x,dx):# > outputtg:=[seq(t[j],j=1..n)]: > for i from 1 to 12 do outputq||i:=[seq(x[j][i],j=1..n)]; outputdq||i:=[seq(dx[j][i],j=1..n)]; outputddq||i:=[seq(ddx[j][i],j=1..n)]; od: > fd1 := fopen("DHthuan.txt",WRITE,TEXT): writedata(fd1,[n]); writedata(fd1,outputtg); > for i from 1 to 12 do writedata(fd1,outputq||i); writedata(fd1,outputdq||i); writedata(fd1,outputddq||i); od: > fclose(fd1); > * Bài toán động học ngược: > restart: > with(linalg):with(plots): Nhap du lieu > r0:=1.089: > rp:=0.689: > b1u:=rp;b1v:=0; > b2u:=-rp/2;b2v:=(sqrt(3)*rp/2); > b3u:=-rp/2;b3v:=-(sqrt(3)*rp/2); > a1x:=r0;a1y:=0; > a2x:=-r0/2;a2y:=(sqrt(3)*r0/2); > a3x:=-r0/2;a3y:=-(sqrt(3)*r0/2);a1z:=0:a2z:=0:a3z:=0: > fn:=vector(12):Jan:=matrix(12,12):tg2:=matrix(12,12):fnn:=vector(12):Jann:=matrix(12,12):Jannn:=matrix(12,12):Jannnn:=matrix(12,12): > ptlk:=proc(t,a1x,a1y,a2x,a2y,a3x,a3y,b1u,b1v,b2u,b2v,b3u,b3v) global eq1,eq2,eq3,eq4,eq5,eq6,eq7,eq8,eq9,eq10,eq11,eq12,f,Ja,Jat,Jalk,Jad,flk,fd,dv,JaJalk,JaJad,tg1,Jatt: eq1:=q7+cos(q10)*cos(q11)*b1u+(cos(q10)*sin(q11)*sin(q12)-sin(q10)*cos(q12))*b1v-a1x-q1*sin(q4): eq2:=q8+sin(q10)*cos(q11)*b1u+(sin(q10)*sin(q11)*sin(q12)+cos(q10)*cos(q12))*b1v-a1y: eq3:=q9-sin(q11)*b1u+cos(q11)*sin(q12)*b1v-q1*cos(q4): eq4:=q7+cos(q10)*cos(q11)*b2u+(cos(q10)*sin(q11)*sin(q12)-sin(q10)*cos(q12))*b2v-a2x+q2*sin(q5)/2: eq5:=q8+sin(q10)*cos(q11)*b2u+(sin(q10)*sin(q11)*sin(q12)+cos(q10)*cos(q12))*b2v-a2y-sqrt(3)*q2*sin(q5)/2: eq6:=q9-sin(q11)*b2u+cos(q11)*sin(q12)*b2v-a2z-q2*cos(q5):eq7:=q7+cos(q10)*cos(q11)*b3u+(cos(q10)*sin(q11)*sin(q12)-sin(q10)*cos(q12))*b3v-a3x+q3*sin(q6)/2: eq8:=q8+sin(q10)*cos(q11)*b3u+(sin(q10)*sin(q11)*sin(q12)+cos(q10)*cos(q12))*b3v-a3y+sqrt(3)*q3*sin(q6)/2: eq9:=q9-sin(q11)*b3u+cos(q11)*sin(q12)*b3v-a3z-q3*cos(q6):eq10:=q7-0.0025*sin(10*t): eq11:=q8-0.0025*cos(10*t): eq12:=q9-1.5*(1+0.08*sin(50*t)): end: > cal_Ja:=proc(t,eq1,eq2,eq3,eq4,eq5,eq6,eq7,eq8,eq9,eq10,eq11,eq12) local j4,j5,i,j1,j2: global f,Ja,Jat,Jalk,Jad,flk,fd,dv,JaJalk,JaJad,Jatt,fn,fnn,Jan,Jann,b: f:=vector([eq1,eq2,eq3,eq4,eq5,eq6,eq7,eq8,eq9,eq10,eq11,eq12]): flk:=vector([eq1,eq2,eq3,eq4,eq5,eq6,eq7,eq8,eq9]): fd:=vector([eq10,eq11,eq12]): Ja:=jacobian(f,[q1,q2,q3,q4,q5,q6,q7,q8,q9,q10,q11,q12]): Jalk:=jacobian(flk,[q1,q2,q3,q4,q5,q6,q7,q8,q9,q10,q11,q12]): Jad:=jacobian(fd,[q1,q2,q3,q4,q5,q6,q7,q8,q9,q10,q11,q12]): Jat:=vector([-0.025*cos(10*t),0.025*sin(10*t),-6*cos(50*t)]); b:=vector([0,0,0,0,0,0,0,0,0,0.025*cos(10*t),-0.025*sin(10*t),6*cos(50*t)]): Jatt:=vector([0.25*sin(10*t),0.25*cos(10*t),300*sin(50*t)]): for i from 1 to 12 do fn[i]:=unapply(f[i],q1,q2,q3,q4,q5,q6,q7,q8,q9,q10,q11,q12): od: for j1 from 1 to 12 do for j2 from 1 to 12 do Jan[j1,j2]:=unapply(Ja[j1,j2],q1,q2,q3,q4,q5,q6,q7,q8,q9,q10,q11,q12): od:od: end: > vitri:=proc() local k,i,j1,j2,t,j3,l: global u,fn,Jan,deltax,T,dt,tg,x,JaJad,n,Jann,fnn,ii,dx; k:=0:t:=0:T:=0.6:dt:=0.002:l:=0:n:=round(T/dt): x[0]:=vector([1.570419200,1.558934618,1.528116514,-.2577114557,-.2593662929,-.2653086263,0,.25e-2,1.5,-.3629763906e-3,-.2693699074e-1,.2694677269e-1]): while (t<=T) do u[0]:=x[l]: for k from 0 to 5 do ptlk(t,a1x,a1y,a2x,a2y,a3x,a3y,b1u,b1v,b2u,b2v,b3u,b3v): cal_Ja(t,eq1,eq2,eq3,eq4,eq5,eq6,eq7,eq8,eq9,eq10,eq11,eq12): for i from 1 to 12 do fnn[i]:=fn[i](u[k][1],u[k][2],u[k][3] ,u[k][4],u[k][5],u[k][6], u[k][7],u[k][8],u[k][9],u[k][10],u[k][11],u[k][12]): od: for j1 from 1 to 12 do for j2 from 1 to 12 do Jann[j1,j2]:=Jan[j1,j2](u[k][1] ,u[k][2],u[k][3],u[k][4],u[k][5] ,u[k][6],u[k][7],u[k][8],u[k][9], u[k][10],u[k][11],u[k][12]): od:od: deltax:=linsolve(Jann,-fnn): for ii from 1 to 12 do u[k+1][ii]:=evalm(u[k][ii]+deltax[ii]): od: od: for j3 from 1 to 12 do x[l+1][j3]:=u[k][j3]: od: tg[l+1]:=t: l:=l+1: t:=t+dt: od: end: > vitri():#print(x); > vantoc:=proc(x) local j1,j2,l: global dx,Jannn,dt,T,n,t,tg: t:=0:T:=0.6:dt:=0.002:l:=0:n:=round(T/dt): while (t<=T) do ptlk(t,a1x,a1y,a2x,a2y,a3x,a3y,b1u,b1v,b2u,b2v,b3u,b3v): cal_Ja(t,eq1,eq2,eq3,eq4,eq5,eq6,eq7,eq8,eq9,eq10,eq11,eq12): for j1 from 1 to 12 do for j2 from 1 to 12 do Jannn[j1,j2]:=Jan[j1,j2](x[l+1][1] ,x[l+1][2],x[l+1][3],x[l+1][4],x[l+1][5] ,x[l+1][6],x[l+1][7],x[l+1][8],x[l+1][9], x[l+1][10],x[l+1][11],x[l+1][12]): od:od: dx[l+1]:=linsolve(Jannn,b): tg[l+1]:=t: l:=l+1: t:=t+dt: od: end: > vantoc(x):#print(dx): > giatoc:=proc(x,dx) local j3,j4,j1,j2,l,gan,ii: global ddx,Jannnn,dt,T,n,t,tg,tinh1,tinh2,tinh3,tinh5,tinh6,tinh7,tinh10,tinh11,tinh12,i,j,tinh13,tinh14:tinh14:=vector(12):tinh12:=vector(12): gan:=vector(12): t:=0:T:=0.6:dt:=0.002:l:=0:n:=round(T/dt): while (t<=T) do ptlk(t,a1x,a1y,a2x,a2y,a3x,a3y,b1u,b1v,b2u,b2v,b3u,b3v): cal_Ja(t,eq1,eq2,eq3,eq4,eq5,eq6,eq7,eq8,eq9,eq10,eq11,eq12): for ii from 1 to 12 do gan[ii]:=dx[l+1][ii]:#nhan ma tran voi vecto od: for j1 from 1 to 12 do for j2 from 1 to 12 do Jannnn[j1,j2]:=Jan[j1,j2](x[l+1][1] ,x[l+1][2],x[l+1][3],x[l+1][4],x[l+1][5] ,x[l+1][6],x[l+1][7],x[l+1][8],x[l+1][9], x[l+1][10],x[l+1][11],x[l+1][12]): od:od: tinh1:=multiply(Jalk,gan): tinh2:=jacobian(tinh1,[q1,q2,q3,q4,q5,q6,q7,q8,q9,q10,q11,q12]): tinh3:=multiply(tinh2,gan): tinh5:=multiply(Jad,gan): tinh6:=jacobian(tinh5,[q1,q2,q3,q4,q5,q6,q7,q8,q9,q10,q11,q12]): tinh7:=multiply(tinh6,gan): tinh10:=Jatt: tinh11:=evalm(tinh7+tinh10): for i from 1 to 9 do tinh12[i]:=tinh3[i]: od: for j from 1 to 3 do tinh12[j+9]:=tinh11[j]: od: for j3 from 1 to 12 do tinh13[j3]:=unapply(tinh12[j3],q1,q2,q3,q4,q5,q6,q7,q8,q9,q10,q11,q12): tinh14[j3]:=tinh13[j3](x[l+1][1],x[l+1][2],x[l+1][3] ,x[l+1][4],x[l+1][5],x[l+1][6], x[l+1][7],x[l+1][8],x[l+1][9],x[l+1][10],x[l+1][11],x[l+1][12]): od: ddx[l+1]:=linsolve(Jannnn,-tinh14): tg[l+1]:=t: l:=l+1: t:=t+dt: od: end: > giatoc(x,dx):#print(ddx); Xuat du lieu ra file lam dau vao cho bai toan thuan > outputtg:=[seq(tg[j],j=1..n+1)]: > for i from 1 to 12 do outputq||i:=[seq(x[j][i],j=1..n+1)]; outputdq||i:=[seq(dx[j][i],j=1..n+1)]; outputddq||i:=[seq(ddx[j][i],j=1..n+1)]; od: > fd1 := fopen("DHnguoc.txt",WRITE,TEXT): writedata(fd1,[n+1]); writedata(fd1,outputtg); > for i from 1 to 12 do writedata(fd1,outputq||i); writedata(fd1,outputdq||i); writedata(fd1,outputddq||i); od: > fclose(fd1); >