Autonomous Navigation With Nao

AUTONOMOUS NAVIGATION WITH NAO Summary We introduce a navigation system for the humanoid robot NAO that seeks to find a ball, navigate to it, and kick it. The proposed method uses data acquired using the 2 cameras mounted on the robot to estimate the position of the ball with respect to the robot and then to navigate towards it. Our method is also capable of searching for the ball if is not within the robots immediate range of view or if at some point, NAO loses track of it. Variations of this method are currently used by different universities around the world in the international robotic soccer competition called Robocup. This project was completed within EC720 graduate course entitled “Digital Video Processing” at Boston University in the fall of 2015. i Contents 1 Introduction 1 2 Literature Review 1 3 Problem Solution 3.1 Find position of the ball . . . . . . . 3.2 Positioning the robot facing the ball . 3.3 Walking to the ball . . . . . . . . . . 3.4 Kicking the ball . . . . . . . . . . . . . . . . 2 3 10 11 12 4 Implementation 4.1 Image Acquisition . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.2 Finding Contours and Circles . . . . . . . . . . . . . . . . . . . . . . 4.3 Navigation and kicking for NAO . . . . . . . . . . . . . . . . . . . . . 12 13 13 13 5 Experimental Results 5.1 Ball identification results by method . . . . . . . . . . . . . . . . . . 5.2 Confusion matrices . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5.3 Centering NAO to face the ball . . . . . . . . . . . . . . . . . . . . . 14 14 14 15 6 Conclusion and Future Work 16 7 Appendix 7.1 NAO features . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7.2 Python Code . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17 17 17 ii . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . List of Figures 1 2 3 4 5 6 7 8 9 Robocup in China - 2015 . . . . . . . . . . . . . . . . . . . . . . . . . Flowchart of the algorithm for the system proposed . . . . . . . . . . (a) resulting mask of thresholding only on hue channel, (b) resulting mask of thresholding only on saturation channel, (c) resulting mask of threshold on hue and saturation channels . . . . . . . . . . . . . . . . PDF approximation of ball pixels in hue and saturation channels . . . Masked image as seen by NAO. All pixels not identified as the ball are darkened, and the green dot is where the center of the ball is as the output of the center of mass calculation. (a) Center of the ball is correctly identified as the threshold is tight enough to reduce all external noise to zero. (b) Center of the ball is not correctly identified due to external noise pixels. (c) small red objects greatly skew the center of mass calculation. . . . . . . . . . . . . . . . . . . . . . . . . Images as seen by NAO after contour algorithm. Non-identified pixels are darkened, orange lines are identified contours, and the green dot is the center of the identified ball. (a) Center of the ball is correctly identified with noise and small red objects in the image. (b) A red chair is marked as the ball even though no ball is in the image. (c) padding of a red chair is identified as the ball even though the ball is in the image as well. . . . . . . . . . . . . . . . . . . . . . . . . . . . A visual representation of the error metric. Vector d is the maximum X displacement from the center of the contour, and vector ρi is the vector from the center of the contour to each point. . . . . . . . . . . Images as seen by NAO after contour algorithm with the error metric. Non-identified pixels are darkened, the orange outline is the largest identified contour, and the green dot is the center of the identified ball. The green dot being in the top left corner indicates a negative read on the image, i.e. no ball is present. (a) The red chairs larger cushion is rejected as not a ball. (b) the small visible part of the chair back is marked as not a ball, but the ball is in the image as well and is not identified. (c) the back of the chair has a false roundness and is identified as a ball. . . . . . . . . . . . . . . . . . . . . . . . . . . . . A visual representation of the new error metric. Vector r̄ is the radius of the minimum enclosing circle, and vector ρ̄i is the vector from the center of the minimum enclosing circle to each point. . . . . . . . . . iii 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 Images as seen by NAO after minimum enclosing circle algorithm with the error metric. The green dot being in the top left corner indicates a negative read on the image, i.e. no ball is present. (a) The green circle correctly identifies the ball not identified in Figure 8. (b) The chair back previously misidentified as a ball is rejected. The best remaining contour is shown in orange. (c) The pink sticky note on the wall is identified as a ball inside the green circle because it fits in all parameters. Visualization of ball position based on the robot facing’ direction . . Straight walk test for NAO . . . . . . . . . . . . . . . . . . . . . . . . NAO side-kicking the ball with its right foot . . . . . . . . . . . . . . NAO coordinates for motion [4] . . . . . . . . . . . . . . . . . . . . . Ball position: Expected (blue circle) vs Real (green circle) . . . . . . Stages of angle correction to face the ball . . . . . . . . . . . . . . . . NAO Overview [4] . . . . . . . . . . . . . . . . . . . . . . . . . . . . iv 10 11 11 12 13 14 15 17 List of Tables 1 2 3 4 5 Results for different ball identification methods. Find Ball is the ability to turn towards the ball's initial position. Reach Ball is the ability to walk towards a ball starting faced at the ball. Kick Ball is the ability to use fine movements to position and kick the ball from close. . . . . Contour: 60% Acc . . . . . . . . . . . . . . . . . . . . . . . . . . . . Contour + EM: 83.3% Acc . . . . . . . . . . . . . . . . . . . . . . . . MEC: 93.3% Acc . . . . . . . . . . . . . . . . . . . . . . . . . . . . . MEC + EM: 83% Acc . . . . . . . . . . . . . . . . . . . . . . . . . . v 14 15 15 15 15 1 1 Introduction Autonomous navigation and tracking requires appropriate modeling of environment. Remarkable progress has been done over the years and there is an ongoing research every year. For this project we utilize NAO, a complete humanoid robotics platform created by Aldebaran Robotics. NAO has 2 cameras mounted on his head which gives an enlarged field of vision, making it able to see in front of him or at his feet. His extensive capabilities due to the variety of sensors that he has plus its easy-to-use SDK makes him an excellent choice to fuse computer vision and motion dynamics into one single project. In this project, we will enable NAO to perform a low-level control of motion as well as use his cameras for streaming images and do a further processing of the video for tracking a target object. This concept has been widely used in a popular competition called Robocup that happens every year, where robots play soccer among themselves. Figure 1: Robocup in China - 2015 2 Literature Review In the last few years, humanoid robots have become a popular research tool as they offer new perspectives compared to wheeled vehicles. For example, humanoids are able to access different types of terrain and to climb stairs. However, compared to wheeled robots, humanoids also have several drawbacks such as foot slippage, stability problems during walking, and limited payload capabilities. For example, there is often serious noise when executing motion commands depending on ground friction and backlash in the joints, i.e., odometry can only be estimated very inaccurately. Also, the observations provided by the lightweight sensors which typically have to be used with humanoids are rather noisy and unreliable. As a result, accurate navigation, which is considered to be mainly solved for wheeled robots, is still a challenging problem for humanoid robots. Numerous papers are written every year addressing this problem, especially using NAO [6], [2], [3]. A whole list of publications can be found in [5]. 2 Based on these publications, we extracted some ideas to create our own method of tracking. This system is explained in the next section. 3 Problem Solution Our approach to solving the problem (navigate to an object and interact with it) will be broken into 4 main parts: Initial scan of the room to find the ball, positioning the robot facing the ball, walking to the ball while updating the position every certain distance based on the visual information and finally kicking the ball. Figure 2: Flowchart of the algorithm for the system proposed 3 3.1 Find position of the ball For every image scanned, these are what is used to determine whether the robot can see the ball and its location with respect to the robot. The methods used to identify the ball need to be as simple as possible to be used in real time during the robots search algorithm trying to locate and kick the ball.To find the ball position and approximate distance for scanning and feedback looping while the robot is in motion, three key features are taken into account to find and estimate where the ball is in front of the robot. The features used are color of individual pixels belonging to the ball, the expectation that pixels within the ball are connected and filled in, and the roundness of the ball. Several different methods were used to take these features into account. The following subsections, specifically 3.1.2 through 3.1.5 explain different solutions that were tried in chronological order as well as order of increasing complexity. Section 3.1.1 discusses the binary hypothesis test done as the first step to each method, and section 3.1.5 explains the final implemented solution. 3.1.1 Thresholding HSV color space To determine which pixels belong to the ball, it was found that using the H and S channels of HSV space produce good results when used co-dependently in a binary hypothesis test to find pixels which most likely belong to the ball. This is more beneficial than using RGB space, for example, because color information is stored on all three dimensions, and luminance is tied into all three channels as well. As seen in Figure 3, using only one of the hue and saturation channels produces less than favorable results, but using both allows for a much more precise approximation of ball pixels at this stage. Also, as seen in Figure 4, the pdf for pixels belonging to the ball are assumed unimodal in both the circular H channel and the linear S channel. (a) (b) (c) Figure 3: (a) resulting mask of thresholding only on hue channel, (b) resulting mask of thresholding only on saturation channel, (c) resulting mask of threshold on hue and saturation channels p(I[x]|H1 ) p(I[x]|H0 ) not ball ≶ ball thresh (1) 4 Figure 4: PDF approximation of ball pixels in hue and saturation channels This means that the binary hypothesis test simplifies to a single set of upper and lower thresholds for each channel that can be implemented on pixel values instead of using a pdf lookup process (Eq 1). After the image is masked using the binary hypothesis test, morphological operations are then performed to reduce noise (opening) and fill in the ball (closing). 3.1.2 Center of mass approach The first and simplest method used is a simple center of mass calculation (Eq. 2). Cm = [xCm yCm ] where: xCm m̄x .x̄T = M y Cm m̄y .ȳ T = M (2) 5 This was used because from observing the masks generated by thresholding in HSV color space, ball pixels are clumped together, and for a perfect threshold, this will produce the exact center of the ball. Also, single noise pixels should not affect the center of mass calculation because the weight of any coordinate is proportional to the number of pixels in that coordinate. This means that because the ball is a very large number of pixels in one place compared to small numbers of noise pixels distributed randomly throughout the image. This is a very simple process and the steps can be seen in Algorithm 1 1: Threshold image in HSV color space 2: Calculate center of mass on masked image This method works very well when the threshold is finely tuned so that only the ball is captured, and works acceptably well if the threshold is too tight and some pixels from the ball are excluded. However, as seen in Figure 5, when small numbers of noise pixels are identified outside of the ball, the center of mass is skewed slightly, and when there are smaller red objects in the field of view, the center of mass is skewed greatly. Also, one very important missing functionality of this test is that there is no way to reject an image that has no ball in it unless there are zero identified noise pixels in the image. This means that any single reddish pixel will be a false positive. (a) (b) (c) Figure 5: Masked image as seen by NAO. All pixels not identified as the ball are darkened, and the green dot is where the center of the ball is as the output of the center of mass calculation. (a) Center of the ball is correctly identified as the threshold is tight enough to reduce all external noise to zero. (b) Center of the ball is not correctly identified due to external noise pixels. (c) small red objects greatly skew the center of mass calculation. Moving forward from this method to the next, several important observations were made. The first is that all of the pixels belonging to the ball should be connected because of morphological operations and fairly well-tuned thresholds. Also, noise pixels are not completely erased through the morphological operations, but are 6 typically much smaller than the group of pixels belonging to the ball. From these observations, the next method needs to find groups of pixels and can make decisions based on the size of the groups of pixels identified in the image. 3.1.3 Largest contour approach After the center of mass observations, it follows that each group of identified pixels can be identified by the contour that can be drawn around it. The ball is very likely to be the largest group of identified pixels since external pixels are noise-like or from smaller red objects. This is only slightly more complex than the center of mass method, and reduces the impact of external noise pixels to zero. The steps for this method can be seen in Algorithm 2. Algorithm 2 1: Threshold image in HSV color space 2: Find contours around groups of identified pixels 3: Define largest contour as the ball 4: Find the center of the contour as the center of a minimum enclosing rectangle around the contour This method produces much more favorable results than the center of mass method. Using contours works very well when the ball is the largest group of identified pixels, and can identify the ball with external groups of pixels from smaller objects. However, there is still no error rejection or way to decide whether a contour is actually the ball. As seen in Figure 6, when there is no ball or there are larger areas than the ball of identified pixels, this method fails. (a) (b) (c) Figure 6: Images as seen by NAO after contour algorithm. Non-identified pixels are darkened, orange lines are identified contours, and the green dot is the center of the identified ball. (a) Center of the ball is correctly identified with noise and small red objects in the image. (b) A red chair is marked as the ball even though no ball is in the image. (c) padding of a red chair is identified as the ball even though the ball is in the image as well. 7 The contour method performed better than the center of mass method because it now can ignore small noise pixels, but this method needs a way to determine if the contours belong to the ball or some object in the background. From these observations, the next method will need to implement a metric by which to test contours to decide whether or not a contour belongs to the ball. 3.1.4 Contour error metric approach Using the contour method as a new baseline, there needs to be a way to determine whether or not the identified contour is actually a ball or not. To improve the contour method, an error metric was applied to the contour to test roundness of the identified contour (Eq. 3). The idea behind this error metric is that the distance from the center of the contour to each point on the contour should be almost the same as the maximum displacement in the X direction of the contour from its center (Figure 7). Instead of just subtracting these two values, the absolute value of this difference is divided by the maximum x displacement to make this metric a percent difference, thus is size-invariant. PC ¯ − |ρ̄i | |d| αi αi = (3) E = i=1 ; ¯ C |d| Figure 7: A visual representation of the error metric. Vector d is the maximum X displacement from the center of the contour, and vector ρi is the vector from the center of the contour to each point. The average of the absolute value of the percent difference of these two vectors should be small for the ball, and should be large for non-round objects that are in the image. The implementation of this method can be seen in Algorithm 3. 8 Algorithm 3 1: Threshold image in HSV color space 2: Find contours around groups of identified pixels 3: Test the largest con tours error metric 4: Reject contour if error metric is too large (a) (b) (c) Figure 8: Images as seen by NAO after contour algorithm with the error metric. Nonidentified pixels are darkened, the orange outline is the largest identified contour, and the green dot is the center of the identified ball. The green dot being in the top left corner indicates a negative read on the image, i.e. no ball is present. (a) The red chairs larger cushion is rejected as not a ball. (b) the small visible part of the chair back is marked as not a ball, but the ball is in the image as well and is not identified. (c) the back of the chair has a false roundness and is identified as a ball. The contour with an error metric method works well when the ball is the largest contour or not in the image. Issues arise for this method, however, when the ball is in the image with a non-round larger red object, where the output is a false negative, or an object that isnt round has a false roundness, where the maximum X displacement is not an extreme displacement in terms of the distance to each point on the contour from its center, which reduces the error metric for that contour for non-ball contours (Figure 8). Adding the error metric is an obvious improvement over just using the contours alone. With this method, there is a way to reject images with identified pixels as not having a ball. Important observations made here are that a better way to define the error metric is needed to reject false roundness, and a way to look at all contours is needed as well to look for a ball in an image where there is a larger red object also in the field of view. 3.1.5 Minimum enclosing circle (MEC) approach The final method used is a slight improvement upon the contour method that allows two main features of the ball to be taken into account in addition to color- the size 9 and shape of the ball. The shape of the ball is determined by an error metric very similar to the error metric from the last method, but first a minimum enclosing circle is found around each contour. This minimum enclosing circle is the smallest circle possible that can fit every point in the contour inside of it. Each circle is defined by its center and its radius. This means that the error metric can be defined similarly to before in Eq. 3, but d is replaced by r, the radius of the minimum enclosing circle as seen in Figure 9 (Eq. 4). Similarly, for very round objects such as the ball, the radius of the minimum enclosing circle should be the same as or very close to the radius of the ball in the image. Because of this and the fact that the size of the ball is known and is assumed to be within a reasonable distance to the robot, radius of the minimum enclosing circle is also subjected to a minimum and maximum threshold. Figure 9: A visual representation of the new error metric. Vector r̄ is the radius of the minimum enclosing circle, and vector ρ̄i is the vector from the center of the minimum enclosing circle to each point. PC E= i=1 C αi ; αi = ||r̄| − |ρ̄i || |r̄| (4) The algorithm for this method has not changed much, but now uses three thresholds to determine if there is a ball in an image and where it is. The first, thresholding in HSV color space, is common to all algorithms so far. The second and third are used in unison for each contour. The error metric and the radius of the minimum enclosing circle are tested on each contour so that a ball may be found in an image where a much larger red object is sitting as well (Algorithm 4). Using the minimum enclosing circle to define the error metric clearly outperforms using the error metric on the largest contour. This approach now accurately rejects all non-round objects while also allowing another parameter to be tested with no noticeable impact on computation time. As seen in Figure 10, having color, size, and shape as parameters produces very favorable results, even on images misidentified in previous methods. 10 Algorithm 4 1: Threshold image in HSV color space 2: Find contours around groups of identified pixels 3: Find a minimum enclosing circle around each contour 4: Find the contour with the smallest error metric that is within the range of expected radius values 5: Reject contour if error metric is too large (a) (b) (c) Figure 10: Images as seen by NAO after minimum enclosing circle algorithm with the error metric. The green dot being in the top left corner indicates a negative read on the image, i.e. no ball is present. (a) The green circle correctly identifies the ball not identified in Figure 8. (b) The chair back previously misidentified as a ball is rejected. The best remaining contour is shown in orange. (c) The pink sticky note on the wall is identified as a ball inside the green circle because it fits in all parameters. As seen in Figure 10, there are still objects that are not a ball that fit within the expected parameters of the ball. A future method could possibly account for the mean pixel of the ball as another possible metric to decide whether or not a contour is a ball. Disregarding this unlikely circumstance, however, this method works very well compared to previous methods. 3.2 Positioning the robot facing the ball Once we have the location of the center of the ball, we will make use of pixel information of the image to rotate the robot and face it directly. By using a 640x480 pixel image, we need to have the ball centered at 320. We need at least 2 images where the ball has been detected, and we also make use of the rotation angles where every picture has been taken. Fusing these two different informations we can calculate the closest rotation angle, which gives us a good centering of the ball. 11 Figure 11: Visualization of ball position based on the robot facing’ direction 3.3 Walking to the ball Once it has aligned its body facing the ball, NAO will start walking towards it. Because NAO doesn’t have a feedback for its odometry, information about real data can’t be extracted. In Figure 12 we show an initial state where NAO is directly facing the ball in a straight line, and the final state after walking 30 cm. (a) Initial position (b) Final position after walking 30cm Figure 12: Straight walk test for NAO As we can see there is an obvious drift due to sliding, friction between the robot's feet and floor, etc. This is why we can’t just rely on the motion dynamics. The approach we took was to correct this drifting in two stages. The first stage is when NAO farther than 10cm from the ball; NAO will do a 20cm displacement, and using the visual information from his cameras (upper or lower, depending where it sees the 12 ball) it will correct the drifting. Once it gets close enough (∼10cm) it needs to walk in a more precise manner, as we are trying to have NAO stand right in front of the ball, so we rely again on the visual information from its lower camera but this time the displacement and rotations are done separately to compensate imprecisions. 3.4 Kicking the ball In this final step, NAO should be positioned correctly and ready to kick the ball. This motion is pretty complicated as we need to take into consideration a lot of dynamics of the robot to have a good kick. Developers who have worked with NAO state that programming a whole kick requires good knowledge of humanoid balance, and the official documentation shows a very basic program to make NAO kick the ball. We played around with this code and found a decent kick, which has worked every time we have tested it. Although a much better kick will be desired, this will be part of our future works once we have better understanding of motion dynamics. Figure 13: NAO side-kicking the ball with its right foot 4 Implementation Our final implementation was written in Python (see Appendix). Although it would be nice to have it in C++ for a much faster compilation and running, it was a challenging task to learn most of the SDK for NAO (NAOqi) [4] using C++, whereas Python, being a friendly language, made our programming easier. We also used OpenCV [1] for image processing, we focused on using this library because NAOqi has OpenCV built-in, and has very useful functions for the processing we described before.