1
Xây dựng hệ thí nghiệm Điều khiển Rô bốt
với quá trình nhận dạng vật thể động
trên cơ sở Camera

TS Lê Bá Dũng
Ths Hà Mạnh Đào
KS Nguyễn Thanh Tùng
Viện CNTT
Tóm tắt:

Bài báo trình bầy quá trình xây dựng, thiết kế hệ thí nghiệm điều khiển Rô bốt với quá trình nhận dạng
vật thể động trên cơ sở Camera. Với quá trình nhận dạng động các vị trí và hớng của vật thể thông qua ảnh
vật thể từ thời điểm cắt mẫu, một thuật toán ớc lợc vị trí đợc xây dựng. Với các vị trí đã ớc lợng, thuật
toán điều khiển đợc thiết kế để đa đầu tay nắm đến vị trí đã đợc ớc lợng và di chuyển vật thể đến
một vị trí xác định trong không gian làm việc và không gian quan sát đợc của Camera.

Abstract:

The paper presents a process to create a robot control system with dynamic recognition of image
object based on Camera. With the position and orientation of object is determined on the sampling time via
image recognition, an estimation algorithm is created. By the position and orientation are determined, an
robot control algorithm is constructed to move the end effectors to estimated position and orientation of the
object and take the object to other position and orientation on the working space and camera space.

1. Mở đầu


Hình 1 Sơ đồ hệ thí nghiệm
Các thông tin từ các đầu đo đợc chính xác hoá thông qua các phép nhận dạng. Các vật
thể chuyển động đợc xác định bằng các phơng pháp ảnh trôi (optic flow method), ở đây
không xét cụ thể các điểm ảnh mà vị trí và tốc độ ảnh coi nh là các giá trị gián tiếp. Nếu vị trí
của vật thể chuyển động đợc xác định từ ảnh, thì giá trị vị trí đó khi tính toán đã là giá trị cũ
của một vài thời điểm cắt mẫu trớc đó. Do đó để có thể có đợc giá trị tính toán vị trí và tốc
độ một cách chính xác, hay ta còn gọi là vị trí và tốc độ của vật thể tơng đối với camera, trong
trờng hợp này chúng ta cần phải xây dựng một mô hình dự báo. Thông số dự báo đợc xác
định thông qua chuyển động của vật thể. Từ đó có thể thiết kế đợc quỹ đạo theo thời gian thực
tế của chuyển động Robot và thiết kế thuật điều khiển
Giả sử vật thể với vị trí và tốc độ đợc xác định liên tục. Nếu ở thời điểm kT
V
có vị trí là
P
0
(kT
V
) và hớng của vật thể là

0
x
(kT
v
),


0
y
(kT
v
),

0
z
(kT
v
)]
T
(1)

Với T
V
thời gian quan sát
x,y,z là toạ độ Đề Các của hệ
T miêu tả chuyển vị

ở đây có thể hiểu là:
Vector vị trí bao gồm vị trí và hớng của Robot


là vị trí đợc ớc lợng ở thời điểm T
v
+ mT
c
thông qua các thông tin ở thời điểm (k -1)T
v
với
T
c
là chu kỳ cắt mẫu của vòng điều khiển (T
v
>T
c
) và m = 1 T/T
v
là số nguyên

