As a first approach, the details of how to implement a common nonparametric

Transcription

1 Chapter 3 3D EKF-SLAM Delayed initialization As a first approach, the details of how to implement a common nonparametric Bayesian filter for the simultaneous localization and mapping (SLAM) problem is presented in this chapter. The approach detailed, is based on a sequential Monte Carlo algorithm, commonly known as Particle Filter, to solve the initialization stage of landmarks. The vehicle localization is estimated with an Extended Kalman Filter (EKF) which is dynamically increased with those nodes whose particle filter has converged into a single Gaussian distribution. Thus, at the end, the complete SLAM problem is carried out as a centralized EKF-SLAM. 3.1 Overview of the method The first part of this work was focused on learning how to implement the classic approaches presented at the introduction of this document, so that the main drawbacks of the RO-SLAM problem could be detected. For that purpose, this chapter propose a RO-SLAM approximation which extends the mapping solution presented in [9], adding the localization of the robot. The proposed SLAM solution is mainly based on an EKF which initially contains only the estimation of a mobile robot (ground or aerial vehicle) position. On the other hand, a set of range sensors are used as the main sensor to correct the estimation of the robot localization. However, the location of these sensors (or landmarks) is completely unknown and hence, their position must also be estimated by means of mapping algorithms. TomapthepositionofthesesensorswithanextendedKalmanfilter, itisnecessary to have an initial position estimation of them. Due to the low-informative measurements provided by these devices, (i.e. only distance between robot and one landmark), it is necessary to employ an initialization strategy which allows to

2 3.2 Localization approach Figure 3.1: Initial uniform distribution of a range sensor position when only one range measurement is received from a robot. The estimated position from which the robot detected the landmark the first time (i.e. first range measurement received) is depicted with a yellow point and represent the center of the uniform spherical distribution in which the landmark might be located. The real position of the landmark is depicted with a green point. get an initial estimation of landmarks which deals with the complex observation model of such kind of sensors. Here, it is considered the use of range sensors such as radio emitters or ultrasonic sensors which provides the identification of each landmark, so that the data association problem is directly solved by the use of this unique identifiers. The method presented in this chapter proposes a delayed initialization based on a sequential Monte Carlo filter (or Particle filter) which, as stated above, consist on sampling the state space of the sensor location using an initial probability distribution with which particles/samples of the particle filter are drawn. The framework which uses the EKF to solve the SLAM problem is usually called EKF-SLAM framework In this case, the initialization stage takes place when the first range measurement r i is received from beacon i. This stage consist on initializing a new particle filter P i,t which particles are drawn according to the probability distribution of the range observation model, like the one depicted in Figure 3.1; the general observation model of this kind of sensors is detailed in Appendix A.3. Then, once the particle filter converges into a single Gaussian distribution, the particle set P i,t can be integrated in the same EKF where the robot localization takes place. By following this procedure with every landmark detected, the EKF vector state increase dynamically. At the end, the complete SLAM problem is solved in a centralized EKF. 4 3D EKF-SLAM Delayed initialization

