Xây dựng hệ thí nghiệm RoBot trên cơ sở camera

Chia sẻ: Nguyen Van Chung Chung | Ngày: | Loại File: PDF | Số trang:9

0
75
lượt xem
32
download

Xây dựng hệ thí nghiệm RoBot trên cơ sở camera

Mô tả tài liệu
  Download Vui lòng tải xuống để xem tài liệu đầy đủ

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 Robot với quá trình nhận dạng vật thể động trên cơ sở camera

Chủ đề:
Lưu

Nội dung Text: Xây dựng hệ thí nghiệm RoBot 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 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] 1
  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Ó 1 2 1 6 1 1 4 9 5 PhÇn mÒm 11 Adapte 1 Adapte Nguån ®iÖn 1 Keyboard 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µ P (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 0 thÊy qua c¸c thêi ®iÓm kTV (k = 1 ...) ®−îc x¸c ®Þnh W0(kTV)= [P0x(kTv), P0y(kTv), P0z(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 2
  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 kTV+TC kTV+2TC (k-1)TV kTV (k+1)TV H×nh 2 Qu¸ tr×nh c¾t mÉu 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) = {W0(kTv) - W0[(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) = [v0x(kTv) v0y(kTv) v0z(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]: −1 v o ( kT v ) = A 0 (z ) v 0 ( kT v ) + e 0 ( kT v ) (3) o o -1 vector e (kTv) lµ sai sè m« h×nh vµ ma trËn A (z ) ®−îc ®Þnh nghÜa Ao ( z −1 ) = A10 z −1 + A2 z −2 + ... + An z − n 0 0 (4) 0 -1 Ma trËn vu«ng A i , i=1,2,...,n chøa c¸c gi¸ trÞ ch−a biÕt, z lµ to¸n tö trÔ liªn quan ®Õn chu kú cÊt mÉu quan s¸t vµ z-1v0(kTi)=v0[(k-1)T] ; n- sè nguyªn lµ ®é lín cña Ma trËn [ M 0 ( k − 1) = ( v 0 ( k − 1)) T ( v 0 ( k − 2 )) T .....( v 0 ( k − n )) T ] T (5) α 0 = [A10 A 20 ... A n ] = [α 0 T 0 1 α 2 ...α 6 ] 0 0 (6 α i0 = [a i10 ... a i10 ... a in10 ... a in60 ] T 1 6 (7) 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] 3
  4. v 0 (k ) = (α 0 )T M 0 (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] ∧ T ∧  ∧  α 0 j ( k ) = α 0 ( k − 1) + P 0 ( k ) * M 0 ( k − 1)( v i0 ( k ) − α 0 ( k − 1)  M 0 ( k − 1)) i  i    P 0 (k ) = 1  0  P ( k − 1) − [T ] P 0 ( k − 1) M 0 ( k − 1) M 0 ( k − 1) P 0 ( k − 1)   (9 ) µ0   [ T ] µ 0 + M 0 ( k − 1) P 0 ( k − 1) M 0 ( k − 1)   0< µ0 ≤1 VÞ trÝ vËt thÓ cã thÓ viÕt tõ (2) ∧ ∧ ∧ W 0 [kT + mT C /( k − 1)TV ] = W 0 [kT V /( k − 1)TV ] + v 0 [kT V /( k − 1)TV ]mT C (10 ) 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 z θ3 θ2 θ4 θ5 θ1 y θ6 o a n x 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. 4
  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  x θ  y  2 §éng häc thuËn   θ3  z θ=   φ  θ  4 §éng häc ng−îc  x  θ5  φ y  θ6    φ 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 z θ2 y1 y4 o z0 θ1 y2 10 mm y3 x4 n 93 mm x1 x3 170 mm y0 O1 x2 H a θ3 O4 θ z4 O hand O2 O3 R O x0 z2 θ z3 75 mm 28 mm 55 mm 10 mm 38 mm 70 mm 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 5
  6. nx ox ax px  n oy ay py  TH = R2 . R3 . R4 . R5 . RH =   R 1 2 3 4 5 y (11) nz oz az pz  0  0 0 1  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: R TH = Trans (z, pz ). Trans (y, py ). Trans (x, px ). Rot( z , φz ). Rot( y , φy ). Rot( x , φx ) Cφ z Cφ y C φ z Sφ y Sφ x − Sφ z C φ x C φ z Sφ y C φ x + Sφ z Sφ x px   Sφ C φ Sφ z Sφ y Sφ x + C φ z C φ x Sφ z Sφ y C φ x − C φ z Sφ x py  =  z y  (13)  − Sφ y Cφ y Sφ x Cφ y C φ x pz     0 0 0 1 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 6
  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 VÞ trÝ vËt thÓ n»m Kh«ng trong vïng lµm viÖc cña robot ? Cã ¦í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 7
  8. 3.2.4. C¸c module ch−¬ng tr×nh vµ c¸c kÕt qu¶ ®¹t ®−îc 8
  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 9

CÓ THỂ BẠN MUỐN DOWNLOAD

Đồng bộ tài khoản