Autonomous docking of a tracked wheels robot to its tether cable using a vision-based algorithm

Transcription

1 Autonomous ocking of a tracke wheels robot to its tether cable using a vision-base algorithm Fausto Ferreira an Rorigo Ventura Abstract Search an Rescue (SAR) Robotics has been gaining an increasing interest in recent years. In spite of that, there are still many challenges to be aresse in terms of autonomy level. RAPOSA is a semi-autonomous, tracke wheels robot, esigne for SAR operations. This paper presents an autonomous ocking algorithm. The ocking is performe to a ocking station that provies both power an a wireless access point. The ocking station consists on a small, lightweight pyramial structure at the en of a flexible cable, allowing the robot to continue operating while attache to this cable. The autonomous ocking is mainly base on both vision, whenever possible, an ea reckoning (oometry) that was also evelope specially for this work. The results show the effectiveness of the propose algorithm, even in the presence of ifficult environmental conitions. Keywors: Search an Rescue Robotics, Autonomous Docking, Vision base control I. INTRODUCTION In scenarios of natural catastrophes, a quick response by rescue teams is essential, but not always possible ue to the hazars to human teams (e.g., builings near collapse). Search an Rescue (SAR) robots able to operate in these contexts can act rapily [1], for instance to gain situation awareness, an to provie voice contact with victims. RAPOSA is one example of a semi-autonomous robot esigne for these situations [2]. The main features of this robot are motor articulation between its main boy an a frontal arm, the two-sie tracke wheels, the wireless communications with or without the cable (access point at the en of the cable), an the possibility of operating the robot even if it flips own. The tethere option inclues both wireless communication an power. The robot has three vision cameras: two in the frontal arm, pointing forwar in the irection of the boy, an one in the back, to ai ocking operations. This paper is organize as follows. In the next section the oometry is iscusse. Then, the relate work on autonomous ocking is reviewe. The escription of the autonomous ocking is presente next, followe by the experimental results. This paper closes with some conclusions an future work. II. AUTONOMOUS DOCKING Several methos for automatic ocking can be foun in the literature. The use of computer vision has some avantages: This work was partially supporte by FCT (ISR/IST plurianual funing) through the POS Conhecimento Program that inclues FEDER funs. Fausto Ferreira an Rorigo Ventura are with Institute for Systems an Robotics / Instituto Superior Técnico, Lisbon, Portugal {fferreira,yoa}@isr.ist.utl.pt. on the one han, it is a passive sensor eliminating the nee of an emitting source, an on the other han, other sensors like ultrasonic ones or infrare are not as accurate. In this work, only one camera is use to provie feeback to the control algorithm. Color segmentation, together with some geometric consierations, are use to perform the ocking. A. Relate work The first successful attempts to an autonomous mobile robot recharge were in the late 40 s with the Elsie an Elmer the so calle Machina Speculatrix by Walter [3]. These turtle-shape robots ha a light following behavior into a hut that containe the battery recharger an a light bulb. In the last ecae, there were several ocking strategies for recharging purposes, as well as other purposes. In [4] a Noma XR4000 use a IR beacon, a sonar, an a Sick laser range finer to ock to a recharging station. More recently, a Raio-Frequency IDentification (RFID) ocking was presente in [5]. Among the ones that use computer vision, in [6] it is escribe a ocking system for marsupial robots with an imprinting biological inspire mechanism. Vision an a laser range finer were use in [7] for a Pioner 2DX. In [8], the Noma has an omniirectional camera an uses 3 non-collinear lanmarks an their bearings to rive autonomously to the ocking station. In [9], a circle on a wall is use to ai the ocking. Another example of a ocking purely relying on vision is escribe in [10] but it involves comparing features to a moel store in atabase. B. Docking system The ocking system is compose by two parts: the grabbing mechanism (insie the robot) an the cable. The cable is flexible but at the en it has a soli pyramial structure esigne to provie a fixe inclination of it while laying on the groun (Fig. 1). To perform the ocking the robot shoul lower its back an enter the metal guie with a certain orientation. Fig. 2 shows the ocking socket at RAPOSA s back, with the camera visible insie the robot. In its back part, there are two sliing oors that grab an pull the pyrami into the robot (an effect of bi-conical shape at the en). The effectiveness of this solution from the esign point of view was aress before in [11]. Nevertheless, it is important to note that this system allows real-time operation because it provies power an communications from the ocking station. Comparing with the state-of-the-art, with the exception of [6], most of the publications on this subject use a visual target as an auxiliary tool to make the ocking possible.

