Tài liệu BÀI TẬP MÔN HỌC ROBOTICS 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 (278.33 KB, 27 trang )
BÀI TẬP MÔN HỌC ROBOTICS
GVHD: PGS-TS Phan Bùi Khôi SVTH Bùi Xuân Tới Page 1
MỤC LỤC
I – XÂY DỰNG CẤU TRÚC, THIẾT LẬP PHƯƠNG TRÌNH ĐỘNG HỌC
ROBOT 2
1 - Phân tích mô hình robot – xây dựng cấu trúc động học robot 2
2 – Thiết lập hệ phương trình động học của robot 3
II- BÀI TOÁN ĐỘNG HỌC THUẬN ROBOT 6
1 – Cho quy luật chuyển động 6
2. xác định quy luật chuyển động khâu cuối 6
III – TÍNH TOÁN TĨNH HỌC 11
1 -
Xác định các ma trận chỉ hướng R 11
2 - xác định các vectơ r , các ma trận sóng 13
3 -Tính toán lực và momen tại các khớp 15
IV TÍNH TOÁN ĐỘNG LỰC HỌC 17
Các tham số động lực học của robot 17
2 – Tính toán động năng, thế năng của robot 18
3, Tính thế năng của robot 22
4 - Thiết lập phương trình vi phân chuyển động- Phương trình lagarange 23
BÀI TẬP MÔN HỌC ROBOTICS
GVHD: PGS-TS Phan Bùi Khôi SVTH Bùi Xuân Tới Page 2
I – XÂY DỰNG CẤU TRÚC, THIẾT LẬP PHƯƠNG TRÌNH ĐỘNG HỌC
ROBOT
1 - Phân tích mô hình robot – xây dựng cấu trúc động học robot
Mô hình robot cho trước
l
Từ mô hình ta thấy robot có 4 khâu với 3 bậc tự do:
Khâu 0 – khâu đế robot
Khâu 1 – là khâu tịnh tiến , khớp tịnh tiến ở 1 đầu khâu và trục khớp tịnh tiến trùng với trục
của khâu đế
Y
0
Z
0
X
1
Z
1
Y
1
X
2
Z
2
Y
2
X
3
Z
3
Y
3
BÀI TẬP MÔN HỌC ROBOTICS
GVHD: PGS-TS Phan Bùi Khôi SVTH Bùi Xuân Tới Page 3
Khâu 2: 1 một khâu động, được gắn khớp bản lề (khớp quay) để có khả năng quay quanh
đầu còn lại của khâu 1, trục quay song song với trục của khâu đế với 1 góc quay giới hạn (
nhỏ hơn 2π)
Khâu 3: khâu cuối (khâu thao tác) với 1 đầu được gắn các cơ cấu thao tác để thực hiện
nhiệm vụ của robot, đầu còn lại được gắn 1 khớp quay, có khả năng quay quanh điểm cuối
của khâu 2 và trục quya vuông góc với trục quya khâu 2.
Từ các phân tích trên, ta xây dựng được cấu trúc động học của robot để phụ vụ cho quá
trình nghiên cứu như sau:
Với mô hình thiết kế như trên ta có các kích thước các khâu như sau:
Khâu 0: cao 1,5 m , độ dài đoạn dẫn hướng là 1,2m
Khâu 1: dài 0,6m
Khau2: dài 0,75m
Khâu 3 dài 0,6 m
Gắn vào robot các hệ tọa độ như sau:
2 – Thiết lập hệ phương trình động học của robot
a, Ma trận biến biến đổi hệ tọa độ thuần nhất biểu diễn trạng thái khâu cuối so với hệ tọa độ
cơ sở O
0
X
0
Y
0
Z
0
nếu thông qua việc tịnh tiến gốc tọa độ 1 lần và sử dụng phép quay theo
các góc CacDan sẽ có dạng như sau:
BÀI TẬP MÔN HỌC ROBOTICS
GVHD: PGS-TS Phan Bùi Khôi SVTH Bùi Xuân Tới Page 4
0
A
E
(p)=
cos β cos η - cos β sin η sin β x
E
sin αsin β cos η + cos αsin η - sin α sin β sin η + cosα cos η-sin α cos βy
E
- cos α sin β cos η + sin α sin η cos α sin β sin η +sin α cosη cos α cosβ z
E
0 0 0 1
b – Thiết lập các ma trận biến đổi tọa độ thuần nhất cho các khâu theo
Denavit _ Hartenberg
Từ việc phân tích cấu trúc động học robot và gắn vào robot các hệ tọa độ như hình vẽ ban
đầu, ta xây dựng được bảng DH , thể hiện các phép biến hệ tọa độ cho các khâu:
Joint d
i
θ
i
a
i
i
1 d
1
0 a
1
0
2 0 θ
2
a
2
π/2
3 0 θ
3
a
3
0
Các ma trận biến đổi hệ tọa độ thuần nhất theo Denavit - Hartenberg như sau:
BÀI TẬP MÔN HỌC ROBOTICS
GVHD: PGS-TS Phan Bùi Khôi SVTH Bùi Xuân Tới Page 5
Ma trận biến đổi hệ tọa cho khâu cuối theo Denavit – Hartenberg :
!"
Phương trình động học robot
#
$)
Từ đây ta rút ra được hệ phương trình động học robot như sau:
%
&
&
'
&
&
(
)
)
*
*
+
+
,
,-,.
,
,
,-,.
,
,
,-,.
,
BÀI TẬP MÔN HỌC ROBOTICS
GVHD: PGS-TS Phan Bùi Khôi SVTH Bùi Xuân Tới Page 6
%
&
&
&
'
&
&
&
(
)
/
0
/
0
0
*
/
0
/
0
0
+
/
/
-
.
0
/
-
0
-
II- BÀI TOÁN ĐỘNG HỌC THUẬN ROBOT
1 – Cho quy luật chuyển động
Để nghiên cứu bài toán động học thuận robot, ta xét 1 trường hợp các khâu chuyển động
theo quy luật tuần hoàn:
Khâu 1: chuyển động tịnh tiến:
1
2
3
45
6
7
Khâu 2 : chuyển động quay với
1
4
3
45
6
89:
Khâu 3 : chuyển động quay với
1
4
3
45
6
89:
2. xác định quy luật chuyển động khâu cuối
Từ quy luật chuyển động của các khâu như trên ta xác định được vận tốc các khâu theo thời
gian:
;
1
2<
=
>?
@
A "
B
;
1
>
0
=
>?
@
C "
B
;
1
>
/
=
>?
@
C "
B
BÀI TẬP MÔN HỌC ROBOTICS
GVHD: PGS-TS Phan Bùi Khôi SVTH Bùi Xuân Tới Page 7
;
D
;
;
;
E
Ma trận tọa độ điểm cuối E:
C
F
)
*
+
G
F
G
Ma trận Jacobi tịnh tiến cho điểm tác động cuối:
HIJ
K
!"
L
Vận tốc điểm cuối
M
NOP;
K
!"
L
K
;
;
;
L
K
"QR
!"
"QR
;
!"
"QR
;
!"
!"
!"
;
"QR
"QR
;
;
!"
;
L
BÀI TẬP MÔN HỌC ROBOTICS
GVHD: PGS-TS Phan Bùi Khôi SVTH Bùi Xuân Tới Page 8
Với
%
&
&
&
&
'
&
&
&
&
(
1
2
3
45
6
7
1
4
3
45
6
89:
1
4
3
45
6
89:
;
1
2<
3
45
6
7 "
B
;
1
4
S
3
45
6
C"
B
;
1
4
S
3
45
6
C"
B
Ta có thể vẽ được sự phụ thuộc của xE, yE, zE theo thời gian và quỹ đạo điểm tác động
cuối như sau:
Đồ thị x-t
BÀI TẬP MÔN HỌC ROBOTICS
GVHD: PGS-TS Phan Bùi Khôi SVTH Bùi Xuân Tới Page 9
Đồ thị y-t
Đồ thị z-t
BÀI TẬP MÔN HỌC ROBOTICS
GVHD: PGS-TS Phan Bùi Khôi SVTH Bùi Xuân Tới Page 10
Đồ thị oxyz
Ma trận xác định hướng khâu cuối:
T
K
L
T
;
D
;
;
;
!"
;
;
;
;
;
;
;
;
;
E
BÀI TẬP MÔN HỌC ROBOTICS
GVHD: PGS-TS Phan Bùi Khôi SVTH Bùi Xuân Tới Page 11
Ta có vận tốc góc khâu cuối
UVT
;
T
W
D
U
X
U
Y
U
X
U
Z
U
Y
U
Z
E
D
;
;
;
;
;
;
E
=> ta có W= [
;
,
;
,
;
]
T
III – TÍNH TOÁN TĨNH HỌC
Lực tác dụng vào khâu thao tác tại điểm E gồm có
F
E
= (F
X,
F
Y
, F
Z
)
T
M
E
= (M
X,
M
Y
, M
Z
)
T
Ta cần tính toán lực dẫn động các khâu để robot cân bằng,
Ta có hệ phương trình cần bằng lực tác dụng lên tất cả các khâu của robot
[
T
\
]
^_
`
]
_,_a
`
]
_,_b
c
\
]
_
d
\
\
]
^_
d
\
\
]
_,_a
d
\
\
]
_,_b
C]
_
e`
]
_,_a
C]
_
ec
\
]
_
Trong dạng ma trận
f
T
^_
`
_,_a
`
_,_b
c
_
d
^_
d
_,_a
d
_,_b
Cg
_
e`
_,_a
Cg
h_
ec
_
Từ đây ta rút ra được:
f
`
_,_a
`
_,_b
c
_
d
_,_a
d
_,_b
Cg
_
e`
_,_a
Cg
h_
ec
_
Xác định trong hệ tọa độ gốc ta có
[
`
_,_a
`
_,_b
c
_
d
_,_a
d
_,_b
Cg
_
e `
_,_a
Cg
h_
e c
_
(*)
Xác định các ma trận chỉ hướng R
Ta có các ma trận biến đổi hệ tọa độ:
BÀI TẬP MÔN HỌC ROBOTICS
GVHD: PGS-TS Phan Bùi Khôi SVTH Bùi Xuân Tới Page 12
Ta có các trận dùng chuyển đổi hệ tọa độ từ khâu 0 sang các khâu khác :
!"
BÀI TẬP MÔN HỌC ROBOTICS
GVHD: PGS-TS Phan Bùi Khôi SVTH Bùi Xuân Tới Page 13
Từ đó ta xác định được các ma trận chỉ hướng của các khâu trong hệ tọa độ gốc
i
K
L
T
K
L
T
K
!"
L
2, xác định các vectơ j
\
]
k
k
và vectơ j
\
]
lk
k
, các ma trận sóng
C
m
n
C
h
F
o0
G
C
m
n
C
h
F
o0
G
C
m
n
C
h
F
o0
G
C
T
e
C]
K
L
pCg
BÀI TẬP MÔN HỌC ROBOTICS
GVHD: PGS-TS Phan Bùi Khôi SVTH Bùi Xuân Tới Page 14
C
h
T
e
C]
o0
o0
p Cg
h
o0
o0
C
T
e
C]
K
L
C
q
F
G
C
h
T
e
C]
h
K
L
0
0
0
C
h
q
ar
S
stuv
S
r
S
wxsv
S
r
S
stuv
S
ar
S
wxsv
S
C
T
e
C]
K
!"
L
0
0
!"
0
BÀI TẬP MÔN HỌC ROBOTICS
GVHD: PGS-TS Phan Bùi Khôi SVTH Bùi Xuân Tới Page 15
Cg
!"
!"
C
h
T
e
C]
h
K
!"
L
0
0
0
!"
0
Cg
h
r
y
r
y
!"
r
y
r
y
r
y
!"
r
y
3 -Tính toán lực và momen tại các khớp
Ta có công thức tính lực và momen tại, ta thay số vào công thức (*) để tính ta được
Khớp nối 3,2:
[
`
,
`
c
d
,
d
Cg
e `
,
Cg
h
e c
Thay giá trị tính toán ở trên vào các biểu thức ta được
1
3
,0
2 1
Fx
F Fy
Fz gm gm gm
−
−
− − − −
=
0
3 2
3
,
Fx
F Fy
Fz
-
-
- -
gm
=
BÀI TẬP MÔN HỌC ROBOTICS
GVHD: PGS-TS Phan Bùi Khôi SVTH Bùi Xuân Tới Page 16
( )
( )
( )( )
(
)
(
)
( )
( )
( )( )
( ) ( )
( ) ( ) ( ) ( )
2 3 3 3
3 3 2 3 3 3
2 3 3 3
0
3 2 3 3 2 3 3 3
2 3 3 2
,
3 3
sin θ cos θ
sin θ sin θ cos θ
2
cos θ cos θ
sin θ cos θ cos θ
2
sin θ cos θ cos θ cos θ
a gm
Mx a Fy a Fz gm
a gm
M My a Fx a Fz gm
Mz a Fx a Fy
− + + − − +
= − − − − − −
− + −
Tính cho khớp nối 2,1
[
`
,
`
,
c
d
,
d
,
Cg
e `
,
Cg
h
e c
0
2 1
3
,
2
Fx
F Fy
Fz gm gm
−
−
− − −
=
(
)
(
)
(
)
(
)
( )
( )
( )
( )
( )
( )
( )
( )( )
( )
( )
( )
( )
( )
( )
( )
( )
( )
0
3 3 2 3 3 3
2 3 3 3
2 2 2
2 2 3 2
3 3 2 3 3 3
21
2 3 3 3
2 2 2
2 2 3 2
2 3 3 2 3 3
,
sin θ sin θ cos θ
sin θ cos θ
sin θ
sin θ
2 2
sin θ cos θ cos θ
cos θ cos θ
cos θ
cos θ
2 2
sin θ cos θ cos θ cos θ
M
Mx a Fy a Fz gm
a gm
a gm
a Fz gm gm
My a Fx a Fz gm
a gm
a gm
a Fz gm gm
Mz a Fx a F
=
− + + − −
+ + − − − +
− − − − −
− − − − − −
− + −
( ) (
)
2 2 2 2
sin θ cos θ
y a Fx a Fy
+ −
BÀI TẬP MÔN HỌC ROBOTICS
GVHD: PGS-TS Phan Bùi Khôi SVTH Bùi Xuân Tới Page 17
Tính cho khâu 1,0
[
`
,
`
,
c
d
,
d
,
Cg
e `
,
Cg
h
e c
1
3
,0
2 1
Fx
F Fy
Fz gm gm gm
−
−
− − − −
=
(
)
(
)
(
)
(
)
( )
( )
( )
( )
( )
( )
( )
( )( )
( )
( )
( )
( )
( )
( )
3 3 2 3 3 3
2 3 3 3
2 2 2
2 2 3 2
2 3 3 3
3 3 2 3 3 3
0
2 2 2
1 1
1 0
2 2 3
,
2 1 3 2 1
sin θ sin θ cos θ
sin θ cos θ
sin θ
sin θ
2 2
cos θ cos θ
sin θ cos θ cos θ
2
cos θ
cos θ
2 2
si
Mx a Fy a Fz gm
a gm
a gm
a Fz gm gm
a gm
My a Fx a Fz gm
a gm
a gm
M
a Fz gm gm a Fz gm gm gm
Mz
− + + − − +
+ + − − − +
− − − − − − −
− − − − − − − − −
− +
=
−
( )
( )
( )
( )
( ) ( )
2 3 3 2 3 3 2 2 2 2 1
n θ cos θ cos θ cos θ sin θ cos θ
a Fx a Fy a Fx a Fy a Fy
− + − −
IV TÍNH TOÁN ĐỘNG LỰC HỌC
Các tham số động lực học của robot
+ các lực thế ở đây là trọng lượng của vật
+ các lực không thế
+ khối lượng các khâu của robot
BÀI TẬP MÔN HỌC ROBOTICS
GVHD: PGS-TS Phan Bùi Khôi SVTH Bùi Xuân Tới Page 18
+ momen quán tính của các khâu
2 – Tính toán động năng, thế năng của robot
A, tính động năng robot
Để đơn giản trong quá trình tính toán ta coi các khâu của robot là các thanh đồng chất, tiết
diện ngang không đáng kể với khối lượng lần lượt là m
1
m
2
m
3
Bảng tham số động lực robot 3 khâu
Khâu Vị trí trọng tâm Khối
lượng
Ma trận quán tính
z
x
c
y
c
z
c
J
ZZ
J
YY
J
XX
J
ZY
J
YX
J
XZ
1
0
0 0
7
0
J
Y
J
X
0 0 0
2
0
0 0
7
0
J
Y
J
X
0 0 0
3
0
0 0
7
0
J
Y
J
X
0 0 0
Cũng từ giả thiết thanh là các thanh thẳng, tiết diện ngang không đáng kể ta có thể dễ
dàng xác định được tọa độ khối tâm C của các khâu cũng như vận tốc khối tâm trong hệ tọa
độ gốc
- Khâu 1
C
h
{
r
|
{
và M
h
K
;
L
- Khâu 2
C
h
{
{
r
S
wxsv
S
r
S
stuv
S
{
{
và M
h
K
r
S
;
r
S
;
;
L
BÀI TẬP MÔN HỌC ROBOTICS
GVHD: PGS-TS Phan Bùi Khôi SVTH Bùi Xuân Tới Page 19
- Khâu 3
C
h
{
{
{
{
và M
h
;
"QR
;
;
;
!"
;
;
;
r
y
wxsv
y
v
;
y
- Xác định các ma trận sóng w
i
Ta có
i
A
ci
=
K
} Q
~ Q
• Q
L
nên khi nhân vào sẽ không ảnh hưởng đến ma trận R
i
Xác định Ri từ biểu thức
0
A
i
là đủ
Ta đã xác định được các
0
R
i
từ phần tĩnh học như sau
i
K
L
T
K
L
T
K
!"
L
Ta đi xác định các ma trận U
€
•
q
BÀI TẬP MÔN HỌC ROBOTICS
GVHD: PGS-TS Phan Bùi Khôi SVTH Bùi Xuân Tới Page 20
UV
•
T
W
T
;
K
LK
L
K
L
UV
•
T
W
T
;
K
LK
;
;
;
;
L
K
;
;
L
UV
•
T
W
T
;
(
)
(
)
(
)
(
)
(
)
( )
( )
( )
( ) ( )
( ) ( )
2 3 2 3 3
2 3 2 3 3
2 2
cos
θ cos θ sin θ cos θ sin θ
cos
θ sin θ sin θ sin θ cos θ
sin θ cos θ 0
− −
−
( )
( )
( )
( )
( )
( )
( )
( )
( )
( )
( )
( )
( )
( )
( )
( )
( )
( )
( ) ( )
2 2 3 2 3 3 2 2 3 2 3 3 2 2
2 2 3 2 3 3 2 2 3 2 3 3 2 2
3 3 3
. . . . .
. . . . .
3
. .
sin θ θ cos θ cos θ sin θ θ sin θ θ sin θ cos θ cos θ θ cos θ
θ
cos θ θ cos θ sin θ sin θ θ cos θ θ sin θ sin θ cos θ θ sin θ
θ
cos θ θ sin θ θ 0
− − −
− − −
−
=
( )
( )
( ) ( )
. .
3 3 2
3 3 2
.
3 2 3 2
.
. .
0
θ cos θ θ
θ 0 sin θ θ
cos θ θ sin θ θ 0
−
−
−
Các ma trận Jacobi tịnh tiến và ma trận Jacobi quay
(
)
( )
2 2
2 2
1 2
sin θ
0 0
2
0 0 0
cos θ
0 0 0 0 0
2
1 0 0
1 0 0
TC TC
a
a
J J
−
= =
BÀI TẬP MÔN HỌC ROBOTICS
GVHD: PGS-TS Phan Bùi Khôi SVTH Bùi Xuân Tới Page 21
( )
(
)
(
)
(
)
(
)
( )
( )
( )
( )
( )
( )
3 2 3 3 2 3
2 2
3 2 3 3 2 3
3 2 2
3 3
sin
θ cos θ cos θ sin θ
0 sin θ
2 2
cos
θ cos θ sin θ sin θ
0 cos θ
2 2
cos θ
1 0
2
TC
a a
a
a a
J a
a
=
− − −
− −
\
( )
3
1 2 3 3
0 sin
θ 0
0 0 0 0 0 0
0 0 0 0 1 0 0 cos
θ 0
0 0 0 0 0 0
0 0 1
JR JR JR
= = =
Tính động năng robot
I
‚
I
_
_ƒ
‚
0
M
h_
W
7
_
M
h_
U
_
•W
z
_
U
_
•
_ƒ
I
0
;
W
„‚
H
W…_
W
7
_
H
W…_
_ƒ
H
†_
W
z
_
H
†_
‡
;
0
;
W
d;
Lập trình trên maple ta tính được giá trị M(q) có dạng như sau:
d
K
7
,
7
,
7
,
7
,
7
,
7
,
7
,
7
,
7
,
L
Trong đó:
BÀI TẬP MÔN HỌC ROBOTICS
GVHD: PGS-TS Phan Bùi Khôi SVTH Bùi Xuân Tới Page 22
(
)
( )
( )
, ,
1 3 3
11 1 1 2 1 3
2 1
2
2
1 2
2 2 2 1 2 3 3 1
,
2 3
,
, 2
cos θ
3 0
2
0
5
cos θ 2 cos θ c
4
m a
m m m m
m
m a
m Tyy m a a m a a
= = =
=
= + +
−
( )
( )
( )
( )
( ) ( )
( )
( ) ( )
( ) ( )
( )
,
2
2
2
1 3 3
3 3 3
2
1 3 3 2 2 3
2 3
2
2
1 3 3 1 3 3 2 2 3
1 3
3, ,1 3 2 3 3 3,
cos θ
os θ cos θ
4
sin θ cos θ sin θ cos θ
2
cos θ sin θ cos θ sin θ cos θ
2 2 4
m a
Tyy
m a
m
m a m a
m a
m m m Tzz
+ +
=
= ; = ; = +
từ đó ta tính được giá trị biểu thức động năng như sau:
( ) ( ) ( ) ( ) ( ) ( ) ( ) ( )
( ) ( ) ( ) ( ) ( ) ( ) ( )
2 2
11 2 2 1 3 31 2 1 2 2 2 2
2
. . . . . . . .
, , , , ,
2 3 3 2 3 1 3 3 2 2 3
. . . . . .
3 3
.
, ,3, ,
1 1 θ 1 θ θ 1 θ
θ θ θ 1 θ θ θ
T d t m d t t m d t t m t d t m t m
t t m t d t m t t m t m
+
+ + + +
+ + + +
=
3, Tính thế năng của robot
Chọn mốc thế năng của robot tại gốc hệ tọa độ cố định, từ đó ta có thế năng của các khâu
robot được tính như sau:
.
m g Z
i i Ci
∏ =
Phần trên ta đã tính tọa đọ các khối tâm được kết quả:
BÀI TẬP MÔN HỌC ROBOTICS
GVHD: PGS-TS Phan Bùi Khôi SVTH Bùi Xuân Tới Page 23
Tọa độ khối tâm khâu 1 : C
h
{
r
|
{
=> Z
C1
= d
1
C
h
{
{
r
S
wxsv
S
r
S
stuv
S
{
{
=> Z
C2
= d
1
C
h
{
{
{
{
=> Z
C3
=
Thế năng robot
3
1
[ ( sin )]
1 1 2 1 3 1 3 3
2
1
1
d g (m m ) sin
1 1 2 3 3 3 3
2
g m d m d m d a
i
i
m m a
θ
θ
∏ = ∏ = + + +
∑
=
= + + +
4 - Thiết lập phương trình vi phân chuyển động- Phương trình lagarange
Phương trình động lực học của robot có dạng
d
ˆ‰
,;
Š‹
Trong đó ta có M(q) đã được tính trong phần động năng của robot:
d
K
7
,
7
,
7
,
7
,
7
,
7
,
7
,
7
,
7
,
L
ˆ là ma trận gia tốc các khâu của robot :
[q , ]
1 2, 3
T
q q q =
&&
&& && &&
BÀI TẬP MÔN HỌC ROBOTICS
GVHD: PGS-TS Phan Bùi Khôi SVTH Bùi Xuân Tới Page 24
( )
˙ ˙ ˙
3
, , ,
, 1
q q k l j q q
k l
k l
q
j
Ψ =−
∑
−
∂Π
∂
=
Với
1
( , , )
2
m m
m
kj lj
kl
k l j
q q q
l k j
∂ ∂
∂
= − −
∂ ∂ ∂
Đi xác định các (k, l , j ) bằng chương trình maple ta được:
(
)
(
)
(
)
( ) ( ) ( )
( ) ( ) ( )
1,1,1 0 1,2,1 0 1,3,1 0
2,1,1 0 2,2,1 0 2,3,1 0
2,1,1 0 2,2,1 0 2,3,1 0
= = =
= = =
= = =
(
)
(
)
(
)
( ) ( ) ( )
( )
( )
( )
( )
( )
1 2 2 3 3 2
2
1 2 3 3
1 2 2
1,1,2 0 1,2,2 0 1,3,2 0
2,1,2 0 2,2,2 2 cos θ cos θ sin θ
sin θ
2,3,2 cos θ
2
m a a
m a a
m a
= = =
= = −
= − +
( )
( ) ( )
( ) ( )
( )
( )
( ) ( )
( )
(
)
2
1 3 3 3
3 3
2 2
2 2
1 3 3 2 3 1 3 3 2 3
3 3 3
cos θ sin θ
sin θ
4
sin
θ sin θ cos θ sin θ cos θ cos θ
cos θ sin θ
2 2
m a
a
m a m a
Tyy
−
− + −
(
)
( )
( )
( )
( )
( ) ( )
( ) ( )
2
2
1 2 3 3 1 3 3 3
1 2 2 3 3
3 3 3
3,1,2 0
sin
θ cos θ sin θ
3,2,2 cos θ sin θ
2 4
cos θ sin θ
(3, 3, 2) = 0
m a a m a
m a a
Tyy
=
= − +
+
BÀI TẬP MÔN HỌC ROBOTICS
GVHD: PGS-TS Phan Bùi Khôi SVTH Bùi Xuân Tới Page 25
( )
( )
( ) ( )
( ) ( )
( ) ( )
( )
2
2
1 2 3 3 1 3 3 3
1 2 2 3 3 3 3 3
(1, 1, 3) = 0 (1, 2, 3) = 0 (1, 3, 3) = 0 (
1, 2, 3) = 0
sin θ cos θ sin θ
2,2,3 cos
θ sin θ cos θ sin θ
2 4
2,3,3 0
m a a m a
m a a Tyy
= − + +
=
( )
( )
( )
( ) ( ) ( ) ( ) ( ) ( )
1 3 3
2 2
2 2
1 3 3 2 2 1 3 3 2 2
sin θ
3,1,3
2
cos θ cos θ sin θ sin θ cos θ sin θ
3,2,3 (3, 3, 3) = 0
2 2
m a
m a m a
=
= − +
( )
( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( )
( ) ( ) ( )
( ) ( )
( )
( ) ( ) ( )
( ) ( ) ( )
( ) ( )
1 2 3
2
3 2 2 2 2 3 3 3 3 3 3 2 2 1 3
2
2 2
2
2 2
3 3 3 2 1 2 3 3 2 1 3
. . . .
. .
3 3
3 2 1 2 2 3 3
.
.
2
1 3 3 3
2 3 3 3
.
1
.
ψ
cos θ 4 cos θ sin θ θ θ sin θ 2θ sin θ cos θ θ
ψ
2
cos
θ θ sin θ θ cos θ sin θ
ψ θ cos θ sin θ
2 2 4
sin θ θ 1
θ cos θ sin θ
gm gm gm
a t t a t a t ma
m ga t ma a t ma
t ma a
ma t d
t Tyy
= + +
− +
−
+ − +
+ +
=
=
( )
( ) ( ) ( ) ( ) ( )
( ) ( ) ( ) ( )
2
2
3 2 1
. .
. .
3 3 2 2
2
3 2 1 3 2 2
θ θ cos θ cos θ sin θ
2
θ θ cos θ sin θ
2
t
t t m a
t t ma
−
+
Các lực không thế trong trường hợp là ngoại lực mà momen tác dụng lên khâu cuối : F
E
=
[Fx, Fy, Fz]
T
và momen là M
E
= [Mx, My, Mz]
T
. vì vậy trong phương trình động lực học
của robot ta cần xét đến lực không thế Q, Q=[0 , 0 , Q
3
]
T
Ta có :
3
3
3
T
i i
r
Q F
q
δ
δ
= + Μ
Ta có tọa đọ điểm đặt lực chính là điểm thao tác của robot
