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.


Introduction
Robot has been used in many areas, such as industry process [1][2][3][4][5][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 [9], 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 [12], the two sensors were used together to eliminate false positions in object detection.In Iocchi and Pellegrini's paper [13], the 2D laser scanner and the stereo camera were used for building the 3D environment map.In Lin et al. 's paper [14], 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 closedloop 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.

Laser and Camera Based SLAM with
Closed-Loop Controller

EKF-SLAM.
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 [15].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 [7] where  is robot state vector and landmark states, ℎ(⋅) is the observation function and V 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. NAO is very light compared to other existing robots of the same height because its BMI is about 13.5 (kg/m 2 ).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.

NAO Humanoid
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 [22].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.

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 closedloop 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 closedloop controller are followed.
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.

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.

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.MacQueen in 1967 [23].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:

The K-Means Clustering Method of Extraction
where   is a set of observations { 1 ⋅ ⋅ ⋅   } assigned to exactly one  and   is an initial set of  means { 1 ⋅ ⋅ ⋅   }.
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.

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.

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.

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  landmark at one time in our paper, but more landmarks are used at one time in the future work.

Figure 2 :Figure 3 :
Figure 2: The experiment motion results of robot without closedloop controller.

Figure 4 :
Figure 4: The experiment motion results of robot with closed-loop controller.
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 (a) The picture of the landmark (b) The contour of the landmark

Figure 5 :
Figure 5: The process of the recognized landmarks.

Figure 6 :
Figure 6: The scenario of the experiment.

Figure 7 :
Figure 7: The overall flow chart of the navigation.

Figure 8 :
Figure 8: The simulation motion result of Robot SLAM with closedloop controller.

Figure 9 :
Figure 9: The experiment motion result of Robot SLAM without closed-loop controller.

Figure 10 :
Figure 10: The experiment motion result of Robot SLAM with closed-loop controller.

Figure 11 :AbstractFigure 12 :
Figure 11: The experiment motion result of Robot SLAM with closed-loop controller and avoidance obstacle.

Table 1 :
Main characteristics of the NAO URG-04LX laser.

Table 2 :
Main python code for using obstacle avoidance function.

Table 3 :
The value of the experiment parameters.