Indoor SLAM Using Laser and Camera with Closed-Loop Controller for NAO Humanoid Robot
We present a SLAM with closed-loop controller method for navigation of NAO humanoid robot from Aldebaran. The method is based on the integration of laser and vision system. The camera is used to recognize the landmarks whereas the laser provides the information for simultaneous localization and mapping (SLAM ). K-means clustering method is implemented to extract data from different objects. In addition, the robot avoids the obstacles by the avoidance function. The closed-loop controller reduces the error between the real position and estimated position. Finally, simulation and experiments show that the proposed method is efficient and reliable for navigation in indoor environments.
Robot has been used in many areas, such as industry process [1–6] and autonomous navigation. Autonomous navigation in an unknown environment is regarded as a key attribute of a service robot and has received considerable attention in the last two decades. The focus of this paper is the indoor environments such as homes, offices, and hospitals. It is important that the service robots can roam around in order to assist humans or to perform various tasks in such surroundings. For example, the robots need to detect and avoid obstacles that they might encounter in the environment.
Precise position estimation is a necessary prerequisite for a reliable navigation. In this case, simultaneous localization and mapping (SLAM) approach is employed to make a robot truly autonomous without the need for any a priori knowledge of location. SLAM algorithm [7, 8] can be recognized and localized in an unknown environment. Some artificial intelligence methods are used in the SLAM of the mobile robots, such as reinforcement learning , which we will study for NAO humanoid robot in the future. SLAM algorithm is the essential process of building a map of the environment while simultaneously determining its location within this map. SLAM has also been implemented in a number of different domains from indoor to outdoor, underwater, and airborne systems. However, the effectiveness of various SLAM methods greatly depends on using different exteroceptive sensors (sonar, laser, camera, odometer, etc.). Some methods are based on vision or laser [10, 11]. Camera and laser have different features, respectively. Cameras can provide the 3D sensing of the environment and obstacle avoidance, but its accuracy of localization is generally inferior to laser scanner. Compared to other sensors, laser provides more accurate range and bearing measurements. However, laser cannot provide the image of the environment. In particular, laser of the robot cannot recognize the object when it is faced with landmarks or obstacles. Therefore, the combination of camera and laser can achieve very good and reliable navigation for indoor SLAM problems. There are some studies reported in the literature whereby the laser and the stereo camera are used together for accomplishing indoor navigation tasks. In Labayrade et al.’s paper , the two sensors were used together to eliminate false positions in object detection. In Iocchi and Pellegrini’s paper , the 2D laser scanner and the stereo camera were used for building the 3D environment map. In Lin et al.’s paper , the proposed approach combined SLAM using a 2D laser scanner and dense 3D depth information from a stereo camera, and this work solved effectively the obstacles at different heights into the 2D map. The objective of this work is to effectively finish accurate position based on SLAM and avoid the obstacle of the environment. The proposed approach combines SLAM using a laser and stereo cameras. Using the cameras, it can recognize landmarks and provide obstacle information in the environment. Laser can provide high accuracy of both range and bearing measurements. K-means method is used to extract data from different objects. Robots with the proposed method can have rich environment information for better navigation and obstacle avoidance. The proposed approach is implemented on an NAO humanoid robot.
The rest of the paper is organized as follows. In Section 2, the background knowledge of laser based SLAM with closed-loop controller, the NAO platform that we have used, and the choosing method of the landmarks by using cameras are introduced. In Section 3, we briefly describe the task scenario and K-means clustering method of extraction data from Laser. The method of extraction data from different objects by using laser and avoidance obstacles is also described. The experiment results are shown in Section 4. Finally, Section 5 gives the conclusion and discusses future work.
2. Laser and Camera Based SLAM with Closed-Loop Controller
There are two key computational solutions to the SLAM problem: the extended Kalman filter (EKF-SLAM) and Rao-Blackwellized particle filters (FastSLAM). EKF-SLAM was presented by Smith and Cheeseman in 1987 and has been used extensively . EKF-SLAM is very well known to navigation problems. The main steps of SLAM include robot motion prediction, new landmarks initialization, and known landmarks correction.
The observation model is  where is robot state vector and landmark states, is the observation function and is the measurement noise.
The covariance matrix is where is Jacobian matrix, is the Gaussian noise.
The EKF correction step is written as where is the innovation’s mean, is the observation position of the robot, is Jacobian matrix, is the covariance matrix of the measurement noise.
Kalman gain is The update position of the robot is The correction covariance matrix is The measurement data is given by the laser, include the distance, angle between the robot and landmarks and the position of the landmarks. The extraction data method from laser is introduced in Section 3.
2.2. NAO Humanoid Robot
The platform selected for my research project is humanoid robot NAO, a commonly used humanoid platform for education environment, produced by the French company Aldebaran Robotics. NAO is a reasonably priced advanced humanoid robot with attractive features that make it an ideal platform for research in humanoid robots. It is designed based on an open architectural philosophy and can be programmed with several different programming environments. NAO is a medium-sized humanoid robot developed mainly for universities and laboratories for research and education purposes. It replaced the Sony AIBO dogs in the RoboCup Standard Platform League (SPL) in 2008. As an autonomous humanoid robot, NAO is capable to move in a biped way, sense its close environment, communicate with human and think by on-board processor [16–21]. NAO is a new biped robot recently developed by French company Aldebaran-Robotics. Figure 1 shows the overall appearance of the robot in our experiment. NAO is 0.57 meters high and weighs about 4.5 kilogram. NAO with laser head is an option provided by Aldebaran robotics for users intending to study specific navigation problems such as SLAM.
NAO is very light compared to other existing robots of the same height because its BMI is about 13.5 (). NAO has a total of 25 degrees of freedom, 11 degrees of freedom (DOF) for its lower parts and another 14 DOF for its upper parts. NAO robot is demonstrated generally by its hardware, mechanical architecture and software. The hardware section mainly includes several sensors as they are critical for the robot to explore the surrounding environment. In the software, the structure of NAOqi is explained by the three components: NAOqi OS, NAOqi Library and Device control Manager. In addition, the dedicated NAO software, Choregraphe, Monitor and Webots, are included.
There is one laser scanner on the robot NAO head. Besides the camera, the laser sensor is another essential device for my research. The SLAM algorithm requires the laser to obtain landmark location information in terms of bearing and distance to perform a full SLAM process. The type of the laser used in our experiment is URG-04LX. The function of the laser is provided in Table 1.
The robot has a CPU in its v4.0 head of our experiment as the main processing unit: it’s a GEODE 500 MHz board with 512 M of flash memory and Wi-Fi connection for connection to the development remote computer . NAO also has a user friendly GUI, so it is easy to program NAO using C, C++, Phython or Urbi. Python 2.7 is used to program in this paper.
2.3. Closed-Loop Controller
NAO deviates from a set target as there is no closed-loop control to compensate the noise of the sensors. This results in an inaccurate position of NAO. Figure 2 shows the experiment result of the real position and estimated position of the NAO when the control is a fixed value. The error is large between the real position and the actual robot position. Therefore, we designed a simple PI controller in order to reduce the error. The closed-loop process is only based on the return error of the real position and the estimated position of NAO. The outline of this closed-loop is presented in Figure 3, where is the real position, is the EKF-estimated position, is the threshold defined by user, and is the proportional value. The steps of closed-loop controller are followed.
Closed-loop controller steps are as follows: (i)define an initial controller value Constant (i.e., );(ii)if the error , then , else Constant (i.e., ).
The real position and EKF-estimated position of the NAO with closed-loop controller are shown in Figure 4.
The error between estimated and reference robot position can be calculated, which is in significant amount during the experiment. Accordingly, a closed-loop PI controller is implemented to minimize this error, such that the deviation that robot travels can be reduced. In addition, to verify the performance of motion controller, the result has been verified with observation during the experiment. It has shown that the motion controller has succeeded in keeping NAO robot with small position error.
2.4. Choosing Landmarks by Using Cameras
In order to distinguish the landmarks from the obstacles in the environment, we used the NAO embedded vision system as the robot is not able to decide if an object is a landmark or an obstacle. Firstly, the robot chooses the objects as landmarks and recognizes them. Then, the robot reads the laser data when it sees the landmarks. The recognizing object function is used in this paper. The steps of recognizing objects are followed.
Recognizing objects steps are as follows:(i)take a picture for the recognized objects by the cameras;(ii)draw the contour of the objects;(iii)save the message of the recognized objects to vision recognition database;(iv)create a Python module called back of picture recognition;(v)call laser when robot sees the objects.
The recognized objects are shown in Figure 5.
(a) The picture of the landmark
(b) The contour of the landmark
3. Task Scenario
3.1. Task Scenario
We test our navigation approach in an indoor environment. There are two landmarks in the environment (see Figure 6).
In the scenario, the NAO is supposed to walk autonomously from its current location to a destination. The robot needs to recognize the landmarks and it can avoid the obstacle when it walks. An outline of implement the task is shown in Figure 7.
In order to finish the navigation indoors environment, we require several components including:(i)recognition of objects by using camera that judge the objects that are landmarks or obstacles;(ii)navigation in an unknown environment that calls SLAM when meeting landmarks;(iii)a laser module that provides the robot’s position while walking;(iv)an avoidance module that calls the function when meeting obstacle.
3.2. The K-Means Clustering Method of Extraction Data from Laser
It is very important to extract different objects data from laser. The data is provided in a two-dimensional array, 684 by 4 in our experiment. Each index includes length (distance between laser and objects), angle (computed value based on the index and URG device configuration), and and (objects coordinates in Cartesian space).
The k-means algorithm is an effective method of extraction laser data. The term “k-means” was first used by MacQueen in 1967 . The steps are followed. The k-means algorithm takes as input the number of clusters and a set of observation vectors to cluster. It returns a set of centroids, one for each of the clusters. An observation vector is classified with the cluster number or centroid index of the closest centroid to it.
Step 1. Assign each observation to the cluster whose mean is the closest to it: where is a set of observations assigned to exactly one and is an initial set of means .
Step 2. Calculate the new means to be the centroids of the observations in the new clusters: The algorithm has converged when the assignments no longer change. There are two landmarks in our experiment.
3.3. Avoidance Obstacles
In order to prevent the robot to knock down other objects, we use avoidance in our experiment. The avoidance function is navigationProxy.moveTo . The key of addressing this issue is calling the ALNavigation module from NAOqi as shown in Table 2. This module assists the robot to use its sonars while it is in motion. By using sonars, the robot is able to stop moving if there are any obstacles located in the security area of sonars. The avoidance steps are followed.
Avoidance obstacles steps are as follows:(i)call navigationProxy.moveTo function;(ii)if there exit obstacles, the robot stops and turns to a certain angle. Assume the security distance is 0.3 m between the robot and the obstacles else walks forward.
4. Simulation and Experiment Results
The proposed method has been evaluated in the autonomous and intelligent systems laboratory (AISL). There are two landmarks in our experiment. In order to test our method, firstly, we did the simulation on the computer. The simulation result is shown in Figure 8 and the result is good. Then, we did the experiment in the lab. The experiment parameters are shown in Table 3.
Figure 9 is the experimental result of SLAM without closed-loop controller. The two triangles are the initial position of the robot. The two stars are the landmarks. The circles on the red points are the covariance of the robot. The circles become smaller when the robot is near the landmark. The control input is 0.1, and the error between the real position and estimated position is bigger and bigger because it is an open loop.
In Figure 10, the error between the real position and estimated position becomes smaller and smaller because PI controller is added. An obstacle is added based on Figure 10. The experiment result is shown in Figures 11 and 12. The yellow circle is the obstacle. The red point is the estimated position of the NAO robot. The motion of the robot is changed because there exits obstacle. The experiment shows that the robot can avoid the obstacles successfully.
5. Conclusion and Future Work
The main contribution of this paper was the full real time implementation of EKF-SLAM on the NAO humanoid robot by combining its camera with laser. EKF-SLAM algorithm was realized in Python code and studied in simulation and experimental implementation. The simulation result validated the effort of EKF-SLAM landmark observation in terms of reducing the uncertainty of robot motion. The camera is used to recognize the landmarks, and then the laser gave the distance and the angle between the robot and landmarks. The robot can avoid the obstacle by using navigationProxy.moveTo function. A closed-loop PI motion controller was used to improve trajectory control. The experiment was the realization of simulation in two landmarks cases, and the NAO robot in the experiment successfully accomplished EKF-SLAM in a realistic exploration task with two landmarks and a fixed obstacle. Our future works include designing an avoidance algorithm to choose the best path which can avoid the obstacles, rather than use the existing avoidance function. The controller is also to be improved in order to obtain better accurate position. We used one landmark at one time in our paper, but more landmarks are used at one time in the future work.
Conflict of Interests
The authors declare that there is no conflict of interests regarding the publication of this paper.
The work was partly supported by the Natural Science Foundation of Hebei Province of China (Project no. F2014203095), the Young Teacher of Yanshan University (Project no. 13LGA007), the National Natural Science Foundation of China (Project no. 51275439), and the Major State Basic Research Development Program of China 973 Program (Project no. 2013CB733000).
W. D. Smart and L. P. Kaelbling, “Effective reinforcement learning for mobile robots,” in Proceedings of the EEE/RSJ International Conference on Robotics and Automation (ICRA '02), pp. 3404–3410, IEEE Press, Washington, DC, USA, May 2002.View at: Google Scholar
D. Fox, D. Hähnel, W. Burgard, and S. Thrun, “An efficient fastslam algorithm for generating maps of large-scale cyclic environments from raw laser range measurements,” in Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS '03), pp. 206–211, IEEE Press, Las Vegas, Nev, USA, October 2003.View at: Google Scholar
L. Iocchi and S. Pellegrini, “Building 3D maps with semantic elements integrating 2D laser, stereo vision and INS on a mobile robot,” in Proceedings of the 2nd International Society for Photogrammetry and Remote Sensing International Workshop 3D-ARCH, (ISPRS '07), pp. 1–8, IEEE Press, Zurich, Switzerland, 2007.View at: Google Scholar
D. Gouailier, V. Hugel, P. Blazevic et al., “Mechatronic design of nao humanoid,” in Proceedings of the IEEE International Conference on Robotics and Automation (ICRA '09), pp. 769–774, IEEE Press, Kobe, Japan, May 2009.View at: Google Scholar
S. Shamsuddin, L. I. Ismail, H. Yussof et al., “Humanoid robot NAO: review of control and motion exploration,” in Proceedings of the IEEE International Conference on Control System, Computing and Engineering (ICCSCE '11), pp. 511–516, IEEE Press, Penang, Malaysia, November 2011.View at: Publisher Site | Google Scholar
D. Tlalolini, C. Chevallereau, and Y. Aoustin, “Optimal reference motions with rotation of the feet for a biped,” in Proceeding of the ASME International Design Engineering Technical Conferences and Computers and Information in Engineering Conference (IDETC/CIE '08), pp. 1027–1036, IEEE Press, New York , NY, USA, August 2008.View at: Google Scholar
K. Nishiwaki, S. Kagami, Y. Kuniyoshi, M. Inaba, and H. Inoue, “Toe joints that enhance bipedal and fullbody motion of humanoid robots,” in Proceedings of the IEEE International Conference on Robotics adn Automation (ICRA '02), pp. 3105–3110, IEEE Press, Washington, DC, USA, May 2002.View at: Google Scholar
Y. Ogura, K. Shimomura, H. Kondo et al., “Human-like walking with knee stretched, heel-contact and toe-off motion by a humanoid robot,” in Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS '06), pp. 3976–3981, October 2006.View at: Publisher Site | Google Scholar
S. Lohmeier, T. Buschmann, H. Ulbrich, and F. Pfeiffer, “Modular joint design for performance enhanced humanoid robot LOLA,” in Proceedings of the IEEE International Conference on Robotics and Automation (ICRA '06), pp. 88–93, IEEE Press, Orlando, Fla, USA, May 2006.View at: Publisher Site | Google Scholar
J. B. MacQueen, “Some methods for classification and analysis of multivariate observations,” in Proceedings of the 5th Berkeley Symposium on Mathematical Statistics and Probability (MSAP '67), pp. 281–297, IEEE Press, New York, NY, USA, 1967.View at: Google Scholar