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 3

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 SCA
TM
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 SCA
TM
12
2.1. Thiết lập phơng trình động học Robot SCA
TM
12
2.1.1. Xác định các hệ tọa độ của Robot SCA
TM
12
2.1.2. Bảng thông số DH của Robot SCA
TM
12

2.1.3. Xác định các ma trận của Robot SCA
TM
12
2.1.4. Tính các ma trận T của Robot SCA
TM
13
2.1.5. Phơng trình động học Robot SCA
TM
14
2.2. Thiết lập bài toán động học ngợc Robot SCA
TM
15
2.3. Xây dựng mô hình động lực học Robot SCA
TM
19
III. Thiết kế cải tiến chế tạo Robot SCA
TM
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 SCA
TM
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 SCA
TM
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 SCA
TM
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 SCA
TM
khí nén

8

H×nh 2.0.3. Robot SCA mét c¸nh tay m«®un hãa
765
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 sca
Tm
khí nén

i. giới thiệu chung

Nghiờn cu thit k ch to Robot SCA
TM
khớ nộn nõng cao mc
linh hot thao tỏc ca nú nhm ỏp ng c iu khin khụng b tr khi x
lý thụng minh cỏc tớn hiu nhn c t h thng sensor
Phn I bỏo cỏo ny trỡnh by 2 ni dung:
1. Xõy dng cỏc mụ hỡnh ng hc v ng lc hc ca Robot
SCA
TM
2. Thit k ci tin v ch to Robot SCA
TM
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 sca
tm

2.1. Thit lp phng trỡnh ng hc Robot SCA
TM
2.1.1. Xác định các hệ tọa độ của robot SCA
TM









Hình 2.1.1. Hệ toạ độ của robot SCA
TM
2.1.2. Bảng thông số DH của robot SCA
TM
Khâu
i
q

i


i
a

i
d

1
*
1
q

0
1
a

0
2
*
2
q

180
o

2
a

0
3 0 0 0
*
3
d

4
*
4
q

0 0
4
d

2.1.3. Xác định các ma trận
i
A
của robot SCA
TM
d4 cố định
3
d
q
4
x
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


13

11 11
11 11
1
0
0
0010
0001

CS aC
SC aS
A
−
⎡⎤
⎢⎥
⎢⎥
=
⎢⎥
⎢⎥
⎣⎦
(2.1)

22 22
22 22
2
0
0
00 10
0001
CS aC
SC aS
A
⎡⎤
⎢⎥
−
⎢⎥
=
⎢⎥
−

⎢⎥
⎣⎦
(2.2)

3
3
100 0
010 0
001
000 1
A
d
⎡⎤
⎢⎥
⎢⎥
=
⎢⎥
⎢⎥
⎣⎦
(2.3)

44
44
4
4
00
00
0010
000
CS

SC
A
d
−
⎡⎤
⎢⎥
⎢⎥
=
⎢⎥
⎢⎥
⎣⎦
(2.4)
2.1.4. TÝnh c¸c ma trËn
i
T
cña robot SCA
TM


44
44
3
44
4
00
00
0010
000
CS
SC

TA
d
−
⎡⎤
⎢⎥
⎢⎥
==
⎢⎥
⎢⎥
⎣⎦
(2.5)

4
0001
x xx x
yy y y
z zz z
nsa p
nsa p
T
nsa p
⎡⎤
⎢⎥
⎢⎥
=
⎢⎥
⎢⎥
⎣⎦
(2.6)

14

44
44
2
434
43
00
00
001
000 1
CS
SC
TAA
dd
−
⎡⎤
⎢⎥
⎢⎥
==
⎢⎥
+
⎢⎥
⎣⎦
(2.7)


24 24 22
24 24 22

1
4234
43
cos( ) sin( ) 0
sin( ) cos( ) 0
001
0001
qq qq Ca
qq qq Sa
TAAA
dd
−−
⎡⎤
⎢⎥
−− −
⎢⎥
==
⎢⎥
−−−
⎢⎥
⎣⎦
(2.8)

