Tính toán động học và mô phỏng 3D rôbốt Gryphon
Đinh Văn Phong
*
, Đỗ Sanh
*
, Nguyễn Trọng Thuần
*
, Đỗ Đăng Khoa
**

*Trờng Đại học Bách Khoa Hà nội, **Đề tài 3.312.01

Tóm tắt: Bài báo đề cập đến bài toán điều khiển động học của rôbốt và áp dụng vào
rôbốt Gryphon. Nội dung của bài báo nhằm giải quyết bài toán thuận, bài toán ngợc và mô
phỏng hoạt động của rô bốt trong không gian đồ hoạ ba chiều. Đồng thời trong bài báo
cũng giới thiệu hớng nghiên cứu việc điều khiển rô bốt Gryphon bằng máy tính.
1. Mở đầu
Trong bài báo này khảo sát bài toán điều khiển động học rô bốt, chủ yếu tập trung
vào hai bài toán :
Bài toán thuận : xác định vị trí điểm cuối và hớng tay kẹp mà tay máy đạt đợc khi qui
luật thay đổi theo thời gian của các thông số định vị của tay máy là hàm đã biết. Bài
toán này nhằm phục vụ bài toán xác định phạm vi hoạt động của tay máy, bài toán
thuận trong động lực học tay máy.

Bài toán ngợc : xác định qui luật thay đổi theo thời gian của tay máy để nó nắm bắt
đợc vị trí đã cho của đối tợng theo một hớng định trớc của tay kẹp. Bài toán này
nhằm phục vụ các bài toán điều khiển quĩ đạo, các bài toán điều khiển tối u...
Bài báo dới đây sẽ trình bày cơ sở lý thuyết của cả hai bài toán và minh hoạ qua việc áp
dụng để mô phỏng một rô bốt cụ thể.
2. Cơ sở lý thuyết.
2.1 Công thức xác định vị trí

Trong bài báo này chúng ta sẽ sử dụng phơng pháp ma trận Denavit-Hartenberg.
gắn các hệ trục tọa độ vào từng khâu của tay máy (rôbốt). Nhờ đó mối quan hệ về toạ độ
của cùng một điểm trên hai hệ trục kế tiếp nhau sẽ đợc xác định bằng một ma trận chuyển
4x4.,xem tr 53 [3].
Gọi
j
j-1
H
là ma trận chuyển toạ độ một điểm từ hệ j ( Ox
j
y
j
z
j
) về hệ j-1 ( Ox
j-1
y
j-1
z
j-1
), ta
có:
cos( ) cos( ).sin( ) sin( ).sin( ) .cos( )
sin( ) cos( ).cos( ) -sin( ).cos( ) .sin( )
0sin() cos()
00 0 1
jjj jjkj
jjj jjkj
jjj
a

a
d






=



j
j-1
H
(2.1)
trong đó các tham số có ý nghĩa nh sau:

j
là góc quay trục x
j-1
đến trục x
j
quanh trục z
j-1
,
d
j
là đoạn dịch trục x
j-1

đến trục x
j
dọc trục z
j-1
,
a
j
là đoạn dịch trục z
j-1
đến trục z
j
dọc trục x
j-1
,

j
là góc quay trục z
j-1
đến trục z
j
quanh trục x
j-1
.
Thông qua việc sử dụng liên tiếp các hệ toạ độ, ma trận chuyển hệ toạ độ gắn vào
tay kẹp về hệ toạ độ gắn vào đế của rô bốt có dạng:
( ) ( ) ( ) ( )
12 n
qq q
tay kep 1 2 n n
de 0 1 n-1 0

H = H .H ....H = H q

( )
n
0
Hq
có dạng:
( ) ( )
1



Rq pq
0
(2.2)
trong đó hệ toạ độ đế rô bốt kí hiệu là 0, hệ toạ độ tay kẹp rô bốt kí hiệu là n, q là ma trận
nx1 của các toạ độ suy rộng,chúng là các thông số định vị của rôbốt. R(q) là ma trận 3x3
xác định hớng của tay kẹp, p(q) là vectơ 3x1 xác định vị trí đầu bàn kẹp so với hệ đế. Ba
cột của ma trận
R
tơng ứng với hớng của ba vectơ đơn vị trên hệ gắn với tay kẹp so với
hệ đế rôbốt (hệ toạ độ nền).
Vị trí của một điểm P thuộc tay kẹp đợc xác định theo công thức :
( )
0
n