3 3.2 Localization approach Particle filter: Update stage In the SLAM strategy presented in this chapter, the localization of the vehicle is directly performed by the EKF, so that the first parameters of the state vector are the vehicle parameters. Thus, an initial value of these parameters must be defined, indicating the associated variance of the initial robot position in the covariance matrix of the EKF. Depending on the motion model and the vehicle Initially, the parameters of the used, the number of robot parameters might differ from 3 to more parameters. The motion models used in this work for simulation and real experimentation vehicle position are are described in Appendix A. often considered As the vehicle moves, depending on the motion model employed, new mo- completely uncorrelatedtion measurements u t are received. The motion measurements are integrated in the EKF during the prediction stage to get a prior distribution of the vehicle position. On the other hand, the correction stage of this localization problem, consist on integrating the range measurements between the vehicle and the different landmarks detected. However, this correction stage cannot be applied until the landmarks have been integrated in the EKF, i.e. until the landmarks are initialized in the EKF. The delay on the integration of landmarks into the EKF, implies that the localization estimation is not corrected until, at least, 3 landmarks are integrated in the EKF and hence, the localization of the vehicle might diverge making the EKF unstable numerically. For this reason, when employing delayed initialization methods, it is mandatory the use of anchors, i.e. landmarks which position is known, so that the robot position estimation can be corrected with the measures of this anchors. Thus, anchors must be deployed throughout the environment taking into account the area coverage of each transmitter. At least 3 anchors are required to perform a good trilateration of a landmark when a 2D 3.3 Hybrid Mapping approach This section explains how to initiate and update the particle filters used during representation is the initialization stage of each landmark, and how to determine the convergence considered, and at of the particle filter so that, finally, these particle filters can be integrated into least 4 anchors for a centralized extended Kalman filter trading off between the precision of this 3D representations. estimation and the convergence velocity Landmark initialization The particle filter is initiated when the first range measurement from the robot is received. The first range measurement defines the radio of the uniform annulus sphere distribution in which the real position of the landmark is located. The center of this sphere is directly related with the estimated position of the robot at this time x r,t. This uniform distribution is depicted in Figure 3.1, in this picture two radio values are depicted, r 1 and r 2, they represent the thickness of the annulus, which depends on the deviation σ(r t ). Particles are then initiated 3D EKF-SLAM Delayed initialization 41

4 3.3.2 Particle filter: Update stage as 3D points represented in Cartessian coordinates x = [XYZ] T and distributed according to that annulus spherical uniform distribution Particle filter: Update stage The objective of the range-only mapping algorithm is to estimate the position of the landmarks from data provided by range sensors. The information about the state is updated with the set of measurements z 1:t received up to time t. This set of measurements consists of range measurements r i generated between one range sensor i and the aerial/ground robot (the algorithm considers a moving node, and thus the time subscript t for the robot position). In the Bayesian framework employed during the initialization stage, the information about the state is represented by the conditional probability distribution p(x t z 1:t ). This distribution (the posterior) can be estimated online while new measurements are received. Indeed, the position will be estimated and updated recursively. In that sense, the likelihood function p(z t x t ) plays a very important role in the estimation process. In this case, this function expresses the probability of obtaining a given range value r i from a range sensor i given the position of the robot x r,t. The model used here considers that p(z t x t ) follows a Gaussian distribution centered on the range value of r [n] i with a standard deviation proportional to the standard deviation of the range measurement received. r i = N(µ(r [n] i ),σ ( r [n] i )) (3.1) A detailed explanation of the observation model used along this document is presented in Appendix A.3. Once the parameters of the Gaussian observation model have been identified, the weights of the particles are updated considering the likelihood of the received data and the current likelihood of each particle (lines 1 to 5 of Algorithm 3.1). In detail, the update procedure is as follows: for each particle p [n] t, the distance r [n] t = p [n] t x r,t is obtained. From this distance, the mean and variance of the conditional distribution p(r t p [n] t ) are obtained, so that p(r t p [n] t ) = N(r t ;µ(r [n] t ),σ(r [n] t )). The probability of the actual range value under this distribution is finally employed to update the weight of the particle ω [n] t as: ω [n] t = ω [n] t 1 σ(r [n] t ) 2π e (r t µ(r[n] t )) 2 2σ(r [n] t ) 2 (3.2) When the filter is running, the weights of the particles with high likelihood increase, while most of the particles rest at places of very low likelihood on the state space. 42 3D EKF-SLAM Delayed initialization

