Towards an appearance-based approach to the leader-follower formation problem

Transcription

1 Towards an appearance-based approach to the leader-follower formation problem James Oliver Frédéric Labrosse Department of Computer Science University of Wales, Aberystwyth Aberystwyth SY23 3DB, United Kingdom Published in: Proceedings of Towards Autonomous Systems, pages (2007), University of Wales, Aberystwyth, United Kingdom,

2 Towards an appearance-based approach to the leader-follower formation problem James Oliver Frédéric Labrosse Department of Computer Science, University of Wales Aberystwyth, Ceredigion, SY23 3ET, UK [jto2 Abstract We present in this paper an application of previously developed techniques to the leader-follower formation problem, techniques that exclusively use vision. Contrary to other vision-based methods, the only information needed to perform this task is a set of images of the back of the leader robot that will be tracked by the follower robot. For this, the follower robot controls its (translational and rotational) speed by performing only pixel-wise comparisons between the tracked images and the current image of its surroundings. Results are presented that quantify and qualify the performance of the method in a number of situations, all using real robots in our research lab. Assumptions and limitations are discussed. 1 Introduction Convoys of vehicles are used in a large variety of situations. For example, the army uses convoys to bring resources to remotely stationed units, and the mining industry uses convoys of dumper trucks from excavation sites to processing sites. Often, these convoys operate in dangerous situations and involve many human drivers. Automating such convoys is therefore a valuable application for robotics. In such an application, a leader robot could be either tele-operated or equipped with the necessary sensors and intelligence to make it autonomous and a number of follower robots would then just have to follow that leader (or follow each other) to create the convoy. This problem has been tackled by many researchers, in particular on the aspects of controlling the geometrical formation (Das et al., 2002, Mariottini et al., 2005, Renaud et al., 2004). Most of these methods use vision, possibly combined with some other sources of relative positioning of the robots and some explicit communication. In this paper we concentrate on the vision aspects of the problem and in particular on the localisation and tracking in images of the leader robot. Most of the related research uses omni-directional cameras because they provide images in which the leader robot is potentially always visible. There are exceptions, such as (Chiem and Cervera, 2004) where a panning camera is used. The visual processing also varies between researchers and is usually simplified to the extreme. For example, a known coloured target installed on the leader is tracked in (Chiem and Cervera, 2004) to estimate distance and orientation of the leader. In (Schneiderman et al., 1995), a geometrical model of a 2D target painted on the back of the leader is used. Requiring less engineering of the leader, the transformation between desired view and current view using a set of matched feature points on the leader is found in (Malis and Chaumette, 2000). The optical flow computed in omni-directional images is used in a control law to reach and keep a desired configuration in (Vidal et al., 2003). However, these methods using features require the detection and matching of these features, which is usually an expensive and not necessarily reliable process in general situations. Computing the optical flow shares most of these problems too. (Eklund et al., 1994) use a correlation filter-based comparison of the target and current views, using optimal filters, and adapt the filters to accommodate for 3D variations of the target. This method is appealing but requires the non-obvious step of determining the filters. Moreover, adapting the filters to cope with visual changes of the target could lead to drift in the used filter that could result in detection failure. In most cases, vision is used to explicitly find the position and possibly orientation of the leader with respect to the follower so that a good (maybe optimal) control law can be devised. On the contrary, we do not detect either the explicit distance between leader and follower, or their relative orientation. Moreover, we only perform low level image, pixel by pixel, comparisons of the current view obtained by the follower and images of the leader seen by the follower from various positions around the ideal relative position. The camera used is of the omni-directional type mostly because of its interesting projection properties (see below). We do not address the problem of planning an accurate

3 trajectory and only use a simple control loop and only consider the formation where the follower is behind the leader at a fixed ideal distance. The reminder of the paper is organised as follows. Section 2 describes the method proposed in this paper with, in Section 2.2, the image comparison method and, in Section 2.3, the control loop. The result of experiments are presented in Section 3 and a discussion and conclusion are given in Section 4. (a) Leader at the correct distance (b) Leader too close 2 The method In this work, in line with our previous work, we do not explicitly extract features from the images that are grabbed. Indeed, such feature extraction and subsequent matching is often computationally expensive and/or requires many assumptions often violated in most but contrived situations. Instead, we perform a simple pixel-wise comparison between images of the back of the leader robot and the current image of the surroundings of the follower robot to localise the leader in that image. This uses a principle that is similar to that we use to compute the orientation of a robot (Labrosse, 2006) or to navigate to targets (Labrosse, 2007). The camera used is omni-directional and the images are unwrapped into panoramic images without any geometrical corrections (see (Labrosse, 2006) for a description of the process and Figure 1 for examples). Closely following our previous work, we first describe how images are compared and show that this cannot be directly used in the context of localising a moving target. We then propose a better method and show how it is used in the control loop of the follower. 2.1 A first naive method An image of the leader (I l ) is compared to a part of the image grabbed by the follower centred at position (x, y) (I f (x, y)) using the Euclidean distance between corresponding pixel values as follows: d(i l, I f (x, y)) = h w c (I l (k, i) I f (x, y, k, i)) 2, k=1 i=1 (1) where I l (k, i) and I f (x, y, k, i) are the i th colour component of the k th pixel of images I l and I f (x, y) respectively, each of size h w and having c colour components. Pixels are enumerated in both images, without loss of generality, in raster order from top-left corner to bottom-right corner. We used for this work the RGB (Red-Green-Blue) colour space, thus having three components per pixel. The combination of Euclidean distance and RGB space is not necessarily the best to use but it is sufficient for our purposes (see (Labrosse, 2006) for a discussion). This function should show a minimum for the position (c) Leader too far away (d) Leader Figure 1: Typical images grabbed by the follower and image of the leader (x l, y l ) where the image of the leader is in the current image of the follower. Figure 1 shows three typical images grabbed by the follower as well as an image of the back of the leader. Figure 2 shows the distance between the image of the leader and the images grabbed by the follower shown in Figure 1. It is clear that when the follower is at the correct distance from the leader, then the distance in Equation (1) presents a minimum at the correct position (approximately at position x = 260). This is also the case when the follower is too close to the leader, although the minimum is not as well marked (and the absolute minimum in that case is not exactly where it should be). However, when the leader-follower distance is too large, the distance between the corresponding images not only does not present a minimum at the desired position, but the absolute minimum corresponds to another event in the image, leading to a wrong estimation of the position of the leader in the image. This is because the part of the image corresponding to the leader is now not larger than other events of the background, that could be considered as noise when compared to the reference image of the leader. This implies that a multi-resolution scheme must be used. Another remark to be made is that the valley surrounding the absolute minimum in Figure 2(a) is very narrow, in particular much narrower than in our previous work (Labrosse, 2006, Labrosse, 2007). Moreover, its size is related to the size of the leader in the images. A better, multi-resolution method is therefore needed to localise the leader in the images grabbed by the follower.

4 (a) Leader at the correct distance (b) Leader too close (c) Leader too far away Figure 2: Distance as a function of (x, y) between the image of the leader and the grabbed images in Figure Localisation of the leader Informal experiments showed that a reasonably good estimate of the position of the leader in images can be found even if the distance follower-to-leader does not exactly correspond to that of the image of the leader (Figure 1(c) shows a rather extreme situation). Moreover, Figure 2(b) shows that when the leader is too close to the follower a good estimate of the leader s position can still be found. Three images of the leader were therefore used; the largest corresponds to the leader at the correct distance (I l1, of size 23 21), the smallest (I l3, of size 10 9) was taken from an image grabbed with the leader at what was considered a maximum distance (approximately 3 m in the reported experiments, due to space constraints imposed by our lab, the correct distance being approximately 1.5 m) and one created by down sampling the large image to a size approximately midway between the large and small images (I l2, of size 18 16). The images are given in Figure 3. Because the three images are now of different sizes, the Euclidean distance in Equation (1) must be normalised Figure 3: The images of the leader used for the tracking: from left to right I l1, I l2, I l3 and magnified so that it can be compared between the three images: d n (I l, I f (x, y)) 1 = h w h w k=1 i=1 c (I l (k, i) I f (x, y, k, i)) 2. (2) The three images are used in turn to find three absolute minima of Equation (2) and the minimum value of the three is used as corresponding to the correct match. The position of the corresponding minimum gives the location of the leader in the images. Not all three images need to (and should) be used

5 however. Indeed, the projection of the world surrounding the follower onto the panoramic image plane is such that the position of the leader in the images moves up and down as a function of the distance leader-to-follower, as can be seen in Figure 1. This implies that the choice of which image of the leader to use in Equation (2) can be directed by where the leader was last located. When the leader s vertical position was last found to be below an experimentally defined threshold, i.e. close to the follower, then all three images were used (using the small image was not considered to be expensive). However, if the leader s vertical position was found to be above the threshold then only images I l2 and I l3 were used. This first reduces the amount of computation to be done and second prevents the detection of false absolute minima as in Figure 2(c). Finally, the search for the minimum can also be guided by where the leader was last found. Assuming that the relative position in Cartesian space of the leader and follower does not change too fast, with respect to the processing speed, the search can be limited to a small area of the grabbed images around the last known position of the leader (twice the size of I l2 in all reported experiments). In that restricted area, an exhaustive search is performed. When the minimum provided by the localisation of the leader was judged too high, i.e. corresponding to a poor match, the leader was reported as not being found and the follower was stopped and an exhaustive search was performed until successful localisation. This is also what is done at the initialisation stage. This was not triggered in any of the experiments reported here, apart from when starting the experiment, and in practise proved to be not very successful because by the time the leader could have been localised, it had usually moved enough to be too small in the images. 2.3 Control loop The result of the localisation of the leader gives the 2D position of the leader in the grabbed images (which image of the leader was found to be the best match is too coarse a scale to be used in the control of the follower and is redundant with the vertical positioning). This position is compared with the desired position and the difference is used in a proportional controller to adjust the speed of the follower: horizontal difference for the steering and vertical difference for the speed. Indeed, the horizontal error indicates a heading misalignment while the vertical error is indicative of the error in the distance between the two robots. This is because the camera, in fact the mirror, is positioned above the two robots, therefore projecting the leader at a height in the panoramic image that depends on the distance to the leader. However, for the height error to be useful, one has to assume that the two robots remain in the same plane, in other words that the floor is flat. This is obviously constraining in outdoor situations but is acceptable in most office-like environments. A proportional controller proved to be enough for the type of motion the leader was performing in our experiments. The gains of the controller were determined manually by experimentation. Sonars were also integrated in the system as a failsafe mechanism but this was not triggered during any of the reported experiments. 3 Experiments We performed a number of experiments to assess the accuracy and repeatability of the system. The results are reported, after the description of the setup. 3.1 Experimental setup The experiments were conducted in our Lab using two Pioneer 2DXe robots. One, as the leader, was teleoperated on trajectories that were made as similar as possible for the repeatability experiments by using markings on the floor. The second was the follower and was equipped with the omni-directional camera. The software was running on the robot s on-board computer (a Pentium III running at 800 MHz). Quantitative measurements were performed using the Vicon 512 motion tracking system that can track in realtime the position and orientation of pre-determined objects using reflective markers. The accuracy of the system is of the order of the millimetre. The Vicon data was obtained from a server running on the Vicon machine by the robot and saved locally on the robot. The experiments were conducted in the Lab during normal hours, when people move about in the Lab. Finally, the same images of the leader were used for all experiments but had been grabbed well before the actual experiments took place, therefore in probably different illumination conditions. 3.2 Results In all the reported experiments, the follower was placed behind the leader at approximately the correct distance and orientation. The first step was to acquire the leader and to align with it. After that, the leader was moved. All distances are given in metres. The starting point of the robots in the paths diagrams is shown as a circle Accuracy The first set of results concerns the accuracy of the system and quantitative results are given in Table 1. The first experiment is that of the simple straight line case. Figure 4 shows the path of the two robots and the distance between them for each frame. The leader s

6 (a) Paths Distance (m) Frames (b) Distance (a) Paths Figure 4: Straight line 1.8 path was not quite straight and the follower closely replicates this. The distance between the two robots is not constant but does not vary significantly (standard deviation of 9 cm). The mean distance is too high, which is visible on the graph. This is due to the follower not quite catching up while the leader was moving, up to frame 150, at which point the follower did catch up and overshoot slightly. The second experiment is for a circular path followed twice by the leader and the results are shown in Figure 5. It is clear that the follower replicates the path of the leader. It is also clear that the follower is systematically inside the circle of the leader, as a trailer would be. This is because in effect the implemented method creates an as rigid as possible link between the two robots. The performance is similar to that of the straight line case. It is to be noted that the glitch visible on the path of the leader around position ( 1, 3) was due to the robot being at the limit of the area covered by the Vicon system and therefore slightly erroneously placed. This obviously reflects on the distance measurements, around frame 130. This does not change significantly the statistics of the distance measurements. A less trivial, given the available space, path was used in the next experiment: the leader was tele-operated on Table 1: Quantitative results for the accuracy of the system Experiment Min Max Mean StdDev Straight line Circle Figure of eight Distance (m) Frames (b) Distance Figure 5: Circle a figure of eight path. Results are shown in Figure 6. Again, the follower is inside the path of the leader, which is well replicated by the follower. The mean distance in that case is closer to the desired distance but its standard deviation is larger, compared to the other experiments. This is because the leader was not moving as fast and the follower did not have to move as fast because of the higher curvature of the path. It is interesting to note that where the curvature was the highest, the leader as seen from the follower was significantly different from the leader when it is aligned with the follower in the correct position, in particular showing a lot more black (of the wheels) and a lot less red (of the body). Despite this, the system worked. Informal experiments showed that when the leader follows a tight circular path of radius close to 1.5 m, therefore such that the follower only needs to turn on the spot, the system still works, with occasional localisation failures. Finally, an almost straight path is followed by the leader once moving forward, then moving backwards,

7 (a) Paths (a) Paths (b) Distance (b) Distance Figure 7: Forward and backward Figure 6: Figure of eight Figure 7 (the statistics for the distance are not given because of the binary character of the results). The trailer effect is very visible here when the leader reverses (and the leader would certainly have been lost by the follower if the experiment had not been interrupted due to space constraints). It is also clear, looking at Figure 7(b) that the desired distance leader-to-follower is never reached; in the first half of the experiment, it is too high (when the leader moves forward, therefore pulling the follower) and it is too low in the second half of the experiment (when the follower is pushed by the reversing leader). (a) Paths Repeatability The system s repeatability was quantified by running the same experiment ten times, each time recording the position of the leader and follower, this for two different paths (straight line and circle). However, the paths followed by the leader were not exactly the same due to mechanical imperfections (the leader was initially manually positioned as close as possible to a fixed starting pose and given the same command). The case of the straight line is shown in Figure 8 1 and Table 2 gives the statistics of the distance for each straight line and overall (all distances together). 1 The straight line experiment in Section is the same as the first straight line here. (b) Distances Figure 8: Repeatability of the straight line It is clear that in both cases all results are within a few centimetres of each other. Note that the circles performed by the leader in

8 Table 2: Quantitative results for the repeatability of the system for the straight line Experiment Min Max Mean StdDev Overall Table 3: Quantitative results for the repeatability of the system for the circle Experiment Min Max Mean StdDev Overall (a) Paths this case are smaller than the circle performed in Section (approximately 3.40 m against 4.30 m) and that the distance leader-to-follower in the former is shorter than in the latter. This confirms the results obtained for the figure of eight in Section that in some cases, the distance was too large due to the follower not catching up with the leader. It is interesting to note that the distance for each frame in the case of the circle is also qualitatively self-similar with a pattern that repeats itself in all experiments. We can only explain this by an oscillating over-shooting behaviour that is the same in all experiments due to the similar paths followed by the leader. This didn t happen in the case of the straight line probably because in that case the follower was never quite able to catch up with the leader. 4 Discussion and conclusion The results show a good performance of the system, despite the crude way of dealing with the changes in size of the leader in grabbed images and the lack of explicit dealing of the changes in appearance when the leader is not aligned with the follower. However, when the leader s (b) Distances Figure 9: Repeatability of the circle appearance becomes dramatically different because of its non-correct orientation, the detection of the leader can fail 2. To solve this problem, more images of the leader need to be used in the matching process. However, this would tend to slow down the process and would certainly not be an elegant solution. A better solution would be to incorporate 3D transformations of the images in the optimisation procedure. The algorithm to localise the leader can also be improved. The limited exhaustive search used here is still expensive and some better prediction of the po- 2 In fact, it often succeeds because the robots used in these experiments are mostly circular and self-similar from all around them, apart from the wheels that do present a different visual appearance.

9 sition of the target could be done using for example a polynomial fit of the previous position as in (Schneiderman et al., 1995) or a Kalman filter. However, for this to be of any use, the leader s motion must be known to be somehow constrained such as when driving on a road, which is not always possible or desirable. The control of the follower as it is implemented does not allow for different configurations of the formation and would not work for a non-holonomic follower in any other configuration. Also, planning trajectories, as in (Chiem and Cervera, 2004), could be another area for future work. This however, requires the estimation of the relative position and orientation of the leader. The method presented here requires a fair amount of calibration: three images of the leader must be available along with their desired position in the images. The method is robust to small changes of these values, but any dramatic change of, for example, the geometry of the follower, such as the alignment of the camera, would result in a failure of the method. Only using one image and the 3D transformations mentioned above would help but not solve all these problems. However, the robustness to changes such as the desired height in the images of the leader was informally tested and used to alter the desired distance leader-to-follower without changing the images, which proved to be successful to some extent. We also assume that the appearance of the leader does not change, e.g. following changes in illumination. Re-acquisition of images would be a solution, but again not an elegant one. A different colour space would certainly help (Woodland and Labrosse, 2005). This does need more work. The flat floor assumption is obviously not desirable. Breaking it would result in a wrong distance between the two robots, which could have dramatic consequences. However, should the localisation of the leader in the images be done using 3D transformations (see above) rather than with an explicit coarse multi-resolution approach, then the distance to the leader would be evaluated from the 3D transformation, thus removing the assumption. Another assumption made in this work is that the leader s appearance is sufficiently different from other visual features of the environment. In a scenario such as convoy formation involving many similar vehicles, this obviously would be a problem as all the vehicles in front of any given vehicle might be visible and could therefore fool the system. However, because of the restricted search domain, both in position and scale, the wrong target would never be selected. In fact, we have informally tested this situation in our lab by driving the leader close to other similar static robots to try to make the follower latch onto these. This has never happened. Despite these limitations, our simple method, using pixel-based comparisons, of localisation of the leader provides a good performance with very good repeatability and accuracy. References Chiem, S. Y. and Cervera, E. (2004). Vision-based robot formations with Bézier trajectories. In Proceedings of the International Conference on Intelligent Robots and Systems, pages Das, A. K., Fierro, R., Kumar, V., Ostrowski, J. P., Spletzer, J., and Taylor, C. J. (2002). A vision-based formation control framework. IEEE Transactions on Robotics and Automation, 18(5): Eklund, M. W., Ravichandran, G., Trivedi, M. M., and Marapane, S. B. (1994). Real-time visual tracking using correlation techniques. In Proceedinds of the IEEE Workshop on Applications of Computer Vision, pages Labrosse, F. (2006). The visual compass: Performance and limitations of an appearance-based method. Journal of Field Robotics, 23(10): Labrosse, F. (2007). Short and long-range visual navigation using warped panoramic images. Robotics and Autonomous Systems. Accepted for publication. Malis, E. and Chaumette, F. (2000). 2 1/2 D visual servoing with respect to unknown objects through a new estimation scheme of camera displacement. International Journal of Computer Vision, 37(1): Mariottini, G. L., Pappas, G., Prattichizzo, D., and Daniilidis, K. (2005). Vision-based localization of leader-follower formations. In Proceedings of the IEEE Conference on Decision and Control, pages Renaud, P., Cervera, E., and Martinet, P. (2004). Towards a reliable vision-based mobile robot formation control. In Proceedings of the International Conference on Intelligent Robots and Systems, volume 4, pages Schneiderman, H., Nashman, M., Wavering, A. J., and Lumia, R. (1995). Vision-based robotic convoy driving. Machine Vision and Applications, 8(6): Vidal, R., Shakernia, O., and Sastry, S. (2003). Formation control of nonholonomic mobile robots with omnidirectional visual servoing and motion segmentation. In Proceedings of the IEEE International Conference on Robotics and Automation, volume 1, pages Woodland, A. and Labrosse, F. (2005). On the separation of luminance from colour in images. In Proceedings of the International Conference on Vision, Video, and Graphics, pages 29 36, University of Edinburgh, UK.