pp
r=H qr
(2.3)

trong đó
p
r
là vectơ định vị điểm P thuộc tay kẹp so với hệ toạ độ nền,

p
r
là vectơ định vị
điểm P trong hệ toạ độ gắn vào tay kẹp rôbốt.
2.2 Công thức xác định vận tốc.
Từ công thức (2.3) ta dễ dàng xác định đợc vận tốc điểm P gắn vào tay kẹp rôbốt
so với hệ toạ độ nền :

pp np
0
v=r=H.r
&
&
(2.4)
Để tiến hành điều khiển hoạt động của từng khâu rôbốt, ta tiến hành biểu diễn ma
trận
n
0
H
&
dới dạng sau :

n
n(i)
0i

i=1
H= Hq
&
&
(2.5)
trong đó H
(i)
là ma trận ứng với khớp i và chỉ phụ thuộc vào biến khớp,
p
r
&
là véc tơ vận tốc
của điểm cuối tay kẹp so với hệ toạ độ nền,
n
0
H
&
là ma trận đạo hàm của ma trận Denavit-
Khâu j-2
Khâu
j-1 Khâu j
Khâu j+1
z
j-2
d
j
z
j-1
y
j-1

x
j-1

j
x
j
y
j
z
j
a
j
a
j-1

x
j
Hình 1

z
j
Khớp j-1
Khớp
j
Khớp j+1

j
Hartenberg giữa hệ toạ độ nền với hệ toạ độ gắn vào tay kẹp,
i
q

&
là vận tốc của chuyển
động tại khớp thứ i.
3. Khảo sát động học của rô bốt Gryphon.
3.1 Giới thiệu mô hình và nguyên tắc hoạt động.
Rô bốt Gryphon do hãng FeedBack của Anh sản xuất phục vụ cho mục đích nghiên
cứu. Đây là một rôbốt năm trục kèm theo bàn kẹp nh hình 2

Trục hông (waist axis).

Trục vai (shoulder axis).

Trục khuỷu tay (elbow axis).

Trục cổ tay (tool pitch axis).

Trục cổ tay (tool roll axis).

Bàn kẹp (gripper).

Hình 2
Những u điểm nổi bật của rôbốt là chuyển động nhanh, chính xác và mềm mại.
Rôbốt đợc điều khiển bởi bốn vi xử lý cho phép điều khiển đặt vật chính xác. Mỗi trục của
rôbốt đợc điều khiển bởi một động cơ bớc với bộ mã hoá phản hồi. Trong bộ điều khiển,
một vi xử lý sẽ giám sát vị trí của các trục. Hai cái khác sẽ quản lý các động cơ và cái còn
lại sẽ giám sát cả ba cái trên đồng thời làm nhiệm vụ giao tiếp với máy chủ.
3.2 Công thức xác định vị trí
Khi áp dụng phơng pháp Denavit-Hartenberg gắn các hệ trục toạ độ vào các khâu
x
0

y
0
z
0
a
1

1
z
1
x
1
y
1
a
2
a
4
a
5
a
6
z
5
y
5
x
5
a
3

x
4
y
4
z
4
z
3
x
3
y
3
z
2

4

5

2
x
2
y
2

3
Hình 3
ta thu đợc sơ đồ động học của rôbốt Gryphon nh hình 3.

Bảng tham số Denavit-Hartenberg:

Theo thuật toán Denavit-Hartenberg ta có bảng tham số Denavit-Hartenberg ứng
với sơ đồ động học trên :
Trục


D


a
1

1
a
1

/2
a
2
2

2
00 a
4
3

3
00 a
5
4


4
-a
3

/2
0
5

5
a
6
0 0

Xác định các ma trận chuyền.
Đây là các ma trận chuyển thành chuyển các hệ toạ độ trên sơ đồ động học (hình 3).

1121
1121
1
0
0
01 0
00 0 1
CSaC
SCaS
a





=



1
0
H
,
22 42
22 42
0
0
0010
0001
CS aC
SC aS




=



2
1
H
,
33 53
33 53

0
0
0010
0001
CS aC
SC aS




=



3
2
H


,
44
44
3
00
00
01 0
00 0 1
CS
SC
a





=