5 3.3.3 Switching from PF to EKF Algorithm 3.1: Particle filter for mapping WSN Input: P t 1 and r t Output: P t 1 for n = 1 to N do 2 Compute r [n] t = p [n] t x r,t 3 Determine µ(r [n] t ) and σ(r [n] t ) 4 Update weight of particle n as ω [n] t = ω t 1p(r [n] t p [n] t ) = ω t 1N(r [n] t ;µ(r [n] t ),σ(r [n] t )) 5 end for 6 Normalize weights ω [n] t 7 Compute N eff 8 if N eff < N th then 9 Resample with replacement N particles from P t. 1 end if As the number of particles is limited, a resampling algorithm (line 9 of Algorithm 3.1) is included to compute the posterior probability bel(x t ). The algorithm duplicates particles with high weights and eliminates those with very low weights according to the probability p(r t p [n] t ), i.e. according to the previously updated weights. The resampling method employed in this chapter is known It is very important to normalize as low-variance resampling method, which is described in Algorithm 3.2. This algorithm allows to spread the particles over the maximum likelihood areas. As the weights updated Algorithm 3.2 shows, this algorithm differs from the one described in [3] in that, with p(r t p [n] t ) before resampling instead of replacing the most probable particles, they are drawn with a normal distribution centered in one of the most likely particles and with variance σ lvr. particles. In order to overcome some of the known problems with the resampling stage, an additional consideration is taken into account: resampling only takes place when the effective number of particles N eff is below a threshold N th. The effective number is computed as follows: N eff = [ N n=1 (ω [n] t ) 2 ] 1 (3.3) Switching from PF to EKF As new measurements are received, each particle filter tends to converge to a solution which can be compared with a Gaussian distribution. Then, in order to determine the convergence of the solution, the algorithm developed uses the averaged standard deviation σ Pt as a measurement of convergence of the particle filter into a Gaussian distribution. The averaged standard deviation is computed from the covariance matrix of the Gaussian distribution followed by the particle filter at current time t. 3D EKF-SLAM Delayed initialization 43

6 3.3.3 Switching from PF to EKF Algorithm 3.2: Low variance resampling Input: Pt Output: P t 1 P t = ; 2 r = rand([,n 1 ]); 3 c = ω [1] t ; 4 i = 1; 5 for n = 1 to N do 6 u = r+(n 1)N 1 ; 7 while u > c do 8 i = i+1; 9 c = c+ ω [i] t ; 1 end while 11 draw p t P t according to N( p [i] t,σ lvr ) with ω t = ω [i] t ; 12 end for 13 Normalize weights ω [n] t ; The mean µ t and variance Σ t of the Gaussian distribution followed by the actual particle filter can be computed as follows: Σ t = µ t = N n=1 N n=1 p [n] t ω [n] t (3.4) (p [n] t µ t ) 2 ω [n] t (3.5) If the value σ Pt is lower than a certain threshold σ th, then the particle set follows a distribution similar to the Gaussian distribution defined by parameters µ t and Σ t. Another convergence criteria tested in this algorithm was the use of the Kullback Leibler divergence factor. The Kullback Leibler divergence factor compares two distributions and gives a value of divergence which is near to when there is a high similarity between both distribution. This divergence factor was used to compare the Gaussian distribution N(µ t,σ t ), with the real distribution of the current particle set, using equation (3.6). This divergence factor was finally discarded as a convergence criteria due to the computational time required to compute this divergence factor, which is very high when there is a huge number of particles. KL = ω [n] t ln( ω [n] t N(p [n] t ;µ t,σ t ) )dp t (3.6) When the PF has converged into a Gaussian distribution, the Gaussian N(µ t,σ t ) can be integrated in the extended Kalman filter. The procedure to 44 3D EKF-SLAM Delayed initialization

7 3.4. Characterization of Nanotron WSN EKF EKF m x1 mxm Ø t 3 x 1 Ø 3x3 t Figure 3.2: This figure shows how the state vector of a EKF can be extended with µ t and Σ t extracted from a particle filter. The initial state vector of the EKF is supposed to have m parameters before incorporating the 3x1 mean vector of the particle filter. include this Gaussian distribution into the current state vector and covariance matrix of the centralized EKF is quite simple: the mean value is appended at the end of the state vector and the covariance matrix obtained from the particle filter is also included at the end of the EKF covariance filter, making the correlations with other elements of the actual EKF equal to zero. Figure 3.2 shows the procedure with a simple scheme; the schemes assumes that µ t is a column vector of 3 elements X,Y,Z and that the actual size of the EKF state vector is m. Once a particle filter has been integrated in the centralized EKF, new measurements are incorporated in this EKF according to the range observation model described in Appendix A.3 for Cartesian coordinates. 3.4 Experimental results To validate the SLAM framework proposed in this chapter, different experiments where carried out. The experiments where performed in a simulated environment and with a real data set. The range sensor used for real experiments, and hence the one considered in simulations, is composed by a radio emitter and a set of receivers of a Wireless Sensor Network. These radio beacons are able to 3D EKF-SLAM Delayed initialization 45

