SLAM in Dynamic Environments via ML-RANSAC

Transcription

1 *Manuscript Clic here to view line References SLAM in Dynamic Environments via ML-RANSAC Masou S. Bahraini 1,2, Mohamma Bozorg 2, Ahma B. Ra 1* 1 School of Mechatronic Systems Engineering, Simon Fraser University, Surrey, BC V3T A3, Canaa 2 Department of Mechanical Engineering, Yaz University, Yaz, Iran *Tel: , ara@sfu.ca, Fax: Abstract! " #"# # " #$% & # $% & ' " ' #$% & " " ' "!'(!'( ) " " ) * + +,#!- #. " " Keywors/!'( $% & # 1 Introuction ' " " "" 1 " " " ' " " " " " " 2 3 ' " "!'( $

2 ! " # $ %& % ' ' $ %& ( )* +, -. % $ %& % $ / ' )* +, - ' ' % 1 " ' & 23 " 22 $ ' 1 )* +, " ' % &% 5 4 " & % '6 &% &% % " " $ ' ' ' 1 4 $ %& 7 2 ' 5 8% : $ $ ; 2!! $ $ %& 2# < 5 <== 6 > '?>@ 4 ' 8% 2. 1 A B % 4 4 & " ' % " ' " 4 ' " ' "

3 ' 5 %% 6 % %% % %% 2/ " 1 " % 2 3 " $ %& <==" 5 $ 6 " 8% A = B 5 A = B 6 <== $ ' " ' 8% %% " " " 1 ' <==" $ 8% A = B 9 % A = B " 4 ( 4 A = B " A = B : " " %%"! " ' " ' %% % $ %&" " " ' $ ' " $ %& <==" $ 8% $ " 1 " $

