Simplified EKF-SLAM by Combining Laser Range Sensor with Retro Reflective Markers for Use in Kindergarten

Recently in Japan, work contents imposed to childminders have increased due to several reasons such as variety of children's growth stage, requirement from parents, and administrative advices. To maintain good childcare service and to support kindergarten staffs, an objective monitoring system to watch over children is expected. In this paper, for the monitoring system, an automatic method to make an indoor map using the SLAM technique is discussed. To solve problem specific to the kindergarten use, using special landmark (LM) markers consisting of retro reflective material, an approach of a simplified EKF-SLAM which is effective by short-distance survey is proposed. Thorough simulation, the SLAM algorithm with an extraction method of the special LM from the laser range finder (LRF) sensor data was verified, and it was confirmed that the estimation error was as small as 0.57 % against total length of robot locomotion. Same algorithm was implemented to an actual robot that was mounted with the LRF sensor, and practical effectiveness was validated. As a result, the ability of the presented SLAM with a margin of estimation error of 5.3 % was also confirmed in the experiment. Keywords—SLAM, EKF, laser range finder, retro reflective material.


I. INTRODUCTION
ECENTLY, number of parents who work while entrusting their child to the day nursery is increasing by several reasons in Japan.So-called an issue of the children on the waiting list for a vacancy at the day nursery becomes an everyday affair.Moreover, through various revisions of administrative advices against the day nursery, the work of childminders in the nursery has been increasing [1].In order to resolve this situation, utilization of objective system for assisting the work of the nursery and monitoring of children is one of solution.For instance, activity recognition of children using acceleration sensor and data-mining classifiers [2] was presented to analyze children's activity and history of their growth.In [3], a child monitoring system using the Kinect sensor to estimate mental status of the children from the measurement was proposed.Although these techniques are useful to know some properties of children, they are not sufficient since children's nature of utilization of livelihood space is not taken into account.That is, in order to adequately estimate the growth and activity of children, it is necessary to know an indoor environmental map of the day nursery or kindergarten, since social meanings of areas in a house (for instance a play zone, corridor, and eating area) are significant for it.It is, however, difficult to obtain an environmental map of the day nursery because the room layout is changed frequently depending on the purpose of use, the number of children, and their age distribution.Therefore, automatic measurement of the indoor environment is desirable, and the SLAM (Simultaneous Localization and Mapping) by a mobile robot is an adequate approach.
Traditional SLAM methods utilize characteristic shape such as the corner of wall as feature points to identify environmental locations, and they usually utilizes an image processing and high speed computation, and large memory tend to be required.To overcome such drawbacks by simplification of computation, for instance, an orthogonal SLAM that limits orthogonal elements in the room is known [4].Such technique, however, might not be suitable for modern kindergarten that is constructed with human-friendly and curved shape wall or furniture.Moreover, a swarm of children around the robot prevents the robot from correct measurement, and so-called kidnapped robot problem frequently occurs if the robot is not so heavy for children.Traditional methods managed to recover a certain amount of accuracy by repeating the SLAM survey motion.For this reason, almost all SLAM techniques assume implicitly that environmental elements of house and furniture are static [5].Various dynamic SLAM techniques are presented; however, they also assume that architectural element are static at least [6].In case of application in a kindergarten, it is feared that dynamic SLAM may not work well because there are many semi-statistical obstacles such as furniture.Therefore in this study, an alternative SLAM is presented by utilizing adroit LM sheets which are attached to the wall.The proposed SLAM algorithm is evaluated by a simulation, and the practical effectiveness was confirmed by experiment with actual mobile robot.

A. Virtual comb-shaped marker
When the LRF measures distance to a retro reflective material, a correct distance cannot be measured due to an effect of virtual reflection from the material.Therefore, data measured by the LRF shows a virtual comb-shape as shown in Figure 1(b), if the LRF scans a LM sheet which consists of blank sheets and reflective sheets alternately.Our method utilizes this adroit LM to facilitate the detection of LM positions for the SLAM algorithm.

