Giải bài toán cục bộ tránh vật cản cho robot tự hành

Gi¶I bµI to¸n côc bé tr¸nh vËt c¶n cho robot tù hµnh TS. NguyÔn V¨n Gi¸p KS. Tõ DiÖp C«ng Thµnh, KS. §oµn ThÕ Th¶o, KS. NguyÔn V¨n Trung Bé M«n C¬ §iÖn Tö, Khoa C¬ KhÝ, Tr−êng §¹i Häc B¸ch Khoa Tp Hå ChÝ Minh Email: nvgiap@dme.hcmut.edu.vn, tdcthanh@dme.hcmut.edu.vn Tãm t¾t Mét ph−¬ng ph¸p tr¸nh vËt c¶n míi cho robot tù hµnh ®· vµ ®ang ®−îc ph¸t triÓn vµ ngµy cµng hoµn thiÖn, gäi lµ kü thuËt dÉn h−íng thÝch nghi (Adaptive navigation). Víi ph−¬ng ph¸p nµy cho phÐp ®¬n gi¶n hãa bµi to¸n côc bé. Bµi b¸o nµy tËp trung vµo viÖc øng dông ph−¬ng ph¸p dÉn h−íng thÝch nghi ®Ó ®iÒu khiÓn m« h×nh robot tù hµnh tr¸nh vËt c¶n. C¸c c¶m biÕn g¾n trªn robot sÏ ph¸t hiÖn vËt c¶n ®Ó ®iÒu khiÓn hÖ thèng dÉn h−íng ®−a robot ®i tõ ®iÓm xuÊt ph¸t tíi ®iÓm ®Ých mµ kh«ng ch¹m vµo c¸c vËt c¶n. Abstract A local navigation technique with obstacle avoidance, called adaptive navigation, had been developed and perfected. This technique allows to simplify the local problem. This paper focuses on the application of adaptive navigation technique to control a mobile robot. Sensors have to be used to detect obstacles and navigation system must be incorporated into the robot to navigate robot from start point reaching to the goal with obstacle avoidance. 1. GIíI THIÖU Bµi to¸n dÉn h−íng cho robot cã thÓ chia lµm hai lo¹i: bµi to¸n toµn côc (global) vµ bµi to¸n côc bé (local), tïy theo m«i tr−êng lµm viÖc cña robot. ë bµi to¸n toµn côc, m«i tr−êng lµm viÖc cña robot ®· biÕt tr−íc vµ tõ ®ã ®−êng ®i tr¸nh vËt c¶n ®−îc x¸c ®Þnh [5]. Trong bµi to¸n côc bé, m«i tr−êng lµm viÖc cña robot hoµn toµn ch−a biÕt tr−íc, hay chØ biÕt mét phÇn. §èi víi bµi to¸n nµy c¸c lo¹i c¶m biÕn g¾n trªn robot sÏ ph¸t hiÖn vËt c¶n ®Ó ®iÒu khiÓn hÖ thèng dÉn h−íng ®−a robot ®i tõ ®iÓm xuÊt ph¸t tíi ®iÓm ®Ých mµ kh«ng ch¹m vµo c¸c vËt c¶n. Néi dung bµi b¸o nµy chØ tËp trung gi¶i quyÕt bµi to¸n côc bé. Mét ph−¬ng ph¸p dÉn h−íng ®−îc ph¸t triÓn rÊt sím ®ã lµ ph−¬ng ph¸p ®i theo ®−êng biªn vËt c¶n (The wall-following method) [2]. ë ph−¬ng ph¸p nµy, robot di chuyÓn däc theo vËt c¶n víi mét kho¶ng c¸ch ®−îc ®Þnh tr−íc. KiÓu dÉn h−íng nµy th−êng ®−îc sö dông cho nh÷ng robot lµm nh÷ng c«ng viÖc nh−: robot lµm viÖc trªn c¸c ®−êng èng, robot lau nhµ, hót bôi. Víi ph−¬ng ph¸p ph¸t hiÖn biªn cña vËt c¶n (Edge detection method) [3,8], robot sö dông thuËt to¸n ®Ó t×m vÞ trÝ biªn cña vËt c¶n vµ t¹o ra c¸c ®iÓm ®Ých phô (subgoal) råi thùc hiÖn gi¶i thuËt t×m ®−êng ®i tèi −u ®Õn c¸c ®iÓm ®Ých phô ®ã. §Ó cã thÓ x©y dùng ®−îc b¶n ®å vÞ trÝ vËt c¶n, ng−êi ta sö dông ph−¬ng ph¸p chia l−íi (Certainty Grid method) [4], khi ®ã kh«ng gian lµm viÖc cña robot ®−îc chia thµnh nhiÒu m¶ng trong kh«ng gian hai chiÒu, c¸c phÇn tö (square elements) chøa mét gi¸ trÞ nhÊt ®Þnh ®Ó chØ cã hay kh«ng vËt c¶n ë vÞ trÝ ®ã. §èi víi nh÷ng robot cã tèc ®é di chuyÓn cao, ng−êi ta sö dông ph−¬ng ph¸p Potetial Field Methods [6,7]. Bµi b¸o nµy tËp trung vµo ph−¬ng ph¸p dÉn h−íng thÝch nghi [1]. ë ®©y ®iÓm ®Çu, ®iÓm ®Ých vµ kÝch th−íc h×nh häc cña robot ®· biÕt tr−íc, cßn h×nh d¸ng h×nh häc vµ täa ®é cña vËt c¶n ch−a ®−îc biÕt. H−íng di chuyÓn cña robot ®−îc x¸c ®Þnh dùa vµo gãc h−íng tíi ®iÓm ®Ých vµ tÝn hiÖu tõ c¶m biÕn. −u ®iÓm cña ph−¬ng ph¸p nµy lµ sö dông Ýt c¶m biÕn vµ luËt dÉn h−íng ®¬n gi¶n h¬n so víi c¸c ph−¬ng ph¸p kh¸c.     ∆−∆ ∆+∆= 12 12 2 ss ssTRD T ss 12 ∆−∆=∆Ψ (1) ( ) ( )   Ψ Ψ    ∆∆ ∆−∆=   ∆ ∆ sin cos xy yx Y X (2) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( )         ∆ ∆ ∆Ψ +        Ψ =        Ψ − − − iD iD iD iD iD iD iD iD iD Y X Y X Y X 1 1 1 (3) 2. M« H×NH HãA Vµ §éNG HäC ROBOT Hai b¸nh xe phÝa tr−íc cña robot (xem h×nh 1 a) ®−îc dÉn ®éng ®éc lËp b»ng hai ®éng c¬ b−íc th«ng qua bé truyÒn ®ai, cßn b¸nh sau ®Ó ch¹y kh«ng. H×nh 1 : M« h×nh vµ ®éng häc robot. Gäi ∆s1, ∆s2 lÇn l−ît lµ ®o¹n dÞch chuyÓn cña hai b¸nh tr−íc, T lµ kho¶ng c¸ch gi÷a hai b¸nh xe (h×nh 1 b). Tõ ®ã ta tÝnh ®−îc b¸n kÝnh gãc quay RD vµ gãc dÞch chuyÓn ∆ψ. Khi ∆s1 = - ∆s2 th× robot quay quanh ®iÓm gi÷a hai b¸nh xe D. Khi ∆s1 = ∆s2 th× robot ®i theo quÜ ®¹o lµ ®−êng th¼ng. DÞch chuyÓn theo hai ph−¬ng trong hÖ täa ®é tham chiÕu viÕt d−íi d¹ng ma trËn : VÞ trÝ cña robot t¹i thêi ®iÓm thø i ®−îc tÝnh nh− sau : 3 PH−¬NG PH¸P DÉN H−íNG THÝCH NGHI Ký hiÖu sö dông: x(t), y(t): vÞ trÝ cña robot t¹i thêi ®iÓm t. (a) (b)    −−= (t)*θθ(t)η(t).θ (4)   >− ≤+= θ(t)φ(t)π;φ(t) θ(t)φ(t)π;φ(t) (t)θ *t (5) ( )πφφ 2(t)0; x(t) y(t)tan(t) 1 <≤= − (6) v(t): vËn tèc cña robot ë thêi ®iÓm t. θ(t): gãc chØ h−íng cña robot t¹i thêi ®iÓm t (0 ≤ θ(t) < 2π) so víi trôc x. θo: gãc chØ h−íng cña robot t¹i thêi ®iÓm ban ®Çu so víi trôc x. (xo,yo) , (0,0): vÞ trÝ ®Çu vµ vÞ trÝ cuèi cña robot. ϕ(t): gãc h−íng ®Ých hîp bëi ®o¹n th¼ng qua hai ®iÓm (x(t), y(t)) vµ (0,0) so víi trôc x. θ*(t): gãc dÉn h−íng tÝnh to¸n cho robot tíi ®Ých hay tr¸nh vËt c¶n. θ*t(t): gãc dÉn h−íng tÝnh to¸n cho robot tíi ®Ých. θa(t): gãc dÉn h−íng tÝnh to¸n cho robot tr¸nh vËt c¶n. Trªn c¬ së lý thuyÕt vµ theo tµi liÖu [1], cã luËt dÉn h−íng nh− sau : Kh«ng gian lµm viÖc lý thuyÕt cña robot nh− h×nh 2. Khi robot kh«ng ph¸t hiÖn vËt c¶n th× θ*(t) = θ*t(t). Víi : H×nh 2: Kh«ng gian lµm viÖc cña robot trong mÆt ph¼ng 2-D C¸ch øng xö cña robot khi ph¸t hiÖn vËt c¶n. Robot sö dông 3 c¶m biÕn kho¶ng c¸ch ®Ó x¸c ®Þnh kho¶ng c¸ch tõ robot tíi vËt c¶n. Gäi dc, dl, dr lµ kho¶ng c¸ch theo 3 ph−¬ng, dl, dr ®Æt lÖch so víi dc mét gãc α. Khi c¶m biÕn kh«ng ph¸t hiÖn ®−îc vËt c¶n hay kho¶ng c¸ch d > dmax, ngâ ra cña sensor b»ng -1. • Ph¸t hiÖn ba vÞ trÝ : h×nh 3(a) Gãc dÉn h−íng tr¸nh vËt c¶n θa(t): Víi : ε = tan-1((dcosα - dc)/dsinα) ( ε < π/2 ) d = max(dl,dr)   <− ≥+= 0:1 0:1 )sgn( x x x H×nh 3: C¸c tr−êng hîp tr¸nh vËt c¶n cho robot tù hµnh ( )    −−+= ε 2 π ddsgnθ(t)(t)θ rl*a (7) • Ph¸t hiÖn hai vÞ trÝ : Ta cã : (xem h×nh 3b) dc > drcosα vµ dl < 0. KÕt luËn : bªn tr¸i kh«ng cã vËt c¶n. Trªn h×nh 3c th× dc < dlcosα vµ dr < 0, suy ra bªn ph¶i kh«ng cã vËt c¶n. Tuy nhiªn, cã thÓ cã vËt c¶n ë bªn ph¶i nh−ng t¹i thêi ®iÓm tøc thêi c¶m biÕn kh«ng ph¸t hiÖn ®−îc. §Ó ®¶m b¶o tr¸nh ®−îc vËt c¶n, robot nªn quay sang tr¸i mét gãc ( π/2 - ε ). Khi dc <0 (xem h×nh 3d) robot kh«ng cÇn quay (gi¶ thuyÕt kho¶ng c¸ch gi÷a hai vËt c¶n lín h¬n ®−êng kÝnh cña robot). • Ph¸t hiÖn mét vÞ trÝ : Ta cã : (xem h×nh 3e) dc < 0. T−¬ng tù nh− tr−êng hîp ë h×nh 3d, robot kh«ng cÇn quay. Khi dc > 0 (h×nh 3f) robot cÇn ph¶i quay. Tuy nhiªn, dl, dr < 0 tøc lµ bªn tr¸i vµ bªn ph¶i ®Òu kh«ng cã vËt c¶n, tr−êng hîp nµy cho robot quay sang tr¸i mét gãc π/2. 4. §IÒU KHIÓN S¬ ®å khèi m¹ch ®iÒu khiÓn robot b»ng m¸y tÝnh nh− h×nh 4a. V× lµ m« h×nh thÝ nghiÖm nªn ta chän ®iÒu khiÓn b»ng m¸y tÝnh ®Ó thuËn tiÖn cho viÖc nghiªn cøu ®¸p øng cña hÖ thèng. Nh− ®· biÕt cæng m¸y in cña m¸y tÝnh cã thÓ giao tiÕp víi thiÕt bÞ ngo¹i vi qua 12 ®−êng tÝn hiÖu (8 ®−êng xuÊt vµ 5 ®−êng nhËp). §Ó t¨ng sè ®−êng xuÊt nhËp ta cã thÓ sö dông thªm mét sè lo¹i IC chuyªn dïng. Ngoµi ra ta cã thÓ sö dông c¸c kit vi xö lý, ch−¬ng tr×nh n¹p trùc tiÕp lªn ROM. Gi¶i thuËt ®iÒu khiÓn (h×nh 4b) t−¬ng ®èi ®¬n gi¶n nh−ng ®¶m b¶o robot tr¸nh ®−îc vËt c¶n vµ vÒ ®óng ®iÓm ®Ých. Ch−¬ng tr×nh ®iÒu khiÓn ph¶i tháa m·n nh÷ng yªu cÇu sau : - Lu«n cËp nhËt th«ng tin vÒ vÞ trÝ vµ gãc cña robot trong mÆt ph¼ng. - Xö lý tÝn hiÖu c¶m biÕn vµ tÝnh gãc tr¸nh vËt c¶n cho robot θ*a(t). - So s¸nh c¸c gi¸ trÞ gãc lÖch, gãc h−íng ®Ých vµ gãc tr¸nh vËt c¶n ®Ó x¸c ®Þnh gãc dÉn h−íng thÝch hîp θ*(t). ( )    −−+= ε 2 π ddsgnθ(t)(t)θ rc*a (8)    −+= ε 2 πθ(t)(t)θ *a (9) θ(t)(t)θ *a = (10) θ(t)(t)θ *a = (11) 2 πθ(t)(t)θ *a += (12) Tãm t¾t thuËt to¸n x¸c ®Þnh gãc dÉn h−íng θ*(t) cho robot : Begin If dc 0 Then θ*(t) = θ*a(t) Else if dc > 0 Then if θ*t(t) ≥ θ*a(t) ≥ θ(t), or θ*t(t) ≤ θ*a(t) ≤ θ(t) Then θ*(t) = θ*t(t) Else θ*(t) = θ*a(t) End if Else if dl < 0 and θ*t(t) ≥ θ(t), or dr < 0 and θ*t(t) ≤ θ(t) Then θ*(t) = θ*t(t) Else θ*(t) = θ*a(t) End if End. M« h×nh robot ®−îc chÕ t¹o t¹i Bé m«n C¬ §iÖn Tö, Khoa C¬ KhÝ tr−êng §¹i Häc B¸ch Khoa TP. Hå ChÝ Minh ®Ó kiÓm chøng gi¶i thuËt ®iÒu khiÓn cã h×nh d¹ng nh− trªn h×nh 5. Qua nhiÒu lÇn thÝ nghiÖm, robot ®· ®i ®Õn ®iÓm ®Ých mµ kh«ng ch¹m vµo c¸c vËt c¶n. H×nh 4: a) s¬ ®å khèi m¹ch ®iÒu khiÓn b) s¬ ®å gi¶i thuËt ®iÒu khiÓn Kh«ng START ®i th¼ng STOP Quay Robot h−íng ®Õn ®Ých KiÓm tra ®iÓm ®Ých Sai ii ®óng Cã vËt c¶n BÎ l¸i chuyÓn h−íng robot ®äc c¶m biÕn (b) M¸y tÝnh M¹ch xö lý tÝn hiÖu C¶m biÕn M¹ch c«ng suÊt ®iÒu khiÓn ®éng c¬ b−íc (a) 5. KÕT LUËN ViÖc øng dông kü thuËt dÉn h−íng thÝch nghi nµy gióp ta ®¬n gi¶n hãa bµi to¸n dÉn h−íng tr¸nh vËt c¶n cho robot tù hµnh trong bµi to¸n ®iÒu khiÓn côc bé. Ph−¬ng ph¸p nµy kh«ng cÇn sö dông nhiÒu c¶m biÕn vµ luËt dÉn h−íng d−íi d¹ng ph−¬ng tr×nh vi ph©n bËc nhÊt. Tuy nhiªn, víi ph−¬ng ph¸p nµy ta kh«ng thÓ x¸c ®Þnh mét c¸ch ®Çy ®ñ biªn d¹ng cña vËt c¶n, ®Ó lµm ®−îc ®iÒu nµy ta ph¶i xö lý ®ång thêi nhiÒu c¶m biÕn h¬n hoÆc sö dông camera. MÆc dï cßn mét sè h¹n chÕ nh−ng trªn thùc tÕ nh÷ng robot lo¹i nµy ®−îc sö dông trong nhiÒu lÜnh vùc nh− robot hµn, s¬n, vËn chuyÓn trong c¸c nhµ m¸y hay nh÷ng robot phôc vô trong sinh ho¹t. H×nh 5 : M« h×nh robot tù hµnh tr¸nh vËt c¶n. TµI LIÖU THAM KH¶O [1] ATSUSHI FUJIMORI, PETER N. NIKIFORUK, Adaptive Navigation of Mobile Robot with Obstacle Advoidance, IEEE Transactions on Robotics and Automation, Vol-13, August 1997. [2] BAUZIL, G., BRIOT, M. AND RIBES, P., "A Navigation Sub-System Using Ultrasonic Sensors for the Mobile Robot HILARE." 1st In.. Conf. on Robot Vision and Sensory [3] CROWLEY, J. L., "Dynamic World Modeling for an Intelligent Mobile Robot." IEEE Seventh International Conference on Pattern Recognition, Proceedings July 30-August 2, Montreal Canada, 1984, pp. 207-210. [4] ELFES, A., "A Sonar-Based Mapping and Navigation System." Carnegie-Mellon University The Robotics Institute, Technical Report, 1985, pp. 25-30. [5] J. C. LATOMBE, Robot Motion Planning, Kluwer Academic Press, Norwell, MA, 1991. [6] KHATIB, O., Real-Time Obstacle Avoidance for Manipulators and Mobile Robots, 1985 IEEE International Conference on Robotics and Automation, March 25-28, 1985. [7] KROGH, B. H., A Generalized Potential Field Approach to Obstacle Avoidance Control International Robotics Research Conference, Bethlehem, PA, August, 1984. [8] R.S. AHLUWALLA, E.Y. HSU, Sensor-Based Obstruction Avoidance Technique for a Mobile Robot, Journal of Robotic Systems, 1984.