Tài liệu Xây dựng hệ thí nghiệm RoBot trên cơ sở camera ppt

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

Điều khiển Robot trên cơ sở Camera đợc thiết kế theo 1 số yêu cầu cơ bản nh sau về
mặt kĩ thuật:
- Camera đợc đặt cố định trong không gian làm việc của Robot
- Camera đợc treo tại đầu tay nắm end- effector của Robot.
Trong bài báo này các tác giả đề cập đến mô hình thí nghiệm nhận dạng vật thể động và
điều khiển Rôbốt với Camera đặt cố định trong không gian làm việc của Rô bốt

2.
Mô hình điều khiển Robot với với Camera đặt cố định trong
không gian làm việc của Rô bốt

Mô hình điều khiển Robot với Camera đặt cố định trong không gian làm việc của Rô
bốt cho phép ta thực hiện hệ điều khiển kín với quá trình quan sát (đo vị trí và hớng của vật
thể thông qua thiết bị đo quang). ở đây các tín hiệu đo đợc thực hiện qua đầu đo (visual
sensor ) và đợc đặt trong hệ kín với việc nhận dạng vật thể trong thời gian thực. Mô hình điều
khiển với các phép đo nhìn trong mạch phản hồi kín đợc gọi là hệ visual sensor system. Với
hệ visual sensor system đợc phân ra hai nhóm:
. Điều khiển Robot trên cơ sở camera thông qua việc xác lập vị trí của vật thể [1], [2],
[3]
. Điều khiển Robot với camera nhằm xác lập các tính chất của vật thể [1], [2],[3]


2
3. Bài toán điều khiển Robot với quá trình nhận dạng động
3.1 Dự báo về vị trí và hớng của vật thể

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
(kT
V
), nh vậy chuyển động của vật thể trong quá trình nhìn
thấy qua các thời điểm kT
V
(k = 1 ) đợc xác định

W
0
(kT
V
)= [P
0
x
(kT
v
), P
0
y
(kT
v
), P
0
z
(kT
v
),

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

Phần mềm
Nguồn điện
Ada
p
te
K
e
y
boar
d
1
2
4
5
6
9
1
11
1
1
1
1
Ada
p
te

3
Vector tốc độ bao gồm phép quay và phép tịnh tiến

Từ việc xác định vị trí vật thể qua ảnh. Các giá trị đo đợc sử dụng cho việc điều khiển

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

v
) là vận tốc của vật thể ở thời điểm k
T
c
đợc xác định bằng

v
0
(kT
v
) = {W
0
(kT
v
) - W
0
[(k-1)T
v
]}/T
v
(2)

Với v
0
(kT
v
) tốc độ ở thời điểm kT
v
có thể viết dới dạng sau
v

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

z
(kT
v
) ]
T