124 124 21211
124 124 21211
1
414
43
cos( ) sin( ) 0
sin( ) cos( ) 0

001
0001
qqq qqq aC aC
qqq qqq aS aS
TAT
dd
+− +− +
⎡⎤
⎢⎥
+− − +− +
⎢⎥
==
⎢⎥
−−−
⎢⎥
⎣⎦
(2.9)
2.1.5. Ph−¬ng tr×nh ®éng häc robot SCA
TM


4
0001
xx x x
yy y y
zz z z
nsa p
nsa p
T
nsa p

⎡⎤
⎢⎥
⎢⎥
=
⎢⎥
⎢⎥
⎣⎦
(2.10)
Tõ ®ã ta cã hÖ ph−¬ng tr×nh sau:

15

124
124
124
124
212 11
212 11
43
cos( )
sin( )
0
sin( )
cos( )
0
0
0
1
x
y

z
x
y
z
x
y
z
x
y
z
nqqq
nqqq
n
sqqq
s qqq
s
a
a
a
paC aC
paS aS
pdd
=+


=+


=

=+


= +


=



=


=

=


=+


=+

=


(2.11)
2.2. Thiết lập bài toán động học ngợc Robot SCA
TM

Nh ó ký hiu trong bi toỏn ng hc thun, ta cú phng trỡnh
sau:
T
n
= A
i

i
T
n
Nhõn hai v ca phng trỡnh ny vi
1
i
A
, nhn 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 1
111
34 3 2 1 4
3
000 1

+++ +

==




xyxy xy x y
xyxyxy x y
zz z z
Cn Sn Cs Ss Ca Sa Cp S p Ca a
Sn Cn Ss Cs Sa Ca Sp Cp Sa
X AAAT
nsa pd

(2.12)

16
12 12 12 12 12 12 12 12 2 1 2
12 12 12 12 12 12 12 12 2 1
11
24 2 1 4
000 1
−−
+++ +−−
⎡⎤
⎢⎥
−−− −−

⎢⎥
==
⎢⎥
−−− −
⎢⎥
⎣⎦
xyxyxy x y
xyxyxy x y
zz z z
Cn Sn Cs Ss Ca Sa C p S p Ca a
Sn Cn Ss Cs Sa Ca S p C p Sa
XAAT
nsa p

(2.13)
11 11 11 1 1 1
11 11 11 1 1
1
14 1 4
000 1
−
++++−
⎡⎤
⎢⎥
−+ −+ −+ −+
⎢⎥
==
⎢⎥
⎢⎥
⎣⎦

xy xy xy x y
xy xy xy x y
zz z z
Cn Sn Cs Ss Ca Sa Cp S p a
Sn Cn Ss Cs Sa Ca S p C p
XAT
nsa p
(2.14)
* T×m
1
q

XÐt phÇn tö hµng 1 cét 4 cña 2 ma trËn
14
X
vµ
1
4
T
tõ (2.14) vµ (2.8)
vµ phÇn tö hµng 2 cét 4 cña 2 ma trËn
14
X
vµ
1
4
T
tõ (2.14) vµ (2.8)

11122

1122
cos( ) sin( ) cos( ) (2.15)
sin( ) cos( ) sin( ) (2.16)
+−=
⎧
⎪
⎨
−+ =
⎪
⎩
xy
xy
pqpqaaq
pqp qaq

2
22
11122
2
22
1122
cos( ) sin( ) cos ( ) (2.17)
sin( ) cos( ) sin ( ) (2.18)
⎧
⎡⎤
+−=
⎪⎣ ⎦
⎨
⎡⎤
⎪

−+ =
⎣⎦
⎩
xy
xy
pqpqaa q
pqp q a q


22 22
111112
(2.17) (2.18) 2 cos( ) 2 sin( )+→+− − +=
xy x y
pp ap q ap qaa
(2.19)