8 3.4.1 Characterization of Nanotron WSN measure the distance between each pair of devices by processing the ToF of the transmitted signal. The first section characterize the Wireless Sensor Network used during the experiments and then some simulation and real experiment results are presented Characterization of Nanotron WSN The Wireless Sensor Network employed for the experiments of this work consist on a set of radio beacons specially designed for Real Time Localization Systems (RTLS). The WSN model employed is nanopan 5375 DK, developed by Nanotron. This model is shown in Figure 3.3 and has the following characteristics: ❼ ATMega 1284P microcontroller at 2MHz. ❼ Radio transceiver 2.4 GHz ISM band. Up to 2dB transmission power. ❼ Ranging accuracy of 2 m indoors / 1 m outdoors. ❼ 128KB flash memory for programs and retrieved data. ❼ Distance measurements computed with SDS-TWR method based on ToF method without needing any clock synchronization between nodes. These radio beacons implement a special protocol derived from the protocol called a. This protocol extends the low consumption characteristics of the previous protocol with a physic layer specially designed for RTLS. Despite the protocol reduces the bandwidth for data transmission, it improves the physic layer to reduce interferences with other devices working on the ISM band (2.4GHz) by adding two channels specially designed for the modulation technique called Chirp Spread Spectrum (CSS). This modulation technique reduces some interference with other existing frequencies by working on two new channels and by employing a robust modulation(css modulation) which reduces the transmission errors with respect the modulation used in On the other hand, radio beacons employed for these experiments implement a ToF based technique which improves the accuracy of common RSSI techniques for distance computation. Although the main drawback of ToF technique is the required synchronization between beacons, nanopan 5375 DK uses a measurement technique known as SDS-TWR (Symmetrical Double-Sided Two Way Ranging) which avoids the synchronization between nodes by performing a symmetrical double-sided two way ranging measurement which works as follows (see Figure 3.4): 1. Emitter node (node A) sends a range request message to node B and saves the local time stamp t r D EKF-SLAM Delayed initialization

9 3.4.1 Characterization of Nanotron WSN Figure 3.3: WSN nodes employed during experiments: nanopan 5375 DB. The figure shows the essential parts of the nanopan 5375 radio beacon architecture. The upper layer represents the hardware of this device, mainly composed by a Wireless interface (omnidirectional antenna), the transmitter microchip developed by Nanotron and the microcontroller used to control this transmitter. The API layer represents the drivers provided by the manufacturer to program the microcontroller to start ranging with other devices, interchanging data messages and to configure some parameters. The last layer represents the host controller, i.e. the computer in which range measurements are processed (in this case the host is the CPU of the robot which has a node attached; beacon nodes doesn t have a host computer). 3D EKF-SLAM Delayed initialization 47

10 3.4.1 Characterization of Nanotron WSN 2. Receiver node (node B) process the request message and sends an acknowledgement message to node A adding information about the time t p1 used to process the received request message. 3. Node A saves the time stamp t ack1 in which received the ACK message. 4. Steps 1, 2 and 3 are repeated starting from node B. 5. Node B sends the results t r2, t ack2 and t p2 of the second ranging to node A. Once the process has finished, the node which started the ranging cycle computes the distance between both radio beacons as follows: r a,b = (t ack1 t r1 t p1 )+(t ack2 t r2 t p2 ) (3.7) 2 Despite this ranging method is more accurate than other existing ranging methods, it is necessary to perform a characterization of the range measurements provided by the Nanotron s WSN. Thus, the first experiments performed during this work were focused on characterizing the noise of range measurements when the emitter and the receiver are both static and placed at different distances. The setup employed for this experiment is shown in Figure 3.5, as this figure shows, experiments were carried out in indoor and outdoor environments, using a metric tape to measure the real distance between both nodes. Using these setups, the characterization of the range sensor was at first performed with two static nodes separated by 1, 2, 4, 8,..., 3 meters. At firt, nodes were configured with the default configuration. The results of these experiments are depicted in Figure 3.6, where green dots represent the mean value of the measures received and the pink line represents the associated standard deviation at each tested distance. The results were quite similar in both scenarios, indoor and outdoor environments. Due to a constant deviation detected in the measures received, red points with the cyan line represents the same information but with an added offset. Even adding this offset correction, the measures were very noisy with several outliers, so a second experiment was performed where nodes were configured to emit with a higher transmission power and increasing the CSS pulse width to 4µs. The results are shown in Figure 3.7, where green dots represent the received range measurements and red dots represent the mean value at each distance tested, cyan and red lines represents the associated standard deviation of real measurements and mean values respectively. The results show a deviation of about 25cm for indoor and outdoor environments which is quite better than the precision specified by the manufacturer. 48 3D EKF-SLAM Delayed initialization