3 thành phần đầu là tốc độ chuyển động thẳng.
3 thành phần sau là tốc độ chuyển động quay.
Với quá trình chuyển động của vật thể, tốc độ trong (2) đợc xác định thông qua mô
hình AR và mô hình AR đợc chọn nh sau theo [9]:


)()()()(
0010
vvv
o
kTekTvzAkTv +=

(3)
vector e
o
(kT
v
) là sai số mô hình và ma trận A
o
(z
-1
) đợc định nghĩa

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

với i=1,2, ,6
T- chu kỳ cắt mẫu

Để ớc lợng các thông số cha biết của (3) ta sử dụng phơng pháp trong [5]

(k-1)T
V
kT
V
(k+1)T
V
kT
V
+T
C
kT
V
+2T
C
H
ình 2 Quá trình cắt mẫu
[
]
[][]
[]
T
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

0000


))( ())2(())1(()1(
=
==
=


(5)
(6

(7)

4
)()1()()(
0000
kekMkv
T
+=

(8)
với phơng pháp bình phơng tối thiểu, ớc lợng giá trị vector

0
trong [5]
[]
[]
10
)9(

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








+

=

+=


à
à
à


kMkPkM
kPkMkMkP
kPkP
kMk
i
kvkMkPk
i
k
j
T
T
T
i

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ó

khớp tịnh tiến) với giả thiết robot co 6 khớp quay









Hình 3 Rôbốt chỉ có kớp quay
Các biến trong

1
,

2
,

3
,

4
,

5
,

6
là các góc chuyển động tơng đối của các khớp (so

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

4

3

2

1
n
o
a

5
Sự biến đổi qua lại giữa các biến ngoài và biến trong thể hiện nh sau:



=






1
2
3
4
5
6

x
y
z
x
y
z

Vấn đề cơ bản của bài toán gắp vật trên sàn là từ vị trí (x, y, z,

x
,

y
,

z
) xác định của vật, ta
phải tính toán động học ngợc để xác định các góc quay (

1

,

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
,

5
,

6
) của các khớp tuỳ thuộc vào hệ phơng trình động học ngợc đã xây dựng có tối u hay
không. Nhiều khi phải giải hệ phơng trình siêu việt trong bài toán ngợc với thời gian rất lâu,
đôi khi lời giải không đạt sự hội tụ. Điều này không đảm bảo thời gian thực trong điều khiển
tay máy. Do đó việc giải bài toán động học ngợc đa ra đợc hệ phơng trình tối u có ý
nghĩa vừa khoa học vừa thực tiễn. Khi áp dụng vào thực tiễn thì tuỳ theo từng loại robot mà ta
có thể xây dựng đợc hệ phơng trình động học ngợc theo cấu trúc hệ thống của nó.
3.2.2 Một phơng pháp tính toán động học ngợc cho robot 5 bậc tự do của hệ
1. Cấu trúc hệ thống của robot 5 bậc tự do







Hình 4 Cấu trúc RôBốt 5 bậc tự do
2. Động học thuận robot 5 bậc tự do theo [8]

Ma trận biến đổi đồng nhất của tay nắm robot nhìn trong hệ toạ độ gốc R
Độn
g
học n
g
ợc
Độn

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

x
1

1

z
3
y
3
x
3
O
3


z
4

y
4
x
4

O
4
o
n
a
O
hand

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

R
5
.
5
R
H
=
noap
noap
noap
xxxx
yyyy
zzzz
0001












(11)
n
x
= -S

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

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

a
x

= 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

= ( 125C
34
+ 38C
3
+75 ) S
12
- 18C
12
+ 93S
1

p
z
= 125S
34
+ 38S
3
+160
Ma trận đồng nhất phép quay của tay nắm robot RPY (

z
,

y
,

x
) và phép tịnh tiến một khoảng
(p
x

, 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
)
=

+
+
1000
zxyxyy
yxzxyzxzxyzyz
xxzxyzxzxyzyz
pCCSCS
pSCCSSCCSSSCS
pSSCSCCSSSCCC



(13)
Từ (11), (12), (13) ta rút ra đợc hệ phơng trình động học thuận:
p
x
= ( 125C
34
+ 38C
3
+75 ) C
12

+ 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

x
) (17)

y
= atan2 ( -n
z
, n
x
cos

z
+ n
y
sin

z
) (18)

x
= atan2 (a
x
sin

z
- a
y
cos

z
, o

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

1
)


2
=

12
-

1
(20)


3
= atan2( S
3
, C
3
)

4
=

34
-

3

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.

Sau đó chơng trình sẽ điều khiển các khớp quay của robot để đa tay nắm tới vị trí mong
muốn. Sau đây là lu đồ điều khiển robot 5 bậc tự do gắp vật thể trên cơ sở camera quan sát:

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

,
3
,
4
,
5

Gắ
p
vật
Kết thúc
Vị trí vật thể nằm
trong vùng làm
việc của robot ?
Có
Khôn
g

8
3.2.4. C¸c module ch−¬ng tr×nh vµ c¸c kÕt qu¶ ®¹t ®−îc




9
4. kết luận
Nhìn chung thuật toán đã đạt đợc các yêu cầu đề ra. Quá trình điều khiển đã đợc thực hiện
trên mô hình và đã cho kết quả tốt. Các kết quả đã thể hiện tính đúng đắn của mô hình
Tài liệu tham khảo
[1] Hager. A tutrial on visual control , IEEE transactions on Robotics and Automation vol 12 ,

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