4
3
H
,
55
55
6
00
00
001
0001
CS
SC
a




=




5
4
H
(3.1)

Ma trận chuyển toàn hệ
taykep
de
H
là ma trận truyền thuần nhất giữa hệ toạ độ nền và hệ toạ độ gắn vào tay kẹp.
taykep 1 2 3 4 5
de 0 1 2 3 4
H =H.H.H.H.H

( )
()
1 234 5 1 5 1 234 5 1 5 1 234 1 234 6 23 5 2 4 2 1 3
1 234 5 1 5 1 234 5 1 5 1 234 1 234 6 23 5 2 4 2 1 3
234 5 234 5 234 234 6 23 5 2 4 1
000 1
CC C SS CC S SC CS C S a C a C a a Sa
SC C CS SC S CC SS S S a C a Ca a Ca
SC SS C Ca Sa Sa a
++ +++

++++

=


+++



taykep
de
H
(3.2).
Để viết đơn giản ta sử dụng các kí hiệu sau: C
1
=cos(

1
), C
234
=cos(

2
+

3
+

4
), S
1
=sin(

1
),

S
234
=sin(

2
+

3
+

4
).

Ma trận R
1 234 5 1 5 1 234 5 1 5 1 234
1 234 5 1 5 1 234 5 1 5 1 234
234 5 234 5 234
CC C SS CC S SC CS
SC C CS SC S CC SS
SC SS C
+ +


=




R
(3.3)


Vectơ
p

()
()
1 234 6 23 5 2 4 2 1 3
1 234 6 23 5 2 4 2 1 3
234 6 23 5 2 4 1
CS a Ca Ca a Sa
SSa Ca Ca a Ca
Ca Sa Sa a
+++

=++++


+++

p
(3.4)
Xem [1].
3.3 Bài toán thuận
Ta có thể xác định vị trí và vận tốc nh sau:
Trong bài toán thuận
( )
n
0
Hq
từ (2.3) là hàm đã biết của thời gian, do đó ta tính

đợc
( )
t=
pp
rr

Trong công thức (2.5) vì
( ) ( )
,tt==qq qq
&&
do đó
(i)
H
là hàm đã biết của t, từ (2.4)
ta tính đợc
( )
t
pp
v=v
.
3.4 Bài toán ngợc
3.4.1 Bài toán vị trí
Mục đích bài toán nhằm xác định các góc
(1,5)
i
i

=
để tay máy nắm bắt đợc đối
tợng tại một vị trí đã cho và theo một hớng xác định. Do đó dữ liệu vào là vị trí đầu bàn

kẹp
T
xyz
ppp

=

T
p
và hớng tay kẹp R.
Từ các công thức (3.3) và (3.4) ta nhận thấy rằng cột thứ 3 của ma trận
R
và
p

không phụ thuộc vào

5
mà chỉ phụ thuộc vào các góc

1
,

2
,

3
,

4

do đó các góc này sẽ
đợc xác định theo vectơ cấu hình tay kẹp :
w=[p
x
, p
y
, p
z
, R
13
, R
23
, R
33
]. Nói cách khác ta thu đợc hệ phơng trình sau:
( )
()
1
12346 235 24 2 13
2
1 234 6 23 5 2 4 2 1 3
3
234 6 23 5 2 4 1
413
1234
523
1 234
633
234
w

w
w
w
w
w
w
x
y
z
p
CS a Ca Ca a Sa
p
SSa Ca Ca a Ca
p
Ca Sa Sa a
R
CS
R
SS
R
C
+++



++++





+++
===













(3.5)
thực hiện các phép biến đổi giải tích, ta thu đợc kết quả sau:

Góc quay ở khớp hông

1
đợc tính nh sau
Đặt
222
123
wwba= +

1213123
atan2(w b-w a ,w b+w a )q
=

(3.6)

Góc quay ở khớp khuỷu

3
đựoc tính nh sau:
( ) ( )
1 1 13 1 2 13 1 2346 2
11 21 2346 2
2312346
ww
ww
w
bSaCCaSSaa
CSSaa
baCa
=+ +
=+
=+

với
234 1 4 1 5 6
atan2( ,-w )qCwSw=+

2222
1245
3
45
2
bbaa

q
aa
+
=
(3.7)

Góc quay ở khớp vai

2
đợc tính nh sau: