Tài liệu hạn chế xem trước, để xem đầy đủ mời bạn chọn Tải xuống
1
/ 15 trang
THÔNG TIN TÀI LIỆU
Thông tin cơ bản
Định dạng
Số trang
15
Dung lượng
585,77 KB
Nội dung
Robotcôngnghiệp
27
Chơng III
phơng trìnhđộnghọccủarobot
(Kinematic Equations)
3.1. Dẫn nhập :
Bất kỳ một robot nào cũng có thể coi là một tập hợp các khâu (links) gắn liền với các
khớp (joints). Ta hãy đặt trên mỗi khâu củarobot một hệ toạ độ. Sử dụng các phép biến đổi
thuần nhất có thể mô tả vị trí tơng đối và hớng giữa các hệ toạ độ nầy. Denavit. J. đã gọi
biến đổi thuần nhất mô tả quan hệ giữa một khâu và một khâu kế tiếp là một ma trận A. Nói
đơn giản hơn, một ma trận A là một mô tả biến đổi thuần nhất bởi phép quay và phép tịnh tiến
tơng đối giữa hệ toạ độ của hai khâu liền nhau. A
1
mô tả vị trí và hớng của khâu đầu tiên; A
2
mô tả vị trí và hớng của khâu thứ hai so với khâu thứ nhất. Nh vậy vị trí và hớng của khâu
thứ hai so với hệ toạ độ gốc đợc biểu diễn bởi ma trận :
T
2
= A
1
.A
2
Cũng nh vậy, A
3
mô tả khâu thứ ba so với khâu thứ hai và :
T
3
= A
1
.A
2
.A
3
; v.v
Cũng theo Denavit, tích của các ma trận A đợc gọi là ma trận T, thờng có hai chỉ số:
trên và dới. Chỉ số trên chỉ hệ toạ độ tham chiếu tới, bỏ qua chỉ số trên nếu chỉ số đó bằng 0.
Chỉ số dới thờng dùng để chỉ khâu chấp hành cuối. Nếu một robot có 6 khâu ta có :
T
6
= A
1
.A
2
.A
3
.A
4
.A
5
.A
6
(3.1)
T
6
mô tả mối quan hệ về hớng và vị trí của khâu chấp hành cuối đối với hệ toạ độ gốc.
Một robot 6 khâu có thể có 6 bậc tự do và có thể đợc định vị trí và định hớng trong trờng
vận độngcủa nó (range of motion). Ba bậc tự do xác định vị trí thuần tuý và ba bậc tự do khác
xác định hớng mong muốn. T
6
sẽ là ma trận trình bày cả hớng và vị trí của robot. Hình 3.1
mô tả quan hệ đó với bàn tay máy. Ta đặt gốc toạ độ của hệ mô tả tại điểm giữa của các ngón
tay. Gốc toạ độ nầy đợc mô tả bởi vectơ p (xác định vị trí của bàn tay). Ba vectơ đơn vị mô tả
hớng của bàn tay đợc xác định nh sau :
n
p
a
o
Hình 3.1 : Các vectơ định vị trí và định hớng của bàn tay máy
TS. Phạm Đăng Phớc
Robot côngnghiệp
28
Vectơ có hớng mà theo đó bàn tay sẽ tiếp cận đến đối tợng, gọi là vectơ a
(approach).
Vectơ có hớng mà theo đó các ngón tay của bàn tay nắm vào nhau khi cầm nắm
đối tợng, gọi là vectơ o (Occupation).
Vectơ cuối cùng là vectơ pháp tuyến n (normal), do vậy ta có :
a
x o= n
r
r
r
Chuyển vị T
6
nh vậy sẽ bao gồm các phần tử :
n
x
O
x
a
x
p
x
T
6
= n
y
O
y
a
y
p
y
(3.2)
n
z
O
z
a
z
p
z
0 0 0 1
Tổng quát, ma trận T
6
có thể biểu diễn gọn hơn nh sau :
Ma trận định hớng R Vectơ vị trí p (3.3)
T
6
=
0 0 0 1
Ma trận R có kích thớc 3x3, là ma trận trực giao biểu diễn hớng của bàn kẹp (khâu
chấp hành cuối) đối với hệ toạ độ cơ bản. Việc xác định hớng của khâu chấp hành cuối còn
có thể thực hiện theo phép quay Euler hay phép quay Roll, Pitch, Yaw.
Vectơ điểm
p
r
có kích thớc 3x1, biểu diễn mối quan hệ tọa độ vị trí củacủa gốc hệ
tọa độ gắn trên khâu chấp hành cuối đối với hệ toạ độ cơ bản.
3.2. Bộ thông số Denavit-Hartenberg (DH) :
Một robot nhiều khâu cấu thành từ các khâu nối tiếp nhau thông qua các khớp động.
Gốc chuẩn (Base) của một robot là khâu số 0 và không tính vào số các khâu. Khâu 1 nối với
khâu chuẩn bởi khớp 1 và không có khớp ở đầu mút của khâu cuối cùng. Bất kỳ khâu nào
cũng đợc đặc trng bởi hai kích thớc :
Độ dài pháp tuyến chung : a
n
.
Góc giữa các trục trong mặt phẳng vuông góc với a
n
:
n
.
a
Khớp n
Khớp n+1
n
Khâu n
Hình 3.5 : Chiều dài và góc xoắn của 1 khâu.
Thông thờng, ngời ta gọi a
n
là chiều dài và
n
là góc xoắn của khâu (Hình 3.5). Phổ
biến là hai khâu liên kết với nhau ở chính trục của khớp (Hình 3.6).
TS. Phạm Đăng Phớc
Robot côngnghiệp
29
n+1
Khâu n+1
Khớp n-1
Khớp n+1Khớp n
x
n
a
n
z
n
O
n
Khâu n
Khâu n-1
d
n
z
n-1
x
n-1
n
n
n
n-1
Khâu n-2
Hình 3.6 : Các thông số của khâu :
, d, a và
.
Mỗi trục sẽ có hai pháp tuyến với nó, mỗi pháp tuyến dùng cho mỗi khâu (trớc và sau
một khớp). Vị trí tơng đối của hai khâu liên kết nh thế đợc xác định bởi d
n
là khoảng cách
giữa các pháp tuyến đo dọc theo trục khớp n và
n
là góc giữa các pháp tuyến đo trong mặt
phẳng vuông góc với trục.
d
n
và
n
thờng đợc gọi là khoảng cách và góc giữa các khâu.
Để mô tả mối quan hệ giữa các khâu ta gắn vào mỗi khâu một hệ toạ độ. Nguyên
tắc chung để gắn hệ tọa độ lên các khâu nh sau :
+ Gốc của hệ toạ độ gắn lên khâu thứ n đặt tại giao điểm của pháp tuyến a
n
với trục
khớp thứ n+1. Trờng hợp hai trục khớp cắt nhau, gốc toạ độ sẽ đặt tại chính điểm cắt đó. Nếu
các trục khớp song song với nhau, gốc toạ độ đợc chọn trên trục khớp của khâu kế tiếp, tại
điểm thích hợp.
+ Trục z của hệ toạ độ gắn lên khâu thứ n đặt dọc theo trục khớp thứ n+1.
+ Trục x thờng đợc đặt dọc theo pháp tuyến chung và hớng từ khớp n đến n+1.
Trong trờng hợp các trục khớp cắt nhau thì trục x chọn theo tích vectơ
.
1-nn
zx z
rr
Trờng hợp khớp quay thì
n
là các biến khớp, trong trờng hợp khớp tịnh tiến thì d
n
là biến khớp và a
n
bằng 0.
Các thông số a
n
,
n
, d
n
và
n
đợc gọi là bộ thông số DH.
Ví dụ 1 : Xét một tay máy có hai khâu phẳng nh hình 3.7 :
1
2
a
1
a
2
O
0
z
1
z
2
x
1
y
1
y
2
O
1
O
2
z
0
x
0
y
0
x
2
Hình 3.7 : Tay máy có hai khâu phẳng (vị trí bất kỳ).
TS. Phạm Đăng Phớc
Robot côngnghiệp
30
Ta gắn các hệ toạ độ lên các khâu nh hình vẽ : trục z
0
, z
1
và z
2
vuông góc với tờ giấy.
Hệ toạ độ cơ sở là O
0
x
0
y
0
z
0
, chiều của x
0
hớng từ O
0
đến O
1
. Sau khi thiết lập hệ toạ độ cơ sở,
Hệ toạ độ o
1
x
1
y
1
z
1
có hớng nh hình vẽ, O
1
đặt tại tâm trục khớp 2. Hệ toạ độ O
2
x
2
y
2
x
2
có gốc
O
2
đặt ở điểm cuối của khâu 2.
Bảng thông số Denavit-Hartenbert của tay máy nầy nh sau :
Khâu
i
i
a
i
d
i
1
1
*
0 a
1
0
2
2
*
0 a
2
0
Trong đó
i
là các biến khớp (dùng dấu * để ký hiệu các biến khớp).
Ví dụ 2 : Xem sơ đồ robot SCARA có 4 khâu nh hình 3.8 :
Đây là robot có cấu hình kiểu RRTR, bàn tay có chuyển động xoay xung quanh trục
đứng. Hệ toạ độ gắn lên các khâu nh hình vẽ.
Hình 3.8 : Robot SCARA và các hệ toạ độ (vị trí ban đầu).
O
0
1
x
0
x
1
d
3
x
2
x
3
x
z
3
, z
4
2
4
O
3
O
4
z
0 z
1
z
2
a
1
a
2
O
1
O
2
d
4
Đối với tay máy nầy các trục khớp đều song song nhau, để tiện lợi tất cả các gốc toạ độ
đặt tại tâm các trục khớp. Trục x
0
nằm trong mặt phẳng tờ giấy. Các hệ toạ độ khác nh hình
vẽ. Bảng thông số DH củarobot SCARA nh sau :
Khâu
i
i
a
i
d
i
1
1
*
0 a
1
0
2
2
*
180
0
a
2
0
3 0 0 0 d
3
*
4
4
*
0 0 d
4
* : Các biến khớp.
3.3. Đặc trng của các ma trận A :
Trên cơ sở các hệ toạ độ đã ấn định cho tất cả các khâu liên kết của robot, ta có thể
thiết lập mối quan hệ giữa các hệ toạ độ nối tiếp nhau (n-1), (n) bởi các phép quay và tịnh tiến
sau đây :
Quay quanh z
n-1
một góc
n
Tịnh tiến dọc theo z
n-1
một khoảng d
n
Tịnh tiến dọc theo x
n-1
= x
n
một đoạn a
n
Quay quanh x
n
một góc xoắn
n
TS. Phạm Đăng Phớc
Robot côngnghiệp
31
Bốn phép biến đổi thuần nhất nầy thể hiện quan hệ của hệ toạ độ thuộc khâu thứ n so
với hệ toạ độ thuộc khâu thứ n-1 và tích của chúng đợc gọi là ma trận A :
A
n
= Rot(z,) Trans(0,0,d) Trans(a,0,0) Rot(x,) (3.4)
cos -sin
0 0 1 0 0 a 1 0 0 0
A
n
=
sin cos
0 0 0 1 0 0 0
cos -sin
0
0 0 1 0 0 0 1 d 0
sin cos
0
0 0 0 1 0 0 0 1 0 0 0 1
cos -sin cos sin sin a cos
A
n
=
sin cos cos -cos sin a sin
(3.5)
0
sin cos
d
0 0 0 1
Đối với khớp tịnh tiến (a = 0 và
i
= 0) thì ma trận A có dạng :
1 0 0 0
A
n
= 0
cos - sin
0 (3.6)
0
sin cos
d
0 0 0 1
Đối với một khâu đi theo một khớp quay thì d, a và là hằng số. Nh vậy ma trận A
của khớp quay là một hàm số của biến khớp .
Đối với một khâu đi theo một khớp tịnh tiến thì , là hằng số. Ma trận A của khớp
tịnh tiến là một hàm số của biến số d.
Nếu các biến số đợc xác định thì giá trị của các ma trận A theo đó cũng đợc xác
định.
3.4. Xác định T
6
theo các ma trận A
n
:
Ta đã biết : T
6
= A
1
A
2
A
3
A
4
A
5
A
6
Trong đó T
6
đợc miêu tả trong hệ toạ độ gốc (hệ toạ độ gắn với khâu cơ bản cố định
của robot). Nếu mô tả T
6
theo các hệ toạ độ trung gian thứ n-1 thì :
=
6
1
n
T
A
i
in=
6
X
Z
T
6
E
A
O
R
Trong trờng hợp tổng quát, khi
xét quan hệ củarobot với các thiết bị
khác, nếu hệ toạ độ cơ bản củarobot có
liên hệ với một hệ toạ độ nào đó bởi phép
biến đổi Z, Khâu chấp hành cuối lại có
gắn một công cụ, có quan hệ với vật thể
bởi phép biến đổi E (hình 3.9) thì vị trí và
hớng của điểm cuối củacông cụ, khảo
sát ở hệ toạ độ tham chiếu mô tả bởi X sẽ
đợc xác định bởi :
Hình 3.9 : Vật thể và Robot
X= Z T
6
E
TS. Phạm Đăng Phớc
Robot côngnghiệp
32
Quan hệ nầy đợc thể hiện trên toán đồ sau :
A
1
A
2
A
3
A
4
A
5
6
5
T
6
4
T
6
3
T
6
2
T
6
1
T
6
T
O
R
O
R
Z
EX
A
O
0
Hình 3.10 : Toán đồ chuyển vị của robot.
Từ toán đồ nầy ta có thể rút ra : T
6
= Z
-1
X E
-1
(Z
-1
và E
-1
là các ma trận nghịch đảo).
3.5. Trình tự thiết lập hệ phơng trìnhđộnghọccủarobot :
Để thiết lập hệ phơng trìnhđộnghọccủa robot, ta tiến hành theo các bớc sau :
1. Chọn hệ toạ độ cơ sở, gắn các hệ toạ độ mở rộng lên các khâu.
Việc gắn hệ toạ độ lên các khâu đóng vai trò rất quan trọng khi xác lập hệ phơng
trình độnghọccủa robot, thông thờng đây cũng là bớc khó nhất. Nguyên tắc gắn hệ toạ độ
lên các khâu đã đợc trình bày một cách tổng quát trong phần 3.5. Trong thực tế, các trục
khớp củarobot thờng song song hoặc vuông góc với nhau, đồng thời thông qua các phép biến
đổi của ma trận A ta có thể xác định các hệ toạ độ gắn trên các khâu củarobot theo trình tự
sau :
+ Giả định một vị trí ban đầu
(
)
(Home Position) của robot.
+ Chọn gốc toạ độ O
0
, O
1
,
+ Các trục z
n
phải chọn cùng phơng với trục khớp thứ n+1.
+ Chọn trục x
n
là trục quay của z
n
thành z
n+1
và góc của z
n
với z
n+1
chính là
n+1
. Nếu z
n
và z
n+1
song song hoặc trùng nhau thì ta có thể căn cứ nguyên tắc chung hay chọn x
n
theo x
n+1
.
+ Các hệ toạ độ Oxyz phải tuân theo qui tắc bàn tay phải.
+ Khi gắn hệ toạ độ lên các khâu, phải tuân theo các phép biến đổi của ma trận A
n
. đó
là bốn phép biến đổi : A
n
= Rot(z,) Trans(0,0,d) Trans(a,0,0) Rot(x,). Nghĩa là ta coi hệ toạ
độ thứ n+1 là biến đổi của hệ toạ độ thứ n; các phép quay và tịnh tiến của biến đổi nầy phải là
một trong các phép biến đổi của A
n
, các thông số DH cũng đợc xác định dựa vào các phép
biến đổi nầy. Trong quá trình gắn hệ tọa độ lên các khâu, nếu xuất hiện phép quay của trục z
n
đối với z
n-1
quanh trục y
n-1
thì vị trí ban đầu củarobot đã giả định là không đúng, ta cần chọn
lại vị trí ban đầu khác cho robot.
2. Lập bảng thông số DH (Denavit Hartenberg).
3. Dựa vào các thông số DH xác định các ma trận A
n
.
4. Tính các ma trận T và viết các phơng trìnhđộnghọccủa robot.
(
)
Vị trí ban đầu là vị trí mà các biến nhận giá trị ban đầu, thờng bằng 0.
TS. Phạm Đăng Phớc
Robot côngnghiệp
33
Ví dụ sau đây trình bày chi tiết của các bớc khi thiết lập hệ phơng trìnhđộnghọc
của robot :
Cho một robot có ba khâu, cấu hình RRT nh hình 3.11. Hãy thiết lập hệ phơng trình
động họccủa robot.
1. Gắn hệ toạ độ lên các khâu :
Ta giả định vị trí ban đầu và chọn gốc toạ độ O
0
củarobot nh hình 3.12. Các trục z đặt
cùng phơng với các trục khớp.
Ta thấy trục z
1
đã quay tơng đối một
góc 90
0
so với trục z
0
, đây chính là phép quay
quanh trục x
0
một góc
1
(phép biến đổi
Rot(x
0
,
1
) trong biểu thức tính A
n
). Nghĩa là
trục x
0
vuông góc với z
0
và z
1
. Ta chọn chiều
của x
0
từ trái sang phải thì góc quay
1
=90
0
(chiều dơng ngợc chiều kim đồng hồ).
Đồng thời ta cũng thấy gốc O
1
đã tịnh tiến
một đoạn dọc theo z
0
, so với O
0
, đó chính là
phép biến đổi Trans(0,0,d
1
) (tịnh tiến dọc theo
z
0
một đoạn d
1
) ; các trục y
0
,và y
1
xác định
theo qui tắc bàn tay phải (Hình 3.12 ) .
Tiếp tục chọn gốc tọa độ O
2
đặt trùng
với O
1
vì trục khớp thứ ba và trục khớp thứ
hai cắt nhau tại O
1
(nh hình 3.12). Trục z
2
cùng phơng với trục khớp thứ ba, tức là đã
quay đi một góc 90
0
so với z
1
quanh trục y
1
;
phép biến đổi nầy không có trong biểu thức
tính A
n
nên không dùng đợc, ta cần chọn lại
vị trí ban đầu củarobot (thay đổi vị trí của
khâu thứ 3) nh hình 3.13.
Theo hình 3.13, O
2
vẫn đợc đặt trùng
với O
1
, trục z
2
có phơng thẳng đứng, nghĩa là
ta đã quay trục z
1
thành z
2
quanh trục x
1
một
góc -90
0
(tức
2
= -90
0
).
Đầu cuối của khâu thứ 3 không có
khớp, ta đặt O
3
tại điểm giữa của các ngón
tay, và trục z
3
, x
3
chọn nh hình vẽ, nh vậy
ta đã tịnh tiến gốc toạ độ dọc theo z
2
một
đoạn d
3
(Phép biến đổi Trans(0,0,d
3
)), vì đây
là khâu tịnh tiến nên d
3
là biến .
H
ình 3.12 : Gắn các h
ệ
to
ạ
đ
ộ
O
0
và O
1
y
1
x
1
y
0
z
1
z
2
O
1
, O
2
O
0
z
0
1
2
d
3
x
0
d
1
1
2
d
3
H
ình 3.11 : Robot RR
T
x
2
O
3
O
2
z
2
z
3
z
0
O
0
x
0
O
1
y
1
d
1
x
1
y
0
z
1
1
2
d
3
x
3
d
3
H
ình 3.13 : Hệ toạ độ
gắn lên các khâu
TS. Phạm Đăng Phớc
Robot côngnghiệp
34
Nh vậy việc gắn các hệ toạ độ lên các khâu củarobot đã hoàn thành. Thông qua các
phân tích trên đây, ta có thể xác định đợc các thông số DH của robot.
2. Lập bảng thông số DH :
Khâu
i
i
a
i
d
i
1
1
*
90 0 d
1
2
i
*
-90 0 0
3 0 0 0 d
3
*
3. Xác định các ma trận A :
Ma trận A
n
có dạng :
cos -sin cos sin sin
0
A
n
=
sin cos cos -cos sin
0
0
sin cos
d
0 0 0 1
Với qui ớc viết tắt : C
1
= cos
1
; S
1
= sin
1
; C
2
= cos
2
. . .
C
1
0 S
1
0
A
1
= S
1
0 -C
1
0
0 1 0 d
1
0 0 0 1
C
2
0 -S
2
0
A
2
= S
2
0 C
2
0
0 -1 0 0
0 0 0 1
1 0 0 0
A
3
= 0 1 0 0
0 0 1 d
3
0 0 0 1
4. Tính các ma trận biến đổi thuần nhất T :
+ Ma trận
2
T
3
= A
3
+ Ma trận
1
T
3
= A
2
.
2
T
3
C
2
0 -S
2
0 1 0 0 0 C
2
0 -S
2
-S
2
*d
3
1
T
3
= S
2
0 C
2
0 0 1 0 0 = S
2
0 C
2
C
2
*d
3
0 -1 0 d
2
0 0 1 d
3
0 -1 0 0
0 0 0 1 0 0 0 1 0 0 0 1
+ Ma trận T
3
= A
1
.
1
T
3
C
1
0 S
1
0 C
2
0 -S
2
-S
2
*d
3
T
3
= S
1
0 -C
1
0 S
2
0 C
2
C
2
*d
3
0 1 0 d
1
0 -1 0 0
0 0 0 1 0 0 0 1
TS. Phạm Đăng Phớc
Robot côngnghiệp
35
C
1
C
2
-S
1
-C
1
S
2
-C
1
S
2
d
3
= S
1
d
2
C
1
-S
1
S
2
-S
1
S
2
d
3
S
2
0 C
2
C
2
d
3
+ d
1
0 0 0 1
Ta có hệ phơng trìnhđộnghọccủarobot nh sau :
n
x
= C
1
C
2
;
O
x
= -S
1
;
a
x
= -C
1
S
2
;
p
x
= -C
1
S
2
d
3
n
y
= S
1
C
2
;
O
y
= C
1
;
a
y
= -S
1
S
2
;
p
y
= -S
1
S
2
d
3
n
z
= S
2
O
z
= 0;
a
z
= C
2
;
p
z
= C
2
d
3
+ d
1
;
(Ta có thể sơ bộ kiểm tra kết quả tính toán bằng cách dựa vào toạ độ vị trí p
x
,p
y
, p
z
đã
tính so với cách tính hình học trên hình vẽ).
3.9. Hệ phơng trìnhđộnghọccủarobot STANFORD :
Stanford là một robot có 6 khâu với cấu hình RRT.RRR (Khâu thứ 3 chuyển động tịnh
tiến, năm khâu còn lại chuyển động quay). Kết cấu củarobot Stanford nh hình 3.14 :
Hình 3.14 : Robot Stanford
TS. Phạm Đăng Phớc
Robot côngnghiệp
36
Trên hình 3.15 trình bày mô hình
của robot Stanford với việc gắn các hệ toạ
độ lên từng khâu. Để đơn giản trong khi
viết các phơng trìnhđộnghọccủa robot,
ta qui ớc cách viết tắt các hàm lợng giác
nh sau :
C
1
= cos
1
;
S
1
= sin
1
;
C
12
= cos(
1
+
2
);
S
12
= sin(
1
+
2
)
S
234
= sin (
2
+
3
+
4
) .
Hệ toạ độ gắn lên các khâu củarobot nh
hình 3.15. (Khâu cuối có chiều dài và
khoảng cách bằng không, để có thể gắn các
loại công cụ khác nhau nên chọn O
6
O
5
).
Bảng thông số DH (Denavit-Hartenberg) củarobot Stanford nh sau :
Khâu
i
i
a
i
d
i
1
1
*
-90
0
0 0
2
2
*
90
0
0 d
2
3 0 0 0 d
3
*
4
4
*
-90
0
0 0
5
5
*
90
0
0 0
6
6
*
0 0 0
(* : Các biến khớp).
Các ma trậm A củarobot Stanford đợc xác định nh sau :
C
1
0 -S
1
0 C
2
0 S
2
0
A
1
= S
1
0 C
1
0 A
2
=S
2
0 -C
2
0
0 -1 0 0 0 1 0 d
2
0 0 0 1 0 0 0 1
1 0 0 0 C
4
0 -S
4
0
A
3
= 0 1 0 0 A
4
=S
4
0 C
4
0
0 0 1 d
3
0 -1 0 0
0 0 0 1 0 0 0 1
C
5
0 S
5
0 C
6
-S
6
0 0
A
5
= S
5
0 -C
5
0 A
6
=S
6
C
6
0 0
0 1 0 0 0 0 1 0
0 0 0 1 0 0 0 1
d
2 d
3
z
4
z
3
,z
5
,z
6
z
2
O
0
,O
1
x
i
x
0
z
0
z
1
H
ình 3.15 : Hệ toạ độ củaRobot Stanfor
d
O
3
,O
4,
O
5
,O
6
x
1
O
2
Tích của các ma trận chuyển vị A đối với robot Stanford đợc bắt đầu ở khâu 6 và
chuyển dần về gốc; theo thứ tự nầy ta có :
TS. Phạm Đăng Phớc
[...]... trận An và thiết lập hệ phơng trìnhđộnghọccủarobot Thiết lập hệ phơng trìnhđộnghọccủarobot là bớc rất quan trọng để có thể dựa vào đó lập trình điều khiển robot Bài toán nầy thờng đợc gọi là bài toán độnghọc thuận robot Việc giải hệ phơng trìnhđộnghọccủarobot đợc gọi là bài toán độnghọc ngợc, nhằm xác định giá trị của các biến khớp theo các thông số đã biết của khâu chấp hành cuối; vấn... và hớng của bàn tay robot sẽ tìm đợc bằng cách xác định các giá trị các phần tử của T6 theo các phơng trình trên Các phơng trình trên gọi là hệ phơng trìnhđộnghọc thuận củarobot Stanford 3.10 Hệ phơng trìnhđộnghọccủarobot ELBOW : Để hiểu rõ hơn về cách thiết lập hệ phơng trìnhđộnghọccủa robot, ta xét thêm trờng hợp robot Elbow Khâu 2 Khâu 3 Khâu 4 Khâu 5 Khâu 1 Khâu 6 Hình 1.16 : Robot Elbow... 3.19 : Robot cấu hình TT Bài 4 : Cho một robot có 2 khâu phẳng nh hình 3.20, cấu hình RT Thiết lập hệ phơng trìnhđộnghọccủarobot Bài 5 : Cho một robot có 3 khâu nh hình 3.21, cấu hình RTR Thiết lập hệ phơng trìnhđộnghọccủarobot Hình 3.20 : Robot cấu hình RT Hình 3.21 : Robot cấu hình RTR Bài 6 : Cho một robot có 3 khâu nh hình 3.22, cấu hình RRR Thiết lập hệ phơng trìnhđộnghọccủarobot Hình...37 Robot côngnghiệp 5 T6 = C6 S6 0 0 -S6 C6 0 0 0 0 1 0 C5C6 S5C6 S6 0 4 T6 = A5A6 = 0 0 0 1 -C5S6 -S5S6 C6 0 S5 -C5 0 0 C4C5C6 - S4S6 S4C5C6 + C4S6 -S5C6 0 T63 = A4A5A6 = -C4C5S6-S4C6 -S4C5S6 + C4C6 S5S6 0 C4C5C6-S4S6 S4C5C + C4S6 -S5C6 0 2 T6 = A3A4A5A6 = 1 T6 =A2 A3A4A5A6 = 0 0 0 1 C4S5 S4S5 C5 0 -C4C5S6 - S4C6 -S4C5S6 + C4C6 S5S6 0 C2(C4C5C6 - S4S6) - S2S5C6 S2(C4C5C6 - S4S6) + C2S5C6... ? ? ? 0 -1 0 0 -1 0 0 0 0 1 2 1 là ma trận biểu diễn hớng và vị trí của khâu chấp hành cuối Tìm các phần tử đợc đánh dấu ? Bài 2 : Cho một robot có 3 khâu phẳng nh hình 3.18, cấu hình RRR Thiết lập hệ phơng trìnhđộnghọccủarobot TS Phạm Đăng Phớc 41 Robot côngnghiệp Bài 3 : Cho một robot có 2 khâu tịnh tiến nh hình 3.19, cấu hình TT Thiết lập hệ phơng trìnhđộnghọccủarobot Hình 3.18 : Robot cấu... toạ độ mở rộng ta cũng xác định đợc vị trí dừng của mỗi robot Tuỳ thuộc kết cấu củarobot cũng nh công cụ gắn lên khâu chấp hành cuối mà ta có thể đa các thông số của khâu chấp hành cuối vào phơng trìnhđộnghọc hay không Việc tính toán các ma trận T để thiết lập hệ phơng trìnhđộnghọccủarobot thờng tốn nhiều thời gian và dễ nhầm lẫn, ta có thể lập trình trên máy tính để tính toán (ở dạng ký hiệu)... + C4S6) nz = -S2(C4C5C6 - S4S6) + C2S5C6 Ox = C1[-C2(C4C5S6 + S4C6) + S2S5S6] - S1(-S4C5S6 + C4C6) Oy = S1[-C2(C4C5S6 + S4C6) + S2S5S6] + C1(-S4C5C6 + C4C6) Oz = S2(C4C5S6 + S4C6) + C2S5S6 aX = C1(C2C4S5 + S2C5) - S1S4S5 ay = S1(C2C4S5 + S2C5) + C1S4S5 az = -S2C4S5 + C2C5 px = C1S2d3 - S1d2 py = S1S2d3 + C1d2 pz = C2d3 TS Phạm Đăng Phớc 38 Robot côngnghiệp Nếu ta biết đợc các giá trị của biến khớp,... d3 1 -C2(C4C5S6-S4C6)+S2S5S6 -S2(C4C5S6+S4C6)-C2S5S6 -S4C5S6+C4C6 0 C2C4S5 + S2C5 S2C4S5 - C2C5 S4S5 0 S2d3 -C2d3 d2 1 Cuối cùng : T6 = nx ny nz 0 Ox Oy Oz 0 ax ay az 0 px py pz 1 = A1T61 Để tính T6, ta phải nhân A1 với T61 sau đó cân bằng các phần tử của ma trận T6 ở hai vế ta đợc một hệ thống các phơng trình sau : nx = C1[C2(C4C5C6 - S4S6) - S2S5C6] - S1(S4C5C6 + C4S6) ny = S1[C2(C4C5C6 - S4S6) - S2S5C6]... : Vị trí ban đầu củarobot Elbow và các hệ toạ độ Bộ thông số DH củarobot Elbow Khâu i * i 1 900 1 2 0 2 3 0 3 4 -9 00 4 5 900 5 6 0 6 (* : các biến khớp ) ai 0 a2 a3 a4 0 0 di 0 0 0 0 0 0 Các ma trận A củarobot Elbow đợc xác định nh sau : A1= C1 S1 0 0 TS Phạm Đăng Phớc 0 0 1 0 S1 -C1 0 0 0 0 0 1 A2= C2 S2 0 0 -S2 C2 0 0 0 0 1 0 C 2 a2 S 2 a2 0 1 39 Robot côngnghiệp A3= C3 S3 0 0 -S3 C3 0 0 A5= C5... A1 với T61 sau đó cân bằng các phần tử của ma trận T6 ta đợc một hệ thống các phơng trình sau : TS Phạm Đăng Phớc 40 Robot côngnghiệp nx = C1(C234C5C 6- S234S6) - S1S5C6 ny = S1(C234C5C 6- S234S6) + C1S5C6 nz = S234C5C6 + C234S6 Ox = -C1(C234C5S6 + S234C6) + S1S5S6 Oy = -S1(C234C5S6 + S234C6) - C1S5S6 Oz = -S234C5S6 + C234C6 aX = C1C234S5 + S1C5 ay = S1C234S5 - C1C5 az = S234S5 px = C1(C234a4 + C23a3 . tử của T
6
theo các phơng trình trên.
Các phơng trình trên gọi là hệ phơng trình động học thuận của robot Stanford.
3.10. Hệ phơng trình động học của. lập hệ phơng trình
động học của robot.
Bài 5 : Cho một robot có 3 khâu nh hình 3.21, cấu hình RTR. Thiết lập hệ phơng trình động
học của robot.
H
ình