Bài tập lớn ROBOTIC docx
Bạn đang xem bản rút gọn của tài liệu. Xem và tải ngay bản đầy đủ của tài liệu tại đây (2.47 MB, 28 trang )
BTL môn ROBOTICS
1
Mục lục
Chƣơng 1
XÂY DỰNG CẤU TRÚC,THIẾT LẬP HỆ PHƢƠNG TRÌNH ĐỘNG HỌC
ROBOT
1.1.
Xây dựng cấu trúc robot
1.2.
Thiết lập phƣơng trình động học robot
Chƣơng 2
BÀI TOÁN ĐỘNG HỌC
2.1.
Bài toán động học thuận
2.2.
Bài toán động học ngƣợc
Chƣơng 3
TÍNH TOÁN TĨNH HỌC
3.1.
Tính lực dẫn động tại các khớp đảm bảo cân bằng tĩnh
Chƣơng 4
TÍNH TOÁN ĐỘNG LỰC HỌC
4.1
Xây dựng cấu trúc động lực học
4.2
Cơ sở lý thuyết
4.3
Xây dựng bảng tham số động học
4.4
Ma trận jacobi các khâu
4.5
Ma trận khối lƣợng của robot
4.6
Ma trận ly tâm và quán tính coriolits
4.7
Thế năng của robot
4.8
Phƣơng trình vi phân chuyển động của các khâu
Chƣơng 5
CHỌN BỘ ĐIỀU KHIỂN
Phụ lục Code maple
BTL môn ROBOTICS
2
Chƣơng 1
XÂY DỰNG CẤU TRÚC THIẾT LẬP HỆ PHƢƠNG TRÌNH ĐỘNG HỌC
ROBOT
1.1 XÂY DỰNG CẤU TRÚC ROBOT
1.1.1 Đặt hệ quy chiếu
Hình 1.1 Mô hình robot và hệ trục tọa độ
- Hệ trục tọa độ OX
0
Y
0
Z
0
đặt tại khâu đế, trục OZ
0
có hƣớng dọc trục khớp động 1, trục
OX
0
nằm trong mặt phẳng vuông góc với OZ
o
và có hƣớng từ trên xuống, trục OY
0
xác
định theo quy tắc bàn tay phải.
- Hệ trục tọa độ OX
1
Y
1
Z
1
tại khớp động 2, trục OZ
1
đặt dọc trục khớp động 2, trục OX
1
vuông góc với OZ
0
,OZ
1
có hƣớng dọc theo khâu 1, trục OY
1
xác định theo quy tắc bàn
tay phải.
- Hệ trục tọa độ OX
2
Y
2
Z
2
đặt tại trục khớp động 3, trục OZ
2
đặt dọc trục khớp động 3, trục
OX
2
vuông góc với OZ
1
và OZ
2
hƣớng từ OZ
1
sang OZ
2
, trục OY
2
xác định theo quy tắc
bàn tay phải.
- Hệ trục tọa độ OX
3
Y
3
Z
3
đặt tại khâu thao tác, trục OX
3
hƣớng theo hƣớng khâu 3. OZ
3
song song với trục OZ
2
, trục OY
3
xác định theo quy tắc bàn tay phải.
BTL môn ROBOTICS
3
1.1.2 Thiết lập bộ thông số Denavit-Hartenbeg
Từ mô hình và hệ trục tọa độ ở trên ta xây dựng đƣợc bảng thông số Danavit-
Hartenbeg nhƣ sau :
Khâu
θ
i
α
i
a
i
d
i
1
θ
1
90
0
a
1
d
1
2
θ
2
0
a
2
0
3
θ
3
0
a
3
0
Trong đó:
θ
i
là góc quay quanh Z
i-1
đển biến X
i-1
thành X
i
α
i
là góc quay quanh X
i
để biến Z
i-1
thành Z
i
Các biến khớp là θ
1
, θ
2
, θ
3
, đặt các biến khớp tƣơng ứng là q
1
,q
2
,q
3
.
Các ma trận biến đổi tọa độ thuần nhất Denavit-Hartenbeg dựa vào bộ thông số
trên :
1000
010
)sin()cos(0)sin(
)cos()sin(0)cos(
1
1111
1111
1
0
d
qaqq
qaqq
A
(1.1)
1000
0100
)sin(0)cos()sin(
)cos(0)sin()cos(
2222
2222
2
1
qaqq
qaqq
A
(1.2)
1000
0100
)sin(0)cos()sin(
)cos(0)sin()cos(
3333
3333
3
2
qaqq
qaqq
A
(1.3)
Bảng 1.1: Bộ thông số Denavit-Hartenbeg
BTL môn ROBOTICS
4
1.2 THIẾT LẬP PHƢƠNG TRÌNH ĐỘNG HỌC ROBOT
Phƣơng trình động học robot nhận đƣợc trong dạng ma trận nhƣ sau :
)()(
3
0
3
0
tAqA
(1.4)
Trong đó
1000
0
)(
1222332323
1121223131231231
1121223131231231
3
2
2
1
1
0
3
0
dSaSaCS
SaCSaCSaCSSCS
CaCCaCCaSSCCC
AAAqA
(1.5)
Trong đó C
1
,C
2
,S
1
,S
2
,C
23
và S
23
lần lƣợt là viết tắt của cos(q
1
), cos(q
2
), sin(q
1
), sin(q
2
),
cos(q
2
+q
3
), sin(q
2
+q
3)
1000
),,(),,(),,(
),,(),,(),,(
),,(),,(),,(
)(
333231
232221
131211
3
0
zeccc
yeccc
xeccc
tA
Trong đó c
ij
(α,β,ɳ) là các phần tử trong ma trận Cardan
)cos()cos()cos(sinsin)sin()cos())sin()cos()cos()sin()cos(
)cos()sin()cos(cossin)sin()sin()sin()cos()cos()sin()sin(
)sin()sin()cos()cos()cos(
cd
R
Ta có phƣơng trình dạng ma trận nhƣ sau:
1000
),,(),,(),,(
),,(),,(),,(
),,(),,(),,(
333231
232221
131211
zeccc
yeccc
xeccc
1000
0
1222332323
1121223131231231
1121223131231231
dSaSaCS
SaCSaCSaCSSCS
CaCCaCCaSSCCC
(1.6)
BTL môn ROBOTICS
5
Chƣơng 2
BÀI TOÁN ĐỘNG HỌC
2.1 BÀI TOÁN ĐỘNG HỌC THUẬN
Xây dựng quy luật chuyển động, vị trí khâu thao tác và ma trận chỉ hƣớng
Chọn thông số chiều dài các khâu nhƣ sau:
d1=100 mm, a1=200 mm ,a2=200 mm , a3 = 200mm
Và chọn quy luật chuyển động các khâu nhƣ sau:
1
4
1
3
2
3
2
2
1
2
1
1
8
1
3
3
1
22
4
1
1
2
2
2
tq
tq
tq
ttq
ttq
ttq
với 0(s)≤t≤5(s) (2.1)
Đồ thị sự biến đổi của các biến khớp:
Đồ thị q
1
(t)
BTL môn ROBOTICS
6
Đồ thị q
2
(t)
Đồ thị q
3
(t)
BTL môn ROBOTICS
7
Từ phƣơng trình 1.6 ta có :
(2.2)
Thay các giá trị của biến vào ta có:
Hƣớng của bàn kẹp có thể đƣợc xác định từ các góc Cardan, ký hiệu tƣơng ứng là α, β, γ
quay lần lƣợt quanh các trục x-y-z.
)cos()cos()cos(sinsin)sin()cos())sin()cos()cos()sin()cos(
)cos()sin()cos(cossin)sin()sin()sin()cos()cos()sin()sin(
)sin()sin()cos()cos()cos(
cd
R
để tính đƣợc các góc α, β, η ta so sánh ma trận chỉ hƣớng của (1.5) và ma trận chỉ
hƣớng của (1.6) giải các hệ phƣơng trình ta có :
122233
112122313
112122313
dSaSaz
SaCSaCSay
CaCCaCCax
E
E
E
BTL môn ROBOTICS
8
2
3
,
2
khi
arctan
,,,,
,,
arctan
arctan
,,
,,
arctan
arctan
,,
,,
arctan
2
3
,
2
khi
arctan
,,,,
,,
arctan
arctan
,,
,,
arctan
arctan
,,
,,
arctan
2
231
2
231
23
2
12
2
11
13
231
231
11
21
1
1
31
32
2
231
2
231
23
2
12
2
11
13
231
231
11
21
1
1
31
32
CSSC
S
cc
c
CC
SC
c
c
S
C
c
c
CSSC
S
cc
c
CC
SC
c
c
S
C
c
c
Tính vận tốc điểm tác động cuối E, vận tốc góc khâu thao tác
Từ phần trên ta đã xây dựng đƣợc quy luật chuyển cũng nhƣ tìm đƣợc tọa độ của
khâu thao tác cuối, các biến khớp và đạo hàm các cấp theo t đã biết :
T
qqqq ],,[
321
],,[
321
qqqq
T
Vận tốc góc của khâu thao tác:
A
3
=
10
3
2
2
1
1
EE
rR
AAA
(2.3)
Vận tốc của khâu thao tác chính là đạo hàm vị trí khâu thao tác theo thời gian:
V
E
=
r
E
=
T
EEE
zyx
,,
22232233
11122112123223112313
11122112123223112313
)(
)(
)(
qCaqqCazV
qCaqSSqCCaqqSSqCCayV
qSaqSCqCSaqqSCqCSaxV
EEz
EEy
EEx
(2.4)
Thay (2.1) vào (2.4) ta tìm đƣợc vấn tốc của các khâu thao tác cuối.
Vận tốc góc của khâu thao tác:
0
0
0
.
~
xy
xz
yz
T
EEE
RR
BTL môn ROBOTICS
9
0
23232323
11232311231232312311
11231232311232311231
qSqC
qSqCSqSCqSSCCq
qCCCqSSqqSCqCS
0
11
23231231
23231231
CS
CSSSC
SCSCC
0
)(0
)(0
13121312
1321
1321
SqSqCqCq
Sqqq
Cqqq
(2.5)
Suy ra vận tốc góc khâu thao tác:
T
T
E
qCqqSqq ])()[(
1132132211332
2
1
2
132
2
132
2
1
1
)
4
1
cos()
12
11
3()(
)
4
1
sin()
12
11
3()(
tq
tttCqq
tttSqq
z
y
x
(2.6)
Ứng dụng phần mềm Matlab, Maple vẽ quỹ đạo chuyển động của khâu thao
tác cuối
Quỹ đạo điểm khâu thao tác.
Sử dụng phần mềm Maple ta vẽ đƣợc đồ thị quỹ đạo chuyển động của khâu thao
tác cuối nhƣ sau :
Chuyển động điểm cuối E theo phương X
BTL môn ROBOTICS
10
Chuyển động điểm cuối E theo phương Y
Chuyển động điểm cuối theo phương Z
2.2 BÀI TOÁN ĐỘNG HỌC NGƢỢC
Bài toán động học ngƣợc thông thƣờng cho biết trƣớc vị trí của khâu thao tác yêu
cầu tìm giá trị các biến khớp ứng với vị trí đó. Ở tiểu luận này robot 3 bậc tự do kiểu RRR
ta không cần biết hƣớng của khâu thao tác mà vẫn có thể tìm đƣợc các góc quay tƣơng
ứng.
BTL môn ROBOTICS
11
2.2.1 Xây dựng quy luật chuyển động của khâu thao tác cuối
Ta chọn quy luật chuyển động bất kì của khâu thao tác E của robot nhƣ sau:
tz
ty
tx
E
E
E
30100
100
60600
(2.7)
2.2.2 Khảo sát bài toán động học ngƣợc của robot tìm quy luật chuyển động của các
khâu
Ta có phƣơng trình ma trận (xem 1.4):
)()(
3
0
3
0
tAqA
Từ (2.2) kết hợp với (2.7) ta có hệ ba phƣơng trình vị trí :
30100
100
60600
122233
112122313
112122313
tzdSaSa
tySaCSaCSa
txCaCCaCCa
E
E
E
(2.8)
Dựa vào cấu tạo hình học của robot ta xác định đƣợc q1 nhƣ sau:
t
t
X
Y
q
X
Y
q
E
E
E
E
60600
100
arctanarctan
)tan(
1
1
Nhân phƣơng trình 1 với C
1
và phƣơng trình 2 với S
1
ta đƣợc phƣơng trình:
11122233
100)60600( tSCtaCaCa
(2.9)
Đặt:
1
111
30100
100)60600(
dtP
atSCtP
y
x
Kết hợp với phƣơng trình 3 của hệ (2.8) và phƣơng trình (2.9) ta đƣợc hệ phƣơng
trình sau:
y
x
PSaSa
PCaCa
22233
22233
(2.10)
Bình phƣơng hai vế của hai phƣơng trình (2.10) sau đó cộng hai phƣơng trình lại
với nhau ta đƣợc phƣơng trình :
BTL môn ROBOTICS
12
22
332
2
3
2
2
22
22323223223232
2
3
2
2
22
22322332
2
3
2
2
2
)(2
)(2
yx
yx
yx
PPCaaaa
PPSCSSCSCSSCCCaaaa
PPSSCCaaaa
32
2
3
2
2
22
3
2 aa
aaPP
C
yx
(2.11)
Mặt khác ta có :
2
33
1 CS
Vậy ta tính đƣợc q
3
:
)
322
32
(1artan
22
2
33
aa
aaPP
Cq
yx
(2.12)
Từ hệ phƣơng trình (2.10) thay C
3
và S
3
vào ta có hệ phƣơng trình sau:
y
x
PaCaSSaC
PSSaaCaC
)(
)(
2332332
3232332
(2.13)
Giải hệ phƣơng trình trên ta có nghiệm nhƣ sau:
323
2
2
2
3
33332
2
323
2
2
2
3
33332
2
2
2
Caaaa
SaPCaaP
S
Caaaa
SaPCaaP
C
xy
yx
(2.14)
Vậy ta tính đƣợc q
2
:
323
2
2
2
3
33332
323
2
2
2
3
33332
2
22
artan
Caaaa
SaPCaaP
Caaaa
SaPCaaP
q
yxxy
(2.15)
BTL môn ROBOTICS
13
Chƣơng 3
TÍNH TOÁN TĨNH HỌC
3.1 Tính lực dẫn động tại các khớp đảm bảo cân bằng tĩnh
Theo đầu bài ta có các lực tác dụng vào khâu thao tác tại điểm E gồm các vector lực F
E3
,
và mô men M :
00
00
2
r 00
2
r 00
2
r 00r
00ar ]00a[r M
T
1
C1
1
T
2
C2
2
T
3
C3
3
T
11
1
T
22
2T
33
3
T
E33
gmP
aa
a
a
MMMFFFF
ii
T
zyx
T
ZYXE
Tính lực và momen của khâu 3 tác dụng lên khâu 2 tại khớp 3
Hệ phƣơng trình cân bằng dạng mà trận khảo sát trong hệ tọa độ cơ sở :
3
0
3
0
2,3
0
3
0
3,
0
2,3
0
3
0
3,
0
2,3
0
~~
PrFrMM
PFF
cE
E
BTL môn ROBOTICS
14
0
0
~~
0
0
33
0
33
0
2,3
0
332,3
0
gmr
F
gmF
F
r
Mz
My
Mx
M
F
gmF
F
gm
F
F
F
F
c
z
y
x
z
y
x
z
y
x
(3.1)
Trong đó :
23
3
231
3
231
3
3
3
3
0
3
0
233
2313
2313
3
3
3
0
3
0
2
2
2
và
S
a
CS
a
CC
a
rRr
Sa
SSa
CCa
rRr
cc
(3.2)
Chú ý :
3
2
2
1
1
0
3
0
RRRR
=
0
2323
1231231
1231231
CS
CSSCS
SSCCC
Thay (3.2) vào (3.1) ta tìm đƣợc
2,3
0
M
:
0
0
0
22
2
0
2
22
0
0
0
0
~~
0
0
3
231
3
231
3
231
3
23
3
231
3
23
3
3
23132313
2313233
2313233
3
0
3
0
2,3
0
3
0
3,
0
2,3
0
332,3
0
gm
CC
a
CS
a
CC
a
S
a
CS
a
S
a
F
gmF
F
CCaSSa
CCaSa
SSaSa
M
M
M
PrFrMM
F
gmF
F
gm
F
F
F
F
z
y
x
z
y
x
cE
z
y
x
z
y
x
2313
3
231332313
2313233
233323132333
2,3
32,3
0
2
)(
2
1
)(
CgCm
a
CCgmFyaCFzSaMz
CFzSaFxSaMy
gSmaCFzSaSgmFyaMx
M
F
gmF
F
F
z
y
x
(3.3)
BTL môn ROBOTICS
15
Tính lực và momen của khâu 2 tác dụng lên khâu 1 tại khớp 2
Hệ phƣơng trình cân bằng dạng mà trận khảo sát trong hệ tọa độ cơ sở :
2
0
2
0
1,2
0
2
0
2,3
0
1,2
0
2
0
2,3
0
1,2
0
~~
PrFrMM
PFF
c
(3.4)
Trong đó:
0100
0
0
010
0
0
22
12121
12121
22
22
11
11
2
1
1
0
2
0
CS
CSSCS
SSCCC
CS
SC
CS
SC
RRR
2
2
21
2
21
2
2
2
2
0
2
0
22
212
212
2
2
2
0
2
0
2
2
2
và
S
a
CS
a
CC
a
rRr
Sa
CSa
CCa
rRr
cc
(3.5)
Thay (3.5) và (3.3) vào (3.4) ta đƣợc:
2122213222122313332313323132313
212222313232
2222123222323323132333
1,2
0
231,2
0
2
1
2
1
)(
2
1
)(
2
1
CgCmaCCgmgmFyaCSaFCgCmagmFCgCmaCFyCaCFxSaMz
CFzCaFxSaCFzCaFxSaMy
gmSaCFzSagmgmFSagmSaCFzSaSgmFaMx
M
F
gmmF
F
F
xy
yy
z
y
x
Tính lực và momen của khâu 1 tác dụng lên khâu 0 đế tại khớp 1
Hệ phƣơng trình cân bằng dạng mà trận khảo sát trong hệ tọa độ cơ sở :
1
0
1
0
0,1
0
1
0
1,2
0
0,1
0
1
0
1,2
0
0,1
0
~~
PrFrMM
PFF
c
(3.7)
Trong đó :
BTL môn ROBOTICS
16
010
0
0
11
11
1
0
CS
SC
R
0
2
2
và
0
1
1
1
1
2
2
2
0
2
0
11
11
1
1
1
0
1
0
S
a
C
a
rRrSa
Ca
rRr
cc
(3.8)
Thay (3.8) và (3.6) vào (3.7) ta đƣợc hệ phƣơng trình:
0
1
2
1
321
0
0
1,2
0
0,1
0
1231230,1
0
Fxd
g
m
mmFyd
MM
F
gmmmF
F
gm
F
gmmF
F
F
z
y
x
z
y
x
213
2
2123132312310,1
0
212231230,1
0
21
3
2
22312330,1
0
1230,1
0
2
2
2
2
3
33
12232
2
1
3212
2
23
2
3
3
CCgm
gm
Fya
CSFxaCgCm
a
CFyCaCFxSaMzzM
FxdCFzCaFxSaCFzCaFxSaMyyM
g
m
mmFydCFzSa
Fygm
gm
SaCFzSaS
a
gmFyaMxxM
F
gmmmF
F
F
z
y
x
(3.9)
BTL môn ROBOTICS
17
Chƣơng 4
TÍNH TOÁN ĐỘNG LỰC HỌC
4.1 XÂY DỰNG CẤU TRÚC ĐỘNG HỌC
Vì các khâu coi nhƣ thanh đồng chất tiết diện ngang không đáng kể nên ta có trọng tâm
mỗi khâu nằm tại trung điểm của nó.
4.2 Cơ sở lý thuyết
Động năng của robot có dạng:
11
( ) ( , )
22
TT
T q M q q q b q q
Trong đó :
( , )b q q
=
()M q q
,
1
T
n
b b b
Phƣơng trình Lagrange loại II:
BTL môn ROBOTICS
18
( ) ( )
( ) ( )
T T T
T
T
T
d T T
dt q q q
TT
q M q M q q
dT
M q q M q q
dt q
Sử dụng định lý đạo hàm riêng theo vector tích của hai ma trận ta có:
11
( ) ( )
22
T
TT
n
T q b
q b b I q
q q q q
(4.1)
Trong đó:
1
1 1 1
.
( )
.
n
T
T T T T
n n n n n n
nn
bI
q
b I e e be I b e I
q
bI
1
( ) ( )
T
TT
n
b b b M q q q M q
(4.2)
Mặt khác:
q
q
qMIq
q
qM
qqqM
q
q
q
b
q
n
TTT
0 ( ) ( )
TT
n
q M q I q M q
(4.3)
Thay (2) và (3) vào (1) ta đƣợc:
( ) ( )
T
T
TT
q M q M q q
,
( ) ( )
T
dT
M q q M q q
dt q
(4.4)
Tính toán tƣơng tự ta có:
1 1 1
( ) ( ) 0
2 2 2
1 1 ( )
( ) ( ) ( )
22
1 ( ) 1 ( )
( ) 0 ( )
22
T
T T T
n
TT
n
TT
nn
T q b b
q b b I q q
q q q q q
M q q
q M q q q q I M q
q q q
M q M q
q q I q q I
(4.5)
Thay (4) và (5) vào phƣơng trình lagrange II ta đƣợc:
BTL môn ROBOTICS
19
1 ( )
( ) ( ) ( )
2
()
( ) ( )
TT
T
n
n
Mq
M q q M q q q q I
Mq
M q I q
q
Đặt :
( ) 1 ( )
( , ) ( ) ( ) ( , )
2
T
nn
M q M q
v q q I q q I q C q q q
Ta có:
1 ( )
( , ) ( ) ( )
2
T
n
Mq
v q q M q q q I q
q
(4.6)
Theo định lý đạo hàm toàn phần và đạo hàm riêng ta có:
()
( ) ( )
n
Mq
M q I q
q
Thay vào (6) ta đƣợc:
( ) 1 ( )
( , ) ( ) ( ) ( , )
2
( ) 1 ( )
( , ) ( ) ( )
2
T
nn
T
nn
M q M q
v q q I q q I q C q q q
M q M q
C q q I q q I
Ma trận ly tâm và coriolis có dạng:
( ) 1 ( )
( , ) ( ) ( )
2
T
nn
M q M q
C q q I q q I
Khi đó phƣơng trình vi phân chuyển đông của các khâu :
( ) ( , )
T
M q q C q q q
q
4.3 Xây dựng bảng tham số động lực học
Khâu
Vị trí trọng tâm
Khối
lƣợng
Ma trận mômen quán tính
C
x
C
y
C
z
xx
I
yy
I
zz
I
xy
I
yz
I
zx
I
1
2
1
a
0
0
1
m
x
I
1
y
I
1
z
I
1
0
0
0
2
2
2
a
0
0
2
m
x
I
2
y
I
2
z
I
2
0
0
0
3
2
3
a
0
0
3
m
x
I
3
y
I
3
z
I
3
0
0
0
Bảng 4.1 Bảng mô tả vị trí trọng tâm khối lượng và mô men quán tính khối của từng khâu
BTL môn ROBOTICS
20
4.3 Ma trận Jacobi của các khâu
Tạo độ trọng tâm của khâu i trong hệ tọa độ 0 tính nhƣ sau :
ci
i
iici
rRrr .
000
Với
ci
r
0
là tọa độ trọng tâm khâu i trong hệ tọa độ i
ci
i
r
là tọa độ trọng tâm khâu i trong hệ tọa độ i
i
R
0
là ma trận quay biến đổi hệ 0 thành hệ i
i
r
0
là tọa độ của gốc tọa độ i trong hệ tọa độ 0
Ta có các ma trận tọa độ trọng tâm của các khâu nhƣ sau :
1
1
1
1
1
11
11
1
11
11
1
0
)1sin(
2
)cos(
2
0
0
2
010
0
0
)sin(.
)cos(.
d
q
a
q
a
a
CS
SC
d
qa
qa
r
C
(4.7)
12
2
212
1121
22
22
12121
12121
122
11212
11212
2
0
2
2
0
0
2
0
dS
a
CSl
CaCC
aa
CS
CSSCS
SSCCC
dSa
SaCSa
CaCCa
r
C
(4.8)
12223
3
11231
3
212
11212231
3
3
2323
1231231
1231231
123322
112313212
112313212
3
0
2
2
2
0
0
2
0
dSaS
a
SaCS
a
CSa
CaCCqCC
a
a
CS
CSSCS
SSCCC
dSaSa
SaCSaCSa
CaCCaCCa
r
C
Từ (4.7) (4.8) và (4.9) ta có ma trận Jacobi tịnh tiến của các khâu :
000
00
2
00
2
1
1
1
1
1
1
C
a
S
a
q
r
J
C
T
(4.10)
0
2
1
0
0
2
1
2
1
0
2
1
2
1
22
21211212
21211212
1
2
Ca
SSaaCCCa
SCaaSCSa
q
r
J
C
T
(4.11)
BTL môn ROBOTICS
21
23323322
23132313212112313212
23132313212112313212
1
3
2
1
2
1
0
2
1
2
1
2
1
2
1
2
1
2
1
CaCaCa
SSaSSaSSaaCCCaCCa
SCaSCaSCaaCCSaCSa
q
r
J
C
T
(4.12)
Cũng từ các ma trận Denanvit- Hartenberg ta xác định đƣợc các ma trận cosin chỉ hƣởng
của các khâu (xem 1.1, 1.5, 3.5) :
Ma trận cosin chỉ hƣớng của khâu 1 :
010
0
0
11
11
1
0
CS
SC
R
Ma trận cosin chỉ hƣớng của khâu 2 :
0
22
12121
12121
2
0
CS
CSSCS
SSCCC
R
Ma trận cosin chỉ hƣớng của khâu 3:
0
2323
1231231
1231231
3
0
CS
CSSCS
SSCCC
R
Toán tử sóng của vector vận tốc góc của khâu 1:
000
00
00
~
1
.
1
11
`
1
q
q
RR
T
Suy ra vận tốc góc và ma trận Jacobian của khâu 1 :
qJq
R
T
11
`
1
00
~
001
000
000
1
1
q
J
R
(4.13)
Toán tử sóng của vector vận tốc góc của khâu 2:
0
0
0
~
1212
121
121
22
`
2
SqCq
Sqq
Cqq
RR
T
BTL môn ROBOTICS
22
Suy ra vận tốc góc và ma trận Jacobian của khâu 2 :
qJqCqSq
R
T
211212
`
2
~
001
00
00
1
1
2
2
C
S
q
J
R
(4.14)
Toán tử sóng của vector vận tốc góc của khâu 3:
0
)(0
)(0
~
13121312
1321
1321
33
`
3
SqSqCqCq
Sqqq
Cqqq
RR
T
Suy ra vận tốc góc và ma trận Jacobian của khâu 3 :
qJqCqqqqS
R
T
31132321
`
3
)()(
~
001
0
0
11
11
3
3
CC
SS
q
J
R
(4.15)
4.5 Ma trận khối lƣợng suy rộng của robot
3
1
()
T T T
i Ti Ti Ri i i i Ri
i
M m J J J A I A J
=
11 12 13
21 22 23
31 32 33
m m m
m m m
m m m
Trong đó:
m
12
=m
13
=m
31
=m
21
=0
BTL môn ROBOTICS
23
4.6 Ma trận ly tâm và coriolis :
( ) 1 ( )
( , ) ( * ) ( * )
2
T
nn
M q M q
C q q I q q I
=
11 12 13
21 22 23
31 32 33
c c c
c c c
c c c
3
2
1
).,(
c
c
c
qqqC
Sử dụng phần mềm Maple nhân ra ta đƣợc :
Trong đó :
ii
qdiffq
BTL môn ROBOTICS
24
4.7 Thế năng của robot
1 1 2 1 2 2 3 1 2 2 3 23
( ) ( ) m gd m g d l S m g d l S l S
Từ đó suy ra :
3
2
1
g
g
g
q
g
Trong đó :
4.8 Phuơng trình vi phân chuyển động của các khâu
Thế các biểu thức vào phƣơng trình Lagrange loại hai :
( ) ( , ) ( )M q q C q q q g q
Ta nhận đƣợc hệ phƣơng trình vi phân chuyển động của robot ba khâu trong không gian :
Khâu 1 :
BTL môn ROBOTICS
25
Khâu 2 :
Khâu 3 :
Trong đó
qdiffq
qqd
i
ii
2