2222
12 1 1 1
2 cos( ) sin( )
xy x y
ppaa ap q p q
⎡ ⎤
++−= +
⎣ ⎦
(2.20)

2222
12
11
22 22 22

1
cos( ) sin( )
2
xy y
x
xy xy xy
ppaa p
p
qq
ap p p p p p
++−
=+
++ +
(2.21)
§Æt

17
22
sin
x
xy
p
pp
ϕ
=
+
(2.22)

22
cos

y
xy
p
pp
ϕ
=
+
(2.23)

Tõ (2.19) ta cã:

()
2222
12
1
22
1
sin
2
xy
xy
ppaa
q
ap p
ϕ
++−
=+
+
(2.24)
()

2
2222
12
1
22
1
cos 1
2
ϕ
⎛⎞
++−
⎜⎟
⇒+=−
⎜⎟
+
⎝⎠
xy
xy
ppaa
q
ap p
(2.25)
2
2222 2222
12 12
1
22 22
11
2,1
22

xy xy
xy xy
ppaa ppaa
qarctg
ap p ap p
ϕ
⎛⎞
⎛⎞
++− ++−
⎜⎟
⎜⎟
+= −
⎜⎟
⎜⎟
++
⎜⎟
⎝⎠
⎝⎠
(2.26)
Tõ (2.22) vµ (2.23)
( )
2,
x y
arctg p p
ϕ
⇒=
(2.27)
Tõ (2.26) vµ (2.27) ta cã:
()
2

2 222 2 222
12 12
1
22 22
11
2,1 ,
22
xy xy
x y
xy xy
ppaa ppaa
qarctg arctgpp
ap p ap p
⎛⎞
⎛⎞
++− ++−
⎜⎟
⎜⎟
=−−
⎜⎟
⎜⎟
++
⎜⎟
⎝⎠
⎝⎠
(2.28)
* T×m q
2
.
Tõ (2.15)

( ) ( ) ( )
11122
cos sin cos
xy
p qp qaa q⇒+−=
(2.29)
()
12
11 2
22 22 22 22
cos( ) sin( ) cos
y
x
xy xy xy xy
p
p
aa
qq q
pp pp pp pp
+−=
++++
(2.30)
§Æt:

18
22
sin
x
xy
p

pp
ϕ
=
+
(2.31)

22
cos
y
xy
p
pp
ϕ
=
+
(2.32)
() ()
12
12
22 22
sin cos
xy xy
aa
qq
pp pp
ϕ
⎡⎤
⎢⎥
⇒+− =
⎢⎥

++
⎣⎦
(2.33)
KÕt hîp víi (2.24), ta ®−îc:
()
22
2222
12
1
2
22 22
2
1
cos
2
⎛⎞
+
++−
⎜⎟
−=
⎜⎟
++
⎝⎠
xy
xy
xy xy
pp
ppaa
a
q

a
ap p p p
(2.34)
()
2 222
12
2
12
cos
2
+−−
⇒=
xy
p paa
q
aa
(2.35)
()
2
2222
12
2
12
1sin
2
⎛⎞
+−−
⇒− =
⎜⎟
⎜⎟

⎝⎠
xy
ppaa
q
aa
(2.36)
2
2222 2222
12 12
2
12 12
21 ,
22
⎡⎤
⎛⎞
+−− +−−
⎢⎥
⇒= −
⎜⎟
⎜⎟
⎢⎥
⎝⎠
⎢⎥
⎣⎦
xy xy
p paa p paa
qarctg
aa aa

(2.37)

* T×m d
3

XÐt phÇn tö hµng 3 cét 4 cña hai ma trËn
14
X
vµ
1
4
T
tõ (2.14) vµ (2.8)

43
=− −
z
p dd
(2.38)
34
⇒=−−
z
dpd
(2.39)
* T×m q
4

XÐt phÇn tö hµng 2 cét 1 cña hai ma trËn
34
X
vµ
3