2 alignment maneuver is necessary to bring it aligne with the pyrami axis, thus reucing α to a small value. The strategy followe to tackle this problem consists in iviing the space aroun the pyrami in angular sectors, relative to the pyrami axis, as shown in Fig. 4. Fig. 1. Docking pyrami In [6], the robot moves towars the mother robot but the marker is relatively large (larger areas are easier to etect) an it is always visible. In RAPOSA, the situation is more complex because the ocking station enters the robot by the hole that is use by the back camera to look for it. Moreover, the optical center of the camera is not mounte at the height of the station an the robot has to lower its back part in orer to ock. So, control in the y-axis is also one. It coul not be foun any other work that controls in 3D the robot to enter the ocking station recurring only to vision. C. Solution propose The goal is to start the autonomous ocking algorithm from the moment that the pyrami is visible by the back camera. The algorithm ens with the fastening of the sliing oors. Due to the location of the camera an the insertion hole size, the useful Fiel of View (FOV) is approximately 25. To minimally a visible marks to ai the vision algorithm, the bi-conical metal guie was painte with orange, an some eges of the pyrami were painte with blue (Fig. 2). This allows to both etect the cable en, an to estimate the relative orientation of the pyrami to the robot. Fig. 3. Top view of the geometry of the problem: the horizontal axis is aligne with the pyrami, at the top left corner of the figure, while the robot is shown at the bottom right corner, with the pyrami in the FOV of the back camera. Four regions were efine: region I, where the robot approaches the ocking station going forwar, while using visual servoing to make small ajustments to compensate for rifts in the alignment; region II, where the robot can etect both colors to estimate the angle α, thus performing a maneuver to place the robot within region I (an then use the region I strategy to perform the final approach); region III, where it is not possible for the robot to ock successfuly by lack of visual information, an a eazone too close to the robot for it to operate without coliing with the pyrami. If α 73 while in region II, the estimate α is incorrectly estimate, ue to visual ambiguity, however it can be prove that the manoeuver performe in region II always re-positions the robot in region II with α < 73. The istinction between region I an II is not a straight line. A tolerance was introuce within a certain istance range, after preliminary experimentation, to accommoate for oometry errors that prevente the manoeuver in region II to conuct the robot to region I. Fig. 2. Left: ocking socket with the back camera visible insie the robot. Right: ocking pyrami with the blue eges an the orange metal guie. 1) Docking regions: Fig. 3 epicts a top view of the geometry of the problem. The robot is assume to have the pyrami within the FOV of its back camera. The relative angle α that the pyrami axis makes with the robot position is crucial to the ocking process: when this angle is small, the robot is able to ock in a straightforwar fashion, by going forwar 1 while performing small ajustments. Otherwise, an 1 In fact, the robot moves backwars relative to its front, since the robot ocks its back with the ocking station. We will, however, ahere to the wor forwar to avoi confusion. Fig. 4. The efine regions aroun the pyrami (top view). 2) Mathematical escription: All the computation is base on the pin-hole moel of the camera an on the perspective projection. The angle α is estimate from the etecte blue rectangle (pyrami corners) an orange ellipse