Tốc độ chuyển động vật thể đợc tính toán qua vị trí tâm điểm của vật thể đợc xác
định ở thời điểm hiện tại và thời điểm trớc đó. Gọi v
0
(kT

0
(kT
v
) = [v
0
x
(kT
v
) v
0
y
(kT
v
) v
0
z
(kT
v
),
0
x
(kT
v
)
0
y
(kT
v
)
0

n
n
o
zAzAzAzA

+++=
020
2
10
1
1
)(
(4)
Ma trận vuông A
0
i
, i=1,2, ,n chứa các giá trị cha biết, z
-1
là toán tử trễ liên quan đến
chu kỳ cất mẫu quan sát và z
-1
v
0
(kT
i
)=v
0
[(k-1)T] ; n- số nguyên là độ lớn của Ma trận
i
n
iiii
T
n
T
TTT
aaaa
AAA
nkvkvkvkM
0
6
0
1
10
6
10
1
0
0
6
0
2
0
1
00
2
0
1
0

)1()1()1(
)1()1()1()1(
)1(
1
)(
))1()1(
0
)()(1(*)()1(
0
)(
0
0
0000
0000
0
0
0
0000
<








+

=


Vị trí vật thể có thể viết từ (2)
[ ][][]
)10()1/()1/()1/(
000
CVVVVVC
mTTkkTvTkkTWTkmTkTW +=+


3.2 Thiết kế bộ điều khiển:
3.2.1 Một số vấn đề cơ bản của động học vị trí
Một vật thể trong không gian thờng đợc mô tả bằng sáu tham số sau nh [8]:
- 3 tham số về vị trí (position): x, y, z hoặc p
x
, p
y
, p
z
, nh biểu diễn ở (1)
- 3 tham số về hớng (orientation):

x
,

y
,

z
.
Xét robot trong hệ toạ độ Đềcác với loại robot nối khớp (chỉ có khớp quay mà không có

với vị trí cũ).
Các biến ngoài x, y, z,

x
,

y
,

z
là vị trí hình học của tay nắm robot.
- x, y, z là toạ độ của tay nắm robot.
-

x
,

y
,

z
là các góc quay của tay nắm robot quanh các trục x, y, z.
x
y
z

6

5





















x
y
z
x
y
z






,

2
,

3
,

4
,

5
,

6
) của các khớp
và điều khiển các khớp quay của robot theo các góc đã tính toán để di chuyển tay nắm robot
đến đúng vị trí và hớng của vật. Việc tính toán bộ thông số các góc quay (

1
,

2
,

3
,

4
,

g
học thuận
z
0


2

O
1
y
0
z
x
0
O
y
1

3

z
2

y
2
x
2
O
2

170 mm
93 mm
75 mm
28 mm
10 mm
38 mm
70 mm
10 mm
55 mm
R
H

6
R
T
H
=
1
R
2
.
2
R
3
.
3
R
4
.
4

12
C
5
- C
12
S
34
S
5
n
y
= - C
12
C
5
- S
12
S
34
S
5
n
z
= C
34
S
5
o
x
= S

= C
12
C
34
a
y
= S
12
C
34
a
z
= S
34
(12)
p
x
= ( 125C
34
+ 38C
3
+75 ) C
12
+ 18S
12
+ 93C
1

p
y

, p
y
, p
z
) đợc xác định nh sau:
R
T
H
= Trans (z, p
z
). Trans (y, p
y
). Trans (x, p
x
). Rot( z ,

z
). Rot( y ,

y
). Rot( x ,

x
)
=






+ 18S
12
+ 93C
1
(14)
p
y
= ( 125C
34
+ 38C
3
+75 ) S
12
- 18C
12
+ 93S
1
(15)
p
z
= 125S
34
+ 38S
3
+160 (16)

z
= atan2 ( n
y
, n

y
cos

z
- o
x
sin

z
) (19)
3. Động học ngợc robot 5 bậc tự do [8]


5
= atan2( -S

y
, C

y
S

x
)


1
= atan2( S
1
, C

7
3.2.3. ứng dụng phơng pháp nhận dạng động vị trí, hớng vào chơng trình
điều khiển robot 5 bậc tự do trên cơ sở camera
Trên cơ sở nhận dạng vị tri vật thể tĩnh trên sàn từ camera quan sát, ta có đợc bộ
thông số của vật thể (x, y, z,

x
,

y
,

z
). Từ đó dùng các phơng trình động học ngợc đã
tính ở mục 3.3 , ta xác định đợc bộ góc quay (

1
,

2
,

3
,

4
,

5
) tơng ứng với từng khớp.


Bắt đầu
Camera
q
uan sát
x,
y
, z,

x
,

y
,

z
Ước lợng và Tính
động học ngợc
Điều khiển các khớp
quay theo các góc

1
,
2

N0 5 -10-1996
[2] P. Papanikolopnous, Six degree freedom Hand/eye visual tracking with Uncertain
Parameters, IEEE transaction on Robotics and Automation. vol 11 No5 October 1995
[3] Koichi Hashimoto, Visual servoing with Hand/eye manipulator optimal control approach,
IEEE transcraction on Robotics and Automation vol 12 No 5 October 1996
[4] Le Ba Dung Ha Manh Dao, An Algorithm to Robot Control using a Camera, Military
workshop in Hanoi 12/2000
[5] Lê Bá Dũng và cộng sự, Một thuật toán điều khiển Rôbốt trên cơ sở Camera, Tạp chí Khoa
học và công nghệ các trờng Đại học sô 34+35, năm 2002
[7] Hà Mạnh Đào, Một thuật toán điều khiển Rôbốt trên cơ sở Camera, Tạp chí Khoa học và
công nghệ các trờng Đại học sô 34+35, năm 2002
[8] Báo cáo đề tài Trung tâm KHTN & CNQG Về điều khiển Rô bốt trên cơ sở Camera,
năm 2001
[9] Vladimir Strejtr, Automatic theory, Praha 1980