4
T
tõ (2.12) vµ (2.5)
12 12 4xy
Sn Cn S−=
(2.40)

19
XÐt phÇn tö hµng 1 cét 1 cña hai ma trËn
34
X
vµ
3
4
T
tõ (2.12) vµ (2.5)
12 12 4xy
Cn Sn C+=
(2.41)
( )
444
2,qarctgSC⇒=
(2.42)
2.3. Xây dựng mô hình động lực học Robot SCA
TM

Ta thiÕt lËp c¸c ma trËn
i-1
A
i

Tõ c¸c c«ng thøc (1.22) vµ (1.23) ta cã:
11 11
11 11
0
1
0
0
0010
0001
CS aC
SC aS
A
−
⎡⎤
⎢⎥
⎢⎥
=
⎢⎥
⎢⎥
⎣⎦
(2.43)

22 22
22 22
1
2
0
0
00 10

0001
CS aC
SC aS
A
⎡⎤
⎢⎥
−
⎢⎥
=
⎢⎥
−
⎢⎥
⎣⎦
(2.44)

2
3
3
100 0
010 0
001
000 1
A
d
⎡⎤
⎢⎥
⎢⎥
=
⎢⎥
⎢⎥

⎣⎦
(2.45)
44
44
3
4
4
00
00
0010
000
CS
SC
A
d
−
⎡⎤
⎢⎥
⎢⎥
=
⎢⎥
⎢⎥
⎣⎦
(2.46)
Tõ c«ng thøc (1.21) ta cã:
12 12 2 12 1 1
12 12 2 12 1 1
001
212
0

0
00 1 0
000 1
CS aCaC
SC aSaS
AAA
+
⎡⎤
⎢⎥
−+
⎢⎥
==
⎢⎥
−
⎢⎥
⎣⎦
(2.47)

20

12 12 2 12 1 1
12 12 2 12 1 1
0012
3123
3
0
0
00 1
000 1
CS aCaC

SC aSaS
AAAA
d
+
⎡⎤
⎢⎥
−+
⎢⎥
==
⎢⎥
−−
⎢⎥
⎣⎦
(2.48)

()( )
()()
124 124 21211
00123
124 124 21211
41234
34
cos sin 0
sin cos 0
001
0001
qqq qqq aC aC
qqq qqq aS aS
AAAAA
dd

⎡+− +− +⎤
⎢⎥
+− − +− +
⎢⎥
==
⎢⎥
−−−
⎢⎥
⎣⎦
(2.49)

22 22
22 22
112
323
3
0
0
00 1
0001
CS aC
SC aS
AAA
d
⎡⎤
⎢⎥
−
⎢⎥
==
⎢⎥

−−
⎢⎥
⎣⎦
(2.50)

( ) ( )
()()
24 24 22
1123
24 24 22
4234
34
cos sin 0
sin cos 0
001
0001
qq qq aC
qq qq aS
AAAA
dd
⎡−+ −−+ ⎤
⎢⎥
−−+ − −+
⎢⎥
==
⎢⎥
−−−
⎢⎥
⎣⎦
(2.51)

44
44
223
434
34
00
00
001
000 1
CS
SC
AAA
dd
−
⎡⎤
⎢⎥
⎢⎥
==
⎢⎥
+
⎢⎥
⎣⎦
(2.52)
Tõ c«ng thøc (1.26) vµ (1.27) ta cã:
1
0100
1000
0000
0000
D

−
⎡⎤
⎢⎥
⎢⎥
=
⎢⎥
⎢⎥
⎣⎦
(2.53)

21
2
0100
1000
0000
0000
D
−
⎡⎤
⎢⎥
⎢⎥
=
⎢⎥
⎢⎥
⎣⎦
(2.54)

3
0000
0000

0001
0000
D
⎡⎤
⎢⎥
⎢⎥
=
⎢⎥
⎢⎥
⎣⎦
(2.55)

4
0100
1000
0000
0000
D
−
⎡⎤
⎢⎥
⎢⎥
=
⎢⎥
⎢⎥
⎣⎦
(2.56)