11 3.4.1 Characterization of Nanotron WSN Node A Node B TWR- 1st ranging RangeRequest tr1 ACK tack1 Time processing tp1 RangeRequest Time processing tp2 tr2 ACK tack2 TWR- 2 nd ranging ToF 2 Figure 3.4: Symmetric Double-Sided, Two Way Ranging (SDS-TWR): method to measure the distance between a pair of radio-based devices employing ToF measurements. The method performs the operation in both directions to correct errors related to the local clock of devices, the method is asynchronous, i.e. it does not need any synchronization between emitter and receiver to compute the ToF. 3D EKF-SLAM Delayed initialization 49

12 3.4.1 Simulations Emitter Metric tape Receiver (a) Outdoor setup (b) Indoor setup (c) Host connected to base node (emitter) Figure 3.5: Setup of nanopan 5375 characterization for outdoor and indoor environments. (a) shows the setup for outdoor experiments and, (b) and (c) shows the same setup for indoor experiments. Finally, a last characterization was performed employing the same configuration as in the second experiment. In this case, the experiment was performed in order to test the effect of other phenomenon which appears when one of the radio beacons moves, like doppler shifting and others. Most of these effects are virtually suppressed by using the CSS modulation of a protocol but, as the results shown in Figure 3.8, the motion of a beacon still affects the signal propagation, and hence the ranging precision. The characterization experiments with motion where carried out with a Pioneer-3AT with a beacon on-board this mobile robot. In this experiment the groundtruth of the robot was computed with a precise localization algorithm using laser data and a map of the environment in which the robot was moving through. In this case, the results in Figure 3.8 show a non constant behaviour of the standard deviation (cyan line) with respect the mean value (green line) of samples registered (blue dots) from the moving robot to 5 different static nodes. The standard deviation follows a linear function (3.8) which increase with the distance between radio beacons. σ(r i ) = 1.25r i +.25 (3.8) This characterization was only performed for indoor environments, a new dataset for outdoor environments is going to be gathered in the near future to characterize the WSN of Nanotron with a quadrotor where the the velocity is higher than the one used with the Pioneer-3AT. This increase in velocity can be 5 3D EKF-SLAM Delayed initialization

13 3.4.1 Simulations Measured distance (m) Avg. received distance Std received distance Avg. received distance + offset Std received distance + offset Real distance (m) Figure 3.6: First characterization of nanopan 5375 with static beacons. X axis represent the real distance between beacons and Y axis represent the measured distance. Green dots represent the mean value of the range measures received at each distance tested, and the pink line represents the associated standard deviation at each distance. Red points represent the mean value of the range measures with an added offset, while the cyan line represents the associated standard deviation at each tested distance. Measured distance (m) Measures received Std. averaged mesures Avg. measures received Std. measures received Real distance (m) Figure 3.7: Second characterization of nanopan 5375 with static beacons. X axis represent the real distance between beacons and Y axis represent the measured distance. Green dots represent the range measures received at each distance tested, and the cyan line represents the associated standard deviation of all measurements received. Red points represent the mean value of the range measures, while the red line represents the associated standard deviation of all mean values. 3D EKF-SLAM Delayed initialization 51