3 (ball at the pyrami en). The istance is estimate base on the geometric properties of the etecte blue regions, whenever the robot is not aligne with the pyrami. Otherwise, it can use simply the orange ellipse area because the ellipse will maintain its shape as long as the robot moves in the irection of the pyrami. The blue color is not use here because occlusions occur for some istance range, while the orange is essential an enough for little misalignments. From the sie view of the robot (Fig. 5), one can get by trigonometric relationships: g f = h, (1) where f enotes the focal length of the camera, is the istance between the focal point an the vertical passing through the top blue vertex of the pyrami, g is the height of the blue rectangle etecte in pixels, while h is an approximation of the length of the blue ege. While h is the real length of the ege, this approximation h h has a reasonable small error (max 1.5%). So, (1) is approximate here by g f h. (2) Fig. 5. Sie view from the ocking pyrami an its corresponing projection From the top view (Fig. 6), the geometric relationship to estimate c is given by c f = a + b = a + b f. (3) However, since cannot be accurately estimate, we use the relation c f = a + b a + b, (4) while assuming a a. The variables a an b are given by a = m sin(α), (5) b = h cos(α), (6) 2 where m is the istance between the mipoint of a blue ege an the center of the ellipse an α the angle to be estimate. Using (4) an (2), we can obtain chm sin(α) = g(m 2 + ( h 2 )2 ) ± (7) ( ch g m)2 (m 2 + ( h 2 )2 )(( ch g )2 ( h 2 )2 ) (m 2 + ( h. 2 )2 ) As any quaratic equation, (7) has always 2 solutions (ignoring singularities). So, one has to chose one of them. If Fig. 6. Top view from the ocking pyrami an its corresponing projection one equals the term ( ch g )2 ( h 2 )2 c ) to 0, g is 0.5 meaning that the istance from the ellipse center to the ege en is half the with of the ege. This situation occurs at 0 so it implies sin(α) = 0 which requires choosing the minus sign on the solution of the quaratic equation. For continuity reasons, the solution use will be always with the minus sign. Even though, there are still 2 possible solutions. The inflexion point correspons to about 73 which means that the angles estimate will be in the range [0, 73] (for both sies). For angles greater than that inflexion point there is an estimation error. Although, the robot will always get closer to the pyrami an will enter a position where α < 73 eliminating the error after one step for initial angles lower than 110 which is also the maximum angle for which the robot can see both colors without occlusions. When the robot is not aligne (non-small α), there is the nee to align it. To o so, an taking in consieration the geometry formulation in Fig. 3, the robot rotates β at a first step (relative to referential where the robot is looking straightforwar to the pyrami), travels the istance an then rotates ϕ. This will position the robot at a istance δ of the pyrami aligne with it. 3) Vision algorithm: The high level vision algorithm coul be generically escribe in 3 steps: color base image segmentation shape base lanmark extraction lanmark recognition For the color base segmentation, the color space chosen was HSV (Hue, Saturation, Value). A color is specifie by a volume in this space, efine by the intersection of one interval for each axis H, S, an V. These slices offer a more robust etection performance, since RGB is very sensitive to lighting [12]. Still, lighting changes prevent the use of fixe an unchangeable threshols to efine the bans of H, S an V components. So, it is given to the user the possibility of ajusting the threshols in real time. The use of HSV allows for an intuitive tuning, contrasting with ajustment in RGB that coul be fastiious. For the shape base analysis, several metrics are use both for elimination an for choosing the best caniate among several ones. For the orange color there is an ellipse fitting for all the orange blobs an then some ellipses are filtere out first by area an then by ratio between axes. For the blue color, the process is similar. But here, the fitting of the rectangle is not by the circumscribe rectangle of minimal