B. Computation of LM's position using Hough transformation
In our previous work, a basic algorithm to derive a position of LM was presented [7]; however, the detection accuracy was insufficient due to error in extraction of LM sheets.Therefore, in this paper, a new method to improve the extraction is proposed by utilizing Hough transformation since it is known that a line detection method using Hough transformation has high tolerance ability against measurement noise.First, a linear part detected by LRF against a flat wall surface (the linear part is called wall line hereafter) is searched from the measurement data by utilizing Hough transformation.Second, the virtual comb-shape is found by checking an offset difference from the detected wall line.Finally, a position of LM is computed as a center of the comb-shape.Details of the procedure to obtain the LM position are explained in the following sections.
In the following experiment, URG-04-LX-UG01 (HOKUYOU DENKI corp., JAPAN) is used and the measurable range and resolution of the sensor are plus-minus 120 degree and 0.36 degree, respectively.Hence, the measurement data of the LRF sensor are described as ( ) , where , , and are an angle index, a measurement distance, and the corresponding laser irradiation angle, respectively.The raw LRF data are converted into two-dimensional local coordinate value as ( ) by , .

C. Detection of the LM maker sheet
After the wall lines are detected by Hough transformation, the comb-shape markers are found by checking offset between each data point measured by LRF and each wall line which was detected.The detection algorithm is explained below.
Values of slope and an intercept which correspond to candidates of the wall lines are computed after applying an orthodox Hough transformation.Distance between each local coordinate value computed from raw LRF data and the segment of each wall line is computed by Step 2.
After boundary points between the reflective sheet and the blank sheet are found, the LM position is determined by detecting three boundary points that are caused by the virtual comb-shape.The boundary point is found as a point where a sequence of the distance changes rapidly because the reflective material gives virtual longer measurement data than actual distance.To tell the boundary point from others, perpendicular distances from the wall line between adjoined pairs at indexes and are compared, and the discrimination is performed by comparing the predetermined threshold .hold simultaneously for and such are found three time continuously, the is determined as an index of the LM marker's position, where is a threshold.Finally, the LM marker's coordinate values ( ) are computed as follows. (2)

III. SLAM ALGORITHM
A two wheeled differential drive mobile robot is assumed to be used for the following SLAM algorithm.The state of the robot is described as where and are coordinate values of a center of the robot, is the robot's orientation, and an integer subscript represents the sampling time index and the same notation is used below.An input vector is defined as [ ] , where is a translational velocity, and is an angular velocity.Then the kinematics of the robot can be described as follows by considering two cases of and : where is a constant sampling interval.For latter discussion, the following expression is used instead of (4).
where function expresses the second term of RHS in (4).Moreover, Jacobians of concerning and for and are obtained as follows, respectively. where

A. Derivation of sensor model
In this section, a sensor model of LRF is derived for a use of the measurement update in the SLAM algorithm.The model is computed under the assumption that true values of geometrical information are known.That is, distance and relative angle between the LRF sensor and a certain LM are assumed to be measured.The details are as follows., that calculates the distance ̃ ( ) and relative angle ̃ ( ) between the robot and the LM can be expressed as Another Jacobian ( ) of ( ) concerning the state is computed as follows.
where , and

B. Estimation of positions using EKF
In order to perform the SLAM computation, an extended Kalman filter (EKF) procedure is applied to estimate status of robot variable and the LM's positions.To utilize the procedure, the state of the map, that are ̃

