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 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 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 . 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. 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