Đề tài Nghiên cứu thiết kế chế tạo các robot thông minh phục vụ cho các ứng dụng quan trọng

Công ty Viha trình bày kết quả nghiên cứu về xe lăn tự động (XLTĐ) này và đã nhất trí với công ty hợp tác nghiên cứu hòan thiện sản phẩm để đ−a vào sản xuất, thậm chí còn viết hộ phần khoa học, kỹ thuật của thuyết minh Đề tài, Dự án cấp Thành phố. Rất tiếc răng khi kinh phí của Thành phố đã đ−ợc cấp thì Công ty lại cố tình lãng tránh sự hợp tác đã thỏa thuận (!). Phiên bản đầu tiên về XLTĐ này, Đề tài đã theo ph−ơng án dùng 2 động cơ 1 chiều thông th−ờng, qua bộ truyền xích truyền tới 2 bánh. Nh− đã phân tích trong phần nghiên cứu về môđun xe di động của Robocar RP, theo ph−ơng án này rất khó đảm bảo xe chạy theo đ−ờng thẳng, nhất là khi mặt đ−ờng không quá bằng phẳng và đồng đều. Kết quả nghiên cứu tiếp theo về XLTĐ này là một ph−ơng án rất độc đáo là dùng động cơ xe đạp điện bọc bánh lốp trực tiếp làm bánh xe chủ động. Xe có thể lái bằng tay hoặc tự động. Đồng thời không lắp cho khung xe lăn nữa là lắp ngay cho các ghế ngồi thông th−ờng và ký hiệu vắn tắt là XGTĐ (xe ghế chạy điện tự động). XLTĐ-02 và XGTĐ-02 là ph−ơng án xe lăn tự động và xe ghế tự động dùng 1 bánh chủ động bằng bản thân động cơ xe đạp điện (XĐĐ) đ−ợc bọc trực tiếp bánh lốp. Ph−ơng án này có rất nhiều −u điểm: - Động cơ XĐĐ là loại động cơ 1 chiều không chổi than (brushless motor) có vành ngoài quay cùng với rotor, nguồn cung cấp trên thị tr−ờng luôn luôn có sẵn. So với ph−ơng án dùng 2 động cơ 1 chiều thông th−ờng với các bộ truyền xích và bánh răng kèm theo thì đối với ph−ơng án này là rẻ hơn và bên vững hơn. - Theo ph−ơng án XLTĐ-02 là XGTĐ-02 thì lái trực tiếp bánh xe chủ động, tức là bánh xe bọc trực tiếp động cơ XĐĐ. Vì vậy điều khiển việc di chuyển rất tin cậy không phụ thuộc nhiều vào chất l−ợng mặt đ−ờng nh− trong tr−ờng hợp dùng 2 động cơ 1 chiều thông th−ờng tác động vào 2 bánh. Ngoài ra để đảm bảo đồng tốc thực tế cho 2 động cơ này vừa rất khó khăn vừa đòi hỏi thiết bị phức tạp và đắt tiền hơn. - Ph−ơng án dùng động cơ XĐĐ sẽ tiết kiệm năng l−ợng hơn nhiều nên rất phù hợp cho tr−ờng hợp chạy bằng ắcquy. Theo số liệu của Nhà sản xuất động cơ XĐĐ thì công suất định mức: 200w, tốc độ quay định mức: 170v/ph, điện áp: 36v – 12Ah, bộ nạp điện dùng điện nguồn AC 220v, quãng đ−ờng di chuyển đ−ợc cho 1 lần nạp điện là 20km. - Ph−ơng án XLTĐ-02 là XGTĐ-02 có kết cấu đơn giản hơn nhiều, mà lại bền vững hơn. Hệ điều khiển có nhiều hình thức: từ điều khiển bằng tay đến điều khiển tự động với các mức độ khác nhau tùy thuộc vào yêu cầu và loại sensor đ−ợc trang bị. Hệ điều khiển đã đ−ợc môđun hóa khi nghiên cứu chúng trên Robocar TN và có thể dùng để lắp thêm vào XLTĐ-02 hoặc XGTĐ-02 theo yêu cầu của khách hàng. Đây là những sản phẩm giá thành thấp, ch−a thể so sánh với các loại xe ghế (Wheel Chair) chào bán của n−ớc ngoài (xem thông tin trên ảnh) nh−ng lại phù hợp với đại đa số đối t−ợng th−ơng binh và th−ơng tật. Tuy vậy để thành sản phẩm phục vụ đ−ợc nhiều ng−ời phải có sự đầu t− nh− một xí nghiệp. ở phòng thí nghiệm của Đề tài nghiên cứu mới chỉ có điều kiện “cung cấp” cho những ng−ời quen biết thôi. Price $ 9,900.00 Price $ 18,900 $10,725 $ 14.695 Hình 1.2.13. Một số lại xe ghế (Wheelchair) của n−ớc ngoài kèm theo giá bán - Mô tả sản phẩm + Xe lăn tự động: Xe lăn điện chạy tự động cú thể xếp vào loại robocar đơn giản. Nú cú thể được điều khiển hoàn toàn tự động hoặc chạy theo chương trỡnh được “học” trước. Và nếu được trang bị cỏc mụđun cảm biến thớch hợp nú hoàn toàn cú thể biết bỏo hiệu và tự xử lý khi gặp chướng ngại hoặc đi men theo tường v.v. như một robocar. Tuy nhiờn, với mục đớch tạo ra phương tiện di chuyển với tốc độ chõm 4,5km/h, tương đối rẻ tiền phự hợp với đại đa số thương binh, người tàng tật hoặc già yếu, chỳng tụi đó cải tiến xe lăn tay thành xe lăn điện chạy tự động hoặc theo chế độ điều khiển bằng tay nhờ cỏc nỳt bấm gắn trờn tay ghế hoặc “tay lỏi” kiểu “joystick”. Nhưng trờn xe lăn này cũng cú thể lắp cỏc mụđun cảm biến và cài đặt những chương trỡnh phần mềm tương ứng thỡ sẽ biến thành xe lăn chạy tự động hoàn toàn, như trỡnh diễn trờn băng hỡnh Các thông số kỹ thuật: - Trọng tải: - Công suất định mức của động cơ: 200w - Tốc độ trung bình: 4,5km/h - Điện áp: 36v – 12Ah - Bộ nạp sử dụng áp nguồn AC 220v. - Thời gian nạp 4 đến 7 h (tự ngắt khi đầy) - Quãng đ−ờng đi sau 1 lần nạp đầy: 20km + Xe ghế tự động Xe ghế (Wheelchair) chạy tự động cũng là một dạng đơn giản của robocar phục vụ người già ốm yếu, tàng tật. Xe chạy với tốc độ tối đa là 4,5km/h. Tuỳ theo yờu cầu, mức độ điều khiển cú thể cao thấp khỏc nhau. Đơn giản nhất là chỉ dựng tay lỏi để thay đổi hướng đi và tốc độ di chuyển. Cao hơn cú thể lắp thờm vào xe cỏc thiết bị tự động khỏc, như tự bỏo hiệu và xử lý khi gặp chướng ngại, tự đi men theo và giữ khoảng cỏch với tường ngăn, tự quan sỏt và hiển thị hỡnh ảnh từ khoảng cỏch xa nhất định Các thông số kỹ thuật: - Trọng tải: - Công suất định mức của động cơ: 200w - Tốc độ trung bình: 4,5km/h - Điện áp: 36v - 12Ah - Bộ nạp sử dụng áp nguồn AC 220v. - Thời gian nạp 4 đến 7 h (tự ngắt khi đầy) - Quãng đ−ờng đi sau 1 lần nạp đầy: 20km - Các ảnh và bản vẽ kỹ thuật Phần tiếp theo trình bày các ảnh chụp các sản phẩm và các bản vẽ, hồ sơ kỹ thuật liên quan đến sản phẩm. ở mỗi ảnh hoặc bản vẽ, hồ sơ đều đã có phần ghi chú Hình 1.2.14. ảnh chụp XLTĐ Hình 1.2.15. Bản vẽ lắp XLTĐ Hình 1.2.16. ảnh chụp XGTĐ-01 Hình 1.2.17. ảnh chụp XGTĐ- 02 V. Kết luận về nhiệm vụ 1 Nhiệm vụ 1 của Đề tài KC.03.08 đ−ợc đăng ký là nghiên cứu tạo ra nhóm sản phẩm Robot RP đ−ợc nâng cấp và thông minh hóa. Để hệ thống chấp hành có thể đáp ứng linh hoạt các yêu cầu về thông minh hóa Robot RP đã đ−ợc tạo thêm khả năng di động bằng xe và gọi tên là Robocar RP. Nh− vậy, Robocar RP gồm cơ cấu robot RP và cơ cấu xe di chuyển đ−ợc điều khiển thống nhất. Trong hệ thống đó đ−ợc trang bị thêm các bộ phận cảm biến, thiết bị xử lý tín hiệu và ch−ơng trình phần mềm điều khiển. Đề tài đã hoàn thành nghiên cứu thiết kế, chế tạo và vận hành điều khiển Robocar RP với một vài nhiệm vụ ứng dụng khác nhau đã đặt ra. Thoạt đầu là định h−ớng ứng dụng trong phân x−ởng công nghiệp. Sau đó cũng theo xu thế của chuyên ngành robot trên thế giới chuyển từ “robot công nghiệp” sang “robot dịch vụ” (service robots). Robocar RP-01 đ−ợc cải tiến từ nhiều mặt và hòan thành phiên bản Robocar RP - 02 nhằm phục vụ một vài công việc dịch vụ nh− phòng chống bệnh dịch và hỗ trợ ng−ời già yếu, tàng tật. Theo định h−ớng ứng dụng trong các lĩnh vực này đòi hỏi về mức độ “thông minh hóa” cao hơn. Theo định h−ớng ứng dụng đó Đề tài đã phát triển nhóm sản phẩm. Ngoài Robocar RP-01, Robocar RP-02 đã hoàn thành Robocar “Chữ thập đỏ”, Xe lăn tự động, Xe ghế tự động. Một vài sản phẩm nói trên đã đ−ợc ứng dụng thử nghiệm còn số khác cũng đ−ợc nhiều cơ sở quan tâm đến, nh−ng để thành sản phẩm th−ơng mại thì còn cần có nhiều đầu t− thích đáng. Tuy nhiên nếu có đ−ợc những phiên bản đầu tiên ở phòng thí nghiệm thì mới thu hút đ−ợc sự quan tâm của cơ sở tiếp nhận kết quả nghiên cứu. Đó là ý nghĩa thực tiễn của Đề tài. Với cách tiếp cận đó Đề tài đã nhận đ−ợc các kết quả rất cơ bản và mở ra nhiều triển vọng ứng dụng. Các kết quả cơ bản là đã nghiên cứu chuẩn hóa đ−ợc các môđun hợp thành robocar thông minh. Đó là môđun di chuyển, môđun robot đặt trên xe, môđun nhận biết, xử lý và điều khiển với các môđun ch−ơng trình phần mềm cho máy tính. Đó là ý nghĩa khoa học của Đề tài. Vận dụng các môđun này khi ghép nối lại để đáp ứng một yêu cầu cụ thể là hòan toàn khả thi trong thời gian ngắn. Từ thực tế triển khai Đề tài cũng nhận thấy rằng sau khi đã định hình đ−ợc các môđun hợp thành robot thông minh Đơn vị nghiên cứu đã nhanh chóng tạo ra các phiên bản đầu tiên về Robocar địa hình, Robocar Cảnh vệ, Robocar h−ớng dẫn viên và đang phác thảo các kiểu robocar an ninh và robocar quân sự. báo cáo tóm tắt Các kết quả nghiên cứu nhiệm vụ 2 đề tài kc.03.08 NGHIấN CỨU THIẾT KẾ CHẾ TẠO NHểM SẢN PHẨM ROBOT SCA Phần Mở đầu Robot SCA là loại robot thuộc nhúm robot SCARA (selective Compliance Assembly Robot Arm) cú cấu trỳc động học theo kiểu cơ cấu tay mỏy phỏng sinh trục đứng. SCARA là loại robot dựng để lắp rỏp linh hoạt. Tuy SCARA mới xuất hiện ở Nhật trong những năm 80, nhưng do cú nhiều ưu điểm nờn nhanh chúng được nhiều nước ỏp dụng và cải tiến. Cú thể xếp vào nhúm robot SCARA cỏc kiểu robot sau: Adept - One, IBM - 7545, Intelldex 440, Rhino SCARA của Hoa Kỳ; Skilam, SR-2, Nam Robo, Puha 2 của Nhật bản; IS 600 của Đức; Serpent của Anh v.v. Tại Trung tõm NCKT tự động húa, ĐHBK - HN cũng đó thiết kế chế tạo kiểu robot SCA là một biến thể của SCARA. Kiểu robot SCA này cú cấu hỡnh RRRT, với 4 bậc tự do, toàn dựng động cơ bước. Hỡnh 2.0.1. Chế tạo và lắp rỏp Robot SCA Nhằm nõng cao khả năng thao tỏc linh họat cho robot SCAđể đỏp ứng được những tỡnh huống xử lý thụng minh của bộ điều khiển theo cỏc tớn hiệu nhận được từ cỏc sensors, Đề tài đó nghiờn cứu thiết kế cải tiến robot SCA theo 2 định hướng: dựng khớ nộn để tỏc động nhanh và mụđun húa để vạn năng húa kết cấu. 1) Robot SCATM khớ nộn cú 3 bậc tự do đầu dựng động cơ bước, cũn bậc cuối thực hiện chuyển động tịnh tiến dựng truyền động khớ nộn. Trờn hỡnh 2.0.2 là bản vẽ thiết kế robot SCATM khớ nộn. Kết quả nghiờn cứu thiết kế, chế tạo và điều khiển vận hành robot SCATM khớ nộn đ−ợc trình bày trong Phần I của Bỏo cỏo về nhiệm vụ 2 của Đề tài. 2) Robot SCA mụđun húa là một thử nghiệm tạo ra mụđun cỏnh tay chuẩn húa, từ đú cú thể lắp ghộp thành nhiều kiểu robot SCA. Trờn hỡnh 2.0.3 là vớ dụ một kiểu robot SCA tạo ra từ cỏc mụđun cỏnh tay chuẩn húa và hỡnh 2.0.4. là một vớ dụ khỏc: robot SCA cú 2 tay. Mỗi mụđun cỏnh tay (hỡnh 2.0.5) là một khõu hoàn chỉnh của cơ cấu tay mỏy robot, được thực hiện chuyển động quay rất chậm trực tiếp bởi một động cơ kốm hộp giảm tốc tỷ số truyền cao và khụng tồn tại khe hở cạnh răng. Như vậy, phần cốt lừi của mụđun cỏnh tay là mụđun quay (rotation module). Vỡ thế khi triển khai thực hiện nhiệm vụ 2 của Đề tài đó phỏt sinh một nội dung nghiờn cứu và đó nhận được những kết quả bất ngờ và rất cú ý nghĩa khụng những đối với cỏc truyền động quay trong robot mà cũn cú tỏc dụng trong hầu hết cỏc thiết bị mỏy múc hiện đại. Túm tắt cỏc kết quả chủ yếu về nội dung nghiờn cứu này được trỡnh bày trong phần II của Bỏo cỏo về Nhiệm vụ 2 của Đề tài. Hỡnh 2.0.5. Bản vẽ thiết kế Robot SCATM khớ nộn Hình 2.0.3. Robot SCA một cánh tay môđun hóa 76 5 756 Hình 2.0.4. Robot SCA 2 cánh tay môđun hóa Hình 2.0.5. Môđun cánh tay Nội dung phần III của Bỏo cỏo về Nhiệm vụ 2 của Đề tài là cỏc kết quả nghiờn cứu thiết kế chế tạo mụđun Dõy chuyền sản xuất (DCSX) dựng robot SCA để phõn loại sản phẩm theo màu sắc. Nội dung nghiờn cứu này cốt để minh họa về một cỏch thức thể hiện khả năng thụng minh húa cho robot ở đõy cụ thể là robot SCA, trong mụi trường làm việc cụ thể nào đú. Ngoài ra đơn vị chủ trì Đề tài cũn tham gia xõy dựng Dự ỏn “Chế tạo robot bốc dỡ kột lờn pallet tại phõn xưởng chiết của Cụng ty liờn doanh Bia Sài Gũn”. Hỡnh 2.0.6. là mụ phỏng hoạt hỡnh robot này, đú cũng là một biến thể cỡ lớn của Robot SCARA. Kinh phớ từ ngõn sỏch SNKH được duyệt là 2,6 tỷ đồng, phần tham gia của đơn vị là 250 triệu đồng. Cỏc thụng số kỹ thuật của robot này giới thiệu trong phần phụ lục. Hỡnh 2.0.6. Mụ phỏng Robot bốc dỡ kột bia Nội dung Phần 4 của Bỏo cỏo về Nhiệm vụ 2 của Đề tài là cỏc kết quả nghiờn cứu xõy dựng cơ sở lý thuyết, thuật toỏn và chương trỡnh phần mềm điều khiển robot SCA trong trường hợp hoạt động riờng rẽ và trong trường hợp thao tỏc với dõy chuyền sản xuất. Trong đú có cỏc vấn đề với phương phỏp giải bài toỏn động học thuận, bài toỏn động học ngược, trong lập trỡnh quỹ đạo cho robot, bài toỏn động lực học và phương phỏp thực hiện việc chọn lựa sản phẩm theo màu sắc. Nội dung chủ yếu của Phần I đó được viết thành một luận văn thạc sĩ, bảo vệ năm 2006. Phần II cú một sản phẩm đó trưng bày ở chợ Techmart 2003 tại Hà Nội và được tặng thưởng Huy chương Vàng và một phần nội dung đó cú trong luận văn Thạc sĩ, bảo vệ năm 2005. Phần IV cú 2 chương trỡnh phần mềm được cấp giấy chứng nhận bản quyền. Phần i Báo cáo tóm tắt các kết quả nghiên cứu theo nhiệm vụ 2 của đề tài kc. 03.08 robocar scaTm khí nén i. giới thiệu chung Nghiờn cứu thiết kế chế tạo Robot SCATM khớ nộn để nõng cao mức linh hoạt thao tỏc của nú nhằm đỏp ứng được điều khiển khụng bị trễ khi xử lý thụng minh cỏc tớn hiệu nhận được từ hệ thống sensor Phần I bỏo cỏo này trỡnh bày 2 nội dung: 1. Xõy dựng cỏc mụ hỡnh động học và động lực học của Robot SCATM 2. Thiết kế cải tiến và chế tạo Robot SCATM khớ nộn II. tóm tắt về kết quả nghiên cứu xây dựng các mô hình động học và động lực học của Robot scatm 2.1. Thiết lập phương trỡnh động học Robot SCATM 2.1.1. Xác định các hệ tọa độ của robot SCATM Hình 2.1.1. Hệ toạ độ của robot SCATM 2.1.2. Bảng thông số DH của robot SCATM Khâu iq iα ia id 1 *1q 0 1a 0 2 *2q 180 o 2a 0 3 0 0 0 *3d 4 *4q 0 0 4d d4 cố định 3 d q 4x 4 3 x z 2 , , 3 z z 4 2 a 1 a z 1 2 q 2 xx 1 ,, 0 x q 1 0 z 2.1.3. Xác định các ma trận iA của robot SCATM 1 1 1 1 1 1 1 1 1 0 0 0 0 1 0 0 0 0 1 C S a C S C a S A −⎡ ⎤⎢ ⎥⎢ ⎥= ⎢ ⎥⎢ ⎥⎣ ⎦ (2.1) 2 2 2 2 2 2 2 2 2 0 0 0 0 1 0 0 0 0 1 C S a C S C a S A ⎡ ⎤⎢ ⎥−⎢ ⎥= ⎢ ⎥−⎢ ⎥⎣ ⎦ (2.2) 3 3 1 0 0 0 0 1 0 0 0 0 1 0 0 0 1 A d ⎡ ⎤⎢ ⎥⎢ ⎥= ⎢ ⎥⎢ ⎥⎣ ⎦ (2.3) 4 4 4 4 4 4 0 0 0 0 0 0 1 0 0 0 0 C S S C A d −⎡ ⎤⎢ ⎥⎢ ⎥= ⎢ ⎥⎢ ⎥⎣ ⎦ (2.4) 2.1.4. Tính các ma trận iT của robot SCA TM 4 4 4 43 4 4 4 0 0 0 0 0 0 1 0 0 0 0 C S S C T A d −⎡ ⎤⎢ ⎥⎢ ⎥= = ⎢ ⎥⎢ ⎥⎣ ⎦ (2.5) 4 0 0 0 1 x x x x y y y y z z z z n s a p n s a p T n s a p ⎡ ⎤⎢ ⎥⎢ ⎥= ⎢ ⎥⎢ ⎥⎣ ⎦ (2.6) 4 4 4 42 4 3 4 4 3 0 0 0 0 0 0 1 0 0 0 1 C S S C T A A d d −⎡ ⎤⎢ ⎥⎢ ⎥= = ⎢ ⎥+⎢ ⎥⎣ ⎦ (2.7) 2 4 2 4 2 2 2 4 2 4 2 21 4 2 3 4 4 3 cos( ) sin( ) 0 sin( ) cos( ) 0 0 0 1 0 0 0 1 q q q q C a q q q q S a T A A A d d − −⎡ ⎤⎢ ⎥− − −⎢ ⎥= = ⎢ ⎥− − −⎢ ⎥⎣ ⎦ (2.8) 1 2 4 1 2 4 2 12 1 1 1 2 4 1 2 4 2 12 1 11 4 1 4 4 3 cos( ) sin( ) 0 sin( ) cos( ) 0 0 0 1 0 0 0 1 q q q q q q a C a C q q q q q q a S a S T A T d d + − + − +⎡ ⎤⎢ ⎥+ − − + − +⎢ ⎥= = ⎢ ⎥− − −⎢ ⎥⎣ ⎦ (2.9) 2.1.5. Ph−ơng trình động học robot SCATM 4 0 0 0 1 x x x x y y y y z z z z n s a p n s a p T n s a p ⎡ ⎤⎢ ⎥⎢ ⎥ =⎢ ⎥⎢ ⎥⎣ ⎦ (2.10) Từ đó ta có hệ ph−ơng trình sau: 1 2 4 1 2 4 1 2 4 1 2 4 2 12 1 1 2 12 1 1 4 3 cos( ) sin( ) 0 sin( ) cos( ) 0 0 0 1 x y z x y z x y z x y z n q q q n q q q n s q q q s q q q s a a a p a C a C p a S a S p d d = + −⎧⎪ = + −⎪⎪ =⎪ = + −⎪⎪ = − + −⎪⎪ =⎪→ ⎨ =⎪⎪ =⎪ = −⎪⎪ = +⎪⎪ = +⎪ = − −⎪⎩ (2.11) 2.2. Thiết lập bài toán động học ng−ợc Robot SCATM Như đó ký hiệu trong bài toỏn động học thuận, ta cú phương trỡnh sau: Tn = Ai iTn Nhõn hai vế của phương trỡnh này với 1−iA , nhận được nin i TAT 1−= 12 12 12 12 12 12 12 12 2 1 2 12 12 12 12 12 12 12 12 2 11 1 1 34 3 2 1 4 3 0 0 0 1 − − − + + + + − −⎡ ⎤⎢ ⎥− − − − −⎢ ⎥= = ⎢ ⎥− − − − −⎢ ⎥⎣ ⎦ x y x y x y x y x y x y x y x y z z z z C n S n C s S s C a S a C p S p C a a S n C n S s C s S a C a S p C p S a X A A A T n s a p d (2.12) 12 12 12 12 12 12 12 12 2 1 2 12 12 12 12 12 12 12 12 2 11 1 24 2 1 4 0 0 0 1 − − + + + + − −⎡ ⎤⎢ ⎥− − − − −⎢ ⎥= = ⎢ ⎥− − − −⎢ ⎥⎣ ⎦ x y x y x y x y x y x y x y x y z z z z C n S n C s S s C a S a C p S p C a a S n C n S s C s S a C a S p C p S a X A A T n s a p (2.13) 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 11 14 1 4 0 0 0 1 − + + + + −⎡ ⎤⎢ ⎥− + − + − + − +⎢ ⎥= = ⎢ ⎥⎢ ⎥⎣ ⎦ x y x y x y x y x y x y x y x y z z z z C n S n C s S s C a S a C p S p a S n C n S s C s S a C a S p C p X A T n s a p (2.14) * Tìm 1q Xét phần tử hàng 1 cột 4 của 2 ma trận 14X và 1 4T từ (2.14) và (2.8) và phần tử hàng 2 cột 4 của 2 ma trận 14X và 1 4T từ (2.14) và (2.8) 1 1 1 2 2 1 1 2 2 cos( ) sin( ) cos( ) (2.15) sin( ) cos( ) sin( ) (2.16) + − =⎧⎪⎨− + =⎪⎩ x y x y p q p q a a q p q p q a q 2 2 2 1 1 1 2 2 2 2 2 1 1 2 2 cos( ) sin( ) cos ( ) (2.17) sin( ) cos( ) sin ( ) (2.18) ⎧⎡ ⎤+ − =⎪⎣ ⎦⎨⎡ ⎤⎪ − + =⎣ ⎦⎩ x y x y p q p q a a q p q p q a q 2 2 2 2 1 1 1 1 1 2(2.17) (2.18) 2 cos( ) 2 sin( )+ → + − − + =x y x yp p a p q a p q a a (2.19) 2 2 2 2 1 2 1 1 12 cos( ) sin( )x y x yp p a a a p q p q⎡ ⎤+ + − = +⎣ ⎦ (2.20) 2 2 2 2 1 2 1 12 2 2 2 2 2 1 cos( ) sin( ) 2 x y yx x y x y x y p p a a pp q q a p p p p p p + + − = ++ + + (2.21) Đặt 2 2 sinx x y p p p ϕ=+ (2.22) 2 2 cos y x y p p p ϕ=+ (2.23) Từ (2.19) ta có: ( ) 2 2 2 2 1 2 12 2 1 sin 2 x y x y p p a a q a p p ϕ+ + − = ++ (2.24) ( ) 2 2 2 2 2 1 2 1 2 2 1 cos 1 2 ϕ ⎛ ⎞+ + −⎜ ⎟⇒ + = − ⎜ ⎟+⎝ ⎠ x y x y p p a a q a p p (2.25) 2 2 2 2 2 2 2 2 2 1 2 1 2 1 2 2 2 2 1 1 2 , 1 2 2 x y x y x y x y p p a a p p a a q arctg a p p a p p ϕ ⎛ ⎞⎛ ⎞+ + − + + −⎜ ⎟⎜ ⎟+ = −⎜ ⎟⎜ ⎟+ +⎜ ⎟⎝ ⎠⎝ ⎠ (2.26) Từ (2.22) và (2.23) ( )2 ,x yarctg p pϕ⇒ = (2.27) Từ (2.26) và (2.27) ta có: ( ) 2 2 2 2 2 2 2 2 2 1 2 1 2 1 2 2 2 2 1 1 2 , 1 , 2 2 x y x y x y x y x y p p a a p p a a q arctg arctg p p a p p a p p ⎛ ⎞⎛ ⎞+ + − + + −⎜ ⎟⎜ ⎟= − −⎜ ⎟⎜ ⎟+ +⎜ ⎟⎝ ⎠⎝ ⎠ (2.28) * Tìm q2. Từ (2.15) ( ) ( ) ( )1 1 1 2 2cos sin cosx yp q p q a a q⇒ + − = (2.29) ( )1 21 1 22 2 2 2 2 2 2 2cos( ) sin( ) cosyx x y x y x y x y pp a aq q q p p p p p p p p + − =+ + + + (2.30) Đặt: 2 2 sinx x y p p p ϕ=+ (2.31) 2 2 cos y x y p p p ϕ=+ (2.32) ( ) ( )1 21 22 2 2 2sin cos x y x y a aq q p p p p ϕ ⎡ ⎤⎢ ⎥⇒ + − =⎢ ⎥+ +⎣ ⎦ (2.33) Kết hợp với (2.24), ta đ−ợc: ( ) 2 22 2 2 2 1 2 1 22 2 2 2 21 cos 2 ⎛ ⎞ ++ + −⎜ ⎟− =⎜ ⎟+ +⎝ ⎠ x yx y x y x y p pp p a a a q aa p p p p (2.34) ( )2 2 2 21 2 2 1 2 cos 2 + − −⇒ =x yp p a a q a a (2.35) ( ) 22 2 2 2 1 2 2 1 2 1 sin 2 ⎛ ⎞+ − −⇒ − =⎜ ⎟⎜ ⎟⎝ ⎠ x yp p a a q a a (2.36) 22 2 2 2 2 2 2 2 1 2 1 2 2 1 2 1 2 2 1 , 2 2 ⎡ ⎤⎛ ⎞+ − − + − −⎢ ⎥⇒ = − ⎜ ⎟⎜ ⎟⎢ ⎥⎝ ⎠⎢ ⎥⎣ ⎦ x y x yp p a a p p a aq arctg a a a a (2.37) * Tìm d3 Xét phần tử hàng 3 cột 4 của hai ma trận 14X và 1 4T từ (2.14) và (2.8) 4 3= − −zp d d (2.38) 3 4⇒ = − −zd p d (2.39) * Tìm q4 Xét phần tử hàng 2 cột 1 của hai ma trận 34X và 3 4T từ (2.12) và (2.5) 12 12 4x yS n C n S− = (2.40) Xét phần tử hàng 1 cột 1 của hai ma trận 34X và 3 4T từ (2.12) và (2.5) 12 12 4x yC n S n C+ = (2.41) ( )4 4 42 ,q arctg S C⇒ = (2.42) 2.3. Xõy dựng mụ hỡnh động lực học Robot SCATM Ta thiết lập các ma trận i-1Ai Từ các công thức (1.22) và (1.23) ta có: 1 1 1 1 1 1 1 10 1 0 0 0 0 1 0 0 0 0 1 C S a C S C a S A −⎡ ⎤⎢ ⎥⎢ ⎥= ⎢ ⎥⎢ ⎥⎣ ⎦ (2.43) 2 2 2 2 2 2 2 21 2 0 0 0 0 1 0 0 0 0 1 C S a C S C a S A ⎡ ⎤⎢ ⎥−⎢ ⎥= ⎢ ⎥−⎢ ⎥⎣ ⎦ (2.44) 2 3 3 1 0 0 0 0 1 0 0 0 0 1 0 0 0 1 A d ⎡ ⎤⎢ ⎥⎢ ⎥= ⎢ ⎥⎢ ⎥⎣ ⎦ (2.45) 4 4 4 43 4 4 0 0 0 0 0 0 1 0 0 0 0 C S S C A d −⎡ ⎤⎢ ⎥⎢ ⎥= ⎢ ⎥⎢ ⎥⎣ ⎦ (2.46) Từ công thức (1.21) ta có: 12 12 2 12 1 1 12 12 2 12 1 10 0 1 2 1 2 0 0 0 0 1 0 0 0 0 1 C S a C a C S C a S a S A A A +⎡ ⎤⎢ ⎥− +⎢ ⎥= = ⎢ ⎥−⎢ ⎥⎣ ⎦ (2.47) 12 12 2 12 1 1 12 12 2 12 1 10 0 1 2 3 1 2 3 3 0 0 0 0 1 0 0 0 1 C S a C a C S C a S a S A A A A d +⎡ ⎤⎢ ⎥− +⎢ ⎥= = ⎢ ⎥− −⎢ ⎥⎣ ⎦ (2.48) ( ) ( ) ( ) ( )1 2 4 1 2 4 2 12 1 10 0 1 2 3 1 2 4 1 2 4 2 12 1 1 4 1 2 3 4 3 4 cos sin 0 sin cos 0 0 0 1 0 0 0 1 q q q q q q a C a C q q q q q q a S a S A A A A A d d ⎡ + − + − + ⎤⎢ ⎥+ − − + − +⎢ ⎥= = ⎢ ⎥− − −⎢ ⎥⎣ ⎦ (2.49) 2 2 2 2 2 2 2 21 1 2 3 2 3 3 0 0 0 0 1 0 0 0 1 C S a C S C a S A A A d ⎡ ⎤⎢ ⎥−⎢ ⎥= = ⎢ ⎥− −⎢ ⎥⎣ ⎦ (2.50) ( ) ( ) ( ) ( )2 4 2 4 2 21 1 2 3 2 4 2 4 2 2 4 2 3 4 3 4 cos sin 0 sin cos 0 0 0 1 0 0 0 1 q q q q a C q q q q a S A A A A d d ⎡ − + − − + ⎤⎢ ⎥− − + − − +⎢ ⎥= = ⎢ ⎥− − −⎢ ⎥⎣ ⎦ (2.51) 4 4 4 42 2 3 4 3 4 3 4 0 0 0 0 0 0 1 0 0 0 1 C S S C A A A d d −⎡ ⎤⎢ ⎥⎢ ⎥= = ⎢ ⎥+⎢ ⎥⎣ ⎦ (2.52) Từ công thức (1.26) và (1.27) ta có: 1 0 1 0 0 1 0 0 0 0 0 0 0 0 0 0 0 D −⎡ ⎤⎢ ⎥⎢ ⎥= ⎢ ⎥⎢ ⎥⎣ ⎦ (2.53) 2 0 1 0 0 1 0 0 0 0 0 0 0 0 0 0 0 D −⎡ ⎤⎢ ⎥⎢ ⎥= ⎢ ⎥⎢ ⎥⎣ ⎦ (2.54) 3 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 D ⎡ ⎤⎢ ⎥⎢ ⎥= ⎢ ⎥⎢ ⎥⎣ ⎦ (2.55) 4 0 1 0 0 1 0 0 0 0 0 0 0 0 0 0 0 D −⎡ ⎤⎢ ⎥⎢ ⎥= ⎢ ⎥⎢ ⎥⎣ ⎦ (2.56) Ta tính ijU theo công thức (1.31) 1 1 1 1 1 1 1 10 11 1 1 0 0 0 0 0 0 0 0 0 0 S C a S C S a C U D A − − −⎡ ⎤⎢ ⎥−⎢ ⎥= = ⎢ ⎥⎢ ⎥⎣ ⎦ (2.57) 12 12 2 12 1 1 12 12 2 12 1 10 21 1 2 0 0 0 0 0 0 0 0 0 0 S C a S a S C S a C a C U D A − − −⎡ ⎤⎢ ⎥+⎢ ⎥= = ⎢ ⎥⎢ ⎥⎣ ⎦ (2.58) 12 12 2 12 12 12 2 120 1 22 1 2 2 0 0 0 0 0 0 0 0 0 0 S C a S C S a C U A D A − −⎡ ⎤⎢ ⎥⎢ ⎥= = ⎢ ⎥⎢ ⎥⎣ ⎦ (2.59) 12 12 2 12 1 1 12 12 2 12 2 10 31 1 3 0 0 0 0 0 0 0 0 0 0 S C a S a S C S a C a C U D A − − −⎡ ⎤⎢ ⎥+⎢ ⎥= = ⎢ ⎥⎢ ⎥⎣ ⎦ (2.60) 12 12 2 12 12 12 2 120 1 32 1 2 3 0 0 0 0 0 0 0 0 0 0 S C a S C S a C U A D A − −⎡ ⎤⎢ ⎥⎢ ⎥= = ⎢ ⎥⎢ ⎥⎣ ⎦ (2.61) 0 2 33 2 3 3 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 U A D A ⎡ ⎤⎢ ⎥⎢ ⎥= = ⎢ ⎥−⎢ ⎥⎣ ⎦ (2.62) ( ) ( ) ( ) ( )1 2 4 1 2 4 2 12 1 10 1 2 4 1 2 4 2 12 1 1 41 1 4 sin cos 0 cos sin 0 0 0 0 0 0 0 0 0 q q q q q q a S a S q q q q q q a C a C U D A ⎡− + − + − − − ⎤⎢ ⎥+ − + − +⎢ ⎥= = ⎢ ⎥⎢ ⎥⎣ ⎦ (2.63) ( ) ( ) ( ) ( ) 1 2 4 1 2 4 2 12 0 1 1 2 4 1 2 4 2 12 42 1 2 4 sin cos 0 cos sin 0 0 0 0 0 0 0 0 0 q q q q q q a S q q q q q q a C U A D A ⎡− + − + − − ⎤⎢ ⎥+ − + −⎢ ⎥= = ⎢ ⎥⎢ ⎥⎣ ⎦ (2.64) 0 2 43 2 3 4 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 U A D A ⎡ ⎤⎢ ⎥⎢ ⎥= = ⎢ ⎥−⎢ ⎥⎣ ⎦ (2.65) ( ) ( ) ( ) ( )1 2 4 1 2 40 3 1 2 4 1 2 4 44 3 4 4 sin cos 0 0 cos sin 0 0 0 0 0 0 0 0 0 0 q q q q q q q q q q q q U A D A ⎡ + − − + − ⎤⎢ ⎥− + − − + −⎢ ⎥= = ⎢ ⎥⎢ ⎥⎣ ⎦ (2.66) 2 1 1 1 1 1 1 1 1 1 10 0 3 2 0 0 0 0 0 0 0 0 1 0 0 2 m a m a J m a m ⎛ ⎞−⎜ ⎟⎜ ⎟⎜ ⎟= ⎜ ⎟⎜ ⎟⎜ ⎟−⎜ ⎟⎝ ⎠ (2.67) 2 2 2 2 2 2 2 2 2 1 10 0 3 2 0 0 0 0 0 0 0 0 1 0 0 2 m a m a J m a m ⎛ ⎞−⎜ ⎟⎜ ⎟⎜ ⎟= ⎜ ⎟⎜ ⎟⎜ ⎟−⎜ ⎟⎝ ⎠ (2.68) 2 3 3 3 3 3 3 3 3 1 10 0 3 2 0 0 0 0 0 0 0 0 1 0 0 2 m l m l J m l m ⎛ ⎞−⎜ ⎟⎜ ⎟⎜ ⎟= ⎜ ⎟⎜ ⎟⎜ ⎟−⎜ ⎟⎝ ⎠ (2.69) 2 4 4 4 4 4 4 4 4 1 10 0 3 2 0 0 0 0 0 0 0 0 1 0 0 2 m l m l J m l m ⎛ ⎞−⎜ ⎟⎜ ⎟⎜ ⎟= ⎜ ⎟⎜ ⎟⎜ ⎟−⎜ ⎟⎝ ⎠ (2.70) Từ công thức (1.51): ( ) ( ) ( ) ( )11 11 1 11 21 2 21 31 3 31 41 4 41T T T TD Tr U J U Tr U J U Tr U J U Tr U J U= + + + 2 2 2 4 2 4 1 2 2 2 1 2 2 4 4 2 4 4 4 1 2 4 4 1 2 2 1=m m cos( ) 2 3 a a m a m a a C m l a C m l a q q m a a C+ + + − − − + + 2 2 2 2 2 2 3 3 1 2 3 1 2 2 3 3 2 2 1 3 2 3 1 1 1 3 3 4 4 1 1 12 3 3 3 m l a C m a a C m l a m a m a m a m a m l m l− + − + + + + + + (2.71) ( ) ( ) ( )12 22 2 21 32 3 31 42 4 41T T TD Tr U J Tr U J Tr U J= + +U U U 2 2 2 2 2 2 1 2 2 3 3 3 2 1 2 1 3 3 2 2 4 4 4 3 3 1 1 1 1 3 2 2 3 m a m a a C a m l m a a C a m l C a m l C m l= + − + − − + ( )2 2 24 4 2 3 1 4 4 2 4 2 4 1 2 2 41 1 cos3 2m l a m a m l q q a m a C a m+ + − − + + + (2.72) ( ) ( )13 33 3 31 43 4 41 0T TD Tr U J Tr U J= + =U U (2.73) ( )14 44 4 41TD Tr U J= U ( )24 4 4 4 2 4 4 4 1 2 41 1 1 cos3 2 2m l m l a C m l a q q= + + − + (2.74) ( ) ( ) ( )22 22 2 22 32 3 32 42 4 42T T TD Tr U J Tr U J Tr U J= + +U U U 2 2 2 2 2 2 3 3 2 3 2 2 2 4 2 4 4 4 3 3 4 4 1 1 1 3 3 3 a m l a m a m a m a m l C l m l m= − + + + − + + (2.75) ( ) ( )23 33 3 32 43 4 42 0T TD Tr U J Tr U J= + =U U (2.76) ( ) 224 33 4 42 4 4 2 4 4 41 13 2TD Tr U J l m a m l C= = − +U (2.77) ( ) ( )33 33 3 33 43 4 43 3 4T TD Tr U J Tr U J m m= + = +U U (2.78) ( )34 44 4 33 0TD Tr U J= =U (2.79) ( ) 244 44 4 44 4 413TD Tr U J m l= =U (2.80) ( ) 11 12 13 14 12 22 23 24 12 23 33 34 14 24 34 44 D D D D D D D D D q D D D D D D D D ⎡ ⎤⎢ ⎥⎢ ⎥= ⎢ ⎥⎢ ⎥⎣ ⎦ (2.81) 1 1 1 1 1 1 1 12 0 111 1 1 0 0 0 0 0 0 0 0 0 0 C S a C S C a C U D A − −⎡ ⎤⎢ ⎥− − −⎢ ⎥= = ⎢ ⎥⎢ ⎥⎣ ⎦ (2.82) 12 12 2 12 1 1 12 12 2 12 1 12 0 1 211 1 1 2 0 0 0 0 0 0 0 0 0 0 C S a C a C S C a S a S U D A A − − − −⎡ ⎤⎢ ⎥− − −⎢ ⎥= = ⎢ ⎥⎢ ⎥⎣ ⎦ (2.83) 12 12 2 12 1 1 12 12 2 12 1 12 0 1 2 311 1 1 2 3 0 0 0 0 0 0 0 0 0 0 C S a C a C S C a S a S U D A A A − − − −⎡ ⎤⎢ ⎥− − −⎢ ⎥= = ⎢ ⎥⎢ ⎥⎣ ⎦ (2.84) ( ) ( ) ( ) ( )1 2 4 1 2 4 2 12 1 12 0 1 2 3 1 2 4 1 2 4 2 12 1 1 411 1 1 2 3 4 cos sin 0 sin cos 0 0 0 0 0 0 0 0 0 q q q q q q a C a C q q q q q q a S a S U D A A A A ⎡− + − − + − − − ⎤⎢ ⎥− + − + − − −⎢ ⎥= = ⎢ ⎥⎢ ⎥⎣ ⎦ (2.85) 12 12 2 12 12 12 2 120 1 212 1 1 2 2 0 0 0 0 0 0 0 0 0 0 C S a C S C a S U D A D A − − −⎡ ⎤⎢ ⎥− −⎢ ⎥= = ⎢ ⎥⎢ ⎥⎣ ⎦ (2.86) 12 12 2 12 12 12 2 120 1 2 312 1 1 2 2 3 0