4 area but by its bouning region. This is so because using the bouning region is better in the presence of a ba etection (or occlusions) as shown in Fig. 7. After this, other geometric constrains are use to eliminate pairs of rectangles an ellipses that cannot correspon to the physical imensions of the object. There is a maximum eviation both for height an with scale to the etection istance. There is also an area ratio criterion between the ellipse an the rectangle. A tracking of the ellipse is one: any pair with an ellipse out of a ball of 20 pixels (configurable) aroun the previous ellipse is ignore. After all these filters, if there is more than one possible pair, the same criteria are use but this time tighter. In this way, one is more sensible to noise but in the case of a ba etection where there is only one pair, there is no risk of eliminating it by too strict constrains. Fig. 7 is an example of a well chosen caniate among 3 possible ones. Fig. 7. On the left, the original image. In the mile, the caniate pairs after the filtering operation. On the right, the chosen caniate by the metrics explaine in the main text. After etecting the best orange an blue caniates regions, one of 7 states is chosen. Each state etermines a specific course of action, as escribe in the next section. The choice of the state is performe as follows. After choosing the best pair, the state is chosen between 1 or 2 accoring to the angle an istance. If the robot is too close to the pyrami, the estimation of the angle is not accurate an then state 1 is chosen. That implies the eazone mentione. For angles bigger than 15 an/or istances bigger than 34cm, state 2 is chosen. If there is no pair at the en of the etection, then the state is chosen by the orange ellipses etecte at a first option. The best ellipse is chosen (by the same criteria) an epening on the istance, state 1, 3 (big istance) or 6 (small istance) is chosen. In the case of no etecte ellipse, if there is any blue rectangle state 4 is chosen, otherwise state 5 is the one use. When the robot is at either states 6 or 7, further state choices are inhibite, i.e., it remains in that state until the corresponing behaviour finishes (namely, the completion of a ocking or a rotation). The en of state 7 is etecte base on a preictor to prevent the loss of the ellipse (in the case that the robot has to rotate all the FOV) ue to the elay on the control cycle. The preictor uses the ifference between the present an the previous frame to preict where the robot will be in the next one. D. Motor schema For each state, the velocities are compute in ifferent ways. The arm position is also controlle but only at the en of state 1 near the ocking pyrami. In all other states, the arm is kept at a constant position. For region I, states 1 an 6 are use. In state 1, the linear velocity is constant when the robot is above a certain istance ( 1m). Below that istance, linear velocity is given by K linear Aellipse, where A ellipse is the ratio between the ellipse area at 1m (offline reference image) an the ellipse area at the current frame, an K linear is a gain. The square root is use to make the velocity approximately linear with the istance, because the area of an ellipse ecreases quaratically with the istance. The angular velocity is given by K angular x eviation, where K angular is a gain, while x eviation is the shift in pixels in the x-axis from the image center. The arm position is set at the en of state 1 to K arm y eviation, where K arm is a gain, while y eviation is the shift in pixels in the y-axis from the image center. At the same time that the arm starts to move, the lights are turne on. This is because the state 6 is use when the area etecte is higher than a certain threshol (corresponent to 0.3m). In this situation the lights help the etection nearby. In this final state (6), the velocity is kept constant at a low value until a certain amount of time. After this, the robot stops an starts closing the bi-conical metal guies (see section II-B). For region II, RAPOSA cannot keep visual contact with the pyrami at all times, an thus it has to rely on oometry (see section II-E). Oometry is use only whenever the pyrami is out of the robot FOV. Visual oometry is not avise ue to the low quality of the images. The rotation base on vision correspons to state 7. The first step of the algorithm for region II is a pure rotation of β. Whenever the angle β lies within the FOV of the image, the first step in region II will use only state 7, using visual servoing alone. When it is not, the angle to rotate is ivie in two segments: the first part is limite by the FOV (state 7) an the secon part is an oometry base rotation. The velocities use in state 7 have a low constant value so it can track the ellipse uring the rotation. But, if the robot looses the ellipse for some moments, it stops. If the number of sequence frames without ientifying again the ellipse is too high, it exits state 7 an recomputes a new state. To reuce the oometry error, an oometric calibration is one, where the origin is reset after the en of state 7 with the angle rotate in that state. The inverse controller technique is use then in state 2. The velocities resulte from the controller are saturate an scale, an ifferent tolerances are use for each step mentione at the en of subsubsection II-C.2. The x ref an y ref of Fig. 3 are the references for traveling istance. The use of a bi-imensional controller is justifie by the possibility of correcting some orientation error in the mile step of going straight istance. The last rotation is not one base on vision ue to the blurring noise. It is preferable to rotate base on oometry an then, after the robot is stoppe, if it oes not ientify the pyrami, rotate slowly until it fins it again. This rotation is not performe always to the same sie, but rather to the sie opposite to the one in the previous rotation.