DisplayTex
where N is the number of all LMs and N is assumed to be already known.Then, the augmented state vector y at time t + 1 is estimated using ( ) with status at time t as follows.
where [ ] is a coefficient matrix to extract components of the robot status p from y. Then a Jocobian of (10) concerning y, that is described as , is computed as follows using which was described by (5).
EKF-SLAM is an algorithm to express a belief bel( ) by using an average of the estimation for and an estimated covariance of .The former, that is the average, is denoted as and the size and physical meaning in the element are the same as .On EKF-SLAM, the robot state vector p and positions of LMs ( ( ) and ( ) ) are estimated via an estimation of .In order to update and in an iterative computation, the input to the robot kinematics, an actual measurement value from the sensor, the robot kinematics model described by (4), and the LRF sensor measurement model defined by (7) are used.Specifically, an estimated average at the time t, that is denoted by ̅ , is computed by using the robot kinematics model from the previous average .Subsequently, the covariance matrix for is updated by projecting other covariance of noise perturbation expressed in a control space domain into the state space domain.(This procedure is called a prediction phase.)Next, a difference between an actual measurement value and a predicted measurement ̂ is computed, where ̂ is obtained using the LRF sensor model from the predicted value ̅ .The difference ( ̂ ) is reflected into the update of prediction of ̅ according to other updated covariance matrix .(This procedure is called a measurement update phase.)All the details are explained below.
Step 0. Preparation A new coefficient matrix ( ) that extracts both state variables of robot (x, y, θ) and j th LM ( ( ) ( ) ) from the augmented vector y t is defined by It is assumed that a constant covariance matrix R of the robot status p is known as ( ).Similarly, other covariance matrix Q concerning the measurement value z is predetermined as ( ). Step

Prediction phase 1-Extraction
Temporal variable ̅ is extracted from an estimates average vector that was computed at time .This extraction operation is described as ̅ | , and similar expression is used below.

1-2. Update of the average of predicted variable
The average value ̅ is computed using the robot kinematics model from the temporal variable ̅ and input as follows.

1-3. Update of the covariance matrix
Covariance matrix is obtained as a sum of two variances: one is a variance that comes from a change in , and the other is a projection of a variance in a change of the robot status p into the state space domain.Therefore, a predicted covariance ̅ is calculated by By repeating an above-mentioned procedure, an estimation of augmented vector is updated successively, and the robot state ( ) and all LMs' state ( ) that are included in  can be estimated.In short, an estimation of the robot position and an update of the environmental map are realized simultaneously.

A. Simulation
First, the simplified EKF-SLAM algorithm was solely confirmed by simulation with fixed point LMs not with virtual comb-shape LM's.The simulation environment for a virtual SLAM robot was designed, as shown in Figure 2. The environment was assumed as an H-shape corridor in a building, where the latter experiment was performed, and total 19 land markers' point were put on the virtual space.For the virtual robot motion control, an input to the robot was given by an operator on the simulator.The maximum of input was specified as 0.5 [m/s] and 0.5 [rad/s] for translational and angular velocities.To check the robustability of the EKF effect, the LRF measurement data was intentionally contaminated by random measurement noise (the maximum is 3 [cm]).
In Figure 2, the trajectories obtained by simulation were drawn.The blue and red trajectories show the true trajectory of the virtual robot and the estimated one by the presented EKF-SLAM algorithm.It was confirmed that the estimated trajectory coincides with the true one pretty well.The mean error of the estimation of the robot's position was 112 [mm] under the intentional 3 [cm] perturbation measurement noise.This error rate corresponds to 0.82 [%] against the total length of the robot trajectory.It can be confirmed that mechanism of the simplified EKF-SLAM worked sufficiently.Second, in order to check an effectiveness of the same EKF-SLAM algorithm with the virtual-comb shape maker detection approach, another simulation was executed.Using same virtual corridor environment, total 19 virtual comb-shaped LMs were placed in wall part.Virtual measurement data from a robot to virtual comb-shaped LMs was obtained by a lay-tracing computation in the simulation.At that time, random measurement noise (maximum 5 [mm] and 0.01 [rad] for distance and angle respectively) was added to imitate the actual measurement noise.The command to the robot motion was similarly given by an operator during the simulation.Changing a combination of the start and goal of virtual robot locomotion, total eight cases of the simulation were executed and each result is drawn in Figure 3 and  It can be found that the mean error was almost same small value in spite of existence of disturbance noise in the virtual sensor measurement.Average of the error in eight cases was 103.1 ± 7.28 [mm] which corresponds to 0.57 % accuracy against total length of robot locomotion.It can be said that the presented EKF-SLAM with virtual comb-shaped makers can obtain sufficient presumption of the robot's self-position and environmental geometry position.

