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

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

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

lbdung@ioit.ncst.ac.vn

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

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]

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

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

Adapte

Keyboard

1 2

4

5

6

9

1 1

1

1

1 11

Adapte

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]: ( ) 0 ( 1 ) 0 ( ) 0 ( ) v v v o kT A z v kT e kT v = ư + (3)

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

( (4)

Ma trận vuông A 0 i , i=1,2, ,n chứa các giá trị chưa 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ố chưa biết của (3) ta sử dụng phương pháp trong [5] (k-1)TV kTV (k+1)TV kTV+TC kTV+2TC Hình 2 Quá trình cắt mẫu [ ] [ ] [ ] [ n ]T i n i i i i T n T T T T a a a a A A A n k v k v k v k M 0 6 0 1 10 6 10 1 0 0 6 0 2 0 1 0 0 2 0 1 0 0 0 0 0

)) ( (

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

=

=

=

ư

ư

ư

=

ư

α

α α α α

(5) (6 (7)

v0(k)=(α0)T M0(kư1)+e0(k) (8)

với phương pháp bình phương tối thiểu, ước lượng giá trị vector α0 trong [5]

1

0

) 9 ( )

1 ( ) 1 ( ) 1 (

) 1 ( ) 1 ( ) 1 ( ) 1 ( ) 1 ( 1

)

(

)) 1 ( ) 1 ( 0 ) ( )(

1 (

* ) ( ) 1 ( 0 )

(

0

0

0 0

0 0

0 0

0 0

0 0

0

0 0

0 0

≤

<

















ư

ư

ư +

ư

ư

ư

ư

ư

ư

=

ư

















ư

ư

ư +

ư

=

∧

∧

∧

à

à à

α α

α

k M k P k

M

k P k

M k M k P k

P k

P

k M k

i k

v k M k P k

i

k

j

T

T

T

i

Vị trí vật thể có thể viết từ (2)

0

C V V

V V

V

mT

kT

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

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

Động học ngược

Động học thuận

z0

θ2

O1

y0 z

x0 O

y1

θ3

z2

y2

x2

O2

x1

θ1

z3

y3

x3

O3

4

y4

x4

O4

o n

a

O hand

93 mm

75 mm

28 mm

10 mm

38 mm 70 mm 10 mm

55 mm

R

H

R T H = 1 R 2 2 R 3 3 R 4 4 R 5 5 R H =

























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

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 )

=

























ư

ư +

+

ư

1 0

0 0

z x

y x

y y

y x z x y z x z x y z y z

x x z x y z x z x y z y z

p C

C S

C S

p S C C S S C C S S S C S

p S S C S C C S S S C C C

φ φ φ

φ φ

φ φ φ φ φ φ φ φ φ φ φ φ

φ φ φ φ φ φ φ φ φ φ φ φ

(13)

Từ (11), (12), (13) ta rút ra được hệ phương trình động học thuận:

φ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 )

θ3 = atan2( S 3 , C 3 )

θ = θ - θ

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à lưu đồ đ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 quan 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ủarobot ?

Có

Không

3.2.4 Các module chương trình và các kết quả đạt được

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