5 Moreover, instea of enlessly rotating if no lanmark is foun, the robot switches irection after a certain angle lower than 360. While no lanmark is foun, this angle is ouble after each switch, so that is starts scanning a small angular range, an increases this range progressively. E. Oometry Oometry was not alreay implemente in the robot, so there was the nee to evelop it. It is extremely unavise in the literature to use ea reckoning in this kin of tracke vehicles [13], [14]. Although RAPOSA is a ifferential rive vehicle, it is a particular case of it name ski steering. In ski-steer vehicles, there is always a large slippage whenever the vehicle turns. In RAPOSA, the center of rotation also changes, epening on the position of the frontal arm. One possible solution to estimate a more accurate kinematic moel coul be to use the approach escribe in [15], but it woul nee aitional external sensors that are not feasible to introuce in RAPOSA. Consiering the classic unicycle kinematic moel (Fig. 8), the equations relating the vehicle velocity an the spee of each wheel are the following: V = h ω r + ω l 2 ω = h ω r ω l = V r + V l 2 = V r V l where V an ω are respectively the linear an angular velocities, V r an V l enote the right an left wheel spees, h is the wheel raius (assume equal to both wheels), an is the istance between the wheels. As it is escribe in [13], the istance, in the case of a tracke wheel vehicle, lies within an interval between min an max (see Fig. 9). This istance is not actually fixe, epening for instance on the groun physical properties. However, in this implementation it was empirically estimate an consiere a constant. (8) (9) Low level issues, such as non-accurate access to each wheel velocity, together with an analog rift introuce a significant error margin to the oometry measurement. F. Results In orer to compare the results of this work with others, the benchmark use in [6] an shown in Fig. 10 was chosen. These points are the starting points for the robot an in each point the robot is pointing towars the ocking station. Fig. 10. The gri of points use in both works. However, this gri of points is not completely comparable. Their robot ha a FOV of 70 but for reliability reasons they efine an area with a maximum of 60 (for each sie). RAPOSA s FOV is 25 so using a region of about 120 is a more emaning challenge. The results for this gri of points are presente in Table I, showing the mean time to ock, the mean velocity (v mean ), the stanar eviation (σ), an the percentage of successful ockings (% success ). The number of trials was 4 for all the points. Points 1 an 3 are not presente here because the robot was not able to ock in any of the trials. These points are insie the eazone iscusse previously, an so this was expecte to occur. For point 4 it was not possible to ock in one of the trials. It is clear that for the 0 angle the stanar eviation is kept very low (less than 2.5s) but the mean velocity increases. This can be unerstoo uner the light of the motors schema presente: recall that the velocity is linearly proportional to the istance except when it is too far or when it is too close, where it is kept constant at a moerate spee Fig. 8. Kinematic moel of a unicycle vehicle. Fig. 9. Range of the istance for the case of RAPOSA. TABLE I RESULTS AS A FUNCTION OF THE ANGLE. Angle ( ) Points Mean Time to Dock (s) σ (s) v mean (cm/s) % success , , , , ,