14 3.4.2 Simulations 3 Distancia medida (m) Samples Real model Error. Avg: Error. Std = 1.23x Distancia real (m) Figure 3.8: Characterization of nanopan 5375 with one beacon moving. X axis represent the real distance between beacons and Y axis represent the measured distance. Blue dots represent the range measures received while robot was moving. The green line represents the regression function of the mean values of range measurements received, while the cyan line represents the associated standard deviation of all mean values. compensated with the benefit of working on outdoor environments, where the multipath effect and other interference effects are lower than in indoor environments, specially when employing aerial robots Simulations To validate the algorithms developed in this work, different simulations are shown. This simulations have been performed using Matlab as the main programming language and framework. For this purpose, a simulator has been developed, which simulates not only the motion model of the vehicle but also the observation model of range sensors based on the static characteristic gotten in the previous section. Other sensors are also simulated, like a GPS and IMU. A Matlab toolbox has been used for the visualization of the simulated environment, while the other simple elements of the algorithms are plotted with the common plot3 function of Matlab. These visualization tools are used in this document to show the results of the different algorithms. A pair of examples of the visualization tool used to show the environment simulated can be seen in Figure 3.9a and Figure 3.9b. In this section several simulation experiments are presented, where an aerial robot (Quadrotor) is simulated together with a set of radio beacons with the 52 3D EKF-SLAM Delayed initialization

15 3.4.2 Simulations (a) Simulated beacon in an outdoor environment (b) Simulated quadrotor in an outdoor environment Figure 3.9: Examples of the simulator implemented. same characteristics obtained above. The mapping algorithm (i.e. the case where the position of the robot is well known) is not tested in this work since the mapping method implemented in this SLAM solution is the same that the one implemented in [9] and because this article shows the result of applying this mapping algorithm with an aerial robot. The first simulation considers the case where there is only one beacon to be mapped which real position is the center of a circular trajectory. The Z coordinate of this circular trajectory follows a sinusoidal function and the model of the quadrotor employed during the prediction stage is the one described in Appendix A. The use of a circular trajectory avoids the lack of state observbility caused for example in linear trajectories. An example of the observability problem effect will be shown in the next chapter. In this first simulation, 4 anchors are used as the unique sensors to correct the estimated position of the aerial robot by the EKF while the beacon position is not initialized by the PF used. The results of this experiment are shown in Figure 3.1, where the real position of the beacon and anchors are represented with a blue diamond an the real position and trajec- 3D EKF-SLAM Delayed initialization 53

16 3.4.2 Simulations Figure 3.1: This figure shows the final results of a delayed RO-SLAM employing a particle filter to get a initial estimation of the beacon position for the EKF. In this experiments 4 anchors are used as the unique sensor to correct the estimated position of the aerial robot while the beacon has not been initialized yet. tory of the robot is depicted with a red cross (the red line is the trajectory until the current timestamp). The estimated position of the robot is represented with a light blue cross (the light blue line represents the estimated trajectory until the current timestamp). The estimated position of the beacon is represented with a pink cross, this estimation corresponds to the estimation made by the EKF once the beacon has been initialized by the PF. For the PF, the standard deviation σ lvr used during the resampling algorithm of all simulations is.1 meters. This value was selected after trying different values in different simulations, being σ lvr =.1 the best value which makes the resampled particles not diverge so much from the most probable prior particles and at the same time makes the new set of particles to cover properly the most probable areas where the real position of the beacon is located. An example of convergence of a particle filter is shown in Figure In this figure the cyan diamond represents the current position of the robot, while the cyan dot represents the real position of the beacon to be initialized. The circles represent the particle set P t, where blue circles are the most probable particles (particles with higher weight), the green circles represent those particles with a probability between the most probables and the less probable particles, this particles are represented with red circles. The concentration of particles in different areas is caused by the resampling algorithm, where the sparse distribution around an 54 3D EKF-SLAM Delayed initialization

