Báo cáo Các kết quả nghiên cứu theo nhiệm vụ 2 - Đề tài kc.03.08 nhóm sản phẩm robot sca

BỘ KHOA HỌC VÀ CÔNG NGHỆ CHƯƠNG TRÌNH KC.03 YZ YZ YZ YZ YZ YZY YZ YZ YZY YZ YZ YZY “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” Mà SỐ: KC.03.08 BÁO CÁO CÁC KẾT QUẢ NGHIÊN CỨU THEO NHIỆM VỤ 2 - ĐỀ TÀI KC.03.08 NHÓM SẢN PHẨM ROBOT SCA 6246-2 25/12/2006 HÀ NỘI 2006 2 Môc lôc Më ®Çu 4 PhÇn 1: Robot SCATM khÝ nÐn 11 I. Giíi thiÖu chung 11 II. X©y dùng c¸c m« h×nh ®éng häc vµ ®éng lùc häc Robot SCATM 12 2.1. ThiÕt lËp ph−¬ng tr×nh ®éng häc Robot SCATM 12 2.1.1. X¸c ®Þnh c¸c hÖ täa ®é cña Robot SCATM 12 2.1.2. B¶ng th«ng sè DH cña Robot SCATM 12 2.1.3. X¸c ®Þnh c¸c ma trËn cña Robot SCATM 12 2.1.4. TÝnh c¸c ma trËn T cña Robot SCATM 13 2.1.5. Ph−¬ng tr×nh ®éng häc Robot SCATM 14 2.2. ThiÕt lËp bµi to¸n ®éng häc ng−îc Robot SCATM 15 2.3. X©y dùng m« h×nh ®éng lùc häc Robot SCATM 19 III. ThiÕt kÕ c¶i tiÕn chÕ t¹o Robot SCATM khÝ nÐn 36 PhÇn 2: M«®un quay dïng b¸nh r¨ng con l¨n 40 I. Giíi thiÖu chung 40 II. Nhu cÇu cÇn cã m«®un quay dïng BRCL 41 III. CÊu t¹o vµ nguyªn t¾c lµm viÖc 46 3.1. C¸c bé phËn chñ yÕu cña hép gi¶m tèc BRCL 46 3.2. Nguyªn lý lµm viÖc cña hép gi¶m tèc BRCL 48 IV. D¹ng r¨ng b¸nh r¨ng con l¨n 49 4.1. D¹ng r¨ng l−în sãng 49 4.2. X©y dùng biªn h×nh r¨ng con l¨n 52 4.3. Ph−¬ng ph¸p chän d¹ng r¨ng hîp lý 54 V. Ph−¬ng ph¸p chÕ t¹o BRCL 56 3 VI. LËp tr×nh gia c«ng trªnm¸y c¾t d©y CNC 57 VII. Sö dông BRCL cho m«dun quay Robot lµ gi¶i ph¸p hîp lý nhÊt 58 VIII. ChuÈn hãa thiÕt kÕ hép gi¶m tèc BRCL 59 IX. M«®un quay BRCL 69 X. Mét sè s¶n phÈm ®· chÕ t¹o 72 XI. KÕt luËn 82 PhÇn 3: m«®un d©y chuyÒn s¶n xuÊt tù ®éng 83 I. Giíi thiÖu chung 83 II. HÖ thèng b¨ng chuyÒn 84 III. HÖ thèng ®iÒu khiÓn 98 3.1. Nguyªn t¾c ho¹t ®éng 98 3.2. C¸c bé phËn chñ yÕu 99 PhÇn 4: C¬ së tÝnh to¸n vµ x©y dùng c¸c ch−¬ng tr×nh m¸y tÝnh vµ ®iÒu khiÓn 102 I. Giíi thiÖu chung 102 II. Ch−¬ng tr×nh tù ®éng thiÕt lËp 103 III. Ch−¬ng tr×nh kiÓm nghiÖm lêi gi¶i 105 IV. Ch−¬ng tr×nh phÇn mÒm tÝnh to¸n ®éng lùc häc 107 V. Ch−¬ng tr×nh phÇn mÒm ®iÒu khiÓn SCA 107 VI. Ch−¬ng tr×nh phÇn mÒm ®iÒu khiÓn d©y chuyÒn s¶n xuÊt 109 Tµi liÖu tham kh¶o 131 4 BÁO CÁO CÁC KẾT QUẢ NGHIÊN CỨU THEO NHIỆM VỤ 2 CỦA ĐỀ TÀI KC.03.08 NGHIÊN CỨU THIẾT KẾ CHẾ TẠO NHÓM SẢN PHẨM ROBOT SCA 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. 5 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. 6 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. 7 Hình 2.0.5. Bản vẽ thiết kế Robot SCATM khí nén 8 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 9 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. 10 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. 11 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 12 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 2.1.3. X¸c ®Þnh c¸c ma trËn iA cña robot SCATM 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 ,,0x q 1 0 z 13 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) 14 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: 15 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) 16 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 17 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: 18 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) 19 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) 20 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) 21 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) 22 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 1 0 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 120 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) 23 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) 24 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) 25 ( ) ( )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) 26 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 0 0 0 0 0 0 0 0 0 C S a C S C a S U D A D A A − − −⎡ ⎤⎢ ⎥− −⎢ ⎥= = ⎢ ⎥⎢ ⎥⎣ ⎦ (2.87) ( ) ( ) ( ) ( )1 2 4 1 2 4 2 120 1 2 3 1 2 4 1 2 4 2 12 412 1 1 2 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 q q q q q q a S U D A D A A A ⎡− + − − + − − ⎤⎢ ⎥− + − + − −⎢ ⎥= = ⎢ ⎥⎢ ⎥⎣ ⎦ (2.88) 0 1 2 313 1 1 2 3 3 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 U D A A D A ⎡ ⎤⎢ ⎥⎢ ⎥= = ⎢ ⎥⎢ ⎥⎣ ⎦ (2.89) 0 1 2 3 413 1 1 2 3 3 4 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 U D A A D A A ⎡ ⎤⎢ ⎥⎢ ⎥= = ⎢ ⎥⎢ ⎥⎣ ⎦ (2.90) ( ) ( ) ( ) ( )1 2 4 1 2 40 1 2 3 1 2 4 1 2 4 414 1 1 2 3 4 4 cos sin 0 0 sin cos 0 0 0 0 0 0 0 0 0 0 q q q q q q q q q q q q U D A A A D A ⎡ + − + − ⎤⎢ ⎥+ − − + −⎢ ⎥