B. Experiment
The presented EKF-SLAM approach was implemented to actual robot that was designed in authors' previous study [8].The maximum speed of the robot was specified in similar way to previous simulation conditions.Eight real virtual comb-shaped markers were attached to wall of a real indoor corridor as shown in Figure 5 (a), and size and sensor spec were the same as in the simulation.In case of dead reckoning, the error rate against true trajectory was as large as 40.7 [%].This large error was caused by a slip of the wheels of the robot.On the other hand, the presented method shows a 5.3 [%] error which was smaller than the case of dead reckoning.This value is better than dead-reckoning but is worse as about ten times than the result of simulation.Although the conditions in real and in simulation are not the same, the reason comes from difficulty in the detection of virtual comb-shaped in real environment.That is, the detection by the LRF sensor is weakened depending on the length of the measurement points.This issue remains as a future work.From this reason, objective monitoring system to watch over children is expected.In this study, to solve problem specific to the kindergarten use, an approach of a simplified EKF-SLAM which is effective by short-distance survey is proposed.The proposed SLAM algorithm was evaluated by a simulation, and the practical effectiveness was confirmed by experiment with actual mobile robot.Thorough simulation, the SLAM algorithm with an extraction method of the special LM from LRF sensor data was verified.For this result, it was confirmed that the estimation error was as small as 0.57 % against total length of robot locomotion.Finally, same algorithm was implemented to an actual robot that was equipped with the LRF sensor, and practical effectiveness was validated.As a result, the presented SLAM with a margin of estimation error of 5.3 % was also confirmed in the experiment.
As remained issue, we have to confirm whether the presented SLAM technique operates well when children surround the robot in real situation.

ACKNOWLEDGMENT
Experiment in kindergarten was supported by many children, their parents, and childminders in Benesse Childcare Centor Hiyosi.The present authors really appreciate many participants who embraced our requests kindly.

First
, true values of each robot's variable are assumed to be given, and they are denoted as [ ̃ ̃ ̃] .Moreover, true positions of all LMs are also known and the and coordinate values are described as ̃

Figure 2
Figure 2 Simulation result of the simplified EKF-SLAM using fixed points LMs under intentional measurement noise.

Figure 4 .
As shown in those figures, it was confirmed that each estimated trajectory matched well to true one.TABLE I summarizes the mean error with variance for eight cases.

Figure 5 (
b) shows the layout of the LMs and the planed trajectory for the actual robot.

Figure 5 (
Figure 5 (a) Experimental environment with the actual mobile robot (b) Layout of the environment and robot's trajectory In the experiment, the robot was controller by manual operation along the line of the corridor to about 10 [m] length.Figure 6 (a) shows the obtained trajectory by a dead reckoning form the encoders embedded in the robot's motors.Figure 6 (b) shows the estimated result obtained by presented method.

Figure 6 (
a) shows the obtained trajectory by a dead reckoning form the encoders embedded in the robot's motors.

Figure 6
(b)shows the estimated result obtained by presented method.

Figure 6
Figure 6 Self-position estimation result: (a) by dead reckoning (b) by presented EKF-SLAM with virtual comb-shaped markers.V. CONCLUSION Number of parents who work while entrusting their child to the day nursery is increasing in Japan.
That is, all angle indexes which satisfy are stored in a mutable array which is denoted by .
simultaneously, are searched for all , where is a threshold and notation [ ] means element in the array .Using such , value of [ ] [ ] is adopted as an index corresponding to the position of reflective material parts.Such all indexes indicating reflective material parts are stored in other mutable array .