6 or very low spee, respectively. This fact explains the results, since for points 9 an 14 the robot will move at a constant spee in the beginning, ecreasing its spee only after a while. In point 2 the weight of the low spee moe is greater, thus explaining a lower mean velocity. For the 30 angle, the situation is the same for the mean velocity (increasing here σ). The reasons here are ifferent, since for a higher istance, the movement is softer an more reliable because vision is use longer, an the generate trajectory has smaller rotations. Using vision prouces less error at the en of state 2, which contributes to a faster convergence towars region I. Moreover, for a greater istance, the angle estimation error is smaller because the approximations use have smaller errors. For the 60 angle, the analysis for the mean velocity is the same than for the 30 angle. The stanar eviation is much larger than before even though at 2m is significantly lower than for the other istances. Softer movements an low estimation error explain that fact. The worst situation occurs for points 4 an 6 in terms of mean velocity. The lower v mean makes sense, after consiering the fact that the robot has to move backwars an that increases the travele istance. The gri of points use in the experiments above coul not explore all the possibilities of the algorithm, incluing the recovery of the estimation error in one step for angles lower than about 110. So, other points were teste incluing angles of 80, 90 an 110. For all these points, the robot successfully although sometimes with too much time ranging from 1min till almost 4min for istances of about 1.5m. The minimum iterations on region II was 2 as expecte an the maximum was 4. The problem was not with the angle estimation after the first iteration. The problems are mostly relate with the oometry an with the noise in states 3, 4 an 5. Remember that when there is an estimation error, the robot might not be aligne after the first iteration, an might enter one of those states (3, 4 or 5). The robot coul manage to ientify correctly the pyrami but sometimes took longer because it entere state 3 or 4 with other objects before fining the pyrami. The maximum istance was also teste at 0 angle. After aing a point at 2.5m with 100% of ocking success, other istances were teste. At 2.7m the robot performe well, at 2.8m it ocke successfully only in 75% of the trials, an at 3m it oes not ock at all. At 3m it is not possible to extract enough information from the image receive. For other angles, the maximum istance is the 2m raius iscusse previously. Finally, the robustness to experimental conitions was teste an the robot was able to ock in an outsie environment even though the oometry was not correcte for that soil. Fig. 11 shows a very noisy example for which the robot ocke. Even for an angle ifferent of 0 (about 25 ) it manage to ock in less than 80s even though there are big ifferences in the terrain. III. CONCLUSION AND FUTURE WORK An autonomous ocking algorithm for a SAR robot was presente in this paper. This mechanism employs vision most of the time, recurring to the oometry only when eeme Fig. 11. An example of the color etection an segmentation in outoor environment. necessary. Results have shown a goo performance. Future work inclues improvement of the oometry, which being a tracke wheels robot it becomes a non-trivial task, as well as improvement of the color segmentation, so that the nee to ajust manually the HSV bans threshols, e.g. because of lighting conitions, can be eliminate. REFERENCES [1] R. Murphy, J. Casper, J. Hyams, M. Micire, an B. Minten, Mobility an sensing emans in usar, in IECON-2000: 26th Annual Conference of the IEEE, vol. 1. Inustrial Electronics Society, 2000, pp [2] C. Marques, J. Cristvo, P. Lima, J. Frazo, I. Ribeiro, an R. Ventura, Raposa: Semi-autonomous robot for rescue operations, Intelligent Robots an Systems, 2006 IEEE/RSJ International Conference on, pp , Oct [3] W. G. Walter, The Living Brain. New York: W. W. Norton, [4] K. T. Seungjun Oh, Alexaner Zelinsky, Autonomous battery recharging for inoor mobile robots, in Proceeings of Australian Conference on Robotics an Automation (ACRA2000), [5] M. Kim, H. W. Kim, an N. Y. Chong, Automate robot ocking using irection sensing rfi, 2007 IEEE International Conference on Robotics an Automation (ICRA 2007). Proceeings., pp , April [6] B. Minten, R. Murphy, J. Hyams, an M. Micire, Low-orercomplexity vision-base ocking, IEEE Transactions on Robotics an Automation, vol. 17, no. 6, pp , December [7] M. C. Silverman, D. Nies, B. Jung, an G. S. Sukhatme, Staying alive: a ocking station for autonomous robot recharging, IEEE International Conference on Robotics an Automation, (ICRA 02). Proceeings., vol. 1, pp vol.1, May [8] R. Wei, R. Mahony, an D. Austin, A bearing-only control law for stable ocking of unicycles, IEEE/RSJ International Conference on Intelligent Robots an Systems, (IROS 2003). Proceeings., vol. 4, pp vol.3, Oct [9] R. Luo, C. Liao, K. Su, an K. Lin, Automatic ocking an recharging system for autonomous security robot, IEEE/RSJ International Conference on Intelligent Robots an Systems, (IROS 2005)., pp , Aug [10] U. Kartoun, H. Stern, Y. Ean, C. Feie, J. Hanler, M. Smith, an M. Gillam, Vision-base autonomous robot self-ocking an recharging, Worl Automation Congress, WAC 06, pp. 1 8, July [11] C. Marques, J. Cristovão, P. Alvito, P. Lima, J. Frazão, M. I. Ribeiro, an R. Ventura, A search an rescue robot with tele-operate tether ocking system, Inustrial Robot, vol. 34, no. 4, pp , [12] V. L. B. Bascle, O. Bernier, Learning invariants to illumination changes typical of inoor environments: Application to image color correction, International Journal of Imaging Systems an Technology, vol. 17, no. Issue 3, pp , Oct [13] H. R. Everett, Sensors for mobile robots: theory an application. Natick, MA, USA: A. K. Peters, Lt., [14] R. Siegwart an I. Nourbakhsh, Introuction to Autonomous Mobile Robots. Brafor Book, [15] J. Martinez, A. Manow, J. Morales, A. Garcia, an S. Peraza, Kinematic moelling of tracke vehicles by experimental ientification, IEEE/RSJ International Conference on Intelligent Robots an Systems, (IROS 2004). Proceeings., vol. 2, pp , Oct 2004.