4 % ( A = B 1 % A = B 5 16 A = B % " % : ' %% A = B % ' % A = B ' " % 1 ' % 2 " A = B " ' ' & & ' A = B A 2 % ' % 4 % $ %& % A = B :! " # 2 Revisiting SLAM an DATMO ' 4 % ' ' ' % :

5 % # ". " 9 $ %& ' 9 " ' 9 " -, )+ + ' $ " ' x 1 r xr 1 yr yr 1 r r t cos r t sin r r r t t t, (1) r by efining r / 2 an R L R L / r D, where raius, is the istance between them, an is the sample time of the iscrete fusion process. The state vector of the mobile robot is escribe by its position (, r) an orientation ( r). The variables! R an! L stan for the angular velocity of the right an the left wheels, respectively. The increment of encoer reaings can be use to approximate the velocity inputs in this moel. " # " $ %&'()% *+,%- Both ynamic an stationary objects are taen into account in this stuy. Let us moel the state of a stationary object by means of its position an the state of a moving object by means of its position an spee. Therefore, by enoting the location of the. th stationary lanmar as /1 an representing the 2 th moving object as /3 4, they can be augmente to the robot state vector where X r X r 32n4m X s, i (2) X j T 3 x y, X (3) r r r X r as: X s i x y s s 1 1 2n T x y s s, i1,2,..., n, 2 2 T... x y s s n n! T T (4)

6 6. 1,2,...,, m j y x y x y x y x y x y x X m T T T T m m m m j j j j j (5) Note that inices of 1 an, 4 stan for the. static an 2 ynamic objects, x an y represent the position of objects in Cartesian coorinates, an x an y are ynamic object velocities. The movement of the ynamic object is efine as a iscrete-time ynamic system in Cartesian coorinates (a quite general constant velocity moel for motion estimation in horizontal plane) as:, 1, 1 m j w X t T X j j (6) where, is the state of the moving object at time step escribe by T j j j j j y x y x X.In the moel given by (6), the acceleration of the trac is moele as noise of process. Matrix T is the transition matrix for the sampling perio, an is a zero mean Gaussian process noise with covariance matrix. The transition matrix an the covariance matrix can be evaluate by:, t t t T (7), t t t t t t t t Q Q (8) where Q is the stanar eviation of the process noise. As mentione before is the time perio between measurements. The final inematic equation can be augmente as:

7 X 1 F I X T wr w. (9) Note that the matrix F is the Jacobian matrix which is obtaine by linearization of the inematic moel of the robot (Eq. (1)), w r is the process noise after linearization, an the matrix I is ientity matrix. The Jacobian matrix will be sparse, since the most of the elements are zero. Therefore, using sparse techniques for saving arrays can reuce computational efforts an memory usage. 2.3 Observation Moel The observation moel can be expresse by a nonlinear function in the form: z i r i i ( i) 2 ( i) xm xv ym yv ( i) y y arctan m v ( ) i x x m 2 v v v r, (1) x v y v are the position of the observation evice at time step. m i represents the i th feature where, an ( i) ( i) in the surrouning environment with the pose of x m, ym in the global coorinate X G-Y G. The position of i th ( i) ( i) feature is inicate by r, with respect to the observation evice frame X R-Y R. The observation noise r an with the stanar eviation r an are efine for the range an bearing noise, respectively. It shoul be note that ynamic objects will be initiate using etecte position measure by laser scanner an zero initial velocities. It is assume that the vehicle is equippe with a evice (e.g., laser or raar) to observe the environmental features. In aition, the spee an the relative angle of the robot can be obtaine from the encoers of the wheels. Note that there is an offset between the mounting positions of the observation evice an the wheelbase of the robot. The efine observation in the robot coorinates can be transforme into the global coorinates by a transformation matrix [27]: x y v v x y acos asin, bsin (11) bcos, where, the position of the observation evice is efine by parameters a an b with respect to the robot frame. The basic layout of the observation process an the robot moel is presente in Fig. 2. 7

8 Fig. 2 Vehicle an observation inematics The measurement moel of stationary objects, after linearization of Eq. (1), can be expresse as: z v, H X (12) s s where H s is the observation matrix. The measurement moel of moving objects can be expresse as: z v, H X (13) where H = [H s 2 2], an is zero-mean Gaussian noise with covariance R, 2 R r. 2 (14) 3 Features Detection The available literature reveals that the perception problem an etection of features in ynamic environment are investigate using variety of sensors in ifferent scenarios. We employ a 2D laser 8

9 scanner to etect features an their position with respect to the robot in a ynamic environment. An overview of moving object etection by laser scanners can be foun in [18]. It shoul be note that the propose algorithm can be implemente by common observation sensors such as RGBD, monocular camera, stereo camera, laser an raar. Also, a istance-base segmentation technique is use as a simple metho to fin the features in 2D laser ata (by searching the ifferences in range an bearing in polar coorinate, or x an y in the Cartesian case). The propose ML-RANSAC algorithm oes not require a prior nowlege about the type of features to segment them to stationary an moving objects. Therefore, segmentation of features to stationary an ynamical objects is inclue in the ML-RANSAC algorithm. Laser rangefiners with a sufficient resolution an accuracy can be use to navigate a mobile robot in structure inoor environments. In orer to use the laser ata for navigation, a feature extraction an segmentation metho shoul be applie on raw ata. In this paper, a breapoint etection metho base on aaptive threshol is applie to etect features from raw ata provie by a conventional 2D laser scanner. Breapoint etection is an important proceure to cluster features. Breapoints refer to scan iscontinuities in the range image ue to the change in range an bearing in the scanning process. The computational effort of main processor an amount of sening ata to the main CPU will be reuce by using a ecentralize processing metho. A micro-controller can be applie to perform a pre-processing stage using segmentation an feature extraction algorithms in real-time application an finally the extracte features will be sent to the main processor. The aaptive breapoint etector evelope in [28] is use to etect an segment features. This algorithm is base on the istance between two consecutive points [r(i+1), (i+1)] T an [r(i), (i)] T. To separate the laser beam, the segmentation criterion can be state as: r i1 i1 r i i r i. sin sin 3, (15) p where is the angular resolution of laser, is an auxiliary constant parameter an p is the resiual variance to tae into account the stochastic behavior of the sequence of the scanne points an the noise associate with the range of laser scanner. In our experimental ata, we set the parameters to p =.3 m an = 2 respectively which was foun to be acceptable. The main interest of using the aaptive breapoint etector is its simplicity an intuitive appeal along with reasonable performance. The implementation of the aaptive breapoint etector is escribe in Algorithm 1. To valiate the algorithm, it is applie to one of our experimental ataset. There are some stationary an moving objects in the laser ata taen from a real environment. The aaptive breapoint etector is applie on the laser ata. The output results an etecte objects are shown in Fig. 3. After obtaining brea points, the features will be segmente. The first point an the last point of laser ata for a feature can be foun from breapoint etector. Therefore, we can approximate the feature imension an the angle between them from those points in a set of etecte laser points. 9

10 Fig. 3 Real scan example by 2D laser scanner along with etecte objects Algorithm 1: the aaptive breapoint etector Inputs: [r, ] T (range an bearing of scanne points), (the angular resolution), (constant parameter) an p (the resiual variance) Outputs: [Z r, Z ] T (range an bearing of extracte features center) 1. n=number of points 2. for i = 1 to n-1 o sin 3. Dmax r i. 3 sin 4. if r i1 i1 r p i Dmax i then a. feature_points [r(i), (i)] T 5. elseif feature_points is not empty a. feature() feature_points b en if 7. en for 8. [Z r, Z ] T = the mile point of feature points 9. fin imension of features 1

11 4 ML-RANSAC Algorithm for SLAMMTT RANSAC [29] is one of the most successful an wiely use iterative algorithms to estimate the parameters of a mathematical moel robustly, from a set of (given) observe ata in presence of outliers. The main iea of RANSAC is to construct a number of moel hypotheses (a number of ranom samples) from ranomly-sample minimal subsets of ataset (observation), an then evaluate the quality of these hypotheses on the entire ataset, where a user specifie threshol is require to separate inliers from outliers. The hypothesis with the highest consensus will be selecte as the solution. The number of iterations n hyp that is necessary to ensure that a correct solution with probability p is foun, can be compute from [3]: log1 p nhyp (16) m log 1 1, where, m is the minimum number of ata points necessary to fin estimation successfully an is the outliers ratio (percentage of outliers) in the ata points. Note that the initial number of iterations is usually chosen high enough (for example 1), then it will be upate from Eq. (16). The probability p is set to.99 in our implementations which means that at least one ranom sample oes not inclue an outlier. In our framewor, the estimate moel is the motion of moving objects in a ynamic environment which shoul be estimate from observation ata to search for corresponences or ata association. Increasing the number of iterations results in improving the probability quality an growth of the computational effort exponentially. In summary, the RANSAC algorithm can be state as two repeate steps: The first step is hypothesis generation which a ranomly minimal sample subset is selecte from the input ataset to form a set of hypothesis. The parameters of the moel are evaluate using only the hypotheses of this smallest sufficient sample subset. The secon step is hypothesis valiation which the entire ataset is verifie to be consistent with the estimate moel obtaine from the first step. Those hypotheses which lie outsie of the estimate moel within an error threshol will be consiere as outliers. Algorithm 2: ML-RANSAC Inputs: ˆ 1 X, P (EKF estimate state an covariance at time step -1), Z (measurement at time step ), 1 T th (trac threshol), N (maximum number of iterations allowe in the algorithm), 1 (first gating area for ata association), 2 (secon gating area for ata association), n (gating area for generating new tracs), (time interval) Outputs: Xˆ, P (EKF estimate state an covariance at time step ), number of stationary an moving objects 1. for each time step o 2. Propagate state estimate an covariance of all states (robot position, static an ynamic X ˆ, P preiction Xˆ, P features) via EKF. 11

12 3. Search for iniviual compatibility match using the first gating area 1 1 if inlier v 1 4. Compute the association matrix J, where J othervise 5. Fin the tracs with one associate array in the matrix J 6. First level EKF upate of one associate tracs foun in the previous step 7. if there is another observation which nees to mae a ecision then a. fin the tracs with the associate feature using RANSAC b. n hyp = N c. for i = to n hyp o i. Ranomly select estimate an observation matches ii. Generate hypotheses iii. Only upate states using EKF iv. Preict all measurements v. Compute the hypothesis consensus set vi. if new hypothesis has larger consensus set than previous hypothesis (C n>c ) then 1. Store current hypothesis N In 2. 1 N 3. n hyp IC log log 1 1 p 1 m (upating n hyp) vii. en if. en for e. secon level EKF upate with the gating area 1 8. en if 9. (optional) if there is another observation which nees to mae a ecision then a. fin the tracs with the associate feature using the secon preefine gating area 2 b. thir level EKF upate (with 2> 1) 1. en if 11. Prune static objects an moving tracs (static objects are optional) 12. Determine state of objects (static or ynamic) 13. Initialize new tracs after checing the preefine gating area for initializing new tracs n (it can be assume that all features are moving objects with zero velocity at the beginning) 14. en for 4.1 Algorithm escription: The propose ML-RANSAC algorithm is escribe in etail in this section. The inputs of this algorithm for time step can be summarize as: EKF estimate state an covariance at step (-1), set of measurement at time step, a threshol value to etermine an object is confirme as a trac, the 12

13 maximum number of iterations allowe in the algorithm, gating area for ata association, gating area for generating new tracs an the time interval t. The inematic moel of the robot, stationary an ynamic objects, an the observation moel are given. This algorithm finally returns the outputs: estimate state an covariance (of the vehicle, stationary objects an ynamic objects) at time step, an the number of stationary an moving objects. The implementation of the ML-RANSAC algorithm can be summarize in Algorithm 2. The algorithm is initialize by setting: ˆ, T x E x P E x xˆ x xˆ, (17) at the first time step. Uner the assumption of static environment, the state vector oes not change uring the EKF preiction step, but the state of the moving objects changes in every time step in ynamic environments an it has to be augmente to the state of the robot. Taing this into account results in propagation of the state estimates an their covariance from step (-1) to, incluing the robot position an orientation, an the characteristics of static an ynamic objects via EKF (Line 2). Given an initial conition x ˆ1 an initial covariance given by: x ˆ f x, u, (18) ˆ 1 P F where, 1 P F T G QG T, 13 (19) P 1, the propagate states an covariance are n x R is the state vector at time t with an initial conition x. u is the control inputs to the system at time step, F stans for the Jacobian of f with respect to the state vector whereas f xˆ at time step, n n : R R is nown as vector function, an Q is the covariance matrix of the process noise. Also G stans for the Jacobian of the process noise with respect to the state vector that xˆ xˆ at step. Note is the estimate of x before the measurement z is taen into account (upate step), an the estimate of x, after the measurement z is taen into account. The preicte state can be projecte into preicte measurements using the nown nonlinear measurement equation: ˆ h hxˆ, (2) S i i i i H P where, h H T i R, i (21) n m : R R is nown as vector measurement functions, H i is the Jacobian of the measurement function h i with respect to the state vector xˆ, an R i is the observation noise covariance matrix for the measurement i th assigne to the sensor. Measurements are teste by an active search for iniviual compatibility. The Mahalanobis istance ( ) is calculate from each observation to trac, an then the best observations are selecte with smallest istance to each trac within a preefine valiation gate ( 1) (Line 3). The association matrix J is establishe to ensure the geometric compatibility of features. It states the binary relation of the measurements to the existing tracs without isregaring the ambiguous associations. The value of one an zero in the association matrix J stan for the measurement either is an inlier or is not, respectively (Line 4). Using the association Matrix J, we can generate reasonable hypotheses which can be happene in reality, instea of generating all possible hypotheses. Applying this technique results in reucing computational efforts, ue to etermination of xˆ is

14 the appropriate matching between the estimate tracs an measurement features in the RANSAC part. If the measurement z is an inlier to only one trac, then it is use to upate the associate trac accoring to the association matrix J in the first level (Line 5 an 6). The measurement upate of the state estimate can be performe using the normal Kalman filter equations: K xˆ P xˆ P H T K 1 I K H P, S z, h (ˆ x ), (22) where, H stans for the augmente Jacobian of all measurements an h xˆ ( ) transforms the features positions into the sensor coorinate. So until here we have not use the RANSAC iteration which can tae more computational time. If there are some other observations which nee to be associate with a feature with a har ecision maing, it is trying to fin the tracs with the associate features using the RANSAC algorithm by the previous gating area (Line 7). Otherwise, we sip the RANSAC algorithm to save computational efforts. Then, n hypotheses are ranomly generate from observation ata an the estimate tracs. Then, using these hypotheses, the states are upate using EKF formula. In the next step all measurements are preicte to compute the hypothesis consensus by counting measurements insie a threshol. At the en of this part, the hypothesis will be compare with the previous ones. If the new hypothesis has larger consensus than the maximum consensus of the previous hypotheses, it will be store as the best hypothesis. It shoul be note that the ranom hypotheses will be create base on iniviual compatibility ata as well as the preicte state in the current algorithm. After receiving the association information, it is going to associate the previously gathere ata to implement the measurement upate (Line 7.e). If there is still another observation which nees a ecision maing, it coul be foun by the secon preefine gating area ( 2) which is an optional step (Line 9). In an environment with high ensity of moving objects, this step can help the algorithm to mae har ecisions, smoothly. If a trac oes not have any etection for a continuous number of scans, so it shoul be elete (Line 11). Some of stationary objects can be converte to moving object an vice versa in real worl, so the algorithm shoul be able to hanle this transformation as well. The status of objects can be change by checing the position of them while performing SLAM an DATMO (Line 12). If a measurement in the association matrix J is an outlier to all existing tracs, then it is use to create a new trac after checing the preefine gating area for initializing new tracs n (Line 13). An overall scheme of the ML-RANSAC algorithm for SLAMMTT is illustrate in Fig

15 Fig. 4 Overall scheme of the ML-RANSAC algorithm for SLAMMTT 5 Results an iscussion To evaluate the performance of the propose ML-RANSAC SLAMMTT approach, a series of simulation stuies were conucte. Aitionally, extensive experiments were performe by the mobile robot 15

16 Pioneer P3-DX in an inoor ynamic environment. All experiments emonstrate that our approach can accurately an reliably estimate the robot pose, construct the map, an eep tracing of moving objects even in situations of occlusions in which the robot is moving at spee of up to 2 m/s. 5.1 Simulation results In orer to show the efficiency an robustness of the propose algorithm, the simulation results were carrie out in two ifferent scenarios. The first scenario was performe in an occlusion situation for 5 moving objects in an inoor environment, whilst another scenario was carrie out in a complex inoor environment incluing both stationary an moving objects. In the first experiment, the simulate robot was moving with spee of up to 2 m/s in a ynamic environment. Simultaneously, five moving objects were moving in this environment (shown as blue asteris in Figs. 5 an 6). It was assume that the positions of the objects were capture with respect to the robot by a laser range finer with maximum istance of 3 metres an fiel of view 18 egrees. During all of the simulation experiments, the observation noises were set to.2 m an 2 egrees for range an bearing, respectively. The upate frequency was eight scans per secon for the laser range finer. In this simulation experiment, the tracs are moving with constant velocities while the estimator oes not have information about them. If the estimator oes not have any observation from features for a certain perio of time (which is 3.5 secons in our current system), the trac will be remove from the trac list. Fig. 5 applying SLAM algorithm in ynamic environment 16

17 Fig. 6 applying propose SLAM an DATMO algorithm in ynamic environment Fig. 5 shows a typical example of SLAM in ynamic environment which the restriction of using only SLAM leas to a wrong localization an mapping. Here the robot moves through an unnown environment incluing several moving objects (shown by asteris) which move with unnown velocity towars each other in the vicinity of the mobile robot. The circles inicate the estimate position of the moving objects, which are obtaine by applying EKF SLAM on the ataset. It was observe that we coul not construct a true map from environment an the algorithm iverge. The algorithm mappe the moving objects as stationary objects, wrongly. Also, the estimate path compute by the EKF along with the true path is shown in the Fig. 5. Here the continuous line inicates the estimate path, whereas the trajectory of the robot is inicate by a blac continuous line, the estimate trajectory of the moving objects are shown by the coloure line. This scenario emonstrates the fact that we nee a ifferent algorithm to trac moving objects. The ML-RANSAC correctly localizes the robot an concurrently tracs the moving objects which pass through the environment. This is inicate by a comparison of the two approaches in Figs. 5 an 6. Thus the ML-RANSAC approach generates more accurate localization than methos using only SLAM algorithms. The estimate path of the robot an the map of the environment are illustrate using the propose ML-RANSAC algorithm via EKF filter. To analyze the avantage of the explicit occlusion hanling in the propose algorithm, we constructe a ataset with a situation of occlusions. The estimate trajectory of the robot along with the estimate trajectories of moving objects (coloure continuous lines) are plotte in Fig. 7. This figure shows a particularly challenging situation in which the ML-RANSAC algorithm is able to successfully trac several people (moving objects) waling with velocities of (V x(t 1)=.625, V y(t 1)=.625, V x(t 2)=.625, V y(t 2)=, V x(t 3)= -.1, V y(t 3)=-.75, V x(t 4)= -.25, V y(t 4)= -.8, V x(t 5)=-.75, V y(t 5)=.6) along x an y irections whereas temporarily 17

18 occluing each other. The estimate position of the robot an tracs are plotte in the selecte steps in Fig. 7. It can be seen that the moving objects are trace correctly through the motion. The occlusion situation can be observe in Step 55-65, clearly. Although the ynamic objects are moving towar each other, the estimator eeps tracing of objects continuously while the mobile robot is moving an localizing itself. Aitionally, the final estimate trajectories of moving objects an the robot are plotte in Fig. 8. The mean square errors of the trajectory of tracs are plotte in Fig. 9. It can be seen that losing the observation of features (for example from step 7 for trac T3 (violet)) leas to increasing the error of estimation. Although losing observation leas to increasing error in tracing process, the error of the estimate trajectory is still acceptable using the propose algorithm. Figure 1 shows the estimate velocities of tracs in each step through the motion. The coloure lines in this figure correspon to each trac in Fig. 8 from T1 to T5. The velocity error was going to zero for some tracs an then it was increase. The reason is ue to losing observation when those tracs were eeme to be out of the observation area since the assume laser scanner in the simulation stuies have ha 18-egree fiel of view. To improve the target tracing in a real environment, we have use a laser scanner with 36-egree fiel of view in our experiment in real ynamic environment. 18

19 Fig. 7 the estimate trajectories of the robot an moving objects in the selecte steps 19

20 Fig. 8 The estimate trajectories of the robot an moving objects (colour continuous lines) 2

21 Fig. 9 the mean square error of estimate trajectories of the moving objects (T1 to T5 from up to own) 21

22 Fig. 1 velocities of tracs in each step (T1 to T5 from up to own) In the secon experiment, the simulate robot was moving with spee of up to 2 m/s in a ynamic environment lie the lobby of a builing, where some stationary objects existe. Simultaneously, several moving persons were waling in this area, with frequently changing their orientation of motion. Continuously, the types of objects were classifie an tracs were etecte as moving objects. The robot was planne to move in the illustrate path while the moving objects wale aroun the robot an entere to the environment or exite from it. Figure 11 shows the robot an the ynamic environment. It can be seen that using the propose algorithm on the occlusion maps, the system constructe the map of the stationary objects an trace the moving objects in a reliable manner. Thus, the ML-RANSAC algorithm improves the performance of the system. It shoul be note that most of tracing algorithms will fail, if one of the two sample sets is remove or if one sample set tracs the wrong moving object after the occlusion too place. The performance of our tracing algorithm is evaluate with an without occlusion situation through the motion of the robot in the escribe environment. 22

23 Fig. 11 The simulation results of SLAMMTT proceure in selecte time steps for a complex inoor scene. 23

24 The estimate position of the robot an tracs are plotte through the motion by the selecte steps in Fig. 11. Here the robot navigates an unnown environment with several stationary an moving objects which are inicate by asteris an plus signs, respectively. The blac circles re squares inicate the estimate position of the stationary an moving objects, respectively. Figures 11-a an 11-b show that the tracs are ept continuously, even by 18 egrees changing the orientation of moving objects. It shoul be note that in the situations lie this, algorithm generates new tracs from moving objects an the ol tracs will be elete afte them anymore. Figures 11-c,, e, f show some particularly challenging situations in which moving objects temporarily occluing each other or even stationary objects to receive observation from features. It can be seen that the propose algorithm is able to successfully trac moving objects an to estimate position of stationary objects. Also, the estimate trajectory of the robot in the inoor ynamic environment is given in Fig. 12 whereas it is compare with the true path of robot an the oometry ata. Fig. 12 the estimate an true trajectory of robot in the inoor ynamic environment The escribe experiments emonstrate that the propose algorithm is able to reliably provie accurate estimate of the robot position, to construct the map of environment, an to trac several moving objects even in presence of occlusion situation. Note that uring all of our experiments the type of objects (static or ynamic) is unnown for the system an estimator. Aitionally, the propose metho is able to reliably eep tracing of both long an short observe moving objects. 24

25 5.2 Experimental results (Inoor scene): The test be for experimental stuies was a Pioneer P3-DX mobile robot augmente with sensors as shown in Fig. 13. Although ifferent types of range finer sensors were installe on the robot, such as sonar, stereo camera an RGBD, LIDAR (RPLIDAR), we only obtaine ataset an valiate the propose algorithm using RPLIDAR. Fig. 13 Pioneer P3-DX mobile robot with mounte sensors A 36 egrees RPLIDAR laser scanner was mounte on the top of the mobile robot. The RPLIDAR (a2) is a low cost 2D laser scanner with range of 8 to 16 meters, precision of 2mm, scan frequency of 1 Hz an mobile robot which is appropriate for inoor environment. Applying LIDAR for observation, leas to ecreasing the computational effort an sequentially, expeite the etection of features in occlue situation, classification of objects an accuracy in position estimation. 25

26 Fig. 14 The estimate trajectories of the robot an moving objects 26

27 The results of etection an tracing of moving objects tas for inoor experiment are presente in Fig. 14. In this experiment, three waling persons were correctly trace in the occlusion situation. The spee of the robot is 25 cm/s. The moving objects (peestrians) performe a ranom wal with a constant velocity aroun 15 cm/s. Although there is no observation from velocities of moving objects (there is a restriction on gathering ata from velocity) an the measure information from sensor was inclue uncertainty about the position of objects, the propose algorithm shows a consierable robustness to eep tracing of moving objects an estimate the trajectory of the robot. Although, we have ha an intermittent observation from one of the moving objects between two Figs. 14-a an 14-b, the propose algorithm ept tracing of moving object. From Fig. 14-c, it can be seen that the tracing of moving objects are ept even behin of the robot by exploiting a 36- egree laser scanner. 6 Conclusion In this paper, an algorithm is propose to inclue moving objects in an SLAM formulation. The rationale for this stuy is noting that many SLAM algorithms fail when the observation is not continuous uring the tracing process [31]. Also, most SLAM algorithms for ynamic environments require the static objects as essential components to stay localize an to construct a map from the surrouning environment while tracing the moving objects. Detection an classification of objects are important problems when a robot is moving through a ynamic environment. Data association is another important problem for SLAM in ynamic scenes. The contribution of the propose ML-RANSAC algorithm is to alleviate the main rawbacs of SLAM in presence of moving objects. To valiate the propose ML-RANSAC SLAMMTT approach in presence of moving objects, a series of simulation stuies were conucte in challenging ynamic environments such as an environment without static objects an ense ynamic environments in presence of occlusion situations an intermittent observations. The propose ML-RANSAC algorithm wors well an maintains tracing of moving objects even by receiving intermittent observations or when there is not any static object in the environment. Simulation stuies as well as experimental results confirm the effectiveness an feasibility of the algorithm in challenging environments. Simulation an experimental stuies have verifie the performance of the propose algorithm even for the cases where an unnown number of ranomly place moving objects exist in a ynamic scene. References [1] E. Zamora, W. Yu, Recent avances on simultaneous localization an mapping for mobile robots, IETE Technical Review, 3 (213) [2] D.Z. Wang, I. Posner, P. Newman, A new approach to moel-free tracing with 2D liar, Robotics Research, Springer, 216, pp [3] O.E. Burlacu, M. Hajiyan, Simultaneous Localization an Mapping Literature Survey, Avance Control System ENGG 658, Aceemia. eu, (212). 27

28 [4] T.S. Ho, Y.C. Fai, E.S.L. Ming, Simultaneous localization an mapping survey base on filtering techniques, Control Conference (ASCC), 215 1th Asian, IEEE, 215, pp [5] C. Caena, L. Carlone, H. Carrillo, Y. Latif, D. Scaramuzza, J. Neira, I. Rei, J.J. Leonar, Past, Present, an Future of Simultaneous Localization an Mapping: Towar the Robust-Perception Age, IEEE Transactions on Robotics, 32 (216) [6] A. Petrovsaya, M. Perrollaz, L. Oliveira, L. Spinello, R. Triebel, A. Maris, J.-D. Yoer, C. Laugier, U. Nunes, P. Bessiere, Awareness of roa scene participants for autonomous riving, Hanboo of Intelligent Vehicles, (212) [7] C.-C. Wang, C. Thorpe, S. Thrun, M. Hebert, H. Durrant-Whyte, Simultaneous localization, mapping an moving object tracing, The International Journal of Robotics Research, 26 (27) [8] M. Darms, P. Rybsi, C. Urmson, Classification an tracing of ynamic objects with multiple sensors for autonomous riving in urban environments, Intelligent Vehicles Symposium, 28 IEEE, IEEE, 28, pp [9] D. Migliore, R. Rigamonti, D. Marzorati, M. Matteucci, D.G. Sorrenti, Use a single camera for simultaneous localization an mapping with mobile object tracing in ynamic environments, Proceeings of International worshop on Safe navigation in open an ynamic environments application to autonomous vehicles, 29. [1] A. Petrovsaya, S. Thrun, Moel base vehicle etection an tracing for autonomous urban riving, Autonomous Robots, 26 (29) [11] T.-D. Vu, O. Aycar, Laser-base etection an tracing moving objects using ata-riven marov chain monte carlo, Robotics an Automation, 29. ICRA'9. IEEE International Conference on, IEEE, 29, pp [12] K.-H. Lin, C.-C. Wang, Stereo-base simultaneous localization, mapping an moving object tracing, Intelligent Robots an Systems (IROS), 21 IEEE/RSJ International Conference on, IEEE, 21, pp [13] T.-D. Vu, J. Burlet, O. Aycar, Gri-base localization an local mapping with moving object etection an tracing, Information Fusion, 12 (211) [14] A. Azim, O. Aycar, Layer-base supervise classification of moving objects in outoor ynamic environment using 3D laser scanner, 214 IEEE Intelligent Vehicles Symposium Proceeings, IEEE, 214, pp [15] Q. Baig, M. Perrollaz, C. Laugier, A robust motion etection technique for ynamic environment monitoring: A framewor for gri-base monitoring of the ynamic environment, IEEE Robotics & Automation Magazine, 21 (214) [16] A. Asvai, P. Peixoto, U. Nunes, Detection an tracing of moving objects using 2.5 motion gris, Intelligent Transportation Systems (ITSC), 215 IEEE 18th International Conference on, IEEE, 215, pp [17] C.-H. Chang, S.-C. Wang, C.-C. Wang, Exploiting Moving Objects: Multi-Robot Simultaneous Localization an Tracing, IEEE Transactions on Automation Science an Engineering, 13 (216) Vanapel, M. Hebert, C. Thorpe, Moving object etection with laser scanners, Journal of Fiel Robotics, 3 (213) [19] P.C. Niefelt, Recursive-RANSAC: A novel algorithm for tracing multiple targets in clutter, Brigham Young University, 214. [2] C. Qiu, Z. Zhang, H. Lu, H. Luo, A Survey of Motion-Base Multitarget Tracing Methos, Progress In Electromagnetics Research B, 62 (215) [21] P.C. Niefelt, R.W. Bear, Multiple target tracing using recursive RANSAC, 214 American Control Conference, IEEE, 214, pp

29 [22] S.W. Yang, C.C. Wang, Simultaneous egomotion estimation, segmentation, an moving object etection, Journal of Fiel Robotics, 28 (211) [23] R. Raguram, J.-M. Frahm, M. Pollefeys, A comparative analysis of RANSAC techniques leaing to aaptive real-time ranom sample consensus, European Conference on Computer Vision, Springer, 28, pp [24] P.C. Niefelt, Recursive-RANSAC: A Novel Algorithm for Tracing Multiple Targets in Clutter, (214). [25] M. Bete, Z. Wu, Data Association for Multi-Object Visual Tracing, Synthesis Lectures on Computer Vision, 6 (216) [26] J.E. Guivant, E.M. Nebot, Optimization of the simultaneous localization an map-builing algorithm for real-time implementation, Robotics an Automation, IEEE Transactions on, 17 (21) [27] T. Bailey, Mobile robot localisation an mapping in extensive outoor environments, Diss. The University of Syney, 22. [28] G.A. Borges, M.-J. Alon, Line extraction in 2D range images for mobile robotics, Journal of Intelligent & Robotic Systems, 4 (24) [29] M.A. Fischler, R.C. Bolles, Ranom sample consensus: a paraigm for moel fitting with applications to image analysis an automate cartography, Communications of the ACM, 24 (1981) ucture from motion an visual oometry, Journal of Fiel Robotics, 27 (21) [31] J. Shi, Y. Li, G. Qi, A. Sheng, Extene target tracing filter with intermittent observations, IET Signal Processing, 1 (216) Masou S. Bahraini receive his B.S. egree from Shahi Bahonar University of Kerman, Kerman, Iran, in 29; an M.S. egree from Shiraz University, Shiraz, Iran, in 212, both in Mechanical Engineering. He is currently a Ph.D. caniate in Department of Mechanical Engineering, Yaz University, Yaz, Iran. He is also a research assistant at the Autonomous an Intelligence Systems Laboratory (AISL), Simon Fraser University, Surrey, BC, Canaa. His research interests inclue robot navigation in ynamic environments, eep learning, vibration analysis an control. Mohamma Bozorg receive his B.S. an M.S. an Ph.D. egrees all in Mechanical Engineering from Shahi Chamran University of Ahwaz, Ahwaz, Iran, Sharif University of Technology, Tehran, Iran an Syney University, Syney, Australia in 1989, 1991 an 1997, respectively. In 1997, he joine the Department of Mechanical Engineering, Yaz University, Yaz, Iran, where he is currently an associate professor. His current research interests inclue sensor fusion, navigation of autonomous robots, robust control, control of time-elay systems an mechatronics. He is a member of "Control Design" Technical Committee of International Feeration of Automatic Control (IFAC) an a founing member of Robotic Society of Iran (RSI). Ahma B. Ra receive the B.S. egree in engineering from Abaan Institute of Technology, Abaan, Iran, in 1977; the M.S. egree in control engineering from University of Brafor, Brafor, U.K., in 29

30 1986; an the Ph.D. egree in control engineering from the University of Sussex, Brighton, U.K., in He is a Professor with the School of Mechatronic Systems Engineering, Simon Fraser University, Surrey, BC, Canaa. His current research interests inclue autonomous an intelligent systems, an intelligent control. 3