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

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 [email protected] 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 kTV có vị trí là P0(kTV) và h−ớng của vật thể là Φ0(kTV), 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 kTV (k = 1 ...) đ−ợc xác định W0(kTV)= [P 0 x(kTv), P 0 y(kTv), P 0 z(kTv), Φ0x(kTv), Φ0y(kTv), Φ0z(kTv)]T (1) Với TV 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 11 1 1 11Adapte 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 Tv + mTc thông qua các thông tin ở thời điểm (k -1)Tv với Tc là chu kỳ cắt mẫu của vòng điều khiển (Tv >Tc) và m = 1....T/Tv 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 v0(kTv) là vận tốc của vật thể ở thời điểm k Tc đ−ợc xác định bằng v0(kTv) = {W 0(kTv) - W 0[(k-1)Tv]}/Tv (2) Với v0(kTv) tốc độ ở thời điểm kTv có thể viết d−ới dạng sau v0(kTv) = [v 0 x(kTv) v 0 y(kTv) v 0 z(kTv), ω0x(kTv) ω 0y(kTv) ω 0z(kTv) ]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 eo(kTv) là sai số mô hình và ma trận A o(z-1) đ−ợc định nghĩa nn o zAzAzAzA −−−− +++= 02021011 ...)( (4) Ma trận vuông A0i , 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-1v0(kTi)=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 [ ] [ ] [ ] [ ]Tniniiii T n TTTT 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 kPkMkMkPkPkP kMkikvkMkPkikj 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 px, py, pz, 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 Động học ng−ợc Động học thuận z0 θ2 O1y0 z x0O y1 θ3 z2 y2 x2 O2 x1 θ1 z3 y3 x3 O3θ θ z4 y4 x4 O4 o n a O hand17 0 m m 93 mm 75 mm 28 mm 10 mm 38 mm 70 mm 10 mm 55 mm R H 6 RTH = 1R2 . 2R3 . 3R4 . 4R5 . 5RH = n o a p n o a p n o a p x x x x y y y y z z z z 0 0 0 1             (11) nx = -S12C5 - C12S34S5 ny = - C12C5 - S12S34S5 nz = C34S5 ox = S12S5 - C12S34C5 oy = -C12S5 - S12S34C5 oz = C34C5 ax = C12C34 ay = S12C34 az = S34 (12) px = ( 125C34 + 38C3 +75 ) C12 + 18S12 + 93C1 py = ( 125C34 + 38C3 +75 ) S12 - 18C12 + 93S1 pz = 125S34 + 38S3 +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 (px , py , pz) đ−ợc xác định nh− sau: RTH = Trans (z, pz ). Trans (y, py ). Trans (x, px ). 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: px = ( 125C34 + 38C3 +75 ) C12 + 18S12 + 93C1 (14) py = ( 125C34 + 38C3 +75 ) S12 - 18C12 + 93S1 (15) pz = 125S34 + 38S3 +160 (16) φz = atan2 ( ny , nx ) (17) φy = atan2 ( -nz , nxcosφz + nysinφz ) (18) φx = atan2 (axsinφz - aycosφz , oycosφz - oxsinφz ) (19) 3. Động học ng−ợc robot 5 bậc tự do [8] θ5 = atan2( -Sφy , CφySφx ) θ1 = atan2( S1 , C1 ) θ2 = θ12 - θ1 (20) θ3 = atan2( S3 , C3 ) θ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à 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ủa robot ? Có Không 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