17 3.4.2 Simulations KL: PF STD Noise: Min. KL: Max. KL: Estimation error: KL:4.62 PF STD Noise: Min. KL: Max. KL: Estimation error: Robot Tracked object distance to robot: High weight higher than Robot Tracked object distance to robot: Low weight less than.183 Medium weight between.183 and.366 High weight higher than (a) Initial uniform distribution (b) After 25sec KL:Inf PF STD Noise: Min. KL: Max. KL: Estimation error: KL: PF STD Noise: Min. KL: Max. KL: Estimation error: Robot Tracked object distance to robot: High weight higher than Robot Tracked object distance to robot: Low weight less than Medium weight between and High weight higher than (c) After 5sec (d) After 59sec Figure 3.11: Example of particle filter convergence. The cyan diamond represents the current position of the robot, while the cyan dot represents the real position of the beacon to be initialized. The circles represent the particle set P t, where blue circles are the most probable particles (particles with higher weight), the green circles represent those particles with a probability between the most probables and the less probable particles, which are represented with red circles. area is the result of employing the standard deviation σ lvr when drawing the new particles instead of replacing an existing particle from the prior distribution. The convergence threshold σ th used is 3 meters, which is the same threshold used in [9]. Figure 3.12 represents the initial estimation of a beacon in the EKF, once the PF ha converged. After several simulations the convergence delay for this trajectory and the velocity of the simulated aerial robot is near to 1 minute (around 3 meters). As Figure 3.12 shows, beacons are not always properly initialized (in this example the initial estimation error is near to 4 meters), this bad initialization makes difficult the convergence of the beacon estimation in the EKF and might make diverge the estimation of the vehicle position. The second simulation experiment was intended to see the effect of fusing other sensor data into the EKF. The sensors fused with the range sensors are 3D EKF-SLAM Delayed initialization 55

18 3.4.3 Real dataset Figure 3.12: Example of the convergence of a PF into a Gaussian distribution. The picture shows the initial estimation of the EKF once the PF has converged. After several simulations the convergence delay for this trajectory and the velocity of the simulated aerial robot is near to 1 minute (around 3 meters). a GPS and an IMU. The results of the delayed RO-SLAM solution using other sensors are shown in Figure The results show how the inclusion of new sensor smooths the estimation of the robot trajectory. On the other hand, the estimation of the beacon position is quite worst compared with previous results, but this is due to a bad initialization of the beacon estimation with the PF (near to 5 meters of initial error). The last simulation experiment was focused on checking the effect of removing the anchor nodes of the WSN which are the most accurate sensors for robot positioning considering the three sensors used in the previous experiment. The results of this experiment are shown in Figure The final results show how the estimation of the robot position is quite worst, this is mainly related to the error associated with the GPS sensor (near to 3 meters). In this case, the estimation of the beacon position is quite better due to a better initialization of the EKF after the convergence of the PF Real dataset For real experiments, the Pioneer 3-AT ground robot was used together with the Nanotron WSN composed by 4 anchor nodes, 1 beacon node and 1 base node 56 3D EKF-SLAM Delayed initialization

19 3.4.3 Real dataset Figure 3.13: This figure shows the final results of a delayed RO-SLAM employing a particle filter to get a initial estimation of the beacon position for the EKF. In this experiments 4 anchors are used to correct the estimated position of the aerial robot together with a GPS and an IMU sensor. Figure 3.14: This figure shows the final results of a delayed RO-SLAM employing a particle filter to get a initial estimation of the beacon position for the EKF. In this experiments only the GPS and the IMU are used to correct the estimated position of the aerial robot. 3D EKF-SLAM Delayed initialization 57

20 3.4.3 Real dataset Beacon 5 Beacon 2 Beacon 1 Beacon 3 Beacon 4 Base (a) Pioneer 3-AT (b) Setup of radio beacons (c) Nanotron radio beacon Figure 3.15: Setup used in CONET testbed for real experiments. Pioneer 3-AT ground robot was used together with the Nanotron WSN composed by 4 anchor nodes, 1 beacon node and 1 base node (node attached to the robot). (node attached to the robot). The experiments were performed in the CONET testbed at the Engineering School (University of Seville). Figure 3.15 shows the setup used for this experiment. The motion and observation models used for this experiments are the WSN observation model and the Pioneer 3-AT model explained in Appendix A. For the groundtruth registration of the robot, an MCL (Monte Carlo Localization) algorithm was used employing a LIDAR sensor (Hokuyo UTM-3LX laser sensor). The real position of beacons was measured with a metric tape, taking into accont the global frame of the CONET testbed. In this experiment, the parameters of the robot position are the 2D position and orientation of the robot [x r,y r,φ r ], while the parameters of the beacon estimation are represented as 3D Cartesian coordinates [x b,y b,z b ]. The results of this experiment employing the delayed RO-SLAM algorithm are shown in Figure In this figure the groundtruth of the robot is represented with a red line and a red cross is used to represent its real position, the 58 3D EKF-SLAM Delayed initialization