Ta tÝnh
ij

U
theo c«ng thøc (1.31)

11 11
11 11
0
11 1 1
0
0
0000
0000
SC aS
CS aC
UDA
−− −
⎡⎤
⎢⎥
−
⎢⎥
==
⎢⎥
⎢⎥
⎣⎦
(2.57)


12 12 2 12 1 1
12 12 2 12 1 1
0
21 1 2

0
0
000 0
000 0
SC aSaS
CS aCaC
UDA
−−−
⎡⎤
⎢⎥
+
⎢⎥
==
⎢⎥
⎢⎥
⎣⎦
(2.58)


22

12 12 2 12
12 12 2 12
01
22 1 2 2
0
0
0000
0000
SC aS

CS aC
UADA
−−
⎡⎤
⎢⎥
⎢⎥
==
⎢⎥
⎢⎥
⎣⎦
(2.59)


12 12 2 12 1 1
12 12 2 12 2 1
0
31 1 3
0
0
000 0
000 0
SC aSaS
C S aC aC
UDA
−−−
⎡⎤
⎢⎥
+
⎢⎥
==

⎢⎥
⎢⎥
⎣⎦
(2.60)


12 12 2 12
12 12 2 12
01
32 1 2 3
0
0
0000
0000
SC aS
CS aC
UADA
−−
⎡⎤
⎢⎥
⎢⎥
==
⎢⎥
⎢⎥
⎣⎦
(2.61)


02
33 2 3 3

000 0
000 0
000 1
000 0
UADA
⎡⎤
⎢⎥
⎢⎥
==
⎢⎥
−
⎢⎥
⎣⎦
(2.62)

()( )
()()
124 124 21211
0
124 124 21211
41 1 4
sin cos 0
cos sin 0
0000
0000
qqq qqq aS aS
qqq qqq aC aC
UDA
⎡− +− +− − − ⎤
⎢⎥

+− +− +
⎢⎥
==
⎢⎥
⎢⎥
⎣⎦
(2.63)

()( )
()()
124 124 212
01
124 124 212
42 1 2 4
sin cos 0
cos sin 0
0000
0000
qqq qqq aS
qqq qqq aC
UADA
⎡− +− +− − ⎤
⎢⎥
+− +−
⎢⎥
==
⎢⎥
⎢⎥
⎣⎦
(2.64)

23

02
43 2 3 4
000 0
000 0
000 1
000 0
UADA
⎡⎤
⎢⎥
⎢⎥
==
⎢⎥
−
⎢⎥
⎣⎦
(2.65)


()( )
()()
124 124
03
124 124
44 3 4 4
sin cos 0 0
cos sin 0 0
0000

0000
qqq qqq
qqq qqq
UADA
⎡+−−+− ⎤
⎢⎥
−+−−+−
⎢⎥
==
⎢⎥
⎢⎥
⎣⎦
(2.66)
2
11 11
1
11 1
11
00
32
0000
0000
1
00
2
ma ma
J
ma m
⎛⎞
−

⎜⎟
⎜⎟
⎜⎟
=
⎜⎟
⎜⎟
⎜⎟
−
⎜⎟
⎝⎠
(2.67)

2
22 22
2
22 2
11
00
32
0000
0000
1
00
2
ma ma
J
ma m
⎛⎞
−
⎜⎟

⎜⎟
⎜⎟
=
⎜⎟
⎜⎟
⎜⎟
−
⎜⎟
⎝⎠
(2.68)
2
33 33
3
33 3
11
00
32
0000
0000
1
00
2
ml ml
J
ml m
⎛⎞
−
⎜⎟
⎜⎟
⎜⎟

=
⎜⎟
⎜⎟
⎜⎟
−
⎜⎟
⎝⎠
(2.69)

24
2
44 44
4
44 4
11
00
32
0000
0000
1
00
2
ml ml
J
ml m
⎛⎞
−
⎜⎟
⎜⎟
⎜⎟