21 3.5 Summary and conclusions Figure 3.16: 3D representation of the final results gotten with the delayed 3D RO- SLAM developed in this chapter. The groundtruth of the robot is represented with a red line and a red cross for the current real position, the result of the dead reckoning algorithm is represented with the green line and the green cross. The localization result of the RO-SLAM algorithm employed is presented with a light blue line and cross. The real position of the anchors and beacon is represented with a blue diamond, while the estimated position of the beacon is represented with a pink cross, the represented estimation is the estimation of the EKF. The number next to the identifier of each blue diamond, represents the distance measurement received in that instant from robot to this radio beacon. result of the dead reckoning algorithm is represented with the green line and the green cross. The localization result of the RO-SLAM algorithm employed is presented with a light blue line and cross. The real position of the anchors and beacon is represented with a blue diamond, while the estimated position of the beacon is represented with a pink cross, the represented estimation is the estimation of the EKF. The convergence of the PF is represented in Figure 3.18, in the figure it is depicted the final estimation of the PF from which the EKF estimation is initialized with an initial estimation error of 1.82 meters. On the other hand, the final estimation of the EKF filter has an error of 75cm, whereas the final estimation error of the robot localization is 1.13 meters. The difference between the dead reckoning algorithm and the RO-SLAM developed in this chapter is represented in the 2D representation of the final results in Figure D EKF-SLAM Delayed initialization 59

22 3.5 Summary and conclusions Figure 3.17: 2D representation of the final results gotten with the delayed 3D RO- SLAM developed in this chapter. The groundtruth of the robot is represented with a red line and a red cross for the current real position, the result of the dead reckoning algorithm is represented with the green line and the green cross. The localization result of the RO-SLAM algorithm employed is presented with a light blue line and cross. The real position of the anchors and beacon is represented with a blue diamond, while the estimated position of the beacon is represented with a pink cross, the represented estimation is the estimation of the EKF. The number next to the identifier of each blue diamond, represents the distance measurement received in that instant from robot to this radio beacon. The legend of Figure 3.16 is the same of this figure. 1 Robot Tracked object distance to robot: Low weight less than.5383 Medium weight between.5383 and.1617 High weight higher than Z(meters) X(meters) Y(meters) 1 15 Figure 3.18: Convergence of the PF used to initialize the position of Beacon 1 in the EKF. The most probable particles are depicted with blue circles, the less probable ones are represented with red circles and other intermediate particles are represented with green particles. The cyan point represents the real position oftheradiobeacontobemappedandtherealpositionoftherobotisrepresented with a cyan circle. 6 3D EKF-SLAM Delayed initialization

23 3.5 Summary and conclusions 3.5 Summary and conclusions This chapter has presented an implementation for the RO-SLAM problem applied to 3D environments. The solution presented in this chapter is a delayed 3D RO-SLAM which uses a particle filter (PF) to initiate each landmark of the map in a centralized EKF where the localization of the robot takes place. Then the EKF presented increase dynamically the size of the state vector when each landmark is initialized. Different simulations have been shown and a real experiment too. The main problem of this solution is that is a delayed solution so, only those measures received after the initialization of each beacon are integrated in the EKF loosing the first ones. The results of the experiments show that the main problem of the delayed initialization with a PF is the convergence of the particle filter into a Gaussian distribution. The results show that some times the initialization is quite far from the real solution, making the convergence of the landmarks estimation in the EKF very difficult and some times, this estimation might even diverge. Additionally the main drawback of the PF employed to initiate the landmarks is its computational complexity when the number of particles are very large. On the other hand, this chapter has also shown how different measures can be fused in a single filter, making the estimation of the robot position smoother when a GPS is employed together with the range sensors (WSN) and the IMU. In order to estimate properly the position of each landmark, a characterization of the WSN employed in this work as range sensors is presented in this chapter. The characterization of the WSN showed a linear relation between the error of the range measured by the WSN and the received range measurement. 3D EKF-SLAM Delayed initialization 61