=
⎜⎟
⎜⎟
⎜⎟
−
⎜⎟
⎝⎠
(2.70)
Tõ c«ng thøc (1.51):
()( )( ) ( )
11 11 1 11 21 2 21 31 3 31 41 4 41
TTTT
D TrU JU TrU JU TrU JU TrU JU=+ ++
22 2
42 41 22 212 2 442 4 441 2 4 412 2
1
=m m cos( ) 2
3
a a ma maaC mlaC mla q q maaC++ + − − −++
222 2 2 2
33 1 2 3 1 2 2 33 2 2 1 3 2 3 1 1 1 33 44
111
2
333
mlaC maaCmlamamama ma ml ml−+ −++++++
(2.71)
( ) ( ) ( )
12 22 2 21 32 3 31 42 4 41
TTT
D TrU J TrU J TrU J=++UUU

2 2
22 221 2 2 33 321 2 1 33 2 2 44 4 33
11 1 1
32 2 3
ma maaC aml maaC amlC amlC ml=+ −+ − − +

()
22 2
44 2 3 1 44 2 4 2 4 1 2 2 4
11
cos
32
ml a m aml q q amaC a m++− −++ +
(2.72)

()( )
13 33 3 31 43 4 41
0
TT
DTrUJ TrUJ=+=UU
(2.73)
()
14 44 4 41
T
DTrUJ= U


()
2

44 44 2 4 44 1 2 4
11 1
cos
32 2
ml mlaC mla q q=+ + −+
(2.74)
()( ) ( )
22 22 2 22 32 3 32 42 4 42
TTT
D TrU J TrU J TrU J=++UUU
222 22
233 23 2 2 2 4 2444 3 3 4 4
111
333
aml am am am amlC lm lm=− + + + − + +
(2.75)
( ) ( )
23 33 3 32 43 4 42
0
TT
D TrU J TrU J=+=UU
(2.76)
()
2
24 33 4 42 4 4 2 4 4 4
11
32
T
DTrUJ lm amlC==−+U
(2.77)

25
( ) ( )
33 33 3 33 43 4 43 3 4
TT
D TrU J TrU J m m=+=+UU
(2.78)
()
34 44 4 33
0
T
DTrUJ==U
(2.79)
()
2
44 44 4 44 4 4
1
3
T
DTrUJ ml==U
(2.80)

()
11 12 13 14
12 22 23 24
12 23 33 34
14 24 34 44
DDDD
DDDD
Dq

DDDD
DDDD
⎡⎤
⎢⎥
⎢⎥
=
⎢⎥
⎢⎥
⎣⎦
(2.81)
11 11
11 11
20
111 1 1
0
0
0000
0000
CS aC
SC aC
UDA
−−
⎡⎤
⎢⎥
−− −
⎢⎥
==
⎢⎥
⎢⎥
⎣⎦

(2.82)
12 12 2 12 1 1
12 12 2 12 1 1
20 1
211 1 1 2
0
0
000 0
000 0
CS aCaC
SC aSaS
UDAA
−− −−
⎡⎤
⎢⎥
−−−
⎢⎥
==
⎢⎥
⎢⎥
⎣⎦
(2.83)
12 12 2 12 1 1
12 12 2 12 1 1
20 1 2
311 1 1 2 3
0
0
000 0
000 0

CS aCaC
SC aSaS
UDAAA
−− −−
⎡⎤
⎢⎥
−−−
⎢⎥
==
⎢⎥
⎢⎥
⎣⎦
(2.84)
( ) ( )
()()
124 124 21211
20 1 2 3
124 124 21211
411 1 1 2 3 4
cos sin 0
sin cos 0
0000
0000
qqq qqq aC aC
qqq qqq aS aS
UDAAAA
⎡− + − − + − − − ⎤
⎢⎥
−+− +− −−
⎢⎥

==
⎢⎥
⎢⎥
⎣⎦

(2.85)